[RotationShimController] Rotate to goal heading

When arriving in the goal xy tolerance, the rotation shim controller
takes back the control and perform the necessary rotation to be in the
correct goal orientation.

This can be useful when the final orientaion of the robot matters. It
can also simplify the configuration of the primary controller since it
would not have to be configured to be able to reach the target
orientation.

nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp

@@ -115,6 +115,13 @@ class RotationShimController : public nav2_core::Controller
    */
   geometry_msgs::msg::PoseStamped getSampledPathPt();
 
+  /**
+   * @brief Find the goal point in path
+   * May throw exception if the path is empty
+   * @return pt location of the output point
+   */
+  geometry_msgs::msg::PoseStamped getSampledPathGoal();
+
   /**
    * @brief Uses TF to find the location of the sampled path point in base frame
    * @param pt location of the sampled path point
@@ -168,6 +175,7 @@ class RotationShimController : public nav2_core::Controller
   double forward_sampling_distance_, angular_dist_threshold_;
   double rotate_to_heading_angular_vel_, max_angular_accel_;
   double control_duration_, simulate_ahead_time_;
+  bool rotate_to_goal_heading_;
 
   // Dynamic parameters handler
   std::mutex mutex_;

nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp

@@ -60,6 +60,8 @@ void RotationShimController::configure(
     node, plugin_name_ + ".simulate_ahead_time", rclcpp::ParameterValue(1.0));
   nav2_util::declare_parameter_if_not_declared(
     node, plugin_name_ + ".primary_controller", rclcpp::PARAMETER_STRING);
+  nav2_util::declare_parameter_if_not_declared(
+    node, plugin_name_ + ".rotate_to_goal_heading", rclcpp::ParameterValue(false));
 
   node->get_parameter(plugin_name_ + ".angular_dist_threshold", angular_dist_threshold_);
   node->get_parameter(plugin_name_ + ".forward_sampling_distance", forward_sampling_distance_);
@@ -73,6 +75,8 @@ void RotationShimController::configure(
   node->get_parameter("controller_frequency", control_frequency);
   control_duration_ = 1.0 / control_frequency;
 
+  node->get_parameter("rotate_to_goal_heading", rotate_to_goal_heading_);
+
   try {
     primary_controller_ = lp_loader_.createUniqueInstance(primary_controller);
     RCLCPP_INFO(
@@ -134,11 +138,71 @@ void RotationShimController::cleanup()
   primary_controller_.reset();
 }
 
+/**
+ * @brief Check if the robot pose is within the Goal Checker's tolerances to goal
+ * @param global_checker Pointer to the goal checker
+ * @param robot Pose of robot
+ * @param path Path to retreive goal pose from
+ * @return bool If robot is within goal checker tolerances to the goal
+ */
+static bool withinPositionGoalTolerance(
+  nav2_core::GoalChecker * goal_checker,
+  const geometry_msgs::msg::Pose & robot,
+  const geometry_msgs::msg::Pose & goal)
+{
+  if (goal_checker) {
+    geometry_msgs::msg::Pose pose_tolerance;
+    geometry_msgs::msg::Twist velocity_tolerance;
+    goal_checker->getTolerances(pose_tolerance, velocity_tolerance);
+
+    const auto pose_tolerance_sq = pose_tolerance.position.x * pose_tolerance.position.x;
+
+    auto dx = robot.position.x - goal.position.x;
+    auto dy = robot.position.y - goal.position.y;
+
+    auto dist_sq = dx * dx + dy * dy;
+
+    if (dist_sq < pose_tolerance_sq) {
+      return true;
+    }
+  }
+
+  return false;
+}
+
 geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands(
   const geometry_msgs::msg::PoseStamped & pose,
   const geometry_msgs::msg::Twist & velocity,
   nav2_core::GoalChecker * goal_checker)
 {
+  // Rotate to goal heading when in goal xy tolerance
+  if (rotate_to_goal_heading_) {
+    std::lock_guard<std::mutex> lock_reinit(mutex_);
+
+    try {
+      geometry_msgs::msg::Pose sampled_pt_goal = transformPoseToBaseFrame(getSampledPathGoal());
+      if (withinPositionGoalTolerance(goal_checker, pose.pose, sampled_pt_goal)) {
+        double pose_yaw, pose_pitch, pose_roll;
+        double goal_yaw, goal_pitch, goal_roll;
+        tf2::getEulerYPR(pose.pose.orientation, pose_yaw, pose_pitch, pose_roll);
+        tf2::getEulerYPR(sampled_pt_goal.orientation, goal_yaw, goal_pitch, goal_roll);
+
+        double angular_distance_to_heading = std::abs(goal_yaw - pose_yaw);
+        if (angular_distance_to_heading > M_PI) {
+          angular_distance_to_heading -= M_PI;
+        }
+
+        return computeRotateToHeadingCommand(angular_distance_to_heading, pose, velocity);
+      }
+    } catch (const std::runtime_error & e) {
+      RCLCPP_INFO(
+        logger_,
+        "Rotation Shim Controller was unable to find a goal point,"
+        " a rotational collision was detected, or TF failed to transform"
+        " into base frame! what(): %s", e.what());
+    }
+  }
+
   if (path_updated_) {
     nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap();
     std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getMutex()));
