I have created a c++ executable whose purpose is to generate Poses based on the coordinates provided by the user. More specifically, the user provides a number of points in cartesian coordinates (e.g., (x1,y1), (x2, y2), (x3,y3)) and then the program creates a number of poses between those points using linear interpolation, where the number of the subpoints between two points is proportional to the distance of those two points (e.g., if point a has a Euclidean distance of 2 meters from b then we will generate 4 subpoints between those two points). Then, I dynamically delete some of those poses based on whether that pose is in a grid cell with a high cost (to do that I subscribe to the global costmap).

As the vehicle moves, more and more subpoints are deleted since the LIDAR detects either previously unseen static obstacles (e.g., a side of a cubic obstacle that was previously unseen) or dynamic obstacles. Each time a subpoint is deleted I send the new goal path to the Nav2 stack (FollowPath). That way we do not run into problems where the planner is trying to generate a Navigate Through Poses path which runs through a subpoint that is on top of an obstacle (that would lead the planner to fail to generate a valid path).

This process generally works smoothly, however, sometimes our LIDAR suddenly detects part of an obstacle that is on top of a pose that we are supposed to move to (e.g., when we are turning around one of the edges of a large cubic obstacle). Before, my code has time to delete this subpoint the planner server already fails to create a plan (as no valid path is found since we are "trying" to move towards a pose that in on top of an obstacle). Then the progress of the path of the vehicle is delayed by 15 to 30 seconds (effectively stays still), since a new global costmap is requested from the planner , and we go into recovery behavior before finally, the bt-navigator receives a goal preemption with the new NavThroughPoses path. From the logs and the terminal I can see that effectively, the planner outputs a warning that we "cannot" generate a valid path and after 10 ms the subpoint is deleted by my script. However, the progress of the vehicle is delayed by around 20 seconds and not 10 ms as the planner goes into recovery behavior etc. If my script was faster than the planner server that problem would not occur since the path considered by the planner would already contain only "good" subwaypoints.

Any advice on how to tackle this problem? Ideally, I would like to solve this problem by editing my script (e.g, through the FollowPath server I create) but I am not entirely sure that this is actually possible.

Edit: Taking a look at the RCLCPP INFO one can see the following:

  1. In the FollowPath Server terminal:
[WARN] [1699017481.380706307] [costmap_reader]: Subwaypoint at location (17.870499, 63.003998) with cost 40 was erased (OBSTACLE)
Size of subpoints: 34
[WARN] [1699017481.381329198] [SimpleWaypointFollower]: Number of the goal poses is  34
[INFO] [1699017481.381367433] [simple_path_waypoints_client]: Sending goal poses
[INFO] [1699017481.382638981] [simple_path_waypoints_client]: Goal accepted by server, waiting for result
  1. From the planner and rest of Nav Stack:
[planner_server-4] [WARN] [1699017481.371018314] [planner_server]: GridBased: failed to create plan, no valid path found.
[planner_server-4] [WARN] [1699017481.385618140] [planner_server]: Planning algorithm GridBased failed to generate a valid path to (18.40, 56.45)
[planner_server-4] [WARN] [1699017481.386603146] [planner_server]: [compute_path_through_poses] [ActionServer] Aborting handle.

Do note the timestamp difference between the two outputs (it's around 10ms)


1 Answer 1


Sounds like you should leverage the power of the behavior tree to do this work and remove parts of the BT XML that aren't necessarily helpful for your application in these situations. This is one of the main benefits of Nav2 -- you should get familiar with behavior trees so that you may design your own logic for how this is handled so that you do get the optimal behavior.

This is why the Behavior Tree exists.

  • $\begingroup$ Alright, Steven. I will look into behavior trees then. Thank you for your reply. $\endgroup$ Commented Nov 8, 2023 at 7:09

Your Answer

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

Not the answer you're looking for? Browse other questions tagged or ask your own question.