CI fixes

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

nav2_docking/opennav_docking/include/opennav_docking/controller.hpp

@@ -45,8 +45,8 @@ class Controller
    * @returns True if command is valid, false otherwise.
    */
   bool computeVelocityCommand(
-    const geometry_msgs::msg::Pose & pose,const geometry_msgs::msg::Pose & robot_pose, geometry_msgs::msg::Twist & cmd,
-    bool backward = false);
+    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

nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp

@@ -250,7 +250,9 @@ void DockingServer::rotateToDock(const geometry_msgs::msg::PoseStamped & dock_po
   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.
+  // 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/controller.cpp

@@ -61,8 +61,8 @@ Controller::Controller(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node)
 }
 
  bool Controller::computeVelocityCommand(
-    const geometry_msgs::msg::Pose & pose,const geometry_msgs::msg::Pose & robot_pose, geometry_msgs::msg::Twist & cmd,
-    bool backward)
+    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,robot_pose, backward);
@@ -117,10 +117,11 @@ geometry_msgs::msg::Twist Controller::rotateToTarget(const double & angle_to_tar
   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_);
+    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;
 }

nav2_docking/opennav_docking/src/docking_server.cpp

@@ -64,11 +64,13 @@ DockingServer::on_configure(const rclcpp_lifecycle::State & state)
   get_parameter("initial_rotation", initial_rotation_);
   get_parameter("backward_blind", backward_blind_);
   if(backward_blind_ && !dock_backwards_){
-    RCLCPP_ERROR(get_logger(), "Docking server configuration is invalid. backward_blind is enabled when dock_backwards is disabled.");
+    RCLCPP_ERROR(get_logger(), "Docking server configuration is invalid. 
+    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.
+  } 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_);
@@ -426,7 +428,9 @@ void DockingServer::rotateToDock(const geometry_msgs::msg::PoseStamped & dock_po
   geometry_msgs::msg::PoseStamped robot_pose = getRobotPoseInFrame(dock_pose.header.frame_id);
   double angle_to_goal;
   while(rclcpp::ok()){
-    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));
+    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.1){
       break;
     }