Autonomous Navigation

[g.bartoli]

Hi Mathieu,
sorry again for all these questions, I'm a beginner with ROS and actually I'm learning it using rtabmap in this project, so I just need some directions on how to proceed, I'll do the rest all by myself... you're very kind to take the time to illustrate your system on this forum! :)

On our checklist, before eventually working on improving final accuracy, the next thing to do now is to try the autonomous navigation inside an already mapped environment. We would like to improve our "localization.launch" to add the possibility to make the robot going from one place to another computing the shortest path and dynamically avoiding obstacles.

Actually, the robot is equipped with wheel odometry and Asus Xtion without laser rangefinder, so we are in this configuration. Instead of "depthimage_to_laserscan", I'm using directly the "gen_scan" parameter to extract the fake laser scan from the RGBD sensor and this is correctly shown in rtabmapviz with a cyan line running along the walls and accumulating together with the map point cloud.

1) Is "gen_map" option sufficient for providing the "grid_map" topic?
2) Regarding the data needed by the navigation stack, what is the difference between using "grid_map" or "proj_map"? Can just the second one be used?
3) If we want to setup the whole navigation stack (global_costmap, local_costmap, point_cloud, etc...), is our current rtabmap configuration and an adaptation of this launch file for our hardware all that is needed or do we have to spawn other nodes?

Many thanks,
Guido

[matlabbe]

Hi Guido,

1) Do you mean "gen_scan"? Yes, grid_map should be published.
2) Both can be used. You can compare them in RVIZ to see which one would fit best for your needs.
3) If your robot controller is subscribing to a /cmd_vel topic and that you are able to create a map with rtabmap, you can add only this part of the referred launch file to spawn move_base while modifying the yaml files for your robot and remapping the topics:

<group ns="planner">
        <remap from="openni_points" to="/planner_cloud"/>
        <remap from="base_scan" to="/base_scan"/>
        <remap from="map" to="/map"/>
        <remap from="move_base_simple/goal" to="/planner_goal"/>
        
        <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    	  <rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
     	  <rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
    	  <rosparam file="$(find rtabmap_ros)/launch/azimut3/config/local_costmap_params_3d.yaml" command="load" />
    	  <rosparam file="$(find rtabmap_ros)/launch/azimut3/config/global_costmap_params.yaml" command="load" />
    	  <rosparam file="$(find rtabmap_ros)/launch/azimut3/config/base_local_planner_params.yaml" command="load" />
        </node>
</group>

This tutorial can be useful to understand how to setup move_base with your robot. You can refer to this RTAB-Map on Turtlebot tutorial too.

cheers

[g.bartoli]

1) Yes, sorry, I mispelled the name. We verified that "gen_scan" is publishing the "grid_map".
2) I think we'll use "grid_map", since "proj_map" is messier and full of holes, but it's strange because the 3D Cloud Map seems consistent and well aligned. Maybe there is some problem with projecting ceiling points to the ground.
3) We used your launch file as reference, but there are some problems with topics and data needed by "move_base" as explained here.
Launching the "move_base" node just remapping "odom" to "/odom", but without changing other parameters or remapping arguments, this is the ROS graph showing that move_base needs two footprints and a goal.



These are the topics published by rtabmap and the ones subscribed by move_base.

$ rosnode info /planner/move_base

Subscriptions: 
 * /planner/move_base/local_costmap/footprint [unknown type]
 * /planner/move_base/cancel [unknown type]
 * /planner/move_base_simple/goal [unknown type]
 * /odom [nav_msgs/Odometry]
 * /planner/move_base/global_costmap/footprint [unknown type]
 * /tf_static [unknown type]
 * /planner/move_base/goal [move_base_msgs/MoveBaseActionGoal]
 * /tf [tf2_msgs/TFMessage]

$ rosnode info /rtabmap/rtabmap

