introlab / rtabmap_ros

RTAB-Map's ROS package.
http://wiki.ros.org/rtabmap_ros
BSD 3-Clause "New" or "Revised" License
1.01k stars 558 forks source link

My rtabmap has no image, and the LaserScan is broken in rviz. #1139

Open smile6023326 opened 8 months ago

smile6023326 commented 8 months ago

Hello, hope you can help me, my rtabmap using D435i camera has no image my_rtabmap.launch: my_rtabmap.txt How can I make the image appear normally? Looking forward to your reply. Thank you. 螢幕擷取畫面 2024-03-28 210336

My D435i.launch D435i.txt

In addition, when I execute my_rtabmap.launch, a warning will appear. I guess this warning is a LaserScan conversion problem in rviz. I don’t know how to solve it.I use 2D lidar. Warnings from my_rtabmap.launch execution [ WARN] [1711639210.999439868]: Timed out waiting for transform from base_link to map to become available before running costmap, tf error: Lookup would require extrapolation 21.983554518s into the past. Requested time 1711639211.094828367 but the earliest data is at time 1711639233.078382730, when looking up transform from frame [base_link] to frame [map]. canTransform returned after 0.101144 timeout was 0.1. LaserScan error from rviz : Transform [sender=unknown_publisher] For frame [base_link]: No transform to fixed frame [map]. TF error: [Lookup would require extrapolation 30.758779551s into the future. Requested time 1711639594.517050028 but the latest data is at time 1711639563.758270502, when looking up transform from frame [base_link] to frame [map]] 螢幕擷取畫面 2024-03-28 234104

my_move_base.launch move_base.txt

tf tree 螢幕擷取畫面 2024-03-28 232003

I have referred to many articles and roswiki but failed. I hope you can help me solve it. Thank you again.

matlabbe commented 8 months ago

You need to set subscribe_rgbd to true to input images to rtabmap.

For the 30.758779551s into the future errors, there are timestamps not in sync with the host computer or between the sensors. Compare the stamps in the camera topics and laser scan topics to see if there is a huge difference.

smile6023326 commented 8 months ago

Thank you for your reply Replenish: This is used with reference to http://wiki.ros.org/rtabmap_ros/Tutorials/MappingAndNavigationOnTurtlebot roslaunch rtabmap_demos demo_turtlebot3_navigation.launch. My research purpose is to use my own d435i camera + 2D lidar for navigation. I set subscribe_rgbd to true and a warning appeared. [ WARN] [1711956405.513182993]: /rtabmap/rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=10). /rtabmap/rtabmap subscribed to (approx sync): /odom\ /camera/color/image_raw \ /scan [ WARN] [1711956406.424788308]: Timed out waiting for transform from base_link to map to become available before running costmap, tf error: canTransform: target_frame map does not exist.. canTransform returned after 0.10054 timeout was 0.1. [ WARN] [1711956407.512925625]: /rtabmap/rtabmap_viz: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=10). /rtabmap/rtabmap_viz subscribed to (approx sync): /odom\ /rtabmap/rgb/image \ /rtabmap/depth/image \ /rtabmap/rgb/camera_info \ /scan \ /rtabmap/odom_info

rqt 螢幕擷取畫面 2024-04-01 153936 螢幕擷取畫面 2024-04-01 153956

Please help me solve this problem, thank you

smile6023326 commented 8 months ago

Hello, let me describe my architecture, My architecture is realsense d435i+2D Lidar+AGV odom, AGV will send odom->base_link, I should only need rtabmap to send map->odom, I use it in different devices. My AGV executes odom and 2D Lidar, and another computer executes realsense d435i to do rtabmap. At present, I have tried two programs but there is no image. I need help. Thank you. The first: roslaunch realsense2_camera rs_rgbd.launch roslaunch my_package rtabmap.launch

