Grid map is not showing on RViz using Particle Filter approach and tf broadcaster
Hello, I built an underwater ROV system that surveys the sea-floor using two different methodologies based on the information that is coming from the sensors mounted on board of the ROV and on the boat:
1) EKF using Robot_localization
2) Particle Filter (PF) that will substitute EKF in the moment I loose a specific sensor and will proceed with the estimation of the position/trajectory leveraging Bayes theorem.
With the sonar data I built a grid_map
using the grid_map
package provided by ros
.
I launched grid_map
from a node and after that I launched Robot_localization
Robot_localization approach
As you can see below Robot_localization
works very well and I can see the ROV surveying the sea-floor with no problem. This is confirmed from the rqt_tf_tree
and from RViz
Particle Filter approach
However when I use the Particle filter
approach something is not going totally right and I can't correctly visualize the ROV moving and surveying the sea-floor.
After looking at rqt_tf_tree
it seems that things are correct.
Contrarily from Robot_localization
I don't have here the odom
frame that is present on Robot_localization
and I think this should be correct, but please let me know if this is not.
The following command I used on terminal for the static transform:
rosrun tf static_transform_publisher 1 0 0 0 0 0 1 map base_link 100
The result is the following screenshot below, and the grid_map
is not appearing underneath the ROV, which is where is supposed to be:
The following below is the GridMap node
that I launch with its related reference frames:
void pubMultiBeamLocalFrame(grid_map::Position multiBeamMapCenter, ros::NodeHandle nh)
{
static tf2_ros::StaticTransformBroadcaster br;
geometry_msgs::TransformStamped transformStamped;
transformStamped.header.stamp = ros::Time::now();
transformStamped.header.frame_id = "utm";
transformStamped.child_frame_id = "gridmap_center";
transformStamped.transform.translation.x = multiBeamMapCenter.x();
transformStamped.transform.translation.y = multiBeamMapCenter.y();
tf2::Quaternion q;
q.setRPY(0, 0, 0);
transformStamped.transform.rotation.x = q.x();
transformStamped.transform.rotation.y = q.y();
transformStamped.transform.rotation.z = q.z();
transformStamped.transform.rotation.w = q.w();
br.sendTransform(transformStamped);
}
And below the particle filter node
that I launch with its related reference frames:
void publishParticles()
{
geometry_msgs::PoseArray particleCloudMsg;
particleCloudMsg.header.stamp = ros::Time::now();
particleCloudMsg.header.frame_id = "utm";
// .. Additional operations
particle_pub.publish(particleCloudMsg);
}
void publishPose() // publish of the estimate of the state
{
Pdf<ColumnVector> * posterior = filter->PostGet();
ColumnVector pose = posterior->ExpectedValueGet();
SymmetricMatrix pose_cov = posterior->CovarianceGet();
geometry_msgs::PoseStamped pose_msg;
pose_msg.header.frame_id = "utm";
pose_msg.pose.position.x = -pose(1);
pose_msg.pose.position.y = -pose(2);
pose_msg.pose.position.z = -pose(3);
static tf2_ros::TransformBroadcaster br;
geometry_msgs::TransformStamped transformStamped;
transformStamped.header.stamp = ros::Time::now();
transformStamped.header.frame_id = "utm";
transformStamped.child_frame_id = "map";
transformStamped.transform.translation.x = -pose(1);
transformStamped.transform.translation.y = -pose(2);
transformStamped.transform.translation.z = -pose(3);
tf2::Quaternion q;
q.setRPY(0, 0, 0);
transformStamped.transform.rotation.x = q.x();
transformStamped.transform.rotation.y = q.y();
transformStamped.transform.rotation.z = q.z();
transformStamped.transform.rotation.w = q.w();
br.sendTransform(transformStamped);
pose_pub.publish(pose_msg);
}
The transformation of the coordinate latitude and longitude is handled with the following python file below that intercepts the underwater position message self.pub = rospy.Publisher('/usbl/pose_projected', Odometry, queue_size=10)
:
class TransformPose:
def __init__(self):
self.pub = rospy.Publisher('/usbl/pose_projected', Odometry, queue_size=10)
self.pressed = False
rospy.Subscriber("/usbl/latitude_longitude", NavSatFix, self.callback)
def paramsReady(self):
//.. definitions of parameters.....
def callback(self,data):
rospy.loginfo_once("received data lat lon usbl")
if(self.paramsReady()):
rospy.loginfo_once("Valid survey params found: transform_pose starting")
crs_wgs = proj.Proj(init='epsg:4326') # assuming you're using WGS84 geographic
utm_zone = rospy.get_param("/utm_zone")
crs_bng = proj.Proj(init=utm_zone) # use a locally appropriate projected CRS
originLon = rospy.get_param("/origin_lon")
originLat = rospy.get_param("/origin_lat")
xoff, yoff = proj.transform(crs_wgs, crs_bng, originLon, originLat)
zoff = rospy.get_param("/origin_z",0)
# input from NavSatFix
lon = data.longitude;
lat = data.latitude;
x, y = proj.transform(crs_wgs, crs_bng, lon, lat)
x = x-xoff
y = y-yoff
z = 0;
# output message
transformed_msg = Odometry()
transformed_msg.header.stamp = data.header.stamp;
transformed_msg.child_frame_id = "base_link"
if x== float('Inf') or y==float('Inf'):
pass
else:
transformed_msg.pose.pose.position.x=x
transformed_msg.pose.pose.position.y=y
transformed_msg.pose.pose.position.z=z
transformed_msg.header.frame_id="map"
transformed_msg.pose.covariance = [5., 0., 0., 0., 0., 0.,
0., 5., 0., 0., 0., 0.,
0., 0., 5., 0., 0., 0.,
0., 0., 0., 0., 0., 0.,
0., 0., 0., 0., 0., 0.,
0., 0., 0., 0., 0., 0.]
// other operations.....
def main():
rospy.init_node('nav_projection_node')
transformer = TransformPose()
rospy.loginfo("starting")
rospy.spin()
main()
I can successfully see the ROV and its estimated position but I can't see the grid_map
below the ROV.
I have been struggling with this problem for the past days and can't move on. Thank you in advance for pointing in the right direction for solving this problem.
Asked by RayROS on 2019-08-03 12:21:00 UTC
Comments