Publications: 
 * /rtabmap/goal_reached [std_msgs/Bool]
 * /rtabmap/goal_out [geometry_msgs/PoseStamped]
 * /rtabmap/labels [visualization_msgs/MarkerArray]
 * /rtabmap/mapGraph [rtabmap_ros/MapGraph]
 * /rtabmap/mapData [rtabmap_ros/MapData]
 * /rtabmap/cloud_map [sensor_msgs/PointCloud2]
 * /rtabmap/local_path [nav_msgs/Path]
 * /rtabmap/grid_map [nav_msgs/OccupancyGrid]
 * /rtabmap/move_base/cancel [actionlib_msgs/GoalID]
 * /tf [tf2_msgs/TFMessage]
 * /rtabmap/move_base/goal [move_base_msgs/MoveBaseActionGoal]
 * /rosout [rosgraph_msgs/Log]
 * /rtabmap/info [rtabmap_ros/Info]
 * /rtabmap/proj_map [nav_msgs/OccupancyGrid]
 * /rtabmap/global_path [nav_msgs/Path]

Looking at these "rosnode info" output and your azimut3 launch file, you remap topics that are not currently published by any node, like "/planner_cloud" or "/map", but also some "from" topics like "openni_points" or "base_scan" seem that are not needed by "move_base".

Maybe there are some topics that are implicitly loaded from the yaml files?
For example, we tried to change parameters inside the common_costmap file, but nothing seems to change. Do you have any suggestions on how to proceed?

Many thanks,
Guido

[matlabbe]

2) I will re-verify for the alignment. "grid_map" is also faster to generate.
3) Well, I have never tried move_base without setting any parameters, but on my side I have:

$ roscore

Create fake TF to make move_base working (only for testing!)
$ rosrun tf static_transform_publisher 0 0 0 0 0 0 /odom /base_link 100
$ rosrun tf static_transform_publisher 0 0 0 0 0 0 /map /odom 100

$ rosrun move_base move_base

For this simplified launch, the topics used for navigation are "/tf", "/odom" (not sure about "/odom" though, since it can be get with tf), "/move_base/simple_goal" and "/cmd_vel". I'll show next how to setup the /map for the global costmap and obstacle_layer for the local costmap.

For the stereo navigation tutorial, there is a mismatch with the yaml files on GitHub and the ones shown on the page. You should refer to yaml files shown on the tutorial page to reproduce exactly the experience. Note that remapping arguments under move_base is for remapping those in the yaml files.

For your robot configuration, I've made here some yaml files to help you (I omitted obstacle detection for simplification):
EDIT: expected_update_rate should be 0.1 instead of 10. It is in seconds, not Hz.

Files

costmap_common_params.yaml (notice the "observation_sources" parameter)

footprint: [[ 0.3,  0.3], [-0.3,  0.3], [-0.3, -0.3], [ 0.3, -0.3]]
footprint_padding: 0.02
inflation_layer:
  inflation_radius: 0.5
transform_tolerance: 2

obstacle_layer:
  obstacle_range: 2.5
  raytrace_range: 3
  max_obstacle_height: 0.4
  track_unknown_space: true

  observation_sources: laser_scan_sensor

  laser_scan_sensor: {
    data_type: LaserScan,
    topic: scan,
    expected_update_rate: 0.1, 
    marking: true,
    clearing: true
  }

local_costmap_params.yaml

global_frame: odom
robot_base_frame: base_footprint
update_frequency: 2
publish_frequency: 2
rolling_window: true
width: 3.0
height: 3.0
resolution: 0.025
origin_x: 0
origin_y: 0

plugins:
  - {name: obstacle_layer,   type: "costmap_2d::ObstacleLayer"}
  - {name: inflation_layer,  type: "costmap_2d::InflationLayer"}

global_costmap_params.yaml

global_frame: map
robot_base_frame: base_footprint
update_frequency: 2
publish_frequency: 2
always_send_full_costmap: false
plugins:
   - {name: static_layer, type: "rtabmap_ros::StaticLayer"}
   - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
   - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

base_local_planner_params.yaml

