Navigation and exploration on Turtlebot + Tango

[ibd]

Hi,

I was researching the possibility to run navigation with RTAB-Map and the turtlebot. I'm currently testing the Tango tablet in combination with RTAB-Map, and I was wondering if all 3 things could be combined. I found these 2 related threads:
http://official-rtab-map-forum.206.s1.nabble.com/Autonomous-Navigation-td801.html
http://official-rtab-map-forum.206.s1.nabble.com/Demo-RTAB-Map-on-Turtlebot-td439.html
I'm also using the launch files from the wiki on using Tango + RTAB-Map.

1. What would be required to setup Tango on turtlebot, RTAB-Map & navigation? From my understanding, I should be able to run it using the tutorial provided on the wiki. However, since I'm not using the Kinect, and I'm also using a different sensor placement, do I have to change the robot URDF (or similar config file)?

2. Is it possible to run navigation and mapping at the same time (i.e., NOT in localization mode)? The reason that this would be cool is because you could do autonomous exploration with a frontier-based approach[1].

3. When navigating using code (not using RVIZ), will RTAB-Map provide other nodes with feedback (goal reached, loop closure done), or will it be move_base that provides the feedback?

Thanks a lot.
Kind regards
ibd


[1]: Yamauchi, Brian. "A frontier-based approach for autonomous exploration." Computational Intelligence in Robotics and Automation, 1997. CIRA'97., Proceedings., 1997 IEEE International Symposium on. IEEE, 1997.

[matlabbe]

Hi,

