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); }