Navigate to a point outside graph

[g.bartoli]

Hi Mathieu,
I'm trying the autonomous navigation and the robot is doing fine when I set a goal that is very close to an existing node of the graph; the global path is correct and the navigation is smooth while avoiding obstalces.

Otherwise, when I set a goal that have a slight offset from the graph, move_base still perform the navigation, but it stops on the nearest point of the graph instead of reaching the desired point. I tried many times to set goals, but I couldn't find an exact threshold when this happens, since it seems to depend on the nearest graph node (in the picture, the global path is the orange line). I tried to increase "RGBD/LocalRadius" to 8 or 10, but it had no effect (looking at the following picture, the real distance from the graph is no more than 1 meter).



The path is clear of obstacles and rtabmap correctly publishes the goal to move_base... do you know why is this happening?

Thanks,
Guido

[matlabbe]

Hi Guido,

You can look if rtabmap tells if the goal is reached:

$rostopic echo rtabmap/goal_reached

By default, the goal radius is 50 cm. When you set a goal near the graph, rtabmap will make the robot moving to the nearest node on the graph, then when reached, send the pose to the actual goal. If the final goal is already inside the 50 cm, rtabmap will not publish the new pose to move_base, telling that the goal is reached. You can decrease the radius with the following parameter:

$ rosrun rtabmap_ros rtabmap --params | grep Goal
Param: RGBD/GoalReachedRadius = "0.5"                      [Goal reached radius (m).]

If the goal is actually sent to move_base, I don't really know why move_base wouldn't try to compute a plan to reach it (see the terminal for possible warnings).

cheers

[g.bartoli]

I repeated the test setting a goal that is about 1.5 meters far from the graph and leaving "RGBD/GoalReachedRadius" to 0.5 meters, this is what happens:



On the prompt, you can see that rtabmap states that the goal is reached. Inside RViz you can see the orange global path that is correctly computed from the start position to the provided goal, but in the bottom left corner you can see the actual distance from the robot to the goal (1.48 meters). The red arrow is the current move_base goal pose that is (correctly) reached, but it is not what I have set (the end of the orange path).

In this situation, it seems that the robot cannot reach the target because of obstacles in the local map, but this behaviour also happens when the path to the distant goal is totally free as I showed you the previous screenshot.

Do you have any idea?
What is the practical difference between "RGBD/GoalReachedRadius" and "RGBD/LocalRadius"?

Thanks,

[matlabbe]

If rtabmap tells that the goal is reached, this is the problem (it should not say that it is reached in your test). I'll test on my side.

"RGBD/GoalReachedRadius": When the robot enters in this radius the last pose of the path (the goal), rtabmap tells that the goal is reached and stops sending poses to move_base. move_base can still send commands until the last pose is reached in its own "goal reached" parameters (distance and angle).

"RGBD/LocalRadius": It has multiple roles. Maximum distance from the graph that a goal can be set. Maximum distance on the path from the current position for the poses sent to move_base. Used also for local/proximity loop closure detection. It is preferred to set the value to maximum range of the sensor used to create the map.

cheers

[matlabbe]

Follow-up...

I've found the problem. It happens only in localization mode. In mapping mode, you should have something like this:

1- Set goal (the red arrow), rtabmap plans the purple path, move_base plans the orange path to the current pose (green arrow) set by rtabmap (which is the last pose on the graph):


2- The robot reaches the previous green arrow, then the next pose (new green arrow) is sent on the requested pose (the goal):


3- As soon as the robot enters in the goal radius, rtabmap sends "/rtabmap/goal_reached", and move_base continues to sends velocity commands until the robot is oriented on the goal set:


I've fixed the problem for localization mode, the results above should be reproducible in localization mode too. Thx for your feedback to find this bug!

cheers,
Mathieu

[g.bartoli]

Well, I didn't try in mapping (I enabled move_base navigation only in localization mode), but what you showed is exactly the desired behaviour... many thanks for fixing that, Mathieu!

Have a nice day,
Guido

[g.bartoli]

I updated and recompiled rtabmap, now it works perfectly, great job! :)