From 7ac66d3661c5e2b6cf429ddebfafc9e72bf36809 Mon Sep 17 00:00:00 2001 From: RBT22 Date: Wed, 29 Jan 2025 11:21:47 +0000 Subject: [PATCH 1/6] Initial implementation Signed-off-by: RBT22 --- .../nav2_rotation_shim_controller.hpp | 3 ++ .../src/nav2_rotation_shim_controller.cpp | 40 ++++++++++++++++--- 2 files changed, 38 insertions(+), 5 deletions(-) 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..58d5a80c920 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 #include #include +#include #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::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..940d2e015fa 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_ + ".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::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::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); From eefde5ad1357bfed9798a1703814ba93ac9e864b Mon Sep 17 00:00:00 2001 From: RBT22 Date: Thu, 30 Jan 2025 07:57:11 +0000 Subject: [PATCH 2/6] replace feedback param with closed_loop Signed-off-by: RBT22 --- .../nav2_rotation_shim_controller.hpp | 2 +- .../src/nav2_rotation_shim_controller.cpp | 20 +++---------------- 2 files changed, 4 insertions(+), 18 deletions(-) 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 58d5a80c920..dba72182fec 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 @@ -180,7 +180,7 @@ 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_; + bool closed_loop_; double last_angular_vel_ = std::numeric_limits::max(); // Dynamic parameters handler 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 940d2e015fa..dcf2ea31ea1 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -74,7 +74,7 @@ void RotationShimController::configure( 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, 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_); @@ -91,21 +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_); - - 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; - } + node->get_parameter(plugin_name_ + ".closed_loop", closed_loop_); try { primary_controller_ = lp_loader_.createUniqueInstance(primary_controller); @@ -315,7 +301,7 @@ RotationShimController::computeRotateToHeadingCommand( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity) { - auto current = open_loop_ ? last_angular_vel_ : velocity.angular.z; + auto current = closed_loop_ ? velocity.angular.z : last_angular_vel_; if (current == std::numeric_limits::max()) { current = 0.0; } From d6a4fbcf43494b548342ef32191482f51a848180 Mon Sep 17 00:00:00 2001 From: RBT22 Date: Thu, 30 Jan 2025 07:59:00 +0000 Subject: [PATCH 3/6] Reset last_angular_vel_ in activate method Signed-off-by: RBT22 --- .../src/nav2_rotation_shim_controller.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) 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 dcf2ea31ea1..206c70d7431 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -122,6 +122,7 @@ void RotationShimController::activate() primary_controller_->activate(); in_rotation_ = false; + last_angular_vel_ = std::numeric_limits::max(); auto node = node_.lock(); dyn_params_handler_ = node->add_on_set_parameters_callback( @@ -144,8 +145,6 @@ void RotationShimController::deactivate() node->remove_on_set_parameters_callback(dyn_params_handler_.get()); } dyn_params_handler_.reset(); - - last_angular_vel_ = std::numeric_limits::max(); } void RotationShimController::cleanup() From 3dc5c3c7ea19b2bed3874a9c233ba3e20f2283bc Mon Sep 17 00:00:00 2001 From: RBT22 Date: Thu, 30 Jan 2025 08:00:08 +0000 Subject: [PATCH 4/6] Add closed_loop parameter to dynamicParametersCallback Signed-off-by: RBT22 --- .../src/nav2_rotation_shim_controller.cpp | 2 ++ 1 file changed, 2 insertions(+) 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 206c70d7431..2abddb10a3f 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -415,6 +415,8 @@ RotationShimController::dynamicParametersCallback(std::vector 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(); } } } From 6a22187baec31fc77ffb9b4f1336d0a3a5b854cb Mon Sep 17 00:00:00 2001 From: RBT22 Date: Thu, 30 Jan 2025 08:43:41 +0000 Subject: [PATCH 5/6] Add tests Signed-off-by: RBT22 --- .../test/test_shim_controller.cpp | 82 ++++++++++++++++++- 1 file changed, 81 insertions(+), 1 deletion(-) 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(); + auto node = std::make_shared("ShimControllerTest"); + std::string name = "PathFollower"; + auto tf = std::make_shared(node->get_clock()); + auto listener = std::make_shared(*tf, node, true); + auto costmap = std::make_shared("fake_costmap"); + rclcpp_lifecycle::State state; + costmap->on_configure(state); + auto tf_broadcaster = std::make_shared(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(); + 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(); auto node = std::make_shared("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); } From 4be44fb39e74ef972103cc9d43312ef141222151 Mon Sep 17 00:00:00 2001 From: RBT22 Date: Fri, 31 Jan 2025 08:49:04 +0100 Subject: [PATCH 6/6] Override reset function Signed-off-by: RBT22 --- .../nav2_rotation_shim_controller.hpp | 5 +++++ .../src/nav2_rotation_shim_controller.cpp | 6 ++++++ 2 files changed, 11 insertions(+) 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 dba72182fec..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 @@ -103,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 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 2abddb10a3f..1d8e3d5da6b 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -388,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::max(); + primary_controller_->reset(); +} + rcl_interfaces::msg::SetParametersResult RotationShimController::dynamicParametersCallback(std::vector parameters) {