diff --git a/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp b/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp index 62c5c0247d..8f8d8ddb6e 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp +++ b/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 diff --git a/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp b/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp index 3fbd67937d..52882996d3 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp +++ b/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; diff --git a/nav2_docking/opennav_docking/src/controller.cpp b/nav2_docking/opennav_docking/src/controller.cpp index 1c9b657db8..6a58cab9b8 100644 --- a/nav2_docking/opennav_docking/src/controller.cpp +++ b/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 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; } diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index 0ab17d7342..2f1f9f1b0e 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/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; }