Initial implementation

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

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"
@@ -179,6 +180,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 open_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_ + ".feedback", rclcpp::ParameterValue(std::string("CLOSED_LOOP")));
 
   node->get_parameter(plugin_name_ + ".angular_dist_threshold", angular_dist_threshold_);
   node->get_parameter(plugin_name_ + ".angular_disengage_threshold", angular_disengage_threshold_);
@@ -90,6 +92,21 @@ 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_);
 
+  std::string feedback_type;
+  node->get_parameter(plugin_name_ + ".feedback", feedback_type);
+
+    // Get control type
+  if (feedback_type == "OPEN_LOOP") {
+    open_loop_ = true;
+  } else if (feedback_type == "CLOSED_LOOP") {
+    open_loop_ = false;
+  } else {
+    RCLCPP_ERROR(
+      logger_,
+      "Invalid feedback_type, options are OPEN_LOOP and CLOSED_LOOP. Setting to CLOSED_LOOP!");
+    open_loop_ = false;
+  }
+
   try {
     primary_controller_ = lp_loader_.createUniqueInstance(primary_controller);
     RCLCPP_INFO(
@@ -141,6 +158,8 @@ void RotationShimController::deactivate()
     node->remove_on_set_parameters_callback(dyn_params_handler_.get());
   }
   dyn_params_handler_.reset();
+
+  last_angular_vel_ = std::numeric_limits<double>::max();
 }
 
 void RotationShimController::cleanup()
@@ -184,7 +203,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 +234,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 +255,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 +315,18 @@ RotationShimController::computeRotateToHeadingCommand(
   const geometry_msgs::msg::PoseStamped & pose,
   const geometry_msgs::msg::Twist & velocity)
 {
+  auto current = open_loop_ ? last_angular_vel_ : velocity.angular.z;
+  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);