Attempting to replicate the IROS 2014 Kinect Challenge winner setup

[dafritz]

Hey,

I've been trying to replicate the setup of the winners of the iros 2014 challenge but I'm kind of a beginner with ROS. I'm also using an asus xtion pro instead of a kinect and an iRobot Create 2 as the base. I've modified the launch files to account for this (I think) but I don't think I fully understand what my tf tree should look like. I've gone through the ROS tf tutorials but I'm still kind of lost. Right now it is just the tree generated by the asus camera with camera_link as the parent. I've also tried launching a node for the world to map tf broadcaster but it doesn't seem to do anything. Any guidance on how I should proceed?

Here is the rqt visualization after running the launch file:


And here is the code from the actual launch file itself:
<launch>

    <node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan">
        <remap from="image" to="/camera/depth_registered/image_raw"/>
        <remap from="camera_info" to="/camera/depth_registered/camera_info"/>
        <remap from="scan" to="/base_scan"/>
       
    </node>
  <group ns="rtabmap">
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">


          (param name="subscribe_depth" type="bool" value="true"/)
          (param name="subscribe_laserScan" type="bool" value="true"/)

          <remap from="odom" to="/base_controller/odom"/>
          <remap from="scan" to="/base_scan"/>

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

          (param name="queue_size" type="int" value="10"/)

          (!-- RTAB-Map's parameters --)
          (...)
    </node>
    <node pkg="rtabmap_ros" type="grid_map_assembler" name="grid_map_assembler"/>
  </group>
  <node pkg="rviz" type="rviz" name="rviz"/>
</launch>


Any help would be greatly appreciated.
Thanks

[matlabbe]

Hello dafritz,

What are your launch files used to bring-up the robot (motor, odometry, basic wheel controller, ...)? In the Kinect Challenge, the odometry input of rtabmap was taken from the Pioneer robot. The setup was based on this configuration:


If you have a XTion LIVE Pro, you should use openni2_launch (with argument "depth_registration:=true"). Verify that these topics are published:

$ rostopic hz /camera/rgb/image_rect_color
$ rostopic hz /camera/depth_registered/image_raw
$ rostopic hz /camera/rgb/camera_info
$ rostopic hz /base_controller/odom      (well, your odometry topic name from Create)
$ rostopic hz /base_scan                 (your fake laser scan from the XTion)

Normally, with an appriopriate bring-up of the robot Create, you should have TF published like (the /base_footprint frame may not be here):
/odom -> /base_footprint -> /base_link

To see your tf tree (you can see it also in RVIZ with TF type):

$ rosrun tf view_frames
$ evince frames.pdf

So you only have to set a static_transform_publisher between /base_link -> /camera_link. Rtabmap will publish /map -> /odom.

If everything is correctly connected, rtabmap will publish /rtabmap/mapData topic at 1 Hz:

$ rostopic hz /rtabmap/mapData

After that, grid_map_assembler should assemble a 2d occupancy grid map with the /rtabmap/mapData. This 2d occupancy grid map can be after used for navigation like in the navigation tutorials of the turtlebot. You may also look at this demonstration of rtabmap with turtlebot.

cheers

[dafritz]

Thanks for the reply matlabbe,

I'm still working on the launch files but the plan is to be outputting geometry_msgs/Twist messages from the Create 2 to a node that derives (I guess it's integrates technically) a pose estimation from the velocity info received in the Twist message and then combines it to output a nav_msgs/Odometry message to the "odom" topic. At that point, I wouldn't have to remap the info publishing to the "odom" topic, right?

Also, my openni2.launch file works correctly and is publishing to the appropriate topics. I also have the static transform publisher running correctly as my TF is published base_link -> camera_link -> all other camera tf info. And just to make sure, are you saying the correct final TF tree (assuming there is no base_footprint) should be map -> odom -> base_link -> camera_link where my launch files for the Create 2 have to take care of the odom -> base_link tf?

Thank you!

[matlabbe]

1) Yes, you will only need to connect to "odom" topic of rtabmap node.

