diff --git a/soccer/src/soccer/control/motion_control.cpp b/soccer/src/soccer/control/motion_control.cpp index 7a86486b9f..ceb241d54b 100644 --- a/soccer/src/soccer/control/motion_control.cpp +++ b/soccer/src/soccer/control/motion_control.cpp @@ -7,11 +7,9 @@ #include #include - #include "game_state.hpp" #include "planning/instant.hpp" - namespace control { using planning::RobotInstant; @@ -77,10 +75,12 @@ MotionControl::MotionControl(int shell_id, rclcpp::Node* node) play_state_ = rj_convert::convert_from_ros(*play_state_msg).state(); }); - error_x_pub_ = node->create_publisher("motion_control/pose_error_x", 10); - error_y_pub_ = node->create_publisher("motion_control/pose_error_y", 10); - error_heading_pub_ = node->create_publisher("motion_control/pose_error_heading", 10); - + error_x_pub_ = + node->create_publisher("motion_control/pose_error_x", 10); + error_y_pub_ = + node->create_publisher("motion_control/pose_error_y", 10); + error_heading_pub_ = + node->create_publisher("motion_control/pose_error_heading", 10); } void MotionControl::run(const RobotState& state, const planning::Trajectory& trajectory, @@ -144,7 +144,6 @@ void MotionControl::run(const RobotState& state, const planning::Trajectory& tra std_msgs::msg::Float64 error_heading_msg; error_heading_msg.data = error.heading(); error_heading_pub_->publish(error_heading_msg); - correction = Twist(position_x_controller_.run(static_cast(error.position().x())), position_y_controller_.run(static_cast(error.position().y())), diff --git a/soccer/src/soccer/control/motion_control.hpp b/soccer/src/soccer/control/motion_control.hpp index 53b15c335c..726feb3361 100644 --- a/soccer/src/soccer/control/motion_control.hpp +++ b/soccer/src/soccer/control/motion_control.hpp @@ -1,10 +1,13 @@ #pragma once +#include + #include #include #include #include #include +#include #include "control/motion_setpoint.hpp" #include "game_state.hpp" @@ -12,9 +15,6 @@ #include -#include -#include - namespace control { DECLARE_FLOAT64(params::kMotionControlParamModule, max_acceleration);