Issue with Continuous Map Creation Using YDLidar X2 and ROS2 Humble SLAM Toolbox

Question

I'm currently facing an issue while working on my robotics project. I have set up a dynamic transform for Lidar data from Map-> odam-> base_footprint->base_link. Following this, I run the ydlidar_ros_driver using the Ydlidar_launch.py file, and then I initiate the slam_toolbox using the online_async_launch.py file.

While the setup successfully creates a transform, I'm encountering a problem with the continuous creation of the map. The map doesn't seem to be updating as expected. I'm using the YDLidar X2.

Has anyone experienced a similar issue or could provide guidance on how to troubleshoot this problem? Any insights or suggestions would be greatly appreciated.

This code that i used to generates transforms for the coordinate frames "map" to "odom," "odom" to "base_footprint," and "base_footprint" to "base_link" using lidar data.

class LidarTransformPublisher(Node):
def __init__(self):
    super().__init__('lidar_transform_publisher')
    self.tf_broadcaster = tf2_ros.TransformBroadcaster(self)
    
    # Subscribe to lidar data
    self.create_subscription(LaserScan, '/scan', self.lidar_callback,  rclpy.qos.qos_profile_sensor_data)

def lidar_callback(self, lidar_data):
    # Extract relevant information from lidar data
    ranges = lidar_data.ranges
    angle_increment = lidar_data.angle_increment

    # Assuming lidar_data provides an angle for each range
    theta = lidar_data.angle_min

    for range_value in ranges:
        # Calculate x and y based on range and current angle
        x = range_value * math.cos(theta)
        y = range_value * math.sin(theta)

        # Create dynamic transformations
        map_to_odom_transform = self.create_transform('map', 'odom', x, y, theta)
        odom_to_base_footprint_transform = self.create_transform('odom', 'base_footprint', 2 * x, 2 * y, theta)
        base_footprint_to_base_link_transform = self.create_transform('base_footprint', 'base_link', 3 * x, 3 * y, theta)

        # Send dynamic transformations
        self.tf_broadcaster.sendTransform(map_to_odom_transform)
        self.tf_broadcaster.sendTransform(odom_to_base_footprint_transform)
        self.tf_broadcaster.sendTransform(base_footprint_to_base_link_transform)

        # Increment the angle for the next range
        theta += angle_increment

def create_transform(self, parent_frame, child_frame, x, y, theta):
    transform = TransformStamped()
    transform.header.stamp = self.get_clock().now().to_msg()
    transform.header.frame_id = parent_frame
    transform.child_frame_id = child_frame
    transform.transform.translation.x = x
    transform.transform.translation.y = y
    transform.transform.translation.z = 0.0
    transform.transform.rotation.x = 0.0
    transform.transform.rotation.y = 0.0
    transform.transform.rotation.z = math.sin(theta / 2.0)
    transform.transform.rotation.w = math.cos(theta / 2.0)

    return transform

Thank you!

Answer 1

Have you tried using static transforms first?

If I understand your code as written, you are publishing new transforms for the location of the lidar based on values from the scan. This is incorrect. The transforms should be derived from your robot's physical configuration. I don't see a transform showing how the laser scanner is mounted relative to the base_link.

Second, the odom --> base_link transform should be derived from your robot's odometry, and you may need a specific driver for your specific hardware platform.

Third, the SLAM toolbox should be publishing the map --> odom transform.

Answer 2

1. I would check if you are using the default mapper_params_online_async.yaml file which the launch file you mentioned calls (it is in root, but you should build locally or modify in /opt/ros/distro/share/slam_toolbox/cfg/mapper_params_online...)

Usually is set small width/height for mapping, different lidar range (small) which is only suitable for turtlebot tutorial.

2. Additionally if changing parameters doesn't work, check if map_server is active(sometimes it is not auto started by launch file if not defined):

A) Check:

ros2 lifecycle list
 <map_server_node_name>

B) Get Status

ros2 lifecycle get map_server (or proper node name)

C) Activate if not active

ros2 lifecycle set map_server activate 

3. If the issue is your robot tf, just visualize if is correct or not:

   ros2 run tf2_tools view_frames

Then visualize the tree:

evince frames.pdf

Double check the transform chain from Odom to base_link:

ros2_run tf2_ros tf2_echo <from_frame> <to_frame>