0
$\begingroup$

Rosanswers logo

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-TNDdE

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,


Originally posted by PatoInso on ROS Answers with karma: 109 on 2021-06-17

Post score: 1


Original comments

Comment by stevemacenski on 2021-06-18:
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.

Comment by stevemacenski on 2021-06-18:
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.

Comment by PatoInso on 2021-07-26:
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)

Comment by stevemacenski on 2021-07-26:
That doesn't totally track for me. Even if RPP failed to compute the controller server https://github.com/ros-planning/navigation2/blob/main/nav2_controller/src/nav2_controller.cpp 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!

Comment by PatoInso on 2021-07-28:
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)

Comment by PatoInso on 2021-07-28:
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, only nav2_core::PlannerException are caught and set the vel cmd to zero. In DWB controller, in case or infeasible trajectory it throws a dwb_core::NoLegalTrajectoriesException that inherit from nav2_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 ?

///navigation2\nav2_regulated_pure_pursuit_controller\src\regulated_pure_pursuit_controller.cpp:269
geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocityCommands(...)
{
  ...
  // Collision checking on this velocity heading
  if (isCollisionImminent(pose, linear_vel, angular_vel)) {
    throw std::runtime_error("RegulatedPurePursuitController detected collision ahead!");
  }
  ...
}
$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

It looks like that is indeed the issue. We use the PlannerException in all other places in the code but it looks like I missed one.

Can you test that this PR resolves your concerns? https://github.com/ros-planning/navigation2/pull/2484


Originally posted by stevemacenski with karma: 8272 on 2021-07-28

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by PatoInso on 2021-07-30:
I confirm that replacing std::runtime_error by nav2_core::PlannerException solves the issue.

$\endgroup$

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.