Fixed issue with backward docking and added initial in-place rotation

README.md

@@ -207,14 +207,17 @@ For debugging purposes, there are several publishers which can be used with RVIZ
 | 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 | ""     |
-| controller.k_phi  | TODO | double | 3.0  |
-| controller.k_delta  |  TODO | double | 2.0     |
-| controller.beta  |  TODO | double | 0.4  |
-| controller.lambda  |  TODO | double | 2.0     |
-| controller.v_linear_min  |  TODO | double | 0.1     |
-| controller.v_linear_max |  TODO | double | 0.25    |
-| controller.v_angular_max |  TODO | double | 0.75    |
-| controller.slowdown_radius |  TODO | double | 0.25     |
+| 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    |
+| initial_rotation_min_angle|  The difference in the path orientation and the starting robot orientation to trigger a rotate in place, if initial_rotation is enabled. | double | 0.3     |
+| 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  |
+| controller.lambda  |  Parameter to reduce linear velocity proportional to path curvature. Increasing this exponentially reduces the velocity (v(t) = v_max / (1 + beta * |curv|^lambda)). | double | 2.0     |
+| controller.v_linear_min  |  Minimum velocity for approaching dock. | double | 0.1     |
+| controller.v_linear_max |  Maximum velocity for approaching dock. | double | 0.25    |
+| controller.v_angular_min |  Minimum angular velocity for approaching dock. | double | 0.15    |
+| controller.v_angular_max |  Maximum angular velocity for approaching dock. | double | 0.75    |
+| controller.slowdown_radius |  Radius to end goal to commense slow down. | double | 0.25     |
 
 Note: `dock_plugins` and either `docks` or `dock_database` are required.
 

opennav_docking/CMakeLists.txt

@@ -137,4 +137,4 @@ endif()
 ament_export_include_directories(include)
 ament_export_libraries(${library_name} controller pose_filter)
 ament_export_dependencies(${dependencies})
-ament_package()
+ament_package()

opennav_docking/include/opennav_docking/controller.hpp

@@ -16,12 +16,12 @@
 #define OPENNAV_DOCKING__CONTROLLER_HPP_
 
 #include <memory>
-#include <vector>
 
 #include "geometry_msgs/msg/pose.hpp"
 #include "geometry_msgs/msg/twist.hpp"
 #include "nav2_graceful_controller/smooth_control_law.hpp"
 #include "nav2_util/lifecycle_node.hpp"
+#include <vector>
 
 namespace opennav_docking
 {
@@ -45,25 +45,31 @@ 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
    * @param event ParameterEvent message
    */
   rcl_interfaces::msg::SetParametersResult
   dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
-
   // Dynamic parameters handler
   rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
   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.
+   */
+  geometry_msgs::msg::Twist rotateToTarget(const double & angle_to_target);
+
 protected:
   std::unique_ptr<nav2_graceful_controller::SmoothControlLaw> control_law_;
-
   double k_phi_, k_delta_, beta_, lambda_;
-  double slowdown_radius_, v_linear_min_, v_linear_max_, v_angular_max_;
+  double slowdown_radius_, v_linear_min_, v_linear_max_, v_angular_max_,v_angular_min_;
 };
 
 }  // namespace opennav_docking

opennav_docking/include/opennav_docking/docking_server.hpp

@@ -242,6 +242,10 @@ class DockingServer : public nav2_util::LifecycleNode
   bool dock_backwards_;
   // The tolerance to the dock's staging pose not requiring navigation
   double dock_prestaging_tolerance_;
+  // The difference in the path orientation and the starting robot orientation to trigger a rotate in place, if initial_rotation is enabled.
+  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_;
 
   // This is a class member so it can be accessed in publish feedback
   rclcpp::Time action_start_time_;

opennav_docking/src/controller.cpp

