Bug report

Required Info:

• Operating System:
  • Ubuntu 20.04.2
• ROS2 Version:
  • Foxy binaries,
• Version or commit hash:
  • navigation2 foxy-devel 77c1b74
• DDS implementation:
  • Fast-RTPS

Steps to reproduce issue

In order for the backup recovery action to be used, make the fix in nav2_behavior_tree/plugins/action/back_up_action.cpp:

@@ -75,7 +75,7 @@ BT_REGISTER_NODES(factory) 
     [](const std::string & name, const BT::NodeConfiguration & config)
     {
       return std::make_unique<nav2_behavior_tree::BackUpAction>(
-        name, "back_up", config);
+        name, "backup", config);
     };

Run the navigation2 tb3 demo using the attached parameter and behavior tree files. This replaces the Spin recovery action with BackUp and changes the local planner to the TEB planner. Other param changes make it more likely that the local controller path will hit an obstacle.

nav2_params.yaml.txt
navigate_w_replanning_and_recovery.xml.txt

Run
ros2 launch nav2_bringup tb3_simulation_launch.py
Using Rviz, place the starting position and the goal position on opposite sides of an obstacle.

Expected behavior

The robot should stop when it detects a collision and back up.

Actual behavior

No backup even if there is sufficient space behind the robot, message:

[recoveries_server-9] [INFO] [1613530319.113067431] [recoveries_server]: Attempting backup
[recoveries_server-9] [WARN] [1613530319.113508218] [recoveries_server]: Collision Ahead - Exiting BackUp

is printed in the log.

Additional information

The code for nav2_recoveries/plugins/back_up.cpp contains the following fragment in the function BackUp::isCollisionFree:

  while (cycle_count < max_cycle_count) {
    sim_position_change = cmd_vel->linear.x * (cycle_count / cycle_frequency_);
    pose2d.x += sim_position_change * cos(pose2d.theta);
    pose2d.y += sim_position_change * sin(pose2d.theta);
    cycle_count++;

    if (diff_dist - abs(sim_position_change) <= 0.) {
      break;
    }

    if (!collision_checker_->isCollisionFree(pose2d)) {
      return false;
    }

Note that sim_position_change is a multiple of cycle_count which is incremented each time through the loop. But sim_position_change is then used to increment pose2d, so the total change to pose2d is proportional to cycle_count^2. This results in the collision checker looking much too far ahead and reporting collisions with far away obstacles.