From f8a3e42ebe864098f13658700a584e0972fc4ecd Mon Sep 17 00:00:00 2001 From: Sid Parikh Date: Sun, 2 Jul 2023 16:29:17 -0400 Subject: [PATCH 01/10] delete manipulator control node --- soccer/src/soccer/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/soccer/src/soccer/CMakeLists.txt b/soccer/src/soccer/CMakeLists.txt index 2b7206a7d7e..813a63ad3ee 100644 --- a/soccer/src/soccer/CMakeLists.txt +++ b/soccer/src/soccer/CMakeLists.txt @@ -15,7 +15,6 @@ set(ROBOCUP_LIB_SRC global_params.cpp logger.cpp robot_intent.cpp - control/manipulator_control.cpp control/motion_control.cpp control/motion_control_node.cpp control/trapezoidal_motion.cpp From fd8dcf909f4c5070724a2d9feb9005eb4c65faa1 Mon Sep 17 00:00:00 2001 From: Sid Parikh Date: Sun, 2 Jul 2023 16:37:01 -0400 Subject: [PATCH 02/10] delete node --- .../soccer/control/manipulator_control.cpp | 37 ------------------- .../soccer/control/manipulator_control.hpp | 33 ----------------- 2 files changed, 70 deletions(-) delete mode 100644 soccer/src/soccer/control/manipulator_control.cpp delete mode 100644 soccer/src/soccer/control/manipulator_control.hpp diff --git a/soccer/src/soccer/control/manipulator_control.cpp b/soccer/src/soccer/control/manipulator_control.cpp deleted file mode 100644 index 2dec082be4c..00000000000 --- a/soccer/src/soccer/control/manipulator_control.cpp +++ /dev/null @@ -1,37 +0,0 @@ -#include "manipulator_control.hpp" - -#include - -#include "global_params.hpp" -#include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" - -namespace control { - -using soccer::robot::PARAM_max_chip_speed; -using soccer::robot::PARAM_max_kick_speed; -using soccer::robot::PARAM_min_safe_kick_power; - -ManipulatorControl::ManipulatorControl(int shell_id, rclcpp::Node* node) : shell_id_(shell_id) { - auto manipulator_pub = node->create_publisher( - topics::manipulator_setpoint_topic(shell_id), rclcpp::QoS(10)); - - manipulator_pub_ = manipulator_pub; - coach_state_sub_ = node->create_subscription( - "/strategy/coach_state", rclcpp::QoS(1), - [this](rj_msgs::msg::CoachState::SharedPtr coach_state) { // NOLINT - last_coach_state_ = *coach_state; - dribbler_speed_ = last_coach_state_.global_override.max_dribbler_speed; - }); - - intent_sub_ = node->create_subscription( - gameplay::topics::robot_intent_topic(shell_id), rclcpp::QoS(1), - [manipulator_pub, this](rj_msgs::msg::RobotIntent::SharedPtr intent) { // NOLINT - manipulator_pub->publish(rj_msgs::build() - .shoot_mode(intent->shoot_mode) - .trigger_mode(intent->trigger_mode) - .kick_speed(intent->kick_speed) - .dribbler_speed(dribbler_speed_)); - }); -} - -} // namespace control \ No newline at end of file diff --git a/soccer/src/soccer/control/manipulator_control.hpp b/soccer/src/soccer/control/manipulator_control.hpp deleted file mode 100644 index dd37f0318e1..00000000000 --- a/soccer/src/soccer/control/manipulator_control.hpp +++ /dev/null @@ -1,33 +0,0 @@ -#pragma once - -#include - -#include -#include -#include -#include -#include - -namespace control { - -class ManipulatorControl { -public: - ManipulatorControl(int shell_id, rclcpp::Node* node); - ~ManipulatorControl() = default; - - ManipulatorControl(ManipulatorControl&& other) = default; - ManipulatorControl& operator=(ManipulatorControl&& other) = default; - ManipulatorControl(const ManipulatorControl& other) = delete; - ManipulatorControl& operator=(const ManipulatorControl& other) = delete; - -private: - int shell_id_; - rclcpp::Publisher::SharedPtr manipulator_pub_; - rclcpp::Subscription::SharedPtr intent_sub_; - rclcpp::Subscription::SharedPtr coach_state_sub_; - - rj_msgs::msg::CoachState last_coach_state_{}; - double dribbler_speed_ = 0; -}; - -} // namespace control \ No newline at end of file From c3fc0703e9587f4bc27c4cfd837a31299341758d Mon Sep 17 00:00:00 2001 From: Sid Parikh Date: Sun, 2 Jul 2023 16:38:10 -0400 Subject: [PATCH 03/10] delete maniuplators vector --- soccer/src/soccer/control/motion_control_node.cpp | 1 - soccer/src/soccer/control/motion_control_node.hpp | 1 - 2 files changed, 2 deletions(-) diff --git a/soccer/src/soccer/control/motion_control_node.cpp b/soccer/src/soccer/control/motion_control_node.cpp index 271ae6fb885..21578173aab 100644 --- a/soccer/src/soccer/control/motion_control_node.cpp +++ b/soccer/src/soccer/control/motion_control_node.cpp @@ -13,7 +13,6 @@ MotionControlNode::MotionControlNode() viz::topics::kDebugDrawTopic, rclcpp::QoS(10)); for (int i = 0; i < kNumShells; i++) { controllers_.emplace_back(i, this); - manipulators_.emplace_back(i, this); } } diff --git a/soccer/src/soccer/control/motion_control_node.hpp b/soccer/src/soccer/control/motion_control_node.hpp index a16cf6f3fa3..b904852fe7c 100644 --- a/soccer/src/soccer/control/motion_control_node.hpp +++ b/soccer/src/soccer/control/motion_control_node.hpp @@ -25,7 +25,6 @@ class MotionControlNode : public rclcpp::Node { private: ::params::LocalROS2ParamProvider param_provider_; std::vector controllers_{}; - std::vector manipulators_{}; }; } // namespace control \ No newline at end of file From bdcfa8fafac338c13e32cc31c420af236e41963c Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Mon, 14 Aug 2023 20:53:38 -0400 Subject: [PATCH 04/10] Fix Code Style On delete_maninpulator_node (#2084) automated style fixes Co-authored-by: sid-parikh --- rj_gameplay/stp/rc.py | 6 ++---- util/run-clang-tidy.py | 6 ++---- 2 files changed, 4 insertions(+), 8 deletions(-) diff --git a/rj_gameplay/stp/rc.py b/rj_gameplay/stp/rc.py index 3d911c510ed..7b0aaae001d 100644 --- a/rj_gameplay/stp/rc.py +++ b/rj_gameplay/stp/rc.py @@ -184,10 +184,8 @@ def lethal_fault(self) -> bool: """ if not self.is_ours: warnings.warn( - ( - "Attempting to retrieve lethal fault information from an opposing" - " robot" - ), + "Attempting to retrieve lethal fault information from an opposing" + " robot", RuntimeWarning, ) diff --git a/util/run-clang-tidy.py b/util/run-clang-tidy.py index 2e78ae61c6e..c306f551614 100644 --- a/util/run-clang-tidy.py +++ b/util/run-clang-tidy.py @@ -152,10 +152,8 @@ def check_clang_apply_replacements_binary(args): subprocess.check_call([args.clang_apply_replacements_binary, "--version"]) except: print( - ( - "Unable to run clang-apply-replacements. Is clang-apply-replacements " - "binary correctly specified?" - ), + "Unable to run clang-apply-replacements. Is clang-apply-replacements " + "binary correctly specified?", file=sys.stderr, ) traceback.print_exc() From fb613c5f1b9ba2e7b7cdcc2ef35aa8b6daffdc01 Mon Sep 17 00:00:00 2001 From: Robojackets Date: Tue, 12 Sep 2023 19:34:00 -0400 Subject: [PATCH 05/10] Remove mention to removed filed --- soccer/src/soccer/control/motion_control_node.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/soccer/src/soccer/control/motion_control_node.hpp b/soccer/src/soccer/control/motion_control_node.hpp index b904852fe7c..a1ce5eb8492 100644 --- a/soccer/src/soccer/control/motion_control_node.hpp +++ b/soccer/src/soccer/control/motion_control_node.hpp @@ -7,7 +7,6 @@ #include #include -#include "manipulator_control.hpp" #include "motion_control.hpp" #include "motion_setpoint.hpp" #include "node.hpp" From 3ecd267672e2a262c8763f83179a0331154cebe1 Mon Sep 17 00:00:00 2001 From: Ninjatest321 Date: Tue, 26 Sep 2023 20:06:23 -0400 Subject: [PATCH 06/10] Add Referee/PlayState to files using CoachState --- soccer/src/soccer/strategy/agent/agent_action_client.cpp | 8 ++++++++ soccer/src/soccer/strategy/agent/agent_action_client.hpp | 2 ++ 2 files changed, 10 insertions(+) diff --git a/soccer/src/soccer/strategy/agent/agent_action_client.cpp b/soccer/src/soccer/strategy/agent/agent_action_client.cpp index a8288568615..f1745323cf9 100644 --- a/soccer/src/soccer/strategy/agent/agent_action_client.cpp +++ b/soccer/src/soccer/strategy/agent/agent_action_client.cpp @@ -28,6 +28,10 @@ AgentActionClient::AgentActionClient(int r_id) topics::kCoachStateTopic, 1, [this](rj_msgs::msg::CoachState::SharedPtr msg) { coach_state_callback(msg); }); + play_state_sub_ = this->create_subscription( + ::referee::topics::kPlayStateTopic, 10, + [this](const rj_msgs::msg::PlayState::SharedPtr msg) { play_state_callback(msg); }); + field_dimensions_sub_ = create_subscription( "config/field_dimensions", 1, [this](rj_msgs::msg::FieldDimensions::SharedPtr msg) { field_dimensions_callback(msg); }); @@ -95,6 +99,10 @@ void AgentActionClient::world_state_callback(const rj_msgs::msg::WorldState::Sha last_world_state_ = std::move(world_state); } +void AgentActionClient::play_state_callback(const rj_msgs::msg::PlayState::SharedPtr& msg) { + return; +} + void AgentActionClient::coach_state_callback(const rj_msgs::msg::CoachState::SharedPtr& msg) { if (current_position_ == nullptr) { return; diff --git a/soccer/src/soccer/strategy/agent/agent_action_client.hpp b/soccer/src/soccer/strategy/agent/agent_action_client.hpp index 7ea3769660f..7bbe71c7edd 100644 --- a/soccer/src/soccer/strategy/agent/agent_action_client.hpp +++ b/soccer/src/soccer/strategy/agent/agent_action_client.hpp @@ -56,6 +56,7 @@ class AgentActionClient : public rclcpp::Node { // ROS pub/subs rclcpp::Subscription::SharedPtr world_state_sub_; rclcpp::Subscription::SharedPtr coach_state_sub_; + rclcpp::Subscription::SharedPtr play_state_sub_; rclcpp::Subscription::SharedPtr positions_sub_; rclcpp::Subscription::SharedPtr field_dimensions_sub_; rclcpp::Subscription::SharedPtr alive_robots_sub_; @@ -64,6 +65,7 @@ class AgentActionClient : public rclcpp::Node { // callbacks for subs void world_state_callback(const rj_msgs::msg::WorldState::SharedPtr& msg); + void play_state_callback(const rj_msgs::msg::PlayState::SharedPtr& msg); void coach_state_callback(const rj_msgs::msg::CoachState::SharedPtr& msg); void field_dimensions_callback(const rj_msgs::msg::FieldDimensions::SharedPtr& msg); void alive_robots_callback(const rj_msgs::msg::AliveRobots::SharedPtr& msg); From ec56dc2e80b9cb68343383be0a4abe3a34bc4e12 Mon Sep 17 00:00:00 2001 From: Ninjatest321 Date: Tue, 3 Oct 2023 20:08:07 -0400 Subject: [PATCH 07/10] Forward PlayState to Position --- .../soccer/strategy/agent/agent_action_client.cpp | 14 +++++++++----- .../soccer/strategy/agent/position/position.cpp | 4 ++++ .../soccer/strategy/agent/position/position.hpp | 4 ++++ 3 files changed, 17 insertions(+), 5 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/agent_action_client.cpp b/soccer/src/soccer/strategy/agent/agent_action_client.cpp index f1745323cf9..b6c39e3ea74 100644 --- a/soccer/src/soccer/strategy/agent/agent_action_client.cpp +++ b/soccer/src/soccer/strategy/agent/agent_action_client.cpp @@ -100,15 +100,19 @@ void AgentActionClient::world_state_callback(const rj_msgs::msg::WorldState::Sha } void AgentActionClient::play_state_callback(const rj_msgs::msg::PlayState::SharedPtr& msg) { - return; -} - -void AgentActionClient::coach_state_callback(const rj_msgs::msg::CoachState::SharedPtr& msg) { if (current_position_ == nullptr) { return; } - current_position_->update_coach_state(*msg); + current_position_->update_play_state(*msg); +} + +void AgentActionClient::coach_state_callback(const rj_msgs::msg::CoachState::SharedPtr& msg) { + // if (current_position_ == nullptr) { + // return; + // } + + // current_position_->update_coach_state(*msg); } void AgentActionClient::field_dimensions_callback( diff --git a/soccer/src/soccer/strategy/agent/position/position.cpp b/soccer/src/soccer/strategy/agent/position/position.cpp index 8da5f16d369..05b0510486f 100644 --- a/soccer/src/soccer/strategy/agent/position/position.cpp +++ b/soccer/src/soccer/strategy/agent/position/position.cpp @@ -56,6 +56,10 @@ void Position::update_coach_state(rj_msgs::msg::CoachState msg) { * match_state_, our_possession_); */ } +void Position::update_play_state(rj_msgs::msg::PlayState msg) { + current_play_state_ = msg; +} + void Position::update_field_dimensions(FieldDimensions field_dims) { field_dimensions_ = std::move(field_dims); } diff --git a/soccer/src/soccer/strategy/agent/position/position.hpp b/soccer/src/soccer/strategy/agent/position/position.hpp index 96c53aefc87..60b8e638f8d 100644 --- a/soccer/src/soccer/strategy/agent/position/position.hpp +++ b/soccer/src/soccer/strategy/agent/position/position.hpp @@ -71,6 +71,7 @@ class Position { // communication with AC void update_world_state(WorldState world_state); void update_coach_state(rj_msgs::msg::CoachState coach_state); + void update_play_state(rj_msgs::msg::PlayState msg); void update_field_dimensions(FieldDimensions field_dimensions); void update_alive_robots(std::vector alive_robots); const std::string get_name(); @@ -220,6 +221,9 @@ class Position { // (if so match world_state below) bool our_possession_{}; rj_msgs::msg::GlobalOverride global_override_{}; + rj_msgs::msg::PlayState current_play_state_; + + FieldDimensions field_dimensions_ = FieldDimensions::kDefaultDimensions; From 8ed637259c79f4f437f28940386d2ddd308e5142 Mon Sep 17 00:00:00 2001 From: Ninjatest321 Date: Tue, 3 Oct 2023 20:10:28 -0400 Subject: [PATCH 08/10] Formatting --- soccer/src/soccer/strategy/agent/position/position.cpp | 4 +--- soccer/src/soccer/strategy/agent/position/position.hpp | 2 -- 2 files changed, 1 insertion(+), 5 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/position.cpp b/soccer/src/soccer/strategy/agent/position/position.cpp index 05b0510486f..8da74e7a783 100644 --- a/soccer/src/soccer/strategy/agent/position/position.cpp +++ b/soccer/src/soccer/strategy/agent/position/position.cpp @@ -56,9 +56,7 @@ void Position::update_coach_state(rj_msgs::msg::CoachState msg) { * match_state_, our_possession_); */ } -void Position::update_play_state(rj_msgs::msg::PlayState msg) { - current_play_state_ = msg; -} +void Position::update_play_state(rj_msgs::msg::PlayState msg) { current_play_state_ = msg; } void Position::update_field_dimensions(FieldDimensions field_dims) { field_dimensions_ = std::move(field_dims); diff --git a/soccer/src/soccer/strategy/agent/position/position.hpp b/soccer/src/soccer/strategy/agent/position/position.hpp index 60b8e638f8d..c2b720a5e12 100644 --- a/soccer/src/soccer/strategy/agent/position/position.hpp +++ b/soccer/src/soccer/strategy/agent/position/position.hpp @@ -223,8 +223,6 @@ class Position { rj_msgs::msg::GlobalOverride global_override_{}; rj_msgs::msg::PlayState current_play_state_; - - FieldDimensions field_dimensions_ = FieldDimensions::kDefaultDimensions; /* From 1dfa12d518d6e4f65739f800a0e01285d2eb7f4a Mon Sep 17 00:00:00 2001 From: Ninjatest321 Date: Tue, 17 Oct 2023 20:08:09 -0400 Subject: [PATCH 09/10] Calculate Global Overrides in Planner Node --- soccer/src/soccer/planning/planner_node.cpp | 51 +++++++++++++++++++-- 1 file changed, 48 insertions(+), 3 deletions(-) diff --git a/soccer/src/soccer/planning/planner_node.cpp b/soccer/src/soccer/planning/planner_node.cpp index 55adb2f21ef..276985a4775 100644 --- a/soccer/src/soccer/planning/planner_node.cpp +++ b/soccer/src/soccer/planning/planner_node.cpp @@ -244,9 +244,52 @@ PlanRequest PlannerForRobot::make_request(const RobotIntent& intent) { const auto* world_state = global_state_.world_state(); const auto goalie_id = global_state_.goalie_id(); const auto play_state = global_state_.play_state(); - const auto min_dist_from_ball = global_state_.coach_state().global_override.min_dist_from_ball; - const auto max_robot_speed = global_state_.coach_state().global_override.max_speed; - const auto max_dribbler_speed = global_state_.coach_state().global_override.max_dribbler_speed; + + float min_dist_from_ball; + float max_robot_speed; + float max_dribbler_speed; + + // Global Override code + // rj_msgs::msg::GlobalOverride global_override; + switch (play_state.state()) { + case PlayState::State::Halt: + // global_override.max_speed = 0; + // global_override.min_dist_from_ball = 0; + // global_override.max_dribbler_speed = 0; + + min_dist_from_ball = 0; + max_robot_speed = 0; + max_dribbler_speed = 0; + break; + case PlayState::State::Stop: + // global_override.max_speed = 1.5; + // global_override.min_dist_from_ball = 0.5; + // global_override.max_dribbler_speed = 0; + + min_dist_from_ball = 0.5; + max_robot_speed = 1.5; + max_dribbler_speed = 0; + break; + case PlayState::State::Playing: + default: + // Unbounded speed. Setting to -1 or 0 crashes planner, so use large number + // instead. + // global_override.max_speed = 10.0; + // global_override.min_dist_from_ball = 0; + // global_override.max_dribbler_speed = 255; + + min_dist_from_ball = 0; + max_robot_speed = 10.0; + max_dribbler_speed = 255; + break; + } + + // publish new necessary information + + // const auto min_dist_from_ball = global_override.min_dist_from_ball; + // const auto max_robot_speed = global_override.max_speed; + // const auto max_dribbler_speed = global_override.max_dribbler_speed; + const auto& robot = world_state->our_robots.at(robot_id_); const auto start = RobotInstant{robot.pose, robot.velocity, robot.timestamp}; @@ -397,4 +440,6 @@ bool PlannerForRobot::is_done() const { return current_path_planner_->is_done(); } +// void PlannerForRobot::calcGlobalOverrides + } // namespace planning From 1a5c8df794c241d8c99e668ce630330768f48dba Mon Sep 17 00:00:00 2001 From: Ninjatest321 Date: Sun, 29 Oct 2023 21:16:08 -0400 Subject: [PATCH 10/10] Fix Warnings --- soccer/src/soccer/planning/planner_node.cpp | 6 +++--- soccer/src/soccer/strategy/agent/agent_action_client.cpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/soccer/src/soccer/planning/planner_node.cpp b/soccer/src/soccer/planning/planner_node.cpp index 276985a4775..a6050ada40f 100644 --- a/soccer/src/soccer/planning/planner_node.cpp +++ b/soccer/src/soccer/planning/planner_node.cpp @@ -245,9 +245,9 @@ PlanRequest PlannerForRobot::make_request(const RobotIntent& intent) { const auto goalie_id = global_state_.goalie_id(); const auto play_state = global_state_.play_state(); - float min_dist_from_ball; - float max_robot_speed; - float max_dribbler_speed; + float min_dist_from_ball{}; + float max_robot_speed{}; + float max_dribbler_speed{}; // Global Override code // rj_msgs::msg::GlobalOverride global_override; diff --git a/soccer/src/soccer/strategy/agent/agent_action_client.cpp b/soccer/src/soccer/strategy/agent/agent_action_client.cpp index b6c39e3ea74..9ea4231b2c7 100644 --- a/soccer/src/soccer/strategy/agent/agent_action_client.cpp +++ b/soccer/src/soccer/strategy/agent/agent_action_client.cpp @@ -28,7 +28,7 @@ AgentActionClient::AgentActionClient(int r_id) topics::kCoachStateTopic, 1, [this](rj_msgs::msg::CoachState::SharedPtr msg) { coach_state_callback(msg); }); - play_state_sub_ = this->create_subscription( + play_state_sub_ = create_subscription( ::referee::topics::kPlayStateTopic, 10, [this](const rj_msgs::msg::PlayState::SharedPtr msg) { play_state_callback(msg); });