Added an option for docking backward without back sensors and fixed b…

…ackward docking issues

Signed-off-by: Jakubach <jakubach@gmail.com>

nav2_docking/opennav_docking/include/opennav_docking/controller.hpp

@@ -47,6 +47,7 @@ class Controller
   bool computeVelocityCommand(
     const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Twist & cmd,
     bool backward = false);
+
 
   /**
    * @brief Callback executed when a parameter change is detected
@@ -59,11 +60,17 @@ class Controller
   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

nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp

@@ -244,6 +244,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_;

nav2_docking/opennav_docking/src/controller.cpp

@@ -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

nav2_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_ = std::make_unique<nav2_util::TwistPublisher>(node, "cmd_vel", 1);
@@ -437,15 +441,12 @@ 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;
+
+    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<geometry_msgs::msg::TwistStamped>();
@@ -557,7 +558,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");
   }
 
@@ -733,6 +734,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") {
@@ -744,6 +747,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();
+      }
     }
   }
 

nav2_docking/opennav_docking/test/test_controller.cpp

@@ -39,8 +39,9 @@ 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();
 }
@@ -63,6 +64,7 @@ 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
@@ -76,6 +78,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);
 }