@@ -26,7 +26,7 @@ Controller::Controller(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node)
   nav2_util::declare_parameter_if_not_declared(
     node, "controller.k_phi", rclcpp::ParameterValue(2.0));
   nav2_util::declare_parameter_if_not_declared(
-    node, "controller.k_delta", rclcpp::ParameterValue(1.0));
+    node, "controller.k_delta", rclcpp::ParameterValue(2.0));
   nav2_util::declare_parameter_if_not_declared(
     node, "controller.beta", rclcpp::ParameterValue(0.4));
   nav2_util::declare_parameter_if_not_declared(
@@ -35,6 +35,8 @@ Controller::Controller(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node)
     node, "controller.v_linear_min", rclcpp::ParameterValue(0.1));
   nav2_util::declare_parameter_if_not_declared(
     node, "controller.v_linear_max", rclcpp::ParameterValue(0.25));
+  nav2_util::declare_parameter_if_not_declared(
+    node, "controller.v_angular_min", rclcpp::ParameterValue(0.15));
   nav2_util::declare_parameter_if_not_declared(
     node, "controller.v_angular_max", rclcpp::ParameterValue(0.75));
   nav2_util::declare_parameter_if_not_declared(
@@ -46,35 +48,34 @@ 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<nav2_graceful_controller::SmoothControlLaw>(
-    k_phi_, k_delta_, beta_, lambda_, slowdown_radius_, v_linear_min_, v_linear_max_,
-    v_angular_max_);
 
-  // Add callback for dynamic parameters
+  control_law_ = std::make_unique<nav2_graceful_controller::SmoothControlLaw>(
+    k_phi_, k_delta_, beta_, lambda_, slowdown_radius_, v_linear_min_, v_linear_max_, v_angular_max_);
   dyn_params_handler_ = node->add_on_set_parameters_callback(
     std::bind(&Controller::dynamicParametersCallback, this, std::placeholders::_1));
+
 }
 
-bool Controller::computeVelocityCommand(
-  const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Twist & cmd, bool backward)
+ 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, backward);
+  cmd = control_law_->calculateRegularVelocity(pose,robot_pose, backward);
   return true;
 }
 
 rcl_interfaces::msg::SetParametersResult
 Controller::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
 {
   std::lock_guard<std::mutex> 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();
@@ -90,19 +91,34 @@ Controller::dynamicParametersCallback(std::vector<rclcpp::Parameter> 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();
       }
-
       // 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;
 }
 
+geometry_msgs::msg::Twist Controller::rotateToTarget(const double & angle_to_target)
+{
+  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;
+}
+
+
 }  // namespace opennav_docking

opennav_docking/src/docking_server.cpp

@@ -40,6 +40,8 @@ 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("initial_rotation_min_angle", 0.3);
 }
 
 nav2_util::CallbackReturn
@@ -59,6 +61,8 @@ 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("initial_rotation_min_angle", initial_rotation_min_angle_);
   RCLCPP_INFO(get_logger(), "Controller frequency set to %.4fHz", controller_frequency_);
 
   vel_publisher_ = create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 1);
@@ -416,21 +420,20 @@ 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.
-    const double backward_projection = 0.25;
-    const double yaw = tf2::getYaw(target_pose.pose.orientation);
-    target_pose.pose.position.x += cos(yaw) * backward_projection;
-    target_pose.pose.position.y += sin(yaw) * backward_projection;
-    tf2_buffer_->transform(target_pose, target_pose, base_frame_);
-
     // Compute and publish controls
     geometry_msgs::msg::Twist command;
-    if (!controller_->computeVelocityCommand(target_pose.pose, command, dock_backwards_)) {
+
+    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));
+
+    if(initial_rotation_ && fabs(angle_to_goal) > initial_rotation_min_angle_){
+      command = controller_->rotateToTarget(angle_to_goal);
+    }
+    else if (!controller_->computeVelocityCommand(target_pose.pose, robot_pose.pose, command, dock_backwards_)) {
       throw opennav_docking_core::FailedToControl("Failed to get control");
     }
