Ask Your Question
0

Build a map with RTABMAP, RealSense, Sick and IMU with sensor fusion

asked 2016-07-19 07:33:13 -0500

Marcus Barnet gravatar image

updated 2016-07-19 07:34:01 -0500

Hi to all,

I'm trying to continue my work on 3D map building by using RealSense R200, a SICK laser and a XSENS IMU.

In particular, I would like to integrate 3D data coming from the RealSense with the 2D data from the SICK laser by compensating them with the XSENS IMU data. Currently, I'm using rtabmap_ros package with the subscribe_scan option enabled and I can successfully display 3D data under RTAB-Map.

These are the commands I use to run Realsense+SICK topics on RTAB-Map:

$ roscore
$ rosparam set use_sim_time true
$ rosrun tf static_transform_publisher 0 0 0 -1.5707963 0 -1.5707963 base_link camera_color_optical_frame 100
$ rosrun tf static_transform_publisher 0 0 0 0 0 0 camera_color_optical_frame camera_depth_optical_frame 100
$ 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 test1.bag (the bag contains both realsense and laser data)

And I have these transformations at the end of rgbd_mapping.launch file:

<node pkg="tf" type="static_transform_publisher"  name="base_to_realsense"
      args="0 0 0 -1.5707963 0 -1.5707963 /base_link /realsense_frame 100" />
<node pkg="tf" type="static_transform_publisher"  name="base_to_laser"
      args="0 0 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" />

The problem is that without using the /imu/data messages, I'm not able to add information about pitch, roll and yaw angles; these are very important for my research since my robot moves on agricultural terrains which are often rough.

I've read that in order to add IMU information to RTABMAP, I need to perform some kind of sensor fusion by using robot_localization package.

If I well understood, I need to run robot_localization before to call rgbd_mapping.launch because I need to publish /odometry/filtered, is it correct?

I tried to create a new robot_localization launch file as below. Can you tell me if it is OK, please?

Moreover, I don't know how to generate the /odometry message since I only have some recorded bag files which contains encoders and differential_velocity messages. Can I generate /odometry message with this information?

Thank you!

<launch>

    <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">

      <param name="frequency" value="30"/>

      <param name="sensor_timeout" value="0.1"/>
      <param name="two_d_mode" value="false"/>
      <!-- 
             2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data,
                set "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state
                estimation nodes.
       -->
      <!-- Defaults to "map" if unspecified -->
      <param name="map_frame" value="map"/>
      <!-- Defaults to "odom" if unspecified -->
      <param name="odom_frame" value="odom"/>
      <!-- Defaults to "base_link" if unspecified -->
      <param name="base_link_frame" value="base_link"/>
      <!-- Defaults to the value of "odom_frame" if unspecified -->
      <param name="world_frame" value="odom"/>

      <param name="transform_time_offset" value="0.0"/>

      <param name="odom0" value ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2016-07-20 13:40:54 -0500

matlabbe gravatar image

updated 2016-07-22 00:09:38 -0500

Hi,

You don't need to do

$ rosrun tf static_transform_publisher 0 0 0 -1.5707963 0 -1.5707963 base_link camera_color_optical_frame 100
$ rosrun tf static_transform_publisher 0 0 0 0 0 0 camera_color_optical_frame camera_depth_optical_frame 100

if they are already created in your launch file.

Rtabmap should compute already 6DoF motion estimation. Well, rgbd_mapping.launch assumes that you want to do 3DoF mapping when subscribe_scan is true. Remove Optimizer/Slam2D, Reg/Strategy and Reg/Force3DoF from that launch file.

You can take a look at this example sensor_fusion.launch, which is a similar setup to first configuration in this post: image description

You may use this launch file instead of rgbd_mapping.launch while adding your laser scan to rtabmap node. To add the laser scan:

<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap">
   <param name="subscribe_scan" value="true"/>
   <remap from="scan" to="/scan"/>
   ...
</node>

cheers

edit flag offensive delete link more

Comments

Thank you for your support. Is it possible to add laser scan in rtabmap node without having problems with the 6DoF motion estimation?

Marcus Barnet gravatar imageMarcus Barnet ( 2016-07-20 19:05:08 -0500 )edit

Not sure what do you mean by problems? If you set only the laser scan to rtabmap node without changing registration parameters, the mapping will be in 6DoF. See subscribe_scan parameter on rtabmap_ros.

matlabbe gravatar imagematlabbe ( 2016-07-21 10:22:23 -0500 )edit

Unfortunately, I can't understand how to proceed. Do I have to only add this line: <arg name="subscribe_scan" default="true"/> inside sensor_fusion.launch file and then launch it? I tried to launch sensor_fusion, but it doesn't open RTAB-Map.Or do I have to launch also other files?

Marcus Barnet gravatar imageMarcus Barnet ( 2016-07-21 18:19:55 -0500 )edit

Indeed sensor_fusion.launch doesn't start a GUI. You can start RVIZ if you want. Updated end of the answer above...

matlabbe gravatar imagematlabbe ( 2016-07-22 00:09:39 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

4 followers

Stats

Asked: 2016-07-19 07:33:13 -0500

Seen: 1,069 times

Last updated: Jul 22 '16