From 945acf1c5ca553f8fb5bbedeb4e9f6dc9688fbc3 Mon Sep 17 00:00:00 2001 From: Jakubach Date: Sat, 9 Nov 2024 22:04:54 +0100 Subject: [PATCH] Added an option for docking backward without back sensors and fixed backward docking issues Signed-off-by: Jakubach --- .../include/opennav_docking/controller.hpp | 11 +++++- .../opennav_docking/docking_server.hpp | 4 ++ .../opennav_docking/src/controller.cpp | 38 +++++++++++++------ .../opennav_docking/src/docking_server.cpp | 27 ++++++++----- .../opennav_docking/test/test_controller.cpp | 5 ++- 5 files changed, 61 insertions(+), 24 deletions(-) diff --git a/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp b/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp index afae5965d22..0aa6628cab2 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp @@ -47,6 +47,7 @@ class Controller bool computeVelocityCommand( const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Twist & cmd, bool backward = false); + /** * @brief Callback executed when a parameter change is detected @@ -59,11 +60,17 @@ class Controller rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; std::mutex dynamic_params_lock_; + /** + * @brief Perform a rotation about an angle. + * @param angle_to_target Rotation angle <-2*pi;2*pi>. + * @returns Twist command. + */ + geometry_msgs::msg::Twist rotateToTarget(const double & angle_to_target); + protected: std::unique_ptr control_law_; - double k_phi_, k_delta_, beta_, lambda_; - double slowdown_radius_, v_linear_min_, v_linear_max_, v_angular_max_; + double slowdown_radius_, v_linear_min_, v_linear_max_, v_angular_max_,v_angular_min_; }; } // namespace opennav_docking diff --git a/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp b/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp index 08c3cc1f7c2..08c4a47a107 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp @@ -244,6 +244,10 @@ class DockingServer : public nav2_util::LifecycleNode bool dock_backwards_; // The tolerance to the dock's staging pose not requiring navigation double dock_prestaging_tolerance_; + // The difference in the path orientation and the starting robot orientation to trigger a rotate in place, if initial_rotation is enabled. + double initial_rotation_min_angle_; + // Enable a rotation in place to the goal before starting the path. The control law may generate large sweeping arcs to the goal pose, depending on the initial robot orientation and k_phi, k_delta. + bool initial_rotation_; // This is a class member so it can be accessed in publish feedback rclcpp::Time action_start_time_; diff --git a/nav2_docking/opennav_docking/src/controller.cpp b/nav2_docking/opennav_docking/src/controller.cpp index 8613f2d27fc..722e6f3abb1 100644 --- a/nav2_docking/opennav_docking/src/controller.cpp +++ b/nav2_docking/opennav_docking/src/controller.cpp @@ -35,6 +35,8 @@ Controller::Controller(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) node, "controller.v_linear_min", rclcpp::ParameterValue(0.1)); nav2_util::declare_parameter_if_not_declared( node, "controller.v_linear_max", rclcpp::ParameterValue(0.25)); + nav2_util::declare_parameter_if_not_declared( + node, "controller.v_angular_min", rclcpp::ParameterValue(0.15)); nav2_util::declare_parameter_if_not_declared( node, "controller.v_angular_max", rclcpp::ParameterValue(0.75)); nav2_util::declare_parameter_if_not_declared( @@ -46,22 +48,23 @@ Controller::Controller(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) node->get_parameter("controller.lambda", lambda_); node->get_parameter("controller.v_linear_min", v_linear_min_); node->get_parameter("controller.v_linear_max", v_linear_max_); + node->get_parameter("controller.v_angular_min", v_angular_min_); node->get_parameter("controller.v_angular_max", v_angular_max_); node->get_parameter("controller.slowdown_radius", slowdown_radius_); - control_law_ = std::make_unique( - k_phi_, k_delta_, beta_, lambda_, slowdown_radius_, v_linear_min_, v_linear_max_, - v_angular_max_); - // Add callback for dynamic parameters + control_law_ = std::make_unique( + k_phi_, k_delta_, beta_, lambda_, slowdown_radius_, v_linear_min_, v_linear_max_, v_angular_max_); dyn_params_handler_ = node->add_on_set_parameters_callback( std::bind(&Controller::dynamicParametersCallback, this, std::placeholders::_1)); + } -bool Controller::computeVelocityCommand( - const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Twist & cmd, bool backward) + bool Controller::computeVelocityCommand( + const geometry_msgs::msg::Pose & pose,const geometry_msgs::msg::Pose & robot_pose, geometry_msgs::msg::Twist & cmd, + bool backward) { std::lock_guard lock(dynamic_params_lock_); - cmd = control_law_->calculateRegularVelocity(pose, backward); + cmd = control_law_->calculateRegularVelocity(pose,robot_pose, backward); return true; } @@ -69,12 +72,10 @@ rcl_interfaces::msg::SetParametersResult Controller::dynamicParametersCallback(std::vector parameters) { std::lock_guard lock(dynamic_params_lock_); - rcl_interfaces::msg::SetParametersResult result; for (auto parameter : parameters) { const auto & type = parameter.get_type(); const auto & name = parameter.get_name(); - if (type == rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) { if (name == "controller.k_phi") { k_phi_ = parameter.as_double(); @@ -90,19 +91,34 @@ Controller::dynamicParametersCallback(std::vector parameters) v_linear_max_ = parameter.as_double(); } else if (name == "controller.v_angular_max") { v_angular_max_ = parameter.as_double(); + } else if (name == "controller.v_angular_min") { + v_angular_min_ = parameter.as_double(); } else if (name == "controller.slowdown_radius") { slowdown_radius_ = parameter.as_double(); } - // Update the smooth control law with the new params control_law_->setCurvatureConstants(k_phi_, k_delta_, beta_, lambda_); control_law_->setSlowdownRadius(slowdown_radius_); control_law_->setSpeedLimit(v_linear_min_, v_linear_max_, v_angular_max_); } } - result.successful = true; return result; } +geometry_msgs::msg::Twist Controller::rotateToTarget(const double & angle_to_target) +{ + geometry_msgs::msg::Twist vel; + vel.linear.x = 0.0; + vel.angular.z = 0.0; + if(angle_to_target > 0) { + vel.angular.z = std::clamp(1.0 * angle_to_target * v_angular_max_, v_angular_min_, v_angular_max_); + } + else if (angle_to_target < 0) { + vel.angular.z = std::clamp(1.0 * angle_to_target * v_angular_max_, -v_angular_max_, -v_angular_min_); + } + return vel; +} + + } // namespace opennav_docking diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index b350cb40262..c6ecdc58beb 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -40,6 +40,8 @@ DockingServer::DockingServer(const rclcpp::NodeOptions & options) declare_parameter("fixed_frame", "odom"); declare_parameter("dock_backwards", false); declare_parameter("dock_prestaging_tolerance", 0.5); + declare_parameter("initial_rotation", true); + declare_parameter("initial_rotation_min_angle", 0.3); } nav2_util::CallbackReturn @@ -59,6 +61,8 @@ DockingServer::on_configure(const rclcpp_lifecycle::State & state) get_parameter("fixed_frame", fixed_frame_); get_parameter("dock_backwards", dock_backwards_); get_parameter("dock_prestaging_tolerance", dock_prestaging_tolerance_); + get_parameter("initial_rotation", initial_rotation_); + get_parameter("initial_rotation_min_angle", initial_rotation_min_angle_); RCLCPP_INFO(get_logger(), "Controller frequency set to %.4fHz", controller_frequency_); vel_publisher_ = std::make_unique(node, "cmd_vel", 1); @@ -437,15 +441,12 @@ bool DockingServer::approachDock(Dock * dock, geometry_msgs::msg::PoseStamped & tf2::getYaw(target_pose.pose.orientation) + M_PI); } - // The control law can get jittery when close to the end when atan2's can explode. - // Thus, we backward project the controller's target pose a little bit after the - // dock so that the robot never gets to the end of the spiral before its in contact - // with the dock to stop the docking procedure. - const double backward_projection = 0.25; - const double yaw = tf2::getYaw(target_pose.pose.orientation); - target_pose.pose.position.x += cos(yaw) * backward_projection; - target_pose.pose.position.y += sin(yaw) * backward_projection; - tf2_buffer_->transform(target_pose, target_pose, base_frame_); + // Compute and publish controls + geometry_msgs::msg::Twist command; + + geometry_msgs::msg::PoseStamped robot_pose = getRobotPoseInFrame(target_pose.header.frame_id); + const double angle_to_goal = angles::shortest_angular_distance( + tf2::getYaw(robot_pose.pose.orientation), atan2(robot_pose.pose.position.y - target_pose.pose.position.y, robot_pose.pose.position.x - target_pose.pose.position.x)); // Compute and publish controls auto command = std::make_unique(); @@ -557,7 +558,7 @@ bool DockingServer::getCommandToPose( tf2_buffer_->transform(target_pose, target_pose, base_frame_); // Compute velocity command - if (!controller_->computeVelocityCommand(target_pose.pose, cmd, backward)) { + if (!controller_->computeVelocityCommand(target_pose.pose,robot_pose.pose, cmd, backward)) { throw opennav_docking_core::FailedToControl("Failed to get control"); } @@ -733,6 +734,8 @@ DockingServer::dynamicParametersCallback(std::vector paramete undock_linear_tolerance_ = parameter.as_double(); } else if (name == "undock_angular_tolerance") { undock_angular_tolerance_ = parameter.as_double(); + } else if (name == "initial_rotation_min_angle") { + initial_rotation_min_angle_ = parameter.as_double(); } } else if (type == ParameterType::PARAMETER_STRING) { if (name == "base_frame") { @@ -744,6 +747,10 @@ DockingServer::dynamicParametersCallback(std::vector paramete if (name == "max_retries") { max_retries_ = parameter.as_int(); } + } else if(type == ParameterType::PARAMETER_BOOL){ + if (name == "initial_rotation") { + initial_rotation_ = parameter.as_bool(); + } } } diff --git a/nav2_docking/opennav_docking/test/test_controller.cpp b/nav2_docking/opennav_docking/test/test_controller.cpp index c2f1e25f1c1..d89a6132659 100644 --- a/nav2_docking/opennav_docking/test/test_controller.cpp +++ b/nav2_docking/opennav_docking/test/test_controller.cpp @@ -39,8 +39,9 @@ TEST(ControllerTests, ObjectLifecycle) auto controller = std::make_unique(node); geometry_msgs::msg::Pose pose; + geometry_msgs::msg::Pose robot_pose; geometry_msgs::msg::Twist cmd_out, cmd_init; - EXPECT_TRUE(controller->computeVelocityCommand(pose, cmd_out)); + EXPECT_TRUE(controller->computeVelocityCommand(pose,robot_pose, cmd_out)); EXPECT_NE(cmd_init, cmd_out); controller.reset(); } @@ -63,6 +64,7 @@ TEST(ControllerTests, DynamicParameters) { rclcpp::Parameter("controller.v_linear_min", 5.0), rclcpp::Parameter("controller.v_linear_max", 6.0), rclcpp::Parameter("controller.v_angular_max", 7.0), + rclcpp::Parameter("controller.v_angular_min", 2.0), rclcpp::Parameter("controller.slowdown_radius", 8.0)}); // Spin @@ -76,6 +78,7 @@ TEST(ControllerTests, DynamicParameters) { EXPECT_EQ(node->get_parameter("controller.v_linear_min").as_double(), 5.0); EXPECT_EQ(node->get_parameter("controller.v_linear_max").as_double(), 6.0); EXPECT_EQ(node->get_parameter("controller.v_angular_max").as_double(), 7.0); + EXPECT_EQ(node->get_parameter("controller.v_angular_min").as_double(), 2.0); EXPECT_EQ(node->get_parameter("controller.slowdown_radius").as_double(), 8.0); }