Initial rotation taken from rotation shim controller

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

nav2_docking/opennav_docking/include/opennav_docking/controller.hpp

@@ -60,17 +60,23 @@ class Controller
   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.
+   * @brief Perform a command for in-place rotation.
+   * @param angular_distance_to_heading Angular distance to goal
+   * @param current_velocity Current angular velocity
+   * @param dt Control loop duration [s]
+   * @returns TwistStamped command.
    */
-  geometry_msgs::msg::Twist rotateToTarget(const double & angle_to_target);
+  geometry_msgs::msg::Twist computeRotateToHeadingCommand(
+    const double & angular_distance_to_heading,
+    const geometry_msgs::msg::Twist & current_velocity,
+    const double & dt);
 
 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_;
+  double slowdown_radius_, v_linear_min_, v_linear_max_, v_angular_max_;
+  double rotate_to_heading_angular_vel_, rotate_to_heading_max_angular_accel_;
 };
 
 }  // namespace opennav_docking

nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp

@@ -33,6 +33,7 @@
 #include "opennav_docking/navigator.hpp"
 #include "opennav_docking_core/charging_dock.hpp"
 #include "tf2_ros/transform_listener.h"
+#include "nav_2d_utils/odom_subscriber.hpp"
 
 namespace opennav_docking
 {
@@ -251,11 +252,14 @@ class DockingServer : public nav2_util::LifecycleNode
   double dock_prestaging_tolerance_;
   // Enable aproaching a docking station only with initial detection without updates
   bool backward_blind_;
+  // Angle in which robot can stop initial rotation
+  double backward_rotation_tolerance_;
 
   // This is a class member so it can be accessed in publish feedback
   rclcpp::Time action_start_time_;
 
   std::unique_ptr<nav2_util::TwistPublisher> vel_publisher_;
+  std::unique_ptr<nav_2d_utils::OdomSubscriber> odom_sub_;
   std::unique_ptr<DockingActionServer> docking_action_server_;
   std::unique_ptr<UndockingActionServer> undocking_action_server_;
 

nav2_docking/opennav_docking/src/controller.cpp

@@ -48,7 +48,6 @@ 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>(
@@ -94,8 +93,6 @@ 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();
       }
@@ -111,20 +108,19 @@ Controller::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
   return result;
 }
 
-geometry_msgs::msg::Twist Controller::rotateToTarget(const double & angle_to_target)
+geometry_msgs::msg::Twist Controller::computeRotateToHeadingCommand(
+  const double & angular_distance_to_heading,
+  const geometry_msgs::msg::Twist & current_velocity,
+  const double & dt)
 {
-  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;
+  geometry_msgs::msg::Twist cmd_vel;
+  const double sign = angular_distance_to_heading > 0.0 ? 1.0 : -1.0;
+  const double angular_vel = sign * rotate_to_heading_angular_vel_;
+  const double min_feasible_angular_speed = current_velocity.angular.z - rotate_to_heading_max_angular_accel_ * dt;
+  const double max_feasible_angular_speed = current_velocity.angular.z + rotate_to_heading_max_angular_accel_ * dt;
+  cmd_vel.angular.z =
+    std::clamp(angular_vel, min_feasible_angular_speed, max_feasible_angular_speed);
+  return cmd_vel;
 }
 
-
 }  // namespace opennav_docking

nav2_docking/opennav_docking/src/docking_server.cpp

@@ -41,13 +41,16 @@ DockingServer::DockingServer(const rclcpp::NodeOptions & options)
   declare_parameter("dock_backwards", false);
   declare_parameter("dock_prestaging_tolerance", 0.5);
   declare_parameter("backward_blind", false);
+  declare_parameter("odom_topic", "odom");
+  declare_parameter("backward_rotation_tolerance", 0.02);
 }
 
 nav2_util::CallbackReturn
 DockingServer::on_configure(const rclcpp_lifecycle::State & state)
 {
   RCLCPP_INFO(get_logger(), "Configuring %s", get_name());
   auto node = shared_from_this();
+  std::string odom_topic;
 
   get_parameter("controller_frequency", controller_frequency_);
   get_parameter("initial_perception_timeout", initial_perception_timeout_);
@@ -61,14 +64,20 @@ DockingServer::on_configure(const rclcpp_lifecycle::State & state)
   get_parameter("dock_backwards", dock_backwards_);
   get_parameter("dock_prestaging_tolerance", dock_prestaging_tolerance_);
   get_parameter("backward_blind", backward_blind_);
+  get_parameter("odom_topic", odom_topic);
+  get_parameter("backward_rotation_tolerance",backward_rotation_tolerance_);
   if(backward_blind_ && !dock_backwards_) {
     RCLCPP_ERROR(get_logger(), "backward_blind is enabled when dock_backwards is disabled.");
     return nav2_util::CallbackReturn::FAILURE;
   }
+  if(odom_topic.empty()) {
+    odom_topic = "odom";
+  }
   RCLCPP_INFO(get_logger(), "Controller frequency set to %.4fHz", controller_frequency_);
 
   vel_publisher_ = std::make_unique<nav2_util::TwistPublisher>(node, "cmd_vel", 1);
   tf2_buffer_ = std::make_shared<tf2_ros::Buffer>(node->get_clock());
+  odom_sub_ = std::make_unique<nav_2d_utils::OdomSubscriber>(node, odom_topic);
 
   double action_server_result_timeout;
   nav2_util::declare_parameter_if_not_declared(
@@ -160,6 +169,7 @@ DockingServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
   curr_dock_type_.clear();
   controller_.reset();
   vel_publisher_.reset();
+  odom_sub_.reset();
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
@@ -415,18 +425,25 @@ void DockingServer::rotateToDock(const geometry_msgs::msg::PoseStamped & dock_po
 {
   rclcpp::Rate loop_rate(controller_frequency_);
   geometry_msgs::msg::PoseStamped robot_pose;
-  double angle_to_goal;
+  geometry_msgs::msg::PoseStamped target_pose = dock_pose;
   auto command = std::make_unique<geometry_msgs::msg::TwistStamped>();
+  auto current_vel = std::make_unique<geometry_msgs::msg::TwistStamped>();
+  double angular_distance_to_heading;
+  const double dt = 1.0 / controller_frequency_;
+  target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(
+    tf2::getYaw(target_pose.pose.orientation) + M_PI);
   while(rclcpp::ok()) {
     robot_pose = getRobotPoseInFrame(dock_pose.header.frame_id);
-    angle_to_goal = angles::shortest_angular_distance(tf2::getYaw(robot_pose.pose.orientation),
-    atan2(robot_pose.pose.position.y - dock_pose.pose.position.y,
-    robot_pose.pose.position.x - dock_pose.pose.position.x));
-    if(fabs(angle_to_goal) < 0.02) {
+    angular_distance_to_heading = angles::shortest_angular_distance(
+    tf2::getYaw(robot_pose.pose.orientation),
+    tf2::getYaw(target_pose.pose.orientation));
+    if(fabs(angular_distance_to_heading) < backward_rotation_tolerance_) {
       break;
     }
-    command->header.stamp = now();
-    command->twist = controller_->rotateToTarget(angle_to_goal);
+    current_vel->twist.angular.z = odom_sub_->getTwist().theta;
+    command->twist = controller_->computeRotateToHeadingCommand(angular_distance_to_heading,
+    current_vel->twist, dt);
+    command->header = robot_pose.header;
     vel_publisher_->publish(std::move(command));
     loop_rate.sleep();
   }