TrajectoryPlannerROS:

  # Current limits based on AZ3 standalone configuration.
  acc_lim_x:  0.75
  acc_lim_y:  0.75
  acc_lim_theta: 4
  max_vel_x:  0.5
  min_vel_x:  0.24
  max_vel_theta: 0.5
  min_vel_theta: -0.5
  min_in_place_vel_theta: 0.25
  holonomic_robot: true

  xy_goal_tolerance:  0.25
  yaw_goal_tolerance: 0.25
  latch_xy_goal_tolerance: true
  
  # make sure that the minimum velocity multiplied by the sim_period is less than twice the tolerance on a goal. Otherwise, the robot will prefer to rotate in place just outside of range of its target position rather than moving towards the goal.
  sim_time: 1.5 # set between 1 and 2. The higher he value, the smoother the path (though more samples would be required).
  sim_granularity: 0.025
  angular_sim_granularity: 0.05
  vx_samples: 12
  vtheta_samples: 20

  meter_scoring: true

  pdist_scale: 0.7 # The higher will follow more the global path.
  gdist_scale: 0.8
  occdist_scale: 0.01
  publish_cost_grid_pc: false

#move_base
controller_frequency: 10.0 #The robot can move faster when higher.

#global planner 
NavfnROS:
    allow_unknown: true
    visualize_potential: false

navigation.launch

<launch>
  <arg name="scan_topic" default="/scan"/>
  <arg name="map_topic" default="/map"/>

  <!-- ROS navigation stack move_base -->
  <remap from="scan"             to="$(arg scan_topic)"/>  <!-- see costmap_common_params_2d.yaml -->
  <remap from="map"              to="$(arg map_topic)"/>   <!-- see global_costmap_params.yaml -->
        
  <node pkg="move_base" type="move_base" respawn="true" name="move_base" output="screen">
     <param name="base_global_planner" value="navfn/NavfnROS"/>
     <rosparam file="$(env HOME)/costmap_common_params.yaml" command="load" ns="global_costmap" />
     <rosparam file="$(env HOME)/costmap_common_params.yaml" command="load" ns="local_costmap" />
     <rosparam file="$(env HOME)/local_costmap_params.yaml" command="load" ns="local_costmap" />
     <rosparam file="$(env HOME)/global_costmap_params.yaml" command="load" ns="global_costmap"/>
     <rosparam file="$(env HOME)/base_local_planner_params.yaml" command="load" />
  </node>
</launch>

Example

If you put all these files in $HOME path and launch navigation.launch like the following:

$ roslaunch freenect_launch freenect.launch depth_registration:=true

$ rosrun depthimage_to_laserscan depthimage_to_laserscan image:=/camera/depth_registered/image_raw camera_info:=/camera/depth_registered/camera_info

$ rosrun tf static_transform_publisher 0 0 0 0 0 0 /base_footprint /camera_link 100

$ roslaunch rtabmap_ros rgbd_mapping.launch frame_id:=base_footprint rtabmap_args:="--delete_db_on_start" rtabmapviz:=false subscribe_scan:=true

$ roslaunch navigation.launch map_topic:=/rtabmap/grid_map

$ roslaunch rtabmap_ros demo_turtlebot_rviz.launch
(In RVIZ, you can add TF and change map topic to "/rtabmap/grid_map", you can also send 
 goals to actually see the planned path and some commands published from move_base 
 over /cmd_vel topic)

You then have this rqt_graph:


Hope this will help to get you starting with navigation!
cheers,
Mathieu

[g.bartoli]

Mathieu,
thanks for the very detailed explanation, we are currently investigating it and tomorrow I will tell our outcomes!

Do you know where I can find some more informations about these parameters in yaml files? I understood the launch file parameters and run across the ROS Wiki many times, but the only place I found is the Navigation tutorial, but many parameters from your files are missing there, is there an official reference somewhere?

Have a nice day,
Guido

[g.bartoli]

YES!!! Following your advices we succeeded in make the Navigation Stack work together with RTAB-Map, the robot plans a path inside the mapped environment to reach the provided goal. Many many thanks for al your precious help, Mathieu! :)

