Correction of merge

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

nav2_docking/opennav_docking/include/opennav_docking/controller.hpp

@@ -45,9 +45,8 @@ 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
@@ -69,6 +68,7 @@ class Controller
 
 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_,v_angular_min_;
 };

nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp

@@ -248,6 +248,8 @@ class DockingServer : public nav2_util::LifecycleNode
   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_;
+  // Enable aproaching a docking station only with initial detection without updates
+  bool dock_backwards_without_sensor_;
 
   // 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

@@ -51,12 +51,13 @@ Controller::Controller(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node)
   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_);
+    k_phi_, k_delta_, beta_, lambda_, slowdown_radius_, v_linear_min_, v_linear_max_,
+    v_angular_max_);
+
+  // Add callback for dynamic parameters
   dyn_params_handler_ = node->add_on_set_parameters_callback(
     std::bind(&Controller::dynamicParametersCallback, this, std::placeholders::_1));
-
 }
 
  bool Controller::computeVelocityCommand(
@@ -72,10 +73,12 @@ 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();
@@ -96,12 +99,14 @@ Controller::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
       } 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;
 }

nav2_docking/opennav_docking/src/docking_server.cpp

@@ -41,7 +41,8 @@ DockingServer::DockingServer(const rclcpp::NodeOptions & options)
   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);
+  declare_parameter("initial_rotation_min_angle", 0.5);
+  declare_parameter("dock_backwards_without_sensor", true);
 }
 
 nav2_util::CallbackReturn
@@ -63,6 +64,7 @@ DockingServer::on_configure(const rclcpp_lifecycle::State & state)
   get_parameter("dock_prestaging_tolerance", dock_prestaging_tolerance_);
   get_parameter("initial_rotation", initial_rotation_);
   get_parameter("initial_rotation_min_angle", initial_rotation_min_angle_);
+  get_parameter("dock_backwards_without_sensor", dock_backwards_without_sensor_);
   RCLCPP_INFO(get_logger(), "Controller frequency set to %.4fHz", controller_frequency_);
 
   vel_publisher_ = std::make_unique<nav2_util::TwistPublisher>(node, "cmd_vel", 1);
@@ -389,6 +391,17 @@ void DockingServer::doInitialPerception(Dock * dock, geometry_msgs::msg::PoseSta
   publishDockingFeedback(DockRobot::Feedback::INITIAL_PERCEPTION);
   rclcpp::Rate loop_rate(controller_frequency_);
   auto start = this->now();
+  // If docking without sensors, stop the robot for short time to get a stable and quality detection of docking pose
+  if(dock_backwards_without_sensor_){
+      publishZeroVelocity();
+      while (rclcpp::ok()) {
+        if (this->now() - start > rclcpp::Duration::from_seconds(1.0)) {
+          break;
+        }
+      }
+  }
+
+  start = this->now();
   auto timeout = rclcpp::Duration::from_seconds(initial_perception_timeout_);
   while (!dock->plugin->getRefinedPose(dock_pose, dock->id)) {
     if (this->now() - start > timeout) {
@@ -426,7 +439,7 @@ bool DockingServer::approachDock(Dock * dock, geometry_msgs::msg::PoseStamped &
     }
 
     // Update perception
-    if (!dock->plugin->getRefinedPose(dock_pose, dock->id)) {
+    if (!dock_backwards_without_sensor_ && !dock->plugin->getRefinedPose(dock_pose, dock->id)) {
       throw opennav_docking_core::FailedToDetectDock("Failed dock detection");
     }
 
@@ -442,16 +455,16 @@ bool DockingServer::approachDock(Dock * dock, geometry_msgs::msg::PoseStamped &
     }
 
     // Compute and publish controls
-    geometry_msgs::msg::Twist command;
+    auto command = std::make_unique<geometry_msgs::msg::TwistStamped>();
 
     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>();
-    command->header.stamp = now();
-    if (!controller_->computeVelocityCommand(target_pose.pose, command->twist, dock_backwards_)) {
+    if(initial_rotation_ && fabs(angle_to_goal) > initial_rotation_min_angle_){
+      command->twist = controller_->rotateToTarget(angle_to_goal);
+    }
+    else if (!controller_->computeVelocityCommand(target_pose.pose, robot_pose.pose, command->twist, dock_backwards_)) {
       throw opennav_docking_core::FailedToControl("Failed to get control");
     }
     vel_publisher_->publish(std::move(command));
@@ -751,6 +764,9 @@ DockingServer::dynamicParametersCallback(std::vector<rclcpp::Parameter> paramete
       if (name == "initial_rotation") {
         initial_rotation_ = parameter.as_bool();
       }
+      if (name == "dock_backwards_without_sensor") {
+        dock_backwards_without_sensor_ = parameter.as_bool();
+      }
     }
   }