made requested changes

RoboJackets · Feb 17, 2025 · 3639054 · 3639054
1 parent 75562a0
commit 3639054
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 13 deletions.
diff --git a/rj_gameplay/rj_gameplay.egg-info/PKG-INFO b/rj_gameplay/rj_gameplay.egg-info/PKG-INFO
@@ -1,4 +1,3 @@
-Metadata-Version: 2.1
 Name: rj-gameplay
 Version: 0.0.0
 Summary: Rewrite of the gameplay library.

diff --git a/soccer/src/soccer/control/motion_control.cpp b/soccer/src/soccer/control/motion_control.cpp
@@ -76,11 +76,11 @@ MotionControl::MotionControl(int shell_id, rclcpp::Node* node)
         });
 
     error_x_pub_ =
-        node->create_publisher<std_msgs::msg::Float64>("motion_control/pose_error_x", 10);
+        node->create_publisher<std_msgs::msg::Float64>("debug/motion_control/pose_error_x", 10);
     error_y_pub_ =
-        node->create_publisher<std_msgs::msg::Float64>("motion_control/pose_error_y", 10);
+        node->create_publisher<std_msgs::msg::Float64>("debug/motion_control/pose_error_y", 10);
     error_heading_pub_ =
-        node->create_publisher<std_msgs::msg::Float64>("motion_control/pose_error_heading", 10);
+        node->create_publisher<std_msgs::msg::Float64>("debug/motion_control/pose_error_heading", 10);
 }
 
 void MotionControl::run(const RobotState& state, const planning::Trajectory& trajectory,
@@ -133,17 +133,24 @@ void MotionControl::run(const RobotState& state, const planning::Trajectory& tra
         Pose error = maybe_pose_target.value() - state.pose;
         error.heading() = fix_angle_radians(error.heading());
 
-        std_msgs::msg::Float64 error_x_msg;
-        error_x_msg.data = error.position().x();
-        error_x_pub_->publish(error_x_msg);
+        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);
+        }
 
-        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);
+        }
 
-        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())),