diff --git a/docs/fsm-diagrams.md b/docs/fsm-diagrams.md
index 8059b8e10d..541682f7d6 100644
--- a/docs/fsm-diagrams.md
+++ b/docs/fsm-diagrams.md
@@ -33,20 +33,24 @@ stateDiagram-v2
classDef terminate fill:white,color:black,font-weight:bold
direction LR
[*] --> StartState
-StartState --> AlignPlacementState : [!shouldKickOffWall]\nalignPlacement
-StartState --> KickOffWallState : [shouldKickOffWall]
-KickOffWallState --> KickOffWallState : [!kickDone && shouldKickOffWall]\nkickOffWall
-KickOffWallState --> KickOffWallState : [kickDone]
-KickOffWallState --> AlignPlacementState : [!kickDone]
-AlignPlacementState --> KickOffWallState : [shouldKickOffWall]
+StartState --> AlignPlacementState : [!shouldPickOffWall]\nalignPlacement
+StartState --> AlignWallState : [shouldPickOffWall]\nalignWall
+AlignWallState --> AlignWallState : [!wallAlignDone && shouldPickOffWall]\nalignWall
+AlignWallState --> PickOffWallState : [wallAlignDone]\nsetPickOffDest, pickOffWall
+AlignWallState --> AlignPlacementState : [!shouldPickOffWall]\nalignPlacement
+PickOffWallState --> PickOffWallState : [!wallPickOffDone]\npickOffWall
+PickOffWallState --> ReleaseBallState : [wallPickOffDone]\nstartWait
+AlignPlacementState --> AlignWallState : [shouldPickOffWall]\nalignWall
AlignPlacementState --> AlignPlacementState : [!alignDone]\nalignPlacement
-AlignPlacementState --> PlaceBallState : [alignDone]
+AlignPlacementState --> PlaceBallState : [alignDone]\nplaceBall
PlaceBallState --> PlaceBallState : [!ballPlaced]\nplaceBall
-PlaceBallState --> WaitState : [ballPlaced]\nstartWait
-WaitState --> WaitState : [!waitDone]
-WaitState --> RetreatState : [waitDone]
+PlaceBallState --> ReleaseBallState : [ballPlaced]\nstartWait
+ReleaseBallState --> ReleaseBallState : [!waitDone && ballPlaced]\nreleaseBall
+ReleaseBallState --> StartState : [!ballPlaced]
+ReleaseBallState --> RetreatState : [waitDone]\nretreat
RetreatState --> Terminate:::terminate : [retreatDone && ballPlaced]
RetreatState --> RetreatState : [ballPlaced]\nretreat
+RetreatState --> StartState : [!ballPlaced]
```
@@ -403,7 +407,7 @@ direction LR
MoveState --> MoveState : [!moveDone]\nupdateMove
MoveState --> Terminate:::terminate : [moveDone]\nupdateMove
Terminate:::terminate --> MoveState : [!moveDone]\nupdateMove
-Terminate:::terminate --> Terminate:::terminate : updateMove
+Terminate:::terminate --> Terminate:::terminate : SET_STOP_PRIMITIVE_ACTION
```
diff --git a/src/cli/cli_params.py b/src/cli/cli_params.py
index 1bb9e52372..26c5bd8398 100644
--- a/src/cli/cli_params.py
+++ b/src/cli/cli_params.py
@@ -88,4 +88,4 @@ class Platform(str, Enum):
EnableThunderscopeOption = Annotated[bool, Option("-t", "--enable_thunderscope")]
EnableVisualizerOption = Annotated[bool, Option("-v", "--enable_visualizer")]
-StopAIOnStartOption = Annotated[bool, Option("-t", "--stop_ai_on_start")]
+StopAIOnStartOption = Annotated[bool, Option("-s", "--stop_ai_on_start")]
diff --git a/src/proto/message_translation/tbots_protobuf.cpp b/src/proto/message_translation/tbots_protobuf.cpp
index cc21417cd7..5c30e6e6e6 100644
--- a/src/proto/message_translation/tbots_protobuf.cpp
+++ b/src/proto/message_translation/tbots_protobuf.cpp
@@ -499,6 +499,8 @@ int convertDribblerModeToDribblerSpeed(TbotsProto::DribblerMode dribbler_mode,
return robot_constants.max_force_dribbler_speed_rpm;
case TbotsProto::DribblerMode::OFF:
return 0;
+ case TbotsProto::DribblerMode::RELEASE_BALL_SLOW:
+ return robot_constants.release_ball_dribbler_speed_rpm;
default:
LOG(WARNING) << "DribblerMode is invalid" << std::endl;
return 0;
@@ -518,6 +520,12 @@ double convertMaxAllowedSpeedModeToMaxAllowedSpeed(
STOP_COMMAND_SPEED_SAFETY_MARGIN_METERS_PER_SECOND;
case TbotsProto::MaxAllowedSpeedMode::COLLISIONS_ALLOWED:
return COLLISION_ALLOWED_ROBOT_MAX_SPEED_METERS_PER_SECOND;
+ case TbotsProto::MaxAllowedSpeedMode::BALL_PLACEMENT_RETREAT:
+ return robot_constants.ball_placement_retreat_max_speed_m_per_s;
+ case TbotsProto::MaxAllowedSpeedMode::BALL_PLACEMENT_WALL_DRIBBLE:
+ return robot_constants.ball_placement_wall_max_speed_m_per_s;
+ case TbotsProto::MaxAllowedSpeedMode::DRIBBLE:
+ return robot_constants.dribble_speed_m_per_s;
default:
LOG(WARNING) << "MaxAllowedSpeedMode is invalid" << std::endl;
return 0.0;
diff --git a/src/proto/parameters.proto b/src/proto/parameters.proto
index 662325f03d..4e03a53f63 100644
--- a/src/proto/parameters.proto
+++ b/src/proto/parameters.proto
@@ -462,7 +462,7 @@ message SensorFusionConfig
// Number of dropped detections before we consider the ball not to be in the dribbler
required int32 num_dropped_detections_before_ball_not_in_dribbler = 12
- [default = 3, (bounds).min_int_value = 0, (bounds).max_int_value = 1000];
+ [default = 10, (bounds).min_int_value = 0, (bounds).max_int_value = 1000];
// Possession tracker for determining which team has possession of the ball
required PossessionTrackerConfig possession_tracker_config = 13;
@@ -562,6 +562,13 @@ message RobotNavigationObstacleConfig
// robot's position for obstacle avoidance
required double dynamic_enemy_robot_obstacle_horizon_sec = 4
[default = 0.4, (bounds).min_double_value = 0.0, (bounds).max_double_value = 2.0];
+
+ // Additional amount to inflate ball obstacle by for extra safety
+ required double additional_ball_obstacle_inflation_meters = 5 [
+ default = 0.05,
+ (bounds).min_double_value = 0.0,
+ (bounds).max_double_value = 0.5
+ ];
}
message CostVisualizationConfig
diff --git a/src/proto/primitive.proto b/src/proto/primitive.proto
index 7b8ae7bf53..99d944da29 100644
--- a/src/proto/primitive.proto
+++ b/src/proto/primitive.proto
@@ -57,9 +57,10 @@ enum MotionConstraint
enum DribblerMode
{
- OFF = 0;
- INDEFINITE = 1;
- MAX_FORCE = 2;
+ OFF = 0;
+ INDEFINITE = 1;
+ MAX_FORCE = 2;
+ RELEASE_BALL_SLOW = 3;
}
enum MaxAllowedSpeedMode
@@ -70,6 +71,12 @@ enum MaxAllowedSpeedMode
STOP_COMMAND = 1;
// Move at speed at which collisions are allowed
COLLISIONS_ALLOWED = 2;
+ // Slow speed for retreating in ball placement
+ BALL_PLACEMENT_RETREAT = 3;
+ // Slow speed for picking the ball off the wall in ball placement
+ BALL_PLACEMENT_WALL_DRIBBLE = 4;
+ // Slower speed for dribbling
+ DRIBBLE = 5;
}
message AutoChipOrKick
diff --git a/src/shared/2021_robot_constants.cpp b/src/shared/2021_robot_constants.cpp
index 1513e825f5..1f3a9597c4 100644
--- a/src/shared/2021_robot_constants.cpp
+++ b/src/shared/2021_robot_constants.cpp
@@ -17,16 +17,20 @@ RobotConstants_t create2021RobotConstants(void)
.front_of_robot_width_meters = 0.11f,
.dribbler_width_meters = 0.07825f,
// Dribbler speeds are negative as that is the direction that sucks the ball in
- .indefinite_dribbler_speed_rpm = -10000,
- .max_force_dribbler_speed_rpm = -12000,
+ .indefinite_dribbler_speed_rpm = -10000,
+ .max_force_dribbler_speed_rpm = -12000,
+ .release_ball_dribbler_speed_rpm = 2000,
// Motor constant
.motor_max_acceleration_m_per_s_2 = 4.5f,
// Robot's linear movement constants
- .robot_max_speed_m_per_s = 3.000f,
- .robot_max_acceleration_m_per_s_2 = 3.0f,
- .robot_max_deceleration_m_per_s_2 = 3.0f,
+ .robot_max_speed_m_per_s = 3.000f,
+ .ball_placement_wall_max_speed_m_per_s = 0.3f,
+ .ball_placement_retreat_max_speed_m_per_s = 0.3f,
+ .dribble_speed_m_per_s = 1.5f,
+ .robot_max_acceleration_m_per_s_2 = 3.0f,
+ .robot_max_deceleration_m_per_s_2 = 3.0f,
// Robot's angular movement constants
.robot_max_ang_speed_rad_per_s = 10.0f,
diff --git a/src/shared/constants.h b/src/shared/constants.h
index d5047d92b9..1a2554a27b 100644
--- a/src/shared/constants.h
+++ b/src/shared/constants.h
@@ -61,25 +61,25 @@ static const short unsigned int MAXIMUM_TRANSFER_UNIT_BYTES = 1500;
/* Game Rules */
// The max allowed speed of the ball, in metres per second
// https://robocup-ssl.github.io/ssl-rules/sslrules.html#_ball_speed
-static const double BALL_MAX_SPEED_METERS_PER_SECOND = 6.5;
+static constexpr double BALL_MAX_SPEED_METERS_PER_SECOND = 6.5;
// The safe max speed of the ball that we should shoot at, in metres per second
-static const double BALL_SAFE_MAX_SPEED_METERS_PER_SECOND =
+static constexpr double BALL_SAFE_MAX_SPEED_METERS_PER_SECOND =
BALL_MAX_SPEED_METERS_PER_SECOND - 0.5;
// The distance that the ball has to travel for it to be considered in play
// after a kick-off, free kick, or penalty kick.
// https://robocup-ssl.github.io/ssl-rules/sslrules.html#_ball_in_and_out_of_play
-static const double BALL_IN_PLAY_DISTANCE_THRESHOLD_METERS = 0.05;
+static constexpr double BALL_IN_PLAY_DISTANCE_THRESHOLD_METERS = 0.05;
// The max allowed height of the robots, in metres
-static const double ROBOT_MAX_HEIGHT_METERS = 0.15;
+static constexpr double ROBOT_MAX_HEIGHT_METERS = 0.15;
// The max allowed radius of the robots, in metres
-static const double ROBOT_MAX_RADIUS_METERS = 0.09;
+static constexpr double ROBOT_MAX_RADIUS_METERS = 0.09;
// The distance from the center of the robot to the front face (the flat part), in meters
-static const double DIST_TO_FRONT_OF_ROBOT_METERS = 0.078;
+static constexpr double DIST_TO_FRONT_OF_ROBOT_METERS = 0.078;
// The approximate radius of the ball according to the SSL rulebook
-static const double BALL_MAX_RADIUS_METERS = 0.0215;
+static constexpr double BALL_MAX_RADIUS_METERS = 0.0215;
// According to the rules, 80% of the ball must be seen at all times. Robots may not
// cover more than 20% of the ball
-static const double MAX_FRACTION_OF_BALL_COVERED_BY_ROBOT = 0.2;
+static constexpr double MAX_FRACTION_OF_BALL_COVERED_BY_ROBOT = 0.2;
// The radius of a circle region where ball placement is acceptable (in meters).
constexpr double BALL_PLACEMENT_TOLERANCE_RADIUS_METERS = 0.15;
diff --git a/src/shared/robot_constants.h b/src/shared/robot_constants.h
index af9bbee29e..afdc9deebf 100644
--- a/src/shared/robot_constants.h
+++ b/src/shared/robot_constants.h
@@ -73,12 +73,25 @@ typedef struct RobotConstants
// the ball [rpm]
int max_force_dribbler_speed_rpm;
+ // Speed at which to spin the dribbler to release the ball right in front of a
+ // stationary robot [rpm]
+ int release_ball_dribbler_speed_rpm;
+
// The maximum acceleration achievable by our motors [m/s^2]
float motor_max_acceleration_m_per_s_2;
// The maximum speed achievable by our robots, in metres per second [m/s]
float robot_max_speed_m_per_s;
+ // The max speed at which we will pick the ball off the wall
+ float ball_placement_wall_max_speed_m_per_s;
+
+ // The max speed at which we will retreat away from the ball after placing it [m/s]
+ float ball_placement_retreat_max_speed_m_per_s;
+
+ // The max speed at which we dribble the ball
+ float dribble_speed_m_per_s;
+
// The maximum acceleration achievable by our robots [m/s^2]
float robot_max_acceleration_m_per_s_2;
diff --git a/src/software/ai/hl/stp/play/ball_placement/ball_placement_play_fsm.cpp b/src/software/ai/hl/stp/play/ball_placement/ball_placement_play_fsm.cpp
index 63037ab4b0..a3a04c6313 100644
--- a/src/software/ai/hl/stp/play/ball_placement/ball_placement_play_fsm.cpp
+++ b/src/software/ai/hl/stp/play/ball_placement/ball_placement_play_fsm.cpp
@@ -3,96 +3,158 @@
BallPlacementPlayFSM::BallPlacementPlayFSM(
std::shared_ptr ai_config_ptr)
: PlayFSM(ai_config_ptr),
- pivot_kick_tactic(std::make_shared(ai_config_ptr)),
- place_ball_tactic(std::make_shared(ai_config_ptr)),
- align_placement_tactic(std::make_shared(ai_config_ptr)),
+ align_wall_tactic(std::make_shared(ai_config_ptr)),
+ pickoff_wall_tactic(std::make_shared(ai_config_ptr)),
+ place_ball_tactic(std::make_shared(ai_config_ptr)),
+ align_placement_tactic(std::make_shared(ai_config_ptr)),
retreat_tactic(std::make_shared(ai_config_ptr)),
- move_tactics(std::vector>())
+ wait_tactic(std::make_shared(ai_config_ptr)),
+ move_tactics(std::vector>())
{
}
-void BallPlacementPlayFSM::kickOffWall(const Update &event)
+// Subtract b from a, or return 0 if b is larger than a
+static unsigned int subSat(unsigned int a, unsigned int b)
{
- PriorityTacticVector tactics_to_run = {{}};
+ if (b > a)
+ {
+ return 0;
+ }
+ else
+ {
+ return a - b;
+ }
+}
+
+void BallPlacementPlayFSM::alignWall(const Update &event)
+{
+ PriorityTacticVector tactics_to_run = {{align_wall_tactic}};
+
+ setPickOffDest(event);
- // setup move tactics for robots away from ball placing robot
- setupMoveTactics(event);
+ align_wall_tactic->updateControlParams(
+ pickoff_point, pickoff_final_orientation, TbotsProto::DribblerMode::OFF,
+ TbotsProto::BallCollisionType::AVOID, {AutoChipOrKickMode::OFF, 0},
+ TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT,
+ TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE);
+
+ setupMoveTactics(event, subSat(event.common.num_tactics,
+ static_cast(tactics_to_run[0].size())));
tactics_to_run[0].insert(tactics_to_run[0].end(), move_tactics.begin(),
move_tactics.end());
- // setup wall kickoff tactic for ball placing robot
- Point ball_pos = event.common.world_ptr->ball().position();
- Rectangle field_lines = event.common.world_ptr->field().fieldLines();
- AutoChipOrKick auto_chick = {AutoChipOrKickMode::AUTOKICK,
- WALL_KICKOFF_VELOCITY_M_PER_S};
+ event.common.set_tactics(tactics_to_run);
+}
+
+void BallPlacementPlayFSM::setPickOffDest(const Update &event)
+{
+ Point ball_pos = event.common.world_ptr->ball().position();
+ Rectangle field_boundary = event.common.world_ptr->field().fieldBoundary();
+
+ auto [orientation, dest] = calculateWallPickOffDest(ball_pos, field_boundary);
+ pickoff_final_orientation = orientation;
+ pickoff_destination = dest;
- Angle kick_angle = calculateWallKickoffAngle(ball_pos, field_lines);
- pivot_kick_tactic->updateControlParams(ball_pos, kick_angle, auto_chick);
- tactics_to_run[0].emplace_back(pivot_kick_tactic);
+ Vector approach_vector = Vector::createFromAngle(orientation);
+ pickoff_point = ball_pos - approach_vector.normalize(ALIGNMENT_VECTOR_LENGTH_M);
+}
+
+void BallPlacementPlayFSM::pickOffWall(const Update &event)
+{
+ PriorityTacticVector tactics_to_run = {{pickoff_wall_tactic}};
+
+ pickoff_wall_tactic->updateControlParams(
+ pickoff_destination, pickoff_final_orientation, true,
+ TbotsProto::MaxAllowedSpeedMode::BALL_PLACEMENT_WALL_DRIBBLE,
+ TbotsProto::MaxAllowedSpeedMode::BALL_PLACEMENT_WALL_DRIBBLE);
+
+ setupMoveTactics(event, subSat(event.common.num_tactics,
+ static_cast(tactics_to_run[0].size())));
+ tactics_to_run[0].insert(tactics_to_run[0].end(), move_tactics.begin(),
+ move_tactics.end());
event.common.set_tactics(tactics_to_run);
}
void BallPlacementPlayFSM::alignPlacement(const Update &event)
{
+ PriorityTacticVector tactics_to_run = {{}};
+
std::optional placement_point =
event.common.world_ptr->gameState().getBallPlacementPoint();
if (placement_point.has_value())
{
- PriorityTacticVector tactics_to_run = {{}};
-
- // setup move tactics for robots away from ball placing robot
- setupMoveTactics(event);
- tactics_to_run[0].insert(tactics_to_run[0].end(), move_tactics.begin(),
- move_tactics.end());
-
// find position behind the ball where the ball is aligned directly in front
// placement point from the placing robot's POV
Vector alignment_vector =
(placement_point.value() - event.common.world_ptr->ball().position())
- .normalize();
+ .normalize(ALIGNMENT_VECTOR_LENGTH_M);
Angle setup_angle = alignment_vector.orientation();
- setup_point = event.common.world_ptr->ball().position() -
- 2 * alignment_vector * ROBOT_MAX_RADIUS_METERS;
+ setup_point = event.common.world_ptr->ball().position() - alignment_vector;
align_placement_tactic->updateControlParams(
setup_point, setup_angle, TbotsProto::DribblerMode::OFF,
TbotsProto::BallCollisionType::AVOID, {AutoChipOrKickMode::OFF, 0},
- TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT,
+ TbotsProto::MaxAllowedSpeedMode::DRIBBLE,
TbotsProto::ObstacleAvoidanceMode::SAFE);
- tactics_to_run[0].emplace_back(align_placement_tactic);
-
- event.common.set_tactics(tactics_to_run);
+ tactics_to_run[0].push_back(align_placement_tactic);
}
+
+ setupMoveTactics(event, subSat(event.common.num_tactics,
+ static_cast(tactics_to_run[0].size())));
+ tactics_to_run[0].insert(tactics_to_run[0].end(), move_tactics.begin(),
+ move_tactics.end());
+
+ event.common.set_tactics(tactics_to_run);
}
void BallPlacementPlayFSM::placeBall(const Update &event)
{
PriorityTacticVector tactics_to_run = {{}};
- // setup move tactics for robots away from ball placing robot
- setupMoveTactics(event);
- tactics_to_run[0].insert(tactics_to_run[0].end(), move_tactics.begin(),
- move_tactics.end());
-
- Angle final_angle = Angle::zero();
std::optional placement_point =
event.common.world_ptr->gameState().getBallPlacementPoint();
- Vector placement_dribble_vector;
if (placement_point.has_value())
{
- placement_dribble_vector =
- placement_point.value() - event.common.world_ptr->ball().position();
- final_angle = placement_dribble_vector.orientation();
+ Point ball_pos = event.common.world_ptr->ball().position();
+ std::optional robot_placing_ball =
+ event.common.world_ptr->friendlyTeam().getNearestRobot(ball_pos);
+
+ if (robot_placing_ball.has_value())
+ {
+ Vector placement_vector =
+ placement_point.value() - event.common.world_ptr->ball().position();
+
+ Angle final_angle;
+ if (placement_vector.length() > APPROACHING_PLACEMENT_DIST_THRESHOLD_M)
+ {
+ // We are still far away from the placement point, so just point
+ // towards it
+ final_angle = placement_vector.orientation();
+ }
+ else
+ {
+ // We are near the placement point; keep the same orientation
+ // so we don't switch direction (in case the ball rolls slightly
+ // past the placement point)
+ final_angle = robot_placing_ball->orientation();
+ }
+
+ place_ball_tactic->updateControlParams(
+ event.common.world_ptr->gameState().getBallPlacementPoint(), final_angle,
+ true, TbotsProto::MaxAllowedSpeedMode::DRIBBLE);
+
+ tactics_to_run[0].push_back(place_ball_tactic);
+ }
}
- // setup ball placement tactic for ball placing robot
- place_ball_tactic->updateControlParams(
- event.common.world_ptr->gameState().getBallPlacementPoint(), final_angle, true);
- tactics_to_run[0].emplace_back(place_ball_tactic);
+ setupMoveTactics(event, subSat(event.common.num_tactics,
+ static_cast(tactics_to_run[0].size())));
+ tactics_to_run[0].insert(tactics_to_run[0].end(), move_tactics.begin(),
+ move_tactics.end());
event.common.set_tactics(tactics_to_run);
}
@@ -102,64 +164,92 @@ void BallPlacementPlayFSM::startWait(const Update &event)
start_time = event.common.world_ptr->getMostRecentTimestamp();
}
-void BallPlacementPlayFSM::retreat(const Update &event)
+void BallPlacementPlayFSM::releaseBall(const Update &event)
{
+ PriorityTacticVector tactics_to_run = {{}};
+
WorldPtr world_ptr = event.common.world_ptr;
std::optional nearest_robot =
world_ptr->friendlyTeam().getNearestRobot(world_ptr->ball().position());
if (nearest_robot.has_value())
{
- PriorityTacticVector tactics_to_run = {{}};
+ wait_tactic->updateControlParams(
+ nearest_robot->position(), nearest_robot->orientation(),
+ TbotsProto::DribblerMode::RELEASE_BALL_SLOW,
+ TbotsProto::BallCollisionType::ALLOW, {AutoChipOrKickMode::OFF, 0},
+ TbotsProto::MaxAllowedSpeedMode::BALL_PLACEMENT_RETREAT,
+ TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE);
+
+ tactics_to_run[0].push_back(wait_tactic);
+ }
- // setup move tactics for robots away from ball placing robot
- setupMoveTactics(event);
- tactics_to_run[0].insert(tactics_to_run[0].end(), move_tactics.begin(),
- move_tactics.end());
+ setupMoveTactics(event, subSat(event.common.num_tactics,
+ static_cast(tactics_to_run[0].size())));
+ tactics_to_run[0].insert(tactics_to_run[0].end(), move_tactics.begin(),
+ move_tactics.end());
+ event.common.set_tactics(tactics_to_run);
+}
+
+void BallPlacementPlayFSM::retreat(const Update &event)
+{
+ PriorityTacticVector tactics_to_run = {{}};
+
+ WorldPtr world_ptr = event.common.world_ptr;
+ std::optional nearest_robot =
+ world_ptr->friendlyTeam().getNearestRobot(world_ptr->ball().position());
+
+ if (nearest_robot.has_value())
+ {
Point ball_pos = world_ptr->ball().position();
- // robot will try to retreat backwards from wherever it is currently facing
- Angle final_orientation = nearest_robot.value().orientation();
- Vector retreat_direction =
- (nearest_robot.value().position() - ball_pos).normalize();
- Point retreat_position =
- ball_pos +
- retreat_direction * (RETREAT_DISTANCE_METERS + 2 * ROBOT_MAX_RADIUS_METERS);
+ // Robot will try to retreat backwards from wherever it is currently facing
+ Angle final_orientation = nearest_robot->orientation();
+ Vector retreat_dir = nearest_robot->position() - ball_pos;
+ Point retreat_pos = ball_pos + retreat_dir.normalize(RETREAT_DISTANCE_M);
- // if the initial retreat position is out of the field boundary, have it retreat
+ // If the initial retreat position is out of the field boundary, have it retreat
// towards the closest goal
- if (!contains(world_ptr->field().fieldBoundary(), retreat_position))
+ if (!contains(world_ptr->field().fieldBoundary(), retreat_pos))
{
- bool in_friendly_half = contains(world_ptr->field().friendlyHalf(), ball_pos);
- Point closer_goal = world_ptr->field().friendlyGoalCenter();
- if (!in_friendly_half)
+ Point closer_goal;
+ if (contains(world_ptr->field().friendlyHalf(), ball_pos))
+ {
+ closer_goal = world_ptr->field().friendlyGoalCenter();
+ }
+ else
{
closer_goal = world_ptr->field().enemyGoalCenter();
}
- retreat_direction = (closer_goal - ball_pos).normalize();
- retreat_position = ball_pos + retreat_direction * (RETREAT_DISTANCE_METERS +
- ROBOT_MAX_RADIUS_METERS);
+
+ retreat_dir = closer_goal - ball_pos;
+ retreat_pos = ball_pos + retreat_dir.normalize(RETREAT_DISTANCE_M);
}
- // setup ball placement tactic for ball placing robot
retreat_tactic->updateControlParams(
- retreat_position, final_orientation, TbotsProto::DribblerMode::OFF,
+ retreat_pos, final_orientation, TbotsProto::DribblerMode::OFF,
TbotsProto::BallCollisionType::AVOID, {AutoChipOrKickMode::OFF, 0},
- TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT,
+ TbotsProto::MaxAllowedSpeedMode::BALL_PLACEMENT_RETREAT,
TbotsProto::ObstacleAvoidanceMode::SAFE);
- tactics_to_run[0].emplace_back(retreat_tactic);
- event.common.set_tactics(tactics_to_run);
+ tactics_to_run[0].push_back(retreat_tactic);
}
+
+ setupMoveTactics(event, subSat(event.common.num_tactics,
+ static_cast(tactics_to_run[0].size())));
+ tactics_to_run[0].insert(tactics_to_run[0].end(), move_tactics.begin(),
+ move_tactics.end());
+
+ event.common.set_tactics(tactics_to_run);
}
-bool BallPlacementPlayFSM::shouldKickOffWall(const Update &event)
+bool BallPlacementPlayFSM::shouldPickOffWall(const Update &event)
{
- // check if ball is too close to border
- Point ball_pos = event.common.world_ptr->ball().position();
- Rectangle field_lines = event.common.world_ptr->field().fieldLines();
- return !contains(field_lines, ball_pos);
+ const Point ball_pos = event.common.world_ptr->ball().position();
+ const Rectangle field_lines = event.common.world_ptr->field().fieldBoundary();
+ return std::abs(signedDistance(ball_pos, field_lines)) <=
+ MIN_DISTANCE_FROM_WALL_FOR_ALIGN_M;
}
bool BallPlacementPlayFSM::alignDone(const Update &event)
@@ -167,136 +257,161 @@ bool BallPlacementPlayFSM::alignDone(const Update &event)
std::optional nearest_robot =
event.common.world_ptr->friendlyTeam().getNearestRobot(
event.common.world_ptr->ball().position());
+
if (nearest_robot.has_value())
{
- return comparePoints(nearest_robot.value().position(), setup_point);
+ return comparePoints(nearest_robot->position(), setup_point);
}
- else
+
+ return false;
+}
+
+bool BallPlacementPlayFSM::wallAlignDone(const Update &event)
+{
+ std::optional nearest_robot =
+ event.common.world_ptr->friendlyTeam().getNearestRobot(
+ event.common.world_ptr->ball().position());
+
+ if (nearest_robot.has_value())
{
- return false;
+ return comparePoints(nearest_robot->position(), pickoff_point);
}
+
+ return false;
}
-bool BallPlacementPlayFSM::kickDone(const Update &event)
+bool BallPlacementPlayFSM::wallPickOffDone(const Update &event)
{
- const auto ball_velocity = event.common.world_ptr->ball().velocity().length();
- const auto ball_is_kicked_m_per_s_threshold =
- this->ai_config_ptr->ai_parameter_config().ball_is_kicked_m_per_s_threshold();
- return ball_velocity > ball_is_kicked_m_per_s_threshold;
+ return pickoff_wall_tactic->done();
}
bool BallPlacementPlayFSM::ballPlaced(const Update &event)
{
- Point ball_pos = event.common.world_ptr->ball().position();
+ const Point ball_pos = event.common.world_ptr->ball().position();
+ const double ball_speed = event.common.world_ptr->ball().velocity().length();
+
std::optional placement_point =
event.common.world_ptr->gameState().getBallPlacementPoint();
std::vector robots = event.common.world_ptr->friendlyTeam().getAllRobots();
- // see if the ball is at the placement destination
if (placement_point.has_value())
{
- return comparePoints(ball_pos, placement_point.value(),
- PLACEMENT_DIST_THRESHOLD_METERS) &&
- event.common.world_ptr->ball().velocity().length() <
- this->ai_config_ptr->ai_parameter_config()
- .ball_is_kicked_m_per_s_threshold();
- }
- else
- {
- return true;
+ const bool is_ball_at_placement_point =
+ comparePoints(ball_pos, placement_point.value(), PLACEMENT_DIST_THRESHOLD_M);
+
+ const bool is_ball_stationary =
+ ball_speed <
+ ai_config_ptr->ai_parameter_config().ball_is_kicked_m_per_s_threshold();
+
+ return is_ball_at_placement_point && is_ball_stationary;
}
+
+ return true;
}
bool BallPlacementPlayFSM::waitDone(const Update &event)
{
- Timestamp current_time = event.common.world_ptr->getMostRecentTimestamp();
+ const Timestamp current_time = event.common.world_ptr->getMostRecentTimestamp();
return (current_time - start_time) > Duration::fromSeconds(BALL_IS_PLACED_WAIT_S);
}
bool BallPlacementPlayFSM::retreatDone(const Update &event)
{
- Point ball_position = event.common.world_ptr->ball().position();
- return distance(ball_position, event.common.world_ptr->friendlyTeam()
- .getNearestRobot(ball_position)
- ->position()) > RETREAT_DISTANCE_METERS;
+ return retreat_tactic->done();
}
-Angle BallPlacementPlayFSM::calculateWallKickoffAngle(const Point &ball_pos,
- const Rectangle &field_lines)
+std::pair BallPlacementPlayFSM::calculateWallPickOffDest(
+ const Point &ball_pos, const Rectangle &field_boundary)
{
- Angle kick_angle;
- if (ball_pos.x() > field_lines.xMax())
+ Angle facing_angle;
+ Point backoff_point;
+
+ const bool near_positive_y_boundary =
+ (field_boundary.yMax() - ball_pos.y()) < MIN_DISTANCE_FROM_WALL_FOR_ALIGN_M;
+ const bool near_negative_y_boundary =
+ (ball_pos.y() - field_boundary.yMin()) < MIN_DISTANCE_FROM_WALL_FOR_ALIGN_M;
+ const bool near_positive_x_boundary =
+ (field_boundary.xMax() - ball_pos.x()) < MIN_DISTANCE_FROM_WALL_FOR_ALIGN_M;
+ const bool near_negative_x_boundary =
+ (ball_pos.x() - field_boundary.xMin()) < MIN_DISTANCE_FROM_WALL_FOR_ALIGN_M;
+
+ if (near_positive_y_boundary && near_positive_x_boundary)
{
- if (ball_pos.y() > 0)
- {
- kick_angle = Angle::fromDegrees(45);
- }
- else
- {
- kick_angle = Angle::fromDegrees(-45);
- }
+ facing_angle = Angle::fromDegrees(45);
+ backoff_point =
+ field_boundary.posXPosYCorner() -
+ Vector::createFromAngle(facing_angle).normalize(BACK_AWAY_FROM_CORNER_M);
}
- else if (ball_pos.x() < field_lines.xMin())
+ else if (near_positive_y_boundary && near_negative_x_boundary)
{
- if (ball_pos.y() > 0)
- {
- kick_angle = Angle::fromDegrees(135);
- }
- else
- {
- kick_angle = Angle::fromDegrees(-135);
- }
+ facing_angle = Angle::fromDegrees(135);
+ backoff_point =
+ field_boundary.negXPosYCorner() -
+ Vector::createFromAngle(facing_angle).normalize(BACK_AWAY_FROM_CORNER_M);
}
- else if (ball_pos.y() > field_lines.yMax())
+ else if (near_negative_y_boundary && near_positive_x_boundary)
{
- if (ball_pos.x() > 0)
- {
- kick_angle = Angle::fromDegrees(135);
- }
- else
- {
- kick_angle = Angle::fromDegrees(45);
- }
+ facing_angle = Angle::fromDegrees(-45);
+ backoff_point =
+ field_boundary.posXNegYCorner() -
+ Vector::createFromAngle(facing_angle).normalize(BACK_AWAY_FROM_CORNER_M);
}
- else if (ball_pos.y() < field_lines.yMin())
+ else if (near_negative_y_boundary && near_negative_x_boundary)
{
- if (ball_pos.x() > 0)
- {
- kick_angle = Angle::fromDegrees(-135);
- }
- else
- {
- kick_angle = Angle::fromDegrees(-45);
- }
+ facing_angle = Angle::fromDegrees(-135);
+ backoff_point =
+ field_boundary.negXNegYCorner() -
+ Vector::createFromAngle(facing_angle).normalize(BACK_AWAY_FROM_CORNER_M);
+ }
+ else if (near_positive_y_boundary)
+ {
+ facing_angle = Angle::fromDegrees(90);
+ backoff_point =
+ Point(ball_pos.x(), field_boundary.yMax()) -
+ Vector::createFromAngle(facing_angle).normalize(BACK_AWAY_FROM_WALL_M);
+ }
+ else if (near_positive_x_boundary)
+ {
+ facing_angle = Angle::fromDegrees(0);
+ backoff_point =
+ Point(field_boundary.xMax(), ball_pos.y()) -
+ Vector::createFromAngle(facing_angle).normalize(BACK_AWAY_FROM_WALL_M);
+ }
+ else if (near_negative_y_boundary)
+ {
+ facing_angle = Angle::fromDegrees(-90);
+ backoff_point =
+ Point(ball_pos.x(), field_boundary.yMin()) -
+ Vector::createFromAngle(facing_angle).normalize(BACK_AWAY_FROM_WALL_M);
}
- return kick_angle;
+ else if (near_negative_x_boundary)
+ {
+ facing_angle = Angle::fromDegrees(180);
+ backoff_point =
+ Point(field_boundary.xMin(), ball_pos.y()) -
+ Vector::createFromAngle(facing_angle).normalize(BACK_AWAY_FROM_WALL_M);
+ }
+
+ return {facing_angle, backoff_point};
}
-void BallPlacementPlayFSM::setupMoveTactics(const Update &event)
+void BallPlacementPlayFSM::setupMoveTactics(const Update &event, unsigned int num_tactics)
{
- // assign all but one of the robots to line up away from the ball placing robot
- int num_move_tactics = event.common.num_tactics - 1;
-
- if (num_move_tactics <= 0)
+ if (move_tactics.size() != num_tactics)
{
- return;
+ move_tactics = std::vector>(num_tactics);
+ std::generate(
+ move_tactics.begin(), move_tactics.end(),
+ [&] { return std::make_shared(ai_config_ptr); });
}
- move_tactics = std::vector>(num_move_tactics);
- std::generate(move_tactics.begin(), move_tactics.end(),
- [this]()
- { return std::make_shared(this->ai_config_ptr); });
-
- // non goalie and non ball placing robots line up along a line just outside the
- // friendly defense area to wait for ball placement to finish
Vector waiting_line_vector =
event.common.world_ptr->field().friendlyDefenseArea().posXPosYCorner() -
event.common.world_ptr->field().friendlyDefenseArea().posXNegYCorner();
+
Point waiting_line_start_point =
event.common.world_ptr->field().friendlyDefenseArea().posXNegYCorner() +
- Vector(ROBOT_MAX_RADIUS_METERS * 3,
- 0); // Path planner can slow down when pathing through objects - buffer
- // zone of radius x 3 should help
+ Vector(WAITING_LINE_OFFSET_M, 0);
for (unsigned int i = 0; i < move_tactics.size(); i++)
{
@@ -304,6 +419,7 @@ void BallPlacementPlayFSM::setupMoveTactics(const Update &event)
waiting_line_start_point +
waiting_line_vector.normalize(waiting_line_vector.length() * i /
static_cast(move_tactics.size()));
+
move_tactics.at(i)->updateControlParams(
waiting_destination, Angle::zero(),
TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT,
diff --git a/src/software/ai/hl/stp/play/ball_placement/ball_placement_play_fsm.h b/src/software/ai/hl/stp/play/ball_placement/ball_placement_play_fsm.h
index 1f065163b8..1fa834b8b1 100644
--- a/src/software/ai/hl/stp/play/ball_placement/ball_placement_play_fsm.h
+++ b/src/software/ai/hl/stp/play/ball_placement/ball_placement_play_fsm.h
@@ -5,10 +5,6 @@
#include "software/ai/hl/stp/play/play_fsm.hpp"
#include "software/ai/hl/stp/tactic/dribble/dribble_tactic.h"
#include "software/ai/hl/stp/tactic/move/move_tactic.h"
-#include "software/ai/hl/stp/tactic/pivot_kick/pivot_kick_tactic.h"
-#include "software/ai/passing/eighteen_zone_pitch_division.h"
-
-using Zones = std::unordered_set;
struct BallPlacementPlayFSM : public PlayFSM
{
@@ -20,15 +16,15 @@ struct BallPlacementPlayFSM : public PlayFSM
};
class StartState;
- class KickOffWallState;
+ class PickOffWallState;
+ class AlignWallState;
class AlignPlacementState;
class PlaceBallState;
- class WaitState;
+ class ReleaseBallState;
class RetreatState;
-
/**
- * Creates a ball placement play FSM
+ * Creates a BallPlacementPlayFSM
*
* @param ai_config_ptr shared pointer to ai_config
*/
@@ -36,12 +32,26 @@ struct BallPlacementPlayFSM : public PlayFSM
std::shared_ptr ai_config_ptr);
/**
- * Action that has the placing robot kick the ball off the wall to give more space to
- * dribble
+ * Action that has the robot align with the wall in order to pick the ball off of it
+ *
+ * @param event the BallPlacementPlayFSM Update event
+ */
+ void alignWall(const Update& event);
+
+ /**
+ * Sets the target wall pickoff destination.
+ * This action is called one time before transitioning into PickOffWallState.
+ *
+ * @param event the BallPlacementPlayFSM Update event
+ */
+ void setPickOffDest(const Update& event);
+
+ /**
+ * Action that has the robot slowly pick up the ball and dribble it away the wall
*
* @param event the BallPlacementPlayFSM Update event
*/
- void kickOffWall(const Update& event);
+ void pickOffWall(const Update& event);
/**
* Action that finds a point where the ball's current position aligns with the ball
@@ -61,12 +71,20 @@ struct BallPlacementPlayFSM : public PlayFSM
void placeBall(const Update& event);
/**
- * Action that waits stationary 5 seconds for the dribbler to stop spinning
+ * Action that records the time at which the ReleaseBallState was entered so that
+ * we can wait for an appropriate amount of time for the dribbler to stop spinning
*
* @param event the BallPlacementPlayFSM Update event
*/
void startWait(const Update& event);
+ /**
+ * Action that stops the robot and spins the dribbler out while waiting
+ *
+ * @param event the BallPlacementPlayFSM Update event
+ */
+ void releaseBall(const Update& event);
+
/**
* Action that has the placing robot retreat after placing the ball
*
@@ -75,31 +93,42 @@ struct BallPlacementPlayFSM : public PlayFSM
void retreat(const Update& event);
/**
- * Guard on whether the ball is in a "dead zone"
+ * Guard on whether the ball is close enough to the wall that the robot cannot safely
+ * fit behind it
*
* @param event the BallPlacementPlayFSM Update event
*
- * @return whether the ball is in a "dead zone"
+ * @return whether the ball is close to the wall
*/
- bool shouldKickOffWall(const Update& event);
+ bool shouldPickOffWall(const Update& event);
/**
* Guard on whether the robot is aligned with the ball and placement point
*
* @param event the BallPlacementPlayFSM Update event
+ *
* @return whether the robot is in position to begin placing the ball
*/
bool alignDone(const Update& event);
+ /**
+ * Guard on whether the robot is in position to pick the ball off the wall
+ *
+ * @param event the BallPlacementPlayFSM Update event
+ *
+ * @return whether the robot is in position to begin picking the ball off the wall
+ */
+ bool wallAlignDone(const Update& event);
+
/**
* Guard on whether the placing robot has finished moving the ball into a better
- * position with a kick
+ * position
*
* @param event the BallPlacementPlayFSM Update event
*
- * @return whether the kick has been performed
+ * @return whether the pick off has been performed
*/
- bool kickDone(const Update& event);
+ bool wallPickOffDone(const Update& event);
/**
* Guard on whether the placing robot has finished placing the ball into the desired
@@ -112,93 +141,139 @@ struct BallPlacementPlayFSM : public PlayFSM
bool ballPlaced(const Update& event);
/**
- * Guard on whether the robot has waited sufficiently
+ * Guard on whether the robot has waited sufficiently for the dribbler to stop
+ * spinning
+ *
* @param event the BallPlacementPlayFSM Update event
- * @return whether the robot has waited for 5 seconds
+ *
+ * @return whether the robot has waited for 3 seconds
*/
bool waitDone(const Update& event);
/**
* Guard on whether the robot has retreated outside of the required range
+ *
* @param event the BallPlacementPlayFSM Update event
+ *
* @return whether the robot has retreated outside of the required range
*/
bool retreatDone(const Update& event);
/**
- * Helper function for calculating the angle to kick the ball off of a wall
+ * Calculates the angle at which the robot must face towards and the location
+ * the robot should dribble the ball to in order to pick the ball off the wall
*
- * @param ball_pos the ball position to use when calculating the kick angle
+ * @param ball_pos the current ball position
* @param field_lines the field lines of the playing area
*
* @return the kick angle
*/
- Angle calculateWallKickoffAngle(const Point& ball_pos, const Rectangle& field_lines);
+ std::pair calculateWallPickOffDest(const Point& ball_pos,
+ const Rectangle& field_lines);
/**
- * Helper function that populates the move_tactics field with MoveTactics that
- * organize the robots away from the ball placing robot
+ * Populates the move_tactics field with MoveTactics that organize the robots away
+ * from the ball placing robot
+ *
+ * Non goalie and non ball placing robots line up along a line just outside the
+ * friendly defense area and wait for ball placement to finish
*
* @param event the BallPlacementPlayFSM Update event
+ * @param num_tactics the number of MoveTactics to populate move_tactics with
*/
- void setupMoveTactics(const Update& event);
+ void setupMoveTactics(const Update& event, unsigned int num_tactics);
auto operator()()
{
using namespace boost::sml;
DEFINE_SML_STATE(StartState)
- DEFINE_SML_STATE(KickOffWallState)
+ DEFINE_SML_STATE(AlignWallState)
+ DEFINE_SML_STATE(PickOffWallState)
DEFINE_SML_STATE(AlignPlacementState)
DEFINE_SML_STATE(PlaceBallState)
- DEFINE_SML_STATE(WaitState)
+ DEFINE_SML_STATE(ReleaseBallState)
DEFINE_SML_STATE(RetreatState)
DEFINE_SML_EVENT(Update)
DEFINE_SML_ACTION(alignPlacement)
DEFINE_SML_ACTION(placeBall)
- DEFINE_SML_ACTION(kickOffWall)
+ DEFINE_SML_ACTION(setPickOffDest)
+ DEFINE_SML_ACTION(alignWall)
+ DEFINE_SML_ACTION(pickOffWall)
DEFINE_SML_ACTION(startWait)
DEFINE_SML_ACTION(retreat)
+ DEFINE_SML_ACTION(releaseBall)
- DEFINE_SML_GUARD(shouldKickOffWall)
+ DEFINE_SML_GUARD(shouldPickOffWall)
DEFINE_SML_GUARD(alignDone)
- DEFINE_SML_GUARD(kickDone)
+ DEFINE_SML_GUARD(wallAlignDone)
+ DEFINE_SML_GUARD(wallPickOffDone)
DEFINE_SML_GUARD(ballPlaced)
DEFINE_SML_GUARD(waitDone)
DEFINE_SML_GUARD(retreatDone)
return make_transition_table(
// src_state + event [guard] / action = dest_state
- *StartState_S + Update_E[!shouldKickOffWall_G] / alignPlacement_A =
+ *StartState_S + Update_E[!shouldPickOffWall_G] / alignPlacement_A =
AlignPlacementState_S,
- StartState_S + Update_E[shouldKickOffWall_G] = KickOffWallState_S,
- KickOffWallState_S + Update_E[!kickDone_G && shouldKickOffWall_G] /
- kickOffWall_A = KickOffWallState_S,
- KickOffWallState_S + Update_E[kickDone_G] = KickOffWallState_S,
- KickOffWallState_S + Update_E[!kickDone_G] = AlignPlacementState_S,
- AlignPlacementState_S + Update_E[shouldKickOffWall_G] = KickOffWallState_S,
+ StartState_S + Update_E[shouldPickOffWall_G] / alignWall_A = AlignWallState_S,
+
+ AlignWallState_S +
+ Update_E[!wallAlignDone_G && shouldPickOffWall_G] / alignWall_A,
+ AlignWallState_S + Update_E[wallAlignDone_G] /
+ (setPickOffDest_A, pickOffWall_A) = PickOffWallState_S,
+ AlignWallState_S + Update_E[!shouldPickOffWall_G] / alignPlacement_A =
+ AlignPlacementState_S,
+
+ PickOffWallState_S + Update_E[!wallPickOffDone_G] / pickOffWall_A,
+ PickOffWallState_S + Update_E[wallPickOffDone_G] / startWait_A =
+ ReleaseBallState_S,
+
+ AlignPlacementState_S + Update_E[shouldPickOffWall_G] / alignWall_A =
+ AlignWallState_S,
AlignPlacementState_S + Update_E[!alignDone_G] / alignPlacement_A =
AlignPlacementState_S,
- AlignPlacementState_S + Update_E[alignDone_G] = PlaceBallState_S,
+ AlignPlacementState_S + Update_E[alignDone_G] / placeBall_A =
+ PlaceBallState_S,
+
PlaceBallState_S + Update_E[!ballPlaced_G] / placeBall_A = PlaceBallState_S,
- PlaceBallState_S + Update_E[ballPlaced_G] / startWait_A = WaitState_S,
- WaitState_S + Update_E[!waitDone_G] = WaitState_S,
- WaitState_S + Update_E[waitDone_G] = RetreatState_S,
+ PlaceBallState_S + Update_E[ballPlaced_G] / startWait_A = ReleaseBallState_S,
+
+ ReleaseBallState_S + Update_E[!waitDone_G && ballPlaced_G] / releaseBall_A =
+ ReleaseBallState_S,
+ ReleaseBallState_S + Update_E[!ballPlaced_G] = StartState_S,
+ ReleaseBallState_S + Update_E[waitDone_G] / retreat_A = RetreatState_S,
+
RetreatState_S + Update_E[retreatDone_G && ballPlaced_G] = X,
- RetreatState_S + Update_E[ballPlaced_G] / retreat_A = RetreatState_S);
+ RetreatState_S + Update_E[ballPlaced_G] / retreat_A = RetreatState_S,
+ RetreatState_S + Update_E[!ballPlaced_G] = StartState_S);
}
private:
- std::shared_ptr pivot_kick_tactic;
- std::shared_ptr place_ball_tactic;
- std::shared_ptr align_placement_tactic;
+ TbotsProto::AiConfig ai_config;
+
+ std::shared_ptr align_wall_tactic;
+ std::shared_ptr pickoff_wall_tactic;
+ std::shared_ptr place_ball_tactic;
+ std::shared_ptr align_placement_tactic;
std::shared_ptr retreat_tactic;
- std::vector> move_tactics;
+ std::shared_ptr wait_tactic;
+ std::vector> move_tactics;
+
Point setup_point;
+ Point pickoff_point;
+ Point pickoff_destination;
+ Angle pickoff_final_orientation;
Timestamp start_time;
- constexpr static double const WALL_KICKOFF_VELOCITY_M_PER_S = 3.0;
- constexpr static double const RETREAT_DISTANCE_METERS = 0.6;
- constexpr static double const PLACEMENT_DIST_THRESHOLD_METERS = 0.15;
- constexpr static double const BALL_IS_PLACED_WAIT_S = 3.0;
+
+ static constexpr double BACK_AWAY_FROM_CORNER_M = 0.9;
+ static constexpr double BACK_AWAY_FROM_WALL_M = 0.5;
+ static constexpr double MIN_DISTANCE_FROM_WALL_FOR_ALIGN_M = 0.35;
+ static constexpr double RETREAT_DISTANCE_M = 0.7;
+ static constexpr double PLACEMENT_DIST_THRESHOLD_M = 0.15;
+ static constexpr double APPROACHING_PLACEMENT_DIST_THRESHOLD_M = 0.3;
+ static constexpr double WAITING_LINE_OFFSET_M = ROBOT_MAX_RADIUS_METERS * 3;
+ static constexpr double BALL_IS_PLACED_WAIT_S = 2.0;
+ static constexpr double ALIGNMENT_VECTOR_LENGTH_M = ROBOT_MAX_RADIUS_METERS * 2.5;
};
diff --git a/src/software/ai/hl/stp/play/ball_placement/ball_placement_play_fsm_test.cpp b/src/software/ai/hl/stp/play/ball_placement/ball_placement_play_fsm_test.cpp
index 9e2bebf746..9d466fdba0 100644
--- a/src/software/ai/hl/stp/play/ball_placement/ball_placement_play_fsm_test.cpp
+++ b/src/software/ai/hl/stp/play/ball_placement/ball_placement_play_fsm_test.cpp
@@ -3,16 +3,27 @@
#include
#include "proto/parameters.pb.h"
-#include "software/test_util/equal_within_tolerance.h"
+#include "shared/constants.h"
#include "software/test_util/test_util.h"
-TEST(BallPlacementPlayFSMTest, test_transitions)
+static void update(FSM& fsm, std::shared_ptr world_ptr,
+ int num_tactics)
+{
+ fsm.process_event(BallPlacementPlayFSM::Update(
+ BallPlacementPlayFSM::ControlParams{},
+ PlayUpdate(
+ world_ptr, num_tactics, [](PriorityTacticVector new_tactics) {},
+ InterPlayCommunication{}, [](InterPlayCommunication comm) {})));
+}
+
+TEST(BallPlacementPlayFSMTest, test_align_placement)
{
int num_tactics = 5;
+ Point ball_point(2, 2);
std::shared_ptr world_ptr = ::TestUtil::createBlankTestingWorld();
// ball starts within the field lines
- world_ptr->updateBall(Ball(Point(2, 2), Vector(0, 0), Timestamp::fromSeconds(0)));
+ world_ptr->updateBall(Ball(ball_point, Vector(0, 0), Timestamp::fromSeconds(0)));
GameState game_state;
game_state.updateRefereeCommand(RefereeCommand::STOP);
@@ -26,24 +37,36 @@ TEST(BallPlacementPlayFSMTest, test_transitions)
EXPECT_TRUE(fsm.is(boost::sml::state));
- fsm.process_event(BallPlacementPlayFSM::Update(
- BallPlacementPlayFSM::ControlParams{},
- PlayUpdate(
- world_ptr, num_tactics, [](PriorityTacticVector new_tactics) {},
- InterPlayCommunication{}, [](InterPlayCommunication comm) {})));
+ update(fsm, world_ptr, num_tactics);
EXPECT_TRUE(fsm.is(boost::sml::state));
+
+ double ALIGNMENT_VECTOR_LENGTH_M = ROBOT_MAX_RADIUS_METERS * 2.5;
+ Vector alignment_vector =
+ (ball_placement_point - ball_point).normalize(ALIGNMENT_VECTOR_LENGTH_M);
+ Point expected_setup_point = ball_point - alignment_vector;
+
+ ::TestUtil::setFriendlyRobotPositions(world_ptr, {expected_setup_point},
+ Timestamp::fromSeconds(0));
+
+ update(fsm, world_ptr, num_tactics);
+
+ EXPECT_TRUE(fsm.is(boost::sml::state));
+
+ world_ptr->updateBall(
+ Ball(ball_placement_point, Vector(0, 0), Timestamp::fromSeconds(1)));
+
+ update(fsm, world_ptr, num_tactics);
+ EXPECT_TRUE(fsm.is(boost::sml::state));
}
-TEST(BallPlacementPlayFSMTest, test_kick_off_wall_transitions)
+TEST(BallPlacementPlayFSMTest, test_align_wall)
{
int num_tactics = 5;
- // default field type is DIV_B
std::shared_ptr world_ptr = ::TestUtil::createBlankTestingWorld();
- // ball starts outside the field lines, so FSM will enter KickOfWallState
- world_ptr->updateBall(
- Ball(Point(-2.0, 3.2), Vector(0, 0), Timestamp::fromSeconds(0)));
+ // ball starts outside the field lines
+ world_ptr->updateBall(Ball(Point(4.8, 3.3), Vector(0, 0), Timestamp::fromSeconds(0)));
GameState game_state;
game_state.updateRefereeCommand(RefereeCommand::STOP);
@@ -51,77 +74,97 @@ TEST(BallPlacementPlayFSMTest, test_kick_off_wall_transitions)
game_state.setBallPlacementPoint(ball_placement_point);
world_ptr->updateGameState(game_state);
-
FSM fsm(
BallPlacementPlayFSM{std::make_shared()});
EXPECT_TRUE(fsm.is(boost::sml::state));
- fsm.process_event(BallPlacementPlayFSM::Update(
- BallPlacementPlayFSM::ControlParams{},
- PlayUpdate(
- world_ptr, num_tactics, [](PriorityTacticVector new_tactics) {},
- InterPlayCommunication{}, [](InterPlayCommunication comm) {})));
+ update(fsm, world_ptr, num_tactics);
- EXPECT_TRUE(fsm.is(boost::sml::state));
+ // ball in +X +Y corner
+ Angle expected_angle = Angle::fromDegrees(45);
+ Vector approach_vector = Vector::createFromAngle(expected_angle);
+ Point expected_pickoff_point =
+ Point(4.8, 3.3) - approach_vector.normalize(ROBOT_MAX_RADIUS_METERS * 2.5);
- // After the ball is kicked off a wall, it ends up somewhere inside the field lines
- // (but still needs to be moved to the ball placement point)
- world_ptr->updateBall(Ball(Point(-1, 1), Vector(0, 0), Timestamp::fromSeconds(1)));
+ ::TestUtil::setFriendlyRobotPositions(world_ptr, {expected_pickoff_point},
+ Timestamp::fromSeconds(0));
- fsm.process_event(BallPlacementPlayFSM::Update(
- BallPlacementPlayFSM::ControlParams{},
- PlayUpdate(
- world_ptr, num_tactics, [](PriorityTacticVector new_tactics) {},
- InterPlayCommunication{}, [](InterPlayCommunication comm) {})));
+ EXPECT_TRUE(fsm.is(boost::sml::state));
+
+ update(fsm, world_ptr, num_tactics);
+
+ EXPECT_TRUE(fsm.is(boost::sml::state));
+}
+
+TEST(BallPlacementPlayFSMTest, test_back_and_forth)
+{
+ int num_tactics = 5;
+ std::shared_ptr world_ptr = ::TestUtil::createBlankTestingWorld();
+ // ball starts outside the field lines
+ world_ptr->updateBall(Ball(Point(4.8, 3.3), Vector(0, 0), Timestamp::fromSeconds(0)));
+
+ GameState game_state;
+ game_state.updateRefereeCommand(RefereeCommand::STOP);
+ Point ball_placement_point(0, 0);
+ game_state.setBallPlacementPoint(ball_placement_point);
+ world_ptr->updateGameState(game_state);
+
+ FSM fsm(
+ BallPlacementPlayFSM{std::make_shared()});
+
+ EXPECT_TRUE(fsm.is(boost::sml::state));
+
+ update(fsm, world_ptr, num_tactics);
+ EXPECT_TRUE(fsm.is(boost::sml::state));
+
+ // put ball back inside field lines
+ world_ptr->updateBall(Ball(Point(4.4, 2.9), Vector(0, 0), Timestamp::fromSeconds(0)));
+
+ update(fsm, world_ptr, num_tactics);
EXPECT_TRUE(fsm.is(boost::sml::state));
+
+ // put ball back outside field lines
+ world_ptr->updateBall(Ball(Point(4.8, 3.3), Vector(0, 0), Timestamp::fromSeconds(0)));
+ update(fsm, world_ptr, num_tactics);
+
+ EXPECT_TRUE(fsm.is(boost::sml::state));
}
-TEST(BallPlacementPlayFSMTest, test_kick_off_wall_angle)
+TEST(BallPlacementPlayFSMTest, test_reset_when_ball_dropped_during_release)
{
- BallPlacementPlayFSM fsm(std::make_shared());
-
- Field field = Field::createField(TbotsProto::FieldType::DIV_B);
- Rectangle field_lines = field.fieldLines();
-
- // friendly half, ball outside left sideline
- Point start_point(-2.0, 3.2);
- Angle kick_angle = fsm.calculateWallKickoffAngle(start_point, field_lines);
- EXPECT_TRUE(kick_angle.toDegrees() == 45);
-
- // friendly half, ball outside right sideline
- start_point = Point(-2.0, -3.2);
- kick_angle = fsm.calculateWallKickoffAngle(start_point, field_lines);
- EXPECT_TRUE(kick_angle.toDegrees() == -45);
-
- // enemy half, ball outside left sideline
- start_point = Point(2.0, 3.2);
- kick_angle = fsm.calculateWallKickoffAngle(start_point, field_lines);
- EXPECT_TRUE(kick_angle.toDegrees() == 135);
-
- // enemy half, ball outside right sideline
- start_point = Point(2.0, -3.2);
- kick_angle = fsm.calculateWallKickoffAngle(start_point, field_lines);
- EXPECT_TRUE(kick_angle.toDegrees() == -135);
-
- // friendly half, ball outside friendly goal line (left of goal)
- start_point = Point(-4.7, 1.6);
- kick_angle = fsm.calculateWallKickoffAngle(start_point, field_lines);
- EXPECT_TRUE(kick_angle.toDegrees() == 135);
-
- // friendly half, ball outside friendly goal line (right of goal)
- start_point = Point(-4.7, -1.6);
- kick_angle = fsm.calculateWallKickoffAngle(start_point, field_lines);
- EXPECT_TRUE(kick_angle.toDegrees() == -135);
-
- // enemy half, ball outside enemy goal line (left of goal)
- start_point = Point(4.7, 1.6);
- kick_angle = fsm.calculateWallKickoffAngle(start_point, field_lines);
- EXPECT_TRUE(kick_angle.toDegrees() == 45);
-
- // enemy half, ball outside enemy goal line (right of goal)
- start_point = Point(4.7, -1.6);
- kick_angle = fsm.calculateWallKickoffAngle(start_point, field_lines);
- EXPECT_TRUE(kick_angle.toDegrees() == -45);
+ int num_tactics = 5;
+ Point ball_placement_point(0, 0);
+
+ std::shared_ptr world_ptr = ::TestUtil::createBlankTestingWorld();
+ GameState game_state;
+ game_state.updateRefereeCommand(RefereeCommand::STOP);
+ game_state.setBallPlacementPoint(ball_placement_point);
+ world_ptr->updateGameState(game_state);
+
+ FSM fsm(
+ BallPlacementPlayFSM{std::make_shared()});
+
+ world_ptr->updateBall(Ball(Point(2, 2), Vector(0, 0), Timestamp::fromSeconds(0)));
+ update(fsm, world_ptr, num_tactics);
+
+ Vector alignment_vector =
+ (ball_placement_point - Point(2, 2)).normalize(ROBOT_MAX_RADIUS_METERS * 2.5);
+ ::TestUtil::setFriendlyRobotPositions(world_ptr, {Point(2, 2) - alignment_vector},
+ Timestamp::fromSeconds(0));
+ update(fsm, world_ptr, num_tactics);
+
+ world_ptr->updateBall(
+ Ball(ball_placement_point, Vector(0, 0), Timestamp::fromSeconds(1)));
+ update(fsm, world_ptr, num_tactics);
+
+ EXPECT_TRUE(fsm.is(boost::sml::state));
+
+ // ball bumped away during release
+ world_ptr->updateBall(
+ Ball(Point(1.0, 1.0), Vector(0.5, 0.5), Timestamp::fromSeconds(2)));
+ update(fsm, world_ptr, num_tactics);
+
+ EXPECT_TRUE(fsm.is(boost::sml::state));
}
diff --git a/src/software/ai/hl/stp/play/ball_placement/ball_placement_play_test.py b/src/software/ai/hl/stp/play/ball_placement/ball_placement_play_test.py
index eba1625288..fc7fcba123 100644
--- a/src/software/ai/hl/stp/play/ball_placement/ball_placement_play_test.py
+++ b/src/software/ai/hl/stp/play/ball_placement/ball_placement_play_test.py
@@ -117,23 +117,23 @@ def test_two_ai_ball_placement(
"ball_start_point, ball_placement_point",
[
# TODO: Remove these ball placement tests until #3561 is resolved
- # # 2023 RoboCup ball placement scenarios
- # # Scenario 1
- # (tbots_cpp.Point(-0.2, -2.8), tbots_cpp.Point(-0.2, 2.8)),
- # # Scenario 2
- # (tbots_cpp.Point(-3.5, -2.25), tbots_cpp.Point(0, 0)),
- # # Scenario 3
- # (tbots_cpp.Point(-1.5, -2.25), tbots_cpp.Point(-0.2, -2.8)),
- # # Scenario 4
- # (tbots_cpp.Point(-4.4, -2.9), tbots_cpp.Point(-0.2, 2.8)),
- # # Scenario 5
- # (tbots_cpp.Point(-0.5, -0), tbots_cpp.Point(-4.3, 2.8)),
- # # Scenario 6
- # (tbots_cpp.Point(-1, -3.15), tbots_cpp.Point(-3.5, -2.8)),
- # # Scenario 7
- # (tbots_cpp.Point(-1, 3.15), tbots_cpp.Point(-3.5, 2.8)),
- # # Scenario 8
- # (tbots_cpp.Point(-4.45, -0.1), tbots_cpp.Point(-0.5, 2.8)),
+ # 2023 RoboCup ball placement scenarios
+ # Scenario 1
+ (tbots_cpp.Point(-0.2, -2.8), tbots_cpp.Point(-0.2, 2.8)),
+ # Scenario 2
+ (tbots_cpp.Point(-3.5, -2.25), tbots_cpp.Point(0, 0)),
+ # Scenario 3
+ (tbots_cpp.Point(-1.5, -2.25), tbots_cpp.Point(-0.2, -2.8)),
+ # Scenario 4
+ (tbots_cpp.Point(-4.4, -2.9), tbots_cpp.Point(-0.2, 2.8)),
+ # Scenario 5
+ (tbots_cpp.Point(-0.5, -0), tbots_cpp.Point(-4.3, 2.8)),
+ # Scenario 6
+ (tbots_cpp.Point(-1, -3.15), tbots_cpp.Point(-3.5, -2.8)),
+ # Scenario 7
+ (tbots_cpp.Point(-1, 3.15), tbots_cpp.Point(-3.5, 2.8)),
+ # Scenario 8
+ (tbots_cpp.Point(-4.45, -0.1), tbots_cpp.Point(-0.5, 2.8)),
],
)
def test_robocup_technical_challenge_placement(
diff --git a/src/software/ai/hl/stp/tactic/dribble/dribble_fsm.cpp b/src/software/ai/hl/stp/tactic/dribble/dribble_fsm.cpp
index 3c08e72145..bdd7c2cc41 100644
--- a/src/software/ai/hl/stp/tactic/dribble/dribble_fsm.cpp
+++ b/src/software/ai/hl/stp/tactic/dribble/dribble_fsm.cpp
@@ -135,7 +135,7 @@ void DribbleFSM::getPossession(const Update &event)
event.common.set_primitive(std::make_unique(
event.common.robot, intercept_position, face_ball_orientation,
- TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT,
+ event.control_params.max_speed_get_possession,
TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE,
TbotsProto::DribblerMode::MAX_FORCE, TbotsProto::BallCollisionType::ALLOW,
AutoChipOrKick{AutoChipOrKickMode::OFF, 0}));
@@ -151,7 +151,7 @@ void DribbleFSM::dribble(const Update &event)
event.common.set_primitive(std::make_unique(
event.common.robot, target_destination, target_orientation,
- TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT,
+ event.control_params.max_speed_dribble,
TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE,
TbotsProto::DribblerMode::MAX_FORCE, TbotsProto::BallCollisionType::ALLOW,
AutoChipOrKick{AutoChipOrKickMode::OFF, 0}));
@@ -170,7 +170,7 @@ void DribbleFSM::loseBall(const Update &event)
event.common.set_primitive(std::make_unique(
event.common.robot, away_from_ball_position, face_ball_orientation,
- TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT,
+ event.control_params.max_speed_get_possession,
TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE, TbotsProto::DribblerMode::OFF,
TbotsProto::BallCollisionType::AVOID,
AutoChipOrKick{AutoChipOrKickMode::OFF, 0}));
diff --git a/src/software/ai/hl/stp/tactic/dribble/dribble_fsm.h b/src/software/ai/hl/stp/tactic/dribble/dribble_fsm.h
index 54586c305a..9dd691a209 100644
--- a/src/software/ai/hl/stp/tactic/dribble/dribble_fsm.h
+++ b/src/software/ai/hl/stp/tactic/dribble/dribble_fsm.h
@@ -24,6 +24,11 @@ struct DribbleFSM : TacticFSM
std::optional final_dribble_orientation;
// whether to allow excessive dribbling, i.e. more than 1 metre at a time
bool allow_excessive_dribbling;
+ // Max allowed speed mode
+ TbotsProto::MaxAllowedSpeedMode max_speed_dribble =
+ TbotsProto::MaxAllowedSpeedMode::DRIBBLE;
+ TbotsProto::MaxAllowedSpeedMode max_speed_get_possession =
+ TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT;
};
using Update = TacticFSM::Update;
diff --git a/src/software/ai/hl/stp/tactic/dribble/dribble_tactic.cpp b/src/software/ai/hl/stp/tactic/dribble/dribble_tactic.cpp
index d9031af4cf..57b6ad0681 100644
--- a/src/software/ai/hl/stp/tactic/dribble/dribble_tactic.cpp
+++ b/src/software/ai/hl/stp/tactic/dribble/dribble_tactic.cpp
@@ -9,13 +9,17 @@ DribbleTactic::DribbleTactic(std::shared_ptr ai_conf
{
}
-void DribbleTactic::updateControlParams(std::optional dribble_destination,
- std::optional final_dribble_orientation,
- bool allow_excessive_dribbling)
+void DribbleTactic::updateControlParams(
+ std::optional dribble_destination,
+ std::optional final_dribble_orientation, bool allow_excessive_dribbling,
+ TbotsProto::MaxAllowedSpeedMode max_speed_dribble,
+ TbotsProto::MaxAllowedSpeedMode max_speed_get_possession)
{
control_params.dribble_destination = dribble_destination;
control_params.final_dribble_orientation = final_dribble_orientation;
control_params.allow_excessive_dribbling = allow_excessive_dribbling;
+ control_params.max_speed_dribble = max_speed_dribble;
+ control_params.max_speed_get_possession = max_speed_get_possession;
}
void DribbleTactic::accept(TacticVisitor &visitor) const
diff --git a/src/software/ai/hl/stp/tactic/dribble/dribble_tactic.h b/src/software/ai/hl/stp/tactic/dribble/dribble_tactic.h
index 5e2f511f36..25be5140de 100644
--- a/src/software/ai/hl/stp/tactic/dribble/dribble_tactic.h
+++ b/src/software/ai/hl/stp/tactic/dribble/dribble_tactic.h
@@ -32,12 +32,19 @@ class DribbleTactic : public TacticBase
* finishing dribbling
* @param allow_excessive_dribbling Whether to allow excessive dribbling, i.e. more
* than 1 metre at a time
+ * @param max_speed_dribble The max speed allowed while we are dribbling the ball
+ * @param max_speed_get_possession The max speed allowed while we are moving towards
+ * the ball to get possession
*/
void updateControlParams(std::optional dribble_destination,
std::optional final_dribble_orientation,
- bool allow_excessive_dribbling = false);
+ bool allow_excessive_dribbling = false,
+ TbotsProto::MaxAllowedSpeedMode max_speed_dribble =
+ TbotsProto::MaxAllowedSpeedMode::DRIBBLE,
+ TbotsProto::MaxAllowedSpeedMode max_speed_get_possession =
+ TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT);
void accept(TacticVisitor& visitor) const override;
};
-COPY_TACTIC(PlaceBallTactic, DribbleTactic)
+COPY_TACTIC(BallPlacementDribbleTactic, DribbleTactic)
diff --git a/src/software/ai/hl/stp/tactic/move/move_fsm.cpp b/src/software/ai/hl/stp/tactic/move/move_fsm.cpp
index 080a5171f8..cb3e1a7b2e 100644
--- a/src/software/ai/hl/stp/tactic/move/move_fsm.cpp
+++ b/src/software/ai/hl/stp/tactic/move/move_fsm.cpp
@@ -20,6 +20,11 @@ void MoveFSM::updateMove(const Update &event)
bool MoveFSM::moveDone(const Update &event)
{
- return robotReachedDestination(event.common.robot, event.control_params.destination,
+ // Only finish moving if not dribbling/kicking/chipping.
+ // Sometimes we just want to hold the ball somewhere when dribbling/kicking/chipping.
+ return event.control_params.dribbler_mode == TbotsProto::DribblerMode::OFF &&
+ event.control_params.auto_chip_or_kick.auto_chip_kick_mode ==
+ AutoChipOrKickMode::OFF &&
+ robotReachedDestination(event.common.robot, event.control_params.destination,
event.control_params.final_orientation);
}
diff --git a/src/software/ai/hl/stp/tactic/move/move_fsm.h b/src/software/ai/hl/stp/tactic/move/move_fsm.h
index ad517dd4d6..f474a3a9b2 100644
--- a/src/software/ai/hl/stp/tactic/move/move_fsm.h
+++ b/src/software/ai/hl/stp/tactic/move/move_fsm.h
@@ -75,6 +75,6 @@ struct MoveFSM : TacticFSM
*MoveState_S + Update_E[!moveDone_G] / updateMove_A = MoveState_S,
MoveState_S + Update_E[moveDone_G] / updateMove_A = X,
X + Update_E[!moveDone_G] / updateMove_A = MoveState_S,
- X + Update_E / updateMove_A = X);
+ X + Update_E / SET_STOP_PRIMITIVE_ACTION = X);
}
};
diff --git a/src/software/ai/hl/stp/tactic/move/move_tactic.h b/src/software/ai/hl/stp/tactic/move/move_tactic.h
index 2c6bfaa2ca..ecf4b103b2 100644
--- a/src/software/ai/hl/stp/tactic/move/move_tactic.h
+++ b/src/software/ai/hl/stp/tactic/move/move_tactic.h
@@ -61,5 +61,5 @@ class MoveTactic : public TacticBase
COPY_TACTIC(PenaltySetupTactic, MoveTactic)
COPY_TACTIC(MoveGoalieToGoalLineTactic, MoveTactic)
COPY_TACTIC(PrepareKickoffMoveTactic, MoveTactic)
-COPY_TACTIC(PlaceBallMoveTactic, MoveTactic)
+COPY_TACTIC(BallPlacementMoveTactic, MoveTactic)
COPY_TACTIC(AvoidInterferenceTactic, MoveTactic)
diff --git a/src/software/ai/hl/stp/tactic/pivot_kick/pivot_kick_tactic.h b/src/software/ai/hl/stp/tactic/pivot_kick/pivot_kick_tactic.h
index 1c1285d50c..07b1d4dd54 100644
--- a/src/software/ai/hl/stp/tactic/pivot_kick/pivot_kick_tactic.h
+++ b/src/software/ai/hl/stp/tactic/pivot_kick/pivot_kick_tactic.h
@@ -33,5 +33,3 @@ class PivotKickTactic : public TacticBase
void accept(TacticVisitor& visitor) const override;
};
-
-COPY_TACTIC(WallKickoffTactic, PivotKickTactic)
diff --git a/src/software/ai/hl/stp/tactic/tactic_visitor.h b/src/software/ai/hl/stp/tactic/tactic_visitor.h
index ea5c6e5ab0..dbc7e7d770 100644
--- a/src/software/ai/hl/stp/tactic/tactic_visitor.h
+++ b/src/software/ai/hl/stp/tactic/tactic_visitor.h
@@ -27,9 +27,8 @@ class HaltTactic;
class StopTestTactic;
class MoveGoalieToGoalLineTactic;
class PrepareKickoffMoveTactic;
-class PlaceBallTactic;
-class PlaceBallMoveTactic;
-class WallKickoffTactic;
+class BallPlacementDribbleTactic;
+class BallPlacementMoveTactic;
class AvoidInterferenceTactic;
/**
@@ -69,8 +68,7 @@ class TacticVisitor
virtual void visit(const StopTestTactic &tactic) = 0;
virtual void visit(const MoveGoalieToGoalLineTactic &tactic) = 0;
virtual void visit(const PrepareKickoffMoveTactic &tactic) = 0;
- virtual void visit(const PlaceBallTactic &tactic) = 0;
- virtual void visit(const PlaceBallMoveTactic &tactic) = 0;
- virtual void visit(const WallKickoffTactic &tactic) = 0;
+ virtual void visit(const BallPlacementDribbleTactic &tactic) = 0;
+ virtual void visit(const BallPlacementMoveTactic &tactic) = 0;
virtual void visit(const AvoidInterferenceTactic &tactic) = 0;
};
diff --git a/src/software/ai/hl/stp/tactic/transition_conditions.h b/src/software/ai/hl/stp/tactic/transition_conditions.h
index 6e2545d984..d2f47e878f 100644
--- a/src/software/ai/hl/stp/tactic/transition_conditions.h
+++ b/src/software/ai/hl/stp/tactic/transition_conditions.h
@@ -38,7 +38,7 @@ bool robotStopped(const Robot& robot, double SPEED_THRESHOLD = 0.05);
* @param pt2 The second point
* @param DISTANCE_THRESHOLD The threshold for how close the two points are
*/
-bool comparePoints(const Point& pt1, const Point& pt2, double DISTANCE_THRESHOLD = 0.02);
+bool comparePoints(const Point& pt1, const Point& pt2, double DISTANCE_THRESHOLD = 0.05);
/**
* Compares two angles and returns true if within ANGLE_THRESHOLD
diff --git a/src/software/ai/motion_constraint/motion_constraint_visitor.cpp b/src/software/ai/motion_constraint/motion_constraint_visitor.cpp
index cbedffc25c..ecb765b0bc 100644
--- a/src/software/ai/motion_constraint/motion_constraint_visitor.cpp
+++ b/src/software/ai/motion_constraint/motion_constraint_visitor.cpp
@@ -87,17 +87,12 @@ void MotionConstraintVisitor::visit(const MoveGoalieToGoalLineTactic &tactic)
current_motion_constraints.erase(TbotsProto::MotionConstraint::FRIENDLY_DEFENSE_AREA);
}
-void MotionConstraintVisitor::visit(const PlaceBallTactic &tactic)
+void MotionConstraintVisitor::visit(const BallPlacementDribbleTactic &tactic)
{
current_motion_constraints.clear();
}
-void MotionConstraintVisitor::visit(const PlaceBallMoveTactic &tactic)
-{
- current_motion_constraints.clear();
-}
-
-void MotionConstraintVisitor::visit(const WallKickoffTactic &tactic)
+void MotionConstraintVisitor::visit(const BallPlacementMoveTactic &tactic)
{
current_motion_constraints.clear();
}
diff --git a/src/software/ai/motion_constraint/motion_constraint_visitor.h b/src/software/ai/motion_constraint/motion_constraint_visitor.h
index af6053cf40..b73fa7d428 100644
--- a/src/software/ai/motion_constraint/motion_constraint_visitor.h
+++ b/src/software/ai/motion_constraint/motion_constraint_visitor.h
@@ -40,9 +40,8 @@ class MotionConstraintVisitor : public TacticVisitor
void visit(const PivotKickTactic &tactic) override;
void visit(const MoveGoalieToGoalLineTactic &tactic) override;
void visit(const PrepareKickoffMoveTactic &tactic) override;
- void visit(const PlaceBallTactic &tactic) override;
- void visit(const PlaceBallMoveTactic &tactic) override;
- void visit(const WallKickoffTactic &tactic) override;
+ void visit(const BallPlacementDribbleTactic &tactic) override;
+ void visit(const BallPlacementMoveTactic &tactic) override;
void visit(const AvoidInterferenceTactic &tactic) override;
void visit(const PassDefenderTactic &tactic) override;
diff --git a/src/software/ai/navigator/obstacle/robot_navigation_obstacle_factory.cpp b/src/software/ai/navigator/obstacle/robot_navigation_obstacle_factory.cpp
index 4cc336db2d..dae95df9fe 100644
--- a/src/software/ai/navigator/obstacle/robot_navigation_obstacle_factory.cpp
+++ b/src/software/ai/navigator/obstacle/robot_navigation_obstacle_factory.cpp
@@ -188,7 +188,9 @@ RobotNavigationObstacleFactory::createObstaclesFromMotionConstraints(
ObstaclePtr RobotNavigationObstacleFactory::createFromBallPosition(
const Point &ball_position) const
{
- return createFromShape(Circle(ball_position, BALL_MAX_RADIUS_METERS));
+ return createFromShape(Circle(
+ ball_position,
+ BALL_MAX_RADIUS_METERS + config.additional_ball_obstacle_inflation_meters()));
}
ObstaclePtr RobotNavigationObstacleFactory::createStadiumEnemyRobotObstacle(
diff --git a/src/software/ai/navigator/obstacle/robot_navigation_obstacle_factory_test.cpp b/src/software/ai/navigator/obstacle/robot_navigation_obstacle_factory_test.cpp
index 9fa925862b..87a7497fdf 100644
--- a/src/software/ai/navigator/obstacle/robot_navigation_obstacle_factory_test.cpp
+++ b/src/software/ai/navigator/obstacle/robot_navigation_obstacle_factory_test.cpp
@@ -104,7 +104,7 @@ TEST_F(RobotNavigationObstacleFactoryTest, create_rectangle_obstacle)
TEST_F(RobotNavigationObstacleFactoryTest, create_ball_obstacle)
{
Point origin(2.5, 4);
- Circle expected(origin, 0.1385);
+ Circle expected(origin, 0.1885);
ObstaclePtr obstacle =
robot_navigation_obstacle_factory.createFromBallPosition(origin);
diff --git a/src/software/field_tests/BUILD b/src/software/field_tests/BUILD
index ad76156e99..5659b748af 100644
--- a/src/software/field_tests/BUILD
+++ b/src/software/field_tests/BUILD
@@ -32,10 +32,6 @@ py_test(
srcs = [
"movement_robot_field_test.py",
],
- # TODO (#2619) Remove tag to run in parallel
- tags = [
- "exclusive",
- ],
deps = [
"//software:conftest",
"//software/simulated_tests:tbots_test_runner",
@@ -49,9 +45,19 @@ py_test(
srcs = [
"pivot_kick_field_test.py",
],
- # TODO (#2619) Remove tag to run in parallel
- tags = [
- "exclusive",
+ deps = [
+ "//software/field_tests:field_test_fixture",
+ "//software/simulated_tests:simulated_test_fixture",
+ "//software/simulated_tests:tbots_test_runner",
+ "//software/simulated_tests:validation",
+ requirement("pytest"),
+ ],
+)
+
+py_test(
+ name = "ball_placement_field_test",
+ srcs = [
+ "ball_placement_field_test.py",
],
deps = [
"//software:conftest",
diff --git a/src/software/field_tests/ball_placement_field_test.py b/src/software/field_tests/ball_placement_field_test.py
new file mode 100644
index 0000000000..4b5eff252d
--- /dev/null
+++ b/src/software/field_tests/ball_placement_field_test.py
@@ -0,0 +1,43 @@
+import software.python_bindings as tbots_cpp
+import sys
+from proto.ssl_gc_state_pb2 import Command
+from proto.import_all_protos import *
+from software.field_tests.field_test_fixture import *
+
+from software.simulated_tests.simulated_test_fixture import *
+from software.logger.logger import createLogger
+
+logger = createLogger(__name__)
+
+
+def test_ball_placement(field_test_runner):
+ placement_point = tbots_cpp.Point(0, 0)
+
+ world = field_test_runner.world_buffer.get(block=True, timeout=WORLD_BUFFER_TIMEOUT)
+ print("Here are the robots:")
+ print(
+ [
+ robot.current_state.global_position
+ for robot in world.friendly_team.team_robots
+ ]
+ )
+
+ field_test_runner.send_gamecontroller_command(
+ Command.Type.BALL_PLACEMENT, proto.ssl_gc_common_pb2.Team.BLUE, placement_point
+ )
+
+ field_test_runner.run_test(
+ always_validation_sequence_set=[[]],
+ eventually_validation_sequence_set=[[]],
+ test_timeout_s=10,
+ )
+
+ # Send a stop tactic after the test finishes
+ stop_tactic = StopTactic()
+ params = AssignedTacticPlayControlParams()
+ params.assigned_tactics[id].stop.CopyFrom(stop_tactic)
+
+
+if __name__ == "__main__":
+ # Run the test, -s disables all capturing and -vv increases verbosity
+ sys.exit(pytest_main(__file__))
diff --git a/src/software/field_tests/field_test_fixture.py b/src/software/field_tests/field_test_fixture.py
index 42c0c9f6ee..30b19b5abe 100644
--- a/src/software/field_tests/field_test_fixture.py
+++ b/src/software/field_tests/field_test_fixture.py
@@ -89,7 +89,7 @@ def __init__(
@override
def send_gamecontroller_command(
self,
- gc_command: proto.ssl_gc_state_pb2.Command,
+ gc_command: proto.ssl_gc_state_pb2.Command.Type,
team: proto.ssl_gc_common_pb2.Team,
final_ball_placement_point=None,
):
@@ -99,7 +99,7 @@ def send_gamecontroller_command(
:param team: The team which the command as attributed to
:param final_ball_placement_point: The ball placement point
"""
- self.gamecontroller.send_ci_input(
+ self.gamecontroller.send_gc_command(
gc_command=gc_command,
team=team,
final_ball_placement_point=final_ball_placement_point,
diff --git a/src/software/field_tests/movement_robot_field_test.py b/src/software/field_tests/movement_robot_field_test.py
index bd436869bf..1933f15939 100644
--- a/src/software/field_tests/movement_robot_field_test.py
+++ b/src/software/field_tests/movement_robot_field_test.py
@@ -69,7 +69,7 @@
# this test can only be run on the field
def test_basic_rotation(field_test_runner):
- test_angles = [0, 45, 90, 180, 270, 0]
+ test_angles = [0, math.pi, 0, math.pi, 0, math.pi]
world = field_test_runner.world_buffer.get(block=True, timeout=WORLD_BUFFER_TIMEOUT)
if len(world.friendly_team.team_robots) == 0:
@@ -111,7 +111,7 @@ def test_basic_rotation(field_test_runner):
field_test_runner.run_test(
always_validation_sequence_set=[[]],
eventually_validation_sequence_set=[[]],
- test_timeout_s=5,
+ test_timeout_s=3,
)
# Send a halt tactic after the test finishes
halt_tactic = HaltTactic()
@@ -123,8 +123,6 @@ def test_basic_rotation(field_test_runner):
# validate by eye
logger.info(f"robot set to {angle} orientation")
- time.sleep(2)
-
def test_one_robots_square(field_test_runner):
world = field_test_runner.world_buffer.get(block=True, timeout=WORLD_BUFFER_TIMEOUT)
diff --git a/src/software/thunderscope/binary_context_managers/game_controller.py b/src/software/thunderscope/binary_context_managers/game_controller.py
index 79cc8a0925..c12cbcbf2c 100644
--- a/src/software/thunderscope/binary_context_managers/game_controller.py
+++ b/src/software/thunderscope/binary_context_managers/game_controller.py
@@ -314,7 +314,7 @@ def __send_referee_command(data: Referee) -> None:
def send_gc_command(
self,
- gc_command: proto.ssl_gc_state_pb2.Command,
+ gc_command: proto.ssl_gc_state_pb2.Command.Type,
team: proto.ssl_gc_common_pb2.Team,
final_ball_placement_point: tbots_cpp.Point = None,
) -> Any: