[RotationShimController] Rotate to goal heading (ros-navigation#4332)

When arriving in the goal xy tolerance, the rotation shim controller
takes back the control to command the robot to rotate in the goal
heading orientation.

The initial goal of the rotationShimController was to rotate the robot
at the beginning of a navigation towards the paths orientation because
some controllers are not good at performing in place rotations. For the
same reason, the rotationShimController should be able to rotate the
robot towards the goal heading.

Signed-off-by: Antoine Gennart <gennart.antoine@gmail.com>

nav2_rotation_shim_controller/README.md

@@ -12,6 +12,8 @@ This is useful for situations when working with plugins that are either too spec
 
 As such, this controller will check the rough heading difference with respect to the robot and a newly received path. If within a threshold, it will pass the request onto the controller to execute. If it is outside of the threshold, this controller will rotate the robot towards that path heading. Once it is within the tolerance, it will then pass off control-execution from this rotation shim controller onto the primary controller plugin. At this point, the robot is still going to be rotating, allowing the current plugin to take control for a smooth hand off into path tracking. It is recommended to be more generous than strict in the angular threshold to allow for a smoother transition, but should be tuned for a specific application's desired behaviors.
 
+When the `rotate_to_goal_heading` parameter is set to true, this controller is also able to take back control of the robot when reaching the XY goal tolerance of the goal checker. In this case, the robot will rotate towards the goal heading until the goal checker validate the goal and ends the current navigation task.
+
 The Rotation Shim Controller is suitable for:
 - Robots that can rotate in place, such as differential and omnidirectional robots.
 - Preference to rotate in place rather than 'spiral out' when starting to track a new path that is at a significantly different heading than the robot's current heading.
@@ -35,6 +37,7 @@ See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/
 | `primary_controller` | Internal controller plugin to use for actual control behavior after rotating to heading | 
 | `max_angular_accel` | Maximum angular acceleration for rotation to heading | 
 | `simulate_ahead_time` | Time in seconds to forward simulate a rotation command to check for collisions. If a collision is found, forwards control back to the primary controller plugin. | 
+| `rotate_to_goal_heading` | If true, the rotationShimController will take back control of the robot when in XY tolerance of the goal and start rotating to the goal heading | 
 
 Example fully-described XML with default parameter values:
 
@@ -66,6 +69,7 @@ controller_server:
       rotate_to_heading_angular_vel: 1.8
       max_angular_accel: 3.2
       simulate_ahead_time: 1.0
+      rotate_to_goal_heading: false
 
       # DWB parameters
       ...

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/include/nav2_rotation_shim_controller/tools/utils.hpp

@@ -0,0 +1,60 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+// Copyright (c) 2023 Open Navigation LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_ROTATION_SHIM_CONTROLLER__TOOLS__UTILS_HPP_
+#define NAV2_ROTATION_SHIM_CONTROLLER__TOOLS__UTILS_HPP_
+
+#include "nav2_core/goal_checker.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+namespace nav2_rotation_shim_controller::utils
+{
+
+/**
+* @brief get the current pose of the robot
+* @param goal_checker goal checker to get tolerances
+* @param robot robot pose
+* @param goal goal pose
+* @return bool Whether the robot is in the distance tolerance ignoring rotation and speed
+*/
+inline 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;
+}
+
+}  // namespace nav2_rotation_shim_controller::utils
+
+#endif  // NAV2_ROTATION_SHIM_CONTROLLER__TOOLS__UTILS_HPP_

nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp

@@ -19,6 +19,7 @@
 #include <utility>
 
 #include "nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp"
+#include "nav2_rotation_shim_controller/tools/utils.hpp"
 
 using rcl_interfaces::msg::ParameterType;
 
@@ -60,6 +61,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 +76,8 @@ void RotationShimController::configure(
   node->get_parameter("controller_frequency", control_frequency);
   control_duration_ = 1.0 / control_frequency;
 
+  node->get_parameter(plugin_name_ + ".rotate_to_goal_heading", rotate_to_goal_heading_);
+
   try {
     primary_controller_ = lp_loader_.createUniqueInstance(primary_controller);
     RCLCPP_INFO(
@@ -139,6 +144,41 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands
   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::PoseStamped sampled_pt_goal = getSampledPathGoal();
+
+      if (!nav2_util::transformPoseInTargetFrame(
+          sampled_pt_goal, sampled_pt_goal, *tf_,
+          pose.header.frame_id))
+      {
+        throw nav2_core::ControllerTFError("Failed to transform pose to base frame!");
+      }
+
+      if (utils::withinPositionGoalTolerance(
+          goal_checker,
+          pose.pose,
+          sampled_pt_goal.pose))
+      {
+        double pose_yaw = tf2::getYaw(pose.pose.orientation);
+        double goal_yaw = tf2::getYaw(sampled_pt_goal.pose.orientation);
+
+        double angular_distance_to_heading = angles::shortest_angular_distance(pose_yaw, goal_yaw);
+
+        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()));
@@ -198,6 +238,17 @@ geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathPt()
   return current_path_.poses.back();
 }
 
+geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathGoal()
+{
+  if (current_path_.poses.empty()) {
+    throw nav2_core::InvalidPath("Path is empty - cannot find a goal point");
+  }
+
+  auto goal = current_path_.poses.back();
+  goal.header.stamp = clock_->now();
+  return goal;
+}
+
 geometry_msgs::msg::Pose
 RotationShimController::transformPoseToBaseFrame(const geometry_msgs::msg::PoseStamped & pt)
 {
@@ -302,6 +353,10 @@ RotationShimController::dynamicParametersCallback(std::vector<rclcpp::Parameter>
       } else if (name == plugin_name_ + ".simulate_ahead_time") {
         simulate_ahead_time_ = parameter.as_double();
       }
+    } else if (type == ParameterType::PARAMETER_BOOL) {
+      if (name == plugin_name_ + ".rotate_to_goal_heading") {
+        rotate_to_goal_heading_ = parameter.as_bool();
+      }
     }
   }
 

nav2_rotation_shim_controller/test/test_shim_controller.cpp

@@ -309,6 +309,67 @@ 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[0].pose.position.x = 0.0;
+  path.poses[0].pose.position.y = 0.0;
+  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);
+  EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker), std::runtime_error);
+}
+
 TEST(RotationShimControllerTest, testDynamicParameter)
 {
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("ShimControllerTest");
@@ -338,7 +399,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 +411,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);
 }