After this, we made some more tests, and some other questions came along:

1) We used "depthimage_to_laserscan" as you suggested, but does the alternative "gen_scan" option also provide "/scan" other than "grid_map"?
2) As I told you before, "proj_map" is much worse than "grid_map", I made two screenshot to compare them... this not a question, it's just to show you the difference! :)



3) Do you know how we can remotely show a RGB decimated camera stream? Currently RViz runs on a remote server that, other than "grid_map" and "tf", shows the compressed "camera/rgb/image_rect_color", but sometimes it's very heavy on the bandwidth and we don't need all that resolution (you can see this in the bottom left corner of the previous images)
4) What is the correct way to show the optimized graph in RViz? We tried "rtabmap/MapGraph" plugin enabling "Download graph", but nothing happens...
5) You mentioned that, for sake of simplicity, you omitted the obstacle detection in yaml files. It seems to me that the planner already uses the local cost map to plan a local path avoiding obstacles in front of it. What are the benefits of adding the obstacle detection (plugin?), too? Do you have some suggestions on how to do it? Additionally, do you know where can we find an official documentation about all these plugins and parameters used by move_base? We explored the ROS Wiki, but we could not find any information other than the official "move_base" tutorial, but your files contains a lot of parameters that seems not elsewhere documented
6) Connected to 5), how can all the external parameters supported by a generic node be shown? Your "rtabmap" supports "--params" argument, but "move_base" seems to not have that
7) I checked the Wiki, but I could not find a way to set the vertical height (image row) of the scanline extracted by "depthimage_to_laserscan". Do you know if it is possible or do I have to implement a custom node by myself to do this?

Thanks again,
Guido

[matlabbe]

Hi,

Great that you make it work!

1) The reason I used depthimage_to_laserscan is to provide /scan to move_base at high frame rate for its local costmap (obstacle detection). Outputting /scan from rtabmap would not be enough fast.

2) The camera should see the ground to fill "empty" cells. If the ground is too reflective (depending on the angle), less points will be found on the ground. To have more projections on the ground, you may also want to deactivate map filtering by setting map_filter_radius to 0:

<node type="rtabmap" ...
<param name="map_filter_radius" type="double" value="0"/>
</node>

For comparison, here some results from the Multi-session demo.

/rtabmap/grid_map (in this case a laser rangefinder was used)

/rtabmap/proj_map


3) You can throttle the images at lower framerate and use "theora" image_transport for more compression. Similar to bandwidth efficiency section in the tutorials, for convenience, you can use rtabmap_ros/data_throttle nodelet by setting "rate" and "decimation" parameters:

<group ns="camera">
    <!-- Attaching to nodelet manager from OpenNI: camera_nodelet_manager -->
    <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap/data_throttle camera_nodelet_manager" output="screen">
      <param name="rate" type="double" value="5.0"/>
      <param name="decimation" type="int" value="2"/>

      <remap from="rgb/image_in"       to="rgb/image_rect_color"/>
      <remap from="depth/image_in"     to="depth_registered/image_raw"/>
      <remap from="rgb/camera_info_in" to="rgb/camera_info"/>

      <remap from="rgb/image_out"       to="rgb/image_rect_color_throttle"/>
      <remap from="depth/image_out"     to="depth_registered/image_raw_throttle"/>
      <remap from="rgb/camera_info_out" to="rgb/camera_info_throttle"/>
    </node>
  </group>

4) Normally, if you use the rtabmap_ros/MapGraph or rtabmap_ros/MapCloud plugins, you should subscribe to /rtabmap/mapGraph and /rtabmap/mapData topics respectively. As these topics are published, you should see the map created in RVIZ.

5) I was referring to obstacles_detection nodelet of rtabmap. But yes, the /scan topic is used to create the local costmap, and thus, to avoid obstacles. Using obstacles_detection helps to detect low objects on the ground, for which the /scan topic cannot detect. To use it, you must change the "costmap_common_params.yaml" for (note that the expected_update_rate for the laser should be "0.1" instead of "10" (it is in seconds not Hz)):

