diff --git a/nav2_docking/opennav_docking/src/controller.cpp b/nav2_docking/opennav_docking/src/controller.cpp index 79a4f7ebe0..b851ba9807 100644 --- a/nav2_docking/opennav_docking/src/controller.cpp +++ b/nav2_docking/opennav_docking/src/controller.cpp @@ -60,10 +60,10 @@ Controller::Controller(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) std::bind(&Controller::dynamicParametersCallback, this, std::placeholders::_1)); } - bool Controller::computeVelocityCommand( - const geometry_msgs::msg::Pose & pose, - const geometry_msgs::msg::Pose & robot_pose, - geometry_msgs::msg::Twist & cmd, bool backward) +bool Controller::computeVelocityCommand( + 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); diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index 037a357312..134a9f04ca 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -481,7 +481,8 @@ bool DockingServer::approachDock(Dock * dock, geometry_msgs::msg::PoseStamped & command->header.stamp = now(); geometry_msgs::msg::PoseStamped robot_pose = getRobotPoseInFrame(dock_pose.header.frame_id); if (!controller_->computeVelocityCommand(target_pose.pose, robot_pose.pose, - command->twist, dock_backwards_)) { + command->twist, dock_backwards_)) + { throw opennav_docking_core::FailedToControl("Failed to get control"); } vel_publisher_->publish(std::move(command));