From 6cd44510a48f0459d34189cccd416f7af2aea349 Mon Sep 17 00:00:00 2001 From: Squid6578 Date: Sun, 26 Jan 2025 19:41:29 -0500 Subject: [PATCH 1/5] initial commit --- rj_gameplay/rj_gameplay.egg-info/PKG-INFO | 12 +++ rj_gameplay/rj_gameplay.egg-info/SOURCES.txt | 90 +++++++++++++++++++ .../rj_gameplay.egg-info/dependency_links.txt | 1 + .../rj_gameplay.egg-info/entry_points.txt | 3 + rj_gameplay/rj_gameplay.egg-info/requires.txt | 1 + .../rj_gameplay.egg-info/top_level.txt | 2 + rj_gameplay/rj_gameplay.egg-info/zip-safe | 1 + 7 files changed, 110 insertions(+) create mode 100644 rj_gameplay/rj_gameplay.egg-info/PKG-INFO create mode 100644 rj_gameplay/rj_gameplay.egg-info/SOURCES.txt create mode 100644 rj_gameplay/rj_gameplay.egg-info/dependency_links.txt create mode 100644 rj_gameplay/rj_gameplay.egg-info/entry_points.txt create mode 100644 rj_gameplay/rj_gameplay.egg-info/requires.txt create mode 100644 rj_gameplay/rj_gameplay.egg-info/top_level.txt create mode 100644 rj_gameplay/rj_gameplay.egg-info/zip-safe diff --git a/rj_gameplay/rj_gameplay.egg-info/PKG-INFO b/rj_gameplay/rj_gameplay.egg-info/PKG-INFO new file mode 100644 index 00000000000..85e86975ea7 --- /dev/null +++ b/rj_gameplay/rj_gameplay.egg-info/PKG-INFO @@ -0,0 +1,12 @@ +Metadata-Version: 2.1 +Name: rj-gameplay +Version: 0.0.0 +Summary: Rewrite of the gameplay library. +Home-page: UNKNOWN +Maintainer: oswinso +Maintainer-email: oswinso@gmail.com +License: UNKNOWN +Platform: UNKNOWN + +UNKNOWN + diff --git a/rj_gameplay/rj_gameplay.egg-info/SOURCES.txt b/rj_gameplay/rj_gameplay.egg-info/SOURCES.txt new file mode 100644 index 00000000000..83aa7c4406b --- /dev/null +++ b/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 \ No newline at end of file diff --git a/rj_gameplay/rj_gameplay.egg-info/dependency_links.txt b/rj_gameplay/rj_gameplay.egg-info/dependency_links.txt new file mode 100644 index 00000000000..8b137891791 --- /dev/null +++ b/rj_gameplay/rj_gameplay.egg-info/dependency_links.txt @@ -0,0 +1 @@ + diff --git a/rj_gameplay/rj_gameplay.egg-info/entry_points.txt b/rj_gameplay/rj_gameplay.egg-info/entry_points.txt new file mode 100644 index 00000000000..a641d186309 --- /dev/null +++ b/rj_gameplay/rj_gameplay.egg-info/entry_points.txt @@ -0,0 +1,3 @@ +[console_scripts] +gameplay_node = rj_gameplay.gameplay_node:main + diff --git a/rj_gameplay/rj_gameplay.egg-info/requires.txt b/rj_gameplay/rj_gameplay.egg-info/requires.txt new file mode 100644 index 00000000000..49fe098d9e6 --- /dev/null +++ b/rj_gameplay/rj_gameplay.egg-info/requires.txt @@ -0,0 +1 @@ +setuptools diff --git a/rj_gameplay/rj_gameplay.egg-info/top_level.txt b/rj_gameplay/rj_gameplay.egg-info/top_level.txt new file mode 100644 index 00000000000..9efb5b023a4 --- /dev/null +++ b/rj_gameplay/rj_gameplay.egg-info/top_level.txt @@ -0,0 +1,2 @@ +rj_gameplay +stp diff --git a/rj_gameplay/rj_gameplay.egg-info/zip-safe b/rj_gameplay/rj_gameplay.egg-info/zip-safe new file mode 100644 index 00000000000..8b137891791 --- /dev/null +++ b/rj_gameplay/rj_gameplay.egg-info/zip-safe @@ -0,0 +1 @@ + From 8c8c2c88d9909c92e27a81de78d501f69b9f3711 Mon Sep 17 00:00:00 2001 From: Squid6578 Date: Sun, 26 Jan 2025 21:13:40 -0500 Subject: [PATCH 2/5] x works, but goes fast --- soccer/src/soccer/control/motion_control.cpp | 14 ++++++++++++++ soccer/src/soccer/control/motion_control.hpp | 4 ++++ 2 files changed, 18 insertions(+) diff --git a/soccer/src/soccer/control/motion_control.cpp b/soccer/src/soccer/control/motion_control.cpp index 8df91f7c207..46e4778d0e3 100644 --- a/soccer/src/soccer/control/motion_control.cpp +++ b/soccer/src/soccer/control/motion_control.cpp @@ -7,9 +7,11 @@ #include #include + #include "game_state.hpp" #include "planning/instant.hpp" + namespace control { using planning::RobotInstant; @@ -74,6 +76,9 @@ 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("motion_control/pose_error_x", 10); + } void MotionControl::run(const RobotState& state, const planning::Trajectory& trajectory, @@ -121,9 +126,18 @@ 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()); + SPDLOG_INFO("pose error: {}", error.position().x()); + + std_msgs::msg::Float64 error_x_msg; + error_x_msg.data = error.position().x(); + error_x_pub_->publish(error_x_msg); + + + correction = Twist(position_x_controller_.run(static_cast(error.position().x())), position_y_controller_.run(static_cast(error.position().y())), angle_controller_.run(static_cast(error.heading()))); diff --git a/soccer/src/soccer/control/motion_control.hpp b/soccer/src/soccer/control/motion_control.hpp index 755c36f5b7a..eb5edd8d3de 100644 --- a/soccer/src/soccer/control/motion_control.hpp +++ b/soccer/src/soccer/control/motion_control.hpp @@ -12,6 +12,9 @@ #include +#include +#include + namespace control { DECLARE_FLOAT64(params::kMotionControlParamModule, max_acceleration); @@ -95,6 +98,7 @@ class MotionControl { rclcpp::Subscription::SharedPtr play_state_sub_; rclcpp::Publisher::SharedPtr motion_setpoint_pub_; rclcpp::Publisher::SharedPtr target_state_pub_; + rclcpp::Publisher::SharedPtr error_x_pub_; }; } // namespace control From 1adbd2e5abb7411ee03f10ce68593124880b80e5 Mon Sep 17 00:00:00 2001 From: Squid6578 Date: Tue, 28 Jan 2025 20:06:19 -0500 Subject: [PATCH 3/5] pid errors publish --- soccer/src/soccer/control/motion_control.cpp | 9 +++++++++ soccer/src/soccer/control/motion_control.hpp | 2 ++ 2 files changed, 11 insertions(+) diff --git a/soccer/src/soccer/control/motion_control.cpp b/soccer/src/soccer/control/motion_control.cpp index 46e4778d0e3..c68ed6e2b36 100644 --- a/soccer/src/soccer/control/motion_control.cpp +++ b/soccer/src/soccer/control/motion_control.cpp @@ -78,6 +78,8 @@ MotionControl::MotionControl(int shell_id, rclcpp::Node* node) }); 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); } @@ -136,6 +138,13 @@ void MotionControl::run(const RobotState& state, const planning::Trajectory& tra error_x_msg.data = error.position().x(); error_x_pub_->publish(error_x_msg); + 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_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())), diff --git a/soccer/src/soccer/control/motion_control.hpp b/soccer/src/soccer/control/motion_control.hpp index eb5edd8d3de..53b15c335cd 100644 --- a/soccer/src/soccer/control/motion_control.hpp +++ b/soccer/src/soccer/control/motion_control.hpp @@ -99,6 +99,8 @@ class MotionControl { rclcpp::Publisher::SharedPtr motion_setpoint_pub_; rclcpp::Publisher::SharedPtr target_state_pub_; rclcpp::Publisher::SharedPtr error_x_pub_; + rclcpp::Publisher::SharedPtr error_y_pub_; + rclcpp::Publisher::SharedPtr error_heading_pub_; }; } // namespace control From b7ed7afe1db9bb7a5f6e156ca339154012a00b8e Mon Sep 17 00:00:00 2001 From: Squid6578 Date: Tue, 28 Jan 2025 20:13:44 -0500 Subject: [PATCH 4/5] removing printouts --- soccer/src/soccer/control/motion_control.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/soccer/src/soccer/control/motion_control.cpp b/soccer/src/soccer/control/motion_control.cpp index c68ed6e2b36..7a86486b9fc 100644 --- a/soccer/src/soccer/control/motion_control.cpp +++ b/soccer/src/soccer/control/motion_control.cpp @@ -132,7 +132,6 @@ void MotionControl::run(const RobotState& state, const planning::Trajectory& tra if (maybe_pose_target) { Pose error = maybe_pose_target.value() - state.pose; error.heading() = fix_angle_radians(error.heading()); - SPDLOG_INFO("pose error: {}", error.position().x()); std_msgs::msg::Float64 error_x_msg; error_x_msg.data = error.position().x(); From 75562a0fc5a8c353056d7765f59d31ec02cf9b09 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Tue, 28 Jan 2025 21:05:00 -0500 Subject: [PATCH 5/5] Fix Code Style On pid_error (#2329) automated style fixes Co-authored-by: Squid5678 --- soccer/src/soccer/control/motion_control.cpp | 13 ++++++------- soccer/src/soccer/control/motion_control.hpp | 6 +++--- 2 files changed, 9 insertions(+), 10 deletions(-) diff --git a/soccer/src/soccer/control/motion_control.cpp b/soccer/src/soccer/control/motion_control.cpp index 7a86486b9fc..ceb241d54b1 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 53b15c335cd..726feb3361f 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);