footprint: [[ 0.3,  0.3], [-0.3,  0.3], [-0.3, -0.3], [ 0.3, -0.3]]
footprint_padding: 0.02
inflation_layer:
  inflation_radius: 0.5
transform_tolerance: 2

obstacle_layer:
  obstacle_range: 2.5
  raytrace_range: 3
  max_obstacle_height: 0.4
  track_unknown_space: true

  observation_sources: laser_scan_sensor point_cloud_sensorA point_cloud_sensorB

  laser_scan_sensor: {
    data_type: LaserScan,
    topic: scan,
    expected_update_rate: 0.1,
    marking: true,
    clearing: true
  }

  point_cloud_sensorA: {
    sensor_frame: base_footprint,
    data_type: PointCloud2,
    topic: obstacles_cloud,
    expected_update_rate: 0.5,
    marking: true,
    clearing: true,
    min_obstacle_height: 0.04
  }

  point_cloud_sensorB: {
    sensor_frame: base_footprint,
    data_type: PointCloud2,
    topic: ground_cloud,
    expected_update_rate: 0.5,
    marking: false,
    clearing: true,
    min_obstacle_height: -1.0 # make sure the ground is not filtered
  }

Then in the launch file, you need to provide "obstacles_cloud" and "ground_cloud" topics with obstacles_detection:

<group ns="camera">
   <!-- Attaching to nodelet manager from OpenNI: camera_nodelet_manager -->
   <!-- use topics from data_throttle at lower frame rate -->
   <node pkg="nodelet" type="nodelet" name="points_xyz_planner" args="load rtabmap_ros/point_cloud_xyz camera_nodelet_manager">
      <remap from="depth/image"            to="depth_registered/image_raw_throttle"/>
      <remap from="depth/camera_info"      to="rgb/camera_info_throttle"/>
      <remap from="cloud"                  to="cloudXYZ" />
      <param name="decimation" type="int" value="1"/>                     <!-- already decimated in data_throttle above -->
      <param name="max_depth" type="double" value="3.0"/>
      <param name="voxel_size" type="double" value="0.02"/>
    </node>
  
    <node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection camera_nodelet_manager">
      <remap from="cloud" to="cloudXYZ"/>
      <remap from="obstacles" to="/obstacles_cloud"/> <!-- move_base topic -->
      <remap from="ground"    to="/ground_cloud"/>    <!-- move_base topic -->

      <param name="frame_id" type="string" value="base_footprint"/>		
      <param name="map_frame_id" type="string" value="map"/>
      <param name="wait_for_transform" type="bool" value="true"/>
      <param name="min_cluster_size" type="int" value="20"/>
      <param name="max_obstacles_height" type="double" value="0.4"/>
    </node>  
</group>

move_base uses plugins to provide functionalities. You should look in the referred plugins page. For example, for the obstacle_layer, see here. This page is linked from costmap_2d page (see Layer specifications section).

6) I've found all the parameters with the move_base tutorials and wiki pages of its plugins. For a better understanding on how they are used, I've looked in the code of the navigation stack too.

7) There is the "scan_height" parameter that can be increased. If you want to modify its behavior, you can still clone the code and make your changes.

cheers,
Mathieu

[g.bartoli]

Ok, thanks for these info, I solved every issue following your advices, except for the obstacle detection, which we'll consider later.

Some more questions... :)

1) "rtabmap" node has "subscribe_laserScan" set to true, "grid_map" and "local_costmap" are built accordingly. In the options I saw also "RGBD/PoseScanMatching" and I thought it would be a good idea to enable it to improve localization accuracy. I tried to enable it, but when rtabmap is running I continuosly received this warning, regardless that flag is enabled or not:

[WARN] Memory.cpp:2856::computeScanMatchingTransform() Depth2D not found for signature 1