2) Yes, the final tree should be something like /map -> /odom -> /base_link -> camera_link, where /map -> /odom will be published by rtabmap node. For /odom -> /base_link, either you take care of it (integrating Create's velocity or wheel encoder) or you can use rgbd_odometry to generate it. However, I still recommend the first approach if you can have a relatively good odometry with Create, which would be like in the Kinect Challenge. With rgbd_odometry, the odometry can be lost sometimes.

cheers

[dafritz]

Alright I have the Create 2 publishing odometry messages in a geometry_msgs/Twist form which I then integrate to get a pose estimate and output both of them as a nav_msgs/Odometry message to the "odom" topic. I also broadcast an odom -> base_link transform in this node. However, I still have two problems.

1) The rtabmap node is not subscribing to the "odom" topic. I thought I found this in the OdomToTFNode.cpp code but after running my launch file, the rtabmap node still doesn't subscribe to it. (Also do I need to change the public variables

frameId_(""),
odomFrameId_("")
to
frameId_("/base_link"),
odomFrameId_("/odom") ?)

I noticed this file subscribes to rtabmap/odom so I tried remapping to that but that also didn't work. I found out that the odom_to_tf_node is also not a valid executable so maybe there's something wrong with the makefile?

2) The rtabmap node also doesn't publish the map -> odom transform. The problem isn't that it isn't linked to my tf tree, it just doesn't publish it at all. My current tf tree is just odom -> base_link -> camera_link. I thought the problem could be odom vs /odom and base_link vs /base_link but changing those in my odometry publisher didn't affect the outcome at all. (Does the /... vs ... matter?)

I'm guessing both problems could be due to some bug with building the package and the odom_to_tf_node just isn't being made properly. Any thoughts?

Thank you for all the help!

[matlabbe]

1) To remap your "odom" message:

<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">

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

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

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

          (...)
</node>

You will see in the terminal when initializing to which topics rtabmap is subscribing. If any of them is not published, rtabmap won't start and /map -> /odom frame won't be published. You should not use odom_to_tf_node unless you use it to generate a TF for odom (but normally, your odometry node should take care of it already). For message synchronization, your odometry message should have a valid stamp and frames:

nav_msgs::Odometry odom;
odom.header.stamp = ros::Time::now();
odom.header.frame_id = "odom";
odom.child_frame_id = "base_link";

 
If your node is publishing also a TF "odom" -> "base_link", another way is to use TF for odometry instead of the message:

<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">

          <param name="subscribe_depth" type="bool" value="true"/>
          <param name="subscribe_laserScan" type="bool" value="true"/>
          <param name="odom_frame_id" type="string" value="odom"/>

          <remap from="scan" to="/base_scan"/>

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

          (...)
</node>

when "odom_frame_id" is set, rtabmap won't subscribe to an odometry topic, but will use TF instead with the frame set.

2) rtabmap will not publish "map" -> "odom" until its subscribed topics are correctly set. The "/" of the topic names matters. A leading "/" tells that the topic is in global namespace, without "/" a relative namespace is used. See Remapping on ROS wiki. In your example of the first post, rtabmap node is in the "rtabmap" namespace (<group ns="rtabmap>)


cheers

[dafritz]

Got it. Now it is subscribing to the correct "odom" topic and the tf tree appears to be correct. However, the topics /rtabmap/grid_map and /rtabmap/proj_map are still empty. They are being published though. Is there a parameter I have to change to true to get this to work?

I'm assuming both are empty because rviz says "no map received" when I add a grid map and when I echo any messages on them through rostopic echo, I get nothing back. (Not sure if I'm supposed to since it's map data anyway)

Attached is my launch file:

create_mapping.launch

Edit: rtabmap/mapData is not empty though. It definitely has data

[matlabbe]

Hello dafritz,

Looking your launch file, you need to set depth_registration parameter for OpenNI2:

<include file='$(find openni2_launch)/launch/openni2.launch'>
    <arg name="depth_registration" value="true" />
</include>

If you are using the current rtabmap_ros binaries or source, you don't need the grid_map_assembler node.

As you are using [fake] laser scans, you should be able to have the three types of map created by rtabmap:
- "/rtabmap/grid_map" (OccupancyGrid): created with laser scans
- "/rtamap/proj_map" (OccupancyGrid): created from 3D cloud projection
- "/rtabmap/cloudmap" (PointCloud2):  3D point cloud

If inputs of rtabmap are sent, rtabmap will publish /rtabmap/mapData at 1Hz:

$ rostopic hz /rtabmap/mapData
subscribed to [/rtabmap/mapData]
average rate: 1.010
	min: 0.990s max: 0.990s std dev: 0.00000s window: 2
average rate: 0.993
	min: 0.990s max: 1.024s std dev: 0.01660s window: 3
average rate: 0.996
	min: 0.990s max: 1.024s std dev: 0.01436s window: 4

cheers