Robotics StackExchange | Archived questions

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

robot_localization

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.

particle_filter

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:

pf_screenshot

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

Answers