Do you know what is going wrong?

2) This is something I already asked you, but now I cannot find a workaround to make it work as expected...
We are using the "2D Navigation Goal" tool in RViz to set the goal. The path is correctly computed and the robot drives to it costantly updating the local map to avoid obstacles. But the annoying behaviour is that once the goal is set on the map, if the map is optimized by RTAB-Map the goal coordinates become invalid, since the environment is moving. The local map still works because it is centered on the robot, but the global one is continuosly oscillating even if we are in localization mode (database loaded and incremental memory = false). You told me to use the rtabmap set_goal service, but it seems to work only on already existant nodes, while the RViz Navigation Goal works in real time on any map point.
Loop closures are correctly found and this is really helpful to correct drifts and relocalize the robot, but when just moving around or standing still the map is never resting. You told that this was changed in latest build, but it seems to still happen... In localization mode, wouldn't it be better to just find correspondences in visual database to relocalize the robot and avoiding optimizing the graph? Can a flag parameter be added to control this behaviour?

Many thanks!

PS: I have to say that during the mapping phase the graph is now much more stable when the robot is standing still compared to the localization phase

[matlabbe]

Hi,

1) I tried your setup (Odom+Kinect+ScanFromKinect) on our robot. The launch file I used on the robot is this one: az3_mapping_kinect_scan.launch
"RGBD/PoseScanMatching" is true, I don't see the warnings you have. It is strange that the /grid_map is created and no depth2D is found in the nodes. Are you using a previous database for which the scans were not saved?

2) You can use the RVIZ tool ("2D Navigation Goal") to set the goal to /rtabmap/goal topic. Yes "set_goal" service only plans to a node, but with /rtabmap/goal topic you can send a pose like for move_base but the pose will be modified if there are graph optimizations. Are you using "RGBD/OptimizeFromGraphEnd"=true? I prefer to set it to true for navigation (all is relative to the current position of the robot).

I've made little videos with the launch file above:
https://youtu.be/xeRxsahGKkw
https://youtu.be/H4-Fbd8WJW0 
We can see that the map is updated when a localization occurs. The videos are in localization mode:

$roslaunch rtabmap_ros az3_mapping_robot_kinect_scan.launch localization:=true

I would say that the narrow field of view of the kinect makes it difficult to relocalize on paths where the camera was not looking almost exactly in the same direction. In the mapping phase, I did the trajectory back and forth to make sure that the robot can localize on the robot, otherwise the map would drift.

cheers,
Mathieu

[g.bartoli]

1) Well, magically that error never happened again, I modified nothing, but just waiting some more time before firing up RViz on the remote computer did not interfere with scan matching, so consider that solved, it was my fault! :)

2) How can I redirect the RViz 2D Navigation Goal to the /rtabmap/goal topic? Can I use a launch file to launch the rviz node and remap that topic or rtabmapviz is needed to graphically sey that goal type? In this configuration, is the move_base node still needed by rtabmap or some other remapping stuff needed?

OptimizeFromGraphEnd is true for both mapping and localization mode. Yes, the behaviour you showed in the videos is exactly what we are trying to achieve! Except for Azimut stuff, I need to setup RViz in a manner very similar to yours, what is the .rviz config file you used for them, "azimut3_nav" or "azimut3"?

When the final goal is not moving too much (which I plan to solve following your advices), the trajectory planner works pretty well, I tweaked the parameters inside yaml files and it smoothly drives to the goal while avoiding obstacles and using recovery behaviours when needed. Actually we use a simple local/global map, but I have to better investigate the obstacle/ground layers you talked about, I think I need take another look at "az3_mapping_robot_kinect_scan.launch" to see if I can improve on something.

Regarding the relocalization, we also noted what you mean and we are already mapping in back and forth direction, it greatly improved localization accuracy. For final costs, now we are trying to achieve the best possible result only with odometry and kinect, eventually some ultrasonic proximity sensor for redundant security. If the final results won't be enough, we'll look into laser rangefinders.

