Grid map is not showing on RViz using Particle Filter approach and tf broadcaster

edit

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 ... (more)