diff --git a/soccer/src/soccer/strategy/agent/position/defense.cpp b/soccer/src/soccer/strategy/agent/position/defense.cpp index 7f91db23882..80bd460902c 100644 --- a/soccer/src/soccer/strategy/agent/position/defense.cpp +++ b/soccer/src/soccer/strategy/agent/position/defense.cpp @@ -36,15 +36,19 @@ Defense::State Defense::update_state() { } switch (current_state_) { - case IDLING: + case IDLING: { break; - case JOINING_WALL: + } + + case JOINING_WALL: { send_join_wall_request(); // SPDLOG_INFO("join wall {}", robot_id_); next_state = WALLING; walling_robots_ = {(u_int8_t)robot_id_}; break; - case WALLING: + } + + case WALLING: { // If a wall is already full, // Remove the robot with the highest ID from a wall // and make them a marker instead. @@ -54,36 +58,49 @@ Defense::State Defense::update_state() { // SPDLOG_INFO("leave wall {}", robot_id_); next_state = ENTERING_MARKING; } + // wall stealing + if (can_steal_ball()) { + send_leave_wall_request(); + next_state = STEALING; + } break; - case SEARCHING: + } + + case SEARCHING: { break; - case RECEIVING: + } + + case RECEIVING: { // transition to idling if we are close enough to the ball if (distance_to_ball < ball_receive_distance_) { next_state = IDLING; } break; - case PASSING: + } + + case 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; - } - - if (distance_to_ball > ball_lost_distance_) { - next_state = IDLING; + if (check_is_done() || distance_to_ball > ball_lost_distance_) { + next_state = JOINING_WALL; } break; - case FACING: + } + + case FACING: { if (check_is_done()) { next_state = IDLING; } - case MARKING: + } + + case MARKING: { if (marker_.get_target() == -1 || marker_.target_out_of_bounds(world_state)) { next_state = ENTERING_MARKING; } break; - case ENTERING_MARKING: + } + + case ENTERING_MARKING: { marker_.choose_target(world_state); int target_id = marker_.get_target(); if (target_id == -1) { @@ -91,6 +108,26 @@ Defense::State Defense::update_state() { } else { next_state = MARKING; } + break; + } + + case STEALING: { // wall steal + // Go to passing if successful + if (check_is_done()) { + send_leave_wall_request(); + next_state = CLEARING; + } + + if (!can_steal_ball()) { + next_state = JOINING_WALL; + } + } + + case CLEARING: { + if (check_is_done()) { + next_state = JOINING_WALL; + } + } } return next_state; @@ -164,6 +201,22 @@ std::optional Defense::state_to_task(RobotIntent intent) { } else if (current_state_ == MARKING) { // Marker marker = Marker((u_int8_t) robot_id_); return marker_.get_task(intent, last_world_state_, this->field_dimensions_); + } else if (current_state_ == STEALING) { // wall stealer + auto collect_cmd = planning::MotionCommand{"collect"}; + intent.motion_command = collect_cmd; + intent.dribbler_speed = 255.0; + return intent; + } else if (current_state_ == CLEARING) { + planning::LinearMotionInstant target{clear_point_}; + auto line_kick_cmd = planning::MotionCommand{"line_kick", target}; + intent.motion_command = line_kick_cmd; + intent.shoot_mode = RobotIntent::ShootMode::CHIP; + intent.trigger_mode = RobotIntent::TriggerMode::ON_BREAK_BEAM; + intent.kick_speed = 4.0; + intent.dribbler_speed = 255.0; + intent.is_active = true; + + return intent; } return std::nullopt; diff --git a/soccer/src/soccer/strategy/agent/position/defense.hpp b/soccer/src/soccer/strategy/agent/position/defense.hpp index b7123f0541a..2be95517ba6 100644 --- a/soccer/src/soccer/strategy/agent/position/defense.hpp +++ b/soccer/src/soccer/strategy/agent/position/defense.hpp @@ -44,6 +44,8 @@ class Defense : public Position { void revive() override; private: + const rj_geometry::Point clear_point_{0.0, 4.5}; + // static constexpr int kMaxWallers{6}; static constexpr int kMaxWallers{ static_cast(kNumShells)}; // This effectively turns off marking @@ -69,6 +71,8 @@ class Defense : public Position { FACING, // turning to face the passing robot MARKING, // Following closely to an offense robot ENTERING_MARKING, // Choosing/waiting for a robot to mark + STEALING, // wall stealing + CLEARING, }; State update_state(); @@ -85,6 +89,67 @@ class Defense : public Position { */ void send_leave_wall_request(); + /** + * @return if the current state has timed out + * wall steal + */ + /*bool timed_out() const { + // Defined here so it can be inlined + using namespace std::chrono_literals; + + + return (max_time > 0s) && (last_time_ + max_time < RJ::now()); + };*/ + + /** + * @return distance from this agent to ball + * wall steal + */ + double distance_to_ball() const { + return last_world_state_->ball.position.dist_to( + last_world_state_->get_robot(true, robot_id_).pose.position()); + }; + + /** + * @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 PASSING: + return RJ::Seconds{5}; + case STEALING: + return RJ::Seconds{10}; + } + } + + /** + * @return if the current state has timed out + */ + 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()); + }; + + // The time at which the last state started. + RJ::Time last_time_; + /** * @brief Adds the new waller to this robot's list of wallers and updates this robot's position * in the wall. diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 3daf2fb1bc3..3a466bc1903 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -268,7 +268,6 @@ std::optional Offense::state_to_task(RobotIntent intent) { auto collect_cmd = planning::MotionCommand{"collect"}; intent.motion_command = collect_cmd; intent.dribbler_speed = 255.0; - // } return intent; } @@ -521,48 +520,6 @@ double Offense::distance_from_their_robots(rj_geometry::Point tail, rj_geometry: return min_angle; } -bool Offense::can_steal_ball() const { - // Ball in red zone or not - if (ball_in_red()) { - return false; - } - // Ball location - rj_geometry::Point ball_position = this->last_world_state_->ball.position; - - // Our robot is closest robot to ball - bool closest = true; - - auto current_pos = last_world_state_->get_robot(true, robot_id_).pose.position(); - - auto our_dist = (current_pos - ball_position).mag(); - for (auto enemy : this->last_world_state_->their_robots) { - auto dist = (enemy.pose.position() - ball_position).mag(); - if (dist < our_dist) { - closest = false; - break; - } - } - - if (!closest) { - return closest; - } - - for (auto pal : this->last_world_state_->our_robots) { - // if (pal.robot_id_ == robot_id_) { - // continue; - // } - auto dist = (pal.pose.position() - ball_position).mag(); - if (dist < our_dist) { - closest = false; - break; - } - } - - return closest; - - // 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(); @@ -587,13 +544,6 @@ rj_geometry::Point Offense::calculate_best_shot() const { return best_shot; } -// Checks whether ball is out of range for stealing/receiving -bool Offense::ball_in_red() const { - auto& ball_pos = last_world_state_->ball.position; - return (field_dimensions_.our_defense_area().contains_point(ball_pos) || - field_dimensions_.their_defense_area().contains_point(ball_pos) || - !field_dimensions_.field_rect().contains_point(ball_pos)); -} void Offense::broadcast_seeker_request(rj_geometry::Point seeking_point, bool adding) { communication::SeekerRequest seeker_request{}; communication::generate_uid(seeker_request); diff --git a/soccer/src/soccer/strategy/agent/position/offense.hpp b/soccer/src/soccer/strategy/agent/position/offense.hpp index cbfa029496b..3c8fb3ed613 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/offense.hpp @@ -210,10 +210,10 @@ class Offense : public Position { */ 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() const; + // /** + // * @brief Check if this agent could easily steal the ball + // */ + // bool can_steal_ball() const; /** * @return distance from this agent to ball @@ -233,10 +233,10 @@ class Offense : public Position { */ rj_geometry::Point calculate_best_shot() const; - /** - * @return whether the ball is in an area that non-goalies cannot reach. - */ - bool ball_in_red() const; + // /** + // * @return whether the ball is in an area that non-goalies cannot reach. + // */ + // bool ball_in_red() const; void broadcast_seeker_request(rj_geometry::Point seeking_point, bool adding); diff --git a/soccer/src/soccer/strategy/agent/position/position.cpp b/soccer/src/soccer/strategy/agent/position/position.cpp index 91286c0ebdd..9d59a850d73 100644 --- a/soccer/src/soccer/strategy/agent/position/position.cpp +++ b/soccer/src/soccer/strategy/agent/position/position.cpp @@ -286,4 +286,54 @@ communication::Acknowledge Position::acknowledge_ball_in_transit( return acknowledge_response; } +// Checks whether ball is out of range for stealing/receiving +bool Position::ball_in_red() const { + auto& ball_pos = last_world_state_->ball.position; + return (field_dimensions_.our_defense_area().contains_point(ball_pos) || + field_dimensions_.their_defense_area().contains_point(ball_pos) || + !field_dimensions_.field_rect().contains_point(ball_pos)); +} + +bool Position::can_steal_ball() const { + // Ball in red zone or not + if (ball_in_red()) { + return false; + } + // Ball location + rj_geometry::Point ball_position = this->last_world_state_->ball.position; + + // Our robot is closest robot to ball + bool closest = true; + + auto current_pos = last_world_state_->get_robot(true, robot_id_).pose.position(); + + auto our_dist = (current_pos - ball_position).mag(); + for (auto enemy : this->last_world_state_->their_robots) { + auto dist = (enemy.pose.position() - ball_position).mag(); + if (dist < our_dist) { + closest = false; + break; + } + } + + if (!closest) { + return closest; + } + + for (auto pal : this->last_world_state_->our_robots) { + // if (pal.robot_id_ == robot_id_) { + // continue; + // } + auto dist = (pal.pose.position() - ball_position).mag(); + if (dist < our_dist) { + closest = false; + break; + } + } + + return closest; + + // return distance_to_ball() < kStealBallRadius; +} + } // namespace strategy diff --git a/soccer/src/soccer/strategy/agent/position/position.hpp b/soccer/src/soccer/strategy/agent/position/position.hpp index 191b3a16060..3ed9a776c10 100644 --- a/soccer/src/soccer/strategy/agent/position/position.hpp +++ b/soccer/src/soccer/strategy/agent/position/position.hpp @@ -272,6 +272,11 @@ class Position { */ bool check_goal_canceled(); + /** + * @brief Check if this agent could easily steal the ball + */ + bool can_steal_ball() const; + // const because should never be changed, but initializer list will allow // us to set this once initially const int robot_id_; @@ -290,6 +295,11 @@ class Position { // set to true when the ball gets close to this robot bool chasing_ball = false; + /** + * @return whether the ball is in an area that non-goalies cannot reach. + */ + bool ball_in_red() const; + // farthest distance the robot is willing to go to receive a pass static constexpr double ball_receive_distance_ = 0.1;