PID Error Graphs (#2328)

* initial commit * x works, but goes fast * pid errors publish * removing printouts * Fix Code Style On pid_error (#2329) automated style fixes Co-authored-by: Squid5678 <[email protected]> * made requested changes * Fix Code Style On pid_error (#2343) automated style fixes Co-authored-by: Squid5678 <[email protected]> * Update PKG-INFO * Delete rj_gameplay/rj_gameplay.egg-info/PKG-INFO * Delete rj_gameplay/rj_gameplay.egg-info/SOURCES.txt * Delete rj_gameplay/rj_gameplay.egg-info/dependency_links.txt * Delete rj_gameplay/rj_gameplay.egg-info/entry_points.txt * Delete rj_gameplay/rj_gameplay.egg-info/requires.txt * Delete rj_gameplay/rj_gameplay.egg-info/top_level.txt * Delete rj_gameplay/rj_gameplay.egg-info/zip-safe --------- Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> Co-authored-by: Squid5678 <[email protected]>
RoboJackets · Feb 17, 2025 · fc3ba75 · fc3ba75
1 parent 2f42c68
commit fc3ba75
Show file tree

Hide file tree

Showing 2 changed files with 33 additions and 0 deletions.
diff --git a/soccer/src/soccer/control/motion_control.cpp b/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())));

diff --git a/soccer/src/soccer/control/motion_control.hpp b/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