diff --git a/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp b/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp index b279e57f3a..985248fc51 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp @@ -67,6 +67,18 @@ class Controller const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Twist & cmd, bool is_docking, bool backward = false); + /** + * @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 computeRotateToHeadingCommand( + const double & angular_distance_to_heading, + const geometry_msgs::msg::Twist & current_velocity, + const double & dt); + protected: /** * @brief Check if a trajectory is collision free. @@ -103,18 +115,6 @@ class Controller rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; std::mutex dynamic_params_lock_; - /** - * @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 computeRotateToHeadingCommand( - const double & angular_distance_to_heading, - const geometry_msgs::msg::Twist & current_velocity, - const double & dt); - protected: rclcpp::Logger logger_{rclcpp::get_logger("Controller")}; rclcpp::Clock::SharedPtr clock_; diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index 2bab3407f4..e09221814b 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -783,7 +783,7 @@ DockingServer::dynamicParametersCallback(std::vector paramete undock_linear_tolerance_ = parameter.as_double(); } else if (name == "undock_angular_tolerance") { undock_angular_tolerance_ = parameter.as_double(); - else if (name == "backward_rotation_tolerance") { + } else if (name == "backward_rotation_tolerance") { backward_rotation_tolerance_ = parameter.as_double(); } } else if (type == ParameterType::PARAMETER_STRING) {