Added atan2 correction, removed initial_rotation parameter, added tes…

…ts for backward docking, fixed dynamic params

Signed-off-by: jakubach <[email protected]>

nav2_docking/README.md

@@ -203,13 +203,12 @@ For debugging purposes, there are several publishers which can be used with RVIZ
 | base_frame        | Robot's base frame for control law   | string |  "base_link"      |
 | fixed_frame        | Fixed frame to use, recommended to be a smooth odometry frame **not** map   | string |  "odom"      |
 | dock_backwards        | Whether the robot is docking with the dock forward or backward in motion | bool |  false      |
-| backward_blind        | Initial forward detections, then dock in reverse | bool |  false      |
+| backward_blind        | Initial forward detection, then dock in reverse | bool |  false      |
 | dock_prestaging_tolerance  |  L2 distance in X,Y,Theta from the staging pose to bypass navigation | double |  0.5      |
 | dock_plugins  | A set of dock plugins to load | vector<string> |  N/A      |
 | 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<string> | 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    |
 | 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  |

nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp

@@ -249,10 +249,6 @@ class DockingServer : public nav2_util::LifecycleNode
   bool dock_backwards_;
   // The tolerance to the dock's staging pose not requiring navigation
   double dock_prestaging_tolerance_;
-  // 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 backward_blind_;
 

nav2_docking/opennav_docking/src/docking_server.cpp

@@ -40,7 +40,6 @@ 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("backward_blind", false);
 }
 
@@ -61,16 +60,10 @@ 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("backward_blind", backward_blind_);
   if(backward_blind_ && !dock_backwards_) {
     RCLCPP_ERROR(get_logger(), "backward_blind is enabled when dock_backwards is disabled.");
     return nav2_util::CallbackReturn::FAILURE;
-  } else {
-    // If you have backward_blind and dock_backward then
-    // we know we need to do the initial rotation to go from forward to reverse
-    // before doing the rest of the procedure. The initial_rotation would thus always be true.
-    initial_rotation_ = true;
   }
   RCLCPP_INFO(get_logger(), "Controller frequency set to %.4fHz", controller_frequency_);
 
@@ -474,6 +467,17 @@ 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.
+    // (1 - 2 * dock_backwards_) converts `true` to -1 and `false` to 1
+    const double backward_projection = 0.25;
+    const double yaw = tf2::getYaw(target_pose.pose.orientation);
+    target_pose.pose.position.x += (1 - 2 * dock_backwards_) * cos(yaw) * backward_projection;
+    target_pose.pose.position.y += (1 - 2 * dock_backwards_) * sin(yaw) * backward_projection;
+    tf2_buffer_->transform(target_pose, target_pose, base_frame_);
+
     // Compute and publish controls
     auto command = std::make_unique<geometry_msgs::msg::TwistStamped>();
     command->header.stamp = now();
@@ -780,8 +784,6 @@ DockingServer::dynamicParametersCallback(std::vector<rclcpp::Parameter> paramete
       }
       if (name == "backward_blind") {
         backward_blind_ = parameter.as_bool();
-        initial_rotation_ = parameter.as_bool();
-        dock_backwards_ = parameter.as_bool();
       }
     }
   }

nav2_docking/opennav_docking/test/test_docking_server_unit.cpp

@@ -250,6 +250,8 @@ TEST(DockingServerTests, testDynamicParams)
       rclcpp::Parameter("undock_angular_tolerance", 0.125),
       rclcpp::Parameter("base_frame", std::string("hi")),
       rclcpp::Parameter("fixed_frame", std::string("hi")),
+      rclcpp::Parameter("backward_blind", true),
+      rclcpp::Parameter("dock_backwards", true),
       rclcpp::Parameter("max_retries", 7)});
 
   rclcpp::spin_until_future_complete(
@@ -264,6 +266,8 @@ TEST(DockingServerTests, testDynamicParams)
   EXPECT_EQ(node->get_parameter("base_frame").as_string(), std::string("hi"));
   EXPECT_EQ(node->get_parameter("fixed_frame").as_string(), std::string("hi"));
   EXPECT_EQ(node->get_parameter("max_retries").as_int(), 7);
+  EXPECT_EQ(node->get_parameter("backward_blind").as_bool(), true);
+  EXPECT_EQ(node->get_parameter("dock_backwards").as_bool(), true);
 
   node->on_deactivate(rclcpp_lifecycle::State());
   node->on_cleanup(rclcpp_lifecycle::State());