From 15753e45d6d625ef70dc1675b55d4f9185302529 Mon Sep 17 00:00:00 2001 From: Jakubach Date: Sat, 9 Nov 2024 22:27:45 +0100 Subject: [PATCH] Correction of merge --- .../include/opennav_docking/controller.hpp | 4 +-- .../opennav_docking/docking_server.hpp | 2 ++ .../opennav_docking/src/controller.cpp | 11 +++++-- .../opennav_docking/src/docking_server.cpp | 30 ++++++++++++++----- 4 files changed, 35 insertions(+), 12 deletions(-) diff --git a/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp b/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp index 0aa6628cab2..c96502dafe4 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp @@ -45,9 +45,8 @@ 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 @@ -69,6 +68,7 @@ class Controller 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_; }; 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 08c4a47a107..d0e87e60405 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp @@ -248,6 +248,8 @@ class DockingServer : public nav2_util::LifecycleNode 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_; + // Enable aproaching a docking station only with initial detection without updates + bool dock_backwards_without_sensor_; // 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 722e6f3abb1..1c9b657db81 100644 --- a/nav2_docking/opennav_docking/src/controller.cpp +++ b/nav2_docking/opennav_docking/src/controller.cpp @@ -51,12 +51,13 @@ Controller::Controller(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) 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_); + k_phi_, k_delta_, beta_, lambda_, slowdown_radius_, v_linear_min_, v_linear_max_, + v_angular_max_); + + // Add callback for dynamic parameters dyn_params_handler_ = node->add_on_set_parameters_callback( std::bind(&Controller::dynamicParametersCallback, this, std::placeholders::_1)); - } bool Controller::computeVelocityCommand( @@ -72,10 +73,12 @@ 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(); @@ -96,12 +99,14 @@ Controller::dynamicParametersCallback(std::vector parameters) } 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; } diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index c6ecdc58beb..664473971b4 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -41,7 +41,8 @@ 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.3); + declare_parameter("initial_rotation_min_angle", 0.5); + declare_parameter("dock_backwards_without_sensor", true); } nav2_util::CallbackReturn @@ -63,6 +64,7 @@ DockingServer::on_configure(const rclcpp_lifecycle::State & state) get_parameter("dock_prestaging_tolerance", dock_prestaging_tolerance_); get_parameter("initial_rotation", initial_rotation_); get_parameter("initial_rotation_min_angle", initial_rotation_min_angle_); + get_parameter("dock_backwards_without_sensor", dock_backwards_without_sensor_); RCLCPP_INFO(get_logger(), "Controller frequency set to %.4fHz", controller_frequency_); vel_publisher_ = std::make_unique(node, "cmd_vel", 1); @@ -389,6 +391,17 @@ void DockingServer::doInitialPerception(Dock * dock, geometry_msgs::msg::PoseSta publishDockingFeedback(DockRobot::Feedback::INITIAL_PERCEPTION); rclcpp::Rate loop_rate(controller_frequency_); auto start = this->now(); + // If docking without sensors, stop the robot for short time to get a stable and quality detection of docking pose + if(dock_backwards_without_sensor_){ + publishZeroVelocity(); + while (rclcpp::ok()) { + if (this->now() - start > rclcpp::Duration::from_seconds(1.0)) { + break; + } + } + } + + start = this->now(); auto timeout = rclcpp::Duration::from_seconds(initial_perception_timeout_); while (!dock->plugin->getRefinedPose(dock_pose, dock->id)) { if (this->now() - start > timeout) { @@ -426,7 +439,7 @@ bool DockingServer::approachDock(Dock * dock, geometry_msgs::msg::PoseStamped & } // Update perception - if (!dock->plugin->getRefinedPose(dock_pose, dock->id)) { + if (!dock_backwards_without_sensor_ && !dock->plugin->getRefinedPose(dock_pose, dock->id)) { throw opennav_docking_core::FailedToDetectDock("Failed dock detection"); } @@ -442,16 +455,16 @@ bool DockingServer::approachDock(Dock * dock, geometry_msgs::msg::PoseStamped & } // Compute and publish controls - geometry_msgs::msg::Twist command; + auto command = std::make_unique(); 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(); - command->header.stamp = now(); - if (!controller_->computeVelocityCommand(target_pose.pose, command->twist, dock_backwards_)) { + if(initial_rotation_ && fabs(angle_to_goal) > initial_rotation_min_angle_){ + command->twist = controller_->rotateToTarget(angle_to_goal); + } + else if (!controller_->computeVelocityCommand(target_pose.pose, robot_pose.pose, command->twist, dock_backwards_)) { throw opennav_docking_core::FailedToControl("Failed to get control"); } vel_publisher_->publish(std::move(command)); @@ -751,6 +764,9 @@ DockingServer::dynamicParametersCallback(std::vector paramete if (name == "initial_rotation") { initial_rotation_ = parameter.as_bool(); } + if (name == "dock_backwards_without_sensor") { + dock_backwards_without_sensor_ = parameter.as_bool(); + } } }