+
     vel_publisher_->publish(command);
 
     if (this->now() - start > timeout) {
@@ -529,7 +532,7 @@ bool DockingServer::getCommandToPose(
   tf2_buffer_->transform(target_pose, target_pose, base_frame_);
 
   // Compute velocity command
-  if (!controller_->computeVelocityCommand(target_pose.pose, cmd, backward)) {
+  if (!controller_->computeVelocityCommand(target_pose.pose,robot_pose.pose, cmd, backward)) {
     throw opennav_docking_core::FailedToControl("Failed to get control");
   }
 
@@ -696,6 +699,8 @@ DockingServer::dynamicParametersCallback(std::vector<rclcpp::Parameter> paramete
         undock_linear_tolerance_ = parameter.as_double();
       } else if (name == "undock_angular_tolerance") {
         undock_angular_tolerance_ = parameter.as_double();
+      } else if (name == "initial_rotation_min_angle") {
+        initial_rotation_min_angle_ = parameter.as_double();
       }
     } else if (type == ParameterType::PARAMETER_STRING) {
       if (name == "base_frame") {
@@ -707,6 +712,10 @@ DockingServer::dynamicParametersCallback(std::vector<rclcpp::Parameter> paramete
       if (name == "max_retries") {
         max_retries_ = parameter.as_int();
       }
+    } else if(type == ParameterType::PARAMETER_BOOL){
+      if (name == "initial_rotation") {
+        initial_rotation_ = parameter.as_bool();
+      }
     }
   }
 

opennav_docking/test/test_controller.cpp

@@ -39,21 +39,19 @@ TEST(ControllerTests, ObjectLifecycle)
   auto controller = std::make_unique<opennav_docking::Controller>(node);
 
   geometry_msgs::msg::Pose pose;
+  geometry_msgs::msg::Pose robot_pose;
   geometry_msgs::msg::Twist cmd_out, cmd_init;
-  EXPECT_TRUE(controller->computeVelocityCommand(pose, cmd_out));
+  EXPECT_TRUE(controller->computeVelocityCommand(pose,robot_pose, cmd_out));
   EXPECT_NE(cmd_init, cmd_out);
   controller.reset();
 }
-
 TEST(ControllerTests, DynamicParameters) {
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
   auto controller = std::make_shared<opennav_docking::Controller>(node);
-
   auto params = std::make_shared<rclcpp::AsyncParametersClient>(
     node->get_node_base_interface(), node->get_node_topics_interface(),
     node->get_node_graph_interface(),
     node->get_node_services_interface());
-
   // Set parameters
   auto results = params->set_parameters_atomically(
     {rclcpp::Parameter("controller.k_phi", 1.0),
@@ -63,11 +61,10 @@ TEST(ControllerTests, DynamicParameters) {
       rclcpp::Parameter("controller.v_linear_min", 5.0),
       rclcpp::Parameter("controller.v_linear_max", 6.0),
       rclcpp::Parameter("controller.v_angular_max", 7.0),
+      rclcpp::Parameter("controller.v_angular_min", 2.0),
       rclcpp::Parameter("controller.slowdown_radius", 8.0)});
-
   // Spin
   rclcpp::spin_until_future_complete(node->get_node_base_interface(), results);
-
   // Check parameters
   EXPECT_EQ(node->get_parameter("controller.k_phi").as_double(), 1.0);
   EXPECT_EQ(node->get_parameter("controller.k_delta").as_double(), 2.0);
@@ -76,6 +73,7 @@ TEST(ControllerTests, DynamicParameters) {
   EXPECT_EQ(node->get_parameter("controller.v_linear_min").as_double(), 5.0);
   EXPECT_EQ(node->get_parameter("controller.v_linear_max").as_double(), 6.0);
   EXPECT_EQ(node->get_parameter("controller.v_angular_max").as_double(), 7.0);
+  EXPECT_EQ(node->get_parameter("controller.v_angular_min").as_double(), 2.0);
   EXPECT_EQ(node->get_parameter("controller.slowdown_radius").as_double(), 8.0);
 }