diff --git a/src/rj_strategy/include/rj_strategy/agent/position/solo_offense.hpp b/src/rj_strategy/include/rj_strategy/agent/position/solo_offense.hpp index 4ca2d86af5..db211d2f97 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/solo_offense.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/solo_offense.hpp @@ -33,14 +33,15 @@ class SoloOffense : public Position { private: enum State { - IDLE, // The nothing doer - MARKER, // Aggressively sit between ball and goal pos - TO_BALL, // Collect - ROTATE, // After successful collect, aim and fire - KICK // The more naive line kick + IDLE, // The nothing doer + MARKER, // Aggressively sit between ball and goal pos + TO_BALL, // Collect + ROTATE, // After successful collect, aim and fire + KICK // The more naive line kick }; - State kick_strategy_ = TO_BALL; // set to TO_BALL for collect kicking, set to KICK for line kicking - planning::LinearMotionInstant kick_target_; // aiming point + State kick_strategy_ = + TO_BALL; // set to TO_BALL for collect kicking, set to KICK for line kicking + planning::LinearMotionInstant kick_target_; // aiming point State current_state_ = IDLE; diff --git a/src/rj_strategy/include/rj_strategy/agent/position_utils.hpp b/src/rj_strategy/include/rj_strategy/agent/position_utils.hpp index c2cdb8fb6a..d6fce47e67 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position_utils.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position_utils.hpp @@ -1,7 +1,7 @@ #pragma once -#include #include +#include #include #include #include @@ -23,15 +23,17 @@ /** * This file is just to collect a bunch of common utilities. * Longterm TODO: - * - a lot functions in here do pairwise distance between various objects. can we have a coordinator that does a grand pairwise distance we can just reference? + * - a lot functions in here do pairwise distance between various objects. can we have a + * coordinator that does a grand pairwise distance we can just reference? * Rules: * - these functions have to be stateless and context-free * - try to keep big object args as const references (to minimize overhead) * - try to keep the execution time minimal (to minimize overhead) * - try to keep function names unnecessarily verbose and accurate (to minimize headache) - * - remember that if you change something, other people may be using that; don't fundamentally change invariants - * if you're doing something a little different, do it in your own file, or add a new util, avoid revising where possible - * - if you love me, do not use auto. figure out what type you want + * - remember that if you change something, other people may be using that; don't fundamentally + * change invariants if you're doing something a little different, do it in your own file, + * or add a new util, avoid revising where possible + * - please don't use auto. figure out what type you want :) */ // namespace strategy { @@ -39,7 +41,7 @@ // Example /** * @brief Euclidean distance between two points. - * + * * @param a The first point. * @param b The second point. * @return the Euclidean distance between a and b @@ -48,12 +50,10 @@ inline double distance(const rj_geometry::Point& a, const rj_geometry::Point& b) return (a - b).mag(); } - - // Geometry confirmation /** * @brief Determines whether the ball is in bounds (whole field rectangle). - * + * * @param world_state (often named last_world_state_ in Position subclasses) * @param field_dimensions (often named field_dimensions_ in Position subclasses) * @return if ball.position is in bounds @@ -65,76 +65,85 @@ inline bool ball_on_field(const WorldState* world_state, const FieldDimensions& /** * @brief Determines whether the ball is in our defense area (red region around goalie box). - * + * * @param world_state (often named last_world_state_ in Position subclasses) * @param field_dimensions (often named field_dimensions_ in Position subclasses) * @return if ball.position is in our goalie box */ -inline bool ball_in_our_defense_area(const WorldState* world_state, const FieldDimensions& field_dimensions) { +inline bool ball_in_our_defense_area(const WorldState* world_state, + const FieldDimensions& field_dimensions) { const rj_geometry::Point& ball_point = world_state->ball.position; return field_dimensions.our_defense_area().contains_point(ball_point); } /** * @brief Determines whether the ball is in our defense area (red region around goalie box). - * + * * @param world_state (often named last_world_state_ in Position subclasses) * @param field_dimensions (often named field_dimensions_ in Position subclasses) * @return if ball.position is in their goalie box */ -inline bool ball_in_their_defense_area(const WorldState* world_state, const FieldDimensions& field_dimensions) { +inline bool ball_in_their_defense_area(const WorldState* world_state, + const FieldDimensions& field_dimensions) { const rj_geometry::Point& ball_point = world_state->ball.position; return field_dimensions.their_defense_area().contains_point(ball_point); } // There is also "penalty area". This refers to the actual white lines of the goalie box. // Annoyingly, this is what is called "defense area" in the rules document. -// I have chosen not to expose this, since we have a widened red region around the box lines for rules compliance. -// We're not yet at the skill level to bother with tussles at the defense lines. +// I have chosen not to expose this, since we have a widened red region around the box lines for +// rules compliance. We're not yet at the skill level to bother with tussles at the defense lines. /** * @return Determines whether the ball is in an area that non-goalies can reach. - * + * * @param world_state (often named last_world_state_ in Position subclasses) * @param field_dimensions (often named field_dimensions_ in Position subclasses) * @return if ball.position is in playable space */ -inline bool ball_in_play_area(const WorldState* world_state, const FieldDimensions& field_dimensions) { +inline bool ball_in_play_area(const WorldState* world_state, + const FieldDimensions& field_dimensions) { return ball_on_field(world_state, field_dimensions) && !ball_in_our_defense_area(world_state, field_dimensions) && !ball_in_their_defense_area(world_state, field_dimensions); } - // Shot calculation /** * @brief Gets the shortest angular distance from a proposed shotline to an opponent. - * Can be thought of as half of the the arc of the largest circular sector centered around the shot that has no opponent bits in it. - * + * Can be thought of as half of the the arc of the largest circular sector centered around the shot + * that has no opponent bits in it. + * * The idea is that we want to take shots that are far away from opponents. - * Currently we pick the one that is furthest angularly from an opponent, to minimize the chance of them blocking. - * 0 means that there is an opponent directly on the shotline. - * pi/2 means that there is no opponent bot in the forward FOV. - * + * Currently we pick the one that is furthest angularly from an opponent, to minimize the chance of + * them blocking. 0 means that there is an opponent directly on the shotline. pi/2 means that there + * is no opponent bot in the forward FOV. + * * @param origin where the ball starts * @param shot where the shot is aimed * @param world_state (often named last_world_state_ in Position subclasses) * @return the size of the smallest angle to defender */ -inline double shot_clearance(const rj_geometry::Point& origin, const rj_geometry::Point& shot, const WorldState* world_state) { +inline double shot_clearance(const rj_geometry::Point& origin, const rj_geometry::Point& shot, + const WorldState* world_state) { rj_geometry::Point shot_vec = shot - origin; const std::vector& their_robots = world_state->their_robots; double min_angle = M_PI_2; - for (const RobotState& enemy : their_robots) { // TODO: our robots are not programmed to dodge our own shots, frankly, we may need to consider them opponents + for (const RobotState& enemy : + their_robots) { // TODO: our robots are not programmed to dodge our own shots, frankly, we + // may need to consider them opponents rj_geometry::Point enemy_vec = enemy.pose.position() - origin; if (enemy_vec.dot(shot_vec) < 0) { - continue; // if the enemy is behind us, ignore them + continue; // if the enemy is behind us, ignore them } double projection = enemy_vec.dot(shot_vec) / shot_vec.dot(shot_vec); - double dist_to_block = (enemy_vec - projection * shot_vec).mag(); // magnitude of the perpendicular component - dist_to_block = std::max(0.0, dist_to_block - kRobotRadius - kBallRadius); // (robot positions are the center, but even glancing a shot will usually block) - double dist_along_shot = projection * shot_vec.mag(); // magnitude of the tangent component + double dist_to_block = + (enemy_vec - projection * shot_vec).mag(); // magnitude of the perpendicular component + dist_to_block = std::max(0.0, dist_to_block - kRobotRadius - + kBallRadius); // (robot positions are the center, but + // even glancing a shot will usually block) + double dist_along_shot = projection * shot_vec.mag(); // magnitude of the tangent component double clearance_angle = std::atan2(dist_to_block, dist_along_shot); min_angle = std::min(min_angle, clearance_angle); @@ -145,13 +154,14 @@ inline double shot_clearance(const rj_geometry::Point& origin, const rj_geometry /** * @brief Gets a good shot worth taking, given our pratical realities. * Use this function and rest assured it will go in on an undefended goal. - * + * * @param world_state (often named last_world_state_ in Position subclasses) * @param field_dimensions (often named field_dimensions_ in Position subclasses) * @return a point to aim at for the good shot * * currently just returns the center of the goal */ -inline rj_geometry::Point calculate_a_shot([[maybe_unused]] const WorldState* world_state, const FieldDimensions& field_dimensions) { +inline rj_geometry::Point calculate_a_shot([[maybe_unused]] const WorldState* world_state, + const FieldDimensions& field_dimensions) { // TODO: could be a fun collab with hardware! // Can we quantify our uncertainty and aim closer to the goalposts when close enough? return field_dimensions.their_goal_loc(); @@ -159,19 +169,25 @@ inline rj_geometry::Point calculate_a_shot([[maybe_unused]] const WorldState* wo /** * @brief Gets a the best shot available, given excellent aim. - * The definition of "best" may need refining, but I think my approach is sound. - * + * The definition of "best" may need refining, but I think my approach is sound. + * * @param world_state (often named last_world_state_ in Position subclasses) * @param field_dimensions (often named field_dimensions_ in Position subclasses) * @param granularity the spacing between considered points on the goal line (meters) - * @param ignore_posts whether to count the goalposts as candidate shots (you should use True, aiming for the goalposts is stupid) + * @param ignore_posts whether to count the goalposts as candidate shots (you should use True, + * aiming for the goalposts is stupid) * @return a point to aim at for the best shot */ -inline rj_geometry::Point calculate_best_shot(const WorldState* world_state, const FieldDimensions& field_dimensions, double granularity, bool ignore_posts) { +inline rj_geometry::Point calculate_best_shot(const WorldState* world_state, + const FieldDimensions& field_dimensions, + double granularity, bool ignore_posts) { // Geometry - rj_geometry::Point their_goal_pos = field_dimensions.their_goal_loc(); // returns center of goal - if (granularity <= 0.0) { // protection - SPDLOG_ERROR("Invalid granularity value passed into calculate_best_shot, must use a positive float."); + rj_geometry::Point their_goal_pos = + field_dimensions.their_goal_loc(); // returns center of goal + if (granularity <= 0.0) { // protection + SPDLOG_ERROR( + "Invalid granularity value passed into calculate_best_shot, must use a positive " + "float."); return their_goal_pos; } double goal_width = field_dimensions.goal_width(); @@ -179,7 +195,7 @@ inline rj_geometry::Point calculate_best_shot(const WorldState* world_state, con rj_geometry::Point posXmost_shot = their_goal_pos + rj_geometry::Point(goal_width / 2.0, 0.0); rj_geometry::Point increment(granularity, 0.0); if (ignore_posts) { - negXmost_shot = negXmost_shot + increment; + negXmost_shot = negXmost_shot + increment; posXmost_shot = posXmost_shot - increment; } rj_geometry::Point ball_pos = world_state->ball.position; @@ -187,8 +203,9 @@ inline rj_geometry::Point calculate_best_shot(const WorldState* world_state, con // Argmaxxing rj_geometry::Point best_shot(0.0, 0.0); double curr_clearance; - double best_clearance = -1.0; // clearance is in [0,pi/2], so this will be immediately overwritten - + double best_clearance = + -1.0; // clearance is in [0,pi/2], so this will be immediately overwritten + rj_geometry::Point curr_shot = negXmost_shot; while (curr_shot.x() <= posXmost_shot.x()) { curr_clearance = shot_clearance(ball_pos, curr_shot, world_state); @@ -208,11 +225,13 @@ inline rj_geometry::Point calculate_best_shot(const WorldState* world_state, con } // Possession calculation -// TODO: these functions should also account for rotation. a robot doesn't have possession if the ball is sitting at its rear motors -// TODO: overload these functions so that we can have some default possession_radius, such that people using it don't need to turn on they brain +// TODO: these functions should also account for rotation. a robot doesn't have possession if the +// ball is sitting at its rear motors +// TODO: overload these functions so that we can have some default possession_radius, such that +// people using it don't need to turn on they brain /** * @brief Determines whether the enemy has possession of the ball. - * + * * @param world_state (often named last_world_state_ in Position subclasses) * @param possession_radius the distance at which a robot is defined to "have" the ball (m) * @return do they have ball @@ -231,7 +250,7 @@ inline bool they_have_ball(const WorldState* world_state, double possession_radi /** * @brief Determines whether our team has possession of the ball. - * + * * @param world_state (often named last_world_state_ in Position subclasses) * @param possession_radius the distance at which a robot is defined to "have" the ball (m) * @return do we have ball @@ -250,12 +269,13 @@ inline bool we_have_ball(const WorldState* world_state, double possession_radius /** * @brief Determines whether a specific robot has possession of the ball. - * + * * @param world_state (often named last_world_state_ in Position subclasses) * @param possession_radius the distance at which a robot is defined to "have" the ball (m) * @return does it have ball */ -inline bool robot_has_ball(const WorldState* world_state, const RobotState& robot, double possession_radius) { +inline bool robot_has_ball(const WorldState* world_state, const RobotState& robot, + double possession_radius) { rj_geometry::Point ball_pos = world_state->ball.position; rj_geometry::Point robot_pos = robot.pose.position(); if (distance(ball_pos, robot_pos) < possession_radius) { @@ -268,18 +288,20 @@ inline bool robot_has_ball(const WorldState* world_state, const RobotState& robo // Kick speed calculation /** * @brief Provides a good suggestion for kick speed, designed for passing. - * + * * Motion intents take an integer from [0,15]. This provides that. - * + * * @param distance_to_target distance from kicker to target (m) - * @param intended_velo_at_target the velocity you want the ball to be going when it reaches the target (m/s) + * @param intended_velo_at_target the velocity you want the ball to be going when it reaches the + * target (m/s) * @return an int to shove in the motion command */ -inline int calculate_kick_speed(double distance_to_target, [[maybe_unused]] double intended_velo_at_target) { +inline int calculate_kick_speed(double distance_to_target, + [[maybe_unused]] double intended_velo_at_target) { // TODO: this could be a fun collab with hardware! - // v^2 = v_initial^2 - 2ad, where a is a measure of the constant deceleration applied from the ground - // Frankly, that changes from pitch to pitch, so it should probably be parametrized in rqt. - // We would also then need a map from v_initial to power[0,15] + // v^2 = v_initial^2 - 2ad, where a is a measure of the constant deceleration applied from the + // ground Frankly, that changes from pitch to pitch, so it should probably be parametrized in + // rqt. We would also then need a map from v_initial to power[0,15] // The below code is very dumb. I stole it from Offense. if (distance_to_target < 0.6) { @@ -293,9 +315,9 @@ inline int calculate_kick_speed(double distance_to_target, [[maybe_unused]] doub /** * @brief Provides the max allowed kick speed, designed for shooting. - * + * * Motion intents take an integer from [0,15]. This provides that. - * + * * @return 7 */ inline int max_kick_speed() { diff --git a/src/rj_strategy/src/agent/position/offense.cpp b/src/rj_strategy/src/agent/position/offense.cpp index 922918439b..4e9c8c42be 100644 --- a/src/rj_strategy/src/agent/position/offense.cpp +++ b/src/rj_strategy/src/agent/position/offense.cpp @@ -170,7 +170,8 @@ Offense::State Offense::next_state() { } case SHOOTING: { - if (!ball_in_play_area(last_world_state_, field_dimensions_) || check_is_done() || !has_open_shot() || !can_steal_ball()) { + if (!ball_in_play_area(last_world_state_, field_dimensions_) || check_is_done() || + !has_open_shot() || !can_steal_ball()) { return DEFAULT; } // if (distance_to_ball() > kOwnBallRadius) { @@ -275,13 +276,14 @@ std::optional Offense::state_to_task(RobotIntent intent) { case SHOOTING: { // link kick because collect is garbage - planning::LinearMotionInstant target{calculate_best_shot(last_world_state_, field_dimensions_, 0.04, true)}; + planning::LinearMotionInstant target{ + calculate_best_shot(last_world_state_, field_dimensions_, 0.04, true)}; auto shoot_cmd = planning::MotionCommand{"line_kick", target, planning::FaceTarget{}, true}; intent.motion_command = shoot_cmd; intent.trigger_mode = RobotIntent::TriggerMode::ON_BREAK_BEAM; - intent.kick_speed = max_kick_speed(); // Integer value in [0,15] + intent.kick_speed = max_kick_speed(); // Integer value in [0,15] return intent; } } @@ -415,9 +417,12 @@ void Offense::derived_acknowledge_ball_in_transit() { } bool Offense::has_open_shot() const { - rj_geometry::Point best_shot = calculate_best_shot(last_world_state_, field_dimensions_, 0.04, true); - double clearance_angle = shot_clearance(last_world_state_->ball.position, best_shot, last_world_state_); - return clearance_angle >= 0.05; // if there's >= 3 degrees (0.05 radians) of clearance, that's an open shot + rj_geometry::Point best_shot = + calculate_best_shot(last_world_state_, field_dimensions_, 0.04, true); + double clearance_angle = + shot_clearance(last_world_state_->ball.position, best_shot, last_world_state_); + return clearance_angle >= + 0.05; // if there's >= 3 degrees (0.05 radians) of clearance, that's an open shot } diff --git a/src/rj_strategy/src/agent/position/solo_offense.cpp b/src/rj_strategy/src/agent/position/solo_offense.cpp index 0b2dae11a0..2b31d897b4 100644 --- a/src/rj_strategy/src/agent/position/solo_offense.cpp +++ b/src/rj_strategy/src/agent/position/solo_offense.cpp @@ -9,7 +9,8 @@ SoloOffense::SoloOffense(Position&& other) : Position{std::move(other)} { SoloOffense::SoloOffense(int r_id) : Position{r_id, "SoloOffense"} {} std::string SoloOffense::get_current_state() { - return std::string{"Solo Offense ("} + std::to_string(robot_id_) + std::string{") - "} + std::string{state_to_name(current_state_)}; + return std::string{"Solo Offense ("} + std::to_string(robot_id_) + std::string{") - "} + + std::string{state_to_name(current_state_)}; } std::optional SoloOffense::derived_get_task(RobotIntent intent) { @@ -23,47 +24,61 @@ SoloOffense::State SoloOffense::next_state() { if (!ball_in_play_area(last_world_state_, field_dimensions_)) { return IDLE; - } // High-level conditional: SoloOffense does not think when the ball is not playable - if (we_have_ball(last_world_state_, 2*kRobotRadius) && !robot_has_ball(last_world_state_, me, 2*kRobotRadius)) { + } // High-level conditional: SoloOffense does not think when the ball is not playable + if (we_have_ball(last_world_state_, 2 * kRobotRadius) && + !robot_has_ball(last_world_state_, me, 2 * kRobotRadius)) { return IDLE; - } // High-level conditional: SoloOffense does not think when teammates are handling the ball - if (they_have_ball(last_world_state_, 2*kRobotRadius)) { + } // High-level conditional: SoloOffense does not think when teammates are handling the ball + if (they_have_ball(last_world_state_, 2 * kRobotRadius)) { return MARKER; - } // High-level conditional: SoloOffense is bullyable; if the opponents have the ball, they give up shooting and camp the ball - // ^^ that's particularly bad strategy, it's legal for opponents to just roll up on us, but whatever, this is a test Position - + } // High-level conditional: SoloOffense is bullyable; if the opponents have the ball, they + // give up shooting and camp the ball + // ^^ that's particularly bad strategy, it's legal for opponents to just roll up on us, but + // whatever, this is a test Position switch (current_state_) { case IDLE: { // When thinking, SoloOffense will immediately leave IDLE. - kick_target_ = planning::LinearMotionInstant{calculate_best_shot(last_world_state_, field_dimensions_, 0.1, true)}; + kick_target_ = planning::LinearMotionInstant{ + calculate_best_shot(last_world_state_, field_dimensions_, 0.1, true)}; return kick_strategy_; } case MARKER: { // If the opponent has lost possession (see above), SoloOffense will attempt a collect. - kick_target_ = planning::LinearMotionInstant{calculate_best_shot(last_world_state_, field_dimensions_, 0.1, true)}; + kick_target_ = planning::LinearMotionInstant{ + calculate_best_shot(last_world_state_, field_dimensions_, 0.1, true)}; return kick_strategy_; } case TO_BALL: { // If a collect is successful, go to a rotate kick. - if (check_is_done()) { return ROTATE; } - else { return TO_BALL; } + if (check_is_done()) { + return ROTATE; + } else { + return TO_BALL; + } // TODO: the else statement needs logic for a failed collect - // the high-level conditionals catch normal game cases, but suppose a HALT interrupts a TO_BALL state, would it resume in TO_BALL? + // the high-level conditionals catch normal game cases, but suppose a HALT interrupts a + // TO_BALL state, would it resume in TO_BALL? } case ROTATE: { - // TODO: this state needs logic to go back to IDLE early if we drop the ball while rotating. - // If a kick is successful, restart the logic tree. - if (check_is_done()) { return IDLE; } - else { return ROTATE; } + // TODO: this state needs logic to go back to IDLE early if we drop the ball while + // rotating. If a kick is successful, restart the logic tree. + if (check_is_done()) { + return IDLE; + } else { + return ROTATE; + } } case KICK: { // If a kick is successful, restart the logic tree. - if (check_is_done()) { return IDLE; } - else { return ROTATE; } + if (check_is_done()) { + return IDLE; + } else { + return ROTATE; + } } default: { - return current_state_; // unreachable, but compiler wants it + return current_state_; // unreachable, but compiler wants it } } } @@ -71,16 +86,19 @@ SoloOffense::State SoloOffense::next_state() { std::optional SoloOffense::state_to_task(RobotIntent intent) { switch (current_state_) { case IDLE: { - return intent; // TODO: how to notate an idling intent? + return intent; // TODO: how to notate an idling intent? } case MARKER: { // We want to be 5 radii from the ball toward the goal; blocking shots! baskingball :) rj_geometry::Point ball_pos = last_world_state_->ball.position; rj_geometry::Point defending_pos = field_dimensions_.our_goal_loc(); - rj_geometry::Point offset_vector = (defending_pos - ball_pos).normalized(kRobotRadius * 5); + rj_geometry::Point offset_vector = + (defending_pos - ball_pos).normalized(kRobotRadius * 5); rj_geometry::Point target_pos = ball_pos + offset_vector; - - auto mark_cmd = planning::MotionCommand{"path_target", planning::LinearMotionInstant{target_pos}, planning::FaceBall{}, true}; + + auto mark_cmd = + planning::MotionCommand{"path_target", planning::LinearMotionInstant{target_pos}, + planning::FaceBall{}, true}; intent.motion_command = mark_cmd; return intent; } @@ -92,7 +110,8 @@ std::optional SoloOffense::state_to_task(RobotIntent intent) { } case ROTATE: { // Rotate toward the goal, then shoot. - auto pivot_cmd = planning::MotionCommand{"rotate", kick_target_, planning::FaceTarget{}, false}; + auto pivot_cmd = + planning::MotionCommand{"rotate", kick_target_, planning::FaceTarget{}, false}; intent.motion_command = pivot_cmd; intent.dribbler_mode = RobotIntent::DribblerMode::ON; intent.trigger_mode = RobotIntent::TriggerMode::AT_END; @@ -101,7 +120,8 @@ std::optional SoloOffense::state_to_task(RobotIntent intent) { } case KICK: { // Drive behind the ball, then shoot with a run-up. - auto kick_cmd = planning::MotionCommand{"line_kick", kick_target_, planning::FaceTarget{}, true}; + auto kick_cmd = + planning::MotionCommand{"line_kick", kick_target_, planning::FaceTarget{}, true}; intent.motion_command = kick_cmd; intent.shoot_mode = RobotIntent::ShootMode::KICK; intent.trigger_mode = RobotIntent::TriggerMode::ON_BREAK_BEAM;