0
$\begingroup$

Rosanswers logo

I'm continuing the discussion of my previous topic by focusing on the RTAB results. I'm using robot_localization to obtain filtered odometry from wheel encoders, IMU (xsens Mti-10) and visual odometry (RealSense R200). The IMU sensor is mounting on the center of the robot while the RSense is mounted on a vertical axis 116 centimeters above the IMU.

I launch the Husky base.launch node in order to start the teleoperation and the locomotion node; in addition, the base node also launch control.launch file which launch robot_localization. Control.launch file loads parameters from localization.yaml for IMU and wheel_encoders and I added also the odom1 which should be generated by rgbd_odometry from rtabmap_ros package. At the bottom of control.launch file, I also added the transformation for my sensors:

<node pkg="tf" type="static_transform_publisher"  name="base_to_realsense"
      args="0.3 1.16 0 1.5707963 0 -1.5707963 /base_link /realsense_frame 100" />
 <node pkg="tf" type="static_transform_publisher"  name="base_to_laser"
      args="0.3 0.6 0.1 0 0 0 /base_link /laser 100" />
<node pkg="tf" type="static_transform_publisher"  name="base_to_color"
      args="0 0 0.6 1.5707963 0 -1.5707963 /base_link /camera_color_optical_frame 100" />
<node pkg="tf" type="static_transform_publisher"  name="color_to_depth"
      args="0 0 0 0 0 0 /camera_color_optical_frame /camera_depth_optical_frame 100" />

This is my TF tree: frames.pdf. Should the camera_color_optical_frame be a child of realsense_frame?

EDIT (@matlabbe 28/08/2016): This is a very light bag file without /tf and with compressed image topics: /scan /camera/color/image_raw/compressed camera/depth/image_raw/compressed /camera/color/camera_info /imu/data /odometry/filtered /tf /husky_velocity_controller/odom END_EDIT

With this configuration, I recorded a bag file, in particular, these topics:

rosbag record /scan /camera/color/image_raw camera/depth/image_raw /camera/color/camera_info /imu/data /odometry/filtered /tf /husky_velocity_controller/odom

Then, I try to run the rgbd_mapping.launch file in order to obtain the 3D map by playing back the ros bag file (I added the same transformations at the end of this file, too).

$ rosparam set use_sim_time true
$ roslaunch rtabmap_ros rgbd_mapping.launch rgb_topic:=/camera/color/image_raw depth_registered_topic:=/camera/depth/image_raw camera_info_topic:=/camera/color/camera_info compressed:=true frame_id:=base_link rtabmap_args:="--delete_db_on_start" estimation:=1
$ rosbag play --clock file.bag

The problem is that the map is not built correctly since I can only see few frames without any connection. I uplaoded a short video on YouTube.

  1. Do you think the problem is related to wrong transformations set for the sensors?

  2. The problem can come from a wrong /odometry/filtered?

  3. May be the rgb/image, depth/image, rgb/camera_info are set to the wrong published topics? (both in control.launch and in rgbd_mapping.launch files)

  4. Control.launch is OK?

  5. Should the camera_color_optical_frame be a child of realsense_frame?


Originally posted by Marcus Barnet on ROS Answers with karma: 287 on 2016-08-26

Post score: 0


Original comments

Comment by matlabbe on 2016-08-27:
Can you link on a rosbag file? Don't record TF if you are republishing them along replaying the bag.

Comment by Marcus Barnet on 2016-08-27:
Yes, I can upload a ros bag file, do you have problems to download large files? Unfortunately, few seconds of recording are about 2-3 gb since realsense frames are very large files.

Comment by Marcus Barnet on 2016-08-27:
These topics are enough? /scan /camera/color/image_raw camera/depth/image_raw /camera/color/camera_info /imu/data /odometry/filtered /tf /husky_velocity_controller/odom Any topics is missing?

Comment by matlabbe on 2016-08-27:
You should not record /tf if you are republishing them when replaying the bag. You can use $ rosbag compress to compress a bag. Note that 10 sec rosbag is enough, like moving 1 or 2 meters with the robot.

Comment by Marcus Barnet on 2016-08-27:
This is the first recorded file which contains only few topics. Later, I will rosbag compress another bag file and I will post it. It will include all the published topics without /tf.

Comment by Marcus Barnet on 2016-08-27:
Since it takes few hours to upload the new file without the /tf, I uploaded also this other file which contains all the topics (unfortunately, it includes also /tf). I will upload the other bag file during next hours.

Comment by Marcus Barnet on 2016-08-27:
I would like to obtain something like your demo Robot Mapping in RVIZ in order to display a 2D map with its projected 3D view. It would be very interesting for outdoor applications.

