Reversed changes of calculateRegularVelocity method

Signed-off-by: jakubach <[email protected]>
ros-navigation · Nov 13, 2024 · 46cddef · 46cddef
1 parent 9c02a96
commit 46cddef
Show file tree

Hide file tree

Showing 3 changed files with 4 additions and 7 deletions.
diff --git 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
@@ -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<std::mutex> 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
@@ -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<geometry_msgs::msg::TwistStamped>();
     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");
   }