1. Yes, URDF should be updated, or add a custom static_transform_publisher. Note that Tango would give pose accordingly to Tango device, not robot /base_link. If you are using turtlebot's odometry and just using Tango for RGB/depth images (like a kinect), there would be no problems. But if you want to use Tango's odometry, the TF tree may need to be inverted for easier integration. Instead of the standard /odom -> /base_link -> /camera_link, that would be (using Tango ROS Streamer frame names) /start_of_service -> /device -> base_link (while setting rtabmap's frame_id to "device" instead of "base_link").

2. Yes, totally possible. The localization mode is just setting the map as non-incremental.

3. Instead of sending goals on move_base_simple/goal topic of move_base directly (like in the turtlebot tutorial), by code use the ActionLib interface to get feedback of goal reached or failed: http://wiki.ros.org/navigation/Tutorials/SendingSimpleGoals. If you are going to do exploration, you may want to publish on /rtabmap/goal topic so that the goal is linked to map's graph and subscribe to /rtabmap/goal_reached to know if goal has failed or succeeded. The advantage is that if the map gets optimized (after a loop closure), the goal will be republished to move_base accordingly to optimized map (use_action_for_goal should be true, see rtabmap_ros wiki).

cheers,
Mathieu

[ibd]

Hi Mathieu

1. Thank you for the hint about static_transform_publisher. That should make it easier to quickly change and debug configurations. If I understood you directly, static_transform_publisher would then publish with target frame = "base_link", and reference frame = "device", and it would contain the transformation from the tango tablet to the turtlebot's base (or wherever it expects "base_link" to be)?

2. Thanks, I will research this more later.

3. What is the expected behavior of my node: Should I only publish to /rtabmap/goal, and RTAB-map will then communicate with move_base itself? Or should I publish to both (through action lib, as you referenced)? Because of loop closures, I would expect that RTAB-Map has its own ability to communicate with move_base (as you described).

Best
Isaac

[matlabbe]

Hi Isaac,

1. Yes, and you can the use RVIZ afterward to debug TF (show TF display)

3. Yes, rtabmap will communicate with move_base. Look at the terminal if there are any problems.

cheers,
Mathieu

[ibd]

Hi Mathieu

Some things I still don't quite understand.

1. Should the "explore" node communicate with RTAB-map through an ActionLib server/client, or should it publish to the goal topic using regular ros publishers?

2. How is the map server ("map_server") set up? The "RTAB-map on turtlebot" tutorial refers to another tutorial (http://wiki.ros.org/turtlebot_navigation/Tutorials/Autonomously%20navigate%20in%20a%20known%20map), which in turn just publishes a static map from a previously generated .yaml file. How would I connect the nodes and configure RTAB-map so that the current (online!) map is used instead of a previously generated (offline) map?

3. The tutorial above will launch turtlebot navigation stack with "amcl_demo.launch". This launch file will start Kinect nodes, it will start a map server, and it will start some move_base related stuff. If I understood correctly, when used in conjunction with RTAB-map, I would only need to launch the move_base part of the whole navigation stack of turtlebot. Correct?
- AMCL is replaced by the (optimized) RTAB-map odometry, or in my case, tango odometry.
- map server is replaced by RTAB-map map server(?)
- Kinect stuff is replaced by Tango data

Thank you so much for your efforts.

Cheers
- ibd

[matlabbe]

Hi ibd,

1. RTAB-Map doesn't have actionlib interface, we should have a regular publisher to send goals to /rtabmap/goal and a regular subscriber to receive /rtabmap/goal_reached (for status).

2. On this rtabmap+turtlebot tutorial, you don't need a map_server when using rtabmap, rtabmap acts already like a map_server. AMCL is the package to localize with a map published with map_server. Rtabmap also replaces amcl as it does already localization, just launch demo_turtlebot_mapping.launch with "localization:=true", rtabmap will publish the map and localize in it.

3. Yes, see the tutorial referred above, rtabmap replaces gmapping, amcl and map_server.

cheers,
Mathieu

[ibd]

Hi Mathieu.

So far, so good. I got the robot moving, however it is not working correctly yet.
The reason is that I didn't link the TF frames correctly.

The problem I now have is that turtlebot publishes "odom" -> "base_footprint", whereas I previously created a different transformation using a static_transform_publisher. See attached screenshot.
What I did is to add "start_of_service" -> "odom" as an identity transform,
"device" -> "base_link" as the corresponding transform as you mentioned,
and "base_link" -> "base_footprint" as an identity transform (what I expect to be valid according to visualization in RVIZ).
This works fine, but the resulting visualization jitters between my (seemingly correct) tree and the broken tree that contains the "odom" -> "base_footprint" transformation from turtlebot.

My idea to fix this would be stop turtlebot from publishing an odom frame, but it seems to be embedded quite deeply in the turtlebot nodes.
I know this question is not related to RTAB-map, but do you have any idea how to fix the conflicting TF tree?

Thank you.

Cheers
- ibd

[ibd]

I solved the problem by setting "publish_tf" to "false" in the kobuki base configuration!

I am now running into a new problem which is related to the following problem:
http://answers.ros.org/question/40541/extrapolation-error-of-local-cost-map-in-navigation/

Namely, move_base expects "future dated" transformations.
Currently, it throws a lot of errors in the form of:
"Extrapolation Error: Lookup would require extrapolation in the future. Requested time [...] but the latest data is at time [...]".
[...]
Could not transform the global plan to the frame of the controller
Could not get local plan"

The time difference between the desired transform and the latest published transform is usually in the order of 0.01 s.

Possible solutions:
- Modify move_base to include a waitForTransform. Downside is that requires a lot of re-compilation.
- Modify RTAB-map to future date the "map" -> "start_of_service" (in the Tango case) transform.

I will try out the second solution.
What would be the downsides of future dating the "map" -> "start_of_service" by about 0.1 - 0.2 seconds?

Edit: Fixed by adding "tf_tolerance = 0.3" as a parameter.

As an additional remark, I am pretty sure that the Tango and the computer are reasonably well time-synchronized, and that the error is not coming from that source.

Best
- ibd

[ibd]

Apologies for triple post.

Basic navigation is now working! Thank you for the help, Mathieu!

Currently, there are 2 issues that I am trying to resolve:
1. The map is too high above the ground. Not sure how to solve this, I would have to put a fixed offset somewhere that relates start_of_service with the height of the turtlebot.
2. The local costmap is empty. My guess is that this is because I have to configure /tango/point_cloud as a sensor input? It might also be related to issue #1. I did see some erratic local cost maps occasionally when I lifted up the robot as it was running into things (robot radius set too small), but I couldn't reproduce it very well.

Once it is working completely, I will try to make a short video of operation!

Cheers!
- ibd

[matlabbe]

Hi ibd,

1. By default, Tango starts at a fixed height over the ground. If you know that tango is always starting from the same height (e.g., it is fixed to robot), you may add a static transform between map and start_of_service with an offset along Z axis.

2. The local costmap is created from move_base. What is the local costmap configuration (obstacle layer). If you are doing similar to default turtlebot configuration, how the fake laser scan is generated? using depthimage_to_laserscan or pointcloud_to_laserscan? You may use pointcloud_to_laserscan if the depth camera is tilted.

cheers,
Mathieu

[ibd]

Hi!

1. Since RTAB-map publishes this transform already (as an identity transform), how can I stop it from publishing that? I haven't found if there is a parameter to adjust this inside RTAB-map, either.

2. Thank you, I'm currently finding out what is the best way to solve this.

Cheers!
- ibd

[matlabbe]

Hi ibd,

I added a new demo for Tango on Turtlebot that could help you with TF, see this page: http://wiki.ros.org/rtabmap_ros/Tutorials/Tango%20ROS%20Streamer#Turtlebot



cheers,
Mathieu

[ibd]

Thank you Mathieu!

edit2: It works!



I had to make sure that I changed the frame_id's from the old values (as given in the tango ros streamer + rtab-map tutorial) to the ones shown at the linked tutorial. Some of the parts of the TF tree get inverted by doing that, and it is necessary for this setup to work!

Best
Isaac

[ibd]

Hello

To profit from updated goal positions when the map is updated (e.g., loop closures), I am now trying to switch from "/move_base_simple/goal" to "/rtabmap/goal". However, whenever I send a goal pose to "/rtabmap/goal", the robot will not move. I can also see that move_base is not receiving a new goal. I also did not get any messages on "/rtabmap/goal_reached"!

1. Do I need to remap from "/rtabmap/goal_out" to "/move_base_simple/goal"? In the tutorials, I was unable to find such a remapping.

2. What topic should I listen to to see the navigation status? It seems I have 3 possibilities, "/rtabmap/goal_reached" (but no output from this in my experiments), "/move_base/status" (continuous stream of status), or "/move_base/result" (only get new result when goal is reached / navigation failed). When rtab-map creates a new goal after map update, will the the "/move_base/*" topics react to the change?

Thank you!
- ibd

[matlabbe]

Hi,

Either remap "/rtabmap/goal_out" (not "/rtabmap/goal") to "/move_base_simple/goal" or set "use_action_for_goal" parameter to true like here.

If "use_action_for_goal" is used, "/rtabmap/goal_reached" will be published when goal is reached or when move_base fails.

If "/rtabmap/goal_out" is used, "/rtabmap/goal_reached" will be published only when goal is reached.

Note that if "RGBD/PlanStuckIterations" > 0, "/rtabmap/goal_reached" will be published when rtabmap detected it is stuck.

I am not sure if you can get feedback from move_base when rtabmap is publishing goals through actionlib ("use_action_for_goal"=true).

cheers,
Mathieu

[ibd]

Thank you Mathieu,

I figured out that the feedback is better if I remap from goal_out to move_base_simple/goal, since it will not generate a new goal every iteration. By using the ActionLib interface, a lot of new goals are generated which makes handling the feedback much harder.
Also, using ActionLib, I cannot really tell when navigation fails. The robot will just sit there and do nothing, while rtabmap will just keep publishing that goal.

Furthermore, there is some other quirky behavior that I do not yet fully understand. For example, it seems to be willing to navigate to unknown territory sometimes, but only sometimes. It may or may not be related to the distance away from the current map.

Finally, what is the best way to setup the cost maps? I currently have the following for the tango mounted on the robot:

obstacle_layer:
  enabled:              true
  max_obstacle_height:  1.8
  origin_z:             0.0
  z_resolution:         0.1
  z_voxels:             12
  unknown_threshold:    11
  mark_threshold:       2
  combination_method:   1
  track_unknown_space:  true    #true needed for disabling global path planning through unknown space
  obstacle_range: 3.0
  raytrace_range: 3.0
  origin_z: 0.0
  z_resolution: 0.2
  z_voxels: 10
  publish_voxel_map: false
  observation_sources: tango
  tango:
    data_type: PointCloud2
    topic: tango/point_cloud
    marking: true
    clearing: true
    min_obstacle_height: 0.2
    max_obstacle_height: 1.8
    # expected_update_rate: 0.5

The main problem is that the global costmap is too sensitive to noise, while the local cost map is not sensitive enough to information. Would the approach that you described here work better? http://official-rtab-map-forum.206.s1.nabble.com/Navigation-Stack-rtabmap-requesting-map-configuration-problem-tp1378p1488.html
What is the reason for using direct sensor data instead of using rtabmap's nice pre-processed clouds?
Is it because moving obstacle detection will be too slow when using pre-processed clouds?

Thanks for the hint about "RGBD/PlanStuckIterations", I will need to study rtabmap's parameters in detail. There's so many of them!

Thank you again very much.

Kind regards
ibd

[matlabbe]

Hi,

Normally rtabmap would publish new goals on topic or actionlib at the same rate (they are called in the same function here).

move_base tries to plan in known territories in priority. If "allow_unknown" parameter of the global planner is set (like in this example), it will plan in unknown areas.

Tip:Open rqt_reconfigure to play online with all move_base's parameters (and know which ones are actually used).

Why it should be the sensor data is that obstacle_layer requires it. The obstacle_layer should be updated as fast as possible, and should be independent of the SLAM node. If the point cloud of rtabmap is used in obstacle_layer, it will be treated like a raw sensor, and ray tracing will be done for every points, which would use a lot of computing resources. The static_layer should be already a copy of rtabmap's point cloud. You can also test the global costmap without an obstacle_layer too to see the difference.

For planning parameters, do:

$ rosrun rtabmap_ros rtabmap --params | grep RGBD/Plan
Param: RGBD/PlanAngularVelocity = "0"                      [Angular velocity (rad/sec) used to compute path weights.]
Param: RGBD/PlanLinearVelocity = "0"                       [Linear velocity (m/sec) used to compute path weights.]
Param: RGBD/PlanStuckIterations = "0"                      [Mark the current goal node on the path as unreachable if it is not updated after X iterations (0=disabled). If all upcoming nodes on the path are unreachabled, the plan fails.]

$ rosrun rtabmap_ros rtabmap --params | grep Goal
Param: RGBD/GoalReachedRadius = "0.5"                      [Goal reached radius (m).]
Param: RGBD/GoalsSavedInUserData = "false"                 [When a goal is received and processed with success, it is saved in user data of the location with this format: "GOAL:#".]

You may just tune "RGBD/GoalReachedRadius" and "RGBD/PlanStuckIterations".

cheers,
Mathieu

[ibd]

Much appreciated.

1. Should I use costmap_2d::VoxelLayer or costmap_2d::ObstacleLayer for the obstacle_layer? The documentation says that VoxelLayer can deal with 3d data, and the turtlebot navigation uses it, however you use ObstacleLayer in your linked example.

2. I will replace costmap_2d::StaticLayer with rtabmap_ros::StaticLayer. I have not run into any issues with the global costmap, but it cannot hurt to use it. Does this plugin need additional setup?

3. I'm not perfectly clear about the parameter RGBD/PlanStuckIterations. For example, if RTAB-map runs at 3 Hz update rate, and I want to wait 4s before declaring a goal as unreachable, should I set it to 3*4 = 12? What causes a goal node to be updated, are you referring to the status of "goal reached"?

edit: for clarification, here's the output of "rostopic echo /move_base/status" if I use the actionlib interface:
https://pastebin.com/MnzjQpzZ
rtabmap will create new ids every update, multiple times per second.

OTOH, if I hook up "goal_out" to "move_base_simple" directly, I run into the same error as here: http://official-rtab-map-forum.206.s1.nabble.com/Navigation-Stack-rtabmap-requesting-map-configuration-problem-tp1378p1484.html
Next, I will try to replace the raw tango point cloud that I'm using now with the filtered output of that rtabmap nodelet. Maybe it will prevent this crash...

Also, I just noticed that I do not get any local path planning in RVIZ. For the global plan, I get a straight line. Something seems a bit off...

Merci.
- ibd

[matlabbe]

Hi,

1. The difference between VoxelLayer and ObstacleLayer is that VoxelLayer will keep in cache a 3D volume instead of a 2D grid for ray tracing. The volume is after projected to ground to create the 2D obstacles. So there is a slightly difference on how the obstacle cells are updated, but ObstacleLayer would be faster.

2. It doesn't need additional setup. The bug it fixes is explained here: https://github.com/ros-planning/navigation/issues/320

3. Rtabmap keeps track of the sub goals reached so far. The sub goals are the nodes on the planned path on the map's graph. If the robot hasn't reach a subgoal since last X iterations, it is considered stuck and goal is canceled. Yes, if update rate is 3 Hz and you want to fail after 4 seconds, then set "RGBD/PlanStuckIterations" to 12.

Rtabmap publishes a new goal each time it is updated (in your case, at 3 Hz), even if the actual goal pose didn't change (no map corrections). I never tried planning with rtabmap update rate other than 1 Hz, but I can see the problem when the rate is higher. I added an issue for this.

For the global plan straight line, if it is through obstacles, this may be caused by the bug linked on 2 above.

cheers,
Mathieu