PID Error Graphs

soccer/src/soccer/control/motion_control.cpp

@@ -74,6 +74,13 @@ MotionControl::MotionControl(int shell_id, rclcpp::Node* node)
         [this](PlayState::Msg::SharedPtr play_state_msg) {  // NOLINT
             play_state_ = rj_convert::convert_from_ros(*play_state_msg).state();
         });
+
+    error_x_pub_ =
+        node->create_publisher<std_msgs::msg::Float64>("debug/motion_control/pose_error_x", 10);
+    error_y_pub_ =
+        node->create_publisher<std_msgs::msg::Float64>("debug/motion_control/pose_error_y", 10);
+    error_heading_pub_ = node->create_publisher<std_msgs::msg::Float64>(
+        "debug/motion_control/pose_error_heading", 10);
 }
 
 void MotionControl::run(const RobotState& state, const planning::Trajectory& trajectory,
@@ -121,9 +128,29 @@ void MotionControl::run(const RobotState& state, const planning::Trajectory& tra
     // TODO(Kyle): Clamp acceleration
 
     Twist correction = Twist::zero();
+
     if (maybe_pose_target) {
         Pose error = maybe_pose_target.value() - state.pose;
         error.heading() = fix_angle_radians(error.heading());
+
+        if (error_x_pub_->get_subscription_count() > 0) {
+            std_msgs::msg::Float64 error_x_msg;
+            error_x_msg.data = error.position().x();
+            error_x_pub_->publish(error_x_msg);
+        }
+
+        if (error_y_pub_->get_subscription_count() > 0) {
+            std_msgs::msg::Float64 error_y_msg;
+            error_y_msg.data = error.position().y();
+            error_y_pub_->publish(error_y_msg);
+        }
+
+        if (error_heading_pub_->get_subscription_count() > 0) {
+            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<float>(error.position().x())),
                            position_y_controller_.run(static_cast<float>(error.position().y())),
                            angle_controller_.run(static_cast<float>(error.heading())));

soccer/src/soccer/control/motion_control.hpp

@@ -1,10 +1,13 @@
 #pragma once
 
+#include <rclcpp/rclcpp.hpp>
+
 #include <context.hpp>
 #include <rj_common/time.hpp>
 #include <rj_constants/topic_names.hpp>
 #include <rj_geometry/point.hpp>
 #include <rj_param_utils/param.hpp>
+#include <std_msgs/msg/float64.hpp>
 
 #include "control/motion_setpoint.hpp"
 #include "game_state.hpp"
@@ -95,6 +98,9 @@ class MotionControl {
     rclcpp::Subscription<PlayState::Msg>::SharedPtr play_state_sub_;
     rclcpp::Publisher<MotionSetpoint::Msg>::SharedPtr motion_setpoint_pub_;
     rclcpp::Publisher<RobotState::Msg>::SharedPtr target_state_pub_;
+    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr error_x_pub_;
+    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr error_y_pub_;
+    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr error_heading_pub_;
 };
 
 }  // namespace control