<?xml version="1.0"?>
<launch>

  <!-- Arguments -->
  <arg name="open_rviz" default="true"/>
  <arg name="rtabmap_viz" default="true"/>
  <arg name="move_forward_only" default="false"/>
  <arg name="localization" default="false"/>
  <arg name="database_path" default="~/.ros/rtabmap.db"/>
  <arg     if="$(arg localization)" name="rtabmap_args" default=""/>
  <arg unless="$(arg localization)" name="rtabmap_args" default="-d"/>

  <group ns="rtabmap">
    <node pkg="rtabmap_sync" type="rgbd_sync" name="rgbd_sync" output="screen">
      <remap from="rgb/image"         to="/camera/color/image_raw"/>
      <remap from="depth/image"       to="/camera/aligned_depth_to_color/image_raw"/>
      <remap from="rgb/camera_info"   to="/camera/color/camera_info"/>
      <remap from="rgbd_image"       to="rgbd_image"/> 

    </node>

    <node name="rtabmap" pkg="rtabmap_slam" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
      <param name="database_path"       type="string" value="$(arg database_path)"/>
      <param name="frame_id"            type="string" value="base_link"/>
      <param name="subscribe_rgb"   type="bool" value="false"/>
      <param name="subscribe_depth" type="bool" value="false"/>
      <param name="subscribe_rgbd" type="bool" value="false"/>
      <param name="subscribe_scan"      type="bool"   value="true"/>
      <param name="approx_sync"         type="bool"   value="true"/>
            <!--param name="publish_tf" value="true"/-->

      <!-- use actionlib to send goals to move_base -->
      <param name="use_action_for_goal" type="bool" value="true"/>
      <remap from="move_base"            to="/move_base"/>

      <!-- inputs -->
      <remap from="scan"            to="/scan"/>
      <remap from="odom"            to="/odom"/>
      <remap from="rgbd_image"       to="rgbd_image"/> 

      <!-- output -->
      <remap from="grid_map" to="/map"/>

      <!-- RTAB-Map's parameters -->
      <param name="Reg/Strategy"                 type="string" value="1"/>
      <param name="Reg/Force3DoF"                type="string" value="true"/>
      <param name="GridGlobal/MinSize"           type="string" value="20"/>

      <!-- localization mode -->
      <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
      <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
    </node>

    <!-- visualization with rtabmap_viz -->
    <node if="$(arg rtabmap_viz)" pkg="rtabmap_viz" type="rtabmap_viz" name="rtabmap_viz" args="-d $(find rtabmap_demos)/launch/config/rgbd_gui.ini" output="screen">
      <param name="subscribe_scan"   type="bool"   value="true"/>
      <param name="subscribe_odom"   type="bool"   value="true"/>
      <param name="frame_id"         type="string" value="base_link"/>
      <param name="approx_sync"      type="bool"   value="true"/>

      <remap from="odom"            to="/odom"/>
      <remap from="scan"            to="/scan"/>
    </node>
  </group>

  <!-- move_base -->
  <include file="/home/smile/catkin_ws/src/my_package/launch/move_base.launch">
    <arg name="move_forward_only" value="$(arg move_forward_only)"/>
  </include>

  <!-- rviz -->
  <group if="$(arg open_rviz)">
    <node pkg="rviz" type="rviz" name="rviz" required="true"
      args="-d /home/smile/catkin_ws/rviz/navigation.rviz"/>

  </group>

</launch>

Set as:

<node name="rtabmap" pkg="rtabmap_slam" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
  <param name="database_path"       type="string" value="$(arg database_path)"/>
  <param name="frame_id"            type="string" value="base_link"/>
  <param name="subscribe_rgb"   type="bool" value="false"/>
  <param name="subscribe_depth" type="bool" value="false"/>
  <param name="subscribe_rgbd" type="bool" value="false"/>
  <param name="subscribe_scan"      type="bool"   value="true"/>
  <param name="approx_sync"         type="bool"   value="true"/>

Solve the 2D Lidar error problem, but there is still no image appearing in rtabmap, and there are also warnings. WARN: [ WARN] [1711976859.935083411]: Timed out waiting for transform from base_link to map to become available before running costmap, tf error: canTransform: target_frame map does not exist.. canTransform returned after 0.100606 timeout was 0.1. [ WARN] [1711976860.489925736]: There is no image subscription, bag-of-words loop closure detection will be disabled... [ WARN] [1711976863.721100474]: /rtabmap/rtabmap_viz: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=500). /rtabmap/rtabmap_viz subscribed to (approx sync): /odom \ /scan [ WARN] (2024-04-01 21:07:43.882) Rtabmap.cpp:4502::process() Republishing data of requested node(s) 1 (Rtabmap/MaxRepublished=2) [ WARN] [1711976864.979651994]: Timed out waiting for transform from base_link to map to become available before running costmap, tf error: Lookup would require extrapolation 11.480052515s into the past. Requested time 1711976849.486896992 but the earliest data is at time 1711976860.966949463, when looking up transform from frame [base_link] to frame [map]. canTransform returned after 0.100583 timeout was 0.1. Information obtained by the terminal roslaunch_get.txt

