diff --git a/soccer/src/soccer/planning/planner_node.cpp b/soccer/src/soccer/planning/planner_node.cpp index 55adb2f21ef..a6050ada40f 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 diff --git a/soccer/src/soccer/strategy/agent/agent_action_client.cpp b/soccer/src/soccer/strategy/agent/agent_action_client.cpp index a8288568615..9ea4231b2c7 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_ = 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,12 +99,20 @@ void AgentActionClient::world_state_callback(const rj_msgs::msg::WorldState::Sha last_world_state_ = std::move(world_state); } -void AgentActionClient::coach_state_callback(const rj_msgs::msg::CoachState::SharedPtr& msg) { +void AgentActionClient::play_state_callback(const rj_msgs::msg::PlayState::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/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); diff --git a/soccer/src/soccer/strategy/agent/position/position.cpp b/soccer/src/soccer/strategy/agent/position/position.cpp index 8da5f16d369..8da74e7a783 100644 --- a/soccer/src/soccer/strategy/agent/position/position.cpp +++ b/soccer/src/soccer/strategy/agent/position/position.cpp @@ -56,6 +56,8 @@ 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..c2b720a5e12 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,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;