From 7d62a439fb98995db916456789f4e9946047bce4 Mon Sep 17 00:00:00 2001 From: Jakubach Date: Fri, 15 Nov 2024 15:35:39 +0100 Subject: [PATCH] Initial rotation taken from rotation shim controller --- .../include/opennav_docking/controller.hpp | 16 +++++++--- .../opennav_docking/docking_server.hpp | 4 +++ .../opennav_docking/src/controller.cpp | 28 +++++++---------- .../opennav_docking/src/docking_server.cpp | 31 ++++++++++++++----- 4 files changed, 51 insertions(+), 28 deletions(-) diff --git a/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp b/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp index f6490969559..354522cbc44 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp @@ -60,17 +60,23 @@ class Controller 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. + * @brief Perform a command for in-place rotation. + * @param angular_distance_to_heading Angular distance to goal + * @param current_velocity Current angular velocity + * @param dt Control loop duration [s] + * @returns TwistStamped command. */ - geometry_msgs::msg::Twist rotateToTarget(const double & angle_to_target); + geometry_msgs::msg::Twist computeRotateToHeadingCommand( + const double & angular_distance_to_heading, + const geometry_msgs::msg::Twist & current_velocity, + const double & dt); 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_, v_angular_min_; + double slowdown_radius_, v_linear_min_, v_linear_max_, v_angular_max_; + double rotate_to_heading_angular_vel_, rotate_to_heading_max_angular_accel_; }; } // 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 3a7ccdb2b1c..7e834aab670 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp @@ -33,6 +33,7 @@ #include "opennav_docking/navigator.hpp" #include "opennav_docking_core/charging_dock.hpp" #include "tf2_ros/transform_listener.h" +#include "nav_2d_utils/odom_subscriber.hpp" namespace opennav_docking { @@ -251,11 +252,14 @@ class DockingServer : public nav2_util::LifecycleNode double dock_prestaging_tolerance_; // Enable aproaching a docking station only with initial detection without updates bool backward_blind_; + // Angle in which robot can stop initial rotation + double backward_rotation_tolerance_; // This is a class member so it can be accessed in publish feedback rclcpp::Time action_start_time_; std::unique_ptr vel_publisher_; + std::unique_ptr odom_sub_; std::unique_ptr docking_action_server_; std::unique_ptr undocking_action_server_; diff --git a/nav2_docking/opennav_docking/src/controller.cpp b/nav2_docking/opennav_docking/src/controller.cpp index f8e1bb2bb79..8e178818646 100644 --- a/nav2_docking/opennav_docking/src/controller.cpp +++ b/nav2_docking/opennav_docking/src/controller.cpp @@ -48,7 +48,6 @@ 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( @@ -94,8 +93,6 @@ 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(); } @@ -111,20 +108,19 @@ Controller::dynamicParametersCallback(std::vector parameters) return result; } -geometry_msgs::msg::Twist Controller::rotateToTarget(const double & angle_to_target) +geometry_msgs::msg::Twist Controller::computeRotateToHeadingCommand( + const double & angular_distance_to_heading, + const geometry_msgs::msg::Twist & current_velocity, + const double & dt) { - 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; + geometry_msgs::msg::Twist cmd_vel; + 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 min_feasible_angular_speed = current_velocity.angular.z - rotate_to_heading_max_angular_accel_ * dt; + const double max_feasible_angular_speed = current_velocity.angular.z + rotate_to_heading_max_angular_accel_ * dt; + cmd_vel.angular.z = + std::clamp(angular_vel, min_feasible_angular_speed, max_feasible_angular_speed); + return cmd_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 ffc83f311a8..7cbc7a26c83 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -41,6 +41,8 @@ DockingServer::DockingServer(const rclcpp::NodeOptions & options) declare_parameter("dock_backwards", false); declare_parameter("dock_prestaging_tolerance", 0.5); declare_parameter("backward_blind", false); + declare_parameter("odom_topic", "odom"); + declare_parameter("backward_rotation_tolerance", 0.02); } nav2_util::CallbackReturn @@ -48,6 +50,7 @@ DockingServer::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring %s", get_name()); auto node = shared_from_this(); + std::string odom_topic; get_parameter("controller_frequency", controller_frequency_); get_parameter("initial_perception_timeout", initial_perception_timeout_); @@ -61,14 +64,20 @@ DockingServer::on_configure(const rclcpp_lifecycle::State & state) get_parameter("dock_backwards", dock_backwards_); get_parameter("dock_prestaging_tolerance", dock_prestaging_tolerance_); get_parameter("backward_blind", backward_blind_); + get_parameter("odom_topic", odom_topic); + get_parameter("backward_rotation_tolerance",backward_rotation_tolerance_); if(backward_blind_ && !dock_backwards_) { RCLCPP_ERROR(get_logger(), "backward_blind is enabled when dock_backwards is disabled."); return nav2_util::CallbackReturn::FAILURE; } + if(odom_topic.empty()) { + odom_topic = "odom"; + } RCLCPP_INFO(get_logger(), "Controller frequency set to %.4fHz", controller_frequency_); vel_publisher_ = std::make_unique(node, "cmd_vel", 1); tf2_buffer_ = std::make_shared(node->get_clock()); + odom_sub_ = std::make_unique(node, odom_topic); double action_server_result_timeout; nav2_util::declare_parameter_if_not_declared( @@ -160,6 +169,7 @@ DockingServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) curr_dock_type_.clear(); controller_.reset(); vel_publisher_.reset(); + odom_sub_.reset(); return nav2_util::CallbackReturn::SUCCESS; } @@ -415,18 +425,25 @@ void DockingServer::rotateToDock(const geometry_msgs::msg::PoseStamped & dock_po { rclcpp::Rate loop_rate(controller_frequency_); geometry_msgs::msg::PoseStamped robot_pose; - double angle_to_goal; + geometry_msgs::msg::PoseStamped target_pose = dock_pose; auto command = std::make_unique(); + auto current_vel = std::make_unique(); + double angular_distance_to_heading; + const double dt = 1.0 / controller_frequency_; + target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis( + tf2::getYaw(target_pose.pose.orientation) + M_PI); while(rclcpp::ok()) { robot_pose = getRobotPoseInFrame(dock_pose.header.frame_id); - angle_to_goal = angles::shortest_angular_distance(tf2::getYaw(robot_pose.pose.orientation), - atan2(robot_pose.pose.position.y - dock_pose.pose.position.y, - robot_pose.pose.position.x - dock_pose.pose.position.x)); - if(fabs(angle_to_goal) < 0.02) { + angular_distance_to_heading = angles::shortest_angular_distance( + tf2::getYaw(robot_pose.pose.orientation), + tf2::getYaw(target_pose.pose.orientation)); + if(fabs(angular_distance_to_heading) < backward_rotation_tolerance_) { break; } - command->header.stamp = now(); - command->twist = controller_->rotateToTarget(angle_to_goal); + current_vel->twist.angular.z = odom_sub_->getTwist().theta; + command->twist = controller_->computeRotateToHeadingCommand(angular_distance_to_heading, + current_vel->twist, dt); + command->header = robot_pose.header; vel_publisher_->publish(std::move(command)); loop_rate.sleep(); }