螢幕擷取畫面 2024-04-01 204720 螢幕擷取畫面 2024-04-01 214624 螢幕擷取畫面 2024-04-01 204937

The second: roslaunch realsense2_camera rs_rgbd.launch roslaunch my_package rtabmap2.launch

<?xml version="1.0"?>
<launch>

  <!-- Arguments -->
  <arg name="open_rviz" default="true"/>
  <arg name="rtabmap_viz" default="true"/>
  <arg name="move_forward_only" default="false"/>
  <arg name="localization" default="false"/>
  <arg name="database_path" default="~/.ros/rtabmap.db"/>
  <arg     if="$(arg localization)" name="rtabmap_args" default=""/>
  <arg unless="$(arg localization)" name="rtabmap_args" default="-d"/>

  <group ns="rtabmap">
    <node pkg="rtabmap_sync" type="rgbd_sync" name="rgbd_sync" output="screen">
      <remap from="rgb/image"         to="/camera/color/image_raw"/>
      <remap from="depth/image"       to="/camera/aligned_depth_to_color/image_raw"/>
      <remap from="rgb/camera_info"   to="/camera/color/camera_info"/>
      <remap from="rgbd_image"       to="rgbd_image"/> 

    </node>

    <node name="rtabmap" pkg="rtabmap_slam" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
      <param name="database_path"       type="string" value="$(arg database_path)"/>
      <param name="frame_id"            type="string" value="base_link"/>
      <param name="subscribe_rgb"   type="bool" value="false"/>
      <param name="subscribe_depth" type="bool" value="false"/>
      <param name="subscribe_rgbd" type="bool" value="true"/>
      <param name="subscribe_scan"      type="bool"   value="true"/>
      <param name="approx_sync"         type="bool"   value="true"/>
            <!--param name="publish_tf" value="true"/-->

      <!-- use actionlib to send goals to move_base -->
      <param name="use_action_for_goal" type="bool" value="true"/>
      <remap from="move_base"            to="/move_base"/>

      <!-- inputs -->
      <remap from="scan"            to="/scan"/>
      <remap from="odom"            to="/odom"/>
      <remap from="rgbd_image"       to="rgbd_image"/> 

      <!-- output -->
      <remap from="grid_map" to="/map"/>

      <!-- RTAB-Map's parameters -->
      <param name="Reg/Strategy"                 type="string" value="1"/>
      <param name="Reg/Force3DoF"                type="string" value="true"/>
      <param name="GridGlobal/MinSize"           type="string" value="20"/>

      <!-- localization mode -->
      <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
      <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
    </node>

    <!-- visualization with rtabmap_viz -->
    <node if="$(arg rtabmap_viz)" pkg="rtabmap_viz" type="rtabmap_viz" name="rtabmap_viz" args="-d $(find rtabmap_demos)/launch/config/rgbd_gui.ini" output="screen">
      <param name="subscribe_scan"   type="bool"   value="true"/>
      <param name="subscribe_odom"   type="bool"   value="true"/>
      <param name="frame_id"         type="string" value="base_link"/>
      <param name="approx_sync"      type="bool"   value="true"/>
      <param name="queue_size" type="int" value="500"/>
      <remap from="odom"            to="/odom"/>
      <remap from="scan"            to="/scan"/>

    </node>
  </group>

  <!-- move_base -->
  <include file="/home/smile/catkin_ws/src/my_package/launch/move_base.launch">
    <arg name="move_forward_only" value="$(arg move_forward_only)"/>
  </include>

  <!-- rviz -->
  <group if="$(arg open_rviz)">
    <node pkg="rviz" type="rviz" name="rviz" required="true"
      args="-d /home/smile/catkin_ws/rviz/navigation.rviz"/>

  </group>

</launch>

Set as:

<node name="rtabmap" pkg="rtabmap_slam" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
  <param name="database_path"       type="string" value="$(arg database_path)"/>
  <param name="frame_id"            type="string" value="base_link"/>
  <param name="subscribe_rgb"   type="bool" value="false"/>
  <param name="subscribe_depth" type="bool" value="false"/>
  <param name="subscribe_rgbd" type="bool" value="true"/>
  <param name="subscribe_scan"      type="bool"   value="true"/>
  <param name="approx_sync"         type="bool"   value="true"/>

