0
$\begingroup$

Rosanswers logo

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="/odometry"/>
      <param name="imu0" value="/imu/data"/>
    
      <rosparam param="odom0_config">[false, false, false,
                                      false, false, false,
                                      true,  false, false,
                                      false, false, false,
                                      false, false, false]</rosparam>
      <rosparam param="imu0_config">[false, false, false,
                                     true,  true,  true,
                                     false, false, false,
                                     true,  true,  true,
                                     true,  true,  true]</rosparam>
   
      <param name="odom0_differential" value="true"/>
      <param name="imu0_differential" value="false"/>
  
      <param name="odom0_relative" value="false"/>
    
      <param name="imu0_relative" value="true"/>
     
      <param name="imu0_remove_gravitational_acceleration" value="true"/>
    
      <param name="print_diagnostics" value="true"/>
    
      <param name="odom0_queue_size" value="2"/>
      <param name="odom1_queue_size" value="1"/>
      <param name="pose0_queue_size" value="10"/>
      <param name="twist0_queue_size" value="3"/>
      <param name="imu0_queue_size" value="10"/>
  
      <param name="odom0_pose_rejection_threshold" value="5"/>
      <param name="odom0_twist_rejection_threshold" value="1"/>
      <param name="pose0_rejection_threshold" value="2"/>
      <param name="twist0_rejection_threshold" value="1.2"/>
      <param name="imu0_pose_rejection_threshold" value="0.3"/>
      <param name="imu0_twist_rejection_threshold" value="0.1"/>
      <param name="imu0_linear_acceleration_rejection_threshold" value="0.1"/>
     
      <param name="debug"           value="false"/>
      <!-- Defaults to "robot_localization_debug.txt" if unspecified. -->
      <param name="debug_out_file"  value="debug_ekf_localization.txt"/>
  
      <rosparam param="process_noise_covariance">[0.05, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0.05, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0.06, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0.03, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0.03, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0.06, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0.025, 0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0.025, 0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0.04, 0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0.01, 0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.01, 0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.02, 0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.01, 0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.01, 0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.015]</rosparam>
   
           <rosparam param="initial_estimate_covariance">[1e-9, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    1e-9, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    1e-9, 0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-9,  0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-9,  0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1e-9,  0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1e-9, 0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-9, 0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]</rosparam>
    
    </node>
</launch>

Originally posted by Marcus Barnet on ROS Answers with karma: 287 on 2016-07-19

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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 http://answers.ros.org/upfiles/14550571761943305.jpg

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


Originally posted by matlabbe with karma: 6409 on 2016-07-20

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by Marcus Barnet on 2016-07-20:
Thank you for your support. Is it possible to add laser scan in rtabmap node without having problems with the 6DoF motion estimation?

Comment by matlabbe on 2016-07-21:
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.

Comment by Marcus Barnet on 2016-07-21:
Unfortunately, I can't understand how to proceed. Do I have to only add this line: 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?

Comment by matlabbe on 2016-07-22:
Indeed sensor_fusion.launch doesn't start a GUI. You can start RVIZ if you want. Updated end of the answer above...

$\endgroup$

Your Answer

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