@@ -201,6 +265,16 @@ geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathPt()
             "passing off to primary controller plugin.", forward_sampling_distance_));
 }
 
+geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathGoal()
+{
+  if (current_path_.poses.size() == 0) {
+    throw nav2_core::ControllerException(
+            "Path is empty - cannot find a goal point");
+  }
+
+  return current_path_.poses[current_path_.poses.size() - 1];
+}
+
 geometry_msgs::msg::Pose
 RotationShimController::transformPoseToBaseFrame(const geometry_msgs::msg::PoseStamped & pt)
 {
@@ -306,6 +380,9 @@ RotationShimController::dynamicParametersCallback(std::vector<rclcpp::Parameter>
         simulate_ahead_time_ = parameter.as_double();
       }
     }
+    if (name == plugin_name_ + ".rotate_to_goal_heading") {
+      rotate_to_goal_heading_ = parameter.as_bool();
+    }
   }
 
   result.successful = true;

nav2_rotation_shim_controller/test/test_shim_controller.cpp

@@ -309,6 +309,65 @@ TEST(RotationShimControllerTest, computeVelocityTests)
   EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker), std::runtime_error);
 }
 
+TEST(RotationShimControllerTest, computeVelocityGoalRotationTests) {
+  auto ctrl = std::make_shared<RotationShimShim>();
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("ShimControllerTest");
+  std::string name = "PathFollower";
+  auto tf = std::make_shared<tf2_ros::Buffer>(node->get_clock());
+  auto listener = std::make_shared<tf2_ros::TransformListener>(*tf, node, true);
+  auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("fake_costmap");
+  rclcpp_lifecycle::State state;
+  costmap->on_configure(state);
+  auto tf_broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>(node);
+
+  geometry_msgs::msg::TransformStamped transform;
+  transform.header.frame_id = "base_link";
+  transform.child_frame_id = "odom";
+  transform.transform.rotation.x = 0.0;
+  transform.transform.rotation.y = 0.0;
+  transform.transform.rotation.z = 0.0;
+  transform.transform.rotation.w = 1.0;
+  tf_broadcaster->sendTransform(transform);
+
+  // set a valid primary controller so we can do lifecycle
+  node->declare_parameter(
+    "PathFollower.primary_controller",
+    std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"));
+  node->declare_parameter(
+    "PathFollower.rotate_to_goal_heading",
+    true);
+
+  auto controller = std::make_shared<RotationShimShim>();
+  controller->configure(node, name, tf, costmap);
+  controller->activate();
+
+  // Test state update and path setting
+  nav_msgs::msg::Path path;
+  path.header.frame_id = "fake_frame";
+  path.poses.resize(4);
+
+  geometry_msgs::msg::PoseStamped pose;
+  pose.header.frame_id = "base_link";
+  geometry_msgs::msg::Twist velocity;
+  nav2_controller::SimpleGoalChecker checker;
+  checker.initialize(node, "checker", costmap);
+
+  path.header.frame_id = "base_link";
+  path.poses[1].pose.position.x = 0.05;
+  path.poses[1].pose.position.y = 0.05;
+  path.poses[2].pose.position.x = 0.10;
+  path.poses[2].pose.position.y = 0.10;
+  path.poses[3].pose.position.x = 0.20;
+  path.poses[3].pose.position.y = 0.20;
+  path.poses[3].header.frame_id = "base_link";
+
+  // this should make the goal checker to validated the fact that the robot is in range
+  // of the goal. The rotation shim controller should rotate toward the goal heading
+  // then it will throw an exception because the costmap is bogus
+  controller->setPlan(path);
+  controller->computeVelocityCommands(pose, velocity, &checker);
+}
+
 TEST(RotationShimControllerTest, testDynamicParameter)
 {
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("ShimControllerTest");
@@ -338,7 +397,8 @@ TEST(RotationShimControllerTest, testDynamicParameter)
       rclcpp::Parameter("test.rotate_to_heading_angular_vel", 7.0),
       rclcpp::Parameter("test.max_angular_accel", 7.0),
       rclcpp::Parameter("test.simulate_ahead_time", 7.0),
-      rclcpp::Parameter("test.primary_controller", std::string("HI"))});
+      rclcpp::Parameter("test.primary_controller", std::string("HI")),
+      rclcpp::Parameter("test.rotate_to_goal_heading", true)});
 
   rclcpp::spin_until_future_complete(
     node->get_node_base_interface(),
@@ -349,4 +409,5 @@ TEST(RotationShimControllerTest, testDynamicParameter)
   EXPECT_EQ(node->get_parameter("test.rotate_to_heading_angular_vel").as_double(), 7.0);
   EXPECT_EQ(node->get_parameter("test.max_angular_accel").as_double(), 7.0);
   EXPECT_EQ(node->get_parameter("test.simulate_ahead_time").as_double(), 7.0);
+  EXPECT_EQ(node->get_parameter("test.rotate_to_goal_heading").as_bool(), true);
 }