Hello!
I am trying to implement RTAB-Map on my Starling 2 drone using the rtabmap_drone_example, but I am facing some difficulties. Initially, I attempted to use all the sensors to create a good map, but it didn't work. So, I tried using only the ToF (Time of Flight) sensor and its PointCloud. I had to run RTAB-Map in a Docker container due to the numerous libraries that I can't use directly on the drone. ![]() First, I created a node that publishes the data for RTAB-Map with the correct information, taking data from the mpa_to_ros node published by the drone. This runs on my drone. mpa_to_ros_interface.py : #!/usr/bin/env python import rospy from sensor_msgs.msg import Imu, Image, CameraInfo, PointCloud2 from nav_msgs.msg import Odometry class MpaToRosInterface(object): def __init__(self): rospy.init_node('mpa_to_ros_interface') self.tof_pc_pub = rospy.Publisher('/drone/tof_pc/pc', PointCloud2, queue_size=1000) self.odometry_pub = rospy.Publisher('/drone/odometry', Odometry, queue_size=1000) rospy.Subscriber("/tof_pc", PointCloud2, self.tof_pc_callback) rospy.Subscriber("/qvio/odometry", Odometry, self.odometry_callback) def tof_pc_callback(self, data): data.header.frame_id = "tof_link" data.header.stamp = rospy.Time.now() self.tof_pc_pub.publish(data) def odometry_callback(self, data): data.header.frame_id = "odom" data.header.stamp = rospy.Time.now() self.odometry_pub.publish(data) def main(): mpa_to_ros_interface = MpaToRosInterface() rospy.spin() if __name__ == '__main__': main() mpa_to_ros_tf_boradcast.py : #!/usr/bin/env python import rospy import tf from nav_msgs.msg import Odometry def handle_odom(msg): br = tf.TransformBroadcaster() current_time = rospy.Time.now() # Extract the position and orientation from the odometry message position = msg.pose.pose.position orientation = msg.pose.pose.orientation # Broadcast the transform from odom to base_link br.sendTransform((position.x, position.y, position.z), (orientation.x, orientation.y, orientation.z, orientation.w), current_time, "odom", "map") def broadcast_static_transforms(): br = tf.TransformBroadcaster() current_time = rospy.Time.now() br.sendTransform((0.0, 0.0, 0.0), tf.transformations.quaternion_from_euler(0, 0, 0), current_time, "base_link", "odom") # Transform from base_link to imu_link br.sendTransform((0.0295, -0.0065, -0.016), tf.transformations.quaternion_from_euler(0, 0, 0), current_time, "imu_link", "base_link") # Transform from base_link to tof_link br.sendTransform((0.066, 0.009, -0.012), tf.transformations.quaternion_from_euler(0, 1.5708, 3.1416), current_time, "tof_link", "base_link") # Transform from base_link to hires_link br.sendTransform((0.066, 0.009, -0.012), tf.transformations.quaternion_from_euler(0, 1.5708, 3.1416), current_time, "hires_link", "base_link") # Transform from imu_link to tracking_front_link br.sendTransform((0.037, 0.000, 0.0006), tf.transformations.quaternion_from_euler(0, 1.5708, 1.5708), current_time, "tracking_front_link", "imu_link") # Transform from imu_link to tracking_down_link br.sendTransform((-0.08825, -0.0045, 0.00269), tf.transformations.quaternion_from_euler(0, 0, 3.1416), current_time, "tracking_down_link", "imu_link") def main(): rospy.init_node('mpa_to_ros_tf_broadcast') rospy.Subscriber('/drone/odometry', Odometry, handle_odom) rate = rospy.Rate(20.0) while not rospy.is_shutdown(): broadcast_static_transforms() rate.sleep() if __name__ == '__main__': try: main() except rospy.ROSInterruptException: pass mavros.launch : <?xml version="1.0"?> <launch> <node name="voxl_mpa_to_ros_node" type="voxl_mpa_to_ros_node" pkg="voxl_mpa_to_ros" output="screen" /> <node name="mpa_to_ros_interface" type="mpa_to_ros_interface.py" pkg="mpa_to_ros_interface" output="screen" /> <node name="mpa_to_ros_tf_broadcast" type="mpa_to_ros_tf_broadcast.py" pkg="mpa_to_ros_interface" output="screen" /> <arg name="fcu_url" default="udp://127.0.0.1:14551@:14551" /> <arg name="gcs_url" default="" /> <arg name="tgt_system" default="$(env PX4_SYS_ID)" /> <arg name="tgt_component" default="1" /> <arg name="log_output" default="screen" /> <arg name="fcu_protocol" default="v2.0" /> <arg name="respawn_mavros" default="false" /> <include file="$(find mavros)/launch/node.launch"> <arg name="pluginlists_yaml" value="$(find mavros)/launch/px4_pluginlists.yaml" /> <arg name="config_yaml" value="$(find mavros)/launch/px4_config.yaml" /> <arg name="fcu_url" value="$(arg fcu_url)" /> <arg name="gcs_url" value="$(arg gcs_url)" /> <arg name="tgt_system" value="$(arg tgt_system)" /> <arg name="tgt_component" value="$(arg tgt_component)" /> <arg name="log_output" value="$(arg log_output)" /> <arg name="fcu_protocol" value="$(arg fcu_protocol)" /> <arg name="respawn_mavros" default="$(arg respawn_mavros)" /> </include> </launch> And this is the launchfile to run rtabmap on docker : <?xml version="1.0"?> <launch> <arg name="localization" default="false"/> <arg name="rtabmap_viz" default="true"/> <arg name="ground_truth" default="false"/> <arg if="$(arg localization)" name="pre_args" default=""/> <arg unless="$(arg localization)" name="pre_args" default="-d"/> <remap from="/rtabmap/move_base" to="/move_base"/> <include file="$(find rtabmap_launch)/launch/rtabmap.launch"> <arg name="subscribe_rgbd" default="false"/> <arg name="depth" default="false"/> <arg name="subscribe_scan_cloud" default="true"/> <arg name="visual_odometry" default="false"/> <arg name="localization" value="$(arg localization)"/> <arg name="args" value="$(arg pre_args) --delete_db_on_start --Optimizer/GravitySigma 0.1 --Vis/FeatureType 10 --Kp/DetectorStrategy 10 --Grid/FromDepth false --Grid/MapFrameProjection true --NormalsSegmentation false --Grid/MaxGroundHeight 0.2 --Grid/MaxObstacleHeight 2 --Grid/3D true --Grid/Sensor 0 --Grid/RangeMax 0"/> <arg name="rtabmap_viz" value="$(arg rtabmap_viz)"/> <arg name="frame_id" value="base_link"/> <arg name="odom_frame_id" value="odom"/> <arg name="publish_tf_odom" default="false"/> <arg name="odom_topic" value="/drone/odometry"/> <arg name="scan_cloud_topic" value="/drone/tof_pc/pc"/> <arg name="imu_topic" value="/drone/imu/data"/> <arg name="wait_imu_to_init" value="true"/> <arg name="approx_sync" value="true"/> <arg name="approx_sync_max_interval" value="0.03"/> <arg if="$(arg ground_truth)" name="ground_truth_frame_id" value="world"/> <arg if="$(arg ground_truth)" name="ground_truth_base_frame_id" value="base_link_gt"/> <arg name="sync_queue_size" default="100"/> </include> <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen"> <remap from="map" to="/rtabmap/grid_map"/> <remap from="odom" to="/drone/odometry"/> <remap from="camera_cloud" to="/drone/tof_pc/pc"/> <rosparam file="$(find rtabmap_drone)/param/costmap_common_params.yaml" command="load" ns="global_costmap"/> <rosparam file="$(find rtabmap_drone)/param/costmap_common_params.yaml" command="load" ns="local_costmap"/> <rosparam file="$(find rtabmap_drone)/param/global_costmap_params.yaml" command="load"/> <rosparam file="$(find rtabmap_drone)/param/local_costmap_params.yaml" command="load"/> <rosparam file="$(find rtabmap_drone)/param/base_local_planner_params.yaml" command="load"/> </node> <node name="empty_voxels_markers" pkg="rtabmap_costmap_plugins" type="voxel_markers" args="voxel_grid:=/move_base/local_costmap/voxel_layer/voxel_grid visualization_marker:=/voxels_empty"> </node> <node name="marked_voxels_markers" pkg="rtabmap_costmap_plugins" type="voxel_markers" args="voxel_grid:=/move_base/local_costmap/voxel_layer/voxel_grid visualization_marker:=/voxels_marked" /> </launch> My nodes on the drone seem to work well, but when I launch RTAB-Map, I get these messages: voxl2:~$ ./rtabmap_launch.sh ... logging to /root/.ros/log/23764bba-48ea-11ef-a0bf-00c0cab3a54a/roslaunch-m0054-1.log Checking log directory for disk usage. This may take a while. Press Ctrl-C to interrupt Done checking log file disk usage. Usage is <1GB. started roslaunch server http://m0054:43045/ SUMMARY ======== CLEAR PARAMETERS * /rtabmap/rtabmap/ * /rtabmap/rtabmap_viz/ PARAMETERS * /empty_voxels_markers/cell_type: 0 * /move_base/DWAPlannerROS/acc_lim_theta: 3 * /move_base/DWAPlannerROS/acc_lim_trans: 1.5 * /move_base/DWAPlannerROS/acc_lim_x: 1.5 * /move_base/DWAPlannerROS/acc_lim_y: 1.5 * /move_base/DWAPlannerROS/forward_point_distance: 0.1 * /move_base/DWAPlannerROS/global_frame_id: odom * /move_base/DWAPlannerROS/goal_distance_bias: 10 * /move_base/DWAPlannerROS/latch_xy_goal_tolerance: False * /move_base/DWAPlannerROS/max_scaling_factor: 0.2 * /move_base/DWAPlannerROS/max_vel_theta: 1 * /move_base/DWAPlannerROS/max_vel_trans: 0.2 * /move_base/DWAPlannerROS/max_vel_x: 0.2 * /move_base/DWAPlannerROS/max_vel_y: 0.1 * /move_base/DWAPlannerROS/min_vel_theta: 0 * /move_base/DWAPlannerROS/min_vel_trans: 0.01 * /move_base/DWAPlannerROS/min_vel_x: -0.01 * /move_base/DWAPlannerROS/min_vel_y: -0.1 * /move_base/DWAPlannerROS/occdist_scale: 0.08 * /move_base/DWAPlannerROS/oscillation_reset_dist: 0.05 * /move_base/DWAPlannerROS/path_distance_bias: 64 * /move_base/DWAPlannerROS/publish_cost_grid_pc: True * /move_base/DWAPlannerROS/publish_traj_pc: True * /move_base/DWAPlannerROS/scaling_speed: 0.25 * /move_base/DWAPlannerROS/sim_time: 4 * /move_base/DWAPlannerROS/stop_time_buffer: 0.2 * /move_base/DWAPlannerROS/theta_stopped_vel: 0.01 * /move_base/DWAPlannerROS/trans_stopped_vel: 0.01 * /move_base/DWAPlannerROS/vth_samples: 20 * /move_base/DWAPlannerROS/vx_samples: 6 * /move_base/DWAPlannerROS/vy_samples: 6 * /move_base/DWAPlannerROS/xy_goal_tolerance: 0.3 * /move_base/DWAPlannerROS/yaw_goal_tolerance: 0.5 * /move_base/NavfnROS/allow_unknown: True * /move_base/base_local_planner: dwa_local_planner... * /move_base/controller_frequency: 10.0 * /move_base/global_costmap/controller_patience: 2.0 * /move_base/global_costmap/cost_scaling_factor: 1.5 * /move_base/global_costmap/footprint: [[0.16, 0.16], [-... * /move_base/global_costmap/footprint_padding: 0.05 * /move_base/global_costmap/global_frame: map * /move_base/global_costmap/inflation_layer/inflate_unkown: True * /move_base/global_costmap/inflation_radius: 1.5 * /move_base/global_costmap/obstacle_range: 3.0 * /move_base/global_costmap/plugins: [{'name': 'static... * /move_base/global_costmap/publish_frequency: 1 * /move_base/global_costmap/raytrace_range: 3.5 * /move_base/global_costmap/robot_base_frame: base_link * /move_base/global_costmap/robot_radius: 0.32 * /move_base/global_costmap/static_map: True * /move_base/global_costmap/transform_tolerance: 1 * /move_base/global_costmap/update_frequency: 1 * /move_base/local_costmap/controller_patience: 2.0 * /move_base/local_costmap/cost_scaling_factor: 1.5 * /move_base/local_costmap/footprint: [[0.16, 0.16], [-... * /move_base/local_costmap/footprint_padding: 0.05 * /move_base/local_costmap/global_frame: map * /move_base/local_costmap/height: 4 * /move_base/local_costmap/inflation_radius: 1.5 * /move_base/local_costmap/obstacle_range: 3.0 * /move_base/local_costmap/plugins: [{'name': 'voxel_... * /move_base/local_costmap/publish_frequency: 2.0 * /move_base/local_costmap/raytrace_range: 3.5 * /move_base/local_costmap/resolution: 0.05 * /move_base/local_costmap/robot_base_frame: base_link * /move_base/local_costmap/robot_radius: 0.32 * /move_base/local_costmap/rolling_window: True * /move_base/local_costmap/static_map: False * /move_base/local_costmap/transform_tolerance: 0.5 * /move_base/local_costmap/update_frequency: 5.0 * /move_base/local_costmap/voxel_layer/3d_cloud/clearing: True * /move_base/local_costmap/voxel_layer/3d_cloud/data_type: PointCloud2 * /move_base/local_costmap/voxel_layer/3d_cloud/marking: True * /move_base/local_costmap/voxel_layer/3d_cloud/max_obstacle_height: 1000 * /move_base/local_costmap/voxel_layer/3d_cloud/min_obstacle_height: -1000 * /move_base/local_costmap/voxel_layer/3d_cloud/topic: /camera_cloud * /move_base/local_costmap/voxel_layer/enabled: True * /move_base/local_costmap/voxel_layer/footprint_clearing_enabled: False * /move_base/local_costmap/voxel_layer/observation_sources: 3d_cloud * /move_base/local_costmap/voxel_layer/publish_voxel_map: True * /move_base/local_costmap/voxel_layer/z_resolution: 0.05 * /move_base/local_costmap/voxel_layer/z_voxels: 9 * /move_base/local_costmap/width: 4 * /move_base/planner_frequency: 1 * /rosdistro: noetic * /rosversion: 1.16.0 * /rtabmap/rtabmap/Mem/IncrementalMemory: true * /rtabmap/rtabmap/Mem/InitWMWithAllNodes: false * /rtabmap/rtabmap/approx_sync: True * /rtabmap/rtabmap/config_path: * /rtabmap/rtabmap/database_path: ~/.ros/rtabmap.db * /rtabmap/rtabmap/frame_id: base_link * /rtabmap/rtabmap/gen_depth: False * /rtabmap/rtabmap/gen_depth_decimation: 1 * /rtabmap/rtabmap/gen_depth_fill_holes_error: 0.1 * /rtabmap/rtabmap/gen_depth_fill_holes_size: 0 * /rtabmap/rtabmap/gen_depth_fill_iterations: 1 * /rtabmap/rtabmap/gen_scan: False * /rtabmap/rtabmap/ground_truth_base_frame_id: * /rtabmap/rtabmap/ground_truth_frame_id: * /rtabmap/rtabmap/initial_pose: * /rtabmap/rtabmap/landmark_angular_variance: 9999.0 * /rtabmap/rtabmap/landmark_linear_variance: 0.0001 * /rtabmap/rtabmap/latch: False * /rtabmap/rtabmap/loc_thr: 0.0 * /rtabmap/rtabmap/map_frame_id: map * /rtabmap/rtabmap/odom_frame_id: odom * /rtabmap/rtabmap/odom_frame_id_init: * /rtabmap/rtabmap/odom_sensor_sync: False * /rtabmap/rtabmap/odom_tf_angular_variance: 0.001 * /rtabmap/rtabmap/odom_tf_linear_variance: 0.001 * /rtabmap/rtabmap/publish_tf: True * /rtabmap/rtabmap/scan_cloud_max_points: 0 * /rtabmap/rtabmap/subscribe_depth: False * /rtabmap/rtabmap/subscribe_rgb: False * /rtabmap/rtabmap/subscribe_rgbd: False * /rtabmap/rtabmap/subscribe_scan: False * /rtabmap/rtabmap/subscribe_scan_cloud: True * /rtabmap/rtabmap/subscribe_scan_descriptor: False * /rtabmap/rtabmap/subscribe_stereo: False * /rtabmap/rtabmap/subscribe_user_data: False * /rtabmap/rtabmap/sync_queue_size: 100 * /rtabmap/rtabmap/topic_queue_size: 1 * /rtabmap/rtabmap/use_action_for_goal: True * /rtabmap/rtabmap/wait_for_transform_duration: 0.2 * /rtabmap/rtabmap_viz/approx_sync: True * /rtabmap/rtabmap_viz/frame_id: base_link * /rtabmap/rtabmap_viz/odom_frame_id: odom * /rtabmap/rtabmap_viz/subscribe_depth: False * /rtabmap/rtabmap_viz/subscribe_rgb: False * /rtabmap/rtabmap_viz/subscribe_rgbd: False * /rtabmap/rtabmap_viz/subscribe_scan: False * /rtabmap/rtabmap_viz/subscribe_scan_cloud: True * /rtabmap/rtabmap_viz/subscribe_scan_descriptor: False * /rtabmap/rtabmap_viz/subscribe_stereo: False * /rtabmap/rtabmap_viz/sync_queue_size: 100 * /rtabmap/rtabmap_viz/topic_queue_size: 1 * /rtabmap/rtabmap_viz/wait_for_transform_duration: 0.2 NODES / empty_voxels_markers (rtabmap_costmap_plugins/voxel_markers) marked_voxels_markers (rtabmap_costmap_plugins/voxel_markers) move_base (move_base/move_base) /rtabmap/ rtabmap (rtabmap_slam/rtabmap) rtabmap_viz (rtabmap_viz/rtabmap_viz) ROS_MASTER_URI=http://localhost:11311 process[rtabmap/rtabmap-1]: started with pid [50] process[rtabmap/rtabmap_viz-2]: started with pid [51] process[move_base-3]: started with pid [52] process[empty_voxels_markers-4]: started with pid [53] process[marked_voxels_markers-5]: started with pid [54] [ WARN] [1721735598.473160994]: global_costmap: Pre-Hydro parameter "static_map" unused since "plugins" is provided [ INFO] [1721735598.475875560]: global_costmap: Using plugin "static_layer" [ INFO] [1721735598.490125923]: Requesting the map... [ INFO] [1721735598.891798017]: Starting node... [ INFO] [1721735598.931406220]: Initializing nodelet with 8 worker threads. [ INFO] [1721735598.972168456]: Starting node... [ INFO] [1721735599.143035720]: /rtabmap/rtabmap(maps): map_filter_radius = 0.000000 [ INFO] [1721735599.143094104]: /rtabmap/rtabmap(maps): map_filter_angle = 30.000000 [ INFO] [1721735599.143114052]: /rtabmap/rtabmap(maps): map_cleanup = true [ INFO] [1721735599.143124937]: /rtabmap/rtabmap(maps): map_always_update = false [ INFO] [1721735599.143141238]: /rtabmap/rtabmap(maps): map_empty_ray_tracing = true [ INFO] [1721735599.143164207]: /rtabmap/rtabmap(maps): cloud_output_voxelized = true [ INFO] [1721735599.143192852]: /rtabmap/rtabmap(maps): cloud_subtract_filtering = false [ INFO] [1721735599.143204622]: /rtabmap/rtabmap(maps): cloud_subtract_filtering_min_neighbors = 2 [ INFO] [1721735599.143438054]: /rtabmap/rtabmap(maps): octomap_tree_depth = 16 [ INFO] [1721735599.165719675]: rtabmap: frame_id = base_link [ INFO] [1721735599.165766288]: rtabmap: odom_frame_id = odom [ INFO] [1721735599.166146956]: rtabmap: map_frame_id = map [ INFO] [1721735599.166185497]: rtabmap: log_to_rosout_level = 4 [ INFO] [1721735599.166213933]: rtabmap: initial_pose = [ INFO] [1721735599.166227527]: rtabmap: use_action_for_goal = true [ INFO] [1721735599.166248932]: rtabmap: tf_delay = 0.050000 [ INFO] [1721735599.166263515]: rtabmap: tf_tolerance = 0.100000 [ INFO] [1721735599.166278984]: rtabmap: odom_sensor_sync = false [ INFO] [1721735599.166294452]: rtabmap: pub_loc_pose_only_when_localizing = false [ INFO] [1721735599.167077922]: rtabmap: gen_scan = false [ INFO] [1721735599.167112244]: rtabmap: gen_depth = false [ INFO] [1721735599.167549681]: rtabmap: scan_cloud_max_points = 0 [ INFO] [1721735599.167576763]: rtabmap: scan_cloud_is_2d = false [ INFO] [1721735599.355361574]: Setting RTAB-Map parameter "Mem/IncrementalMemory"="true" [ INFO] [1721735599.355736825]: Setting RTAB-Map parameter "Mem/InitWMWithAllNodes"="false" [ WARN] (2024-07-23 11:53:19.570) Parameters.cpp:1161::parseArguments() Parameter migration from "Grid/FromDepth" to "Grid/Sensor" (value=false). [ INFO] [1721735599.570785526]: Update RTAB-Map parameter "Grid/3D"="true" from arguments [ INFO] [1721735599.570828338]: Update RTAB-Map parameter "Grid/MapFrameProjection"="true" from arguments [ INFO] [1721735599.570854639]: Update RTAB-Map parameter "Grid/MaxGroundHeight"="0.2" from arguments [ INFO] [1721735599.570881878]: Update RTAB-Map parameter "Grid/MaxObstacleHeight"="2" from arguments [ INFO] [1721735599.570895055]: Update RTAB-Map parameter "Grid/RangeMax"="0" from arguments [ INFO] [1721735599.570905315]: Update RTAB-Map parameter "Grid/Sensor"="0" from arguments [ INFO] [1721735599.570913440]: Update RTAB-Map parameter "Kp/DetectorStrategy"="10" from arguments [ INFO] [1721735599.570922554]: Update RTAB-Map parameter "Optimizer/GravitySigma"="0.1" from arguments [ INFO] [1721735599.570935783]: Update RTAB-Map parameter "Vis/FeatureType"="10" from arguments [ INFO] [1721735599.721372272]: Setting "Icp/PointToPlaneRadius" parameter to 0 (default 0.000000) as "subscribe_scan_cloud" is true. [ INFO] [1721735599.722403236]: RTAB-Map detection rate = 1.000000 Hz [ INFO] [1721735599.722782393]: rtabmap: Using database from "/root/.ros/rtabmap.db" (0 MB). [ INFO] [1721735600.217641220]: rtabmap: Database version = "0.21.5". [ INFO] [1721735600.218029075]: rtabmap: SLAM mode (Mem/IncrementalMemory=true) QStandardPaths: XDG_RUNTIME_DIR not set, defaulting to '/tmp/runtime-root' [ INFO] [1721735600.238467518]: /rtabmap/rtabmap: subscribe_depth = false [ INFO] [1721735600.238526788]: /rtabmap/rtabmap: subscribe_rgb = false [ INFO] [1721735600.238547673]: /rtabmap/rtabmap: subscribe_stereo = false [ INFO] [1721735600.238561162]: /rtabmap/rtabmap: subscribe_rgbd = false (rgbd_cameras=1) [ INFO] [1721735600.238576630]: /rtabmap/rtabmap: subscribe_sensor_data = false [ INFO] [1721735600.238606890]: /rtabmap/rtabmap: subscribe_odom_info = false [ INFO] [1721735600.238620431]: /rtabmap/rtabmap: subscribe_user_data = false [ INFO] [1721735600.238633608]: /rtabmap/rtabmap: subscribe_scan = false [ INFO] [1721735600.238651680]: /rtabmap/rtabmap: subscribe_scan_cloud = true [ INFO] [1721735600.238667670]: /rtabmap/rtabmap: subscribe_scan_descriptor = false [ INFO] [1721735600.238677669]: /rtabmap/rtabmap: topic_queue_size = 1 [ INFO] [1721735600.238693867]: /rtabmap/rtabmap: sync_queue_size = 100 [ INFO] [1721735600.238872821]: /rtabmap/rtabmap: approx_sync = true [ INFO] [1721735600.241093077]: Setup scan callback [ INFO] [1721735600.250247690]: /rtabmap/rtabmap subscribed to: /drone/tof_pc/pc [ WARN] [1721735600.253312978]: There is no image subscription, bag-of-words loop closure detection will be disabled... [ WARN] [1721735600.253397194]: Setting Kp/MaxFeatures=-1 (bag-of-words disabled) [ WARN] [1721735600.253422298]: Setting Reg/Strategy=1 (ICP) [ WARN] [1721735600.253440318]: Setting "RGBD/ProximityPathMaxNeighbors" parameter to 1 (default 0) as "subscribe_scan_cloud" is true and "Reg/Strategy" uses ICP. To disable, set "RGBD/ProximityPathMaxNeighbors" to 0. To suppress this warning, add [ INFO] [1721735600.296892818]: rtabmap_viz: Using configuration from "/root/.ros/rtabmap_gui.ini" [ INFO] [1721735600.422576840]: rtabmap 0.21.5 started... [ WARN] [1721735600.515297370]: Grid map is empty! (local maps=1) [ INFO] [1721735600.516336979]: rtabmap (1): Rate=1.00s, Limit=0.000s, Conversion=0.0306s, RTAB-Map=0.0904s, Maps update=0.0018s pub=0.0012s (local map=1, WM=1) [ WARN] [1721735601.472010518]: Grid map is empty! (local maps=1) [ INFO] [1721735601.472310094]: rtabmap (2): Rate=1.00s, Limit=0.000s, Conversion=0.0290s, RTAB-Map=0.0461s, Maps update=0.0003s pub=0.0004s (local map=1, WM=1) [ WARN] [1721735602.548561794]: Grid map is empty! (local maps=1) [ INFO] [1721735602.549918948]: rtabmap (3): Rate=1.00s, Limit=0.000s, Conversion=0.0341s, RTAB-Map=0.0235s, Maps update=0.0032s pub=0.0014s (local map=1, WM=1) [ WARN] [1721735603.562147317]: Grid map is empty! (local maps=1) [ INFO] [1721735603.562502933]: rtabmap (4): Rate=1.00s, Limit=0.000s, Conversion=0.0285s, RTAB-Map=0.0382s, Maps update=0.0012s pub=0.0004s (local map=1, WM=1) libpng warning: iCCP: known incorrect sRGB profile libpng warning: iCCP: known incorrect sRGB profile libpng warning: iCCP: known incorrect sRGB profile [ WARN] [1721735604.647968496]: Grid map is empty! (local maps=1) [ INFO] [1721735604.648631345]: rtabmap (5): Rate=1.00s, Limit=0.000s, Conversion=0.0230s, RTAB-Map=0.0279s, Maps update=0.0010s pub=0.0007s (local map=1, WM=1) [ WARN] [1721735605.750366819]: Grid map is empty! (local maps=1) [ INFO] [1721735605.750833110]: rtabmap (6): Rate=1.00s, Limit=0.000s, Conversion=0.0313s, RTAB-Map=0.0297s, Maps update=0.0003s pub=0.0006s (local map=1, WM=1) [ INFO] [1721735606.309773620]: rtabmap_viz: Reading parameters from the ROS server... [ INFO] [1721735606.669859919]: rtabmap_viz: Parameters read = 377 [ INFO] [1721735606.669924761]: rtabmap_viz: Parameters successfully read. [ WARN] [1721735606.848397740]: Grid map is empty! (local maps=1) [ INFO] [1721735606.848831011]: rtabmap (7): Rate=1.00s, Limit=0.000s, Conversion=0.0299s, RTAB-Map=0.0260s, Maps update=0.0021s pub=0.0005s (local map=1, WM=1) [ INFO] [1721735607.632474048]: /rtabmap/rtabmap_viz: subscribe_depth = false [ INFO] [1721735607.632528786]: /rtabmap/rtabmap_viz: subscribe_rgb = false [ INFO] [1721735607.632548733]: /rtabmap/rtabmap_viz: subscribe_stereo = false [ INFO] [1721735607.632567743]: /rtabmap/rtabmap_viz: subscribe_rgbd = false (rgbd_cameras=1) [ INFO] [1721735607.632585139]: /rtabmap/rtabmap_viz: subscribe_sensor_data = false [ INFO] [1721735607.632601805]: /rtabmap/rtabmap_viz: subscribe_odom_info = false [ INFO] [1721735607.632619982]: /rtabmap/rtabmap_viz: subscribe_user_data = false [ INFO] [1721735607.632637950]: /rtabmap/rtabmap_viz: subscribe_scan = false [ INFO] [1721735607.632653887]: /rtabmap/rtabmap_viz: subscribe_scan_cloud = true [ INFO] [1721735607.632685397]: /rtabmap/rtabmap_viz: subscribe_scan_descriptor = false [ INFO] [1721735607.632873153]: /rtabmap/rtabmap_viz: topic_queue_size = 1 [ INFO] [1721735607.633012056]: /rtabmap/rtabmap_viz: sync_queue_size = 100 [ INFO] [1721735607.633132730]: /rtabmap/rtabmap_viz: approx_sync = true [ INFO] [1721735607.633300955]: Setup scan callback [ INFO] [1721735607.635796417]: /rtabmap/rtabmap_viz subscribed to: /drone/tof_pc/pc [ INFO] [1721735607.637549605]: rtabmap_viz started. [ WARN] [1721735607.842693916]: Grid map is empty! (local maps=1) [ INFO] [1721735607.843286871]: rtabmap (8): Rate=1.00s, Limit=0.000s, Conversion=0.0225s, RTAB-Map=0.0241s, Maps update=0.0010s pub=0.0007s (local map=1, WM=1) [ WARN] [1721735608.844783757]: Grid map is empty! (local maps=1) [ INFO] [1721735608.850908874]: rtabmap (9): Rate=1.00s, Limit=0.000s, Conversion=0.0185s, RTAB-Map=0.0252s, Maps update=0.0007s pub=0.0061s (local map=1, WM=1) [ WARN] [1721735609.898011464]: Grid map is empty! (local maps=1) [ INFO] [1721735609.899019253]: rtabmap (10): Rate=1.00s, Limit=0.000s, Conversion=0.0215s, RTAB-Map=0.0725s, Maps update=0.0011s pub=0.0010s (local map=1, WM=1) [ WARN] [1721735610.950040302]: Grid map is empty! (local maps=1) [ INFO] [1721735610.952081609]: rtabmap (11): Rate=1.00s, Limit=0.000s, Conversion=0.0518s, RTAB-Map=0.0300s, Maps update=0.0004s pub=0.0017s (local map=1, WM=1) [ INFO] [1721735610.952485350]: rtabmap: paused! [ INFO] [1721735610.960550060]: rtabmap: resumed! [ WARN] (2024-07-23 11:53:31.961) Rtabmap.cpp:4560::process() Republishing data of requested node(s) 1 (Rtabmap/MaxRepublished=2) [ WARN] [1721735611.966008096]: Grid map is empty! (local maps=1) [ INFO] [1721735611.967449834]: rtabmap (12): Rate=1.00s, Limit=0.000s, Conversion=0.0367s, RTAB-Map=0.0405s, Maps update=0.0002s pub=0.0012s (local map=1, WM=1) [ WARN] [1721735612.949552396]: Grid map is empty! (local maps=1) [ INFO] [1721735612.951225952]: rtabmap (13): Rate=1.00s, Limit=0.000s, Conversion=0.0286s, RTAB-Map=0.0289s, Maps update=0.0010s pub=0.0016s (local map=1, WM=1) [ WARN] [1721735613.950026080]: Grid map is empty! (local maps=1) [ INFO] [1721735613.950752314]: rtabmap (14): Rate=1.00s, Limit=0.000s, Conversion=0.0291s, RTAB-Map=0.0285s, Maps update=0.0003s pub=0.0009s (local map=1, WM=1) [ WARN] [1721735615.096640665]: Grid map is empty! (local maps=1) [ INFO] [1721735615.097455595]: rtabmap (15): Rate=1.00s, Limit=0.000s, Conversion=0.0165s, RTAB-Map=0.0729s, Maps update=0.0012s pub=0.0009s (local map=1, WM=1) [ WARN] [1721735616.147982481]: Grid map is empty! (local maps=1) [ INFO] [1721735616.149015115]: rtabmap (16): Rate=1.00s, Limit=0.000s, Conversion=0.0268s, RTAB-Map=0.0270s, Maps update=0.0018s pub=0.0011s (local map=1, WM=1) [ WARN] [1721735617.266636259]: Grid map is empty! (local maps=1) [ INFO] [1721735617.267710715]: rtabmap (17): Rate=1.00s, Limit=0.000s, Conversion=0.0275s, RTAB-Map=0.0418s, Maps update=0.0003s pub=0.0011s (local map=1, WM=1) [ WARN] [1721735618.350049035]: Grid map is empty! (local maps=1) [ INFO] [1721735618.350799280]: rtabmap (18): Rate=1.00s, Limit=0.000s, Conversion=0.0273s, RTAB-Map=0.0287s, Maps update=0.0013s pub=0.0008s (local map=1, WM=1) [ WARN] [1721735619.451121010]: Grid map is empty! (local maps=1) [ INFO] [1721735619.452286662]: rtabmap (19): Rate=1.00s, Limit=0.000s, Conversion=0.0429s, RTAB-Map=0.0294s, Maps update=0.0009s pub=0.0012s (local map=1, WM=1) [ WARN] [1721735620.458261696]: Grid map is empty! (local maps=1) [ INFO] [1721735620.459339225]: rtabmap (20): Rate=1.00s, Limit=0.000s, Conversion=0.0156s, RTAB-Map=0.0363s, Maps update=0.0010s pub=0.0011s (local map=1, WM=1) [ WARN] [1721735621.551124077]: Grid map is empty! (local maps=1) [ INFO] [1721735621.552487642]: rtabmap (21): Rate=1.00s, Limit=0.000s, Conversion=0.0438s, RTAB-Map=0.0307s, Maps update=0.0009s pub=0.0014s (local map=1, WM=1) [ WARN] [1721735622.544289416]: Grid map is empty! (local maps=1) [ INFO] [1721735622.545402622]: rtabmap (22): Rate=1.00s, Limit=0.000s, Conversion=0.0229s, RTAB-Map=0.0232s, Maps update=0.0009s pub=0.0012s (local map=1, WM=1) [ WARN] [1721735623.549656693]: Grid map is empty! (local maps=1) [ INFO] [1721735623.551814409]: rtabmap (23): Rate=1.00s, Limit=0.000s, Conversion=0.0121s, RTAB-Map=0.0262s, Maps update=0.0022s pub=0.0021s (local map=1, WM=1) [ WARN] [1721735624.645383310]: Grid map is empty! (local maps=1) [ INFO] [1721735624.646384644]: rtabmap (24): Rate=1.00s, Limit=0.000s, Conversion=0.0211s, RTAB-Map=0.0254s, Maps update=0.0002s pub=0.0010s (local map=1, WM=1) [ WARN] [1721735625.749224114]: Grid map is empty! (local maps=1) [ INFO] [1721735625.750026598]: rtabmap (25): Rate=1.00s, Limit=0.000s, Conversion=0.0098s, RTAB-Map=0.0265s, Maps update=0.0009s pub=0.0009s (local map=1, WM=1) [ WARN] [1721735626.858550931]: Grid map is empty! (local maps=1) [ INFO] [1721735626.859247115]: rtabmap (26): Rate=1.00s, Limit=0.000s, Conversion=0.0460s, RTAB-Map=0.0307s, Maps update=0.0010s pub=0.0007s (local map=1, WM=1) [ WARN] [1721735627.996090071]: Grid map is empty! (local maps=1) [ INFO] [1721735627.997219424]: rtabmap (27): Rate=1.00s, Limit=0.000s, Conversion=0.0378s, RTAB-Map=0.0721s, Maps update=0.0002s pub=0.0018s (local map=1, WM=1) [ WARN] [1721735628.947040165]: Grid map is empty! (local maps=1) [ INFO] [1721735628.948890076]: rtabmap (28): Rate=1.00s, Limit=0.000s, Conversion=0.0149s, RTAB-Map=0.0246s, Maps update=0.0011s pub=0.0019s (local map=1, WM=1) [ WARN] [1721735630.049006981]: Grid map is empty! (local maps=1) [ INFO] [1721735630.050534816]: rtabmap (29): Rate=1.00s, Limit=0.000s, Conversion=0.0229s, RTAB-Map=0.0262s, Maps update=0.0012s pub=0.0015s (local map=1, WM=1) [ WARN] [1721735631.057318728]: Grid map is empty! (local maps=1) [ INFO] [1721735631.057941581]: rtabmap (30): Rate=1.00s, Limit=0.000s, Conversion=0.0165s, RTAB-Map=0.0304s, Maps update=0.0011s pub=0.0007s (local map=1, WM=1) [ WARN] [1721735632.145360945]: Grid map is empty! (local maps=1) [ INFO] [1721735632.149450605]: rtabmap (31): Rate=1.00s, Limit=0.000s, Conversion=0.0142s, RTAB-Map=0.0251s, Maps update=0.0008s pub=0.0039s (local map=1, WM=1) [ WARN] [1721735633.247327848]: Grid map is empty! (local maps=1) [ INFO] [1721735633.248003773]: rtabmap (32): Rate=1.00s, Limit=0.000s, Conversion=0.0147s, RTAB-Map=0.0250s, Maps update=0.0019s pub=0.0007s (local map=1, WM=1) [ WARN] [1721735634.243701418]: Grid map is empty! (local maps=1) [ INFO] [1721735634.244881656]: rtabmap (33): Rate=1.00s, Limit=0.000s, Conversion=0.0076s, RTAB-Map=0.0246s, Maps update=0.0019s pub=0.0012s (local map=1, WM=1) [ WARN] [1721735635.353711814]: Grid map is empty! (local maps=1) [ INFO] [1721735635.353909049]: rtabmap (34): Rate=1.00s, Limit=0.000s, Conversion=0.0213s, RTAB-Map=0.0321s, Maps update=0.0008s pub=0.0002s (local map=1, WM=1) [ WARN] [1721735636.447681163]: Grid map is empty! (local maps=1) [ INFO] [1721735636.448976607]: rtabmap (35): Rate=1.00s, Limit=0.000s, Conversion=0.0079s, RTAB-Map=0.0252s, Maps update=0.0024s pub=0.0011s (local map=1, WM=1) [ WARN] [1721735637.548408196]: Grid map is empty! (local maps=1) [ INFO] [1721735637.563895926]: rtabmap (36): Rate=1.00s, Limit=0.000s, Conversion=0.0186s, RTAB-Map=0.0251s, Maps update=0.0030s pub=0.0156s (local map=1, WM=1) [ WARN] [1721735638.545292058]: Grid map is empty! (local maps=1) [ INFO] [1721735638.546361569]: rtabmap (37): Rate=1.00s, Limit=0.000s, Conversion=0.0181s, RTAB-Map=0.0250s, Maps update=0.0010s pub=0.0010s (local map=1, WM=1) Does anyone know where the problem might be coming from? My frames and graph seem to be good, but maybe I missed something. ![]() Thanks for your help! |
Hre is what I see on rtabmap viz
![]() And the tf tree : ![]() |
Administrator
|
Hi,
You may add RGBD/CreateOccupancyGrid to false if you currently test just the pose (that will remove some warnings you see). In that trial, was the drone moving? If so, you may check that the odom TF you are publishing is actually changing. Note also that when you republish the odom TF from the odometry topic, you should use the timestamp from the topic header, not doing another ros::Time::now(). To forward back the pose corrected by rtabmap, you may look at this example where we get at 50 Hz the latest transform between map->base_link and provide it as vision_pose for mavros. cheers, Mathieu |
This post was updated on .
Hi there,
Thanks for your reply. Sorry, I was not clear enough but the thing I want to do is to SLAM only with PointCloud that my tof sensor send me, and with odom that my drone send me. I fixed the problem you mentioned and now my frame moves well with my drone. But now I still don't have a good map. In fact, the map is created on a frame and then doesn't move. Do you know where this problem could be coming from? What information do you need to help me? Thank you very much, Matthéo |
Administrator
|
Hi,
Is odom->base_link TF moving in RVIZ? If you set global frame to odom in RVIZ, and show point cloud of your sensor, is it moving correctly accordingly to odom frame? |
Hi,
Actually I made the map -> odom frame move. But when I corrected this, my map appeared! I think I didn't understand well how the frames work and as I don't use rtabmap-odom, this was not done by rtabmap. Thanks for your answer, I will come back to you if I have another problem. |
Free forum by Nabble | Edit this page |