From 1336f479c11c2b46be7b1a176fddbc3c90006f93 Mon Sep 17 00:00:00 2001 From: Jakubach Date: Fri, 8 Nov 2024 20:49:26 +0100 Subject: [PATCH 1/4] Fixed issue with backward docking and added in place rotation --- README.md | 19 ++++++---- opennav_docking/CMakeLists.txt | 2 +- .../include/opennav_docking/controller.hpp | 16 +++++--- .../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 | 10 ++--- 7 files changed, 73 insertions(+), 43 deletions(-) diff --git a/README.md b/README.md index 48d9166..5c827b8 100644 --- a/README.md +++ b/README.md @@ -207,14 +207,17 @@ For debugging purposes, there are several publishers which can be used with RVIZ | dock_database | The filepath to the dock database to use for this environment | string | N/A | | docks | Instead of `dock_database`, the set of docks specified in the params file itself | vector | N/A | | navigator_bt_xml | BT XML to use for Navigator, if non-default | string | "" | -| controller.k_phi | TODO | double | 3.0 | -| controller.k_delta | TODO | double | 2.0 | -| controller.beta | TODO | double | 0.4 | -| controller.lambda | TODO | double | 2.0 | -| controller.v_linear_min | TODO | double | 0.1 | -| controller.v_linear_max | TODO | double | 0.25 | -| controller.v_angular_max | TODO | double | 0.75 | -| controller.slowdown_radius | TODO | double | 0.25 | +| initial_rotation | 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 | true | +| initial_rotation_min_angle| The difference in the path orientation and the starting robot orientation to trigger a rotate in place, if initial_rotation is enabled. | double | 0.5 | +| controller.k_phi | Ratio of the rate of change of angle relative to distance from the target. Much be > 0. | double | 3.0 | +| controller.k_delta | Higher values result in converging to the target more quickly. | double | 2.0 | +| controller.beta | Parameter to reduce linear velocity proportional to path curvature. Increasing this linearly reduces the velocity (v(t) = v_max / (1 + beta * |curv|^lambda)). | double | 0.4 | +| controller.lambda | Parameter to reduce linear velocity proportional to path curvature. Increasing this exponentially reduces the velocity (v(t) = v_max / (1 + beta * |curv|^lambda)). | double | 2.0 | +| controller.v_linear_min | Minimum velocity for approaching dock. | double | 0.1 | +| controller.v_linear_max | Maximum velocity for approaching dock. | double | 0.25 | +| controller.v_angular_min | Minimum angular velocity for approaching dock. | double | 0.15 | +| controller.v_angular_max | Maximum angular velocity for approaching dock. | double | 0.75 | +| controller.slowdown_radius | Radius to end goal to commense slow down. | double | 0.25 | Note: `dock_plugins` and either `docks` or `dock_database` are required. diff --git a/opennav_docking/CMakeLists.txt b/opennav_docking/CMakeLists.txt index 0c40584..c19e888 100644 --- a/opennav_docking/CMakeLists.txt +++ b/opennav_docking/CMakeLists.txt @@ -137,4 +137,4 @@ endif() ament_export_include_directories(include) ament_export_libraries(${library_name} controller pose_filter) ament_export_dependencies(${dependencies}) -ament_package() +ament_package() \ No newline at end of file diff --git a/opennav_docking/include/opennav_docking/controller.hpp b/opennav_docking/include/opennav_docking/controller.hpp index afae596..e52c760 100644 --- a/opennav_docking/include/opennav_docking/controller.hpp +++ b/opennav_docking/include/opennav_docking/controller.hpp @@ -16,12 +16,12 @@ #define OPENNAV_DOCKING__CONTROLLER_HPP_ #include -#include #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/twist.hpp" #include "nav2_graceful_controller/smooth_control_law.hpp" #include "nav2_util/lifecycle_node.hpp" +#include namespace opennav_docking { @@ -45,8 +45,9 @@ class Controller * @returns True if command is valid, false otherwise. */ bool computeVelocityCommand( - const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Twist & cmd, + const geometry_msgs::msg::Pose & pose,const geometry_msgs::msg::Pose & robot_pose, geometry_msgs::msg::Twist & cmd, bool backward = false); + /** * @brief Callback executed when a parameter change is detected @@ -54,16 +55,21 @@ class Controller */ rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(std::vector parameters); - // Dynamic parameters handler 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/opennav_docking/include/opennav_docking/docking_server.hpp b/opennav_docking/include/opennav_docking/docking_server.hpp index 184d67b..fe0a985 100644 --- a/opennav_docking/include/opennav_docking/docking_server.hpp +++ b/opennav_docking/include/opennav_docking/docking_server.hpp @@ -242,6 +242,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/opennav_docking/src/controller.cpp b/opennav_docking/src/controller.cpp index d4400df..d603e9a 100644 --- a/opennav_docking/src/controller.cpp +++ b/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/opennav_docking/src/docking_server.cpp b/opennav_docking/src/docking_server.cpp index 531a049..43fceff 100644 --- a/opennav_docking/src/docking_server.cpp +++ b/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.5); } 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_ = create_publisher("cmd_vel", 1); @@ -416,21 +420,20 @@ 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; - if (!controller_->computeVelocityCommand(target_pose.pose, command, dock_backwards_)) { + + 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)); + + if(initial_rotation_ && fabs(angle_to_goal) > initial_rotation_min_angle_){ + command = controller_->rotateToTarget(angle_to_goal); + } + else if (!controller_->computeVelocityCommand(target_pose.pose, robot_pose.pose, command, dock_backwards_)) { throw opennav_docking_core::FailedToControl("Failed to get control"); } + vel_publisher_->publish(command); if (this->now() - start > timeout) { @@ -529,7 +532,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"); } diff --git a/opennav_docking/test/test_controller.cpp b/opennav_docking/test/test_controller.cpp index c2f1e25..da05bdb 100644 --- a/opennav_docking/test/test_controller.cpp +++ b/opennav_docking/test/test_controller.cpp @@ -39,21 +39,19 @@ 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(); } - TEST(ControllerTests, DynamicParameters) { auto node = std::make_shared("test"); auto controller = std::make_shared(node); - auto params = std::make_shared( node->get_node_base_interface(), node->get_node_topics_interface(), node->get_node_graph_interface(), node->get_node_services_interface()); - // Set parameters auto results = params->set_parameters_atomically( {rclcpp::Parameter("controller.k_phi", 1.0), @@ -63,11 +61,10 @@ 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 rclcpp::spin_until_future_complete(node->get_node_base_interface(), results); - // Check parameters EXPECT_EQ(node->get_parameter("controller.k_phi").as_double(), 1.0); EXPECT_EQ(node->get_parameter("controller.k_delta").as_double(), 2.0); @@ -76,6 +73,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); } From 92d0076abd05a18e67738b6d61db20189cb3d203 Mon Sep 17 00:00:00 2001 From: Jakubach Date: Fri, 8 Nov 2024 21:24:55 +0100 Subject: [PATCH 2/4] Added rqt dynamic parameters --- opennav_docking/src/docking_server.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/opennav_docking/src/docking_server.cpp b/opennav_docking/src/docking_server.cpp index 43fceff..238a427 100644 --- a/opennav_docking/src/docking_server.cpp +++ b/opennav_docking/src/docking_server.cpp @@ -699,6 +699,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") { @@ -710,6 +712,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(); + } } } From afc88198b5e64bb7c487a527af6d34934f28843d Mon Sep 17 00:00:00 2001 From: Jakubach Date: Fri, 8 Nov 2024 21:36:01 +0100 Subject: [PATCH 3/4] Updated the default k_delta value from 1.0 to 2.0 to match the value from readme --- opennav_docking/src/controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opennav_docking/src/controller.cpp b/opennav_docking/src/controller.cpp index d603e9a..b83e584 100644 --- a/opennav_docking/src/controller.cpp +++ b/opennav_docking/src/controller.cpp @@ -26,7 +26,7 @@ Controller::Controller(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) nav2_util::declare_parameter_if_not_declared( node, "controller.k_phi", rclcpp::ParameterValue(2.0)); nav2_util::declare_parameter_if_not_declared( - node, "controller.k_delta", rclcpp::ParameterValue(1.0)); + node, "controller.k_delta", rclcpp::ParameterValue(2.0)); nav2_util::declare_parameter_if_not_declared( node, "controller.beta", rclcpp::ParameterValue(0.4)); nav2_util::declare_parameter_if_not_declared( From 8aeb3f5b181c096a697a74c908a05cd2e4699e44 Mon Sep 17 00:00:00 2001 From: Jakubach Date: Fri, 8 Nov 2024 21:46:04 +0100 Subject: [PATCH 4/4] Decreased a inital rotation min angle a bit --- README.md | 2 +- opennav_docking/src/docking_server.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 5c827b8..afafaaa 100644 --- a/README.md +++ b/README.md @@ -208,7 +208,7 @@ For debugging purposes, there are several publishers which can be used with RVIZ | docks | Instead of `dock_database`, the set of docks specified in the params file itself | vector | N/A | | navigator_bt_xml | BT XML to use for Navigator, if non-default | string | "" | | initial_rotation | 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 | true | -| initial_rotation_min_angle| The difference in the path orientation and the starting robot orientation to trigger a rotate in place, if initial_rotation is enabled. | double | 0.5 | +| initial_rotation_min_angle| The difference in the path orientation and the starting robot orientation to trigger a rotate in place, if initial_rotation is enabled. | double | 0.3 | | controller.k_phi | Ratio of the rate of change of angle relative to distance from the target. Much be > 0. | double | 3.0 | | controller.k_delta | Higher values result in converging to the target more quickly. | double | 2.0 | | controller.beta | Parameter to reduce linear velocity proportional to path curvature. Increasing this linearly reduces the velocity (v(t) = v_max / (1 + beta * |curv|^lambda)). | double | 0.4 | diff --git a/opennav_docking/src/docking_server.cpp b/opennav_docking/src/docking_server.cpp index 238a427..8d67443 100644 --- a/opennav_docking/src/docking_server.cpp +++ b/opennav_docking/src/docking_server.cpp @@ -41,7 +41,7 @@ DockingServer::DockingServer(const rclcpp::NodeOptions & options) declare_parameter("dock_backwards", false); declare_parameter("dock_prestaging_tolerance", 0.5); declare_parameter("initial_rotation", true); - declare_parameter("initial_rotation_min_angle", 0.5); + declare_parameter("initial_rotation_min_angle", 0.3); } nav2_util::CallbackReturn