From 46cddefe2e4e297b835178b7c3a623f5a5495b86 Mon Sep 17 00:00:00 2001 From: jakubach Date: Thu, 14 Nov 2024 00:53:37 +0100 Subject: [PATCH] Reversed changes of calculateRegularVelocity method Signed-off-by: jakubach --- .../opennav_docking/include/opennav_docking/controller.hpp | 1 - nav2_docking/opennav_docking/src/controller.cpp | 3 +-- nav2_docking/opennav_docking/src/docking_server.cpp | 7 +++---- 3 files changed, 4 insertions(+), 7 deletions(-) diff --git a/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp b/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp index f6905c8fb6..f649096955 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp @@ -46,7 +46,6 @@ class Controller */ bool computeVelocityCommand( const geometry_msgs::msg::Pose & pose, - const geometry_msgs::msg::Pose & robot_pose, geometry_msgs::msg::Twist & cmd, bool backward = false); /** diff --git a/nav2_docking/opennav_docking/src/controller.cpp b/nav2_docking/opennav_docking/src/controller.cpp index b851ba9807..f8e1bb2bb7 100644 --- a/nav2_docking/opennav_docking/src/controller.cpp +++ b/nav2_docking/opennav_docking/src/controller.cpp @@ -62,11 +62,10 @@ Controller::Controller(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) 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, robot_pose, backward); + cmd = control_law_->calculateRegularVelocity(pose, backward); return true; } diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index 0dcf7febb6..fa9b60e25d 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -471,7 +471,7 @@ bool DockingServer::approachDock(Dock * dock, geometry_msgs::msg::PoseStamped & // Make sure that the target pose is pointing at the robot when moving backwards // This is to ensure that the robot doesn't try to dock from the wrong side target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis( - yaw + M_PI); + yaw + M_PI); target_pose.pose.position.x -= cos(yaw) * backward_projection; target_pose.pose.position.y -= sin(yaw) * backward_projection; } else { @@ -483,8 +483,7 @@ bool DockingServer::approachDock(Dock * dock, geometry_msgs::msg::PoseStamped & // Compute and publish controls auto command = std::make_unique(); command->header.stamp = now(); - geometry_msgs::msg::PoseStamped robot_pose = getRobotPoseInFrame(dock_pose.header.frame_id); - if (!controller_->computeVelocityCommand(target_pose.pose, robot_pose.pose, + if (!controller_->computeVelocityCommand(target_pose.pose, command->twist, dock_backwards_)) { throw opennav_docking_core::FailedToControl("Failed to get control"); @@ -593,7 +592,7 @@ bool DockingServer::getCommandToPose( tf2_buffer_->transform(target_pose, target_pose, base_frame_); // Compute velocity command - if (!controller_->computeVelocityCommand(target_pose.pose, robot_pose.pose, cmd, backward)) { + if (!controller_->computeVelocityCommand(target_pose.pose, cmd, backward)) { throw opennav_docking_core::FailedToControl("Failed to get control"); }