Laser scan "lags" behind pose estimate

asked 2021-03-13 17:09:54 -0500

JeffR1992 gravatar image

I have the following code snippet that is used to produce corrected pose estimates by scan matching the current laser scan against a buffer of previous laser scans, and then applying the transformation matrix from the scan matching output to update/correct the pose estimate:

void ScanMatchMapperNode::odom_laser_callback(const nav_msgs::OdometryConstPtr &odom_ptr, const sensor_msgs::LaserScanConstPtr &laser_scan_ptr) {
    ROS_INFO("Odometry and laser scan messages received");

    const float heading = utils::quaternion_to_yaw(odom_ptr->pose.pose.orientation.w, odom_ptr->pose.pose.orientation.x, odom_ptr->pose.pose.orientation.y, odom_ptr->pose.pose.orientation.z);
    current_odom_pose_ = Pose(odom_ptr->pose.pose.position.x, odom_ptr->pose.pose.position.y, heading);

        estimated_pose_ = scan_match(estimated_pose_, laser_scan_ptr, current_odom_pose_, previous_odom_pose_);

        broadcast_pose_frame_transform(estimated_pose_, map_frame_id_, estimated_pose_frame_id_);
        transform_laserscan_to_frame(laser_scan_ptr, estimated_pose_frame_id_);
        publish_pose(estimated_pose_);

        previous_odom_pose_ = current_odom_pose_;
}

Once the pose estimate is updated, I then broadcast its frame with broadcast_pose_frame_transform(estimated_pose_, map_frame_id_, estimated_pose_frame_id_) defined as follows:

void ScanMatchMapperNode::broadcast_pose_frame_transform(const Pose &pose, const std::string &frame_id, const std::string &child_frame_id) {
    geometry_msgs::TransformStamped pose_frame_transform;

    pose_frame_transform.header.stamp = ros::Time::now();
    pose_frame_transform.header.frame_id = frame_id;
    pose_frame_transform.child_frame_id = child_frame_id;

    pose_frame_transform.transform.translation.x = pose.x;
    pose_frame_transform.transform.translation.y = pose.y;
    pose_frame_transform.transform.translation.z = 0.0;

    tf2::Quaternion orientation;
    orientation.setRPY(0.0, 0.0, pose.theta);
    pose_frame_transform.transform.rotation.w = orientation.w();
    pose_frame_transform.transform.rotation.x = orientation.x();
    pose_frame_transform.transform.rotation.y = orientation.y();
    pose_frame_transform.transform.rotation.z = orientation.z();

    transform_broadcaster_.sendTransform(pose_frame_transform);
}

and then "attach" the laser scan to this broadcasted estimated pose frame with transform_laserscan_to_frame(laser_scan_ptr, estimated_pose_frame_id_) defined as follows:

void ScanMatchMapperNode::transform_laserscan_to_frame(const sensor_msgs::LaserScanConstPtr &laser_scan_ptr, const std::string &frame_id) {
    sensor_msgs::LaserScan transformed_laser_scan;

    transformed_laser_scan = *laser_scan_ptr;
    transformed_laser_scan.header.stamp = ros::Time::now();
    transformed_laser_scan.header.frame_id = frame_id;

    laser_scan_publisher_.publish(transformed_laser_scan);
}

However, when visualizing the data in Rviz, I see that the laser scan seems to "lag" behind the estimated pose, which is especially noticable when the robot performs turns.

A short video of what I'm describing can be found here: https://imgur.com/a/0eIuHku

As such, I was wondering if there is a way to have the laser scan perfectly synced with the estimated pose frame. I had assumed that setting header.stamp to ros::Time::now() for both the estimated pose frame broadcaster and the transformed laser scan message would have removed the laser scan lag, but unfortunately this does not seem to work. Any help would be appreciated, thanks.

edit retag flag offensive close merge delete