slam_toolbox map not updating
I am trying to create a map using data from a LiDAR (rplidar a1 model) that's plugged into my computer. The map will generate one time but it will not update when the lidar is moved. I think the issue probably has something to do with the transforms, but I don't know how to fix it. These are my steps:
- Launch the "rplidar_a1_launch.py" file from the rplidar_ros package
- Launch the dynamic transform publisher for laser -> base_footprint. here is the file: import rclpy from rclpy.node import Node from tf2_ros import TransformBroadcaster from geometry_msgs.msg import TransformStamped
class TransformPublisher(Node): def __init__(self): super().__init__('laser_transform_publisher') self.broadcaster = TransformBroadcaster(self)
self.timer = self.create_timer(0.1, self.publish_transform)
self.transformStamped = TransformStamped()
self.transformStamped.header.frame_id = 'base_footprint'
self.transformStamped.child_frame_id = 'laser'
self.transformStamped.transform.translation.x = 1.0
self.transformStamped.transform.rotation.w = 1.0
def publish_transform(self):
self.transformStamped.header.stamp = self.get_clock().now().to_msg()
self.broadcaster.sendTransform(self.transformStamped)
def main(args=None): rclpy.init(args=args) transform_publisher = TransformPublisher() rclpy.spin(transform_publisher) rclpy.shutdown()
if __name__ == '__main__': main()
- Launch the dynamic transform publisher for base_footprint -> odom. This is the file:
import rclpy from rclpy.node import Node from tf2_ros import TransformBroadcaster from geometry_msgs.msg import TransformStamped
class TransformPublisher(Node): def __init__(self): super().__init__('odom_transform_publisher') self.broadcaster = TransformBroadcaster(self)
self.timer = self.create_timer(0.1, self.publish_transform)
self.transformStamped = TransformStamped()
self.transformStamped.header.frame_id = 'odom'
self.transformStamped.child_frame_id = 'base_footprint'
self.transformStamped.transform.translation.x = 1.0
self.transformStamped.transform.rotation.w = 1.0
def publish_transform(self):
self.transformStamped.header.stamp = self.get_clock().now().to_msg()
self.broadcaster.sendTransform(self.transformStamped)
def main(args=None): rclpy.init(args=args) transform_publisher = TransformPublisher() rclpy.spin(transform_publisher) rclpy.shutdown()
if __name__ == '__main__': main()
Launch slam_toolbox: ros2 launch slam_toolbox online_async_launch.py use_sim_time:=false
Launch rviz and add a map and laser frame. The map generates and I can see the LiDAR data moving on screen when I move the LiDAR. This line is printed in the terminal I launched rviz in: [INFO] [1690391887.815470686] [rviz2]: Trying to create a map of size 157 x 312 using 1 swatches
My TF tree goes laser, base_footprint, odom, map and it all looks good. The only notable thing is the average rate from map to odom is 50.218. My rqt graph has the laser transform, odom transform, and slam_toolbox all pointing to /tf. /tf is in a sqaure and has 2 arrows pointing to a transform listener and 2 more arrows pointing to a different transform listener.