diff --git a/launch/soccer.launch.py b/launch/soccer.launch.py index ff78f46e42d..c908a878999 100644 --- a/launch/soccer.launch.py +++ b/launch/soccer.launch.py @@ -182,14 +182,6 @@ def generate_launch_description(): ), # spawn internal_ref/external_ref based on internal_ref # LaunchArgument - Node( - condition=IfCondition(PythonExpression(["not ", use_manual_control])), - package="rj_robocup", - executable="coach_node", - output="screen", - parameters=[param_config_filepath], - on_exit=Shutdown(), - ), Node( condition=IfCondition(PythonExpression(["not ", use_manual_control])), package="rj_robocup", diff --git a/soccer/src/soccer/planning/planner_node.cpp b/soccer/src/soccer/planning/planner_node.cpp index 55adb2f21ef..f02271c50a6 100644 --- a/soccer/src/soccer/planning/planner_node.cpp +++ b/soccer/src/soccer/planning/planner_node.cpp @@ -244,9 +244,40 @@ 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 Overrides + switch (play_state.state()) { + case PlayState::State::Halt: + min_dist_from_ball = 0; + max_robot_speed = 0; + max_dribbler_speed = 0; + break; + case PlayState::State::Stop: + 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. + + 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 +428,6 @@ bool PlannerForRobot::is_done() const { return current_path_planner_->is_done(); } +// void PlannerForRobot::calcGlobalOverrides + } // namespace planning diff --git a/soccer/src/soccer/strategy/agent/agent_action_client.cpp b/soccer/src/soccer/strategy/agent/agent_action_client.cpp index 70a578d9176..4084ebfb7cd 100644 --- a/soccer/src/soccer/strategy/agent/agent_action_client.cpp +++ b/soccer/src/soccer/strategy/agent/agent_action_client.cpp @@ -24,6 +24,10 @@ AgentActionClient::AgentActionClient(int r_id) ::vision_filter::topics::kWorldStateTopic, 1, [this](rj_msgs::msg::WorldState::SharedPtr msg) { world_state_callback(msg); }); + play_state_sub_ = 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); }); @@ -77,6 +81,14 @@ 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) { + if (current_position_ == nullptr) { + return; + } + + current_position_->update_play_state(*msg); +} + void AgentActionClient::field_dimensions_callback( const rj_msgs::msg::FieldDimensions::SharedPtr& msg) { if (current_position_ == nullptr) { diff --git a/soccer/src/soccer/strategy/agent/agent_action_client.hpp b/soccer/src/soccer/strategy/agent/agent_action_client.hpp index 2dae9edc111..9165df54b8e 100644 --- a/soccer/src/soccer/strategy/agent/agent_action_client.hpp +++ b/soccer/src/soccer/strategy/agent/agent_action_client.hpp @@ -53,6 +53,8 @@ class AgentActionClient : public rclcpp::Node { private: // ROS pub/subs rclcpp::Subscription::SharedPtr world_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_; rclcpp::Subscription::SharedPtr game_settings_sub_; @@ -60,6 +62,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 field_dimensions_callback(const rj_msgs::msg::FieldDimensions::SharedPtr& msg); void alive_robots_callback(const rj_msgs::msg::AliveRobots::SharedPtr& msg); void game_settings_callback(const rj_msgs::msg::GameSettings::SharedPtr& msg); diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 87cfa486bfe..78a80aeac57 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -8,7 +8,9 @@ Offense::Offense(int r_id) : Position(r_id) { } std::optional Offense::derived_get_task(RobotIntent intent) { + SPDLOG_INFO("MY ID: {} in offense derived task!\n", robot_id_); current_state_ = update_state(); + SPDLOG_INFO("My current offense state is {}", current_state_); return state_to_task(intent); } diff --git a/soccer/src/soccer/strategy/agent/position/position.cpp b/soccer/src/soccer/strategy/agent/position/position.cpp index 93732aa956d..60bf120cfb4 100644 --- a/soccer/src/soccer/strategy/agent/position/position.cpp +++ b/soccer/src/soccer/strategy/agent/position/position.cpp @@ -46,6 +46,12 @@ bool Position::check_goal_canceled() { return false; } +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); +} + void Position::update_alive_robots(std::vector alive_robots) { alive_robots_ = alive_robots; diff --git a/soccer/src/soccer/strategy/agent/position/position.hpp b/soccer/src/soccer/strategy/agent/position/position.hpp index 360fdd56a60..d2715aa583c 100644 --- a/soccer/src/soccer/strategy/agent/position/position.hpp +++ b/soccer/src/soccer/strategy/agent/position/position.hpp @@ -67,9 +67,11 @@ class Position { * https://www.sandordargo.com/blog/2022/08/24/tmp-and-nvi */ - std::optional get_task(WorldState& world_state, FieldDimensions& field_dimensions); + virtual std::optional get_task(WorldState& world_state, FieldDimensions& field_dimensions); // communication with AC + 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(); @@ -218,6 +220,7 @@ 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; diff --git a/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp b/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp index 08778d8af85..6bf591bb6a7 100644 --- a/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp +++ b/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp @@ -14,9 +14,21 @@ if (robot_id_ == 0) { } +std::optional RobotFactoryPosition::get_task(WorldState& ws, FieldDimensions& fd) { + return current_position_->get_task(ws, fd); +} + std::optional RobotFactoryPosition::derived_get_task(RobotIntent intent) { - // This is where the current_position can be reassigned based on the PlayState - return current_position_->get_task(*last_world_state_, field_dimensions_); + // This is where the current_position can be reassigned based on the + // PlayState + + + SPDLOG_ERROR("This should not be called"); + return std::nullopt; + + // no longer called + + // return current_position_->get_task(*last_world_state_, field_dimensions_); } void RobotFactoryPosition::receive_communication_response(communication::AgentPosResponseWrapper response) { diff --git a/soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp b/soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp index fcc855b7e78..2cb299ac2b9 100644 --- a/soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp +++ b/soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp @@ -37,6 +37,8 @@ class RobotFactoryPosition : public Position { RobotFactoryPosition(int r_id); ~RobotFactoryPosition() override = default; + std::optional get_task(WorldState&, FieldDimensions&) override; + void receive_communication_response(communication::AgentPosResponseWrapper response) override; communication::PosAgentResponseWrapper receive_communication_request( communication::AgentPosRequestWrapper request) override;