When I set "subscribe_rgbd" to "true", my images also do not appear on rtabmap and the map is not published. WARN: [ WARN] [1711979715.877260686]: Timed out waiting for transform from base_link to map to become available before running costmap, tf error: canTransform: target_frame map does not exist.. canTransform returned after 0.100746 timeout was 0.1. [ WARN] [1711979720.561208812]: /rtabmap/rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=10). /rtabmap/rtabmap subscribed to (approx sync): /odom \ /rtabmap/rgbd_image \ /scan [ WARN] [1711979720.916541412]: Timed out waiting for transform from base_link to map to become available before running costmap, tf error: canTransform: target_frame map does not exist.. canTransform returned after 0.100175 timeout was 0.1.

Information obtained by the terminal roslaunch_get2.txt 螢幕擷取畫面 2024-04-01 215542 螢幕擷取畫面 2024-04-01 215557 螢幕擷取畫面 2024-04-01 215637

I don't know how to solve the image problem and would like to ask for your help. Thank you.

matlabbe commented 7 months ago
[ WARN] [1711979453.904529676]: /rtabmap/rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=10).
/rtabmap/rtabmap subscribed to (approx sync):
   /odom \
   /rtabmap/rgbd_image \
   /scan

You can do:

rostopic hz /odom \
   /rtabmap/rgbd_image \
   /scan

to see if they all published. Then if they are and the warning is still shown, maybe the timestamp in the topics are not in sync. For example, you could have images with stamps based on epoch host time, while the odometry has time since it started or/and lidar had its own clock. This will make the topics impossible to synchronize. You can echo the timestamp of each topic with:

rostopic echo /odom/header
rostopic echo /rtabmap/rgbd_image/header
rostopic echo /scan/header

then if there is a huge delay between them, you will have to fix the stamps in the packages publishing them (ideally so that they all publish with epoch time of the host computer).

smile6023326 commented 7 months ago

I don't think the gap is too big

rostopic echo /odom/header
seq: 4030
stamp: 
  secs: 1713272773
  nsecs: 170890335
frame_id: "odom"
---
seq: 4031
stamp: 
  secs: 1713272773
  nsecs: 273711513
frame_id: "odom"
---
seq: 4032
stamp: 
  secs: 1713272773
  nsecs: 376474655
frame_id: "odom"
---
seq: 4033
stamp: 
  secs: 1713272773
  nsecs: 479079073
frame_id: "odom"
---

rostopic echo /scan/header
seq: 3582
stamp: 
  secs: 1713272786
  nsecs: 765445000
frame_id: "base_link"
---
seq: 3583
stamp: 
  secs: 1713272786
  nsecs: 882747000
frame_id: "base_link"
---

rostopic echo /rtabmap/rgbd_image/header
seq: 3472
stamp: 
  secs: 1713272783
  nsecs:   3189564
frame_id: "camera_color_optical_frame"
---
seq: 3473
stamp: 
  secs: 1713272783
  nsecs:  36540985
frame_id: "camera_color_optical_frame"
---
matlabbe commented 7 months ago

Were all taken at the same time? the scan is ahead 13 seconds over odom and 3 seconds over image. Ideally, it should be under 100 ms.

smile6023326 commented 7 months ago

I followed this URL https://wiki.ros.org/rtabmap_ros/Tutorials/MappingAndNavigationOnTurtlebot and tried to change the camera to my own D435i and a warning appeared. How should I solve it? Is it possible that rs_camera.launch is set incorrectly?

Usage: roslaunch realsense2_camera rs_camera.launch align_depth:=true

export TURTLEBOT3_MODEL=waffle roslaunch turtlebot3_gazebo turtlebot3_world.launch

export TURTLEBOT3_MODEL=waffle roslaunch my_package demo_d435i_navigation.launch

.launch: rs_camera.txt demo_d435i_navigation.txt

