From 9af76124ef3d7d298fa15ba5573bd3a573faf363 Mon Sep 17 00:00:00 2001 From: Sid Parikh Date: Tue, 30 Jan 2024 22:06:17 -0500 Subject: [PATCH 01/22] WIP offense rewrite --- .../strategy/agent/agent_action_client.cpp | 18 +- .../strategy/agent/agent_action_client.hpp | 4 + .../strategy/agent/position/offense.cpp | 509 +++++------------- .../strategy/agent/position/offense.hpp | 115 ++-- .../strategy/agent/position/position.cpp | 4 + .../strategy/agent/position/position.hpp | 11 + 6 files changed, 214 insertions(+), 447 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/agent_action_client.cpp b/soccer/src/soccer/strategy/agent/agent_action_client.cpp index 5592ff8f0b5..22413ca24b6 100644 --- a/soccer/src/soccer/strategy/agent/agent_action_client.cpp +++ b/soccer/src/soccer/strategy/agent/agent_action_client.cpp @@ -41,6 +41,10 @@ AgentActionClient::AgentActionClient(int r_id) "config/game_settings", 1, [this](rj_msgs::msg::GameSettings::SharedPtr msg) { game_settings_callback(msg); }); + goalie_id_sub_ = create_subscription( + ::referee::topics::kGoalieTopic, 1, + [this](rj_msgs::msg::Goalie::SharedPtr msg) { goalie_id_callback(msg->goalie_id); }); + robot_communication_srv_ = create_service( fmt::format("agent_{}_incoming", r_id), [this](const std::shared_ptr request, @@ -67,7 +71,6 @@ AgentActionClient::AgentActionClient(int r_id) get_communication(); check_communication_timeout(); }); - } void AgentActionClient::world_state_callback(const rj_msgs::msg::WorldState::SharedPtr& msg) { @@ -103,6 +106,14 @@ void AgentActionClient::field_dimensions_callback( current_position_->update_field_dimensions(field_dimensions); } +void AgentActionClient::goalie_id_callback(int goalie_id) { + if (current_position_) { + current_position_->set_goalie_id(goalie_id); + } + + goalie_id_ = goalie_id; +} + void AgentActionClient::alive_robots_callback(const rj_msgs::msg::AliveRobots::SharedPtr& msg) { alive_robots_ = msg->alive_robots; } @@ -176,7 +187,6 @@ void AgentActionClient::send_new_goal() { void AgentActionClient::goal_response_callback( std::shared_future future) { - auto goal_handle = future.get(); if (!goal_handle) { current_position_->set_goal_canceled(); @@ -185,7 +195,6 @@ void AgentActionClient::goal_response_callback( void AgentActionClient::feedback_callback( GoalHandleRobotMove::SharedPtr, const std::shared_ptr feedback) { - double time_left = rj_convert::convert_from_ros(feedback->time_left).count(); if (current_position_ == nullptr) { current_position_->set_time_left(time_left); @@ -193,7 +202,6 @@ void AgentActionClient::feedback_callback( } void AgentActionClient::result_callback(const GoalHandleRobotMove::WrappedResult& result) { - switch (result.code) { case rclcpp_action::ResultCode::SUCCEEDED: // TODO: handle other return codes @@ -351,7 +359,6 @@ void AgentActionClient::receive_response_callback( } void AgentActionClient::check_communication_timeout() { - for (u_int32_t i = 0; i < buffered_responses_.size(); i++) { if (RJ::now() - buffered_responses_[i].created > timeout_duration_) { current_position_->receive_communication_response(buffered_responses_[i]); @@ -360,5 +367,4 @@ void AgentActionClient::check_communication_timeout() { } } - } // namespace strategy diff --git a/soccer/src/soccer/strategy/agent/agent_action_client.hpp b/soccer/src/soccer/strategy/agent/agent_action_client.hpp index ca2a5094fe2..5b1f346cbf9 100644 --- a/soccer/src/soccer/strategy/agent/agent_action_client.hpp +++ b/soccer/src/soccer/strategy/agent/agent_action_client.hpp @@ -15,6 +15,7 @@ #include #include #include +#include #include "game_state.hpp" #include "rclcpp/rclcpp.hpp" @@ -54,6 +55,7 @@ class AgentActionClient : public rclcpp::Node { rclcpp::Subscription::SharedPtr field_dimensions_sub_; rclcpp::Subscription::SharedPtr alive_robots_sub_; rclcpp::Subscription::SharedPtr game_settings_sub_; + rclcpp::Subscription::SharedPtr goalie_id_sub_; // TODO(Kevin): communication module pub/sub here (e.g. passing) // callbacks for subs @@ -62,6 +64,7 @@ class AgentActionClient : public rclcpp::Node { 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); + void goalie_id_callback(int goalie_id); std::unique_ptr current_position_; @@ -129,6 +132,7 @@ class AgentActionClient : public rclcpp::Node { std::vector alive_robots_ = {}; bool is_simulated_ = false; static constexpr double field_padding_ = 0.3; + int goalie_id_; /** * @brief checks whether a robot is visible, in the field, and (if the game is not diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 3c1a6ef680c..c03767c333c 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -2,289 +2,161 @@ namespace strategy { -Offense::Offense(int r_id) : Position(r_id) { - position_name_ = "Offense"; - current_state_ = IDLING; -} +Offense::Offense(int r_id) : Position(r_id) { position_name_ = "Offense"; } 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_); + State next_state = update_state(); + + if (current_state_ != next_state) { + reset_timeout(); + } + + current_state_ = next_state; + return state_to_task(intent); } Offense::State Offense::update_state() { - State next_state = current_state_; // handle transitions between current state WorldState* world_state = this->last_world_state_; - // if no ball found, stop and return to box immediately - if (!world_state->ball.visible) { - return current_state_; - } - rj_geometry::Point robot_position = world_state->get_robot(true, robot_id_).pose.position(); rj_geometry::Point ball_position = world_state->ball.position; double distance_to_ball = robot_position.dist_to(ball_position); - if (current_state_ == IDLING) { - // send_scorer_request(); - next_state = SEEKING; - } else if (current_state_ == SEARCHING) { - if (scorer_) { - next_state = STEALING; - } - } else if (current_state_ == PASSING) { - // transition to idling if we no longer have the ball (i.e. it was passed or it was - // stolen) - if (check_is_done()) { - next_state = IDLING; - } + switch (current_state_) { - if (distance_to_ball > ball_lost_distance_) { - next_state = IDLING; - } - } else if (current_state_ == PREPARING_SHOT) { - if (check_is_done()) { - next_state = SHOOTING; - } - } else if (current_state_ == SHOOTING) { - // transition to idling if we no longer have the ball (i.e. it was passed or it was - // stolen) - if (check_is_done() || distance_to_ball > ball_lost_distance_) { - send_reset_scorer_request(); - next_state = SEARCHING; - } - } else if (current_state_ == RECEIVING) { - // transition to idling if we are close enough to the ball - if (distance_to_ball < 2 * ball_receive_distance_) { - next_state = IDLING; - } - } else if (current_state_ == STEALING) { - // The collect planner check_is_done() is wonky so I added a second clause to check - // distance - if (check_is_done() || distance_to_ball < ball_receive_distance_) { - // send direct pass request to robot 4 - if (scorer_) { - next_state = PREPARING_SHOT; - } else { - /* send_direct_pass_request({4}); */ - /* next_state = SEARCHING; */ + case DEFAULT: + + return SEEKING; + break; + + case SEEKING: + + // Seek + // Stay until moved to receiving or stealing + + // TODO: should_steal() tomatic switch to passing if a pass is accepted. + broadcast_direct_pass_request(); + + return POSSESSION; + + case POSSESSION: + + // If we can make a shot, make it. + // If we need to stop possessing now, shoot. + if (has_open_shot() || timed_out()) { + return SHOOTING; } - } - } else if (current_state_ == FACING) { - if (check_is_done()) { - next_state = IDLING; - } - } else if (current_state_ == AWAITING_SEND_PASS) { - if (distance_to_ball < ball_lost_distance_) { - Position::broadcast_direct_pass_request(); - } - } else if (current_state_ == SEEKING) { - // if the ball comes close to it while it's trying to seek, it should no longer be trying to - // seek - if (distance_to_ball < ball_receive_distance_) { - // next_state = RECEIVING; - } - } - return SHOOTING; + return POSSESSION; + + case PASSING: + + // If we've finished passing, cool! + if (check_is_done()) { + return DEFAULT; + } + + // If we didn't successfully pass in time, take a shot + if (timed_out()) { + return SHOOTING; + } + + // If we lost the ball completely, give up + if (ball_position.dist_to(robot_position) > kBallTooFarDist) { + return DEFAULT; + } + + + case STEALING: + + // Go to possession if successful + if (check_is_done()) { + return POSSESSION; + } + + // If we ran out of time or the ball is out of our radius, give up + if (timed_out() || ball_position.dist_to(robot_position) > kStealBallRadius) { + return DEFAULT; + } + + case RECEIVING: + + // If we got it, cool, we have it! + if (check_is_done()) { + return POSSESSION; + } + + // If we failed to get it in time + if (timed_out()) { + return DEFAULT; + } + + case SHOOTING: + + // If we either succeed or fail, it's time to start over. + if (check_is_done() || timed_out()) { + return DEFAULT; + } + + return DEFAULT; + } } std::optional Offense::state_to_task(RobotIntent intent) { - float dist{0.0f}; - // SPDLOG_INFO(current_state_); - if (current_state_ == IDLING) { - // Do nothing - auto empty_motion_cmd = planning::MotionCommand{}; - intent.motion_command = empty_motion_cmd; - return intent; - } else if (current_state_ == SEARCHING) { - // DEFINE SEARCHING BEHAVIOR - auto empty_motion_cmd = planning::MotionCommand{}; - intent.motion_command = empty_motion_cmd; - return intent; - } else if (current_state_ == PASSING) { - target_robot_id = 2; - rj_geometry::Point target_robot_pos = - last_world_state_->get_robot(true, target_robot_id).pose.position(); - rj_geometry::Point this_robot_pos = - last_world_state_->get_robot(true, this->robot_id_).pose.position(); - planning::LinearMotionInstant target{target_robot_pos}; - auto line_kick_cmd = planning::MotionCommand{"line_kick", target}; - intent.motion_command = line_kick_cmd; - intent.shoot_mode = RobotIntent::ShootMode::KICK; - // NOTE: Check we can actually use break beams - intent.trigger_mode = RobotIntent::TriggerMode::ON_BREAK_BEAM; - // Adjusts kick speed based on distance. Refer to - // TIGERS Mannheim eTDP from 2019 for details - // See also passer.py in rj_gameplay - dist = target_robot_pos.dist_to(this_robot_pos); - intent.kick_speed = std::sqrt((std::pow(kFinalBallSpeed, 2)) - (2 * kBallDecel * dist)); - intent.is_active = true; - return intent; - } else if (current_state_ == PREPARING_SHOT) { - // pivot around ball... - auto ball_pt = last_world_state_->ball.position; - - // ...to face their goal - rj_geometry::Point their_goal_pos = field_dimensions_.their_goal_loc(); - planning::LinearMotionInstant target_instant{their_goal_pos}; - - auto pivot_cmd = planning::MotionCommand{"pivot"}; - pivot_cmd.target = target_instant; - pivot_cmd.pivot_point = ball_pt; - intent.motion_command = pivot_cmd; - intent.dribbler_speed = 255.0; - return intent; - } else if (current_state_ == SHOOTING) { - planning::LinearMotionInstant target{calculate_best_shot()}; - auto line_kick_cmd = planning::MotionCommand{"line_kick", target}; - intent.motion_command = line_kick_cmd; - intent.shoot_mode = RobotIntent::ShootMode::KICK; - intent.trigger_mode = RobotIntent::TriggerMode::ON_BREAK_BEAM; - intent.kick_speed = 4.0; - intent.is_active = true; - - // intent.motion_command = planning::MotionCommand{ - // "path_target", planning::LinearMotionInstant{last_world_state_->ball.position, - // {0.0}}, planning::FaceBall{}}; - - return intent; - } else if (current_state_ == RECEIVING) { - // check how far we are from the ball - rj_geometry::Point robot_position = - last_world_state_->get_robot(true, robot_id_).pose.position(); - rj_geometry::Point ball_position = last_world_state_->ball.position; - double distance_to_ball = robot_position.dist_to(ball_position); - if (distance_to_ball > max_receive_distance && !chasing_ball) { - auto motion_instance = - planning::LinearMotionInstant{robot_position, rj_geometry::Point{0.0, 0.0}}; - auto face_ball = planning::FaceBall{}; - auto face_ball_cmd = planning::MotionCommand{"path_target", motion_instance, face_ball}; - intent.motion_command = face_ball_cmd; - } else { - // intercept the bal - chasing_ball = true; - auto collect_cmd = planning::MotionCommand{"collect"}; - intent.motion_command = collect_cmd; - } - return intent; - } else if (current_state_ == STEALING) { - // intercept the ball - // if ball fast, use settle, otherwise collect - if (last_world_state_->ball.velocity.mag() > 0.75) { - auto settle_cmd = planning::MotionCommand{"settle"}; - intent.motion_command = settle_cmd; - intent.dribbler_speed = 255.0; - return intent; - } else { - auto collect_cmd = planning::MotionCommand{"collect"}; - intent.motion_command = collect_cmd; - intent.dribbler_speed = 255.0; + + switch (current_state_) { + + case DEFAULT: + // Do nothing: empty motion command + intent.motion_command = planning::MotionCommand{}; return intent; - } - } else if (current_state_ == FACING) { - rj_geometry::Point robot_position = - last_world_state_->get_robot(true, robot_id_).pose.position(); - auto current_location_instant = - planning::LinearMotionInstant{robot_position, rj_geometry::Point{0.0, 0.0}}; - auto face_ball = planning::FaceBall{}; - auto face_ball_cmd = - planning::MotionCommand{"path_target", current_location_instant, face_ball}; - intent.motion_command = face_ball_cmd; - return intent; - } else if (current_state_ == AWAITING_SEND_PASS) { - auto empty_motion_cmd = planning::MotionCommand{}; - intent.motion_command = empty_motion_cmd; - return intent; - } else if (current_state_ == SEEKING) { - // Only get a new target position if we have reached our target - if (check_is_done() || - last_world_state_->get_robot(true, robot_id_).velocity.linear().mag() <= 0.01) { - Seeker seeker{robot_id_}; - return seeker.get_task(intent, last_world_state_, this->field_dimensions_); - } - } + + case SEEKING: - // should be impossible to reach, but this is an EmptyMotionCommand - return std::nullopt; -} + // Seek -bool Offense::has_open_shot() { - // Goal location - rj_geometry::Point their_goal_pos = field_dimensions_.their_goal_loc(); - double goal_width = field_dimensions_.goal_width(); // 1.0 meters - - // Ball location - rj_geometry::Point ball_position = this->last_world_state_->ball.position; - - double best_distance = -1.0; - rj_geometry::Point increment(0.05, 0); - rj_geometry::Point curr_point = - their_goal_pos - rj_geometry::Point(goal_width / 2.0, 0) + increment; - for (int i = 0; i < 19; i++) { - double distance = distance_from_their_robots(ball_position, curr_point); - if (distance > best_distance) { - best_distance = distance; - } - curr_point = curr_point + increment; - } + return std::nullopt; // TODO - return best_distance > max_receive_distance; -} + case POSSESSION: -double Offense::distance_from_their_robots(rj_geometry::Point tail, rj_geometry::Point head) { - rj_geometry::Point vec = head - tail; - auto their_robots = this->last_world_state_->their_robots; - double min_angle = -0.5; - for (auto enemy : their_robots) { - rj_geometry::Point enemy_vec = enemy.pose.position() - tail; - if (enemy_vec.dot(vec) < 0) { - continue; - } - auto projection = (enemy_vec.dot(vec) / vec.dot(vec)); - enemy_vec = enemy_vec - (projection)*vec; - double distance = enemy_vec.mag(); - if (distance < (kRobotRadius + kBallRadius)) { - return -1.0; - } - double angle = distance / projection; - if ((min_angle < 0) || (angle < min_angle)) { - min_angle = angle; - } - } - return min_angle; -} + + + return std::nullopt; // TODO + + case PASSING: + + // Pass to target robot + + return std::nullopt; // TODO + + case STEALING: + + // Settle/Collect + + return std::nullopt; // TODO + + case RECEIVING: + + // Settle/Collect but like slower + + return std::nullopt; + + case SHOOTING: -void Offense::receive_communication_response(communication::AgentPosResponseWrapper response) { - Position::receive_communication_response(response); + // Line kick - // Check to see if we are dealing with scorer requests - if (const communication::ScorerRequest* scorer_response = - std::get_if(&response.associated_request)) { - handle_scorer_response(response.responses); - return; + return std::nullopt; } } communication::PosAgentResponseWrapper Offense::receive_communication_request( communication::AgentPosRequestWrapper request) { - communication::PosAgentResponseWrapper comm_response = - Position::receive_communication_request(request); - - // If a scorer request was received override the position receive_communication_request return - if (const communication::ScorerRequest* scorer_request = - std::get_if(&request.request)) { - communication::ScorerResponse scorer_response = receive_scorer_request(*scorer_request); - comm_response.response = scorer_response; - } else if (const communication::ResetScorerRequest* _ = - std::get_if(&request.request)) { - communication::Acknowledge response = receive_reset_scorer_request(); - comm_response.response = response; - } else if (const communication::PassRequest* pass_request = - std::get_if(&request.request)) { + + // PassRequests: only in offense right now + if (const communication::PassRequest* pass_request = + std::get_if(&request.request)) { // If the robot recieves a PassRequest, only process it if we are oppen rj_geometry::Point robot_position = @@ -308,133 +180,42 @@ communication::PosAgentResponseWrapper Offense::receive_communication_request( // determine when we are open, but this may need to change if (min_robot_dist > max_receive_distance && min_path_dist > max_receive_distance) { communication::PassResponse response = Position::receive_pass_request(*pass_request); - comm_response.response = response; + return {response}; } - } - - return comm_response; -} - -void Offense::send_scorer_request() { - communication::ScorerRequest scorer_request{}; - communication::generate_uid(scorer_request); - scorer_request.robot_id = robot_id_; - - // Calculate distance to ball - rj_geometry::Point robot_position = - last_world_state_->get_robot(true, robot_id_).pose.position(); - rj_geometry::Point ball_position = last_world_state_->ball.position; - double ball_distance = robot_position.dist_to(ball_position); - scorer_request.ball_distance = ball_distance; - - communication::PosAgentRequestWrapper communication_request{}; - communication_request.request = scorer_request; - communication_request.broadcast = true; - communication_request_ = communication_request; + return {}; + } else { + // Super: other kinds of requests + return Position::receive_communication_request(request); + } } -void Offense::send_reset_scorer_request() { - communication::ResetScorerRequest reset_scorer_request{}; - communication::generate_uid(reset_scorer_request); - - communication::PosAgentRequestWrapper communication_request{}; - communication_request.request = reset_scorer_request; - communication_request.broadcast = true; - - communication_request_ = communication_request; - last_scorer_ = true; - scorer_ = false; +inline void Offense::reset_timeout() { + last_time_ = RJ::now(); } -communication::ScorerResponse Offense::receive_scorer_request( - communication::ScorerRequest scorer_request) { - communication::ScorerResponse scorer_response{}; - communication::generate_uid(scorer_response); - scorer_response.robot_id = robot_id_; - - // Calculate distance to ball - rj_geometry::Point robot_position = - last_world_state_->get_robot(true, robot_id_).pose.position(); - rj_geometry::Point ball_position = last_world_state_->ball.position; - double ball_distance = robot_position.dist_to(ball_position); - scorer_response.ball_distance = ball_distance; - - // Switch scorers if better scorer - if (scorer_ && scorer_request.ball_distance < ball_distance) { - scorer_ = false; - current_state_ = FACING; - } - - // Give fake answer if previous scorer - if (last_scorer_) { - scorer_response.ball_distance = 300; - } - - return scorer_response; +inline bool Offense::timed_out() { + const auto timeout = timeout(current_state_); + return timeout >= 0 && RJ::now() - last_time_ > timeout; } -communication::Acknowledge Offense::receive_reset_scorer_request() { - communication::Acknowledge acknowledge{}; - communication::generate_uid(acknowledge); - - last_scorer_ = false; - send_scorer_request(); - - return acknowledge; +void Offense::derived_acknowledge_pass() { + // I have been chosen as the receiver + current_state_ = RECEIVING; } -void Offense::handle_scorer_response( - const std::vector& responses) { - rj_geometry::Point this_robot_position = - last_world_state_->get_robot(true, robot_id_).pose.position(); - rj_geometry::Point ball_position = last_world_state_->ball.position; - double this_ball_distance = this_robot_position.dist_to(ball_position); - - for (communication::AgentResponseVariant response : responses) { - if (const communication::ScorerResponse* scorer_response = - std::get_if(&response)) { - if (scorer_response->ball_distance < this_ball_distance) { - return; - } - } +void Offense::derived_pass_ball() { + // When we have the ball we send out a pass request. + // However, if we've since started shooting, just do that. + // Otherwise, we can now pass because somebody has accepted our pass. + if (current_state_ != SHOOTING) { + current_state_ = PASSING; } - - // Make this robot the scorer - scorer_ = true; } -void Offense::derived_acknowledge_pass() { current_state_ = FACING; } - -void Offense::derived_pass_ball() { current_state_ = PASSING; } - void Offense::derived_acknowledge_ball_in_transit() { + // The ball is coming to me current_state_ = RECEIVING; - chasing_ball = false; -} - -rj_geometry::Point Offense::calculate_best_shot() { - // Goal location - rj_geometry::Point their_goal_pos = field_dimensions_.our_goal_loc(); - double goal_width = field_dimensions_.goal_width(); // 1.0 meters - - // Ball location - rj_geometry::Point ball_position = this->last_world_state_->ball.position; - - rj_geometry::Point best_shot = their_goal_pos; - double best_distance = -1.0; - rj_geometry::Point increment(0.05, 0); - rj_geometry::Point curr_point = - their_goal_pos - rj_geometry::Point(goal_width / 2.0, 0) + increment; - for (int i = 0; i < 19; i++) { - double distance = distance_from_their_robots(ball_position, curr_point); - if (distance > best_distance) { - best_distance = distance; - best_shot = curr_point; - } - curr_point = curr_point + increment; - } - return best_shot; } } // namespace strategy diff --git a/soccer/src/soccer/strategy/agent/position/offense.hpp b/soccer/src/soccer/strategy/agent/position/offense.hpp index 705890dc482..9fe189cdfc0 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/offense.hpp @@ -27,8 +27,7 @@ class Offense : public Position { public: Offense(int r_id); ~Offense() override = default; - - void receive_communication_response(communication::AgentPosResponseWrapper response) override; + communication::PosAgentResponseWrapper receive_communication_request( communication::AgentPosRequestWrapper request) override; @@ -37,91 +36,53 @@ class Offense : public Position { void derived_acknowledge_ball_in_transit() override; private: - bool kicking_{true}; - - // These variables are for calculating ball speed when passing - static constexpr float kFinalBallSpeed{0.0f}; - std::optional derived_get_task(RobotIntent intent) override; - // TODO (Kevin): strategy design pattern for BallHandler/Receiver enum State { - IDLING, // simply staying in place - SEARCHING, // moving around on the field to get open - PASSING, // physically kicking the ball towards another robot - PREPARING_SHOT, // pivot around ball in preparation for shot - SHOOTING, // physically kicking the ball towards the net - RECEIVING, // physically intercepting the ball from a pass (gets possession) - STEALING, // attempting to intercept the ball from the other team - FACING, // turning to face the ball - SCORER, // overrides everything and will attempt to steal the bal and shoot it - AWAITING_SEND_PASS, // is waiting to send a pass to someone else - SEEKING, // is trying to get open + DEFAULT, // Decide what to do + SEEKING, // Get open + POSSESSION, // Holding the ball + PASSING, // Getting rid of it + STEALING, // Getting the ball + RECEIVING, // Getting the ball from a pass + SHOOTING // Winning the game }; + // The longest amount of time we should be + static constexpr RJ::Seconds timeout(State s) { + switch (s) { + case DEFAULT: + return RJ::Seconds{-1}; + case SEEKING: + return RJ::Seconds{-1}; + case POSSESSION: + return RJ::Seconds{-1}; + case PASSING: + return RJ::Seconds{5}; + case STEALING: + return RJ::Seconds{5}; + case RECEIVING: + return RJ::Seconds{5}; + case SHOOTING: + return RJ::Seconds{5}; + } + } + + void reset_timeout(); + + bool timed_out(); + + RJ::Time last_time_; + State update_state(); std::optional state_to_task(RobotIntent intent); // current state of the offensive agent (state machine) - State current_state_ = IDLING; - - bool scorer_ = false; - bool last_scorer_ = false; - - bool has_open_shot(); - - /** - * @brief Send request to the other robots to see if this robot should be the scorer - * - */ - void send_scorer_request(); - - /** - * @brief Finds this robot's distance to the ball and sends it back to the robot who asked if - * it should be the scorer - * - * @param scorer_request request from the prospective scorer robot - * @return communication::ScorerResponse this robots response to the prospective scorer robot - */ - communication::ScorerResponse receive_scorer_request( - communication::ScorerRequest scorer_request); - - /** - * @brief This agent can go through the distance of every other offensive robot from the goal - * to decide whether this robot should become the scorer. - * - * @param scorer_responses a vector of the distance to the ball for each other offense robot - */ - void handle_scorer_response( - const std::vector& scorer_responses); - - /** - * @brief Send a request to the other offensive agents to let them know to reset who is the - * scorer. - * - */ - void send_reset_scorer_request(); - - /** - * @brief When the scorer is being reset all offense robots should send a scorer requests - * and reset their last_scorer_ flag. - * - * @param reset_scorer_request the reset scorer request - * @return communication::Acknowledge acknowledgement that this robot will reset their scorer - * status - */ - communication::Acknowledge receive_reset_scorer_request(); - - /** - * @brief Calcualtes the best location for a shot - */ - rj_geometry::Point calculate_best_shot(); - - /** - * @brief Calculates the distance of vector from other team's closest robot - */ - double distance_from_their_robots(rj_geometry::Point tail, rj_geometry::Point heaad); + State current_state_ = State::DEFAULT; + + // These variables are for calculating ball speed when passing + static constexpr float kFinalBallSpeed{0.0f}; }; } // namespace strategy diff --git a/soccer/src/soccer/strategy/agent/position/position.cpp b/soccer/src/soccer/strategy/agent/position/position.cpp index bf329b6ab15..703c13f3a71 100644 --- a/soccer/src/soccer/strategy/agent/position/position.cpp +++ b/soccer/src/soccer/strategy/agent/position/position.cpp @@ -6,6 +6,8 @@ namespace strategy { Position::Position(int r_id) : robot_id_(r_id) {} +Position::Position(int r_id, std::string position_name) : robot_id_{r_id}, position_name_{std::move(position_name)} {}; + std::optional Position::get_task(WorldState& world_state, FieldDimensions& field_dimensions) { // Point class variables to parameter references @@ -33,6 +35,8 @@ void Position::set_is_done() { is_done_ = true; } void Position::set_goal_canceled() { goal_canceled_ = true; } +inline void Position::set_goalie_id(int goalie_id) { goalie_id_ = goalie_id; } + bool Position::check_is_done() { if (is_done_) { is_done_ = false; diff --git a/soccer/src/soccer/strategy/agent/position/position.hpp b/soccer/src/soccer/strategy/agent/position/position.hpp index 308293d6175..6bdf24dc7a4 100644 --- a/soccer/src/soccer/strategy/agent/position/position.hpp +++ b/soccer/src/soccer/strategy/agent/position/position.hpp @@ -208,7 +208,15 @@ class Position { */ virtual void revive(){}; + /** + * @brief setter for goalie id + */ + void set_goalie_id(int goalie_id); + protected: + + Position(int r_id, std::string position_name); + // should be overriden in subclass constructors std::string position_name_{"Position"}; @@ -282,6 +290,9 @@ class Position { // protected to allow WorldState to be accessed directly by derived WorldState* last_world_state_; + // Current goalie + int goalie_id_; + private: /** * @brief allow derived classes to change behavior of get_task(). See From 297f586e4d5ba111db48bdf47a68a9b9a221f588 Mon Sep 17 00:00:00 2001 From: Sid Parikh Date: Thu, 1 Feb 2024 21:33:35 +0000 Subject: [PATCH 02/22] WIP building FSM --- .../soccer/strategy/agent/position/offense.cpp | 17 ++++++++++------- .../soccer/strategy/agent/position/offense.hpp | 13 +++++++++++++ 2 files changed, 23 insertions(+), 7 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index c03767c333c..0652f13ac25 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -29,23 +29,26 @@ Offense::State Offense::update_state() { case DEFAULT: return SEEKING; - break; case SEEKING: - // Seek - // Stay until moved to receiving or stealing + return SEEKING; + + case POSSESSION_START: + + if (has_open_shot()) { + return SHOOTING; + } - // TODO: should_steal() tomatic switch to passing if a pass is accepted. + // No open shot, try to pass. + // This will trigger an automatic switch to passing if a pass is accepted. broadcast_direct_pass_request(); return POSSESSION; case POSSESSION: - // If we can make a shot, make it. - // If we need to stop possessing now, shoot. - if (has_open_shot() || timed_out()) { + if (has_open_shot()) { return SHOOTING; } diff --git a/soccer/src/soccer/strategy/agent/position/offense.hpp b/soccer/src/soccer/strategy/agent/position/offense.hpp index 9fe189cdfc0..9b53aecb84a 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/offense.hpp @@ -83,6 +83,19 @@ class Offense : public Position { // These variables are for calculating ball speed when passing static constexpr float kFinalBallSpeed{0.0f}; + + + // The following functions are calculations to aid state transition or MotionCommand creation. + + /** + * @brief Checks if this agent has a good shot, in which case it should shoot instead of passing. + */ + bool has_open_shot(); + + /** + * @brief Calculates the distance of vector from other team's closest robot + */ + double distance_from_their_robots(rj_geometry::Point tail, rj_geometry::Point heaad); }; } // namespace strategy From 70035fa7ce157a3bacb7079c0bdeeafe63c573f3 Mon Sep 17 00:00:00 2001 From: Sid Parikh Date: Sat, 3 Feb 2024 22:54:03 +0000 Subject: [PATCH 03/22] State Transitions --- soccer/src/soccer/strategy/agent/position/offense.cpp | 10 ++++++++-- soccer/src/soccer/strategy/agent/position/offense.hpp | 9 +++++++++ 2 files changed, 17 insertions(+), 2 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 0652f13ac25..f9d713555d0 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -32,11 +32,15 @@ Offense::State Offense::update_state() { case SEEKING: + // If the ball seems "stealable", we should switch to STEALING + return SEEKING; case POSSESSION_START: - if (has_open_shot()) { + // If we can make a shot, take it + // If we need to stop possessing now, shoot. + if (has_open_shot() || timed_out()) { return SHOOTING; } @@ -48,7 +52,9 @@ Offense::State Offense::update_state() { case POSSESSION: - if (has_open_shot()) { + // If we can make a shot, make it. + // If we need to stop possessing now, shoot. + if (has_open_shot() || timed_out()) { return SHOOTING; } diff --git a/soccer/src/soccer/strategy/agent/position/offense.hpp b/soccer/src/soccer/strategy/agent/position/offense.hpp index 9b53aecb84a..e57832e975e 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/offense.hpp @@ -41,6 +41,7 @@ class Offense : public Position { enum State { DEFAULT, // Decide what to do SEEKING, // Get open + POSSESSION_START, // Try to shoot and send pass request POSSESSION, // Holding the ball PASSING, // Getting rid of it STEALING, // Getting the ball @@ -57,6 +58,8 @@ class Offense : public Position { return RJ::Seconds{-1}; case POSSESSION: return RJ::Seconds{-1}; + case POSSESSION_START: + return RJ::Seconds{-1}; case PASSING: return RJ::Seconds{5}; case STEALING: @@ -84,6 +87,12 @@ class Offense : public Position { // These variables are for calculating ball speed when passing static constexpr float kFinalBallSpeed{0.0f}; + // Used to tell if we have lost all chance of possessing the ball + static constexpr double kBallTooFarDist{0.5}; + + // Used to tell if the ball is close enough to steal + static constexpr double kStealBallRadius{0.5}; + // The following functions are calculations to aid state transition or MotionCommand creation. From 30e4ef91c0f0aa6e028c55faf646b4c1e8efeb72 Mon Sep 17 00:00:00 2001 From: Sid Parikh Date: Sun, 4 Feb 2024 03:39:04 +0000 Subject: [PATCH 04/22] i'm actually trash at git --- .../strategy/agent/position/offense.cpp | 50 ++++++++++++++++++- 1 file changed, 48 insertions(+), 2 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index f9d713555d0..c45b6a0fb0c 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -204,8 +204,8 @@ inline void Offense::reset_timeout() { } inline bool Offense::timed_out() { - const auto timeout = timeout(current_state_); - return timeout >= 0 && RJ::now() - last_time_ > timeout; + const auto time_val = this->timeout(current_state_); + return time_val >= RJ::Seconds{0} && RJ::now() - last_time_ > time_val; } void Offense::derived_acknowledge_pass() { @@ -227,4 +227,50 @@ void Offense::derived_acknowledge_ball_in_transit() { current_state_ = RECEIVING; } +bool Offense::has_open_shot() { + // Goal location + rj_geometry::Point their_goal_pos = field_dimensions_.their_goal_loc(); + double goal_width = field_dimensions_.goal_width(); // 1.0 meters + + // Ball location + rj_geometry::Point ball_position = this->last_world_state_->ball.position; + + double best_distance = -1.0; + rj_geometry::Point increment(0.05, 0); + rj_geometry::Point curr_point = + their_goal_pos - rj_geometry::Point(goal_width / 2.0, 0) + increment; + for (int i = 0; i < 19; i++) { + double distance = distance_from_their_robots(ball_position, curr_point); + if (distance > best_distance) { + best_distance = distance; + } + curr_point = curr_point + increment; + } + + return best_distance > max_receive_distance; +} + +double Offense::distance_from_their_robots(rj_geometry::Point tail, rj_geometry::Point head) { + rj_geometry::Point vec = head - tail; + auto their_robots = this->last_world_state_->their_robots; + double min_angle = -0.5; + for (auto enemy : their_robots) { + rj_geometry::Point enemy_vec = enemy.pose.position() - tail; + if (enemy_vec.dot(vec) < 0) { + continue; + } + auto projection = (enemy_vec.dot(vec) / vec.dot(vec)); + enemy_vec = enemy_vec - (projection)*vec; + double distance = enemy_vec.mag(); + if (distance < (kRobotRadius + kBallRadius)) { + return -1.0; + } + double angle = distance / projection; + if ((min_angle < 0) || (angle < min_angle)) { + min_angle = angle; + } + } + return min_angle; +} + } // namespace strategy From 00c1242d463dd83fd190cae885b951f2e9e78868 Mon Sep 17 00:00:00 2001 From: Sid Parikh Date: Sun, 4 Feb 2024 03:43:28 +0000 Subject: [PATCH 05/22] i knew that --- soccer/src/soccer/strategy/agent/position/position.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/soccer/src/soccer/strategy/agent/position/position.cpp b/soccer/src/soccer/strategy/agent/position/position.cpp index 703c13f3a71..33ae2b9f188 100644 --- a/soccer/src/soccer/strategy/agent/position/position.cpp +++ b/soccer/src/soccer/strategy/agent/position/position.cpp @@ -35,7 +35,7 @@ void Position::set_is_done() { is_done_ = true; } void Position::set_goal_canceled() { goal_canceled_ = true; } -inline void Position::set_goalie_id(int goalie_id) { goalie_id_ = goalie_id; } +void Position::set_goalie_id(int goalie_id) { goalie_id_ = goalie_id; } bool Position::check_is_done() { if (is_done_) { From 05055f8c98dcbb3dee3cb58212cdb4306e10fe53 Mon Sep 17 00:00:00 2001 From: Sid Parikh Date: Sun, 4 Feb 2024 03:51:57 +0000 Subject: [PATCH 06/22] WIP state_to_task --- .../strategy/agent/position/offense.cpp | 21 +++++++++++++++---- .../strategy/agent/position/offense.hpp | 5 +++++ 2 files changed, 22 insertions(+), 4 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index c45b6a0fb0c..bc883c268b0 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -33,6 +33,11 @@ Offense::State Offense::update_state() { case SEEKING: // If the ball seems "stealable", we should switch to STEALING + if (can_steal_ball()) { + return STEALING; + } + + // If we need to get a new seeking target, restart seeking return SEEKING; @@ -73,7 +78,7 @@ Offense::State Offense::update_state() { } // If we lost the ball completely, give up - if (ball_position.dist_to(robot_position) > kBallTooFarDist) { + if (distance_to_ball > kBallTooFarDist) { return DEFAULT; } @@ -86,7 +91,7 @@ Offense::State Offense::update_state() { } // If we ran out of time or the ball is out of our radius, give up - if (timed_out() || ball_position.dist_to(robot_position) > kStealBallRadius) { + if (timed_out() || distance_to_ball > kStealBallRadius) { return DEFAULT; } @@ -124,8 +129,6 @@ std::optional Offense::state_to_task(RobotIntent intent) { case SEEKING: - // Seek - return std::nullopt; // TODO case POSSESSION: @@ -273,4 +276,14 @@ double Offense::distance_from_their_robots(rj_geometry::Point tail, rj_geometry: return min_angle; } +bool Offense::can_steal_ball() { + rj_geometry::Point robot_position = world_state->get_robot(true, robot_id_).pose.position(); + + rj_geometry::Point ball_position = world_state->ball.position; + + double distance_to_ball = robot_position.dist_to(ball_position); + + return distance_to_ball < kStealBallRadius; +} + } // namespace strategy diff --git a/soccer/src/soccer/strategy/agent/position/offense.hpp b/soccer/src/soccer/strategy/agent/position/offense.hpp index e57832e975e..15cbb97899e 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/offense.hpp @@ -105,6 +105,11 @@ class Offense : public Position { * @brief Calculates the distance of vector from other team's closest robot */ double distance_from_their_robots(rj_geometry::Point tail, rj_geometry::Point heaad); + + /** + * @brief Check if this agent could easily steal the ball + */ + bool can_steal_ball(); }; } // namespace strategy From e04fe6019e4c372f0bee6fbb848158b273cf293a Mon Sep 17 00:00:00 2001 From: Sid Parikh Date: Sun, 4 Feb 2024 19:49:43 +0000 Subject: [PATCH 07/22] compiler errors --- .../strategy/agent/agent_action_client.hpp | 2 +- .../strategy/agent/position/offense.cpp | 30 ++++++---------- .../strategy/agent/position/offense.hpp | 34 ++++++++++--------- .../strategy/agent/position/position.cpp | 3 +- .../strategy/agent/position/position.hpp | 3 +- .../soccer/strategy/agent/position/seeker.cpp | 7 +++- .../soccer/strategy/agent/position/seeker.hpp | 4 +++ 7 files changed, 43 insertions(+), 40 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/agent_action_client.hpp b/soccer/src/soccer/strategy/agent/agent_action_client.hpp index 5b1f346cbf9..c551da9643c 100644 --- a/soccer/src/soccer/strategy/agent/agent_action_client.hpp +++ b/soccer/src/soccer/strategy/agent/agent_action_client.hpp @@ -13,9 +13,9 @@ #include #include #include +#include #include #include -#include #include "game_state.hpp" #include "rclcpp/rclcpp.hpp" diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index bc883c268b0..41aa7125139 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -2,7 +2,7 @@ namespace strategy { -Offense::Offense(int r_id) : Position(r_id) { position_name_ = "Offense"; } +Offense::Offense(int r_id) : Position{r_id}, seeker_{r_id}, { position_name_ = "Offense"; } std::optional Offense::derived_get_task(RobotIntent intent) { State next_state = update_state(); @@ -24,8 +24,7 @@ Offense::State Offense::update_state() { rj_geometry::Point ball_position = world_state->ball.position; double distance_to_ball = robot_position.dist_to(ball_position); - switch (current_state_) { - + switch (current_state_) { case DEFAULT: return SEEKING; @@ -82,7 +81,6 @@ Offense::State Offense::update_state() { return DEFAULT; } - case STEALING: // Go to possession if successful @@ -119,35 +117,31 @@ Offense::State Offense::update_state() { } std::optional Offense::state_to_task(RobotIntent intent) { - switch (current_state_) { - case DEFAULT: // Do nothing: empty motion command intent.motion_command = planning::MotionCommand{}; return intent; - + case SEEKING: - return std::nullopt; // TODO + return std::nullopt; // TODO case POSSESSION: - - - return std::nullopt; // TODO + return std::nullopt; // TODO case PASSING: // Pass to target robot - return std::nullopt; // TODO + return std::nullopt; // TODO case STEALING: // Settle/Collect - return std::nullopt; // TODO + return std::nullopt; // TODO case RECEIVING: @@ -165,7 +159,6 @@ std::optional Offense::state_to_task(RobotIntent intent) { communication::PosAgentResponseWrapper Offense::receive_communication_request( communication::AgentPosRequestWrapper request) { - // PassRequests: only in offense right now if (const communication::PassRequest* pass_request = std::get_if(&request.request)) { @@ -202,11 +195,9 @@ communication::PosAgentResponseWrapper Offense::receive_communication_request( } } -inline void Offense::reset_timeout() { - last_time_ = RJ::now(); -} +void Offense::reset_timeout() { last_time_ = RJ::now(); } -inline bool Offense::timed_out() { +bool Offense::timed_out() { const auto time_val = this->timeout(current_state_); return time_val >= RJ::Seconds{0} && RJ::now() - last_time_ > time_val; } @@ -255,7 +246,8 @@ bool Offense::has_open_shot() { double Offense::distance_from_their_robots(rj_geometry::Point tail, rj_geometry::Point head) { rj_geometry::Point vec = head - tail; - auto their_robots = this->last_world_state_->their_robots; + auto& their_robots = this->last_world_state_->their_robots; + double min_angle = -0.5; for (auto enemy : their_robots) { rj_geometry::Point enemy_vec = enemy.pose.position() - tail; diff --git a/soccer/src/soccer/strategy/agent/position/offense.hpp b/soccer/src/soccer/strategy/agent/position/offense.hpp index 15cbb97899e..4a10475c3b1 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/offense.hpp @@ -27,7 +27,7 @@ class Offense : public Position { public: Offense(int r_id); ~Offense() override = default; - + communication::PosAgentResponseWrapper receive_communication_request( communication::AgentPosRequestWrapper request) override; @@ -39,17 +39,17 @@ class Offense : public Position { std::optional derived_get_task(RobotIntent intent) override; enum State { - DEFAULT, // Decide what to do - SEEKING, // Get open - POSSESSION_START, // Try to shoot and send pass request - POSSESSION, // Holding the ball - PASSING, // Getting rid of it - STEALING, // Getting the ball - RECEIVING, // Getting the ball from a pass - SHOOTING // Winning the game + DEFAULT, // Decide what to do + SEEKING, // Get open + POSSESSION_START, // Try to shoot and send pass request + POSSESSION, // Holding the ball + PASSING, // Getting rid of it + STEALING, // Getting the ball + RECEIVING, // Getting the ball from a pass + SHOOTING // Winning the game }; - // The longest amount of time we should be + // The longest amount of time we should be static constexpr RJ::Seconds timeout(State s) { switch (s) { case DEFAULT: @@ -77,6 +77,8 @@ class Offense : public Position { RJ::Time last_time_; + Seeker seeker_; + State update_state(); std::optional state_to_task(RobotIntent intent); @@ -93,22 +95,22 @@ class Offense : public Position { // Used to tell if the ball is close enough to steal static constexpr double kStealBallRadius{0.5}; - // The following functions are calculations to aid state transition or MotionCommand creation. /** - * @brief Checks if this agent has a good shot, in which case it should shoot instead of passing. - */ + * @brief Checks if this agent has a good shot, in which case it should shoot instead of + * passing. + */ bool has_open_shot(); /** * @brief Calculates the distance of vector from other team's closest robot */ - double distance_from_their_robots(rj_geometry::Point tail, rj_geometry::Point heaad); + double distance_from_their_robots(rj_geometry::Point tail, rj_geometry::Point head); /** - * @brief Check if this agent could easily steal the ball - */ + * @brief Check if this agent could easily steal the ball + */ bool can_steal_ball(); }; diff --git a/soccer/src/soccer/strategy/agent/position/position.cpp b/soccer/src/soccer/strategy/agent/position/position.cpp index 33ae2b9f188..69436cce6d3 100644 --- a/soccer/src/soccer/strategy/agent/position/position.cpp +++ b/soccer/src/soccer/strategy/agent/position/position.cpp @@ -6,7 +6,8 @@ namespace strategy { Position::Position(int r_id) : robot_id_(r_id) {} -Position::Position(int r_id, std::string position_name) : robot_id_{r_id}, position_name_{std::move(position_name)} {}; +Position::Position(int r_id, std::string position_name) + : robot_id_{r_id}, position_name_{std::move(position_name)} {}; std::optional Position::get_task(WorldState& world_state, FieldDimensions& field_dimensions) { diff --git a/soccer/src/soccer/strategy/agent/position/position.hpp b/soccer/src/soccer/strategy/agent/position/position.hpp index 6bdf24dc7a4..f6868c2d3a8 100644 --- a/soccer/src/soccer/strategy/agent/position/position.hpp +++ b/soccer/src/soccer/strategy/agent/position/position.hpp @@ -210,11 +210,10 @@ class Position { /** * @brief setter for goalie id - */ + */ void set_goalie_id(int goalie_id); protected: - Position(int r_id, std::string position_name); // should be overriden in subclass constructors diff --git a/soccer/src/soccer/strategy/agent/position/seeker.cpp b/soccer/src/soccer/strategy/agent/position/seeker.cpp index 65467d376a9..183cb09461d 100644 --- a/soccer/src/soccer/strategy/agent/position/seeker.cpp +++ b/soccer/src/soccer/strategy/agent/position/seeker.cpp @@ -9,7 +9,10 @@ std::optional Seeker::get_task(RobotIntent intent, const WorldState // Determine target position for seeking rj_geometry::Point current_loc = last_world_state->get_robot(true, robot_id_).pose.position(); - target_pt_ = get_open_point(last_world_state, current_loc, field_dimensions); + if (!target_valid_) { + target_pt_ = get_open_point(last_world_state, current_loc, field_dimensions); + target_valid_ = true; + } planning::PathTargetFaceOption face_option = planning::FaceBall{}; bool ignore_ball = false; @@ -19,6 +22,8 @@ std::optional Seeker::get_task(RobotIntent intent, const WorldState return intent; } +void Seeker::reset_target() { target_valid_ = false; } + rj_geometry::Point Seeker::get_open_point(const WorldState* world_state, rj_geometry::Point current_position, const FieldDimensions& field_dimensions) const { diff --git a/soccer/src/soccer/strategy/agent/position/seeker.hpp b/soccer/src/soccer/strategy/agent/position/seeker.hpp index e6296b296ae..817f1fed1a6 100644 --- a/soccer/src/soccer/strategy/agent/position/seeker.hpp +++ b/soccer/src/soccer/strategy/agent/position/seeker.hpp @@ -44,12 +44,16 @@ class Seeker : public RoleInterface { std::optional get_task(RobotIntent intent, const WorldState* world_state, FieldDimensions field_dimensions) override; + void reset_target(); + private: // The seeker's id int robot_id_; // The taret point to move to rj_geometry::Point target_pt_{0.0, 0.0}; + bool target_valid_{false}; + /** * @brief Returns the point which is most 'open' * From 9395598d976efd3e70956a8d45bdb326b74690d5 Mon Sep 17 00:00:00 2001 From: Sid Parikh Date: Sun, 4 Feb 2024 20:23:44 +0000 Subject: [PATCH 08/22] cleaning up code organization --- .../strategy/agent/position/offense.cpp | 49 ++++++++++----- .../strategy/agent/position/offense.hpp | 63 +++++++++++++++---- 2 files changed, 85 insertions(+), 27 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 41aa7125139..01a22ff88e6 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -2,31 +2,35 @@ namespace strategy { -Offense::Offense(int r_id) : Position{r_id}, seeker_{r_id}, { position_name_ = "Offense"; } +Offense::Offense(int r_id) : Position{r_id}, seeker_{r_id} { position_name_ = "Offense"; } std::optional Offense::derived_get_task(RobotIntent intent) { - State next_state = update_state(); + + // Get next state, and if different, reset clock + State new_state = next_state(); - if (current_state_ != next_state) { + if (current_state_ != new_state) { reset_timeout(); } - current_state_ = next_state; + current_state_ = new_state; + // Calculate task based on state return state_to_task(intent); } -Offense::State Offense::update_state() { +Offense::State Offense::next_state() { // handle transitions between current state WorldState* world_state = this->last_world_state_; - rj_geometry::Point robot_position = world_state->get_robot(true, robot_id_).pose.position(); - rj_geometry::Point ball_position = world_state->ball.position; - double distance_to_ball = robot_position.dist_to(ball_position); - switch (current_state_) { case DEFAULT: + return SEEKING_START; + + case SEEKING_START: + + // Unconditionally only stay in this state for one tick. return SEEKING; case SEEKING: @@ -37,6 +41,11 @@ Offense::State Offense::update_state() { } // If we need to get a new seeking target, restart seeking + if (check_is_done() || last_world_state_->get_robot(true, robot_id_).velocity.linear().mag() <= 0.01) { + + + + } return SEEKING; @@ -77,7 +86,10 @@ Offense::State Offense::update_state() { } // If we lost the ball completely, give up - if (distance_to_ball > kBallTooFarDist) { + const auto& robot = last_world_state_->get_robot(true, robot_id_); + const auto& ball = last_world_state_->ball; + + if (ball.dist_to(robot.pose.position()) > kBallTooFarDist) { return DEFAULT; } @@ -89,7 +101,10 @@ Offense::State Offense::update_state() { } // If we ran out of time or the ball is out of our radius, give up - if (timed_out() || distance_to_ball > kStealBallRadius) { + const auto& robot = last_world_state_->get_robot(true, robot_id_); + const auto& ball = last_world_state_->ball; + + if (timed_out() || ball.dist_to(robot.pose.position()) > kBallTooFarDist) { return DEFAULT; } @@ -123,9 +138,15 @@ std::optional Offense::state_to_task(RobotIntent intent) { intent.motion_command = planning::MotionCommand{}; return intent; + case SEEKING_START: + + // Calculate a new seeking point + seeker_.reset_target(); + return seeker_.get_task(std::move(intent), last_world_state_, field_dimensions_); + case SEEKING: - return std::nullopt; // TODO + return seeker_.get_task(std::move(intent), last_world_state_, field_dimensions_); case POSSESSION: @@ -269,9 +290,9 @@ double Offense::distance_from_their_robots(rj_geometry::Point tail, rj_geometry: } bool Offense::can_steal_ball() { - rj_geometry::Point robot_position = world_state->get_robot(true, robot_id_).pose.position(); + rj_geometry::Point robot_position = last_world_state_->get_robot(true, robot_id_).pose.position(); - rj_geometry::Point ball_position = world_state->ball.position; + rj_geometry::Point ball_position = last_world_state_->ball.position; double distance_to_ball = robot_position.dist_to(ball_position); diff --git a/soccer/src/soccer/strategy/agent/position/offense.hpp b/soccer/src/soccer/strategy/agent/position/offense.hpp index 4a10475c3b1..131480f6c6c 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/offense.hpp @@ -19,7 +19,7 @@ namespace strategy { -/* +/** * The Offense position handles general offensive tasks, like passing to * teammates, moving to get open, or shooting on goal. */ @@ -36,10 +36,14 @@ class Offense : public Position { void derived_acknowledge_ball_in_transit() override; private: + /** + * @brief Overriden from Position. Calls next_state and then state_to_task on each tick. + */ std::optional derived_get_task(RobotIntent intent) override; enum State { DEFAULT, // Decide what to do + SEEKING_START, // Calculate seeking point SEEKING, // Get open POSSESSION_START, // Try to shoot and send pass request POSSESSION, // Holding the ball @@ -49,11 +53,38 @@ class Offense : public Position { SHOOTING // Winning the game }; - // The longest amount of time we should be + /** + * @return what the state should be right now. called on each get_task tick + */ + State next_state(); + + /** + * @return the task to execute. called on each get_task tick AFTER next_state() + */ + std::optional state_to_task(RobotIntent intent); + + /** + * @brief This FSM has timeouts for certain states. + * Ideally, these would not be necessary; as planners get more sophisticated + * they should not get "stuck". However, empirically, the offense FSM in particular + * has been observed to deadlock often. + * + * One common case is when waiting for a receiver to accept a pass; if no receiver responds, + * the timeout is necessary. In the future receivers may be able to respond in the negative + * instead of ignoring the request. + * + * The timeouts are a safety mechanism, and should not be the primary reason for a + * state transition. They are set relatively high for this reason. + * + * @return the maximum duration to stay in a given state, or -1 if there is no maximum. + * + */ static constexpr RJ::Seconds timeout(State s) { switch (s) { case DEFAULT: return RJ::Seconds{-1}; + case SEEKING_START: + return RJ::Seconds{-1}; case SEEKING: return RJ::Seconds{-1}; case POSSESSION: @@ -71,20 +102,26 @@ class Offense : public Position { } } + /** + * @brief Reset the timeout for the current state + */ void reset_timeout(); - bool timed_out(); + /** + * @return if the current state has timed out + */ + bool timed_out() const; - RJ::Time last_time_; + State current_state_ = State::DEFAULT; - Seeker seeker_; + // The time at which the last state started. + RJ::Time last_time_; - State update_state(); - std::optional state_to_task(RobotIntent intent); + /* RoleInterface Members */ + Seeker seeker_; - // current state of the offensive agent (state machine) - State current_state_ = State::DEFAULT; + /* Constants for State or Task Calculation */ // These variables are for calculating ball speed when passing static constexpr float kFinalBallSpeed{0.0f}; @@ -95,23 +132,23 @@ class Offense : public Position { // Used to tell if the ball is close enough to steal static constexpr double kStealBallRadius{0.5}; - // The following functions are calculations to aid state transition or MotionCommand creation. + /* Utility functions for State or Task Calculation */ /** * @brief Checks if this agent has a good shot, in which case it should shoot instead of * passing. */ - bool has_open_shot(); + bool has_open_shot() const; /** * @brief Calculates the distance of vector from other team's closest robot */ - double distance_from_their_robots(rj_geometry::Point tail, rj_geometry::Point head); + double distance_from_their_robots(rj_geometry::Point tail, rj_geometry::Point head) const; /** * @brief Check if this agent could easily steal the ball */ - bool can_steal_ball(); + bool can_steal_ball() const; }; } // namespace strategy From bf11b1d496b1fb09d0b196838f03b437c974c5ae Mon Sep 17 00:00:00 2001 From: Sid Parikh Date: Sun, 4 Feb 2024 20:53:17 +0000 Subject: [PATCH 09/22] WIP state_to_task --- .../strategy/agent/position/offense.cpp | 39 ++++++------------- .../strategy/agent/position/offense.hpp | 21 +++++++++- 2 files changed, 30 insertions(+), 30 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 01a22ff88e6..982df5b4d9a 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -42,9 +42,7 @@ Offense::State Offense::next_state() { // If we need to get a new seeking target, restart seeking if (check_is_done() || last_world_state_->get_robot(true, robot_id_).velocity.linear().mag() <= 0.01) { - - - + return SEEKING_START; } return SEEKING; @@ -86,13 +84,12 @@ Offense::State Offense::next_state() { } // If we lost the ball completely, give up - const auto& robot = last_world_state_->get_robot(true, robot_id_); - const auto& ball = last_world_state_->ball; - - if (ball.dist_to(robot.pose.position()) > kBallTooFarDist) { + if (distance_to_ball() > kBallTooFarDist) { return DEFAULT; } + return PASSING; + case STEALING: // Go to possession if successful @@ -101,13 +98,12 @@ Offense::State Offense::next_state() { } // If we ran out of time or the ball is out of our radius, give up - const auto& robot = last_world_state_->get_robot(true, robot_id_); - const auto& ball = last_world_state_->ball; - - if (timed_out() || ball.dist_to(robot.pose.position()) > kBallTooFarDist) { + if (timed_out() || distance_to_ball() > kBallTooFarDist) { return DEFAULT; } + return STEALING; + case RECEIVING: // If we got it, cool, we have it! @@ -216,13 +212,6 @@ communication::PosAgentResponseWrapper Offense::receive_communication_request( } } -void Offense::reset_timeout() { last_time_ = RJ::now(); } - -bool Offense::timed_out() { - const auto time_val = this->timeout(current_state_); - return time_val >= RJ::Seconds{0} && RJ::now() - last_time_ > time_val; -} - void Offense::derived_acknowledge_pass() { // I have been chosen as the receiver current_state_ = RECEIVING; @@ -242,7 +231,7 @@ void Offense::derived_acknowledge_ball_in_transit() { current_state_ = RECEIVING; } -bool Offense::has_open_shot() { +bool Offense::has_open_shot() const { // Goal location rj_geometry::Point their_goal_pos = field_dimensions_.their_goal_loc(); double goal_width = field_dimensions_.goal_width(); // 1.0 meters @@ -265,7 +254,7 @@ bool Offense::has_open_shot() { return best_distance > max_receive_distance; } -double Offense::distance_from_their_robots(rj_geometry::Point tail, rj_geometry::Point head) { +double Offense::distance_from_their_robots(rj_geometry::Point tail, rj_geometry::Point head) const { rj_geometry::Point vec = head - tail; auto& their_robots = this->last_world_state_->their_robots; @@ -289,14 +278,8 @@ double Offense::distance_from_their_robots(rj_geometry::Point tail, rj_geometry: return min_angle; } -bool Offense::can_steal_ball() { - rj_geometry::Point robot_position = last_world_state_->get_robot(true, robot_id_).pose.position(); - - rj_geometry::Point ball_position = last_world_state_->ball.position; - - double distance_to_ball = robot_position.dist_to(ball_position); - - return distance_to_ball < kStealBallRadius; +bool Offense::can_steal_ball() const { + return distance_to_ball() < kStealBallRadius; } } // namespace strategy diff --git a/soccer/src/soccer/strategy/agent/position/offense.hpp b/soccer/src/soccer/strategy/agent/position/offense.hpp index 131480f6c6c..cd18c426ac3 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/offense.hpp @@ -105,12 +105,22 @@ class Offense : public Position { /** * @brief Reset the timeout for the current state */ - void reset_timeout(); + void reset_timeout() { + // Defined here so it can be inlined + last_time_ = RJ::now(); + } /** * @return if the current state has timed out */ - bool timed_out() const; + bool timed_out() const { + // Defined here so it can be inlined + using namespace std::chrono_literals; + + const auto max_time = timeout(current_state_); + + return (max_time > 0s) && (last_time_ + max_time < RJ::now()); + }; State current_state_ = State::DEFAULT; @@ -149,6 +159,13 @@ class Offense : public Position { * @brief Check if this agent could easily steal the ball */ bool can_steal_ball() const; + + /** + * @return distance from this agent to ball + */ + double distance_to_ball() const { + return last_world_state_->ball.position.dist_to(last_world_state_->get_robot(true, robot_id_).pose.position()); + }; }; } // namespace strategy From f05d534872bee091ca0aa314bab95af0b51b5801 Mon Sep 17 00:00:00 2001 From: Sid Parikh Date: Sun, 4 Feb 2024 21:03:12 +0000 Subject: [PATCH 10/22] passing task --- .../strategy/agent/position/offense.cpp | 57 +++++++++++------- .../strategy/agent/position/offense.hpp | 58 +++++++++---------- 2 files changed, 66 insertions(+), 49 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 982df5b4d9a..644fd1a345f 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -5,7 +5,6 @@ namespace strategy { Offense::Offense(int r_id) : Position{r_id}, seeker_{r_id} { position_name_ = "Offense"; } std::optional Offense::derived_get_task(RobotIntent intent) { - // Get next state, and if different, reset clock State new_state = next_state(); @@ -41,7 +40,8 @@ Offense::State Offense::next_state() { } // If we need to get a new seeking target, restart seeking - if (check_is_done() || last_world_state_->get_robot(true, robot_id_).velocity.linear().mag() <= 0.01) { + if (check_is_done() || + last_world_state_->get_robot(true, robot_id_).velocity.linear().mag() <= 0.01) { return SEEKING_START; } @@ -129,48 +129,67 @@ Offense::State Offense::next_state() { std::optional Offense::state_to_task(RobotIntent intent) { switch (current_state_) { - case DEFAULT: + case DEFAULT: { // Do nothing: empty motion command intent.motion_command = planning::MotionCommand{}; return intent; + } - case SEEKING_START: - + case SEEKING_START: { // Calculate a new seeking point seeker_.reset_target(); return seeker_.get_task(std::move(intent), last_world_state_, field_dimensions_); + } - case SEEKING: - + case SEEKING: { return seeker_.get_task(std::move(intent), last_world_state_, field_dimensions_); + } - case POSSESSION: - + case POSSESSION: { return std::nullopt; // TODO + } - case PASSING: + case PASSING: { + // Kick to the target robot + rj_geometry::Point target_robot_pos = + last_world_state_->get_robot(true, target_robot_id).pose.position(); - // Pass to target robot + planning::LinearMotionInstant target{target_robot_pos}; + planning::MotionCommand line_kick_cmd{"line_kick", target}; - return std::nullopt; // TODO + // Set intent to kick + intent.motion_command = line_kick_cmd; + intent.shoot_mode = RobotIntent::ShootMode::KICK; + intent.trigger_mode = RobotIntent::TriggerMode::ON_BREAK_BEAM; - case STEALING: + // Adjusts kick speed based on distance. + // Details: TIGERS 2019 eTDP, rj_gameplay/passer.py + rj_geometry::Point this_robot_pos = + last_world_state_->get_robot(true, this->robot_id_).pose.position(); + + double dist = target_robot_pos.dist_to(this_robot_pos); + intent.kick_speed = std::sqrt((std::pow(kFinalBallSpeed, 2)) - (2 * kBallDecel * dist)); + return intent; + } + + case STEALING: { // Settle/Collect return std::nullopt; // TODO + } - case RECEIVING: - + case RECEIVING: { // Settle/Collect but like slower return std::nullopt; + } - case SHOOTING: - + case SHOOTING: { // Line kick return std::nullopt; + } } } @@ -278,8 +297,6 @@ double Offense::distance_from_their_robots(rj_geometry::Point tail, rj_geometry: return min_angle; } -bool Offense::can_steal_ball() const { - return distance_to_ball() < kStealBallRadius; -} +bool Offense::can_steal_ball() const { return distance_to_ball() < kStealBallRadius; } } // namespace strategy diff --git a/soccer/src/soccer/strategy/agent/position/offense.hpp b/soccer/src/soccer/strategy/agent/position/offense.hpp index cd18c426ac3..e04cd4e941a 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/offense.hpp @@ -37,8 +37,8 @@ class Offense : public Position { private: /** - * @brief Overriden from Position. Calls next_state and then state_to_task on each tick. - */ + * @brief Overriden from Position. Calls next_state and then state_to_task on each tick. + */ std::optional derived_get_task(RobotIntent intent) override; enum State { @@ -54,31 +54,31 @@ class Offense : public Position { }; /** - * @return what the state should be right now. called on each get_task tick - */ + * @return what the state should be right now. called on each get_task tick + */ State next_state(); /** - * @return the task to execute. called on each get_task tick AFTER next_state() - */ + * @return the task to execute. called on each get_task tick AFTER next_state() + */ std::optional state_to_task(RobotIntent intent); /** - * @brief This FSM has timeouts for certain states. - * Ideally, these would not be necessary; as planners get more sophisticated - * they should not get "stuck". However, empirically, the offense FSM in particular - * has been observed to deadlock often. - * - * One common case is when waiting for a receiver to accept a pass; if no receiver responds, - * the timeout is necessary. In the future receivers may be able to respond in the negative - * instead of ignoring the request. - * - * The timeouts are a safety mechanism, and should not be the primary reason for a - * state transition. They are set relatively high for this reason. - * - * @return the maximum duration to stay in a given state, or -1 if there is no maximum. - * - */ + * @brief This FSM has timeouts for certain states. + * Ideally, these would not be necessary; as planners get more sophisticated + * they should not get "stuck". However, empirically, the offense FSM in particular + * has been observed to deadlock often. + * + * One common case is when waiting for a receiver to accept a pass; if no receiver responds, + * the timeout is necessary. In the future receivers may be able to respond in the negative + * instead of ignoring the request. + * + * The timeouts are a safety mechanism, and should not be the primary reason for a + * state transition. They are set relatively high for this reason. + * + * @return the maximum duration to stay in a given state, or -1 if there is no maximum. + * + */ static constexpr RJ::Seconds timeout(State s) { switch (s) { case DEFAULT: @@ -103,16 +103,16 @@ class Offense : public Position { } /** - * @brief Reset the timeout for the current state - */ + * @brief Reset the timeout for the current state + */ void reset_timeout() { // Defined here so it can be inlined last_time_ = RJ::now(); } /** - * @return if the current state has timed out - */ + * @return if the current state has timed out + */ bool timed_out() const { // Defined here so it can be inlined using namespace std::chrono_literals; @@ -127,7 +127,6 @@ class Offense : public Position { // The time at which the last state started. RJ::Time last_time_; - /* RoleInterface Members */ Seeker seeker_; @@ -161,10 +160,11 @@ class Offense : public Position { bool can_steal_ball() const; /** - * @return distance from this agent to ball - */ + * @return distance from this agent to ball + */ double distance_to_ball() const { - return last_world_state_->ball.position.dist_to(last_world_state_->get_robot(true, robot_id_).pose.position()); + return last_world_state_->ball.position.dist_to( + last_world_state_->get_robot(true, robot_id_).pose.position()); }; }; From 74cbb31b082ad4d110783946cb6079327bf94723 Mon Sep 17 00:00:00 2001 From: Sid Parikh Date: Sun, 4 Feb 2024 20:20:50 -0500 Subject: [PATCH 11/22] wip --- CMakeLists.txt | 4 +- .../strategy/agent/position/offense.cpp | 134 +++++++++++++----- .../strategy/agent/position/offense.hpp | 35 ++++- .../strategy/agent/position/position.cpp | 8 +- .../agent/position/robot_factory_position.cpp | 2 +- 5 files changed, 143 insertions(+), 40 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 349e4443b47..cdabdeff20c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -66,12 +66,12 @@ list(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake") # Because we use ninja, we have to explicitly turn on color output for the compiler if("${CMAKE_CXX_COMPILER_ID}" MATCHES "Clang") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fcolor-diagnostics -Werror=return-stack-address") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fcolor-diagnostics -Werror=return-stack-address -Werror=switch") set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -fuse-ld=lld") set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -fuse-ld=lld") else() message(WARNING "You are using GCC; prefer to use clang if it is installed with the flags `-DCMAKE_C_COMPILER=clang -DCMAKE_CXX_COMPILER=clang++`.") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fdiagnostics-color=always -Werror=return-local-addr") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fdiagnostics-color=always -Werror=return-local-addr -Werror=switch") endif() # Use compiler optimization if we are making a release build. diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 644fd1a345f..e7762e13ec8 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -14,6 +14,8 @@ std::optional Offense::derived_get_task(RobotIntent intent) { current_state_ = new_state; + // SPDLOG_INFO("{}", state_to_name(current_state_)); + // Calculate task based on state return state_to_task(intent); } @@ -23,17 +25,16 @@ Offense::State Offense::next_state() { WorldState* world_state = this->last_world_state_; switch (current_state_) { - case DEFAULT: - + case DEFAULT: { return SEEKING_START; + } - case SEEKING_START: - + case SEEKING_START: { // Unconditionally only stay in this state for one tick. return SEEKING; + } - case SEEKING: - + case SEEKING: { // If the ball seems "stealable", we should switch to STEALING if (can_steal_ball()) { return STEALING; @@ -46,9 +47,8 @@ Offense::State Offense::next_state() { } return SEEKING; - - case POSSESSION_START: - + } + case POSSESSION_START: { // If we can make a shot, take it // If we need to stop possessing now, shoot. if (has_open_shot() || timed_out()) { @@ -60,9 +60,9 @@ Offense::State Offense::next_state() { broadcast_direct_pass_request(); return POSSESSION; + } - case POSSESSION: - + case POSSESSION: { // If we can make a shot, make it. // If we need to stop possessing now, shoot. if (has_open_shot() || timed_out()) { @@ -70,9 +70,9 @@ Offense::State Offense::next_state() { } return POSSESSION; + } - case PASSING: - + case PASSING: { // If we've finished passing, cool! if (check_is_done()) { return DEFAULT; @@ -89,26 +89,32 @@ Offense::State Offense::next_state() { } return PASSING; + } - case STEALING: - + case STEALING: { // Go to possession if successful if (check_is_done()) { - return POSSESSION; + return POSSESSION_START; } - // If we ran out of time or the ball is out of our radius, give up - if (timed_out() || distance_to_ball() > kBallTooFarDist) { + if (timed_out()) { + // If we timed out and the ball is close, assume we have it + // (because is_done for settle/collect are not great) + if (distance_to_ball() < kOwnBallRadius) { + return POSSESSION_START; + } + } else { + // Otherwise, assume we lost it return DEFAULT; } return STEALING; + } - case RECEIVING: - + case RECEIVING: { // If we got it, cool, we have it! if (check_is_done()) { - return POSSESSION; + return POSSESSION_START; } // If we failed to get it in time @@ -116,14 +122,16 @@ Offense::State Offense::next_state() { return DEFAULT; } - case SHOOTING: - + return RECEIVING; + } + case SHOOTING: { // If we either succeed or fail, it's time to start over. if (check_is_done() || timed_out()) { return DEFAULT; } - return DEFAULT; + return SHOOTING; + } } } @@ -145,8 +153,12 @@ std::optional Offense::state_to_task(RobotIntent intent) { return seeker_.get_task(std::move(intent), last_world_state_, field_dimensions_); } + case POSSESSION_START: { + return intent; + } + case POSSESSION: { - return std::nullopt; // TODO + return intent; } case PASSING: { @@ -174,27 +186,55 @@ std::optional Offense::state_to_task(RobotIntent intent) { } case STEALING: { - // Settle/Collect + // intercept the ball + // if ball fast, use settle, otherwise collect + if (last_world_state_->ball.velocity.mag() > 0.75) { + auto settle_cmd = planning::MotionCommand{"settle"}; + intent.motion_command = settle_cmd; + intent.dribbler_speed = 255.0; + } else { + auto collect_cmd = planning::MotionCommand{"collect"}; + intent.motion_command = collect_cmd; + intent.dribbler_speed = 255.0; + } - return std::nullopt; // TODO + return intent; } case RECEIVING: { - // Settle/Collect but like slower + // intercept the ball + // if ball fast, use settle, otherwise collect + if (last_world_state_->ball.velocity.mag() > 0.75) { + auto settle_cmd = planning::MotionCommand{"settle"}; + intent.motion_command = settle_cmd; + intent.dribbler_speed = 255.0; + } else { + auto collect_cmd = planning::MotionCommand{"collect"}; + intent.motion_command = collect_cmd; + intent.dribbler_speed = 255.0; + } - return std::nullopt; + return intent; } case SHOOTING: { - // Line kick + // Line kick best shot + planning::LinearMotionInstant target{calculate_best_shot()}; + auto line_kick_cmd = planning::MotionCommand{"line_kick", target}; + + intent.motion_command = line_kick_cmd; + intent.shoot_mode = RobotIntent::ShootMode::KICK; + intent.trigger_mode = RobotIntent::TriggerMode::ON_BREAK_BEAM; + intent.kick_speed = 4.0; - return std::nullopt; + return intent; } } } communication::PosAgentResponseWrapper Offense::receive_communication_request( communication::AgentPosRequestWrapper request) { + communication::PosAgentResponseWrapper comm_response = Position::receive_communication_request(request); // PassRequests: only in offense right now if (const communication::PassRequest* pass_request = std::get_if(&request.request)) { @@ -221,9 +261,14 @@ communication::PosAgentResponseWrapper Offense::receive_communication_request( // determine when we are open, but this may need to change if (min_robot_dist > max_receive_distance && min_path_dist > max_receive_distance) { communication::PassResponse response = Position::receive_pass_request(*pass_request); - return {response}; - } + // communication::PosAgentResponseWrapper comm_response{}; + + SPDLOG_INFO("Robot {} accepts pass", robot_id_); + comm_response.response = response; + return comm_response; + } + SPDLOG_INFO("Robot {} rejects pass", robot_id_); return {}; } else { // Super: other kinds of requests @@ -299,4 +344,27 @@ double Offense::distance_from_their_robots(rj_geometry::Point tail, rj_geometry: bool Offense::can_steal_ball() const { return distance_to_ball() < kStealBallRadius; } +rj_geometry::Point Offense::calculate_best_shot() const { + // Goal location + rj_geometry::Point their_goal_pos = field_dimensions_.their_goal_loc(); + double goal_width = field_dimensions_.goal_width(); // 1.0 meters + + // Ball location + rj_geometry::Point ball_position = this->last_world_state_->ball.position; + + rj_geometry::Point best_shot = their_goal_pos; + double best_distance = -1.0; + rj_geometry::Point increment(0.05, 0); + rj_geometry::Point curr_point = + their_goal_pos - rj_geometry::Point(goal_width / 2.0, 0) + increment; + for (int i = 0; i < 19; i++) { + double distance = distance_from_their_robots(ball_position, curr_point); + if (distance > best_distance) { + best_distance = distance; + best_shot = curr_point; + } + curr_point = curr_point + increment; + } + return best_shot; +} } // namespace strategy diff --git a/soccer/src/soccer/strategy/agent/position/offense.hpp b/soccer/src/soccer/strategy/agent/position/offense.hpp index e04cd4e941a..f3d1f3eaa59 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/offense.hpp @@ -50,7 +50,7 @@ class Offense : public Position { PASSING, // Getting rid of it STEALING, // Getting the ball RECEIVING, // Getting the ball from a pass - SHOOTING // Winning the game + SHOOTING, // Winning the game }; /** @@ -102,6 +102,31 @@ class Offense : public Position { } } + // For debugging + static constexpr std::string_view state_to_name(State s) { + + switch (s) { + case DEFAULT: + return "DEFAULT"; + case SEEKING_START: + return "SEEKING_START"; + case SEEKING: + return "SEEKING"; + case POSSESSION: + return "POSSESSION"; + case POSSESSION_START: + return "POSSESSION_START"; + case PASSING: + return "PASSING"; + case STEALING: + return "STEALING"; + case RECEIVING: + return "RECEIVING"; + case SHOOTING: + return "SHOOTING"; + } + } + /** * @brief Reset the timeout for the current state */ @@ -141,6 +166,9 @@ class Offense : public Position { // Used to tell if the ball is close enough to steal static constexpr double kStealBallRadius{0.5}; + // Used to assume we are capable of manipulating the ball + static constexpr double kOwnBallRadius{kRobotRadius + 0.1}; + /* Utility functions for State or Task Calculation */ /** @@ -166,6 +194,11 @@ class Offense : public Position { return last_world_state_->ball.position.dist_to( last_world_state_->get_robot(true, robot_id_).pose.position()); }; + + /** + * @return the target (within the goal) that would be the most clear shot + */ + rj_geometry::Point calculate_best_shot() const; }; } // namespace strategy diff --git a/soccer/src/soccer/strategy/agent/position/position.cpp b/soccer/src/soccer/strategy/agent/position/position.cpp index 69436cce6d3..d62fdaefeb8 100644 --- a/soccer/src/soccer/strategy/agent/position/position.cpp +++ b/soccer/src/soccer/strategy/agent/position/position.cpp @@ -105,10 +105,12 @@ void Position::receive_communication_response(communication::AgentPosResponseWra } else if (const communication::PassResponse* pass_response = std::get_if(&response.responses[i])) { // get the associated pass request for this response + SPDLOG_INFO("Robot {} receives pass response", robot_id_); if (const communication::PassRequest* sent_pass_request = std::get_if(&response.associated_request)) { if (sent_pass_request->direct) { // if direct -> pass to first robot + SPDLOG_INFO("Robot {} receives direct pass response", robot_id_); send_pass_confirmation(response.received_robot_ids[i]); } else { // TODO: handle deciding on indirect passing @@ -122,9 +124,9 @@ communication::PosAgentResponseWrapper Position::receive_communication_request( communication::AgentPosRequestWrapper request) { communication::PosAgentResponseWrapper comm_response{}; // if (const communication::PassRequest* pass_request = - // std::get_if(&request.request)) { - // communication::PassResponse pass_response = receive_pass_request(*pass_request); - // comm_response.response = pass_response; + // std::get_if(&request.request)) { + // communication::PassResponse pass_response = receive_pass_request(*pass_request); + // comm_response.response = pass_response; if (const communication::IncomingBallRequest* incoming_ball_request = std::get_if(&request.request)) { communication::Acknowledge incoming_pass_acknowledge = 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 6667368a014..c9962be8d49 100644 --- a/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp +++ b/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp @@ -7,7 +7,7 @@ RobotFactoryPosition::RobotFactoryPosition(int r_id) : Position(r_id) { if (robot_id_ == 0) { current_position_ = std::make_unique(robot_id_); - } else if (robot_id_ == 1) { + } else if (robot_id_ == 1 || robot_id_ == 2) { current_position_ = std::make_unique(robot_id_); } else { current_position_ = std::make_unique(robot_id_); From 1c59fa578c14c5cd560b5feb0c4c36ffce6dfc10 Mon Sep 17 00:00:00 2001 From: p-nayak11 Date: Sun, 4 Feb 2024 21:54:57 -0500 Subject: [PATCH 12/22] virtual --- .../strategy/agent/position/offense.cpp | 4 +- .../strategy/agent/position/position.hpp | 22 ++++----- .../agent/position/robot_factory_position.hpp | 45 +++++++++++++++++++ 3 files changed, 59 insertions(+), 12 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index e7762e13ec8..fa545dbb7b9 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -14,7 +14,9 @@ std::optional Offense::derived_get_task(RobotIntent intent) { current_state_ = new_state; - // SPDLOG_INFO("{}", state_to_name(current_state_)); + if (current_state_ == PASSING || current_state_ == RECEIVING) { + SPDLOG_INFO("{}", state_to_name(current_state_)); + } // Calculate task based on state return state_to_task(intent); diff --git a/soccer/src/soccer/strategy/agent/position/position.hpp b/soccer/src/soccer/strategy/agent/position/position.hpp index f6868c2d3a8..3dec6523084 100644 --- a/soccer/src/soccer/strategy/agent/position/position.hpp +++ b/soccer/src/soccer/strategy/agent/position/position.hpp @@ -70,15 +70,15 @@ class Position { FieldDimensions& field_dimensions); // communication with AC - void update_play_state(const PlayState& play_state); - void update_field_dimensions(const FieldDimensions& field_dimensions); - void update_alive_robots(std::vector alive_robots); - const std::string get_name(); + virtual void update_play_state(const PlayState& play_state); + virtual void update_field_dimensions(const FieldDimensions& field_dimensions); + virtual void update_alive_robots(std::vector alive_robots); + virtual const std::string get_name(); /** * @brief setter for time_left_ */ - void set_time_left(double time_left); + virtual void set_time_left(double time_left); /** * @brief setter for is_done_ @@ -94,7 +94,7 @@ class Position { * Outside classes can only set to true, Position/derived classes can clear * with check_is_done(). */ - void set_goal_canceled(); + virtual void set_goal_canceled(); // Agent-to-Agent communication /** @@ -127,9 +127,9 @@ class Position { * * @param target_robots a vector of robots to send a request to */ - void send_direct_pass_request(std::vector target_robots); + virtual void send_direct_pass_request(std::vector target_robots); - void broadcast_direct_pass_request(); + virtual void broadcast_direct_pass_request(); /** * @brief receives and handles a pass_request @@ -138,14 +138,14 @@ class Position { * @return communication::PassResponse a response to the robot as to whether or not it * is open for a pass */ - communication::PassResponse receive_pass_request(communication::PassRequest pass_request); + virtual communication::PassResponse receive_pass_request(communication::PassRequest pass_request); /** * @brief tell another robot that this robot will pass to it * * @param target_robot the robot that will be passed to */ - void send_pass_confirmation(u_int8_t target_robot); + virtual void send_pass_confirmation(u_int8_t target_robot); /** * @brief acknowledges the pass confirmation from another robot @@ -211,7 +211,7 @@ class Position { /** * @brief setter for goalie id */ - void set_goalie_id(int goalie_id); + virtual void set_goalie_id(int goalie_id); protected: Position(int r_id, std::string position_name); 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 f5a6aefa827..8038df9c523 100644 --- a/soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp +++ b/soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp @@ -58,6 +58,51 @@ class RobotFactoryPosition : public Position { void die() override; void revive() override; + inline void update_play_state(const PlayState& play_state) override { + current_position_->update_play_state(play_state); + } + + inline void update_field_dimensions(const FieldDimensions& field_dimensions) override { + current_position_->update_field_dimensions(field_dimensions); + } + + inline void update_alive_robots(std::vector alive_robots) override { + current_position_->update_alive_robots(alive_robots); + } + + inline const std::string get_name() override { + current_position_->get_name(); + } + + inline void set_time_left(double time_left) override { + current_position_->set_time_left(time_left); + } + + inline void set_goal_canceled() override { + current_position_->set_goal_canceled(); + } + + inline void send_direct_pass_request(std::vector target_robots) override { + current_position_->send_direct_pass_request(target_robots); + } + + inline void broadcast_direct_pass_request() override { + current_position_->broadcast_direct_pass_request(); + } + + inline communication::PassResponse receive_pass_request(communication::PassRequest pass_request) override { + current_position_->receive_pass_request(pass_request); + } + + inline void send_pass_confirmation(u_int8_t target_robot) override { + current_position_->send_pass_confirmation(target_robot); + } + + inline void set_goalie_id(int goalie_id) override { + current_position_->set_goalie_id(goalie_id); + } + + private: std::unique_ptr current_position_; From efdb4b83ed64555947ed344c9335ce9c61759247 Mon Sep 17 00:00:00 2001 From: Sid Parikh Date: Mon, 5 Feb 2024 14:48:36 +0000 Subject: [PATCH 13/22] SHOOTING_START state --- .../strategy/agent/position/offense.cpp | 23 ++++++++++++++++--- .../strategy/agent/position/offense.hpp | 10 +++++++- 2 files changed, 29 insertions(+), 4 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index fa545dbb7b9..4eefd274adf 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -126,6 +126,11 @@ Offense::State Offense::next_state() { return RECEIVING; } + + case SHOOTING_START: { + return SHOOTING; + } + case SHOOTING: { // If we either succeed or fail, it's time to start over. if (check_is_done() || timed_out()) { @@ -219,10 +224,22 @@ std::optional Offense::state_to_task(RobotIntent intent) { return intent; } - case SHOOTING: { + case SHOOTING_START: { // Line kick best shot - planning::LinearMotionInstant target{calculate_best_shot()}; - auto line_kick_cmd = planning::MotionCommand{"line_kick", target}; + target_ = calculate_best_shot(); + + auto line_kick_cmd = planning::MotionCommand{"line_kick", planning::LinearMotionInstant{target_}}; + + intent.motion_command = line_kick_cmd; + intent.shoot_mode = RobotIntent::ShootMode::KICK; + intent.trigger_mode = RobotIntent::TriggerMode::ON_BREAK_BEAM; + intent.kick_speed = 4.0; + + return intent; + } + + case SHOOTING: { + auto line_kick_cmd = planning::MotionCommand{"line_kick", planning::LinearMotionInstant{target_}}; intent.motion_command = line_kick_cmd; intent.shoot_mode = RobotIntent::ShootMode::KICK; diff --git a/soccer/src/soccer/strategy/agent/position/offense.hpp b/soccer/src/soccer/strategy/agent/position/offense.hpp index f3d1f3eaa59..1160f1a92f9 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/offense.hpp @@ -50,7 +50,8 @@ class Offense : public Position { PASSING, // Getting rid of it STEALING, // Getting the ball RECEIVING, // Getting the ball from a pass - SHOOTING, // Winning the game + SHOOTING_START, // Calculate shot + SHOOTING, // Winning the game }; /** @@ -97,6 +98,8 @@ class Offense : public Position { return RJ::Seconds{5}; case RECEIVING: return RJ::Seconds{5}; + case SHOOTING_START: + return RJ::Seconds{-1}; case SHOOTING: return RJ::Seconds{5}; } @@ -122,6 +125,8 @@ class Offense : public Position { return "STEALING"; case RECEIVING: return "RECEIVING"; + case SHOOTING_START: + return "SHOOTING"; case SHOOTING: return "SHOOTING"; } @@ -155,6 +160,9 @@ class Offense : public Position { /* RoleInterface Members */ Seeker seeker_; + // Used to cache targets between states + rj_geometry::Point target_; + /* Constants for State or Task Calculation */ // These variables are for calculating ball speed when passing From 708e5af69ef5d8bd53691e33fdfb17ab484256b2 Mon Sep 17 00:00:00 2001 From: Sid Parikh Date: Mon, 5 Feb 2024 16:38:14 +0000 Subject: [PATCH 14/22] i forget --- .../strategy/agent/position/offense.cpp | 107 +++++++++--------- .../strategy/agent/position/offense.hpp | 9 +- .../strategy/agent/position/position.cpp | 31 +++-- .../strategy/agent/position/position.hpp | 3 +- .../agent/position/robot_factory_position.hpp | 12 +- 5 files changed, 88 insertions(+), 74 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 4eefd274adf..fa4db156e1b 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -10,13 +10,12 @@ std::optional Offense::derived_get_task(RobotIntent intent) { if (current_state_ != new_state) { reset_timeout(); + SPDLOG_INFO("Robot {}: now {}", robot_id_, state_to_name(current_state_)); } + current_state_ = new_state; - if (current_state_ == PASSING || current_state_ == RECEIVING) { - SPDLOG_INFO("{}", state_to_name(current_state_)); - } // Calculate task based on state return state_to_task(intent); @@ -24,8 +23,6 @@ std::optional Offense::derived_get_task(RobotIntent intent) { Offense::State Offense::next_state() { // handle transitions between current state - WorldState* world_state = this->last_world_state_; - switch (current_state_) { case DEFAULT: { return SEEKING_START; @@ -54,7 +51,7 @@ Offense::State Offense::next_state() { // If we can make a shot, take it // If we need to stop possessing now, shoot. if (has_open_shot() || timed_out()) { - return SHOOTING; + return SHOOTING_START; } // No open shot, try to pass. @@ -68,7 +65,7 @@ Offense::State Offense::next_state() { // If we can make a shot, make it. // If we need to stop possessing now, shoot. if (has_open_shot() || timed_out()) { - return SHOOTING; + return SHOOTING_START; } return POSSESSION; @@ -82,7 +79,7 @@ Offense::State Offense::next_state() { // If we didn't successfully pass in time, take a shot if (timed_out()) { - return SHOOTING; + return SHOOTING_START; } // If we lost the ball completely, give up @@ -228,7 +225,8 @@ std::optional Offense::state_to_task(RobotIntent intent) { // Line kick best shot target_ = calculate_best_shot(); - auto line_kick_cmd = planning::MotionCommand{"line_kick", planning::LinearMotionInstant{target_}}; + auto line_kick_cmd = + planning::MotionCommand{"line_kick", planning::LinearMotionInstant{target_}}; intent.motion_command = line_kick_cmd; intent.shoot_mode = RobotIntent::ShootMode::KICK; @@ -239,7 +237,8 @@ std::optional Offense::state_to_task(RobotIntent intent) { } case SHOOTING: { - auto line_kick_cmd = planning::MotionCommand{"line_kick", planning::LinearMotionInstant{target_}}; + auto line_kick_cmd = + planning::MotionCommand{"line_kick", planning::LinearMotionInstant{target_}}; intent.motion_command = line_kick_cmd; intent.shoot_mode = RobotIntent::ShootMode::KICK; @@ -251,49 +250,51 @@ std::optional Offense::state_to_task(RobotIntent intent) { } } -communication::PosAgentResponseWrapper Offense::receive_communication_request( - communication::AgentPosRequestWrapper request) { - communication::PosAgentResponseWrapper comm_response = Position::receive_communication_request(request); - // PassRequests: only in offense right now - if (const communication::PassRequest* pass_request = - std::get_if(&request.request)) { - // If the robot recieves a PassRequest, only process it if we are oppen - - rj_geometry::Point robot_position = - last_world_state_->get_robot(true, robot_id_).pose.position(); - rj_geometry::Point from_robot_position = - last_world_state_->get_robot(true, pass_request->from_robot_id).pose.position(); - rj_geometry::Segment pass_path{from_robot_position, robot_position}; - double min_robot_dist = 10000; - float min_path_dist = 10000; - - // Calculates the minimum distance from the current robot to all other robots - // Also calculates the minimum distance from another robot to the passing line - for (auto bot : last_world_state_->their_robots) { - rj_geometry::Point opp_pos = bot.pose.position(); - min_robot_dist = std::min(min_robot_dist, robot_position.dist_to(opp_pos)); - min_path_dist = std::min(min_path_dist, pass_path.dist_to(opp_pos)); - } - - // If the current robot is far enough away from other robots and there are no other robots - // in the passing line, process the request Currently, max_receive_distance is used to - // determine when we are open, but this may need to change - if (min_robot_dist > max_receive_distance && min_path_dist > max_receive_distance) { - communication::PassResponse response = Position::receive_pass_request(*pass_request); - // communication::PosAgentResponseWrapper comm_response{}; - - SPDLOG_INFO("Robot {} accepts pass", robot_id_); - - comm_response.response = response; - return comm_response; - } - SPDLOG_INFO("Robot {} rejects pass", robot_id_); - return {}; - } else { - // Super: other kinds of requests - return Position::receive_communication_request(request); - } -} +// communication::PosAgentResponseWrapper Offense::receive_communication_request( +// communication::AgentPosRequestWrapper request) { +// communication::PosAgentResponseWrapper comm_response = +// Position::receive_communication_request(request); +// // PassRequests: only in offense right now +// if (const communication::PassRequest* pass_request = +// std::get_if(&request.request)) { +// // If the robot recieves a PassRequest, only process it if we are oppen + +// rj_geometry::Point robot_position = +// last_world_state_->get_robot(true, robot_id_).pose.position(); +// rj_geometry::Point from_robot_position = +// last_world_state_->get_robot(true, pass_request->from_robot_id).pose.position(); +// rj_geometry::Segment pass_path{from_robot_position, robot_position}; +// double min_robot_dist = 10000; +// float min_path_dist = 10000; + +// // Calculates the minimum distance from the current robot to all other robots +// // Also calculates the minimum distance from another robot to the passing line +// for (auto bot : last_world_state_->their_robots) { +// rj_geometry::Point opp_pos = bot.pose.position(); +// min_robot_dist = std::min(min_robot_dist, robot_position.dist_to(opp_pos)); +// min_path_dist = std::min(min_path_dist, pass_path.dist_to(opp_pos)); +// } + +// // If the current robot is far enough away from other robots and there are no other +// robots +// // in the passing line, process the request Currently, max_receive_distance is used to +// // determine when we are open, but this may need to change +// if (min_robot_dist > max_receive_distance && min_path_dist > max_receive_distance) { +// communication::PassResponse response = Position::receive_pass_request(*pass_request); +// // communication::PosAgentResponseWrapper comm_response{}; + +// SPDLOG_INFO("Robot {} accepts pass", robot_id_); + +// comm_response.response = response; +// return comm_response; +// } +// SPDLOG_INFO("Robot {} rejects pass", robot_id_); +// return {}; +// } else { +// // Super: other kinds of requests +// return Position::receive_communication_request(request); +// } +// } void Offense::derived_acknowledge_pass() { // I have been chosen as the receiver diff --git a/soccer/src/soccer/strategy/agent/position/offense.hpp b/soccer/src/soccer/strategy/agent/position/offense.hpp index 1160f1a92f9..0e90741adb7 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/offense.hpp @@ -28,8 +28,8 @@ class Offense : public Position { Offense(int r_id); ~Offense() override = default; - communication::PosAgentResponseWrapper receive_communication_request( - communication::AgentPosRequestWrapper request) override; + // communication::PosAgentResponseWrapper receive_communication_request( + // communication::AgentPosRequestWrapper request) override; void derived_acknowledge_pass() override; void derived_pass_ball() override; @@ -101,13 +101,12 @@ class Offense : public Position { case SHOOTING_START: return RJ::Seconds{-1}; case SHOOTING: - return RJ::Seconds{5}; + return RJ::Seconds{-1}; } } // For debugging static constexpr std::string_view state_to_name(State s) { - switch (s) { case DEFAULT: return "DEFAULT"; @@ -205,7 +204,7 @@ class Offense : public Position { /** * @return the target (within the goal) that would be the most clear shot - */ + */ rj_geometry::Point calculate_best_shot() const; }; diff --git a/soccer/src/soccer/strategy/agent/position/position.cpp b/soccer/src/soccer/strategy/agent/position/position.cpp index d62fdaefeb8..f46ce1455d3 100644 --- a/soccer/src/soccer/strategy/agent/position/position.cpp +++ b/soccer/src/soccer/strategy/agent/position/position.cpp @@ -94,11 +94,15 @@ std::optional Position::send_communicatio void Position::receive_communication_response(communication::AgentPosResponseWrapper response) { for (u_int32_t i = 0; i < response.responses.size(); i++) { + + + if (const communication::Acknowledge* acknowledge = std::get_if(&response.responses[i])) { // if the acknowledgement is from an incoming pass request -> pass the ball if (const communication::IncomingBallRequest* incoming_ball_request = std::get_if(&response.associated_request)) { + SPDLOG_INFO("Robot {} received incoming ball request", robot_id_); pass_ball(response.received_robot_ids[i]); } @@ -110,10 +114,11 @@ void Position::receive_communication_response(communication::AgentPosResponseWra std::get_if(&response.associated_request)) { if (sent_pass_request->direct) { // if direct -> pass to first robot - SPDLOG_INFO("Robot {} receives direct pass response", robot_id_); + SPDLOG_INFO("Robot {} is sending a pass confirmation", robot_id_); send_pass_confirmation(response.received_robot_ids[i]); } else { // TODO: handle deciding on indirect passing + SPDLOG_WARN("Robot {} received an INDIRECT pass request", robot_id_); } } } @@ -123,19 +128,31 @@ void Position::receive_communication_response(communication::AgentPosResponseWra communication::PosAgentResponseWrapper Position::receive_communication_request( communication::AgentPosRequestWrapper request) { communication::PosAgentResponseWrapper comm_response{}; - // if (const communication::PassRequest* pass_request = - // std::get_if(&request.request)) { - // communication::PassResponse pass_response = receive_pass_request(*pass_request); - // comm_response.response = pass_response; - if (const communication::IncomingBallRequest* incoming_ball_request = - std::get_if(&request.request)) { + if (const communication::PassRequest* pass_request = + std::get_if(&request.request)) { + + if (robot_id_ == 2) { + SPDLOG_INFO("Robot 2 accepts pass"); + communication::PassResponse pass_response = receive_pass_request(*pass_request); + comm_response.response = pass_response; + } else { + SPDLOG_INFO("Robot {} rejects pass", robot_id_); + communication::PassResponse pass_response = receive_pass_request(*pass_request); + pass_response.direct_open = false; + comm_response.response = pass_response; + } + + } else if (const communication::IncomingBallRequest* incoming_ball_request = + std::get_if(&request.request)) { communication::Acknowledge incoming_pass_acknowledge = acknowledge_pass(*incoming_ball_request); + SPDLOG_INFO("Robot {} acknowledges incoming ball request", robot_id_); comm_response.response = incoming_pass_acknowledge; } else if (const communication::BallInTransitRequest* ball_in_transit_request = std::get_if(&request.request)) { communication::Acknowledge ball_in_transit_acknowledge = acknowledge_ball_in_transit(*ball_in_transit_request); + SPDLOG_INFO("Robot {} acknowledges ball in transit request", robot_id_); comm_response.response = ball_in_transit_acknowledge; } else { communication::Acknowledge acknowledge{}; diff --git a/soccer/src/soccer/strategy/agent/position/position.hpp b/soccer/src/soccer/strategy/agent/position/position.hpp index 3dec6523084..bdd62474361 100644 --- a/soccer/src/soccer/strategy/agent/position/position.hpp +++ b/soccer/src/soccer/strategy/agent/position/position.hpp @@ -138,7 +138,8 @@ class Position { * @return communication::PassResponse a response to the robot as to whether or not it * is open for a pass */ - virtual communication::PassResponse receive_pass_request(communication::PassRequest pass_request); + virtual communication::PassResponse receive_pass_request( + communication::PassRequest pass_request); /** * @brief tell another robot that this robot will pass to it 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 8038df9c523..e4250adc1e3 100644 --- a/soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp +++ b/soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp @@ -70,17 +70,13 @@ class RobotFactoryPosition : public Position { current_position_->update_alive_robots(alive_robots); } - inline const std::string get_name() override { - current_position_->get_name(); - } + inline const std::string get_name() override { current_position_->get_name(); } inline void set_time_left(double time_left) override { current_position_->set_time_left(time_left); } - inline void set_goal_canceled() override { - current_position_->set_goal_canceled(); - } + inline void set_goal_canceled() override { current_position_->set_goal_canceled(); } inline void send_direct_pass_request(std::vector target_robots) override { current_position_->send_direct_pass_request(target_robots); @@ -90,7 +86,8 @@ class RobotFactoryPosition : public Position { current_position_->broadcast_direct_pass_request(); } - inline communication::PassResponse receive_pass_request(communication::PassRequest pass_request) override { + inline communication::PassResponse receive_pass_request( + communication::PassRequest pass_request) override { current_position_->receive_pass_request(pass_request); } @@ -102,7 +99,6 @@ class RobotFactoryPosition : public Position { current_position_->set_goalie_id(goalie_id); } - private: std::unique_ptr current_position_; From 419959b41d0bd36910dde541afc445974fa9fa6a Mon Sep 17 00:00:00 2001 From: p-nayak11 Date: Tue, 6 Feb 2024 22:08:57 -0500 Subject: [PATCH 15/22] wip --- makefile | 6 +- .../strategy/agent/position/offense.cpp | 103 +++++++++--------- .../strategy/agent/position/offense.hpp | 4 +- .../strategy/agent/position/position.cpp | 30 ++--- 4 files changed, 66 insertions(+), 77 deletions(-) diff --git a/makefile b/makefile index 3f19c748fc4..1e4443819e5 100644 --- a/makefile +++ b/makefile @@ -72,7 +72,7 @@ run-sim: # run our stack with default flags # TODO: actually name our software stack something run-our-stack: - ros2 launch rj_robocup soccer.launch.py run_sim:=True + ROS_LOCALHOST_ONLY=1 ros2 launch rj_robocup soccer.launch.py run_sim:=True # run sim with external referee (SSL Game Controller) run-sim-external: @@ -86,7 +86,7 @@ run-real: # run on real field computer with real robots and external ref (SSL GC) run-real-ex: - ros2 launch rj_robocup soccer.launch.py run_sim:=False use_sim_radio:=False use_internal_ref:=False + ros2 launch rj_robocup soccer.launch.py run_sim:=False use_sim_radio:=False use_internal_ref:=False # run on real field comp, with real robots and manual control node to override AI movement # use util/manual_control_connect.bash to connect @@ -104,7 +104,7 @@ run-sim2play: run-sim2: run-sim2play # control two teams of robots at once on the field -# +# # as of 9/22/22, hardcoded for one team's server port=25565 (the one-team # default), other=25564 run-real2play: diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index fa4db156e1b..761a6c076c8 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -10,13 +10,11 @@ std::optional Offense::derived_get_task(RobotIntent intent) { if (current_state_ != new_state) { reset_timeout(); - SPDLOG_INFO("Robot {}: now {}", robot_id_, state_to_name(current_state_)); + // SPDLOG_INFO("Robot {}: now {}", robot_id_, state_to_name(current_state_)); } - current_state_ = new_state; - // Calculate task based on state return state_to_task(intent); } @@ -101,10 +99,9 @@ Offense::State Offense::next_state() { // (because is_done for settle/collect are not great) if (distance_to_ball() < kOwnBallRadius) { return POSSESSION_START; + } else { + return DEFAULT; } - } else { - // Otherwise, assume we lost it - return DEFAULT; } return STEALING; @@ -250,51 +247,55 @@ std::optional Offense::state_to_task(RobotIntent intent) { } } -// communication::PosAgentResponseWrapper Offense::receive_communication_request( -// communication::AgentPosRequestWrapper request) { -// communication::PosAgentResponseWrapper comm_response = -// Position::receive_communication_request(request); -// // PassRequests: only in offense right now -// if (const communication::PassRequest* pass_request = -// std::get_if(&request.request)) { -// // If the robot recieves a PassRequest, only process it if we are oppen - -// rj_geometry::Point robot_position = -// last_world_state_->get_robot(true, robot_id_).pose.position(); -// rj_geometry::Point from_robot_position = -// last_world_state_->get_robot(true, pass_request->from_robot_id).pose.position(); -// rj_geometry::Segment pass_path{from_robot_position, robot_position}; -// double min_robot_dist = 10000; -// float min_path_dist = 10000; - -// // Calculates the minimum distance from the current robot to all other robots -// // Also calculates the minimum distance from another robot to the passing line -// for (auto bot : last_world_state_->their_robots) { -// rj_geometry::Point opp_pos = bot.pose.position(); -// min_robot_dist = std::min(min_robot_dist, robot_position.dist_to(opp_pos)); -// min_path_dist = std::min(min_path_dist, pass_path.dist_to(opp_pos)); -// } - -// // If the current robot is far enough away from other robots and there are no other -// robots -// // in the passing line, process the request Currently, max_receive_distance is used to -// // determine when we are open, but this may need to change -// if (min_robot_dist > max_receive_distance && min_path_dist > max_receive_distance) { -// communication::PassResponse response = Position::receive_pass_request(*pass_request); -// // communication::PosAgentResponseWrapper comm_response{}; - -// SPDLOG_INFO("Robot {} accepts pass", robot_id_); - -// comm_response.response = response; -// return comm_response; -// } -// SPDLOG_INFO("Robot {} rejects pass", robot_id_); -// return {}; -// } else { -// // Super: other kinds of requests -// return Position::receive_communication_request(request); -// } -// } +bool Offense::check_if_open(int target_robot_shell) { + rj_geometry::Point robot_position = + last_world_state_->get_robot(true, robot_id_).pose.position(); + rj_geometry::Point from_robot_position = + last_world_state_->get_robot(true, target_robot_shell).pose.position(); + rj_geometry::Segment pass_path{from_robot_position, robot_position}; + double min_robot_dist = 10000; + float min_path_dist = 10000; + + // Calculates the minimum distance from the current robot to all other robots + // Also calculates the minimum distance from another robot to the passing line + for (auto bot : last_world_state_->their_robots) { + rj_geometry::Point opp_pos = bot.pose.position(); + min_robot_dist = std::min(min_robot_dist, robot_position.dist_to(opp_pos)); + min_path_dist = std::min(min_path_dist, pass_path.dist_to(opp_pos)); + } + + // If the current robot is far enough away from other robots and there + // are no other robots + // in the passing line, process the request Currently, max_receive_distance is used to + // determine when we are open, but this may need to change + return (min_robot_dist > max_receive_distance && min_path_dist > max_receive_distance) +} + +communication::PosAgentResponseWrapper Offense::receive_communication_request( + communication::AgentPosRequestWrapper request) { + communication::PosAgentResponseWrapper comm_response = + Position::receive_communication_request(request); + + + + // PassRequests: only in offense right now + if (const communication::PassRequest* pass_request = + std::get_if(&request.request)) { + // If the robot recieves a PassRequest, only process it if we are open + + if (check_is_open()) + response.direct_open = true; + // communication::PosAgentResponseWrapper comm_response{}; + + SPDLOG_INFO("Robot {} accepts pass", robot_id_); + + comm_response.response = response; + return comm_response; + } + SPDLOG_INFO("Robot {} rejects pass", robot_id_); + return {}; + } +} void Offense::derived_acknowledge_pass() { // I have been chosen as the receiver diff --git a/soccer/src/soccer/strategy/agent/position/offense.hpp b/soccer/src/soccer/strategy/agent/position/offense.hpp index 0e90741adb7..54ba5143bc1 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/offense.hpp @@ -28,8 +28,8 @@ class Offense : public Position { Offense(int r_id); ~Offense() override = default; - // communication::PosAgentResponseWrapper receive_communication_request( - // communication::AgentPosRequestWrapper request) override; + communication::PosAgentResponseWrapper receive_communication_request( + communication::AgentPosRequestWrapper request) override; void derived_acknowledge_pass() override; void derived_pass_ball() override; diff --git a/soccer/src/soccer/strategy/agent/position/position.cpp b/soccer/src/soccer/strategy/agent/position/position.cpp index f46ce1455d3..2c8207d3e59 100644 --- a/soccer/src/soccer/strategy/agent/position/position.cpp +++ b/soccer/src/soccer/strategy/agent/position/position.cpp @@ -74,7 +74,6 @@ void Position::update_alive_robots(std::vector alive_robots) { } } - bool Position::assert_world_state_valid() { if (last_world_state_ == nullptr) { SPDLOG_WARN("WorldState!"); @@ -94,9 +93,6 @@ std::optional Position::send_communicatio void Position::receive_communication_response(communication::AgentPosResponseWrapper response) { for (u_int32_t i = 0; i < response.responses.size(); i++) { - - - if (const communication::Acknowledge* acknowledge = std::get_if(&response.responses[i])) { // if the acknowledgement is from an incoming pass request -> pass the ball @@ -112,13 +108,13 @@ void Position::receive_communication_response(communication::AgentPosResponseWra SPDLOG_INFO("Robot {} receives pass response", robot_id_); if (const communication::PassRequest* sent_pass_request = std::get_if(&response.associated_request)) { - if (sent_pass_request->direct) { + + SPDLOG_INFO("Robot {} found associated request from {}: direct: {}, direct_open: {}", robot_id_, response.received_robot_ids[i], sent_pass_request->direct, pass_response->direct_open); + + if (sent_pass_request->direct && pass_response->direct_open) { // if direct -> pass to first robot SPDLOG_INFO("Robot {} is sending a pass confirmation", robot_id_); send_pass_confirmation(response.received_robot_ids[i]); - } else { - // TODO: handle deciding on indirect passing - SPDLOG_WARN("Robot {} received an INDIRECT pass request", robot_id_); } } } @@ -130,29 +126,21 @@ communication::PosAgentResponseWrapper Position::receive_communication_request( communication::PosAgentResponseWrapper comm_response{}; if (const communication::PassRequest* pass_request = std::get_if(&request.request)) { - - if (robot_id_ == 2) { - SPDLOG_INFO("Robot 2 accepts pass"); - communication::PassResponse pass_response = receive_pass_request(*pass_request); - comm_response.response = pass_response; - } else { - SPDLOG_INFO("Robot {} rejects pass", robot_id_); - communication::PassResponse pass_response = receive_pass_request(*pass_request); - pass_response.direct_open = false; - comm_response.response = pass_response; - } + communication::PassResponse pass_response = receive_pass_request(*pass_request); + pass_response.direct_open = false; + comm_response.response = pass_response; } else if (const communication::IncomingBallRequest* incoming_ball_request = std::get_if(&request.request)) { communication::Acknowledge incoming_pass_acknowledge = acknowledge_pass(*incoming_ball_request); - SPDLOG_INFO("Robot {} acknowledges incoming ball request", robot_id_); + SPDLOG_INFO("Robot {} acknowledges incoming ball request", robot_id_); comm_response.response = incoming_pass_acknowledge; } else if (const communication::BallInTransitRequest* ball_in_transit_request = std::get_if(&request.request)) { communication::Acknowledge ball_in_transit_acknowledge = acknowledge_ball_in_transit(*ball_in_transit_request); - SPDLOG_INFO("Robot {} acknowledges ball in transit request", robot_id_); + SPDLOG_INFO("Robot {} acknowledges ball in transit request", robot_id_); comm_response.response = ball_in_transit_acknowledge; } else { communication::Acknowledge acknowledge{}; From 2d9e6a742e86e5bb7c1a88450478192c89e44c65 Mon Sep 17 00:00:00 2001 From: Sid Parikh Date: Sat, 10 Feb 2024 22:17:28 +0000 Subject: [PATCH 16/22] compile error? --- .../strategy/agent/position/offense.cpp | 21 ++++++++----------- .../strategy/agent/position/offense.hpp | 5 +++++ .../strategy/agent/position/position.cpp | 8 ++++--- 3 files changed, 19 insertions(+), 15 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 761a6c076c8..c96051ea8ff 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -268,7 +268,7 @@ bool Offense::check_if_open(int target_robot_shell) { // are no other robots // in the passing line, process the request Currently, max_receive_distance is used to // determine when we are open, but this may need to change - return (min_robot_dist > max_receive_distance && min_path_dist > max_receive_distance) + return (min_robot_dist > max_receive_distance && min_path_dist > max_receive_distance); } communication::PosAgentResponseWrapper Offense::receive_communication_request( @@ -276,25 +276,22 @@ communication::PosAgentResponseWrapper Offense::receive_communication_request( communication::PosAgentResponseWrapper comm_response = Position::receive_communication_request(request); - - // PassRequests: only in offense right now if (const communication::PassRequest* pass_request = std::get_if(&request.request)) { // If the robot recieves a PassRequest, only process it if we are open - if (check_is_open()) - response.direct_open = true; - // communication::PosAgentResponseWrapper comm_response{}; + auto response = Position::receive_pass_request(*pass_request); - SPDLOG_INFO("Robot {} accepts pass", robot_id_); + if (check_if_open(pass_request->from_robot_id)) response.direct_open = true; - comm_response.response = response; - return comm_response; - } - SPDLOG_INFO("Robot {} rejects pass", robot_id_); - return {}; + SPDLOG_INFO("Robot {} accepts pass", robot_id_); + + comm_response.response = response; + return comm_response; } + + return comm_response; } void Offense::derived_acknowledge_pass() { diff --git a/soccer/src/soccer/strategy/agent/position/offense.hpp b/soccer/src/soccer/strategy/agent/position/offense.hpp index 54ba5143bc1..bec119580a0 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/offense.hpp @@ -202,6 +202,11 @@ class Offense : public Position { last_world_state_->get_robot(true, robot_id_).pose.position()); }; + /** + * @return if this agent is open to receive a pass + * @param target_robot_shell the robot shell to check if open + */ + bool check_if_open(int target_robot_shell); /** * @return the target (within the goal) that would be the most clear shot */ diff --git a/soccer/src/soccer/strategy/agent/position/position.cpp b/soccer/src/soccer/strategy/agent/position/position.cpp index 2c8207d3e59..ab0c44889dd 100644 --- a/soccer/src/soccer/strategy/agent/position/position.cpp +++ b/soccer/src/soccer/strategy/agent/position/position.cpp @@ -108,8 +108,10 @@ void Position::receive_communication_response(communication::AgentPosResponseWra SPDLOG_INFO("Robot {} receives pass response", robot_id_); if (const communication::PassRequest* sent_pass_request = std::get_if(&response.associated_request)) { - - SPDLOG_INFO("Robot {} found associated request from {}: direct: {}, direct_open: {}", robot_id_, response.received_robot_ids[i], sent_pass_request->direct, pass_response->direct_open); + SPDLOG_INFO( + "Robot {} found associated request from {}: direct: {}, direct_open: {}", + robot_id_, response.received_robot_ids[i], sent_pass_request->direct, + pass_response->direct_open); if (sent_pass_request->direct && pass_response->direct_open) { // if direct -> pass to first robot @@ -175,7 +177,7 @@ void Position::broadcast_direct_pass_request() { communication::PosAgentRequestWrapper communication_request{}; communication_request.request = pass_request; - communication_request.urgent = true; + communication_request.urgent = false; communication_request.broadcast = true; communication_request_ = communication_request; } From a34efff5c4fa9d6c19a04ac7a56103786b8bc35d Mon Sep 17 00:00:00 2001 From: Sid Parikh Date: Mon, 12 Feb 2024 15:30:10 -0500 Subject: [PATCH 17/22] fix debug flags --- CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index cdabdeff20c..a04557178f1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -76,9 +76,9 @@ endif() # Use compiler optimization if we are making a release build. set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O2 -march=native") -set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -g -Og") +set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -Og") set(CMAKE_CXX_FLAGS_DEBUG - "${CMAKE_CXX_FLAGS_DEBUG} -Werror=return-type -Werror=delete-non-virtual-dtor") + "${CMAKE_CXX_FLAGS_DEBUG} -Werror=return-type -Werror=delete-non-virtual-dtor -Werror=switch") # ====================================================================== # Testing From 57feb2d6a962f2c9a83663ff268d34ade9b4718f Mon Sep 17 00:00:00 2001 From: Sid Parikh Date: Mon, 12 Feb 2024 15:31:20 -0500 Subject: [PATCH 18/22] settle/collect understand ref state --- .../src/soccer/planning/planner/collect_path_planner.cpp | 7 +++++++ .../src/soccer/planning/planner/settle_path_planner.cpp | 8 ++++++++ 2 files changed, 15 insertions(+) diff --git a/soccer/src/soccer/planning/planner/collect_path_planner.cpp b/soccer/src/soccer/planning/planner/collect_path_planner.cpp index 42449537f11..80cc169ca25 100644 --- a/soccer/src/soccer/planning/planner/collect_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/collect_path_planner.cpp @@ -14,6 +14,13 @@ using namespace rj_geometry; namespace planning { Trajectory CollectPathPlanner::plan(const PlanRequest& plan_request) { + + const auto state = plan_request.play_state.state(); + if (state == PlayState::Stop || state == PlayState::Halt) { + // This planner automatically fails if the robot is prohibited from touching the ball. + return Trajectory{}; + } + BallState ball = plan_request.world_state->ball; const RJ::Time cur_time = plan_request.start.stamp; diff --git a/soccer/src/soccer/planning/planner/settle_path_planner.cpp b/soccer/src/soccer/planning/planner/settle_path_planner.cpp index 6fbeb72b49f..65749ae7d2b 100644 --- a/soccer/src/soccer/planning/planner/settle_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/settle_path_planner.cpp @@ -19,6 +19,13 @@ using namespace rj_geometry; namespace planning { Trajectory SettlePathPlanner::plan(const PlanRequest& plan_request) { + + const auto state = plan_request.play_state.state(); + if (state == PlayState::Stop || state == PlayState::Halt) { + // This planner automatically fails if the robot is prohibited from touching the ball. + return Trajectory{}; + } + BallState ball = plan_request.world_state->ball; const RJ::Time cur_time = plan_request.start.stamp; @@ -460,6 +467,7 @@ Trajectory SettlePathPlanner::dampen(const PlanRequest& plan_request, RobotInsta dampen_end.set_debug_text("Damping"); if (!previous_.empty()) { + SPDLOG_INFO("stamp {} {}",previous_.last().stamp.time_since_epoch().count(), dampen_end.first().stamp.time_since_epoch().count()); dampen_end = Trajectory(previous_, dampen_end); } From dc54225cfa8f640e9fad513bdf2e1144cb638dbb Mon Sep 17 00:00:00 2001 From: Sid Parikh Date: Mon, 12 Feb 2024 15:31:45 -0500 Subject: [PATCH 19/22] remove redudandant inlines --- .../agent/position/robot_factory_position.hpp | 28 ++++++++----------- 1 file changed, 12 insertions(+), 16 deletions(-) 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 e4250adc1e3..c60b80c013b 100644 --- a/soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp +++ b/soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp @@ -58,46 +58,42 @@ class RobotFactoryPosition : public Position { void die() override; void revive() override; - inline void update_play_state(const PlayState& play_state) override { + void update_play_state(const PlayState& play_state) override { current_position_->update_play_state(play_state); } - inline void update_field_dimensions(const FieldDimensions& field_dimensions) override { + void update_field_dimensions(const FieldDimensions& field_dimensions) override { current_position_->update_field_dimensions(field_dimensions); } - inline void update_alive_robots(std::vector alive_robots) override { + void update_alive_robots(std::vector alive_robots) override { current_position_->update_alive_robots(alive_robots); } - inline const std::string get_name() override { current_position_->get_name(); } + const std::string get_name() override { return current_position_->get_name(); } - inline void set_time_left(double time_left) override { - current_position_->set_time_left(time_left); - } + void set_time_left(double time_left) override { current_position_->set_time_left(time_left); } - inline void set_goal_canceled() override { current_position_->set_goal_canceled(); } + void set_goal_canceled() override { current_position_->set_goal_canceled(); } - inline void send_direct_pass_request(std::vector target_robots) override { + void send_direct_pass_request(std::vector target_robots) override { current_position_->send_direct_pass_request(target_robots); } - inline void broadcast_direct_pass_request() override { + void broadcast_direct_pass_request() override { current_position_->broadcast_direct_pass_request(); } - inline communication::PassResponse receive_pass_request( + communication::PassResponse receive_pass_request( communication::PassRequest pass_request) override { - current_position_->receive_pass_request(pass_request); + return current_position_->receive_pass_request(pass_request); } - inline void send_pass_confirmation(u_int8_t target_robot) override { + void send_pass_confirmation(u_int8_t target_robot) override { current_position_->send_pass_confirmation(target_robot); } - inline void set_goalie_id(int goalie_id) override { - current_position_->set_goalie_id(goalie_id); - } + void set_goalie_id(int goalie_id) override { current_position_->set_goalie_id(goalie_id); } private: std::unique_ptr current_position_; From 73efb0fa416157adc6505ff65a98732d9d382daa Mon Sep 17 00:00:00 2001 From: Sid Parikh Date: Mon, 12 Feb 2024 15:32:26 -0500 Subject: [PATCH 20/22] add RECIEVING_START; use collect not settle --- .../strategy/agent/position/offense.cpp | 48 +++++++++++++------ .../strategy/agent/position/offense.hpp | 7 ++- 2 files changed, 40 insertions(+), 15 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index c96051ea8ff..64ba013b7b2 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -121,6 +121,14 @@ Offense::State Offense::next_state() { return RECEIVING; } + case RECEIVING_START: { + // Stay in this state until either: + // a) Incoming Ball Request received + // b) Timed out + // both of which are handled in other member functions + return RECEIVING_START; + } + case SHOOTING_START: { return SHOOTING; } @@ -189,15 +197,27 @@ std::optional Offense::state_to_task(RobotIntent intent) { case STEALING: { // intercept the ball // if ball fast, use settle, otherwise collect - if (last_world_state_->ball.velocity.mag() > 0.75) { - auto settle_cmd = planning::MotionCommand{"settle"}; - intent.motion_command = settle_cmd; - intent.dribbler_speed = 255.0; - } else { + // if (last_world_state_->ball.velocity.mag() > 0.75) { + // auto settle_cmd = planning::MotionCommand{"settle"}; + // intent.motion_command = settle_cmd; + // intent.dribbler_speed = 255.0; + // } else { auto collect_cmd = planning::MotionCommand{"collect"}; intent.motion_command = collect_cmd; intent.dribbler_speed = 255.0; - } + // } + + return intent; + } + + case RECEIVING_START: { + // Turn to face the ball + + auto current_pos = last_world_state_->get_robot(true, robot_id_).pose.position(); + + planning::LinearMotionInstant stay_in_place {current_pos}; + + intent.motion_command = planning::MotionCommand{"path_target", stay_in_place, planning::FaceBall{}}; return intent; } @@ -205,15 +225,15 @@ std::optional Offense::state_to_task(RobotIntent intent) { case RECEIVING: { // intercept the ball // if ball fast, use settle, otherwise collect - if (last_world_state_->ball.velocity.mag() > 0.75) { - auto settle_cmd = planning::MotionCommand{"settle"}; - intent.motion_command = settle_cmd; - intent.dribbler_speed = 255.0; - } else { + // if (last_world_state_->ball.velocity.mag() > 0.75) { + // auto settle_cmd = planning::MotionCommand{"settle"}; + // intent.motion_command = settle_cmd; + // intent.dribbler_speed = 255.0; + // } else { auto collect_cmd = planning::MotionCommand{"collect"}; intent.motion_command = collect_cmd; intent.dribbler_speed = 255.0; - } + // } return intent; } @@ -290,13 +310,13 @@ communication::PosAgentResponseWrapper Offense::receive_communication_request( comm_response.response = response; return comm_response; } - + return comm_response; } void Offense::derived_acknowledge_pass() { // I have been chosen as the receiver - current_state_ = RECEIVING; + current_state_ = RECEIVING_START; } void Offense::derived_pass_ball() { diff --git a/soccer/src/soccer/strategy/agent/position/offense.hpp b/soccer/src/soccer/strategy/agent/position/offense.hpp index bec119580a0..eb2a01058bd 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/offense.hpp @@ -49,6 +49,7 @@ class Offense : public Position { POSSESSION, // Holding the ball PASSING, // Getting rid of it STEALING, // Getting the ball + RECEIVING_START, // Facing the ball RECEIVING, // Getting the ball from a pass SHOOTING_START, // Calculate shot SHOOTING, // Winning the game @@ -96,6 +97,8 @@ class Offense : public Position { return RJ::Seconds{5}; case STEALING: return RJ::Seconds{5}; + case RECEIVING_START: + return RJ::Seconds{5}; case RECEIVING: return RJ::Seconds{5}; case SHOOTING_START: @@ -122,6 +125,8 @@ class Offense : public Position { return "PASSING"; case STEALING: return "STEALING"; + case RECEIVING_START: + return "RECEIVING_START"; case RECEIVING: return "RECEIVING"; case SHOOTING_START: @@ -205,7 +210,7 @@ class Offense : public Position { /** * @return if this agent is open to receive a pass * @param target_robot_shell the robot shell to check if open - */ + */ bool check_if_open(int target_robot_shell); /** * @return the target (within the goal) that would be the most clear shot From cd637e7926f7b39864f6760a903939fa80c87395 Mon Sep 17 00:00:00 2001 From: Sid Parikh Date: Mon, 12 Feb 2024 15:41:17 -0500 Subject: [PATCH 21/22] create ScorerCancelRequest --- rj_msgs/CMakeLists.txt | 17 +-- rj_msgs/msg/AgentRequest.msg | 11 +- rj_msgs/msg/AgentResponseVariant.msg | 8 +- rj_msgs/request/ScorerCancelRequest.msg | 6 + .../planning/planner/collect_path_planner.cpp | 1 - .../planning/planner/settle_path_planner.cpp | 4 +- .../agent/communication/communication.cpp | 67 ++++++----- .../agent/communication/communication.hpp | 108 +++++++++--------- .../communication/scorer_cancel_request.hpp | 46 ++++++++ .../strategy/agent/position/offense.cpp | 23 ++-- 10 files changed, 181 insertions(+), 110 deletions(-) create mode 100644 rj_msgs/request/ScorerCancelRequest.msg create mode 100644 soccer/src/soccer/strategy/agent/communication/scorer_cancel_request.hpp diff --git a/rj_msgs/CMakeLists.txt b/rj_msgs/CMakeLists.txt index 9e987316516..ff4c3b1df83 100644 --- a/rj_msgs/CMakeLists.txt +++ b/rj_msgs/CMakeLists.txt @@ -59,24 +59,25 @@ rosidl_generate_interfaces( msg/AgentResponseVariant.msg # Agent Request Messages - request/ResetScorerRequest.msg - request/PositionRequest.msg - request/TestRequest.msg request/BallInTransitRequest.msg request/ScorerRequest.msg - request/IncomingBallRequest.msg + request/TestRequest.msg request/PassRequest.msg - request/LeaveWallRequest.msg + request/IncomingBallRequest.msg + request/ScorerCancelRequest.msg + request/ResetScorerRequest.msg request/JoinWallRequest.msg + request/LeaveWallRequest.msg + request/PositionRequest.msg # Agent Response Messages + response/TestResponse.msg response/LeaveWallResponse.msg - response/Acknowledge.msg - response/PassResponse.msg response/JoinWallResponse.msg response/PositionResponse.msg + response/Acknowledge.msg + response/PassResponse.msg response/ScorerResponse.msg - response/TestResponse.msg # Services srv/AgentCommunication.srv diff --git a/rj_msgs/msg/AgentRequest.msg b/rj_msgs/msg/AgentRequest.msg index 69c8a152580..5e8fbbf5812 100644 --- a/rj_msgs/msg/AgentRequest.msg +++ b/rj_msgs/msg/AgentRequest.msg @@ -3,12 +3,13 @@ # 2 -> PositionResponse # 3 -> PassResponse # ACKNOWLEDGE REQUEST DOES NOT (AND SHOULD NOT) EXIST -ResetScorerRequest[<=1] reset_scorer_request -PositionRequest[<=1] position_request -TestRequest[<=1] test_request BallInTransitRequest[<=1] ball_in_transit_request ScorerRequest[<=1] scorer_request -IncomingBallRequest[<=1] incoming_ball_request +TestRequest[<=1] test_request PassRequest[<=1] pass_request +IncomingBallRequest[<=1] incoming_ball_request +ScorerCancelRequest[<=1] scorer_cancel_request +ResetScorerRequest[<=1] reset_scorer_request +JoinWallRequest[<=1] join_wall_request LeaveWallRequest[<=1] leave_wall_request -JoinWallRequest[<=1] join_wall_request \ No newline at end of file +PositionRequest[<=1] position_request \ No newline at end of file diff --git a/rj_msgs/msg/AgentResponseVariant.msg b/rj_msgs/msg/AgentResponseVariant.msg index c4fc9c33705..1bfa3f8ee97 100644 --- a/rj_msgs/msg/AgentResponseVariant.msg +++ b/rj_msgs/msg/AgentResponseVariant.msg @@ -1,7 +1,7 @@ +TestResponse[<=1] test_response LeaveWallResponse[<=1] leave_wall_response -Acknowledge[<=1] acknowledge -PassResponse[<=1] pass_response JoinWallResponse[<=1] join_wall_response PositionResponse[<=1] position_response -ScorerResponse[<=1] scorer_response -TestResponse[<=1] test_response \ No newline at end of file +Acknowledge[<=1] acknowledge +PassResponse[<=1] pass_response +ScorerResponse[<=1] scorer_response \ No newline at end of file diff --git a/rj_msgs/request/ScorerCancelRequest.msg b/rj_msgs/request/ScorerCancelRequest.msg new file mode 100644 index 00000000000..187fbd2b395 --- /dev/null +++ b/rj_msgs/request/ScorerCancelRequest.msg @@ -0,0 +1,6 @@ +# /** +# * @brief request sent by an agent to announce they are no longer a ball-handler +# * +# */ +uint32 request_uid +uint8 robot_id \ No newline at end of file diff --git a/soccer/src/soccer/planning/planner/collect_path_planner.cpp b/soccer/src/soccer/planning/planner/collect_path_planner.cpp index 80cc169ca25..88b1d23ba9c 100644 --- a/soccer/src/soccer/planning/planner/collect_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/collect_path_planner.cpp @@ -14,7 +14,6 @@ using namespace rj_geometry; namespace planning { Trajectory CollectPathPlanner::plan(const PlanRequest& plan_request) { - const auto state = plan_request.play_state.state(); if (state == PlayState::Stop || state == PlayState::Halt) { // This planner automatically fails if the robot is prohibited from touching the ball. diff --git a/soccer/src/soccer/planning/planner/settle_path_planner.cpp b/soccer/src/soccer/planning/planner/settle_path_planner.cpp index 65749ae7d2b..583fdbde055 100644 --- a/soccer/src/soccer/planning/planner/settle_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/settle_path_planner.cpp @@ -19,7 +19,6 @@ using namespace rj_geometry; namespace planning { Trajectory SettlePathPlanner::plan(const PlanRequest& plan_request) { - const auto state = plan_request.play_state.state(); if (state == PlayState::Stop || state == PlayState::Halt) { // This planner automatically fails if the robot is prohibited from touching the ball. @@ -467,7 +466,8 @@ Trajectory SettlePathPlanner::dampen(const PlanRequest& plan_request, RobotInsta dampen_end.set_debug_text("Damping"); if (!previous_.empty()) { - SPDLOG_INFO("stamp {} {}",previous_.last().stamp.time_since_epoch().count(), dampen_end.first().stamp.time_since_epoch().count()); + SPDLOG_INFO("stamp {} {}", previous_.last().stamp.time_since_epoch().count(), + dampen_end.first().stamp.time_since_epoch().count()); dampen_end = Trajectory(previous_, dampen_end); } diff --git a/soccer/src/soccer/strategy/agent/communication/communication.cpp b/soccer/src/soccer/strategy/agent/communication/communication.cpp index c840fb56456..ceb81da3eda 100644 --- a/soccer/src/soccer/strategy/agent/communication/communication.cpp +++ b/soccer/src/soccer/strategy/agent/communication/communication.cpp @@ -8,11 +8,11 @@ u_int32_t request_uid = 0; std::mutex response_uid_mutex; u_int32_t response_uid = 0; -bool operator==(const ResetScorerRequest& a, const ResetScorerRequest& b) { +bool operator==(const BallInTransitRequest& a, const BallInTransitRequest& b) { return a.request_uid == b.request_uid; } -bool operator==(const PositionRequest& a, const PositionRequest& b) { +bool operator==(const ScorerRequest& a, const ScorerRequest& b) { return a.request_uid == b.request_uid; } @@ -20,39 +20,39 @@ bool operator==(const TestRequest& a, const TestRequest& b) { return a.request_uid == b.request_uid; } -bool operator==(const BallInTransitRequest& a, const BallInTransitRequest& b) { +bool operator==(const PassRequest& a, const PassRequest& b) { return a.request_uid == b.request_uid; } -bool operator==(const ScorerRequest& a, const ScorerRequest& b) { +bool operator==(const IncomingBallRequest& a, const IncomingBallRequest& b) { return a.request_uid == b.request_uid; } -bool operator==(const IncomingBallRequest& a, const IncomingBallRequest& b) { +bool operator==(const ScorerCancelRequest& a, const ScorerCancelRequest& b) { return a.request_uid == b.request_uid; } -bool operator==(const PassRequest& a, const PassRequest& b) { +bool operator==(const ResetScorerRequest& a, const ResetScorerRequest& b) { return a.request_uid == b.request_uid; } -bool operator==(const LeaveWallRequest& a, const LeaveWallRequest& b) { +bool operator==(const JoinWallRequest& a, const JoinWallRequest& b) { return a.request_uid == b.request_uid; } -bool operator==(const JoinWallRequest& a, const JoinWallRequest& b) { +bool operator==(const LeaveWallRequest& a, const LeaveWallRequest& b) { return a.request_uid == b.request_uid; } -bool operator==(const LeaveWallResponse& a, const LeaveWallResponse& b) { - return a.response_uid == b.response_uid; +bool operator==(const PositionRequest& a, const PositionRequest& b) { + return a.request_uid == b.request_uid; } -bool operator==(const Acknowledge& a, const Acknowledge& b) { +bool operator==(const TestResponse& a, const TestResponse& b) { return a.response_uid == b.response_uid; } -bool operator==(const PassResponse& a, const PassResponse& b) { +bool operator==(const LeaveWallResponse& a, const LeaveWallResponse& b) { return a.response_uid == b.response_uid; } @@ -64,11 +64,15 @@ bool operator==(const PositionResponse& a, const PositionResponse& b) { return a.response_uid == b.response_uid; } -bool operator==(const ScorerResponse& a, const ScorerResponse& b) { +bool operator==(const Acknowledge& a, const Acknowledge& b) { return a.response_uid == b.response_uid; } -bool operator==(const TestResponse& a, const TestResponse& b) { +bool operator==(const PassResponse& a, const PassResponse& b) { + return a.response_uid == b.response_uid; +} + +bool operator==(const ScorerResponse& a, const ScorerResponse& b) { return a.response_uid == b.response_uid; } @@ -76,14 +80,14 @@ bool operator==(const AgentResponse& a, const AgentResponse& b) { return (a.associated_request == b.associated_request) && (a.response == b.response); } -void generate_uid(ResetScorerRequest& request) { +void generate_uid(BallInTransitRequest& request) { request_uid_mutex.lock(); request.request_uid = request_uid; request_uid++; request_uid_mutex.unlock(); } -void generate_uid(PositionRequest& request) { +void generate_uid(ScorerRequest& request) { request_uid_mutex.lock(); request.request_uid = request_uid; request_uid++; @@ -97,28 +101,35 @@ void generate_uid(TestRequest& request) { request_uid_mutex.unlock(); } -void generate_uid(BallInTransitRequest& request) { +void generate_uid(PassRequest& request) { request_uid_mutex.lock(); request.request_uid = request_uid; request_uid++; request_uid_mutex.unlock(); } -void generate_uid(ScorerRequest& request) { +void generate_uid(IncomingBallRequest& request) { request_uid_mutex.lock(); request.request_uid = request_uid; request_uid++; request_uid_mutex.unlock(); } -void generate_uid(IncomingBallRequest& request) { +void generate_uid(ScorerCancelRequest& request) { request_uid_mutex.lock(); request.request_uid = request_uid; request_uid++; request_uid_mutex.unlock(); } -void generate_uid(PassRequest& request) { +void generate_uid(ResetScorerRequest& request) { + request_uid_mutex.lock(); + request.request_uid = request_uid; + request_uid++; + request_uid_mutex.unlock(); +} + +void generate_uid(JoinWallRequest& request) { request_uid_mutex.lock(); request.request_uid = request_uid; request_uid++; @@ -132,56 +143,56 @@ void generate_uid(LeaveWallRequest& request) { request_uid_mutex.unlock(); } -void generate_uid(JoinWallRequest& request) { +void generate_uid(PositionRequest& request) { request_uid_mutex.lock(); request.request_uid = request_uid; request_uid++; request_uid_mutex.unlock(); } -void generate_uid(LeaveWallResponse& response) { +void generate_uid(TestResponse& response) { response_uid_mutex.lock(); response.response_uid = response_uid; response_uid++; response_uid_mutex.unlock(); } -void generate_uid(Acknowledge& response) { +void generate_uid(LeaveWallResponse& response) { response_uid_mutex.lock(); response.response_uid = response_uid; response_uid++; response_uid_mutex.unlock(); } -void generate_uid(PassResponse& response) { +void generate_uid(JoinWallResponse& response) { response_uid_mutex.lock(); response.response_uid = response_uid; response_uid++; response_uid_mutex.unlock(); } -void generate_uid(JoinWallResponse& response) { +void generate_uid(PositionResponse& response) { response_uid_mutex.lock(); response.response_uid = response_uid; response_uid++; response_uid_mutex.unlock(); } -void generate_uid(PositionResponse& response) { +void generate_uid(Acknowledge& response) { response_uid_mutex.lock(); response.response_uid = response_uid; response_uid++; response_uid_mutex.unlock(); } -void generate_uid(ScorerResponse& response) { +void generate_uid(PassResponse& response) { response_uid_mutex.lock(); response.response_uid = response_uid; response_uid++; response_uid_mutex.unlock(); } -void generate_uid(TestResponse& response) { +void generate_uid(ScorerResponse& response) { response_uid_mutex.lock(); response.response_uid = response_uid; response_uid++; diff --git a/soccer/src/soccer/strategy/agent/communication/communication.hpp b/soccer/src/soccer/strategy/agent/communication/communication.hpp index 4f32a12a52d..b6527a01004 100644 --- a/soccer/src/soccer/strategy/agent/communication/communication.hpp +++ b/soccer/src/soccer/strategy/agent/communication/communication.hpp @@ -23,6 +23,7 @@ #include "rj_msgs/msg/agent_request.hpp" #include "rj_msgs/msg/agent_response.hpp" #include "rj_msgs/msg/agent_response_variant.hpp" +#include "scorer_cancel_request.hpp" #include "scorer_request.hpp" #include "scorer_response.hpp" #include "test_request.hpp" @@ -33,16 +34,16 @@ namespace strategy::communication { /** * @brief a conglomeration of the different request types. */ -using AgentRequest = std::variant; +using AgentRequest = std::variant; /** * @brief a conglomeration of the different response types. */ using AgentResponseVariant = - std::variant; + std::variant; /** * @brief response message that is sent from the receiver of the request to the @@ -128,33 +129,36 @@ template <> struct RosConverter { static rj_msgs::msg::AgentRequest to_ros(const strategy::communication::AgentRequest& from) { rj_msgs::msg::AgentRequest result; - if (const auto* reset_scorer_request = - std::get_if(&from)) { - result.reset_scorer_request.emplace_back(convert_to_ros(*reset_scorer_request)); - } else if (const auto* position_request = - std::get_if(&from)) { - result.position_request.emplace_back(convert_to_ros(*position_request)); - } else if (const auto* test_request = - std::get_if(&from)) { - result.test_request.emplace_back(convert_to_ros(*test_request)); - } else if (const auto* ball_in_transit_request = - std::get_if(&from)) { + if (const auto* ball_in_transit_request = + std::get_if(&from)) { result.ball_in_transit_request.emplace_back(convert_to_ros(*ball_in_transit_request)); } else if (const auto* scorer_request = std::get_if(&from)) { result.scorer_request.emplace_back(convert_to_ros(*scorer_request)); - } else if (const auto* incoming_ball_request = - std::get_if(&from)) { - result.incoming_ball_request.emplace_back(convert_to_ros(*incoming_ball_request)); + } else if (const auto* test_request = + std::get_if(&from)) { + result.test_request.emplace_back(convert_to_ros(*test_request)); } else if (const auto* pass_request = std::get_if(&from)) { result.pass_request.emplace_back(convert_to_ros(*pass_request)); - } else if (const auto* leave_wall_request = - std::get_if(&from)) { - result.leave_wall_request.emplace_back(convert_to_ros(*leave_wall_request)); + } else if (const auto* incoming_ball_request = + std::get_if(&from)) { + result.incoming_ball_request.emplace_back(convert_to_ros(*incoming_ball_request)); + } else if (const auto* scorer_cancel_request = + std::get_if(&from)) { + result.scorer_cancel_request.emplace_back(convert_to_ros(*scorer_cancel_request)); + } else if (const auto* reset_scorer_request = + std::get_if(&from)) { + result.reset_scorer_request.emplace_back(convert_to_ros(*reset_scorer_request)); } else if (const auto* join_wall_request = std::get_if(&from)) { result.join_wall_request.emplace_back(convert_to_ros(*join_wall_request)); + } else if (const auto* leave_wall_request = + std::get_if(&from)) { + result.leave_wall_request.emplace_back(convert_to_ros(*leave_wall_request)); + } else if (const auto* position_request = + std::get_if(&from)) { + result.position_request.emplace_back(convert_to_ros(*position_request)); } else { throw std::runtime_error("Invalid variant of AgentRequest"); } @@ -163,24 +167,26 @@ struct RosConverter(&(from.response))) { + if (const auto* test_response = + std::get_if(&(from.response))) { + result.response.test_response.emplace_back(convert_to_ros(*test_response)); + } else if (const auto* leave_wall_response = + std::get_if(&(from.response))) { result.response.leave_wall_response.emplace_back(convert_to_ros(*leave_wall_response)); - } else if (const auto* acknowledge = - std::get_if(&(from.response))) { - result.response.acknowledge.emplace_back(convert_to_ros(*acknowledge)); - } else if (const auto* pass_response = - std::get_if(&(from.response))) { - result.response.pass_response.emplace_back(convert_to_ros(*pass_response)); } else if (const auto* join_wall_response = std::get_if(&(from.response))) { result.response.join_wall_response.emplace_back(convert_to_ros(*join_wall_response)); } else if (const auto* position_response = std::get_if(&(from.response))) { result.response.position_response.emplace_back(convert_to_ros(*position_response)); + } else if (const auto* acknowledge = + std::get_if(&(from.response))) { + result.response.acknowledge.emplace_back(convert_to_ros(*acknowledge)); + } else if (const auto* pass_response = + std::get_if(&(from.response))) { + result.response.pass_response.emplace_back(convert_to_ros(*pass_response)); } else if (const auto* scorer_response = std::get_if(&(from.response))) { result.response.scorer_response.emplace_back(convert_to_ros(*scorer_response)); - } else if (const auto* test_response = - std::get_if(&(from.response))) { - result.response.test_response.emplace_back(convert_to_ros(*test_response)); } else { throw std::runtime_error("Invalid variant of AgentResponse"); } @@ -226,20 +232,20 @@ struct RosConverter +#include +#include +#include + +#include +#include +#include "rj_msgs/msg/scorer_cancel_request.hpp" + +namespace strategy::communication { + +struct ScorerCancelRequest { + uint32_t request_uid; + uint8_t robot_id; +}; + +bool operator==(const ScorerCancelRequest& a, const ScorerCancelRequest& b); +void generate_uid(ScorerCancelRequest& request); + +} + +namespace rj_convert { + +template <> +struct RosConverter { + static rj_msgs::msg::ScorerCancelRequest to_ros(const strategy::communication::ScorerCancelRequest& from) { + rj_msgs::msg::ScorerCancelRequest result; + result.request_uid = from.request_uid; + result.robot_id = from.robot_id; + return result; + } + + static strategy::communication::ScorerCancelRequest from_ros(const rj_msgs::msg::ScorerCancelRequest& from) { + return strategy::communication::ScorerCancelRequest{ + from.request_uid, + from.robot_id, + }; + } + +}; + +ASSOCIATE_CPP_ROS(strategy::communication::ScorerCancelRequest, rj_msgs::msg::ScorerCancelRequest); + +} \ No newline at end of file diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 64ba013b7b2..185c0f4a0f5 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -202,9 +202,9 @@ std::optional Offense::state_to_task(RobotIntent intent) { // intent.motion_command = settle_cmd; // intent.dribbler_speed = 255.0; // } else { - auto collect_cmd = planning::MotionCommand{"collect"}; - intent.motion_command = collect_cmd; - intent.dribbler_speed = 255.0; + auto collect_cmd = planning::MotionCommand{"collect"}; + intent.motion_command = collect_cmd; + intent.dribbler_speed = 255.0; // } return intent; @@ -215,9 +215,10 @@ std::optional Offense::state_to_task(RobotIntent intent) { auto current_pos = last_world_state_->get_robot(true, robot_id_).pose.position(); - planning::LinearMotionInstant stay_in_place {current_pos}; + planning::LinearMotionInstant stay_in_place{current_pos}; - intent.motion_command = planning::MotionCommand{"path_target", stay_in_place, planning::FaceBall{}}; + intent.motion_command = + planning::MotionCommand{"path_target", stay_in_place, planning::FaceBall{}}; return intent; } @@ -226,13 +227,13 @@ std::optional Offense::state_to_task(RobotIntent intent) { // intercept the ball // if ball fast, use settle, otherwise collect // if (last_world_state_->ball.velocity.mag() > 0.75) { - // auto settle_cmd = planning::MotionCommand{"settle"}; - // intent.motion_command = settle_cmd; - // intent.dribbler_speed = 255.0; + // auto settle_cmd = planning::MotionCommand{"settle"}; + // intent.motion_command = settle_cmd; + // intent.dribbler_speed = 255.0; // } else { - auto collect_cmd = planning::MotionCommand{"collect"}; - intent.motion_command = collect_cmd; - intent.dribbler_speed = 255.0; + auto collect_cmd = planning::MotionCommand{"collect"}; + intent.motion_command = collect_cmd; + intent.dribbler_speed = 255.0; // } return intent; From e27a0cc3921b869cae8a3d844d1d4a2ae398d915 Mon Sep 17 00:00:00 2001 From: Sid Parikh Date: Mon, 12 Feb 2024 15:44:11 -0500 Subject: [PATCH 22/22] Revert "create ScorerCancelRequest" This reverts commit cd637e7926f7b39864f6760a903939fa80c87395. --- rj_msgs/CMakeLists.txt | 17 ++- rj_msgs/msg/AgentRequest.msg | 11 +- rj_msgs/msg/AgentResponseVariant.msg | 8 +- rj_msgs/request/ScorerCancelRequest.msg | 6 - .../planning/planner/collect_path_planner.cpp | 1 + .../planning/planner/settle_path_planner.cpp | 4 +- .../agent/communication/communication.cpp | 67 +++++------ .../agent/communication/communication.hpp | 108 +++++++++--------- .../communication/scorer_cancel_request.hpp | 46 -------- .../strategy/agent/position/offense.cpp | 23 ++-- 10 files changed, 110 insertions(+), 181 deletions(-) delete mode 100644 rj_msgs/request/ScorerCancelRequest.msg delete mode 100644 soccer/src/soccer/strategy/agent/communication/scorer_cancel_request.hpp diff --git a/rj_msgs/CMakeLists.txt b/rj_msgs/CMakeLists.txt index ff4c3b1df83..9e987316516 100644 --- a/rj_msgs/CMakeLists.txt +++ b/rj_msgs/CMakeLists.txt @@ -59,25 +59,24 @@ rosidl_generate_interfaces( msg/AgentResponseVariant.msg # Agent Request Messages + request/ResetScorerRequest.msg + request/PositionRequest.msg + request/TestRequest.msg request/BallInTransitRequest.msg request/ScorerRequest.msg - request/TestRequest.msg - request/PassRequest.msg request/IncomingBallRequest.msg - request/ScorerCancelRequest.msg - request/ResetScorerRequest.msg - request/JoinWallRequest.msg + request/PassRequest.msg request/LeaveWallRequest.msg - request/PositionRequest.msg + request/JoinWallRequest.msg # Agent Response Messages - response/TestResponse.msg response/LeaveWallResponse.msg - response/JoinWallResponse.msg - response/PositionResponse.msg response/Acknowledge.msg response/PassResponse.msg + response/JoinWallResponse.msg + response/PositionResponse.msg response/ScorerResponse.msg + response/TestResponse.msg # Services srv/AgentCommunication.srv diff --git a/rj_msgs/msg/AgentRequest.msg b/rj_msgs/msg/AgentRequest.msg index 5e8fbbf5812..69c8a152580 100644 --- a/rj_msgs/msg/AgentRequest.msg +++ b/rj_msgs/msg/AgentRequest.msg @@ -3,13 +3,12 @@ # 2 -> PositionResponse # 3 -> PassResponse # ACKNOWLEDGE REQUEST DOES NOT (AND SHOULD NOT) EXIST +ResetScorerRequest[<=1] reset_scorer_request +PositionRequest[<=1] position_request +TestRequest[<=1] test_request BallInTransitRequest[<=1] ball_in_transit_request ScorerRequest[<=1] scorer_request -TestRequest[<=1] test_request -PassRequest[<=1] pass_request IncomingBallRequest[<=1] incoming_ball_request -ScorerCancelRequest[<=1] scorer_cancel_request -ResetScorerRequest[<=1] reset_scorer_request -JoinWallRequest[<=1] join_wall_request +PassRequest[<=1] pass_request LeaveWallRequest[<=1] leave_wall_request -PositionRequest[<=1] position_request \ No newline at end of file +JoinWallRequest[<=1] join_wall_request \ No newline at end of file diff --git a/rj_msgs/msg/AgentResponseVariant.msg b/rj_msgs/msg/AgentResponseVariant.msg index 1bfa3f8ee97..c4fc9c33705 100644 --- a/rj_msgs/msg/AgentResponseVariant.msg +++ b/rj_msgs/msg/AgentResponseVariant.msg @@ -1,7 +1,7 @@ -TestResponse[<=1] test_response LeaveWallResponse[<=1] leave_wall_response -JoinWallResponse[<=1] join_wall_response -PositionResponse[<=1] position_response Acknowledge[<=1] acknowledge PassResponse[<=1] pass_response -ScorerResponse[<=1] scorer_response \ No newline at end of file +JoinWallResponse[<=1] join_wall_response +PositionResponse[<=1] position_response +ScorerResponse[<=1] scorer_response +TestResponse[<=1] test_response \ No newline at end of file diff --git a/rj_msgs/request/ScorerCancelRequest.msg b/rj_msgs/request/ScorerCancelRequest.msg deleted file mode 100644 index 187fbd2b395..00000000000 --- a/rj_msgs/request/ScorerCancelRequest.msg +++ /dev/null @@ -1,6 +0,0 @@ -# /** -# * @brief request sent by an agent to announce they are no longer a ball-handler -# * -# */ -uint32 request_uid -uint8 robot_id \ No newline at end of file diff --git a/soccer/src/soccer/planning/planner/collect_path_planner.cpp b/soccer/src/soccer/planning/planner/collect_path_planner.cpp index 88b1d23ba9c..80cc169ca25 100644 --- a/soccer/src/soccer/planning/planner/collect_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/collect_path_planner.cpp @@ -14,6 +14,7 @@ using namespace rj_geometry; namespace planning { Trajectory CollectPathPlanner::plan(const PlanRequest& plan_request) { + const auto state = plan_request.play_state.state(); if (state == PlayState::Stop || state == PlayState::Halt) { // This planner automatically fails if the robot is prohibited from touching the ball. diff --git a/soccer/src/soccer/planning/planner/settle_path_planner.cpp b/soccer/src/soccer/planning/planner/settle_path_planner.cpp index 583fdbde055..65749ae7d2b 100644 --- a/soccer/src/soccer/planning/planner/settle_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/settle_path_planner.cpp @@ -19,6 +19,7 @@ using namespace rj_geometry; namespace planning { Trajectory SettlePathPlanner::plan(const PlanRequest& plan_request) { + const auto state = plan_request.play_state.state(); if (state == PlayState::Stop || state == PlayState::Halt) { // This planner automatically fails if the robot is prohibited from touching the ball. @@ -466,8 +467,7 @@ Trajectory SettlePathPlanner::dampen(const PlanRequest& plan_request, RobotInsta dampen_end.set_debug_text("Damping"); if (!previous_.empty()) { - SPDLOG_INFO("stamp {} {}", previous_.last().stamp.time_since_epoch().count(), - dampen_end.first().stamp.time_since_epoch().count()); + SPDLOG_INFO("stamp {} {}",previous_.last().stamp.time_since_epoch().count(), dampen_end.first().stamp.time_since_epoch().count()); dampen_end = Trajectory(previous_, dampen_end); } diff --git a/soccer/src/soccer/strategy/agent/communication/communication.cpp b/soccer/src/soccer/strategy/agent/communication/communication.cpp index ceb81da3eda..c840fb56456 100644 --- a/soccer/src/soccer/strategy/agent/communication/communication.cpp +++ b/soccer/src/soccer/strategy/agent/communication/communication.cpp @@ -8,11 +8,11 @@ u_int32_t request_uid = 0; std::mutex response_uid_mutex; u_int32_t response_uid = 0; -bool operator==(const BallInTransitRequest& a, const BallInTransitRequest& b) { +bool operator==(const ResetScorerRequest& a, const ResetScorerRequest& b) { return a.request_uid == b.request_uid; } -bool operator==(const ScorerRequest& a, const ScorerRequest& b) { +bool operator==(const PositionRequest& a, const PositionRequest& b) { return a.request_uid == b.request_uid; } @@ -20,23 +20,19 @@ bool operator==(const TestRequest& a, const TestRequest& b) { return a.request_uid == b.request_uid; } -bool operator==(const PassRequest& a, const PassRequest& b) { - return a.request_uid == b.request_uid; -} - -bool operator==(const IncomingBallRequest& a, const IncomingBallRequest& b) { +bool operator==(const BallInTransitRequest& a, const BallInTransitRequest& b) { return a.request_uid == b.request_uid; } -bool operator==(const ScorerCancelRequest& a, const ScorerCancelRequest& b) { +bool operator==(const ScorerRequest& a, const ScorerRequest& b) { return a.request_uid == b.request_uid; } -bool operator==(const ResetScorerRequest& a, const ResetScorerRequest& b) { +bool operator==(const IncomingBallRequest& a, const IncomingBallRequest& b) { return a.request_uid == b.request_uid; } -bool operator==(const JoinWallRequest& a, const JoinWallRequest& b) { +bool operator==(const PassRequest& a, const PassRequest& b) { return a.request_uid == b.request_uid; } @@ -44,35 +40,35 @@ bool operator==(const LeaveWallRequest& a, const LeaveWallRequest& b) { return a.request_uid == b.request_uid; } -bool operator==(const PositionRequest& a, const PositionRequest& b) { +bool operator==(const JoinWallRequest& a, const JoinWallRequest& b) { return a.request_uid == b.request_uid; } -bool operator==(const TestResponse& a, const TestResponse& b) { +bool operator==(const LeaveWallResponse& a, const LeaveWallResponse& b) { return a.response_uid == b.response_uid; } -bool operator==(const LeaveWallResponse& a, const LeaveWallResponse& b) { +bool operator==(const Acknowledge& a, const Acknowledge& b) { return a.response_uid == b.response_uid; } -bool operator==(const JoinWallResponse& a, const JoinWallResponse& b) { +bool operator==(const PassResponse& a, const PassResponse& b) { return a.response_uid == b.response_uid; } -bool operator==(const PositionResponse& a, const PositionResponse& b) { +bool operator==(const JoinWallResponse& a, const JoinWallResponse& b) { return a.response_uid == b.response_uid; } -bool operator==(const Acknowledge& a, const Acknowledge& b) { +bool operator==(const PositionResponse& a, const PositionResponse& b) { return a.response_uid == b.response_uid; } -bool operator==(const PassResponse& a, const PassResponse& b) { +bool operator==(const ScorerResponse& a, const ScorerResponse& b) { return a.response_uid == b.response_uid; } -bool operator==(const ScorerResponse& a, const ScorerResponse& b) { +bool operator==(const TestResponse& a, const TestResponse& b) { return a.response_uid == b.response_uid; } @@ -80,14 +76,14 @@ bool operator==(const AgentResponse& a, const AgentResponse& b) { return (a.associated_request == b.associated_request) && (a.response == b.response); } -void generate_uid(BallInTransitRequest& request) { +void generate_uid(ResetScorerRequest& request) { request_uid_mutex.lock(); request.request_uid = request_uid; request_uid++; request_uid_mutex.unlock(); } -void generate_uid(ScorerRequest& request) { +void generate_uid(PositionRequest& request) { request_uid_mutex.lock(); request.request_uid = request_uid; request_uid++; @@ -101,35 +97,28 @@ void generate_uid(TestRequest& request) { request_uid_mutex.unlock(); } -void generate_uid(PassRequest& request) { - request_uid_mutex.lock(); - request.request_uid = request_uid; - request_uid++; - request_uid_mutex.unlock(); -} - -void generate_uid(IncomingBallRequest& request) { +void generate_uid(BallInTransitRequest& request) { request_uid_mutex.lock(); request.request_uid = request_uid; request_uid++; request_uid_mutex.unlock(); } -void generate_uid(ScorerCancelRequest& request) { +void generate_uid(ScorerRequest& request) { request_uid_mutex.lock(); request.request_uid = request_uid; request_uid++; request_uid_mutex.unlock(); } -void generate_uid(ResetScorerRequest& request) { +void generate_uid(IncomingBallRequest& request) { request_uid_mutex.lock(); request.request_uid = request_uid; request_uid++; request_uid_mutex.unlock(); } -void generate_uid(JoinWallRequest& request) { +void generate_uid(PassRequest& request) { request_uid_mutex.lock(); request.request_uid = request_uid; request_uid++; @@ -143,56 +132,56 @@ void generate_uid(LeaveWallRequest& request) { request_uid_mutex.unlock(); } -void generate_uid(PositionRequest& request) { +void generate_uid(JoinWallRequest& request) { request_uid_mutex.lock(); request.request_uid = request_uid; request_uid++; request_uid_mutex.unlock(); } -void generate_uid(TestResponse& response) { +void generate_uid(LeaveWallResponse& response) { response_uid_mutex.lock(); response.response_uid = response_uid; response_uid++; response_uid_mutex.unlock(); } -void generate_uid(LeaveWallResponse& response) { +void generate_uid(Acknowledge& response) { response_uid_mutex.lock(); response.response_uid = response_uid; response_uid++; response_uid_mutex.unlock(); } -void generate_uid(JoinWallResponse& response) { +void generate_uid(PassResponse& response) { response_uid_mutex.lock(); response.response_uid = response_uid; response_uid++; response_uid_mutex.unlock(); } -void generate_uid(PositionResponse& response) { +void generate_uid(JoinWallResponse& response) { response_uid_mutex.lock(); response.response_uid = response_uid; response_uid++; response_uid_mutex.unlock(); } -void generate_uid(Acknowledge& response) { +void generate_uid(PositionResponse& response) { response_uid_mutex.lock(); response.response_uid = response_uid; response_uid++; response_uid_mutex.unlock(); } -void generate_uid(PassResponse& response) { +void generate_uid(ScorerResponse& response) { response_uid_mutex.lock(); response.response_uid = response_uid; response_uid++; response_uid_mutex.unlock(); } -void generate_uid(ScorerResponse& response) { +void generate_uid(TestResponse& response) { response_uid_mutex.lock(); response.response_uid = response_uid; response_uid++; diff --git a/soccer/src/soccer/strategy/agent/communication/communication.hpp b/soccer/src/soccer/strategy/agent/communication/communication.hpp index b6527a01004..4f32a12a52d 100644 --- a/soccer/src/soccer/strategy/agent/communication/communication.hpp +++ b/soccer/src/soccer/strategy/agent/communication/communication.hpp @@ -23,7 +23,6 @@ #include "rj_msgs/msg/agent_request.hpp" #include "rj_msgs/msg/agent_response.hpp" #include "rj_msgs/msg/agent_response_variant.hpp" -#include "scorer_cancel_request.hpp" #include "scorer_request.hpp" #include "scorer_response.hpp" #include "test_request.hpp" @@ -34,16 +33,16 @@ namespace strategy::communication { /** * @brief a conglomeration of the different request types. */ -using AgentRequest = std::variant; +using AgentRequest = std::variant; /** * @brief a conglomeration of the different response types. */ using AgentResponseVariant = - std::variant; + std::variant; /** * @brief response message that is sent from the receiver of the request to the @@ -129,36 +128,33 @@ template <> struct RosConverter { static rj_msgs::msg::AgentRequest to_ros(const strategy::communication::AgentRequest& from) { rj_msgs::msg::AgentRequest result; - if (const auto* ball_in_transit_request = - std::get_if(&from)) { + if (const auto* reset_scorer_request = + std::get_if(&from)) { + result.reset_scorer_request.emplace_back(convert_to_ros(*reset_scorer_request)); + } else if (const auto* position_request = + std::get_if(&from)) { + result.position_request.emplace_back(convert_to_ros(*position_request)); + } else if (const auto* test_request = + std::get_if(&from)) { + result.test_request.emplace_back(convert_to_ros(*test_request)); + } else if (const auto* ball_in_transit_request = + std::get_if(&from)) { result.ball_in_transit_request.emplace_back(convert_to_ros(*ball_in_transit_request)); } else if (const auto* scorer_request = std::get_if(&from)) { result.scorer_request.emplace_back(convert_to_ros(*scorer_request)); - } else if (const auto* test_request = - std::get_if(&from)) { - result.test_request.emplace_back(convert_to_ros(*test_request)); - } else if (const auto* pass_request = - std::get_if(&from)) { - result.pass_request.emplace_back(convert_to_ros(*pass_request)); } else if (const auto* incoming_ball_request = std::get_if(&from)) { result.incoming_ball_request.emplace_back(convert_to_ros(*incoming_ball_request)); - } else if (const auto* scorer_cancel_request = - std::get_if(&from)) { - result.scorer_cancel_request.emplace_back(convert_to_ros(*scorer_cancel_request)); - } else if (const auto* reset_scorer_request = - std::get_if(&from)) { - result.reset_scorer_request.emplace_back(convert_to_ros(*reset_scorer_request)); - } else if (const auto* join_wall_request = - std::get_if(&from)) { - result.join_wall_request.emplace_back(convert_to_ros(*join_wall_request)); + } else if (const auto* pass_request = + std::get_if(&from)) { + result.pass_request.emplace_back(convert_to_ros(*pass_request)); } else if (const auto* leave_wall_request = std::get_if(&from)) { result.leave_wall_request.emplace_back(convert_to_ros(*leave_wall_request)); - } else if (const auto* position_request = - std::get_if(&from)) { - result.position_request.emplace_back(convert_to_ros(*position_request)); + } else if (const auto* join_wall_request = + std::get_if(&from)) { + result.join_wall_request.emplace_back(convert_to_ros(*join_wall_request)); } else { throw std::runtime_error("Invalid variant of AgentRequest"); } @@ -167,26 +163,24 @@ struct RosConverter(&(from.response))) { - result.response.test_response.emplace_back(convert_to_ros(*test_response)); - } else if (const auto* leave_wall_response = - std::get_if(&(from.response))) { + if (const auto* leave_wall_response = + std::get_if(&(from.response))) { result.response.leave_wall_response.emplace_back(convert_to_ros(*leave_wall_response)); - } else if (const auto* join_wall_response = - std::get_if(&(from.response))) { - result.response.join_wall_response.emplace_back(convert_to_ros(*join_wall_response)); - } else if (const auto* position_response = - std::get_if(&(from.response))) { - result.response.position_response.emplace_back(convert_to_ros(*position_response)); } else if (const auto* acknowledge = std::get_if(&(from.response))) { result.response.acknowledge.emplace_back(convert_to_ros(*acknowledge)); } else if (const auto* pass_response = std::get_if(&(from.response))) { result.response.pass_response.emplace_back(convert_to_ros(*pass_response)); + } else if (const auto* join_wall_response = + std::get_if(&(from.response))) { + result.response.join_wall_response.emplace_back(convert_to_ros(*join_wall_response)); + } else if (const auto* position_response = + std::get_if(&(from.response))) { + result.response.position_response.emplace_back(convert_to_ros(*position_response)); } else if (const auto* scorer_response = std::get_if(&(from.response))) { result.response.scorer_response.emplace_back(convert_to_ros(*scorer_response)); + } else if (const auto* test_response = + std::get_if(&(from.response))) { + result.response.test_response.emplace_back(convert_to_ros(*test_response)); } else { throw std::runtime_error("Invalid variant of AgentResponse"); } @@ -232,20 +226,20 @@ struct RosConverter -#include -#include -#include - -#include -#include -#include "rj_msgs/msg/scorer_cancel_request.hpp" - -namespace strategy::communication { - -struct ScorerCancelRequest { - uint32_t request_uid; - uint8_t robot_id; -}; - -bool operator==(const ScorerCancelRequest& a, const ScorerCancelRequest& b); -void generate_uid(ScorerCancelRequest& request); - -} - -namespace rj_convert { - -template <> -struct RosConverter { - static rj_msgs::msg::ScorerCancelRequest to_ros(const strategy::communication::ScorerCancelRequest& from) { - rj_msgs::msg::ScorerCancelRequest result; - result.request_uid = from.request_uid; - result.robot_id = from.robot_id; - return result; - } - - static strategy::communication::ScorerCancelRequest from_ros(const rj_msgs::msg::ScorerCancelRequest& from) { - return strategy::communication::ScorerCancelRequest{ - from.request_uid, - from.robot_id, - }; - } - -}; - -ASSOCIATE_CPP_ROS(strategy::communication::ScorerCancelRequest, rj_msgs::msg::ScorerCancelRequest); - -} \ No newline at end of file diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 185c0f4a0f5..64ba013b7b2 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -202,9 +202,9 @@ std::optional Offense::state_to_task(RobotIntent intent) { // intent.motion_command = settle_cmd; // intent.dribbler_speed = 255.0; // } else { - auto collect_cmd = planning::MotionCommand{"collect"}; - intent.motion_command = collect_cmd; - intent.dribbler_speed = 255.0; + auto collect_cmd = planning::MotionCommand{"collect"}; + intent.motion_command = collect_cmd; + intent.dribbler_speed = 255.0; // } return intent; @@ -215,10 +215,9 @@ std::optional Offense::state_to_task(RobotIntent intent) { auto current_pos = last_world_state_->get_robot(true, robot_id_).pose.position(); - planning::LinearMotionInstant stay_in_place{current_pos}; + planning::LinearMotionInstant stay_in_place {current_pos}; - intent.motion_command = - planning::MotionCommand{"path_target", stay_in_place, planning::FaceBall{}}; + intent.motion_command = planning::MotionCommand{"path_target", stay_in_place, planning::FaceBall{}}; return intent; } @@ -227,13 +226,13 @@ std::optional Offense::state_to_task(RobotIntent intent) { // intercept the ball // if ball fast, use settle, otherwise collect // if (last_world_state_->ball.velocity.mag() > 0.75) { - // auto settle_cmd = planning::MotionCommand{"settle"}; - // intent.motion_command = settle_cmd; - // intent.dribbler_speed = 255.0; + // auto settle_cmd = planning::MotionCommand{"settle"}; + // intent.motion_command = settle_cmd; + // intent.dribbler_speed = 255.0; // } else { - auto collect_cmd = planning::MotionCommand{"collect"}; - intent.motion_command = collect_cmd; - intent.dribbler_speed = 255.0; + auto collect_cmd = planning::MotionCommand{"collect"}; + intent.motion_command = collect_cmd; + intent.dribbler_speed = 255.0; // } return intent;