Add footprint expansion to graceful controller

nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp

@@ -159,9 +159,12 @@ class GracefulController : public nav2_core::Controller
    * @param x The x coordinate of the robot in global frame
    * @param y The y coordinate of the robot in global frame
    * @param theta The orientation of the robot in global frame
+   * @param inflation_scale Scaling factor for the robot footprint
    * @return Whether in collision
    */
-  bool inCollision(const double & x, const double & y, const double & theta);
+  bool inCollision(
+    const double & x, const double & y, const double & theta,
+    double inflation_scale = 1.0);
 
   /**
    * @brief Compute the distance to each pose in a path

nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp

@@ -50,6 +50,9 @@ struct Parameters
   bool allow_backward;
   double in_place_collision_resolution;
   bool use_collision_detection;
+  double footprint_scaling_linear_vel;
+  double footprint_scaling_factor;
+  double footprint_scaling_step;
 };
 
 /**

nav2_graceful_controller/src/graceful_controller.cpp

@@ -306,10 +306,17 @@ bool GracefulController::validateTargetPose(
   }
 
   // Actually simulate the path
-  if (simulateTrajectory(target_pose, costmap_transform, trajectory, cmd_vel, reversing)) {
-    // Successfully simulated to target_pose
-    return true;
-  }
+  double sim_linear_velocity = params_->v_linear_max;
+  do {
+    control_law_->setSpeedLimit(params_->v_linear_min, sim_linear_velocity,
+      params_->v_angular_max);
+    if (simulateTrajectory(target_pose, costmap_transform, trajectory, cmd_vel, reversing)) {
+      // Successfully simulated to target_pose
+      return true;
+    }
+    // Reduce velocity and try again for same target_pose
+    sim_linear_velocity -= params_->footprint_scaling_step;
+  } while (sim_linear_velocity >= params_->footprint_scaling_linear_vel);
 
   // Validation not successful
   return false;
@@ -379,12 +386,24 @@ bool GracefulController::simulateTrajectory(
     // Add the pose to the trajectory for visualization
     trajectory.poses.push_back(next_pose);
 
+    // Compute footprint scaling
+    double footprint_scaling = 1.0;
+    if (cmd_vel.twist.linear.x > params_->footprint_scaling_linear_vel) {
+      // Scaling = (vel_x - scaling_vel_x) / (max_vel_x - scaling_vel_x)
+      double ratio = params_->v_linear_max - params_->footprint_scaling_linear_vel;
+      // Avoid divide by zero
+      if (ratio > 0) {
+        ratio = (cmd_vel.twist.linear.x - params_->footprint_scaling_linear_vel) / ratio;
+        footprint_scaling += ratio * params_->footprint_scaling_factor;
+      }
+    }
+
     // Check for collision
     geometry_msgs::msg::PoseStamped global_pose;
     tf2::doTransform(next_pose, global_pose, costmap_transform);
     if (params_->use_collision_detection && inCollision(
         global_pose.pose.position.x, global_pose.pose.position.y,
-        tf2::getYaw(global_pose.pose.orientation)))
+        tf2::getYaw(global_pose.pose.orientation), footprint_scaling))
     {
       return false;
     }
@@ -407,7 +426,9 @@ geometry_msgs::msg::Twist GracefulController::rotateToTarget(double angle_to_tar
   return vel;
 }
 
-bool GracefulController::inCollision(const double & x, const double & y, const double & theta)
+bool GracefulController::inCollision(
+  const double & x, const double & y, const double & theta,
+  double inflation_scale)
 {
   unsigned int mx, my;
   if (!costmap_ros_->getCostmap()->worldToMap(x, y, mx, my)) {
@@ -417,6 +438,11 @@ bool GracefulController::inCollision(const double & x, const double & y, const d
     return false;
   }
 
+  if (inflation_scale < 1.0) {
+    RCLCPP_WARN(logger_, "Inflation ratio cannot be less than 1.0");
+    throw nav2_core::NoValidControl("Inflation ratio less than 1.0");
+  }
+
   // Calculate the cost of the footprint at the robot's current position depending
   // on the shape of the footprint
   bool is_tracking_unknown =
@@ -425,8 +451,14 @@ bool GracefulController::inCollision(const double & x, const double & y, const d
 
   double footprint_cost;
   if (consider_footprint) {
-    footprint_cost = collision_checker_->footprintCostAtPose(
-      x, y, theta, costmap_ros_->getRobotFootprint());
+    std::vector<geometry_msgs::msg::Point> spec = costmap_ros_->getRobotFootprint();
+    if (spec.size() > 3) {
+      for (auto & point : spec) {
+        point.x *= inflation_scale;
+        point.y *= inflation_scale;
+      }
+    }
+    footprint_cost = collision_checker_->footprintCostAtPose(x, y, theta, spec);
   } else {
     footprint_cost = collision_checker_->pointCost(mx, my);
   }

nav2_graceful_controller/src/parameter_handler.cpp

@@ -70,6 +70,12 @@ ParameterHandler::ParameterHandler(
     plugin_name_ + ".in_place_collision_resolution", 0.1);
   params_.use_collision_detection = node->declare_or_get_parameter(
     plugin_name_ + ".use_collision_detection", true);
+  params_.footprint_scaling_linear_vel = node->declare_or_get_parameter(
+    plugin_name_ + ".footprint_scaling_linear_vel", 0.5);
+  params_.footprint_scaling_factor = node->declare_or_get_parameter(
+    plugin_name_ + ".footprint_scaling_factor", 0.25);
+  params_.footprint_scaling_step = node->declare_or_get_parameter(
+    plugin_name_ + ".footprint_scaling_step", 0.1);
   if (params_.initial_rotation && params_.allow_backward) {
     RCLCPP_WARN(
       logger_, "Initial rotation and allow backward parameters are both true, "
@@ -160,6 +166,12 @@ ParameterHandler::updateParametersCallback(
         params_.rotation_scaling_factor = parameter.as_double();
       } else if (param_name == plugin_name_ + ".in_place_collision_resolution") {
         params_.in_place_collision_resolution = parameter.as_double();
+      } else if (param_name == plugin_name_ + ".footprint_scaling_linear_vel") {
+        params_.footprint_scaling_linear_vel = parameter.as_double();
+      } else if (param_name == plugin_name_ + ".footprint_scaling_factor") {
+        params_.footprint_scaling_factor = parameter.as_double();
+      } else if (param_name == plugin_name_ + ".footprint_scaling_step") {
+        params_.footprint_scaling_step = parameter.as_double();
       }
     } else if (param_type == ParameterType::PARAMETER_BOOL) {
       if (param_name == plugin_name_ + ".initial_rotation") {

nav2_graceful_controller/test/test_graceful_controller.cpp

@@ -22,6 +22,7 @@
 #include "nav2_graceful_controller/ego_polar_coords.hpp"
 #include "nav2_graceful_controller/smooth_control_law.hpp"
 #include "nav2_graceful_controller/graceful_controller.hpp"
+#include "nav2_util/path_utils.hpp"
 
 class SCLFixture : public nav2_graceful_controller::SmoothControlLaw
 {
@@ -248,7 +249,10 @@ TEST(GracefulControllerTest, dynamicParameters) {
       rclcpp::Parameter("test.rotation_scaling_factor", 13.0),
       rclcpp::Parameter("test.allow_backward", false),
       rclcpp::Parameter("test.use_collision_detection", false),
-      rclcpp::Parameter("test.in_place_collision_resolution", 15.0)});
+      rclcpp::Parameter("test.in_place_collision_resolution", 15.0),
+      rclcpp::Parameter("test.footprint_scaling_linear_vel", 16.0),
+      rclcpp::Parameter("test.footprint_scaling_factor", 17.0),
+      rclcpp::Parameter("test.footprint_scaling_step", 18.0)});
 
   // Spin
   rclcpp::spin_until_future_complete(node->get_node_base_interface(), results);
@@ -272,6 +276,9 @@ TEST(GracefulControllerTest, dynamicParameters) {
   EXPECT_EQ(node->get_parameter("test.allow_backward").as_bool(), false);
   EXPECT_EQ(node->get_parameter("test.use_collision_detection").as_bool(), false);
   EXPECT_EQ(node->get_parameter("test.in_place_collision_resolution").as_double(), 15.0);
+  EXPECT_EQ(node->get_parameter("test.footprint_scaling_linear_vel").as_double(), 16.0);
+  EXPECT_EQ(node->get_parameter("test.footprint_scaling_factor").as_double(), 17.0);
+  EXPECT_EQ(node->get_parameter("test.footprint_scaling_step").as_double(), 18.0);
 
   // Set allow backward to true
   results = params->set_parameters_atomically(
@@ -799,6 +806,113 @@ TEST(GracefulControllerTest, computeVelocityCommandFinal) {
   EXPECT_LE(cmd_vel.twist.angular.z, 0.5);
 }
 
+TEST(GracefulControllerTest, slowDownForObstacle) {
+  auto node = std::make_shared<nav2::LifecycleNode>("testGraceful");
+  auto tf = std::make_shared<tf2_ros::Buffer>(node->get_clock());
+
+  // Create a costmap of 10x10 meters
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_costmap");
+  costmap_ros->declare_parameter("global_frame", rclcpp::ParameterValue("test_global_frame"));
+  costmap_ros->declare_parameter("robot_base_frame", rclcpp::ParameterValue("test_robot_frame"));
+  costmap_ros->declare_parameter("width", rclcpp::ParameterValue(10));
+  costmap_ros->declare_parameter("height", rclcpp::ParameterValue(10));
+  costmap_ros->declare_parameter("resolution", rclcpp::ParameterValue(0.1));
+  costmap_ros->declare_parameter(
+    "footprint", rclcpp::ParameterValue("[[-0.2, -0.2], [0.2, -0.2], [0.2, 0.2], [-0.2, 0.2]]"));
+  costmap_ros->on_configure(rclcpp_lifecycle::State());
+
+  // Create controller
+  auto controller = std::make_shared<GMControllerFixture>();
+  controller->configure(node, "test", tf, costmap_ros);
+  controller->activate();
+  nav2_controller::FeasiblePathHandler path_handler;
+  path_handler.initialize(node, node->get_logger(), "path_handler", costmap_ros, tf);
+
+  // Place an obstacle near the path
+  auto costmap = costmap_ros->getCostmap();
+  unsigned int mx, my;
+  bool ok = costmap->worldToMap(5.5, 5.3, mx, my);
+  ASSERT_TRUE(ok);
+  costmap->setCost(mx, my, nav2_costmap_2d::LETHAL_OBSTACLE);
+
+  // Set parameters to enable footprint scaling logic
+  auto params = std::make_shared<rclcpp::AsyncParametersClient>(
+    node->get_node_base_interface(), node->get_node_topics_interface(),
+    node->get_node_graph_interface(), node->get_node_services_interface());
+  double max_vel = 1.0;
+  auto fut = params->set_parameters_atomically(
+    {rclcpp::Parameter("test.v_linear_max", max_vel),
+      rclcpp::Parameter("test.footprint_scaling_linear_vel", 0.2),
+      rclcpp::Parameter("test.footprint_scaling_factor", 1.0),
+      rclcpp::Parameter("test.footprint_scaling_step", 0.1),
+      rclcpp::Parameter("test.slowdown_radius", 0.1)});
+  rclcpp::spin_until_future_complete(node->get_node_base_interface(), fut);
+
+  // Create the robot pose
+  geometry_msgs::msg::PoseStamped robot_pose;
+  robot_pose.header.frame_id = "test_robot_frame";
+  robot_pose.pose.position.x = 0.0;
+  robot_pose.pose.position.y = 0.0;
+  robot_pose.pose.position.z = 0.0;
+  robot_pose.pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0));
+
+  // Set transform between global and robot frame
+  geometry_msgs::msg::TransformStamped global_to_robot;
+  global_to_robot.header.frame_id = "test_global_frame";
+  global_to_robot.header.stamp = node->get_clock()->now();
+  global_to_robot.child_frame_id = "test_robot_frame";
+  global_to_robot.transform.translation.x = 5.0;
+  global_to_robot.transform.translation.y = 5.0;
+  global_to_robot.transform.translation.z = 0.0;
+  tf->setTransform(global_to_robot, "test", false);
+
+  // Set a plan in a straight line from the robot
+  nav_msgs::msg::Path plan;
+  plan.header.frame_id = "test_global_frame";
+  plan.poses.resize(5);
+  plan.poses[0].header.frame_id = "test_global_frame";
+  plan.poses[0].pose.position.x = 5.0;
+  plan.poses[0].pose.position.y = 5.0;
+  plan.poses[0].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0));
+  plan.poses[1].header.frame_id = "test_global_frame";
+  plan.poses[1].pose.position.x = 5.25;
+  plan.poses[1].pose.position.y = 5.0;
+  plan.poses[1].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0));
+  plan.poses[2].header.frame_id = "test_global_frame";
+  plan.poses[2].pose.position.x = 5.5;
+  plan.poses[2].pose.position.y = 5.0;
+  plan.poses[2].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0));
+  plan.poses[3].header.frame_id = "test_global_frame";
+  plan.poses[3].pose.position.x = 5.75;
+  plan.poses[3].pose.position.y = 5.0;
+  plan.poses[3].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0));
+  plan.poses[4].header.frame_id = "test_global_frame";
+  plan.poses[4].pose.position.x = 6.0;
+  plan.poses[4].pose.position.y = 5.0;
+  plan.poses[4].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0));
+  path_handler.setPlan(plan);
+
+  // Set velocity
+  geometry_msgs::msg::Twist robot_velocity;
+  robot_velocity.linear.x = 0.0;
+  robot_velocity.linear.y = 0.0;
+
+  // Set the goal checker
+  nav2_controller::SimpleGoalChecker checker;
+  checker.initialize(node, "checker", costmap_ros);
+
+  auto [closest_point, pruned_plan_end] = path_handler.findPlanSegment(robot_pose);
+  nav_msgs::msg::Path transformed_global_plan = path_handler.transformLocalPlan(closest_point,
+    pruned_plan_end);
+  geometry_msgs::msg::PoseStamped goal;
+  auto cmd_vel =
+    controller->computeVelocityCommands(robot_pose, robot_velocity, &checker,
+      transformed_global_plan, goal);
+
+  // And the produced command velocity should be <= configured max
+  EXPECT_LT(cmd_vel.twist.linear.x, max_vel);
+}
+
 int main(int argc, char ** argv)
 {
   testing::InitGoogleTest(&argc, argv);