Add option to use open-loop control with Rotation Shim (#4880)

* Initial implementation

Signed-off-by: RBT22 <rozgonyibalint@gmail.com>

* replace feedback param with closed_loop

Signed-off-by: RBT22 <rozgonyibalint@gmail.com>

* Reset last_angular_vel_ in activate method

Signed-off-by: RBT22 <rozgonyibalint@gmail.com>

* Add closed_loop parameter to dynamicParametersCallback

Signed-off-by: RBT22 <rozgonyibalint@gmail.com>

* Add tests

Signed-off-by: RBT22 <rozgonyibalint@gmail.com>

* Override reset function

Signed-off-by: RBT22 <rozgonyibalint@gmail.com>

---------

Signed-off-by: RBT22 <rozgonyibalint@gmail.com>

Author: RBT22

nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp

@@ -20,6 +20,7 @@
 #include <memory>
 #include <algorithm>
 #include <mutex>
+#include <limits>
 
 #include "rclcpp/rclcpp.hpp"
 #include "pluginlib/class_loader.hpp"
@@ -102,6 +103,11 @@ class RotationShimController : public nav2_core::Controller
    */
   void setSpeedLimit(const double & speed_limit, const bool & percentage) override;
 
+  /**
+   * @brief Reset the state of the controller
+   */
+  void reset() override;
+
 protected:
   /**
    * @brief Finds the point on the path that is roughly the sampling
@@ -179,6 +185,8 @@ class RotationShimController : public nav2_core::Controller
   double rotate_to_heading_angular_vel_, max_angular_accel_;
   double control_duration_, simulate_ahead_time_;
   bool rotate_to_goal_heading_, in_rotation_, rotate_to_heading_once_;
+  bool closed_loop_;
+  double last_angular_vel_ = std::numeric_limits<double>::max();
 
   // Dynamic parameters handler
   std::mutex mutex_;

nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp

@@ -73,6 +73,8 @@ void RotationShimController::configure(
     node, plugin_name_ + ".rotate_to_goal_heading", rclcpp::ParameterValue(false));
   nav2_util::declare_parameter_if_not_declared(
     node, plugin_name_ + ".rotate_to_heading_once", rclcpp::ParameterValue(false));
+  nav2_util::declare_parameter_if_not_declared(
+    node, plugin_name_ + ".closed_loop", rclcpp::ParameterValue(true));
 
   node->get_parameter(plugin_name_ + ".angular_dist_threshold", angular_dist_threshold_);
   node->get_parameter(plugin_name_ + ".angular_disengage_threshold", angular_disengage_threshold_);
@@ -89,6 +91,7 @@ void RotationShimController::configure(
 
   node->get_parameter(plugin_name_ + ".rotate_to_goal_heading", rotate_to_goal_heading_);
   node->get_parameter(plugin_name_ + ".rotate_to_heading_once", rotate_to_heading_once_);
+  node->get_parameter(plugin_name_ + ".closed_loop", closed_loop_);
 
   try {
     primary_controller_ = lp_loader_.createUniqueInstance(primary_controller);
@@ -119,6 +122,7 @@ void RotationShimController::activate()
 
   primary_controller_->activate();
   in_rotation_ = false;
+  last_angular_vel_ = std::numeric_limits<double>::max();
 
   auto node = node_.lock();
   dyn_params_handler_ = node->add_on_set_parameters_callback(
@@ -184,7 +188,9 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands
 
         double angular_distance_to_heading = angles::shortest_angular_distance(pose_yaw, goal_yaw);
 
-        return computeRotateToHeadingCommand(angular_distance_to_heading, pose, velocity);
+        auto cmd_vel = computeRotateToHeadingCommand(angular_distance_to_heading, pose, velocity);
+        last_angular_vel_ = cmd_vel.twist.angular.z;
+        return cmd_vel;
       }
     } catch (const std::runtime_error & e) {
       RCLCPP_INFO(
@@ -213,7 +219,9 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands
           logger_,
           "Robot is not within the new path's rough heading, rotating to heading...");
         in_rotation_ = true;
-        return computeRotateToHeadingCommand(angular_distance_to_heading, pose, velocity);
+        auto cmd_vel = computeRotateToHeadingCommand(angular_distance_to_heading, pose, velocity);
+        last_angular_vel_ = cmd_vel.twist.angular.z;
+        return cmd_vel;
       } else {
         RCLCPP_DEBUG(
           logger_,
@@ -232,7 +240,9 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands
 
   // If at this point, use the primary controller to path track
   in_rotation_ = false;
-  return primary_controller_->computeVelocityCommands(pose, velocity, goal_checker);
+  auto cmd_vel = primary_controller_->computeVelocityCommands(pose, velocity, goal_checker);
+  last_angular_vel_ = cmd_vel.twist.angular.z;
+  return cmd_vel;
 }
 
 geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathPt()
@@ -290,13 +300,18 @@ RotationShimController::computeRotateToHeadingCommand(
   const geometry_msgs::msg::PoseStamped & pose,
   const geometry_msgs::msg::Twist & velocity)
 {
+  auto current = closed_loop_ ? velocity.angular.z : last_angular_vel_;
+  if (current == std::numeric_limits<double>::max()) {
+    current = 0.0;
+  }
+
   geometry_msgs::msg::TwistStamped cmd_vel;
   cmd_vel.header = pose.header;
   const double sign = angular_distance_to_heading > 0.0 ? 1.0 : -1.0;
   const double angular_vel = sign * rotate_to_heading_angular_vel_;
   const double & dt = control_duration_;
-  const double min_feasible_angular_speed = velocity.angular.z - max_angular_accel_ * dt;
-  const double max_feasible_angular_speed = velocity.angular.z + max_angular_accel_ * dt;
+  const double min_feasible_angular_speed = current - max_angular_accel_ * dt;
+  const double max_feasible_angular_speed = current + max_angular_accel_ * dt;
   cmd_vel.twist.angular.z =
     std::clamp(angular_vel, min_feasible_angular_speed, max_feasible_angular_speed);
 
@@ -373,6 +388,12 @@ void RotationShimController::setSpeedLimit(const double & speed_limit, const boo
   primary_controller_->setSpeedLimit(speed_limit, percentage);
 }
 
+void RotationShimController::reset()
+{
+  last_angular_vel_ = std::numeric_limits<double>::max();
+  primary_controller_->reset();
+}
+
 rcl_interfaces::msg::SetParametersResult
 RotationShimController::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
 {
@@ -400,6 +421,8 @@ RotationShimController::dynamicParametersCallback(std::vector<rclcpp::Parameter>
         rotate_to_goal_heading_ = parameter.as_bool();
       } else if (name == plugin_name_ + ".rotate_to_heading_once") {
         rotate_to_heading_once_ = parameter.as_bool();
+      } else if (name == plugin_name_ + ".closed_loop") {
+        closed_loop_ = parameter.as_bool();
       }
     }
   }

nav2_rotation_shim_controller/test/test_shim_controller.cpp

@@ -317,6 +317,84 @@ TEST(RotationShimControllerTest, computeVelocityTests)
   EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker), std::runtime_error);
 }
 
+TEST(RotationShimControllerTest, openLoopRotationTests) {
+  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(
+    "controller_frequency",
+    20.0);
+  node->declare_parameter(
+    "PathFollower.rotate_to_goal_heading",
+    true);
+  node->declare_parameter(
+    "PathFollower.closed_loop",
+    false);
+
+  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 = "base_link";
+  path.poses.resize(4);
+
+  geometry_msgs::msg::PoseStamped pose;
+  pose.header.frame_id = "base_link";
+  geometry_msgs::msg::Twist velocity;
+  nav2_controller::SimpleGoalChecker checker;
+  node->declare_parameter(
+    "checker.xy_goal_tolerance",
+    1.0);
+  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;
+  // goal position within checker xy_goal_tolerance
+  path.poses[3].pose.position.x = 0.20;
+  path.poses[3].pose.position.y = 0.20;
+  // goal heading 45 degrees to the left
+  path.poses[3].pose.orientation.z = -0.3826834;
+  path.poses[3].pose.orientation.w = 0.9238795;
+  path.poses[3].header.frame_id = "base_link";
+
+  // Calculate first velocity command
+  controller->setPlan(path);
+  auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker);
+  EXPECT_NEAR(cmd_vel.twist.angular.z, -0.16, 1e-4);
+
+  // Test second velocity command with wrong odometry
+  velocity.angular.z = 1.8;
+  cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker);
+  EXPECT_NEAR(cmd_vel.twist.angular.z, -0.32, 1e-4);
+}
+
 TEST(RotationShimControllerTest, computeVelocityGoalRotationTests) {
   auto ctrl = std::make_shared<RotationShimShim>();
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("ShimControllerTest");
@@ -550,7 +628,8 @@ TEST(RotationShimControllerTest, testDynamicParameter)
       rclcpp::Parameter("test.simulate_ahead_time", 7.0),
       rclcpp::Parameter("test.primary_controller", std::string("HI")),
       rclcpp::Parameter("test.rotate_to_goal_heading", true),
-      rclcpp::Parameter("test.rotate_to_heading_once", true)});
+      rclcpp::Parameter("test.rotate_to_heading_once", true),
+      rclcpp::Parameter("test.closed_loop", false)});
 
   rclcpp::spin_until_future_complete(
     node->get_node_base_interface(),
@@ -563,4 +642,5 @@ TEST(RotationShimControllerTest, testDynamicParameter)
   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);
   EXPECT_EQ(node->get_parameter("test.rotate_to_heading_once").as_bool(), true);
+  EXPECT_EQ(node->get_parameter("test.closed_loop").as_bool(), false);
 }