Hello everybody
I am trying to make SLAM using a 2D laser scanner (RPLidar A1) supporting it with a Realsense camera T265, which provides an accurate odometry.
The arrangement of both devices is as follows:
The realsense ROS package provides two main frames: camera_odom_frame and camera_pose_frame. This last one is the one that moves around when the camera moves through the space. This is the tf tree created by the camera package:
I have tried to use these two devices with two SLAM packages: hector_slam and gmapping.
hector_slam
This package works well when carrying out SLAM without odometry. My problem comes when I try to include the odometry information. After launching the LIDAR and the camera packages, I launch hector_slam using the following launch file:
<?xml version="1.0"?> <launch> <arg name="tf_map_scanmatch_transform_frame_name" default="scanmatcher_frame"/> <!-- <arg name="base_frame" default="base_footprint"/> --> <arg name="base_frame" default="base_link"/> <!-- <arg name="odom_frame" default="nav"/> --> <arg name="odom_frame" default="camera_odom_frame"/> <arg name="pub_map_odom_transform" default="true"/> <arg name="scan_subscriber_queue_size" default="5"/> <arg name="scan_topic" default="scan"/> <arg name="map_size" default="2048"/> <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen"> <!-- Frame names --> <param name="map_frame" value="map" /> <param name="base_frame" value="$(arg base_frame)" /> <param name="odom_frame" value="$(arg odom_frame)" /> <!-- Tf use --> <param name="use_tf_scan_transformation" value="true"/> <param name="use_tf_pose_start_estimate" value="false"/> <param name="pub_map_odom_transform" value="$(arg pub_map_odom_transform)"/> <!-- Map size / start point --> <param name="map_resolution" value="0.050"/> <param name="map_size" value="$(arg map_size)"/> <param name="map_start_x" value="0.5"/> <param name="map_start_y" value="0.5" /> <param name="map_multi_res_levels" value="2" /> <!-- Map update parameters --> <param name="update_factor_free" value="0.4"/> <param name="update_factor_occupied" value="0.9" /> <param name="map_update_distance_thresh" value="0.4"/> <param name="map_update_angle_thresh" value="0.06" /> <param name="laser_z_min_value" value = "-1.0" /> <param name="laser_z_max_value" value = "1.0" /> <!-- Advertising config --> <param name="advertise_map_service" value="true"/> <param name="scan_subscriber_queue_size" value="$(arg scan_subscriber_queue_size)"/> <param name="scan_topic" value="$(arg scan_topic)"/> <!-- Debug parameters --> <!-- <param name="output_timing" value="false"/> <param name="pub_drawings" value="true"/> <param name="pub_debug_output" value="true"/> --> <param name="tf_map_scanmatch_transform_frame_name" value="$(arg tf_map_scanmatch_transform_frame_name)" /> </node> <node pkg="tf" type="static_transform_publisher" name="map_to_camera_odom_broadcaster" args="0 0 0 0 0 0 map camera_odom_frame 100"/> <node pkg="tf" type="static_transform_publisher" name="base_to_camera_broadcaster" args="0 0.05 -0.10 3.1415/2 0 0 base_link camera_pose_frame 100"/> <node pkg="tf" type="static_transform_publisher" name="base_to_laser_broadcaster" args="0 0 0 0 0 0 base_link laser 100"/> </launch> Note the tf nodes launched at the end of the file. This launcher throws this error:
[ERROR] [1591184110.747014370]: Transform failed during publishing of map_odom transform: Could not find a connection between 'camera_odom_frame' and 'base_link' because they are not part of the same tree.Tf has two or more unconnected trees. And indeed, the tf tree is disconnected:
which wasn't before lauching it (camera_odom_frame and camera_pose_frame were connected). It seems that the tf between camera_odom_frame and camera_pose_frame is lost, although the camera package is still running and publishing the transformation:
--- transforms: - header: seq: 0 stamp: secs: 1591188682 nsecs: 892184019 frame_id: "camera_odom_frame" child_frame_id: "camera_pose_frame" transform: translation: x: -0.000112432426249 y: -8.70456551638e-06 z: -6.21268409304e-05 rotation: x: -9.97320312308e-05 y: 0.0114294840023 z: 7.35611829441e-05 w: 0.999934732914 --- As soon as I kill the hector_slam launched nodes, the transformation shows up again (as in the second picture of this post)
Does anybody know what is going on here??
gmapping
I have also tried gmapping, launched after launching the rplidar and the camera packages with the following launch file:
<launch> <arg name="scan_topic" default="scan" /> <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen"> <rosparam> odom_frame: camera_odom_frame base_frame: camera_pose_frame map_frame: map map_update_interval: 0.5 # Publish new map maxUrange: 5.5 # Should be just less than sensor range maxRange: 6.1 # Should be just greater than sensor range particles: 100 # Increased from 80 # Update frequencies linearUpdate: 0.3 angularUpdate: 0.5 temporalUpdate: 2.0 resampleThreshold: 0.5 # Initial Map Size xmin: -100.0 ymin: -100.0 xmax: 100.0 ymax: 100.0 delta: 0.05 # All default sigma: 0.05 kernelSize: 1 lstep: 0.05 astep: 0.05 iterations: 5 lsigma: 0.075 ogain: 3.0 lskip: 0 llsamplerange: 0.01 llsamplestep: 0.01 lasamplerange: 0.005 lasamplestep: 0.005 </rosparam> <remap from="scan" to="$(arg scan_topic)"/> </node> </launch> The tf tree seems to be ok:
However, the node trows the following message:
[DEBUG] [1591185126.577717647]: MessageFilter [target=camera_odom_frame ]: Added message in frame laser at time 1591185126.417, count now 5 [ WARN] [1591185126.729478752]: MessageFilter [target=camera_odom_frame ]: Dropped 100.00% of messages so far. Please turn the [ros.gmapping.message_filter] rosconsole logger to DEBUG for more information. [DEBUG] [1591185126.745662895]: MessageFilter [target=camera_odom_frame ]: Removed oldest message because buffer is full, count now 5 (frame_id=laser, stamp=1591185125.761198) [DEBUG] [1591185126.745822135]: MessageFilter [target=camera_odom_frame ]: Added message in frame laser at time 1591185126.577, count now 5 Any ideas how to make it work?
Thanks a lot!!
Originally posted by pgarcia on ROS Answers with karma: 13 on 2020-06-03
Post score: 1
Original comments
Comment by billy on 2020-06-03:
no access to your google drive to see TF trees - that may be an issue on my side but I've never run into before using these forums.
I upvoted the post so you may now have the karma needed to post TF trees directly.
Comment by pgarcia on 2020-06-03:
Great, thanks!