From de93463f791fceab7fc51ea14f524813b90bcd26 Mon Sep 17 00:00:00 2001 From: Balint Rozgonyi <43723477+RBT22@users.noreply.github.com> Date: Fri, 31 Jan 2025 20:19:51 +0100 Subject: [PATCH 1/2] Add option to use open-loop control with Rotation Shim (#4880) * Initial implementation Signed-off-by: RBT22 * replace feedback param with closed_loop Signed-off-by: RBT22 * Reset last_angular_vel_ in activate method Signed-off-by: RBT22 * Add closed_loop parameter to dynamicParametersCallback Signed-off-by: RBT22 * Add tests Signed-off-by: RBT22 * Override reset function Signed-off-by: RBT22 --------- Signed-off-by: RBT22 --- .../nav2_rotation_shim_controller.hpp | 8 ++ .../src/nav2_rotation_shim_controller.cpp | 33 ++++++-- .../test/test_shim_controller.cpp | 82 ++++++++++++++++++- 3 files changed, 117 insertions(+), 6 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 fcdc7efaf9c..a1cccf334d5 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" @@ -106,6 +107,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 @@ -176,6 +182,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_; + bool closed_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 f75482e8fe4..f9338c219e7 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -67,6 +67,8 @@ void RotationShimController::configure( 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)); + 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_); @@ -82,6 +84,7 @@ void RotationShimController::configure( control_duration_ = 1.0 / control_frequency; node->get_parameter(plugin_name_ + ".rotate_to_goal_heading", rotate_to_goal_heading_); + node->get_parameter(plugin_name_ + ".closed_loop", closed_loop_); try { primary_controller_ = lp_loader_.createUniqueInstance(primary_controller); @@ -112,6 +115,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( @@ -174,7 +178,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( @@ -203,7 +209,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_, @@ -222,7 +230,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() @@ -280,13 +290,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::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); @@ -345,6 +360,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) { @@ -370,6 +391,8 @@ RotationShimController::dynamicParametersCallback(std::vector } else if (type == ParameterType::PARAMETER_BOOL) { if (name == plugin_name_ + ".rotate_to_goal_heading") { rotate_to_goal_heading_ = 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 1160a5a98af..c4d092120ba 100644 --- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp +++ b/nav2_rotation_shim_controller/test/test_shim_controller.cpp @@ -309,6 +309,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"); @@ -412,7 +490,8 @@ TEST(RotationShimControllerTest, testDynamicParameter) 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.rotate_to_goal_heading", true)}); + rclcpp::Parameter("test.rotate_to_goal_heading", true), + rclcpp::Parameter("test.closed_loop", false)}); rclcpp::spin_until_future_complete( node->get_node_base_interface(), @@ -424,4 +503,5 @@ TEST(RotationShimControllerTest, testDynamicParameter) 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); + EXPECT_EQ(node->get_parameter("test.closed_loop").as_bool(), false); } From ff78a85eaff927a0e03254769c1b26d0d15f273f Mon Sep 17 00:00:00 2001 From: RBT22 Date: Mon, 3 Feb 2025 10:13:03 +0100 Subject: [PATCH 2/2] Remove reset Signed-off-by: RBT22 --- .../nav2_rotation_shim_controller.hpp | 5 ----- .../src/nav2_rotation_shim_controller.cpp | 6 ------ 2 files changed, 11 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 a1cccf334d5..c1777412575 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 @@ -107,11 +107,6 @@ 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 f9338c219e7..12d70a925e7 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -360,12 +360,6 @@ 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) {