Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version
  1. yes, see the following sceenshots. /camera_color_optical_frame is not a child of /realsense_frame. /realsense_frame and /laser don't seem to be on the robot. The camera is also looking backward in RVIZ (second screenshot). It seems also that you have /camera_color_optical_frame published by 2 nodes with 2 different transforms.

    image description image description

  2. Printing /odometry/filterered on terminal, the values jump a lot. Please use default husky's robot_localization config when recording (without /vo)

  3. What are all your camera topics? Make sure you are subscribing to depth image registered with color image.

  4. TF should be fixed first.

  5. yes

Please, don't record a bag with rtabmap running. Just bringup the robot and record. It seems that multiple nodes are publishing /odom -> /base_footprint transform. At least before recording, make sure in RVIZ that laser and depth cloud are correctly aligned together and with the robot. You should be able to answer yes to this question: "Are you able to teleoperate the robot only looking at RVIZ?"

Depth images should not be recorded with /camera/depth/compressed but with /camera/depth/compressedDepth instead.

cheers

  1. yes, see the following sceenshots. /camera_color_optical_frame is not a child of /realsense_frame. /realsense_frame and /laser don't seem to be on the robot. The camera is also looking backward in RVIZ (second screenshot). It seems also that you have /camera_color_optical_frame published by 2 nodes with 2 different transforms.

    image description image description

  2. Printing /odometry/filterered on terminal, the values jump a lot. Please use default husky's robot_localization config when recording (without /vo)

  3. What are all your camera topics? Make sure you are subscribing to depth image registered with color image.

  4. TF should be fixed first.

  5. yes

Please, don't record a bag with rtabmap running. Just bringup the robot and record. It seems that multiple nodes are publishing /odom -> /base_footprint transform. At least before recording, make sure in RVIZ that laser and depth cloud are correctly aligned together and with the robot. You should be able to answer yes to this question: "Are you able to teleoperate the robot only looking at RVIZ?"

Depth images should not be recorded with /camera/depth/compressed but with /camera/depth/compressedDepth instead.

EDIT

With the bag with corrected TF, here is an example launch file to create a map similar to Robot mapping with RVIZ demo:

<launch>
  <param name="use_sim_time" type="bool" value="True"/>
  <group ns="rtabmap">
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
      <param name="frame_id"           type="string" value="base_link"/>

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

      <remap from="odom" to="/odometry_filtered"/>
      <remap from="scan" to="/scan"/>

      <remap from="rgb/image"       to="/camera/color/image_raw"/>
      <remap from="depth/image"     to="/camera/depth/image_raw"/>
      <remap from="rgb/camera_info" to="/camera/color/camera_info"/>

      <param name="rgb/image_transport"   type="string" value="compressed"/>
      <param name="depth/image_transport" type="string" value="compressedDepth"/>

      <param name="Reg/Strategy"              type="string" value="1"/>     <!-- 0=Visual, 1=ICP, 2=Visual+ICP -->
      <param name="Optimizer/Slam2D"          type="string" value="true"/>
      <param name="Reg/Force3DoF"             type="string" value="true"/>
   </node>
  </group>

  <!-- Visualisation RVIZ -->
  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_robot_mapping.rviz" output="screen"/>
</launch>

Using it and resulting map:

$ roslaunch test.launch
$ rosbag play --clock light-tf-ok.bag

image description

At first look, /odometry/filtered seems not very accurate so I gave a try with /husky_velocity_controller/odom alone. I removed from the bag all /odom tf using this filterBagTF.py script (change map for odom), /odom -> /base_link is republished from the /husky_velocity_controller/odom topic using odom_msg_to_tf node:

<launch>
  <param name="use_sim_time" type="bool" value="True"/>
  <group ns="rtabmap">
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
      <param name="frame_id"           type="string" value="base_link"/>

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

      <remap from="odom" to="/husky_velocity_controller/odom"/>
      <remap from="scan" to="/scan"/>

      <remap from="rgb/image"       to="/camera/color/image_raw"/>
      <remap from="depth/image"     to="/camera/depth/image_raw"/>
      <remap from="rgb/camera_info" to="/camera/color/camera_info"/>

      <param name="rgb/image_transport"   type="string" value="compressed"/>
      <param name="depth/image_transport" type="string" value="compressedDepth"/>

      <param name="Reg/Strategy"              type="string" value="1"/>     <!-- 0=Visual, 1=ICP, 2=Visual+ICP -->
      <param name="Optimizer/Slam2D"          type="string" value="true"/>
      <param name="Reg/Force3DoF"             type="string" value="true"/>
   </node>
  </group>

  <node name="odom_msg_to_tf" pkg="rtabmap_ros" type="odom_msg_to_tf">
     <remap from="odom" to="/husky_velocity_controller/odom"/>
  </node>

  <!-- Visualisation RVIZ -->
  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_robot_mapping.rviz" output="screen"/>
