RegulatedPurePursuitController detected a collision ahead but robot doesn't stop
Hello,
We are trying to navigate with obstacle avoidance with ROS2 Foxy and we switch from DWB to the freshly released Pure Pursuit Controller in the Navigation2 stack.
We set a goal in Rviz several meters in front of the robot, and after it starts moving, we place an obstacle on the path. Then the obstacle is effectively visible in the local costmap, and the look_ahead_point and the look_ahead_arc_path effectively stops at the obstacle: so it is somehow detected. But the robot continue at constant speed until it hits the obstacle We expected it to at least stop in front of the obstacle (or ideally to pass around it).
Video showing this behavior: https://www.youtube.com/watch?v=TCGRE...
In the console, we get the following messages:
[controller_server-7] [INFO] [1623946373.622400460] [controller_server]: Passing new path to controller.
[controller_server-7] [ERROR] [1623946373.622598680] [controller_server_rclcpp_node]: Action server failed while executing action callback: "RegulatedPurePursuitController detected collision ahead!"
[controller_server-7] [WARN] [1623946373.622634040] [controller_server_rclcpp_node]: [follow_path] [ActionServer] Aborting handle.
I supposed that because the PurePursuitController fails and is aborted, there is no more velocity command published, that would stop the robot or avoid the obstacle.
What could cause that ? Is the presence of an obstacle really supposed to raise an error ?
Below is the configuration of our PurePursuitController:
controller_server:
ros__parameters:
use_sim_time: True
controller_frequency: 10.0
min_x_velocity_threshold: 0.001
min_y_velocity_threshold: 0.001
min_theta_velocity_threshold: 0.001
progress_checker_plugin: "progress_checker"
goal_checker_plugin: "goal_checker"
controller_plugins: ["FollowPath"]
# Progress checker parameters
progress_checker:
plugin: "nav2_controller::SimpleProgressChecker"
required_movement_radius: 0.5
movement_time_allowance: 10.0
# Goal checker parameters
goal_checker:
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 0.25 # [m]
yaw_goal_tolerance: 0.17 # [rad]
stateful: True
# Pure pursuit parameters
FollowPath:
plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
desired_linear_vel: 0.5
max_linear_accel: 2.5
max_linear_decel: 2.5
lookahead_dist: 2.0
min_lookahead_dist: 0.3
max_lookahead_dist: 0.9
lookahead_time: 1.0
rotate_to_heading_angular_vel: 1.8
transform_tolerance: 0.1
use_velocity_scaled_lookahead_dist: false
min_approach_linear_velocity: 0.05
use_approach_linear_velocity_scaling: false
max_allowed_time_to_collision: 5.0 # Set high as our footprint is large
use_regulated_linear_velocity_scaling: false
use_cost_regulated_linear_velocity_scaling: false
regulated_linear_scaling_min_radius: 0.9
regulated_linear_scaling_min_speed: 0.25
use_rotate_to_heading: false
rotate_to_heading_min_angle: 0.785
max_angular_accel: 3.2
cost_scaling_dist: 0.3
cost_scaling_gain: 1.0
inflation_cost_scaling_factor: 2.0
Thanks for your help,
A video would be very illustrative with rviz and the RPP publishers. A question though: your platform, does it have a timeout? If you stop sending base commands, will it just naively follow the last command until oblivion or will it time out and the robot stop? Just checking before we go into it any deeper.
The log makes me think it sees the collision and the abort means that it stopped publishing velocity commands and the behavior tree node would then be exited as failure to start the recovery processes outlined in your behavior tree. So I want to make sure that its an issue with RPP we can resolve (by the sounds of it, it should be quick) vs the base just continuing even though no more commands are sent (though RPP should publish a “STOP” as its last message, we can see if we find somewhere that didnt happen). It might be illustrative to use ros2 topic echo the cmd_vel topic to see if it stops publishing but the robot is still moving.
Sorry for the delay, but I only had a quick opportunity to test again RPP recently, since we abandoned it for TEB. I added a video showing the issue in the post. Our plateform does not have a timeout (yet) on the commands and indeed just follows the last one. Also, I noted that when we set a goal and that the obstacles is close enough to trigger the mentionned error message immediately, then the robot won't move. It would be consistent with the hypothesis of the velocity command not set explicitly to zero when this error occurs (since velocity commands stay to zeros here)
That doesn't totally track for me. Even if RPP failed to compute the controller server https://github.com/ros-planning/navig... has built in safeties that if an exception is thrown, after the tolerance period, a zero velocity command is published -- no matter the controller algorithm. It shouldn't matter if you use DWB / TEB / RPP they all should react the same. I verified in DWB and TEB both react the same way to failures.
But it sounds like your first issue is solving that big safety gap!
Hum ok but I though that RPP would compute velocities commands to avoid the obstacle or effectively publish zero velocity commands (no error detected on our driver side saying that the command /cmd_vel could not be applied or transmitted) when it "detects a collision ahead". Could it be a matter of config of RPP ? On the video above the lookahead point effectively stops a little time before the obstacle and then "jumps" behind it (obstacle thinner than the lookahead distance ?), so if the controller just follows the point indeed it will hit the obstacle (while knowing it will collide)
I tracked the error in the code of the foxy-devel branch: when a future collision is detected, RPP throws a
std::runtime_error, but in the nav2_controller, onlynav2_core::PlannerExceptionare caught and set the vel cmd to zero. In DWB controller, in case or infeasible trajectory it throws adwb_core::NoLegalTrajectoriesExceptionthat inherit fromnav2_core::PlannerException, so with DWB a zero vel command is indeed published and the robot stop as expected.Could the exception not be caught with RPP because of a wrong type ?