PID Error Graphs

rj_gameplay/rj_gameplay.egg-info/PKG-INFO

@@ -0,0 +1,11 @@
+Name: rj-gameplay
+Version: 0.0.0
+Summary: Rewrite of the gameplay library.
+Home-page: UNKNOWN
+Maintainer: oswinso
+Maintainer-email: [email protected]
+License: UNKNOWN
+Platform: UNKNOWN
+
+UNKNOWN
+

rj_gameplay/rj_gameplay.egg-info/SOURCES.txt

@@ -0,0 +1,90 @@
+README.md
+setup.cfg
+setup.py
+rj_gameplay/__init__.py
+rj_gameplay/gameplay_node.py
+rj_gameplay/play_selector.py
+rj_gameplay.egg-info/PKG-INFO
+rj_gameplay.egg-info/SOURCES.txt
+rj_gameplay.egg-info/dependency_links.txt
+rj_gameplay.egg-info/entry_points.txt
+rj_gameplay.egg-info/requires.txt
+rj_gameplay.egg-info/top_level.txt
+rj_gameplay.egg-info/zip-safe
+rj_gameplay/eval/__init__.py
+rj_gameplay/play/__init__.py
+rj_gameplay/play/defend_restart.py
+rj_gameplay/play/defense.py
+rj_gameplay/play/defensive_clear.py
+rj_gameplay/play/keepaway.py
+rj_gameplay/play/kickoff_play.py
+rj_gameplay/play/offense.py
+rj_gameplay/play/penalty_defense.py
+rj_gameplay/play/penalty_offense.py
+rj_gameplay/play/prep_penalty_offense.py
+rj_gameplay/play/restart.py
+rj_gameplay/play/test_motion_planning.py
+rj_gameplay/role/__init__.py
+rj_gameplay/role/capture_role.py
+rj_gameplay/role/dumb_move.py
+rj_gameplay/role/goalie_role.py
+rj_gameplay/role/marker.py
+rj_gameplay/role/passer.py
+rj_gameplay/role/receiver.py
+rj_gameplay/role/seeker.py
+rj_gameplay/role/striker.py
+rj_gameplay/situation/__init__.py
+rj_gameplay/situation/decision_tree/__init__.py
+rj_gameplay/situation/decision_tree/plays.py
+rj_gameplay/skill/__init__.py
+rj_gameplay/skill/capture.py
+rj_gameplay/skill/dribble.py
+rj_gameplay/skill/intercept.py
+rj_gameplay/skill/kick.py
+rj_gameplay/skill/line_kick.py
+rj_gameplay/skill/mark.py
+rj_gameplay/skill/move.py
+rj_gameplay/skill/pivot.py
+rj_gameplay/skill/pivot_kick.py
+rj_gameplay/skill/receive.py
+rj_gameplay/skill/settle.py
+rj_gameplay/tactic/__init__.py
+rj_gameplay/tactic/clear_tactic.py
+rj_gameplay/tactic/dumb_tactic.py
+rj_gameplay/tactic/goalie_tactic.py
+rj_gameplay/tactic/line_tactic.py
+rj_gameplay/tactic/move_tactic.py
+rj_gameplay/tactic/nmark_tactic.py
+rj_gameplay/tactic/pass_tactic.py
+rj_gameplay/tactic/prep_move.py
+rj_gameplay/tactic/seek.py
+rj_gameplay/tactic/striker_tactic.py
+rj_gameplay/tactic/wall_tactic.py
+stp/__init__.py
+stp/global_parameters.py
+stp/local_parameters.py
+stp/pylint_stp.py
+stp/rc.py
+stp/testing.py
+stp/action/__init__.py
+stp/formations/__init__.py
+stp/formations/diamond_formation.py
+stp/formations/x_formation.py
+stp/play/__init__.py
+stp/play/pure_play.py
+stp/role/__init__.py
+stp/role/constraint.py
+stp/role/cost.py
+stp/role/assignment/__init__.py
+stp/role/assignment/naive.py
+stp/situation/__init__.py
+stp/skill/__init__.py
+stp/skill/action_behavior.py
+stp/tactic/__init__.py
+stp/utils/__init__.py
+stp/utils/constants.py
+stp/utils/enum.py
+stp/utils/fsm.py
+stp/utils/pass_seeker_optimizer.py
+stp/utils/typed_key_dict.py
+stp/utils/world_state_converter.py

rj_gameplay/rj_gameplay.egg-info/dependency_links.txt

@@ -0,0 +1 @@
+

rj_gameplay/rj_gameplay.egg-info/entry_points.txt

@@ -0,0 +1,3 @@
+[console_scripts]
+gameplay_node = rj_gameplay.gameplay_node:main
+

rj_gameplay/rj_gameplay.egg-info/requires.txt

@@ -0,0 +1 @@
+setuptools

rj_gameplay/rj_gameplay.egg-info/top_level.txt

@@ -0,0 +1,2 @@
+rj_gameplay
+stp

rj_gameplay/rj_gameplay.egg-info/zip-safe

@@ -0,0 +1 @@
+

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