</launch>

Using it and resulting map:

$ roslaunch test.launch
$ rosbag play --clock light-tf-ok-without-odomtf.bag

image description

Odometry is much better using /husky_velocity_controller/odom than /odometry/filtered. I recommend to fix your robot_localization fusing IMU+Wheel odometry before trying to add visual odometry. Note that with relatively good wheel odometry and a laser scanner, you may not need visual odometry at all.

cheers

  1. yes, see the following sceenshots. /camera_color_optical_frame is not a child of /realsense_frame. /realsense_frame and /laser don't seem to be on the robot. The camera is also looking backward in RVIZ (second screenshot). It seems also that you have /camera_color_optical_frame published by 2 nodes with 2 different transforms.

    image description image description

  2. Printing /odometry/filterered on terminal, the values jump a lot. Please use default husky's robot_localization config when recording (without /vo)

  3. What are all your camera topics? Make sure you are subscribing to depth image registered with color image.

  4. TF should be fixed first.

  5. yes

Please, don't record a bag with rtabmap running. Just bringup the robot and record. It seems that multiple nodes are publishing /odom -> /base_footprint transform. At least before recording, make sure in RVIZ that laser and depth cloud are correctly aligned together and with the robot. You should be able to answer yes to this question: "Are you able to teleoperate the robot only looking at RVIZ?"

Depth images should not be recorded with /camera/depth/compressed but with /camera/depth/compressedDepth instead.

EDIT

With the bag with corrected TF, here is an example launch file to create a map similar to Robot mapping with RVIZ demo:

<launch>
  <param name="use_sim_time" type="bool" value="True"/>
  <group ns="rtabmap">
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
      <param name="frame_id"           type="string" value="base_link"/>

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

      <remap from="odom" to="/odometry_filtered"/>
to="/odometry/filtered"/>
      <remap from="scan" to="/scan"/>

      <remap from="rgb/image"       to="/camera/color/image_raw"/>
      <remap from="depth/image"     to="/camera/depth/image_raw"/>
      <remap from="rgb/camera_info" to="/camera/color/camera_info"/>

      <param name="rgb/image_transport"   type="string" value="compressed"/>
      <param name="depth/image_transport" type="string" value="compressedDepth"/>

      <param name="Reg/Strategy"              type="string" value="1"/>     <!-- 0=Visual, 1=ICP, 2=Visual+ICP -->
      <param name="Optimizer/Slam2D"          type="string" value="true"/>
      <param name="Reg/Force3DoF"             type="string" value="true"/>
   </node>
  </group>

  <!-- Visualisation RVIZ -->
  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_robot_mapping.rviz" output="screen"/>
</launch>

Using it and resulting map:

$ roslaunch test.launch
$ rosbag play --clock light-tf-ok.bag

image description

At first look, /odometry/filtered seems not very accurate so I gave a try with /husky_velocity_controller/odom alone. I removed from the bag all /odom tf using this filterBagTF.py script (change map for odom), /odom -> /base_link is republished from the /husky_velocity_controller/odom topic using odom_msg_to_tf node:

<launch>
  <param name="use_sim_time" type="bool" value="True"/>
  <group ns="rtabmap">
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
      <param name="frame_id"           type="string" value="base_link"/>

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

      <remap from="odom" to="/husky_velocity_controller/odom"/>
      <remap from="scan" to="/scan"/>

      <remap from="rgb/image"       to="/camera/color/image_raw"/>
      <remap from="depth/image"     to="/camera/depth/image_raw"/>
      <remap from="rgb/camera_info" to="/camera/color/camera_info"/>

      <param name="rgb/image_transport"   type="string" value="compressed"/>
      <param name="depth/image_transport" type="string" value="compressedDepth"/>

      <param name="Reg/Strategy"              type="string" value="1"/>     <!-- 0=Visual, 1=ICP, 2=Visual+ICP -->
      <param name="Optimizer/Slam2D"          type="string" value="true"/>
      <param name="Reg/Force3DoF"             type="string" value="true"/>
   </node>
  </group>

  <node name="odom_msg_to_tf" pkg="rtabmap_ros" type="odom_msg_to_tf">
     <remap from="odom" to="/husky_velocity_controller/odom"/>
  </node>

  <!-- Visualisation RVIZ -->
  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_robot_mapping.rviz" output="screen"/>
</launch>

Using it and resulting map:

$ roslaunch test.launch
$ rosbag play --clock light-tf-ok-without-odomtf.bag

image description

Odometry is much better using /husky_velocity_controller/odom than /odometry/filtered. I recommend to fix your robot_localization fusing IMU+Wheel odometry before trying to add visual odometry. Note that with relatively good wheel odometry and a laser scanner, you may not need visual odometry at all.

cheers