Thanks,
Guido

[matlabbe]

Hi,

2) In RVIZ, open the "Panels->Tool Properties" panel, then set the 2D nav goal topic to /rtabmap/goal like in the first video. rtabmapviz is not required. In that setup, only RVIZ is launched from the client computer.

On the robot, move_base is still required. The goal that rtabmap receives is resent to move_base, you can see it by adding a Pose display in RVIZ and set topic to /rtabmap/goal_out (you will have the red arrow like in the videos). As the robot is moving through this goal, the red arrow will change position until the final goal (sent over /rtabmap/goal) is reached.

If you want to see the paths planned by rtabmap and move_base, add Path displays to RVIZ and add referred topics (rtabmap global plan, move_base global and local plans).

cheers

[g.bartoli]

matlabbe wrote

2) In RVIZ, open the "Panels->Tool Properties" panel, then set the 2D nav goal topic to /rtabmap/goal like in the first video. rtabmapviz is not required. In that setup, only RVIZ is launched from the client computer.

Ok, that works, thanks!

However, even if I use the "/rtabmap/goal", I still see that once "rtabmap/global_path" has been computed and the robot navigate, it does not change when the map is optimized, so sometimes I set a goal, but the robot stops far from it, because after the optimization the goal has fallen inside a local obstacle and the planner rightly cannot reach it. Even worse, if the robot odometry has a large error (slipping wheels or crashing against an undetected obstacle), when the visual relocalization occurs, the map is correctly synchronized with robot position, but the global path remains still (often outside the global map itself) and the robot tries to follow it without success because it cannot reach it.

This a screenshot of what happens to the global path when a relocalization occurs:



Obviously this is not happening with "OptimizeFromGraphEnd=false", because the map is not changing, but the robot position is corrected during the navigation, which is a bit annoying, as you already pointed out. Shouldn't the goal and the intermediate nodes be continuosly updated by rtabmap in relation to map frame?

matlabbe wrote

On the robot, move_base is still required. The goal that rtabmap receives is resent to move_base, you can see it by adding a Pose display in RVIZ and set topic to /rtabmap/goal_out (you will have the red arrow like in the videos). As the robot is moving through this goal, the red arrow will change position until the final goal (sent over /rtabmap/goal) is reached.

Actually, rtabmap node subscribes to goal, but does not publish "goal_out", I used "rostopic list" and it is not present. In RViz I added the Pose display, but only "/rtabmap/goal" (the final pose) can be shown. I found "goal_out" is mentioned also in the rtabmap node wiki, but I still cannot find where it is in ROS.

matlabbe wrote

If you want to see the paths planned by rtabmap and move_base, add Path displays to RVIZ and add referred topics (rtabmap global plan, move_base global and local plans).

Ok, I added them, it's more informative, now.

g.bartoli wrote

Except for Azimut stuff, I need to setup RViz in a manner very similar to yours, what is the .rviz config file you used for them, "azimut3_nav" or "azimut3"?

Can you take a look at this?

[matlabbe]

Hi,

