# 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)
{
geometry_msgs::TransformStamped transformStamped;

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;

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.pose.position.x = -pose(1);
pose_msg.pose.position.y = -pose(2);
pose_msg.pose.position.z = -pose(3);

geometry_msgs::TransformStamped transformStamped;

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 ...

edit retag close merge delete