diff --git a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp
index abaf02c9393..1a13eb00c1d 100644
--- a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp
+++ b/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_;
diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp
index c877355c6ab..1d8e3d5da6b 100644
--- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp
+++ b/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();
}
}
}
diff --git a/nav2_rotation_shim_controller/test/test_shim_controller.cpp b/nav2_rotation_shim_controller/test/test_shim_controller.cpp
index adfc56f8f78..04ce65f54c6 100644
--- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp
+++ b/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);
}