I updated the azimut3_nav.rviz config, accordingly to az3_mapping_robot_kinect_scan.launch file. Well, I use azimut3_nav.rviz for all tests, so maybe some topics are still not the good ones (depending of which configuration I've launched on the robot). I usually modify the topics each time I use it depending on what I am testing. If you are based on this launch file, the "goal_out" topic was renamed to "current_goal":

<remap from="goal_out"        to="current_goal"/>	

Normally, the global path gets updated (aligned to actual graph) until the goal is reached. If the goal is reached, the last global path will be still shown in RVIZ, to clean it, just check/uncheck the path display in RVIZ. Note that your MapGraph topic is not set (it can be useful to see the global path over the actual graph). If the goal was not reached and the global path is still misaligned to the graph, a video of the problem could be useful.

When the localization is poor and a goal is sent over an obstacle, move_base cannot plan a path. You can set "RGBD/PlanStuckIterations" to 10 for example. If after 10 iterations the robot is not moving on the path, a goal on the path closest to the robot will be sent. If every nodes on the path cannot be reached, the rtabmap's plan will fail (topic /rtabmap/goal_reached would be false).

Regards,
Mathieu

[dhagash]

Hello Mathieu,

I am following the same instructions given by you in the following thread

1. Launched freenect and depthimage_to_laserscan
2. Then started rgbd_mapping and then launched move_base.launch

but still I am not able to launch move_base correctly. I am getting the following error,

The origin for the sensor at (0.39, -0.02) is out of map bounds. So, the costmap cannot raytrace for it.

Can you tell me whats the problem in my code:

https://github.com/Dhagash4/auto_navs/tree/master/include

This is the link for the parameters that I set for the move_base.

Thanks,
Dhagash

[matlabbe]

Hi,

It is difficult to debug without every launch files used and without knowing which platform you are using. The only tip I can give you is to try this turtlebot navigation example in simulation (see Section 6), it will help to debug which step/configuration is wrong.

cheers,
Mathieu

[willzoe]

Hi,

I now use depthimage_to_laserscan to convert the depth image generated by the stereo camera to scan, and use it as the LaserScan type observation_sources to generate the obstacle_layer in the costmap as follows:

   

obstacle_layer:
      observation_sources:  scan
      scan:
        data_type: LaserScan
        topic: zed/scan
        observation_persistence: 0.0
        expected_update_rate: 0.0
        marking: true
        clearing: true
        min_obstacle_height: 0.0
        max_obstacle_height: 0.7
        obstacle_range: 1.5
        raytrace_range: 1.5

If I want to use PointCloud2 type observation_sources, what topics can I use? And what is the difference between them?

[matlabbe]

Hi,

you can look at this example: http://wiki.ros.org/rtabmap_ros/Tutorials/StereoOutdoorNavigation#Local_costmap

When using a point cloud for observations, make sure you don't use a super dense cloud. The example above create a downsampled cloud from the disparity image of the stereo camera.

cheers,
Mathieu

[willzoe]

1) According to the above example, through nodelet rtabmap_ros/point_cloud_xyz, I can convert the depth image into a point cloud. Results as shown below:



But when using nodelet rtabmap_ros/obstacles_detection, I encountered some problems:

When launching the nodelet, the nodelet manager reports the following two errors:

[ERROR] [1588255591.812918618]: obstacles_detection: Parameter "min_cluster_size" has moved from rtabmap_ros to rtabmap library. Use parameter "Grid/MinClusterSize" instead. The value is still copied to new parameter name.
[ERROR] [1588255591.826153797]: obstacles_detection: Parameter "max_obstacles_height" has moved from rtabmap_ros to rtabmap library. Use parameter "Grid/MaxObstacleHeight" instead. The value is still copied to new parameter name.

When I subscribe to the topic /planner_cloud in rviz, the nodelet manager keeps reporting the following errors:

[ERROR] [1588256201.869469533]: Lookup would require extrapolation into the future.  Requested time 1588256201.817056506 but the latest data is at time 1588256201.650178506, when looking up transform from frame [base_footprint] to frame [map]

Is it because the parameters are not set correctly?


2)I found that rtabmap released several point cloud maps, namely /rtabmap/cloud_map, /rtabmap/cloud_ground and /rtabmap/cloud_obstacles. I subscribed to them in rviz, and found that when subscribing to /rtabmap/cloud_ground, there was no display; when subscribing to /rtabmap/cloud_obstacles, the point clouds of walls and other objects were projected onto the ground, as shown in the following figure:


Can this topic be used as PointCloud2 type observation_sources?

[matlabbe]

If Grid/3D parameter is disabled, the cloud will be projected to ground. What is the frame_id in the planner_cloud? The map frame would not be used for planner_cloud if rtabmap is not used. How did you configure the obstacles_detection nodelet?

If /rtabmap/cloud_ground is empty, it depends what the camera is seeing and the Grid/ parameters set. If you have a database, it will be easier to see why.