[RPP] remove velocity scaling option (now required) and enable orient…

…ation sp projection times (#2701)

* remove velocity scaling option (now required) and enable orientation sp projection times

* get max radius

nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp

@@ -255,7 +255,6 @@ class RegulatedPurePursuitController : public nav2_core::Controller
   double lookahead_time_;
   bool use_velocity_scaled_lookahead_dist_;
   tf2::Duration transform_tolerance_;
-  bool use_approach_vel_scaling_;
   double min_approach_linear_velocity_;
   double control_duration_;
   double max_allowed_time_to_collision_;

nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

@@ -79,8 +79,6 @@ void RegulatedPurePursuitController::configure(
     rclcpp::ParameterValue(false));
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".min_approach_linear_velocity", rclcpp::ParameterValue(0.05));
-  declare_parameter_if_not_declared(
-    node, plugin_name_ + ".use_approach_linear_velocity_scaling", rclcpp::ParameterValue(true));
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".max_allowed_time_to_collision", rclcpp::ParameterValue(1.0));
   declare_parameter_if_not_declared(
@@ -123,9 +121,6 @@ void RegulatedPurePursuitController::configure(
   node->get_parameter(
     plugin_name_ + ".min_approach_linear_velocity",
     min_approach_linear_velocity_);
-  node->get_parameter(
-    plugin_name_ + ".use_approach_linear_velocity_scaling",
-    use_approach_vel_scaling_);
   node->get_parameter(
     plugin_name_ + ".max_allowed_time_to_collision",
     max_allowed_time_to_collision_);
@@ -409,7 +404,21 @@ bool RegulatedPurePursuitController::isCollisionImminent(
   pose_msg.header.frame_id = arc_pts_msg.header.frame_id;
   pose_msg.header.stamp = arc_pts_msg.header.stamp;
 
-  const double projection_time = costmap_->getResolution() / fabs(linear_vel);
+  double projection_time = 0.0;
+  if (fabs(linear_vel) < 0.01 && fabs(angular_vel) > 0.01) {
+    // rotating to heading at goal or toward path
+    // Equation finds the angular distance required for the largest
+    // part of the robot radius to move to another costmap cell:
+    // theta_min = 2.0 * sin ((res/2) / r_max)
+    // via isosceles triangle r_max-r_max-resolution,
+    // dividing by angular_velocity gives us a timestep.
+    double max_radius = costmap_ros_->getLayeredCostmap()->getCircumscribedRadius();
+    projection_time =
+      2.0 * sin((costmap_->getResolution() / 2) / max_radius) / fabs(angular_vel);
+  } else {
+    // Normal path tracking
+    projection_time = costmap_->getResolution() / fabs(linear_vel);
+  }
 
   geometry_msgs::msg::Pose2D curr_pose;
   curr_pose.x = robot_pose.pose.position.x;
@@ -529,7 +538,7 @@ void RegulatedPurePursuitController::applyConstraints(
   // This expression is eq. to (1) holding time to goal, t, constant using the theoretical
   // lookahead distance and proposed velocity and (2) using t with the actual lookahead
   // distance to scale the velocity (e.g. t = lookahead / velocity, v = carrot / t).
-  if (use_approach_vel_scaling_ && dist_error > 2.0 * costmap_->getResolution()) {
+  if (dist_error > 2.0 * costmap_->getResolution()) {
     double velocity_scaling = 1.0 - (dist_error / lookahead_dist);
     double unbounded_vel = approach_vel * velocity_scaling;
     if (unbounded_vel < min_approach_linear_velocity_) {
@@ -743,8 +752,6 @@ RegulatedPurePursuitController::dynamicParametersCallback(
         use_regulated_linear_velocity_scaling_ = parameter.as_bool();
       } else if (name == plugin_name_ + ".use_cost_regulated_linear_velocity_scaling") {
         use_cost_regulated_linear_velocity_scaling_ = parameter.as_bool();
-      } else if (name == plugin_name_ + ".use_approach_vel_scaling") {
-        use_approach_vel_scaling_ = parameter.as_bool();
       } else if (name == plugin_name_ + ".use_rotate_to_heading") {
         if (parameter.as_bool() && allow_reversing_) {
           RCLCPP_WARN(

nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp

@@ -52,7 +52,6 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure
   void setVelocityScaledLookAhead() {use_velocity_scaled_lookahead_dist_ = true;}
   void setCostRegulationScaling() {use_cost_regulated_linear_velocity_scaling_ = true;}
   void resetVelocityRegulationScaling() {use_regulated_linear_velocity_scaling_ = false;}
-  void resetVelocityApproachScaling() {use_approach_vel_scaling_ = false;}
 
   double getLookAheadDistanceWrapper(const geometry_msgs::msg::Twist & twist)
   {
@@ -184,6 +183,8 @@ TEST(RegulatedPurePursuitTest, lookaheadAPI)
   std::string name = "PathFollower";
   auto tf = std::make_shared<tf2_ros::Buffer>(node->get_clock());
   auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("fake_costmap");
+  rclcpp_lifecycle::State state;
+  costmap->on_configure(state);
   ctrl->configure(node, name, tf, costmap);
 
   geometry_msgs::msg::Twist twist;
@@ -242,6 +243,8 @@ TEST(RegulatedPurePursuitTest, rotateTests)
   std::string name = "PathFollower";
   auto tf = std::make_shared<tf2_ros::Buffer>(node->get_clock());
   auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("fake_costmap");
+  rclcpp_lifecycle::State state;
+  costmap->on_configure(state);
   ctrl->configure(node, name, tf, costmap);
 
   // shouldRotateToPath
@@ -311,6 +314,8 @@ TEST(RegulatedPurePursuitTest, applyConstraints)
   std::string name = "PathFollower";
   auto tf = std::make_shared<tf2_ros::Buffer>(node->get_clock());
   auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("fake_costmap");
+  rclcpp_lifecycle::State state;
+  costmap->on_configure(state);
   ctrl->configure(node, name, tf, costmap);
 
   double dist_error = 0.0;
@@ -321,9 +326,6 @@ TEST(RegulatedPurePursuitTest, applyConstraints)
   double linear_vel = 0.0;
   double sign = 1.0;
 
-  // since costmaps here are bogus, we can't access them
-  ctrl->resetVelocityApproachScaling();
-
   // test curvature regulation (default)
   curr_speed.linear.x = 0.25;
   ctrl->applyConstraintsWrapper(
@@ -418,7 +420,6 @@ TEST(RegulatedPurePursuitTest, testDynamicParameter)
       rclcpp::Parameter("test.use_velocity_scaled_lookahead_dist", false),
       rclcpp::Parameter("test.use_regulated_linear_velocity_scaling", false),
       rclcpp::Parameter("test.use_cost_regulated_linear_velocity_scaling", false),
-      rclcpp::Parameter("test.use_approach_linear_velocity_scaling", false),
       rclcpp::Parameter("test.allow_reversing", false),
       rclcpp::Parameter("test.use_rotate_to_heading", false)});
 
@@ -446,7 +447,6 @@ TEST(RegulatedPurePursuitTest, testDynamicParameter)
   EXPECT_EQ(
     node->get_parameter(
       "test.use_cost_regulated_linear_velocity_scaling").as_bool(), false);
-  EXPECT_EQ(node->get_parameter("test.use_approach_linear_velocity_scaling").as_bool(), false);
   EXPECT_EQ(node->get_parameter("test.allow_reversing").as_bool(), false);
   EXPECT_EQ(node->get_parameter("test.use_rotate_to_heading").as_bool(), false);
 }