I only changed this to the topic of d435i `

` Have I made any mistakes? I really want to use this program. Please help me. [ WARN] [1713357694.084766404, 666.656000000]: /rtabmap/rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=10). /rtabmap/rtabmap subscribed to (approx sync): /odom \ /rtabmap/rgbd_image \ /scan [ WARN] [1713357698.759694043, 670.385000000]: /rtabmap/rtabmap_viz: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=10). /rtabmap/rtabmap_viz subscribed to (approx sync): /odom \ /scan [ WARN] [1713357699.415233827, 670.910000000]: Timed out waiting for transform from base_footprint to map to become available before running costmap, tf error: canTransform: target_frame map does not exist.. canTransform returned after 0.1 timeout was 0.1. [ WARN] [1713357699.765448219, 671.175000000]: /rtabmap/rgbd_sync: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. /rtabmap/rgbd_sync subscribed to (approx sync): /camera/color/image_raw \ /camera/depth/image_raw \ /camera/color/camera_info ![螢幕擷取畫面 2024-04-17 204231](https://github.com/introlab/rtabmap_ros/assets/48814064/cbaa72cb-5184-4046-8d7b-e745d346a27b) d435i rostopic list /camera/align_to_color/parameter_descriptions /camera/align_to_color/parameter_updates /camera/aligned_depth_to_color/camera_info /camera/aligned_depth_to_color/image_raw /camera/aligned_depth_to_color/image_raw/compressed /camera/aligned_depth_to_color/image_raw/compressed/parameter_descriptions /camera/aligned_depth_to_color/image_raw/compressed/parameter_updates /camera/aligned_depth_to_color/image_raw/compressedDepth /camera/aligned_depth_to_color/image_raw/compressedDepth/parameter_descriptions /camera/aligned_depth_to_color/image_raw/compressedDepth/parameter_updates /camera/aligned_depth_to_color/image_raw/theora /camera/aligned_depth_to_color/image_raw/theora/parameter_descriptions /camera/aligned_depth_to_color/image_raw/theora/parameter_updates /camera/color/camera_info /camera/color/image_raw /camera/color/image_raw/compressed /camera/color/image_raw/compressed/parameter_descriptions /camera/color/image_raw/compressed/parameter_updates /camera/color/image_raw/compressedDepth /camera/color/image_raw/compressedDepth/parameter_descriptions /camera/color/image_raw/compressedDepth/parameter_updates /camera/color/image_raw/theora /camera/color/image_raw/theora/parameter_descriptions /camera/color/image_raw/theora/parameter_updates /camera/color/metadata /camera/depth/camera_info /camera/depth/image_rect_raw /camera/depth/image_rect_raw/compressed /camera/depth/image_rect_raw/compressed/parameter_descriptions /camera/depth/image_rect_raw/compressed/parameter_updates /camera/depth/image_rect_raw/compressedDepth /camera/depth/image_rect_raw/compressedDepth/parameter_descriptions /camera/depth/image_rect_raw/compressedDepth/parameter_updates /camera/depth/image_rect_raw/theora /camera/depth/image_rect_raw/theora/parameter_descriptions /camera/depth/image_rect_raw/theora/parameter_updates /camera/depth/metadata /camera/extrinsics/depth_to_color /camera/motion_module/parameter_descriptions /camera/motion_module/parameter_updates /camera/realsense2_camera_manager/bond /camera/rgb_camera/auto_exposure_roi/parameter_descriptions /camera/rgb_camera/auto_exposure_roi/parameter_updates /camera/rgb_camera/parameter_descriptions /camera/rgb_camera/parameter_updates /camera/stereo_module/auto_exposure_roi/parameter_descriptions /camera/stereo_module/auto_exposure_roi/parameter_updates /camera/stereo_module/parameter_descriptions /camera/stereo_module/parameter_updates /diagnostics /rosout /rosout_agg /tf /tf_static
matlabbe commented 7 months ago

You are starting the real camera driver with a simulated environment, it is likely to fail.

If you are using the real robot, start the turtlebot bringup, not the gazebo one on the robot.

smile6023326 commented 7 months ago

Today I tried executing rtabmap directly on my real robot without using another computer. There is an image, but the correct map is not obtained, and the image is disconnected and warnings are obtained during execution. [ WARN] [1713448175.403283909]: The time difference between rgb and depth frames is high (diff=0.033300s, rgb=1713448175.020938s, depth=1713448174.987638s). You may want to set approx_sync_max_interval lower than 0.01s to reject spurious bad synchronizations or use approx_sync=false if streams have all the exact same timestamp. [ WARN] (2024-04-18 21:49:35.573) Memory.cpp:4845::createSignature() Mem/DepthAsMask is true, but RGB size (1280x720) modulo depth size (848x480) is not 0. Ignoring depth mask for feature detection (Mem/ImagePreDecimation=1).

I have 2 questions Q1: Can you help me figure out why I'm not getting the correct map? I hope to be like this 螢幕擷取畫面 2024-04-18 233359 .launch: demo.txt

Q2: Appears 10 seconds after roslaunch [ WARN] [1713455513.280246432]: /rtabmap/rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=10). /rtabmap/rtabmap subscribed to (approx sync): /odom \ /rtabmap/rgbd_image [ WARN] [1713455518.569101173]: /rtabmap/rgbd_sync: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. /rtabmap/rgbd_sync subscribed to (approx sync): /camera/color/image_raw \ /camera/depth/image_rect_raw \ /camera/color/camera_info [ WARN] [1713455518.909931001]: /rtabmap/rtabmap_viz: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=10). /rtabmap/rtabmap_viz subscribed to (approx sync): /odom \ /scan