Comment by Marcus Barnet on 2016-08-28:
Final very light bag file without /tf and with compressed image topics: /scan /camera/color/image_raw/compressed camera/depth/image_raw/compressed /camera/color/camera_info /imu/data /odometry/filtered /tf /husky_velocity_controller/odom

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

  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


Originally posted by matlabbe with karma: 6409 on 2016-08-28

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by Marcus Barnet on 2016-08-28:
@matlabbe, thanks for your suggestions. I fixed the /tf problem and everything is OK since I checked all the frames under RVIZ and /laser and realsense_frame are aligned. This is a compressed bag file.

Comment by Marcus Barnet on 2016-08-28:
This is the control.launch file without /vo. The bag file includes the /tf topic since it is more easy for me to display it under RVIZ. Can you tell me if it is OK now and if it can be used with RTAB to obtain a 2D map and the projected 3d view, please?

Comment by Marcus Barnet on 2016-08-28:
I deleted the RTAB from control.launch file, but now the /odometry/filtered will not contain also the visual odometry. This can still be OK for what I would like to do? I mean, something like your demo.

Comment by Marcus Barnet on 2016-08-28:
The compressed bag file will be available in 30 minutes starting from now. It is in upload right now.

Comment by Marcus Barnet on 2016-08-28:
Now the file has been correctly uploaded.

Comment by Marcus Barnet on 2016-08-29:
@matlabbe: I tried to run your launch file, but I'm not able to build a map since RVIZ says me that: no map is received. If I set base_footprint or /odom as fixed frame, I can see the robot moving around, but no map. Do I have to run some other launch file, too?

Comment by Marcus Barnet on 2016-08-29:
I can select grid_map, cloud_map, mapData and mapGraph in RVIZ, but it always says that no map is received. In my TF frames, there is no map reference. It seems that no map is created. How this can be possible? Do you launch some other file in addition to your test.launch?

Comment by matlabbe on 2016-08-29:
Make sure light-tf-ok.bag is decompressed: $ rosbag decompress light-tf-ok.bag. It is only test.launch running.

Comment by Marcus Barnet on 2016-08-29:
I tested test.launch also with other recorded bags and I have the same error: no map received. This is very strange for me!

Comment by matlabbe on 2016-08-29:
Ah sorry, there was a typo in the odom topic name: /odometry_filtered -> /odometry/filtered

Comment by Marcus Barnet on 2016-08-29:
This is my RVIZ config file. Screenshot 1 and Screenshot 2 shows how it appears if I select /odom or /map as fixed frame. Obviously, I dom't have the option /map in the fixed frame menu and I need to manually type it since it doesn't exist.

Comment by matlabbe on 2016-08-29:
yes rtabmap cannot start if it doesn't receive the input topics (see terminal). The typo above made rtabmap wait for /odometry_filtered instead of /odometry/filtered. The frame /map should be avaialble in RVIZ when rtabmap is receiving the input topics.

Comment by Marcus Barnet on 2016-08-29:
Yes, you have reason, there was a typo in the odometry topic, I didn't noticed it, sorry! Anyway, now I can see the 2d map, but the 3D map cannot build, it only shows frames that move while the robot moves.

Comment by Marcus Barnet on 2016-08-29:
If I set <param name="Reg/Strategy" type="string" value="2"/> then I can see something which can be similar to a 3D map but there are too few points, it is not accurate as you showed in your screenshots. This is what I get.

Comment by matlabbe on 2016-08-29:
Reg/Strategy doesn't change how the point cloud looks like (I recommend to keep it at 1 as you have a lidar). In my screenshots the cloud is more dense because I've set Cloud decimation parameter to 2 in MapCloud RVIZ plugin (yours is set to 4). You can even set it to 1 to see all points

Comment by Marcus Barnet on 2016-08-29:
Thank you, @matlabbe! I'm going to test it outdoor and then I will post the results. In order to improve the 3D map, is it better to record compressed images from camera or it is better to record the raw output?

Comment by matlabbe on 2016-08-31:
It is more for convenience, recording compressed images will make the rosbag grow slower. However when replaying the bag, compressed images should be decompressed, which adds some computational overhead.

Comment by Marcus Barnet on 2016-08-31:
Thank you for support. Sometimes, it happens that rosbag play crashes after few seconds of playback. This happens when RTAB node is launched. If RTAB is not running, rosbag play never gives me problems. It happens both with compressed and raw images.

Comment by matlabbe on 2016-08-31:
Looks like a bug with rosbag. With RTAB running, the CPU is more used.

Comment by Marcus Barnet on 2016-09-02:
Thank you, @matlabbe! Since this topic is solved, I'm going to open another topic about the discussion of RTAB-map results. In particular, in rough terrain conditions.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.