Results using real robots: Screenshot from 2024-04-18 23-12-35 Screenshot from 2024-04-18 23-13-01

I cannot connect to the D435i using my own computer, but the map scanned by the laser has the correct result: 螢幕擷取畫面 2024-04-01 204728 螢幕擷取畫面 2024-04-01 204720

matlabbe commented 7 months ago

I don't see why the grid is not computed from the laser scan with this config:

<node pkg="rtabmap_sync" type="rgbd_sync" name="rgbd_sync" output="screen">
      <remap from="rgb/image"         to="/camera/color/image_raw"/>
      <remap from="depth/image"       to="/camera/depth/image_rect_raw"/>
      <remap from="rgb/camera_info"   to="/camera/color/camera_info"/>
    </node>

    <node name="rtabmap" pkg="rtabmap_slam" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
      <param name="database_path"       type="string" value="$(arg database_path)"/>
      <param name="frame_id"            type="string" value="base_link"/>
      <param name="subscribe_rgb"   type="bool" value="false"/>
      <param name="subscribe_depth" type="bool" value="false"/>
      <param name="subscribe_rgbd" type="bool" value="true"/>
      <param name="suscribe_scan"      type="bool"   value="true"/>
      <param name="approx_sync"         type="bool"   value="true"/>

      <!-- use actionlib to send goals to move_base -->
      <param name="use_action_for_goal" type="bool" value="true"/>
      <remap from="move_base"            to="/move_base"/>

      <!-- inputs -->
      <remap from="scan"            to="/scan"/>
      <remap from="odom"            to="/odom"/>
      <remap from="rgbd_image"       to="rgbd_image"/>

      <!-- output -->
      <remap from="grid_map" to="/map"/>

      <!-- RTAB-Map's parameters -->
      <param name="Reg/Strategy"                 type="string" value="1"/>
      <param name="Reg/Force3DoF"                type="string" value="true"/>
      <param name="GridGlobal/MinSize"           type="string" value="20"/>

      <!-- localization mode -->
      <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
      <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
    </node>

You should see a warning message like this when starting the launch file:

[ WARN] [1714326946.001705042]: Setting "Grid/Sensor" parameter to 0 (default 1) as "subscribe_scan", 
"subscribe_scan_cloud" or "gen_scan" is true. The occupancy grid map will be constructed from laser scans. To get occupancy 
grid map from camera's cloud projection, set "Grid/Sensor" to 1. To suppress this warning, add <param name="Grid/Sensor" 
type="string" value="0"/>

You can try to explicitly set <param name="Grid/Sensor" type="string" value="0"/> to make sure the grid is created from the scan topic.

Oh, just saw there is a typo:

<param name="suscribe_scan"      type="bool"   value="true"/>

should be:

<param name="subscribe_scan"      type="bool"   value="true"/>

For second question, it seems the camera fails after 10 seconds:

[ WARN] [1713455518.569101173]: /rtabmap/rgbd_sync: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set.
/rtabmap/rgbd_sync subscribed to (approx sync):
/camera/color/image_raw
/camera/depth/image_rect_raw
/camera/color/camera_info

so rtabmap won't receive rgbd_image topic, and rtabmap_viz will also complain not receiving data.

smile6023326 commented 5 months ago

Hello, after this period of hard work, I have successfully implemented this program on my own robot. Thank you very much. I am currently trying to use three different Graph Optimizations to test mapping and navigation positioning accuracy. But I'm not sure the full names of these three methods are TORO (Tree-based Network optimizer), G2O (General Graph Optimization) and GTASM (Georgia Tech Smoothing and Mapping)?

matlabbe commented 5 months ago

Here the links to those approaches: TORO: https://openslam-org.github.io/toro.html g2o: https://github.com/RainerKuemmerle/g2o GTSAM: https://gtsam.org/