From ce89e2cc54f1100678ee487e6edfe407bb62d54c Mon Sep 17 00:00:00 2001 From: Mikhael Date: Tue, 4 Jun 2024 18:41:45 -0700 Subject: [PATCH 01/29] wip ball placement improvements --- .../message_translation/tbots_protobuf.cpp | 2 + src/proto/primitive.proto | 1 + .../ball_placement_play_fsm.cpp | 30 +++++++++++- .../ball_placement/ball_placement_play_fsm.h | 14 +++++- .../ai/hl/stp/tactic/move/move_fsm.cpp | 2 +- src/software/ai/hl/stp/tactic/move/move_fsm.h | 2 + .../ai/hl/stp/tactic/move/move_tactic.cpp | 5 +- .../ai/hl/stp/tactic/move/move_tactic.h | 3 +- src/software/field_tests/BUILD | 18 +++++++ .../field_tests/ball_placement_field_test.py | 49 +++++++++++++++++++ .../field_tests/field_test_fixture.py | 13 ++--- src/software/logger/logger.h | 12 ++--- .../game_controller.py | 8 +-- 13 files changed, 134 insertions(+), 25 deletions(-) create mode 100644 src/software/field_tests/ball_placement_field_test.py diff --git a/src/proto/message_translation/tbots_protobuf.cpp b/src/proto/message_translation/tbots_protobuf.cpp index 25595de3db..b3a331fa88 100644 --- a/src/proto/message_translation/tbots_protobuf.cpp +++ b/src/proto/message_translation/tbots_protobuf.cpp @@ -492,6 +492,8 @@ double convertDribblerModeToDribblerSpeed(TbotsProto::DribblerMode dribbler_mode return robot_constants.max_force_dribbler_speed_rpm; case TbotsProto::DribblerMode::OFF: return 0.0; + case TbotsProto::DribblerMode::RELEASE_BALL_SLOW: + return 2000.0; default: LOG(WARNING) << "DribblerMode is invalid" << std::endl; return 0.0; diff --git a/src/proto/primitive.proto b/src/proto/primitive.proto index b679a65127..2d670e33ba 100644 --- a/src/proto/primitive.proto +++ b/src/proto/primitive.proto @@ -57,6 +57,7 @@ enum DribblerMode OFF = 0; INDEFINITE = 1; MAX_FORCE = 2; + RELEASE_BALL_SLOW = 3; } enum MaxAllowedSpeedMode 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 540862033e..741df2af30 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 @@ -6,6 +6,7 @@ BallPlacementPlayFSM::BallPlacementPlayFSM(TbotsProto::AiConfig ai_config) place_ball_tactic(std::make_shared(ai_config)), align_placement_tactic(std::make_shared()), retreat_tactic(std::make_shared()), + wait_tactic(std::make_shared()), move_tactics(std::vector>()) { } @@ -101,6 +102,33 @@ void BallPlacementPlayFSM::startWait(const Update &event) start_time = event.common.world_ptr->getMostRecentTimestamp(); } +void BallPlacementPlayFSM::wait(const Update &event) +{ + 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 = {{}}; + + // 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()); + + + wait_tactic->updateControlParams( + nearest_robot->position(), nearest_robot->orientation(), 0.0, TbotsProto::DribblerMode::RELEASE_BALL_SLOW, + TbotsProto::BallCollisionType::ALLOW, {AutoChipOrKickMode::OFF, 0}, + TbotsProto::MaxAllowedSpeedMode::STOP_COMMAND, // TODO: Instead of stop use a new speed mode for retreating in ball placement + TbotsProto::ObstacleAvoidanceMode::SAFE, 0.0, true); + tactics_to_run[0].emplace_back(wait_tactic); + + event.common.set_tactics(tactics_to_run); + } +} + void BallPlacementPlayFSM::retreat(const Update &event) { WorldPtr world_ptr = event.common.world_ptr; @@ -145,7 +173,7 @@ void BallPlacementPlayFSM::retreat(const Update &event) retreat_tactic->updateControlParams( retreat_position, final_orientation, 0.0, TbotsProto::DribblerMode::OFF, TbotsProto::BallCollisionType::AVOID, {AutoChipOrKickMode::OFF, 0}, - TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, + TbotsProto::MaxAllowedSpeedMode::STOP_COMMAND, // TODO: Instead of stop use a new speed mode for retreating in ball placement TbotsProto::ObstacleAvoidanceMode::SAFE, 0.0); tactics_to_run[0].emplace_back(retreat_tactic); 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 4a489edd37..a1d9fbcf42 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 @@ -7,6 +7,7 @@ #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" +#include "software/ai/hl/stp/tactic/stop/stop_tactic.h" using Zones = std::unordered_set; @@ -65,6 +66,13 @@ struct BallPlacementPlayFSM */ void startWait(const Update& event); + /** + * Action that stops the robot while waiting + * + * @param event the BallPlacementPlayFSM Update event + */ + void wait(const Update& event); + /** * Action that has the placing robot retreat after placing the ball * @@ -123,6 +131,8 @@ struct BallPlacementPlayFSM */ bool retreatDone(const Update& event); + + /** * Helper function for calculating the angle to kick the ball off of a wall * @@ -158,6 +168,7 @@ struct BallPlacementPlayFSM DEFINE_SML_ACTION(kickOffWall) DEFINE_SML_ACTION(startWait) DEFINE_SML_ACTION(retreat) + DEFINE_SML_ACTION(wait) DEFINE_SML_GUARD(shouldKickOffWall) DEFINE_SML_GUARD(alignDone) @@ -181,7 +192,7 @@ struct BallPlacementPlayFSM AlignPlacementState_S + Update_E[alignDone_G] = 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] / wait_A = WaitState_S, WaitState_S + Update_E[waitDone_G] = RetreatState_S, RetreatState_S + Update_E[retreatDone_G && ballPlaced_G] = X, RetreatState_S + Update_E[ballPlaced_G] / retreat_A = RetreatState_S); @@ -193,6 +204,7 @@ struct BallPlacementPlayFSM std::shared_ptr place_ball_tactic; std::shared_ptr align_placement_tactic; std::shared_ptr retreat_tactic; + std::shared_ptr wait_tactic; std::vector> move_tactics; Point setup_point; Timestamp start_time; 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 cc6363e083..ddf0d1e7cd 100644 --- a/src/software/ai/hl/stp/tactic/move/move_fsm.cpp +++ b/src/software/ai/hl/stp/tactic/move/move_fsm.cpp @@ -15,6 +15,6 @@ void MoveFSM::updateMove(const Update &event) bool MoveFSM::moveDone(const Update &event) { - return robotReachedDestination(event.common.robot, event.control_params.destination, + return !event.control_params.never_terminate && 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 936d7d8958..e7b0ded563 100644 --- a/src/software/ai/hl/stp/tactic/move/move_fsm.h +++ b/src/software/ai/hl/stp/tactic/move/move_fsm.h @@ -31,6 +31,8 @@ struct MoveFSM TbotsProto::ObstacleAvoidanceMode obstacle_avoidance_mode; // The target spin while moving in revolutions per second double target_spin_rev_per_s; + // If true, FSM will never terminate and simply hold at the destination + bool never_terminate; }; // this struct defines the only event that the MoveFSM responds to diff --git a/src/software/ai/hl/stp/tactic/move/move_tactic.cpp b/src/software/ai/hl/stp/tactic/move/move_tactic.cpp index b7ae2624e2..969bb096a6 100644 --- a/src/software/ai/hl/stp/tactic/move/move_tactic.cpp +++ b/src/software/ai/hl/stp/tactic/move/move_tactic.cpp @@ -28,7 +28,8 @@ void MoveTactic::updateControlParams( TbotsProto::BallCollisionType ball_collision_type, AutoChipOrKick auto_chip_or_kick, TbotsProto::MaxAllowedSpeedMode max_allowed_speed_mode, TbotsProto::ObstacleAvoidanceMode obstacle_avoidance_mode, - double target_spin_rev_per_s) + double target_spin_rev_per_s, + bool never_terminate) { // Update the control parameters stored by this Tactic control_params.destination = destination; @@ -40,6 +41,7 @@ void MoveTactic::updateControlParams( control_params.max_allowed_speed_mode = max_allowed_speed_mode; control_params.obstacle_avoidance_mode = obstacle_avoidance_mode; control_params.target_spin_rev_per_s = target_spin_rev_per_s; + control_params.never_terminate = never_terminate; } void MoveTactic::updateControlParams( @@ -57,6 +59,7 @@ void MoveTactic::updateControlParams( control_params.max_allowed_speed_mode = max_allowed_speed_mode; control_params.obstacle_avoidance_mode = obstacle_avoidance_mode; control_params.target_spin_rev_per_s = 0.0; + control_params.never_terminate = false; } void MoveTactic::updatePrimitive(const TacticUpdate &tactic_update, bool reset_fsm) 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 7232304473..8cd2343739 100644 --- a/src/software/ai/hl/stp/tactic/move/move_tactic.h +++ b/src/software/ai/hl/stp/tactic/move/move_tactic.h @@ -40,7 +40,8 @@ class MoveTactic : public Tactic TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, TbotsProto::ObstacleAvoidanceMode obstacle_avoidance_mode = TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE, - double target_spin_rev_per_s = 0.0); + double target_spin_rev_per_s = 0.0, + bool never_terminate = false); /** * Updates the params assuming that the dribbler and chicker and while avoiding the diff --git a/src/software/field_tests/BUILD b/src/software/field_tests/BUILD index fb527f2f34..3d88f9e756 100644 --- a/src/software/field_tests/BUILD +++ b/src/software/field_tests/BUILD @@ -64,6 +64,24 @@ py_test( ], ) +py_test( + name = "ball_placement_field_test", + srcs = [ + "ball_placement_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 = "passing_field_test", srcs = [ 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..d7a8109503 --- /dev/null +++ b/src/software/field_tests/ball_placement_field_test.py @@ -0,0 +1,49 @@ +import pytest + +import software.python_bindings as tbots_cpp +import sys +import math +from proto.ssl_gc_common_pb2 import Team +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 +from software.simulated_tests.robot_enters_region import RobotEventuallyEntersRegion +from proto.message_translation.tbots_protobuf import create_world_state + +logger = createLogger(__name__) + + +def test_ball_placement(field_test_runner): + # robot ID + id = 4 + # point to place ball + placement_point = Point(x_meters=0, y_meters=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__, "-svv"])) \ No newline at end of file diff --git a/src/software/field_tests/field_test_fixture.py b/src/software/field_tests/field_test_fixture.py index 7ae36de467..0a9f08db03 100644 --- a/src/software/field_tests/field_test_fixture.py +++ b/src/software/field_tests/field_test_fixture.py @@ -87,12 +87,12 @@ def __init__( 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, ): - self.gamecontroller.send_ci_input( + self.gamecontroller.send_gc_command( gc_command=gc_command, team=team, final_ball_placement_point=final_ball_placement_point, @@ -100,9 +100,7 @@ def send_gamecontroller_command( def time_provider(self): """Provide the current time in seconds since the epoch""" - - with self.timestamp_mutex: - return self.timestamp + return time.time() def run_test( self, @@ -131,11 +129,6 @@ def __runner(): test_end_time = time.time() + test_timeout_s while time.time() < test_end_time: - # Update the timestamp logged by the ProtoLogger - with self.timestamp_mutex: - - ssl_wrapper = self.ssl_wrapper_buffer.get(block=False) - self.timestamp = ssl_wrapper.detection.t_capture while True: try: diff --git a/src/software/logger/logger.h b/src/software/logger/logger.h index 00693fd62d..b1b07eef97 100644 --- a/src/software/logger/logger.h +++ b/src/software/logger/logger.h @@ -3,7 +3,7 @@ #include #include -#include +//#include #include #include #include @@ -81,11 +81,11 @@ class LoggerSingleton // arg. Note: log locations are defaulted to the bazel-out folder due to Bazel's // hermetic build principles - // if log dir doesn't exist, create it - if (!std::experimental::filesystem::exists(runtime_dir)) - { - std::experimental::filesystem::create_directories(runtime_dir); - } +// // if log dir doesn't exist, create it +// if (!std::experimental::filesystem::exists(runtime_dir)) +// { +// std::experimental::filesystem::create_directories(runtime_dir); +// } auto csv_sink_handle = logWorker->addSink(std::make_unique(runtime_dir), &CSVSink::appendToFile); diff --git a/src/software/thunderscope/binary_context_managers/game_controller.py b/src/software/thunderscope/binary_context_managers/game_controller.py index a8c01f87cb..7b3ffecbf8 100644 --- a/src/software/thunderscope/binary_context_managers/game_controller.py +++ b/src/software/thunderscope/binary_context_managers/game_controller.py @@ -162,9 +162,9 @@ 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, + final_ball_placement_point: Point = None, ) -> Any: """Send a ci input to the gamecontroller. @@ -196,8 +196,8 @@ def send_gc_command( ball_placement_pos = SetBallPlacementPos() ball_placement_pos.pos.CopyFrom( Vector2( - x=float(final_ball_placement_point.x()), - y=float(final_ball_placement_point.y()), + x=float(final_ball_placement_point.x_meters), + y=float(final_ball_placement_point.y_meters), ) ) change = Change() From 1b5ab3c747f999f02a545cf7adc98b4b66f3443f Mon Sep 17 00:00:00 2001 From: Mikhael Date: Wed, 26 Jun 2024 14:01:16 -0700 Subject: [PATCH 02/29] Made move tactic only terminate when dribbler is not on release, made ball placement go back to aligning when ball is lost --- .../ball_placement/ball_placement_play_fsm.cpp | 2 +- .../ball_placement/ball_placement_play_fsm.h | 16 +++++++++------- src/software/ai/hl/stp/tactic/move/move_fsm.cpp | 2 +- src/software/ai/hl/stp/tactic/move/move_fsm.h | 2 -- .../ai/hl/stp/tactic/move/move_tactic.cpp | 5 +---- src/software/ai/hl/stp/tactic/move/move_tactic.h | 3 +-- .../binary_context_managers/game_controller.py | 4 ++-- 7 files changed, 15 insertions(+), 19 deletions(-) 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 741df2af30..6cbcb60e40 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 @@ -122,7 +122,7 @@ void BallPlacementPlayFSM::wait(const Update &event) nearest_robot->position(), nearest_robot->orientation(), 0.0, TbotsProto::DribblerMode::RELEASE_BALL_SLOW, TbotsProto::BallCollisionType::ALLOW, {AutoChipOrKickMode::OFF, 0}, TbotsProto::MaxAllowedSpeedMode::STOP_COMMAND, // TODO: Instead of stop use a new speed mode for retreating in ball placement - TbotsProto::ObstacleAvoidanceMode::SAFE, 0.0, true); + TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE, 0.0); tactics_to_run[0].emplace_back(wait_tactic); event.common.set_tactics(tactics_to_run); 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 a1d9fbcf42..14f056a470 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 @@ -189,13 +189,15 @@ struct BallPlacementPlayFSM AlignPlacementState_S + Update_E[shouldKickOffWall_G] = KickOffWallState_S, AlignPlacementState_S + Update_E[!alignDone_G] / alignPlacement_A = AlignPlacementState_S, - AlignPlacementState_S + Update_E[alignDone_G] = 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] / wait_A = WaitState_S, - WaitState_S + Update_E[waitDone_G] = RetreatState_S, - RetreatState_S + Update_E[retreatDone_G && ballPlaced_G] = X, - RetreatState_S + Update_E[ballPlaced_G] / retreat_A = RetreatState_S); + AlignPlacementState_S + Update_E[alignDone_G] = 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 && ballPlaced_G] / wait_A = WaitState_S, + WaitState_S + Update_E[!ballPlaced_G] = StartState_S, + WaitState_S + Update_E[waitDone_G] = 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] = StartState_S); } private: 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 ddf0d1e7cd..99982f593e 100644 --- a/src/software/ai/hl/stp/tactic/move/move_fsm.cpp +++ b/src/software/ai/hl/stp/tactic/move/move_fsm.cpp @@ -15,6 +15,6 @@ void MoveFSM::updateMove(const Update &event) bool MoveFSM::moveDone(const Update &event) { - return !event.control_params.never_terminate && robotReachedDestination(event.common.robot, event.control_params.destination, + return event.control_params.dribbler_mode != TbotsProto::DribblerMode::RELEASE_BALL_SLOW && 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 e7b0ded563..936d7d8958 100644 --- a/src/software/ai/hl/stp/tactic/move/move_fsm.h +++ b/src/software/ai/hl/stp/tactic/move/move_fsm.h @@ -31,8 +31,6 @@ struct MoveFSM TbotsProto::ObstacleAvoidanceMode obstacle_avoidance_mode; // The target spin while moving in revolutions per second double target_spin_rev_per_s; - // If true, FSM will never terminate and simply hold at the destination - bool never_terminate; }; // this struct defines the only event that the MoveFSM responds to diff --git a/src/software/ai/hl/stp/tactic/move/move_tactic.cpp b/src/software/ai/hl/stp/tactic/move/move_tactic.cpp index 969bb096a6..b7ae2624e2 100644 --- a/src/software/ai/hl/stp/tactic/move/move_tactic.cpp +++ b/src/software/ai/hl/stp/tactic/move/move_tactic.cpp @@ -28,8 +28,7 @@ void MoveTactic::updateControlParams( TbotsProto::BallCollisionType ball_collision_type, AutoChipOrKick auto_chip_or_kick, TbotsProto::MaxAllowedSpeedMode max_allowed_speed_mode, TbotsProto::ObstacleAvoidanceMode obstacle_avoidance_mode, - double target_spin_rev_per_s, - bool never_terminate) + double target_spin_rev_per_s) { // Update the control parameters stored by this Tactic control_params.destination = destination; @@ -41,7 +40,6 @@ void MoveTactic::updateControlParams( control_params.max_allowed_speed_mode = max_allowed_speed_mode; control_params.obstacle_avoidance_mode = obstacle_avoidance_mode; control_params.target_spin_rev_per_s = target_spin_rev_per_s; - control_params.never_terminate = never_terminate; } void MoveTactic::updateControlParams( @@ -59,7 +57,6 @@ void MoveTactic::updateControlParams( control_params.max_allowed_speed_mode = max_allowed_speed_mode; control_params.obstacle_avoidance_mode = obstacle_avoidance_mode; control_params.target_spin_rev_per_s = 0.0; - control_params.never_terminate = false; } void MoveTactic::updatePrimitive(const TacticUpdate &tactic_update, bool reset_fsm) 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 8cd2343739..7232304473 100644 --- a/src/software/ai/hl/stp/tactic/move/move_tactic.h +++ b/src/software/ai/hl/stp/tactic/move/move_tactic.h @@ -40,8 +40,7 @@ class MoveTactic : public Tactic TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, TbotsProto::ObstacleAvoidanceMode obstacle_avoidance_mode = TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE, - double target_spin_rev_per_s = 0.0, - bool never_terminate = false); + double target_spin_rev_per_s = 0.0); /** * Updates the params assuming that the dribbler and chicker and while avoiding the diff --git a/src/software/thunderscope/binary_context_managers/game_controller.py b/src/software/thunderscope/binary_context_managers/game_controller.py index 7b3ffecbf8..cadbde2d6f 100644 --- a/src/software/thunderscope/binary_context_managers/game_controller.py +++ b/src/software/thunderscope/binary_context_managers/game_controller.py @@ -196,8 +196,8 @@ def send_gc_command( ball_placement_pos = SetBallPlacementPos() ball_placement_pos.pos.CopyFrom( Vector2( - x=float(final_ball_placement_point.x_meters), - y=float(final_ball_placement_point.y_meters), + x=float(final_ball_placement_point.x()), + y=float(final_ball_placement_point.y()), ) ) change = Change() From d079f304dab3e5a40d352d334e9b4d2c8cef14d6 Mon Sep 17 00:00:00 2001 From: Mikhael Date: Wed, 26 Jun 2024 14:05:49 -0700 Subject: [PATCH 03/29] Added new speed mode for ball placement retreats --- src/proto/message_translation/tbots_protobuf.cpp | 2 ++ src/proto/primitive.proto | 2 ++ src/shared/constants.h | 2 ++ .../ai/hl/stp/play/ball_placement/ball_placement_play_fsm.cpp | 2 +- 4 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/proto/message_translation/tbots_protobuf.cpp b/src/proto/message_translation/tbots_protobuf.cpp index b3a331fa88..e48f43774f 100644 --- a/src/proto/message_translation/tbots_protobuf.cpp +++ b/src/proto/message_translation/tbots_protobuf.cpp @@ -513,6 +513,8 @@ 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 BALL_PLACEMENT_RETREAT_SPEED_METERS_PER_SECOND; default: LOG(WARNING) << "MaxAllowedSpeedMode is invalid" << std::endl; return 0.0; diff --git a/src/proto/primitive.proto b/src/proto/primitive.proto index 2d670e33ba..de8ead51be 100644 --- a/src/proto/primitive.proto +++ b/src/proto/primitive.proto @@ -68,6 +68,8 @@ 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; } message AutoChipOrKick diff --git a/src/shared/constants.h b/src/shared/constants.h index 5ff6cc0b18..1daef627d4 100644 --- a/src/shared/constants.h +++ b/src/shared/constants.h @@ -74,6 +74,8 @@ static const double STOP_COMMAND_ROBOT_MAX_SPEED_METERS_PER_SECOND = 1.5; // The margin by which the speed of the robot should stay below the maximum allowed speed // when the stop command is issued static const double STOP_COMMAND_SPEED_SAFETY_MARGIN_METERS_PER_SECOND = 0.2; +// The max speed at which we will retreat away from the ball after placing it +static const double BALL_PLACEMENT_RETREAT_SPEED_METERS_PER_SECOND = 0.3; // The max allowed speed of the robot before collisions would incur a foul static const double COLLISION_ALLOWED_ROBOT_MAX_SPEED_METERS_PER_SECOND = 0.5; // The minimum distance from the ball all robots must be when the stop command is issued 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 6cbcb60e40..1e5f4d6444 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 @@ -121,7 +121,7 @@ void BallPlacementPlayFSM::wait(const Update &event) wait_tactic->updateControlParams( nearest_robot->position(), nearest_robot->orientation(), 0.0, TbotsProto::DribblerMode::RELEASE_BALL_SLOW, TbotsProto::BallCollisionType::ALLOW, {AutoChipOrKickMode::OFF, 0}, - TbotsProto::MaxAllowedSpeedMode::STOP_COMMAND, // TODO: Instead of stop use a new speed mode for retreating in ball placement + TbotsProto::MaxAllowedSpeedMode::BALL_PLACEMENT_RETREAT, TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE, 0.0); tactics_to_run[0].emplace_back(wait_tactic); From bde271ab78237cda3503297f4ee075617c08c774 Mon Sep 17 00:00:00 2001 From: Mikhael Date: Sat, 29 Jun 2024 15:59:44 -0700 Subject: [PATCH 04/29] ball placement tweaks --- .../message_translation/tbots_protobuf.cpp | 4 + src/proto/parameters.proto | 6 +- src/proto/primitive.proto | 4 + src/shared/constants.h | 4 + .../ball_placement_play_fsm.cpp | 233 +++++++++++++----- .../ball_placement/ball_placement_play_fsm.h | 82 ++++-- .../hl/stp/tactic/attacker/attacker_fsm.cpp | 3 +- .../ai/hl/stp/tactic/dribble/dribble_fsm.cpp | 6 +- .../ai/hl/stp/tactic/dribble/dribble_fsm.h | 2 + .../hl/stp/tactic/dribble/dribble_tactic.cpp | 7 +- .../ai/hl/stp/tactic/dribble/dribble_tactic.h | 3 +- .../stp/tactic/pivot_kick/pivot_kick_fsm.cpp | 3 +- .../robot_navigation_obstacle_factory.cpp | 2 +- .../field_tests/ball_placement_field_test.py | 4 +- src/software/jetson_nano/services/motor.cpp | 4 +- src/software/sensor_fusion/sensor_fusion.cpp | 1 + .../thunderscope/thunderscope_config.py | 8 +- 17 files changed, 274 insertions(+), 102 deletions(-) diff --git a/src/proto/message_translation/tbots_protobuf.cpp b/src/proto/message_translation/tbots_protobuf.cpp index 40aaef5149..a215f595c9 100644 --- a/src/proto/message_translation/tbots_protobuf.cpp +++ b/src/proto/message_translation/tbots_protobuf.cpp @@ -513,6 +513,10 @@ double convertMaxAllowedSpeedModeToMaxAllowedSpeed( return COLLISION_ALLOWED_ROBOT_MAX_SPEED_METERS_PER_SECOND; case TbotsProto::MaxAllowedSpeedMode::BALL_PLACEMENT_RETREAT: return BALL_PLACEMENT_RETREAT_SPEED_METERS_PER_SECOND; + case TbotsProto::MaxAllowedSpeedMode::BALL_PLACEMENT_WALL_DRIBBLE: + return BALL_PLACEMENT_WALL_SPEED_METERS_PER_SECOND; + case TbotsProto::MaxAllowedSpeedMode::DRIBBLE: + return DRIBBLE_SPEED_METERS_PER_SECOND; default: LOG(WARNING) << "MaxAllowedSpeedMode is invalid" << std::endl; return 0.0; diff --git a/src/proto/parameters.proto b/src/proto/parameters.proto index 9791890f2d..7ea2bb4ddb 100644 --- a/src/proto/parameters.proto +++ b/src/proto/parameters.proto @@ -301,7 +301,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; @@ -392,6 +392,10 @@ 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 de8ead51be..40c9e21506 100644 --- a/src/proto/primitive.proto +++ b/src/proto/primitive.proto @@ -70,6 +70,10 @@ enum MaxAllowedSpeedMode 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/constants.h b/src/shared/constants.h index 1daef627d4..025d816d5c 100644 --- a/src/shared/constants.h +++ b/src/shared/constants.h @@ -76,6 +76,10 @@ static const double STOP_COMMAND_ROBOT_MAX_SPEED_METERS_PER_SECOND = 1.5; static const double STOP_COMMAND_SPEED_SAFETY_MARGIN_METERS_PER_SECOND = 0.2; // The max speed at which we will retreat away from the ball after placing it static const double BALL_PLACEMENT_RETREAT_SPEED_METERS_PER_SECOND = 0.3; +// The max speed at which we will pick the ball off the wall +static const double BALL_PLACEMENT_WALL_SPEED_METERS_PER_SECOND = 0.3; +// The max speed at which we dribble the ball +static const double DRIBBLE_SPEED_METERS_PER_SECOND = 1.5; // The max allowed speed of the robot before collisions would incur a foul static const double COLLISION_ALLOWED_ROBOT_MAX_SPEED_METERS_PER_SECOND = 0.5; // The minimum distance from the ball all robots must be when the stop command is issued 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 1e5f4d6444..42a0b1dee1 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 @@ -2,7 +2,8 @@ BallPlacementPlayFSM::BallPlacementPlayFSM(TbotsProto::AiConfig ai_config) : ai_config(ai_config), - pivot_kick_tactic(std::make_shared(ai_config)), + align_wall_tactic(std::make_shared()), + pickoff_wall_tactic(std::make_shared(ai_config)), place_ball_tactic(std::make_shared(ai_config)), align_placement_tactic(std::make_shared()), retreat_tactic(std::make_shared()), @@ -11,8 +12,7 @@ BallPlacementPlayFSM::BallPlacementPlayFSM(TbotsProto::AiConfig ai_config) { } -void BallPlacementPlayFSM::kickOffWall(const Update &event) -{ +void BallPlacementPlayFSM::alignWall(const Update &event) { PriorityTacticVector tactics_to_run = {{}}; // setup move tactics for robots away from ball placing robot @@ -22,13 +22,69 @@ void BallPlacementPlayFSM::kickOffWall(const Update &event) // 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}; + Rectangle field_boundary = event.common.world_ptr->field().fieldBoundary(); + + pickoff_final_orientation = calculateWallPickOffLocation(ball_pos, field_boundary, ROBOT_MAX_RADIUS_METERS * 4).first; + pickoff_point = ball_pos - Vector::createFromAngle(pickoff_final_orientation).normalize(0.4); + align_wall_tactic->updateControlParams(pickoff_point, pickoff_final_orientation, + 0.0, TbotsProto::DribblerMode::OFF, + TbotsProto::BallCollisionType::AVOID, {AutoChipOrKickMode::OFF, 0}, + TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, + TbotsProto::ObstacleAvoidanceMode::SAFE, 0.0); + tactics_to_run[0].emplace_back(align_wall_tactic); + + event.common.set_tactics(tactics_to_run); +} + +void BallPlacementPlayFSM::setPickOffDest(const BallPlacementPlayFSM::Update &event) { + Point ball_pos = event.common.world_ptr->ball().position(); + Rectangle field_boundary = event.common.world_ptr->field().fieldBoundary(); + + std::pair location = calculateWallPickOffLocation(ball_pos, field_boundary, ROBOT_MAX_RADIUS_METERS * 4); + + pickoff_final_orientation = location.first; + pickoff_destination = location.second - Vector::createFromAngle(pickoff_final_orientation).normalize(ROBOT_MAX_RADIUS_METERS * 5.5); +} + +void BallPlacementPlayFSM::pickOffWall(const BallPlacementPlayFSM::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()); + + pickoff_wall_tactic->updateControlParams(pickoff_destination, pickoff_final_orientation, true, TbotsProto::MaxAllowedSpeedMode::BALL_PLACEMENT_WALL_DRIBBLE); + + tactics_to_run[0].emplace_back(pickoff_wall_tactic); + + event.common.set_tactics(tactics_to_run); +} + +void BallPlacementPlayFSM::releaseBall(const BallPlacementPlayFSM::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 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); + 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()) + { + return; + } + + Point release_point = ball_pos + (robot_placing_ball->position() - ball_pos).normalize(.5); + + align_wall_tactic->updateControlParams(release_point, pickoff_final_orientation, + 0.0, TbotsProto::DribblerMode::RELEASE_BALL_SLOW, + TbotsProto::BallCollisionType::ALLOW, {AutoChipOrKickMode::OFF, 0}, + TbotsProto::MaxAllowedSpeedMode::BALL_PLACEMENT_RETREAT, + TbotsProto::ObstacleAvoidanceMode::SAFE, 0.0); + tactics_to_run[0].emplace_back(align_wall_tactic); event.common.set_tactics(tactics_to_run); } @@ -54,7 +110,7 @@ void BallPlacementPlayFSM::alignPlacement(const Update &event) .normalize(); Angle setup_angle = alignment_vector.orientation(); setup_point = event.common.world_ptr->ball().position() - - 2 * alignment_vector * ROBOT_MAX_RADIUS_METERS; + 2.5 * alignment_vector * ROBOT_MAX_RADIUS_METERS; align_placement_tactic->updateControlParams( setup_point, setup_angle, 0.0, TbotsProto::DribblerMode::OFF, @@ -70,6 +126,14 @@ void BallPlacementPlayFSM::alignPlacement(const Update &event) void BallPlacementPlayFSM::placeBall(const Update &event) { + std::optional placement_point = + event.common.world_ptr->gameState().getBallPlacementPoint(); + + if (!placement_point.has_value()) + { + return; + } + PriorityTacticVector tactics_to_run = {{}}; // setup move tactics for robots away from ball placing robot @@ -77,15 +141,21 @@ void BallPlacementPlayFSM::placeBall(const Update &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(); + Point ball_pos = event.common.world_ptr->ball().position(); + std::optional robot_placing_ball = event.common.world_ptr->friendlyTeam().getNearestRobot(ball_pos); - Vector placement_dribble_vector; - if (placement_point.has_value()) + if (!robot_placing_ball.has_value()) + { + return; + } + + Angle final_angle = robot_placing_ball->orientation(); + + + Vector placement_dribble_vector = + placement_point.value() - event.common.world_ptr->ball().position(); + if (placement_dribble_vector.length() > 0.3) // TODO: move to a dynamics parameter { - placement_dribble_vector = - placement_point.value() - event.common.world_ptr->ball().position(); final_angle = placement_dribble_vector.orientation(); } @@ -173,7 +243,7 @@ void BallPlacementPlayFSM::retreat(const Update &event) retreat_tactic->updateControlParams( retreat_position, final_orientation, 0.0, TbotsProto::DribblerMode::OFF, TbotsProto::BallCollisionType::AVOID, {AutoChipOrKickMode::OFF, 0}, - TbotsProto::MaxAllowedSpeedMode::STOP_COMMAND, // TODO: Instead of stop use a new speed mode for retreating in ball placement + TbotsProto::MaxAllowedSpeedMode::BALL_PLACEMENT_RETREAT, // TODO: Instead of stop use a new speed mode for retreating in ball placement TbotsProto::ObstacleAvoidanceMode::SAFE, 0.0); tactics_to_run[0].emplace_back(retreat_tactic); @@ -181,12 +251,13 @@ void BallPlacementPlayFSM::retreat(const Update &event) } } -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); + Rectangle field_lines = event.common.world_ptr->field().fieldBoundary(); + double wiggle_room = std::abs(signedDistance(ball_pos, field_lines)); + + return wiggle_room <= ROBOT_MAX_RADIUS_METERS * 4; // TODO: make this a constant } bool BallPlacementPlayFSM::alignDone(const Update &event) @@ -204,12 +275,22 @@ bool BallPlacementPlayFSM::alignDone(const Update &event) } } -bool BallPlacementPlayFSM::kickDone(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.ai_parameter_config().ball_is_kicked_m_per_s_threshold(); - return ball_velocity > ball_is_kicked_m_per_s_threshold; +bool BallPlacementPlayFSM::wallAlignDone(const BallPlacementPlayFSM::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(), pickoff_point); + } + else + { + return false; + } +} + +bool BallPlacementPlayFSM::wallPickOffDone(const Update &event) { + return pickoff_wall_tactic->done(); } bool BallPlacementPlayFSM::ballPlaced(const Update &event) @@ -248,55 +329,57 @@ bool BallPlacementPlayFSM::retreatDone(const Update &event) ->position()) > RETREAT_DISTANCE_METERS; } -Angle BallPlacementPlayFSM::calculateWallKickoffAngle(const Point &ball_pos, - const Rectangle &field_lines) +std::pair BallPlacementPlayFSM::calculateWallPickOffLocation(const Point &ball_pos, + const Rectangle &field_boundary, + const double max_dist) { - Angle kick_angle; - if (ball_pos.x() > field_lines.xMax()) + Angle facing_angle; + Point backoff_point; + bool near_north = field_boundary.yMax() - ball_pos.y() < max_dist; + bool near_south = ball_pos.y() - field_boundary.yMin() < max_dist; + bool near_east = field_boundary.xMax() - ball_pos.x() < max_dist; + bool near_west = ball_pos.x() - field_boundary.xMin() < max_dist; + if (near_north && near_east) // top right corner { - 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(); } - else if (ball_pos.x() < field_lines.xMin()) + else if (near_north && near_west) // top left corner { - 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(); } - else if (ball_pos.y() > field_lines.yMax()) + else if (near_south && near_east) // bottom right corner { - 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(); } - else if (ball_pos.y() < field_lines.yMin()) + else if (near_south && near_west) // bottom left corner { - 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(); + } + else if (near_north) + { + facing_angle = Angle::fromDegrees(90); + backoff_point = Point(ball_pos.x(), field_boundary.yMax()); + } + else if (near_east) + { + facing_angle = Angle::fromDegrees(0); + backoff_point = Point(field_boundary.xMax(), ball_pos.y()); + } + else if (near_south) + { + facing_angle = Angle::fromDegrees(-90); + backoff_point = Point(ball_pos.x(), field_boundary.yMin()); } - return kick_angle; + else if (near_west) + { + facing_angle = Angle::fromDegrees(180); + backoff_point = Point(field_boundary.xMin(), ball_pos.y()); + } + return std::make_pair(facing_angle, backoff_point); } void BallPlacementPlayFSM::setupMoveTactics(const Update &event) @@ -336,3 +419,17 @@ void BallPlacementPlayFSM::setupMoveTactics(const Update &event) TbotsProto::ObstacleAvoidanceMode::SAFE); } } + +bool BallPlacementPlayFSM::ballReleased(const BallPlacementPlayFSM::Update &event) { + 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()) + { + return false; + } + + return distance(ball_pos, robot_placing_ball->position()) > 0.4; +} + + 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 14f056a470..65c5907c83 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 @@ -8,14 +8,21 @@ #include "software/ai/hl/stp/tactic/pivot_kick/pivot_kick_tactic.h" #include "software/ai/passing/eighteen_zone_pitch_division.h" #include "software/ai/hl/stp/tactic/stop/stop_tactic.h" +#include "software/geom/algorithms/closest_point.h" +//TODO: align closer to the ball in wall pickoff +//TODO: wall pickoff, let go of the ball and drive back, then align for placement +// + using Zones = std::unordered_set; struct BallPlacementPlayFSM { class StartState; - class KickOffWallState; + class PickOffWallState; + class ReleaseWallState; + class AlignWallState; class AlignPlacementState; class PlaceBallState; class WaitState; @@ -40,7 +47,22 @@ struct BallPlacementPlayFSM * * @param event the BallPlacementPlayFSM Update event */ - void kickOffWall(const Update& event); + void alignWall(const Update& event); + + void setPickOffDest(const Update& event); + + /** + * Action that has the placing robot kick the ball off the wall to give more space to + * dribble + * + * @param event the BallPlacementPlayFSM Update event + */ + void pickOffWall(const Update& event); + + void releaseBall(const Update& event); + + + /** * Action that finds a point where the ball's current position aligns with the ball @@ -87,7 +109,7 @@ struct BallPlacementPlayFSM * * @return whether the ball is in a "dead zone" */ - bool shouldKickOffWall(const Update& event); + bool shouldPickOffWall(const Update& event); /** * Guard on whether the robot is aligned with the ball and placement point @@ -97,6 +119,14 @@ struct BallPlacementPlayFSM */ bool alignDone(const Update& event); + /** + * Action that has the placing robot kick the ball off the wall to give more space to + * dribble + * + * @param event the BallPlacementPlayFSM Update event + */ + bool wallAlignDone(const Update& event); + /** * Guard on whether the placing robot has finished moving the ball into a better * position with a kick @@ -105,7 +135,9 @@ struct BallPlacementPlayFSM * * @return whether the kick has been performed */ - bool kickDone(const Update& event); + bool wallPickOffDone(const Update& event); + + bool ballReleased(const Update& event); /** * Guard on whether the placing robot has finished placing the ball into the desired @@ -134,14 +166,14 @@ struct BallPlacementPlayFSM /** - * Helper function for calculating the angle to kick the ball off of a wall + * Helper function for calculating the angle at which the robot must face towards to pick up ball * * @param ball_pos the ball position to use when calculating the kick angle * @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 calculateWallPickOffLocation(const Point& ball_pos, const Rectangle& field_lines, double max_dist); /** * Helper function that populates the move_tactics field with MoveTactics that @@ -156,7 +188,9 @@ struct BallPlacementPlayFSM using namespace boost::sml; DEFINE_SML_STATE(StartState) - DEFINE_SML_STATE(KickOffWallState) + DEFINE_SML_STATE(AlignWallState) + DEFINE_SML_STATE(PickOffWallState) + DEFINE_SML_STATE(ReleaseWallState) DEFINE_SML_STATE(AlignPlacementState) DEFINE_SML_STATE(PlaceBallState) DEFINE_SML_STATE(WaitState) @@ -165,28 +199,36 @@ struct BallPlacementPlayFSM 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(releaseBall) DEFINE_SML_ACTION(startWait) DEFINE_SML_ACTION(retreat) DEFINE_SML_ACTION(wait) - 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(ballReleased) 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] = AlignWallState_S, + AlignWallState_S + Update_E[!wallAlignDone_G] / alignWall_A = AlignWallState_S, + AlignWallState_S + Update_E[wallAlignDone_G] / setPickOffDest_A = PickOffWallState_S, + AlignWallState_S + Update_E[!shouldPickOffWall_G] = AlignPlacementState_S, + PickOffWallState_S + Update_E[!wallPickOffDone_G] / pickOffWall_A = PickOffWallState_S, + PickOffWallState_S + Update_E[wallPickOffDone_G] = ReleaseWallState_S, + ReleaseWallState_S + Update_E[!ballReleased_G] / releaseBall_A = ReleaseWallState_S, + ReleaseWallState_S + Update_E[ballReleased_G] = AlignPlacementState_S, + AlignPlacementState_S + Update_E[shouldPickOffWall_G] = AlignWallState_S, AlignPlacementState_S + Update_E[!alignDone_G] / alignPlacement_A = AlignPlacementState_S, AlignPlacementState_S + Update_E[alignDone_G] = PlaceBallState_S, @@ -202,13 +244,17 @@ struct BallPlacementPlayFSM private: TbotsProto::AiConfig ai_config; - std::shared_ptr pivot_kick_tactic; + 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::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; diff --git a/src/software/ai/hl/stp/tactic/attacker/attacker_fsm.cpp b/src/software/ai/hl/stp/tactic/attacker/attacker_fsm.cpp index a1b7c06323..3167c8af0e 100644 --- a/src/software/ai/hl/stp/tactic/attacker/attacker_fsm.cpp +++ b/src/software/ai/hl/stp/tactic/attacker/attacker_fsm.cpp @@ -81,7 +81,8 @@ void AttackerFSM::keepAway(const Update& event, control_params = {.dribble_destination = keepaway_dribble_dest, .final_dribble_orientation = final_dribble_orientation, - .allow_excessive_dribbling = false}; + .allow_excessive_dribbling = false, + .max_speed = TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT}; processEvent(DribbleFSM::Update(control_params, event.common)); 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 f71cd940e2..d6f6a894be 100644 --- a/src/software/ai/hl/stp/tactic/dribble/dribble_fsm.cpp +++ b/src/software/ai/hl/stp/tactic/dribble/dribble_fsm.cpp @@ -94,7 +94,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, TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE, TbotsProto::DribblerMode::MAX_FORCE, TbotsProto::BallCollisionType::ALLOW, AutoChipOrKick{AutoChipOrKickMode::OFF, 0})); @@ -110,7 +110,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, TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE, TbotsProto::DribblerMode::MAX_FORCE, TbotsProto::BallCollisionType::ALLOW, AutoChipOrKick{AutoChipOrKickMode::OFF, 0})); @@ -127,7 +127,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, TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE, TbotsProto::DribblerMode::OFF, TbotsProto::BallCollisionType::AVOID, AutoChipOrKick{AutoChipOrKickMode::AUTOKICK, 0.5})); 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 d21a35863f..24e46b7910 100644 --- a/src/software/ai/hl/stp/tactic/dribble/dribble_fsm.h +++ b/src/software/ai/hl/stp/tactic/dribble/dribble_fsm.h @@ -36,6 +36,8 @@ struct DribbleFSM 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 = TbotsProto::MaxAllowedSpeedMode::DRIBBLE; }; DEFINE_TACTIC_UPDATE_STRUCT_WITH_CONTROL_AND_COMMON_PARAMS 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 384fd863ea..e25d550302 100644 --- a/src/software/ai/hl/stp/tactic/dribble/dribble_tactic.cpp +++ b/src/software/ai/hl/stp/tactic/dribble/dribble_tactic.cpp @@ -7,7 +7,8 @@ DribbleTactic::DribbleTactic(TbotsProto::AiConfig ai_config) fsm_map(), control_params{DribbleFSM::ControlParams{.dribble_destination = std::nullopt, .final_dribble_orientation = std::nullopt, - .allow_excessive_dribbling = false}}, + .allow_excessive_dribbling = false, + .max_speed = TbotsProto::MaxAllowedSpeedMode::DRIBBLE}}, ai_config(ai_config) { for (RobotId id = 0; id < MAX_ROBOT_IDS; id++) @@ -19,11 +20,13 @@ DribbleTactic::DribbleTactic(TbotsProto::AiConfig ai_config) void DribbleTactic::updateControlParams(std::optional dribble_destination, std::optional final_dribble_orientation, - bool allow_excessive_dribbling) + bool allow_excessive_dribbling, + TbotsProto::MaxAllowedSpeedMode max_speed) { 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 = max_speed; } 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 0a28ec1f03..6d2774a94e 100644 --- a/src/software/ai/hl/stp/tactic/dribble/dribble_tactic.h +++ b/src/software/ai/hl/stp/tactic/dribble/dribble_tactic.h @@ -35,7 +35,8 @@ class DribbleTactic : public Tactic */ 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 = TbotsProto::MaxAllowedSpeedMode::DRIBBLE); void accept(TacticVisitor& visitor) const override; diff --git a/src/software/ai/hl/stp/tactic/pivot_kick/pivot_kick_fsm.cpp b/src/software/ai/hl/stp/tactic/pivot_kick/pivot_kick_fsm.cpp index 9217ef92ed..016f45a77c 100644 --- a/src/software/ai/hl/stp/tactic/pivot_kick/pivot_kick_fsm.cpp +++ b/src/software/ai/hl/stp/tactic/pivot_kick/pivot_kick_fsm.cpp @@ -9,7 +9,8 @@ void PivotKickFSM::getPossessionAndPivot( DribbleFSM::ControlParams control_params{ .dribble_destination = event.control_params.kick_origin, .final_dribble_orientation = event.control_params.kick_direction, - .allow_excessive_dribbling = false}; + .allow_excessive_dribbling = false, + .max_speed = TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT}; processEvent(DribbleFSM::Update(control_params, event.common)); } 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 98458d9ad3..18670f48d0 100644 --- a/src/software/ai/navigator/obstacle/robot_navigation_obstacle_factory.cpp +++ b/src/software/ai/navigator/obstacle/robot_navigation_obstacle_factory.cpp @@ -158,7 +158,7 @@ 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/field_tests/ball_placement_field_test.py b/src/software/field_tests/ball_placement_field_test.py index d7a8109503..52e34c4692 100644 --- a/src/software/field_tests/ball_placement_field_test.py +++ b/src/software/field_tests/ball_placement_field_test.py @@ -18,9 +18,9 @@ def test_ball_placement(field_test_runner): # robot ID - id = 4 + id = 2 # point to place ball - placement_point = Point(x_meters=0, y_meters=0) + 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:") diff --git a/src/software/jetson_nano/services/motor.cpp b/src/software/jetson_nano/services/motor.cpp index b1c095a1a1..4caf2a8fd0 100644 --- a/src/software/jetson_nano/services/motor.cpp +++ b/src/software/jetson_nano/services/motor.cpp @@ -838,9 +838,9 @@ void MotorService::writeToDriverOrDieTrying(uint8_t motor, uint8_t address, int3 // the chip to clear any bad values we just wrote and crash so everything stops. reset_gpio_.setValue(GpioState::LOW); CHECK(read_value == value) << "Couldn't write " << value - << " to the TMC6100 at address " << address - << " at address " << static_cast(address) + << " to the TMC6100" << " on motor " << static_cast(motor) + << " at address " << static_cast(address) << " received: " << read_value; } diff --git a/src/software/sensor_fusion/sensor_fusion.cpp b/src/software/sensor_fusion/sensor_fusion.cpp index ea672d93b1..d6b7175cba 100644 --- a/src/software/sensor_fusion/sensor_fusion.cpp +++ b/src/software/sensor_fusion/sensor_fusion.cpp @@ -231,6 +231,7 @@ void SensorFusion::updateWorld(const SSLProto::SSL_DetectionFrame &ssl_detection { friendly_robot_id_with_ball_in_dribbler = std::nullopt; ball_in_dribbler_timeout = 0; + LOG(DEBUG) << "ball_in_dribbler_timeout: " << ball_in_dribbler_timeout; } if (friendly_robot_id_with_ball_in_dribbler.has_value()) diff --git a/src/software/thunderscope/thunderscope_config.py b/src/software/thunderscope/thunderscope_config.py index 28a7c731b3..a381a56b1c 100644 --- a/src/software/thunderscope/thunderscope_config.py +++ b/src/software/thunderscope/thunderscope_config.py @@ -516,7 +516,9 @@ def configure_replay_view( replay=True, replay_log=blue_replay_log, visualization_buffer_size=visualization_buffer_size, - extra_widgets=[], + extra_widgets=[ + configure_robot_view_fullsystem(proto_unix_io_map[ProtoUnixIOTypes.BLUE]) + ], ), ) ) @@ -536,7 +538,9 @@ def configure_replay_view( replay=True, replay_log=yellow_replay_log, visualization_buffer_size=visualization_buffer_size, - extra_widgets=[], + extra_widgets=[ + configure_robot_view_fullsystem(proto_unix_io_map[ProtoUnixIOTypes.YELLOW]) + ], ), ) ) From 091cedfd01085bd67a62e312635036aaf49d7e0d Mon Sep 17 00:00:00 2001 From: Mikhael Date: Fri, 5 Jul 2024 11:48:28 -0700 Subject: [PATCH 05/29] Tuned motion planning, adjusted field test --- .../ball_placement_play_fsm.cpp | 2 +- .../ball_placement/ball_placement_play_fsm.h | 8 +++- .../field_tests/movement_robot_field_test.py | 42 +++++++++++-------- src/software/jetson_nano/primitive_executor.h | 2 +- 4 files changed, 32 insertions(+), 22 deletions(-) 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 42a0b1dee1..53a30b4fc8 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 @@ -161,7 +161,7 @@ void BallPlacementPlayFSM::placeBall(const Update &event) // setup ball placement tactic for ball placing robot place_ball_tactic->updateControlParams( - event.common.world_ptr->gameState().getBallPlacementPoint(), final_angle, true); + event.common.world_ptr->gameState().getBallPlacementPoint(), final_angle, true, TbotsProto::MaxAllowedSpeedMode::DRIBBLE); tactics_to_run[0].emplace_back(place_ball_tactic); event.common.set_tactics(tactics_to_run); 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 65c5907c83..b7eece06ba 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 @@ -21,6 +21,7 @@ struct BallPlacementPlayFSM { class StartState; class PickOffWallState; + class PickOffWaitState; class ReleaseWallState; class AlignWallState; class AlignPlacementState; @@ -190,6 +191,7 @@ struct BallPlacementPlayFSM DEFINE_SML_STATE(StartState) DEFINE_SML_STATE(AlignWallState) DEFINE_SML_STATE(PickOffWallState) + DEFINE_SML_STATE(PickOffWaitState) DEFINE_SML_STATE(ReleaseWallState) DEFINE_SML_STATE(AlignPlacementState) DEFINE_SML_STATE(PlaceBallState) @@ -225,7 +227,9 @@ struct BallPlacementPlayFSM AlignWallState_S + Update_E[wallAlignDone_G] / setPickOffDest_A = PickOffWallState_S, AlignWallState_S + Update_E[!shouldPickOffWall_G] = AlignPlacementState_S, PickOffWallState_S + Update_E[!wallPickOffDone_G] / pickOffWall_A = PickOffWallState_S, - PickOffWallState_S + Update_E[wallPickOffDone_G] = ReleaseWallState_S, + PickOffWallState_S + Update_E[wallPickOffDone_G] / startWait_A = PickOffWaitState_S, + PickOffWaitState_S + Update_E[!waitDone_G] / wait_A = PickOffWaitState_S, + PickOffWaitState_S + Update_E[waitDone_G] = ReleaseWallState_S, ReleaseWallState_S + Update_E[!ballReleased_G] / releaseBall_A = ReleaseWallState_S, ReleaseWallState_S + Update_E[ballReleased_G] = AlignPlacementState_S, AlignPlacementState_S + Update_E[shouldPickOffWall_G] = AlignWallState_S, @@ -259,5 +263,5 @@ struct BallPlacementPlayFSM 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; + constexpr static double const BALL_IS_PLACED_WAIT_S = 2.0; }; diff --git a/src/software/field_tests/movement_robot_field_test.py b/src/software/field_tests/movement_robot_field_test.py index a904c86095..fa0548f341 100644 --- a/src/software/field_tests/movement_robot_field_test.py +++ b/src/software/field_tests/movement_robot_field_test.py @@ -78,7 +78,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: @@ -122,7 +122,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 stop tactic after the test finishes stop_tactic = StopTactic() @@ -134,7 +134,7 @@ def test_basic_rotation(field_test_runner): # validate by eye logger.info(f"robot set to {angle} orientation") - time.sleep(2) + # time.sleep(2) def test_one_robots_square(field_test_runner): @@ -150,13 +150,18 @@ def test_one_robots_square(field_test_runner): ] ) + + # field_test_runner.gamecontroller.send_gc_command( + # gc_command=Command.Type.FORCE_START, team=Team.BLUE + # ) + id = world.friendly_team.team_robots[0].id print(f"Running test on robot {id}") - point1 = Point(x_meters=-0.3, y_meters=0.6) - point2 = Point(x_meters=-0.3, y_meters=-0.6) - point3 = Point(x_meters=-1.5, y_meters=-0.6) - point4 = Point(x_meters=-1.5, y_meters=0.6) + point1 = Point(x_meters=-0.75, y_meters=0.6) + point2 = Point(x_meters=-0.75, y_meters=-0.6) + point3 = Point(x_meters=0.75, y_meters=-0.6) + point4 = Point(x_meters=0.75, y_meters=0.6) tactic_0 = MoveTactic( destination=point1, @@ -204,17 +209,18 @@ def test_one_robots_square(field_test_runner): ) tactics = [tactic_0, tactic_1, tactic_2, tactic_3] - for tactic in tactics: - print(f"Going to {tactic.destination}") - params = AssignedTacticPlayControlParams() - params.assigned_tactics[id].move.CopyFrom(tactic) - - field_test_runner.set_tactics(params, True) - field_test_runner.run_test( - always_validation_sequence_set=[[]], - eventually_validation_sequence_set=[[]], - test_timeout_s=4, - ) + for i in range(3): + for tactic in tactics: + print(f"Going to {tactic.destination}") + params = AssignedTacticPlayControlParams() + params.assigned_tactics[id].move.CopyFrom(tactic) + + field_test_runner.set_tactics(params, True) + field_test_runner.run_test( + always_validation_sequence_set=[[]], + eventually_validation_sequence_set=[[]], + test_timeout_s=4, + ) # Send a stop tactic after the test finishes stop_tactic = StopTactic() diff --git a/src/software/jetson_nano/primitive_executor.h b/src/software/jetson_nano/primitive_executor.h index c95b4638f2..f08773c6a4 100644 --- a/src/software/jetson_nano/primitive_executor.h +++ b/src/software/jetson_nano/primitive_executor.h @@ -90,7 +90,7 @@ class PrimitiveExecutor RobotId robot_id_; // Estimated delay between a vision frame to AI processing to robot executing - static constexpr double VISION_TO_ROBOT_DELAY_S = 0.03; + static constexpr double VISION_TO_ROBOT_DELAY_S = 0.06; // The distance away from the destination at which we start dampening the velocity // to avoid jittering around the destination. From a1b00b1ea1913922400f9eb1dc020c214afefab5 Mon Sep 17 00:00:00 2001 From: Mikhael Date: Fri, 5 Jul 2024 11:56:41 -0700 Subject: [PATCH 06/29] Reverted some changes made for debugging --- src/software/ai/hl/stp/tactic/attacker/attacker_fsm.cpp | 3 +-- src/software/ai/hl/stp/tactic/pivot_kick/pivot_kick_fsm.cpp | 3 +-- .../thunderscope/binary_context_managers/game_controller.py | 2 +- 3 files changed, 3 insertions(+), 5 deletions(-) diff --git a/src/software/ai/hl/stp/tactic/attacker/attacker_fsm.cpp b/src/software/ai/hl/stp/tactic/attacker/attacker_fsm.cpp index 3167c8af0e..a1b7c06323 100644 --- a/src/software/ai/hl/stp/tactic/attacker/attacker_fsm.cpp +++ b/src/software/ai/hl/stp/tactic/attacker/attacker_fsm.cpp @@ -81,8 +81,7 @@ void AttackerFSM::keepAway(const Update& event, control_params = {.dribble_destination = keepaway_dribble_dest, .final_dribble_orientation = final_dribble_orientation, - .allow_excessive_dribbling = false, - .max_speed = TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT}; + .allow_excessive_dribbling = false}; processEvent(DribbleFSM::Update(control_params, event.common)); diff --git a/src/software/ai/hl/stp/tactic/pivot_kick/pivot_kick_fsm.cpp b/src/software/ai/hl/stp/tactic/pivot_kick/pivot_kick_fsm.cpp index 016f45a77c..9217ef92ed 100644 --- a/src/software/ai/hl/stp/tactic/pivot_kick/pivot_kick_fsm.cpp +++ b/src/software/ai/hl/stp/tactic/pivot_kick/pivot_kick_fsm.cpp @@ -9,8 +9,7 @@ void PivotKickFSM::getPossessionAndPivot( DribbleFSM::ControlParams control_params{ .dribble_destination = event.control_params.kick_origin, .final_dribble_orientation = event.control_params.kick_direction, - .allow_excessive_dribbling = false, - .max_speed = TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT}; + .allow_excessive_dribbling = false}; processEvent(DribbleFSM::Update(control_params, event.common)); } diff --git a/src/software/thunderscope/binary_context_managers/game_controller.py b/src/software/thunderscope/binary_context_managers/game_controller.py index cadbde2d6f..d8a8b15118 100644 --- a/src/software/thunderscope/binary_context_managers/game_controller.py +++ b/src/software/thunderscope/binary_context_managers/game_controller.py @@ -164,7 +164,7 @@ def send_gc_command( self, gc_command: proto.ssl_gc_state_pb2.Command.Type, team: proto.ssl_gc_common_pb2.Team, - final_ball_placement_point: Point = None, + final_ball_placement_point: tbots_cpp.Point = None, ) -> Any: """Send a ci input to the gamecontroller. From a18b9406dafc14bd7f5ec0ddddd19346eb4cc83b Mon Sep 17 00:00:00 2001 From: Mikhael Date: Mon, 8 Jul 2024 12:38:53 -0700 Subject: [PATCH 07/29] Wrote comments --- .../ball_placement/ball_placement_play_fsm.h | 40 ++++++++++++------- .../ai/hl/stp/tactic/dribble/dribble_tactic.h | 1 + 2 files changed, 26 insertions(+), 15 deletions(-) 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 b7eece06ba..87e934e749 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 @@ -10,11 +10,6 @@ #include "software/ai/hl/stp/tactic/stop/stop_tactic.h" #include "software/geom/algorithms/closest_point.h" - -//TODO: align closer to the ball in wall pickoff -//TODO: wall pickoff, let go of the ball and drive back, then align for placement -// - using Zones = std::unordered_set; struct BallPlacementPlayFSM @@ -43,23 +38,31 @@ struct BallPlacementPlayFSM explicit BallPlacementPlayFSM(TbotsProto::AiConfig ai_config); /** - * 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); + /** + * The transition action into picking the ball off the wall, set the target destination. + * + * @param event the BallPlacementPlayFSM Update event + */ void setPickOffDest(const Update& event); /** - * Action that has the placing robot kick the ball off the wall to give more space to - * dribble + * Action that has the robot slowly pick up the ball and dribble it away the wall * * @param event the BallPlacementPlayFSM Update event */ void pickOffWall(const Update& event); + /** + * Action that slowly drives away from the ball after picking it off the wall + * + * @param event the BallPlacementPlayFSM Update event + */ void releaseBall(const Update& event); @@ -104,11 +107,11 @@ struct BallPlacementPlayFSM 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 shouldPickOffWall(const Update& event); @@ -121,23 +124,30 @@ struct BallPlacementPlayFSM bool alignDone(const Update& event); /** - * Action that has the placing robot kick the ball off the wall to give more space to - * dribble + * Guard on wether 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 wallPickOffDone(const Update& event); + /** + * Guard on whether the placing robot has finished moving away from the ball + * + * @param event the BallPlacementPlayFSM Update event + * + * @return whether the robot has backed off from the ball + */ bool ballReleased(const Update& event); /** 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 6d2774a94e..49721e0c73 100644 --- a/src/software/ai/hl/stp/tactic/dribble/dribble_tactic.h +++ b/src/software/ai/hl/stp/tactic/dribble/dribble_tactic.h @@ -32,6 +32,7 @@ class DribbleTactic : public Tactic * finishing dribbling * @param allow_excessive_dribbling Whether to allow excessive dribbling, i.e. more * than 1 metre at a time + * @param max_speed The max speed attained while the ball is in possession */ void updateControlParams(std::optional dribble_destination, std::optional final_dribble_orientation, From 7d0a80e8411cbc09d75e929d6496b21fdd0f24ef Mon Sep 17 00:00:00 2001 From: Mikhael Date: Mon, 8 Jul 2024 12:46:36 -0700 Subject: [PATCH 08/29] Split dribble speed modes into 2 --- .../stp/play/ball_placement/ball_placement_play_fsm.cpp | 4 ++-- src/software/ai/hl/stp/tactic/dribble/dribble_fsm.cpp | 6 +++--- src/software/ai/hl/stp/tactic/dribble/dribble_fsm.h | 3 ++- src/software/ai/hl/stp/tactic/dribble/dribble_tactic.cpp | 9 ++++++--- src/software/ai/hl/stp/tactic/dribble/dribble_tactic.h | 5 +++-- 5 files changed, 16 insertions(+), 11 deletions(-) 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 53a30b4fc8..d015b7cb2e 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 @@ -54,7 +54,7 @@ void BallPlacementPlayFSM::pickOffWall(const BallPlacementPlayFSM::Update &event tactics_to_run[0].insert(tactics_to_run[0].end(), move_tactics.begin(), move_tactics.end()); - pickoff_wall_tactic->updateControlParams(pickoff_destination, pickoff_final_orientation, true, TbotsProto::MaxAllowedSpeedMode::BALL_PLACEMENT_WALL_DRIBBLE); + pickoff_wall_tactic->updateControlParams(pickoff_destination, pickoff_final_orientation, true, TbotsProto::MaxAllowedSpeedMode::BALL_PLACEMENT_WALL_DRIBBLE, TbotsProto::MaxAllowedSpeedMode::BALL_PLACEMENT_WALL_DRIBBLE); tactics_to_run[0].emplace_back(pickoff_wall_tactic); @@ -115,7 +115,7 @@ void BallPlacementPlayFSM::alignPlacement(const Update &event) align_placement_tactic->updateControlParams( setup_point, setup_angle, 0.0, TbotsProto::DribblerMode::OFF, TbotsProto::BallCollisionType::AVOID, {AutoChipOrKickMode::OFF, 0}, - TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, + TbotsProto::MaxAllowedSpeedMode::STOP_COMMAND, TbotsProto::ObstacleAvoidanceMode::SAFE, 0.0); tactics_to_run[0].emplace_back(align_placement_tactic); 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 d6f6a894be..84df6775f8 100644 --- a/src/software/ai/hl/stp/tactic/dribble/dribble_fsm.cpp +++ b/src/software/ai/hl/stp/tactic/dribble/dribble_fsm.cpp @@ -94,7 +94,7 @@ void DribbleFSM::getPossession(const Update &event) event.common.set_primitive(std::make_unique( event.common.robot, intercept_position, face_ball_orientation, - event.control_params.max_speed, + event.control_params.max_speed_get_possession, TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE, TbotsProto::DribblerMode::MAX_FORCE, TbotsProto::BallCollisionType::ALLOW, AutoChipOrKick{AutoChipOrKickMode::OFF, 0})); @@ -110,7 +110,7 @@ void DribbleFSM::dribble(const Update &event) event.common.set_primitive(std::make_unique( event.common.robot, target_destination, target_orientation, - event.control_params.max_speed, + event.control_params.max_speed_dribble, TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE, TbotsProto::DribblerMode::MAX_FORCE, TbotsProto::BallCollisionType::ALLOW, AutoChipOrKick{AutoChipOrKickMode::OFF, 0})); @@ -127,7 +127,7 @@ void DribbleFSM::loseBall(const Update &event) event.common.set_primitive(std::make_unique( event.common.robot, away_from_ball_position, face_ball_orientation, - event.control_params.max_speed, + event.control_params.max_speed_get_possession, TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE, TbotsProto::DribblerMode::OFF, TbotsProto::BallCollisionType::AVOID, AutoChipOrKick{AutoChipOrKickMode::AUTOKICK, 0.5})); 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 24e46b7910..0d3dea5cb7 100644 --- a/src/software/ai/hl/stp/tactic/dribble/dribble_fsm.h +++ b/src/software/ai/hl/stp/tactic/dribble/dribble_fsm.h @@ -37,7 +37,8 @@ struct DribbleFSM // 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 = TbotsProto::MaxAllowedSpeedMode::DRIBBLE; + TbotsProto::MaxAllowedSpeedMode max_speed_dribble = TbotsProto::MaxAllowedSpeedMode::DRIBBLE; + TbotsProto::MaxAllowedSpeedMode max_speed_get_possession = TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT; }; DEFINE_TACTIC_UPDATE_STRUCT_WITH_CONTROL_AND_COMMON_PARAMS 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 e25d550302..03807fc6ae 100644 --- a/src/software/ai/hl/stp/tactic/dribble/dribble_tactic.cpp +++ b/src/software/ai/hl/stp/tactic/dribble/dribble_tactic.cpp @@ -8,7 +8,8 @@ DribbleTactic::DribbleTactic(TbotsProto::AiConfig ai_config) control_params{DribbleFSM::ControlParams{.dribble_destination = std::nullopt, .final_dribble_orientation = std::nullopt, .allow_excessive_dribbling = false, - .max_speed = TbotsProto::MaxAllowedSpeedMode::DRIBBLE}}, + .max_speed_dribble = TbotsProto::MaxAllowedSpeedMode::DRIBBLE, + .max_speed_get_possession = TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT}}, ai_config(ai_config) { for (RobotId id = 0; id < MAX_ROBOT_IDS; id++) @@ -21,12 +22,14 @@ DribbleTactic::DribbleTactic(TbotsProto::AiConfig ai_config) void DribbleTactic::updateControlParams(std::optional dribble_destination, std::optional final_dribble_orientation, bool allow_excessive_dribbling, - TbotsProto::MaxAllowedSpeedMode max_speed) + 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 = max_speed; + 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 49721e0c73..d821748365 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,13 @@ class DribbleTactic : public Tactic * finishing dribbling * @param allow_excessive_dribbling Whether to allow excessive dribbling, i.e. more * than 1 metre at a time - * @param max_speed The max speed attained while the ball is in possession + * @param max_speed_dribble The max speed attained while the ball is in possession */ void updateControlParams(std::optional dribble_destination, std::optional final_dribble_orientation, bool allow_excessive_dribbling = false, - TbotsProto::MaxAllowedSpeedMode max_speed = TbotsProto::MaxAllowedSpeedMode::DRIBBLE); + TbotsProto::MaxAllowedSpeedMode max_speed_dribble = TbotsProto::MaxAllowedSpeedMode::DRIBBLE, + TbotsProto::MaxAllowedSpeedMode max_speed_get_possession = TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT); void accept(TacticVisitor& visitor) const override; From d37546c8f5b204ae312a5d59904d3f8a55a84643 Mon Sep 17 00:00:00 2001 From: Mikhael Date: Mon, 8 Jul 2024 13:11:00 -0700 Subject: [PATCH 09/29] Addressed more arune notes --- .../message_translation/tbots_protobuf.cpp | 2 +- src/shared/2021_robot_constants.cpp | 1 + src/shared/robot_constants.h | 3 ++ .../ball_placement_play_fsm.cpp | 40 +++++++++---------- .../hl/stp/tactic/dribble/dribble_tactic.cpp | 2 +- .../ai/hl/stp/tactic/move/move_fsm.cpp | 2 +- 6 files changed, 27 insertions(+), 23 deletions(-) diff --git a/src/proto/message_translation/tbots_protobuf.cpp b/src/proto/message_translation/tbots_protobuf.cpp index a215f595c9..55d69582fe 100644 --- a/src/proto/message_translation/tbots_protobuf.cpp +++ b/src/proto/message_translation/tbots_protobuf.cpp @@ -491,7 +491,7 @@ double convertDribblerModeToDribblerSpeed(TbotsProto::DribblerMode dribbler_mode case TbotsProto::DribblerMode::OFF: return 0.0; case TbotsProto::DribblerMode::RELEASE_BALL_SLOW: - return 2000.0; + return robot_constants.release_ball_dribbler_speed_rpm; default: LOG(WARNING) << "DribblerMode is invalid" << std::endl; return 0.0; diff --git a/src/shared/2021_robot_constants.cpp b/src/shared/2021_robot_constants.cpp index e14417f0d7..81f5866f18 100644 --- a/src/shared/2021_robot_constants.cpp +++ b/src/shared/2021_robot_constants.cpp @@ -20,6 +20,7 @@ RobotConstants_t create2021RobotConstants(void) // Dribbler speeds are negative as that is the direction that sucks the ball in .indefinite_dribbler_speed_rpm = -10000.0f, .max_force_dribbler_speed_rpm = -12000.0f, + .release_ball_dribbler_speed_rpm = 2000.0f, // Motor constant .motor_max_acceleration_m_per_s_2 = 4.5f, diff --git a/src/shared/robot_constants.h b/src/shared/robot_constants.h index d8f283eb9b..b54f440e6e 100644 --- a/src/shared/robot_constants.h +++ b/src/shared/robot_constants.h @@ -73,6 +73,9 @@ typedef struct RobotConstants // the ball [rpm] float max_force_dribbler_speed_rpm; + // Speed at which to spin the dribbler to release the ball right in front of a stationary robot [rpm] + float release_ball_dribbler_speed_rpm; + // The maximum acceleration achievable by our motors [m/s^2] float motor_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 d015b7cb2e..ca5e658809 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 @@ -30,7 +30,7 @@ void BallPlacementPlayFSM::alignWall(const Update &event) { 0.0, TbotsProto::DribblerMode::OFF, TbotsProto::BallCollisionType::AVOID, {AutoChipOrKickMode::OFF, 0}, TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, - TbotsProto::ObstacleAvoidanceMode::SAFE, 0.0); + TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE, 0.0); tactics_to_run[0].emplace_back(align_wall_tactic); event.common.set_tactics(tactics_to_run); @@ -83,7 +83,7 @@ void BallPlacementPlayFSM::releaseBall(const BallPlacementPlayFSM::Update &event 0.0, TbotsProto::DribblerMode::RELEASE_BALL_SLOW, TbotsProto::BallCollisionType::ALLOW, {AutoChipOrKickMode::OFF, 0}, TbotsProto::MaxAllowedSpeedMode::BALL_PLACEMENT_RETREAT, - TbotsProto::ObstacleAvoidanceMode::SAFE, 0.0); + TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE, 0.0); tactics_to_run[0].emplace_back(align_wall_tactic); event.common.set_tactics(tactics_to_run); @@ -154,7 +154,7 @@ void BallPlacementPlayFSM::placeBall(const Update &event) Vector placement_dribble_vector = placement_point.value() - event.common.world_ptr->ball().position(); - if (placement_dribble_vector.length() > 0.3) // TODO: move to a dynamics parameter + if (placement_dribble_vector.length() > 0.3) { final_angle = placement_dribble_vector.orientation(); } @@ -243,7 +243,7 @@ void BallPlacementPlayFSM::retreat(const Update &event) retreat_tactic->updateControlParams( retreat_position, final_orientation, 0.0, TbotsProto::DribblerMode::OFF, TbotsProto::BallCollisionType::AVOID, {AutoChipOrKickMode::OFF, 0}, - TbotsProto::MaxAllowedSpeedMode::BALL_PLACEMENT_RETREAT, // TODO: Instead of stop use a new speed mode for retreating in ball placement + TbotsProto::MaxAllowedSpeedMode::BALL_PLACEMENT_RETREAT, TbotsProto::ObstacleAvoidanceMode::SAFE, 0.0); tactics_to_run[0].emplace_back(retreat_tactic); @@ -335,46 +335,46 @@ std::pair BallPlacementPlayFSM::calculateWallPickOffLocation(const { Angle facing_angle; Point backoff_point; - bool near_north = field_boundary.yMax() - ball_pos.y() < max_dist; - bool near_south = ball_pos.y() - field_boundary.yMin() < max_dist; - bool near_east = field_boundary.xMax() - ball_pos.x() < max_dist; - bool near_west = ball_pos.x() - field_boundary.xMin() < max_dist; - if (near_north && near_east) // top right corner + bool near_positive_y_boundary = (field_boundary.yMax() - ball_pos.y()) < max_dist; + bool near_negative_y_boundary = (ball_pos.y() - field_boundary.yMin()) < max_dist; + bool near_positive_x_boundary = (field_boundary.xMax() - ball_pos.x()) < max_dist; + bool near_negative_x_boundary = (ball_pos.x() - field_boundary.xMin()) < max_dist; + if (near_positive_y_boundary && near_positive_x_boundary) // top right corner { facing_angle = Angle::fromDegrees(45); - backoff_point = field_boundary.posXPosYCorner(); + backoff_point = field_boundary.posXPosYCorner() - Vector::createFromAngle(facing_angle).normalize(0.9); } - else if (near_north && near_west) // top left corner + else if (near_positive_y_boundary && near_negative_x_boundary) // top left corner { facing_angle = Angle::fromDegrees(135); - backoff_point = field_boundary.negXPosYCorner(); + backoff_point = field_boundary.negXPosYCorner() - Vector::createFromAngle(facing_angle).normalize(0.9); } - else if (near_south && near_east) // bottom right corner + else if (near_negative_y_boundary && near_positive_x_boundary) // bottom right corner { facing_angle = Angle::fromDegrees(-45); - backoff_point = field_boundary.posXNegYCorner(); + backoff_point = field_boundary.posXNegYCorner() - Vector::createFromAngle(facing_angle).normalize(0.9); } - else if (near_south && near_west) // bottom left corner + else if (near_negative_y_boundary && near_negative_x_boundary) // bottom left corner { facing_angle = Angle::fromDegrees(-135); - backoff_point = field_boundary.negXNegYCorner(); + backoff_point = field_boundary.negXNegYCorner() - Vector::createFromAngle(facing_angle).normalize(0.9); } - else if (near_north) + else if (near_positive_y_boundary) { facing_angle = Angle::fromDegrees(90); backoff_point = Point(ball_pos.x(), field_boundary.yMax()); } - else if (near_east) + else if (near_positive_x_boundary) { facing_angle = Angle::fromDegrees(0); backoff_point = Point(field_boundary.xMax(), ball_pos.y()); } - else if (near_south) + else if (near_negative_y_boundary) { facing_angle = Angle::fromDegrees(-90); backoff_point = Point(ball_pos.x(), field_boundary.yMin()); } - else if (near_west) + else if (near_negative_x_boundary) { facing_angle = Angle::fromDegrees(180); backoff_point = Point(field_boundary.xMin(), ball_pos.y()); 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 03807fc6ae..cfb83fa325 100644 --- a/src/software/ai/hl/stp/tactic/dribble/dribble_tactic.cpp +++ b/src/software/ai/hl/stp/tactic/dribble/dribble_tactic.cpp @@ -29,7 +29,7 @@ void DribbleTactic::updateControlParams(std::optional 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 + 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/move/move_fsm.cpp b/src/software/ai/hl/stp/tactic/move/move_fsm.cpp index 99982f593e..b6f4d16488 100644 --- a/src/software/ai/hl/stp/tactic/move/move_fsm.cpp +++ b/src/software/ai/hl/stp/tactic/move/move_fsm.cpp @@ -15,6 +15,6 @@ void MoveFSM::updateMove(const Update &event) bool MoveFSM::moveDone(const Update &event) { - return event.control_params.dribbler_mode != TbotsProto::DribblerMode::RELEASE_BALL_SLOW && robotReachedDestination(event.common.robot, event.control_params.destination, + return event.control_params.dribbler_mode == TbotsProto::DribblerMode::OFF && robotReachedDestination(event.common.robot, event.control_params.destination, event.control_params.final_orientation); } From 758348031474e4ecfffb7ab8962b53fcd8c67670 Mon Sep 17 00:00:00 2001 From: Mikhael Date: Mon, 8 Jul 2024 13:13:08 -0700 Subject: [PATCH 10/29] Removed a debug log --- src/software/sensor_fusion/sensor_fusion.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/software/sensor_fusion/sensor_fusion.cpp b/src/software/sensor_fusion/sensor_fusion.cpp index d495953256..778de57e3d 100644 --- a/src/software/sensor_fusion/sensor_fusion.cpp +++ b/src/software/sensor_fusion/sensor_fusion.cpp @@ -259,7 +259,6 @@ void SensorFusion::updateWorld(const SSLProto::SSL_DetectionFrame &ssl_detection { friendly_robot_id_with_ball_in_dribbler = std::nullopt; ball_in_dribbler_timeout = 0; - LOG(DEBUG) << "ball_in_dribbler_timeout: " << ball_in_dribbler_timeout; } if (shouldTrustRobotStatus()) From 74dbdb394eff1c908a1a3a9be7b3e84dff441c23 Mon Sep 17 00:00:00 2001 From: Mikhael Date: Mon, 8 Jul 2024 15:01:59 -0700 Subject: [PATCH 11/29] Fixed issue where the robot wouldn't exist the wall align state where a wall pickoff is no longer needed --- .../ai/hl/stp/play/ball_placement/ball_placement_play_fsm.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 87e934e749..697b0bf23f 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 @@ -231,9 +231,9 @@ struct BallPlacementPlayFSM return make_transition_table( // src_state + event [guard] / action = dest_state *StartState_S + Update_E[!shouldPickOffWall_G] / alignPlacement_A = - AlignPlacementState_S, + AlignPlacementState_S, StartState_S + Update_E[shouldPickOffWall_G] = AlignWallState_S, - AlignWallState_S + Update_E[!wallAlignDone_G] / alignWall_A = AlignWallState_S, + AlignWallState_S + Update_E[!wallAlignDone_G && shouldPickOffWall_G] / alignWall_A = AlignWallState_S, AlignWallState_S + Update_E[wallAlignDone_G] / setPickOffDest_A = PickOffWallState_S, AlignWallState_S + Update_E[!shouldPickOffWall_G] = AlignPlacementState_S, PickOffWallState_S + Update_E[!wallPickOffDone_G] / pickOffWall_A = PickOffWallState_S, @@ -244,7 +244,7 @@ struct BallPlacementPlayFSM ReleaseWallState_S + Update_E[ballReleased_G] = AlignPlacementState_S, AlignPlacementState_S + Update_E[shouldPickOffWall_G] = AlignWallState_S, AlignPlacementState_S + Update_E[!alignDone_G] / alignPlacement_A = - AlignPlacementState_S, + AlignPlacementState_S, AlignPlacementState_S + Update_E[alignDone_G] = PlaceBallState_S, PlaceBallState_S + Update_E[!ballPlaced_G] / placeBall_A = PlaceBallState_S, PlaceBallState_S + Update_E[ballPlaced_G] / startWait_A = WaitState_S, From ca99bc94321ae43f39fa8859b73a9e65628d0aa9 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci-lite[bot]" <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> Date: Mon, 8 Jul 2024 22:20:19 +0000 Subject: [PATCH 12/29] [pre-commit.ci lite] apply automatic fixes --- docs/fsm-diagrams.md | 22 ++- src/proto/parameters.proto | 7 +- src/proto/primitive.proto | 6 +- src/shared/2021_robot_constants.cpp | 4 +- src/shared/robot_constants.h | 3 +- .../ball_placement_play_fsm.cpp | 157 ++++++++++-------- .../ball_placement/ball_placement_play_fsm.h | 45 +++-- .../ai/hl/stp/tactic/dribble/dribble_fsm.h | 6 +- .../hl/stp/tactic/dribble/dribble_tactic.cpp | 25 +-- .../ai/hl/stp/tactic/dribble/dribble_tactic.h | 6 +- .../ai/hl/stp/tactic/move/move_fsm.cpp | 3 +- .../robot_navigation_obstacle_factory.cpp | 4 +- .../field_tests/ball_placement_field_test.py | 8 +- .../field_tests/movement_robot_field_test.py | 1 - src/software/jetson_nano/services/motor.cpp | 3 +- src/software/logger/logger.h | 10 +- .../thunderscope/thunderscope_config.py | 8 +- 17 files changed, 187 insertions(+), 131 deletions(-) diff --git a/docs/fsm-diagrams.md b/docs/fsm-diagrams.md index bd4286889a..8691a3de78 100644 --- a/docs/fsm-diagrams.md +++ b/docs/fsm-diagrams.md @@ -32,20 +32,28 @@ 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] +AlignWallState --> AlignWallState : [!wallAlignDone && shouldPickOffWall]\nalignWall +AlignWallState --> PickOffWallState : [wallAlignDone]\nsetPickOffDest +AlignWallState --> AlignPlacementState : [!shouldPickOffWall] +PickOffWallState --> PickOffWallState : [!wallPickOffDone]\npickOffWall +PickOffWallState --> PickOffWaitState : [wallPickOffDone]\nstartWait +PickOffWaitState --> PickOffWaitState : [!waitDone]\nwait +PickOffWaitState --> ReleaseWallState : [waitDone] +ReleaseWallState --> ReleaseWallState : [!ballReleased]\nreleaseBall +ReleaseWallState --> AlignPlacementState : [ballReleased] +AlignPlacementState --> AlignWallState : [shouldPickOffWall] AlignPlacementState --> AlignPlacementState : [!alignDone]\nalignPlacement AlignPlacementState --> PlaceBallState : [alignDone] PlaceBallState --> PlaceBallState : [!ballPlaced]\nplaceBall PlaceBallState --> WaitState : [ballPlaced]\nstartWait -WaitState --> WaitState : [!waitDone] +WaitState --> WaitState : [!waitDone && ballPlaced]\nwait +WaitState --> StartState : [!ballPlaced] WaitState --> RetreatState : [waitDone] RetreatState --> Terminate:::terminate : [retreatDone && ballPlaced] RetreatState --> RetreatState : [ballPlaced]\nretreat +RetreatState --> StartState : [!ballPlaced] ``` diff --git a/src/proto/parameters.proto b/src/proto/parameters.proto index 7ea2bb4ddb..be76c77228 100644 --- a/src/proto/parameters.proto +++ b/src/proto/parameters.proto @@ -394,8 +394,11 @@ message RobotNavigationObstacleConfig [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]; + 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 40c9e21506..b64b08e005 100644 --- a/src/proto/primitive.proto +++ b/src/proto/primitive.proto @@ -54,9 +54,9 @@ enum MotionConstraint enum DribblerMode { - OFF = 0; - INDEFINITE = 1; - MAX_FORCE = 2; + OFF = 0; + INDEFINITE = 1; + MAX_FORCE = 2; RELEASE_BALL_SLOW = 3; } diff --git a/src/shared/2021_robot_constants.cpp b/src/shared/2021_robot_constants.cpp index 81f5866f18..0bce47a56f 100644 --- a/src/shared/2021_robot_constants.cpp +++ b/src/shared/2021_robot_constants.cpp @@ -18,8 +18,8 @@ RobotConstants_t create2021RobotConstants(void) // TODO (#2112): update this .dribbler_width_meters = 0.088f, // Dribbler speeds are negative as that is the direction that sucks the ball in - .indefinite_dribbler_speed_rpm = -10000.0f, - .max_force_dribbler_speed_rpm = -12000.0f, + .indefinite_dribbler_speed_rpm = -10000.0f, + .max_force_dribbler_speed_rpm = -12000.0f, .release_ball_dribbler_speed_rpm = 2000.0f, // Motor constant diff --git a/src/shared/robot_constants.h b/src/shared/robot_constants.h index b54f440e6e..7e4c0006aa 100644 --- a/src/shared/robot_constants.h +++ b/src/shared/robot_constants.h @@ -73,7 +73,8 @@ typedef struct RobotConstants // the ball [rpm] float max_force_dribbler_speed_rpm; - // Speed at which to spin the dribbler to release the ball right in front of a stationary robot [rpm] + // Speed at which to spin the dribbler to release the ball right in front of a + // stationary robot [rpm] float release_ball_dribbler_speed_rpm; // The maximum acceleration achievable by our motors [m/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 ca5e658809..436cd648f4 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 @@ -12,7 +12,8 @@ BallPlacementPlayFSM::BallPlacementPlayFSM(TbotsProto::AiConfig ai_config) { } -void BallPlacementPlayFSM::alignWall(const Update &event) { +void BallPlacementPlayFSM::alignWall(const Update &event) +{ PriorityTacticVector tactics_to_run = {{}}; // setup move tactics for robots away from ball placing robot @@ -21,32 +22,40 @@ void BallPlacementPlayFSM::alignWall(const Update &event) { move_tactics.end()); // setup wall kickoff tactic for ball placing robot - Point ball_pos = event.common.world_ptr->ball().position(); - Rectangle field_boundary = event.common.world_ptr->field().fieldBoundary(); - - pickoff_final_orientation = calculateWallPickOffLocation(ball_pos, field_boundary, ROBOT_MAX_RADIUS_METERS * 4).first; - pickoff_point = ball_pos - Vector::createFromAngle(pickoff_final_orientation).normalize(0.4); - align_wall_tactic->updateControlParams(pickoff_point, pickoff_final_orientation, - 0.0, TbotsProto::DribblerMode::OFF, - TbotsProto::BallCollisionType::AVOID, {AutoChipOrKickMode::OFF, 0}, - TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, - TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE, 0.0); + Point ball_pos = event.common.world_ptr->ball().position(); + Rectangle field_boundary = event.common.world_ptr->field().fieldBoundary(); + + pickoff_final_orientation = calculateWallPickOffLocation(ball_pos, field_boundary, + ROBOT_MAX_RADIUS_METERS * 4) + .first; + pickoff_point = + ball_pos - Vector::createFromAngle(pickoff_final_orientation).normalize(0.4); + align_wall_tactic->updateControlParams( + pickoff_point, pickoff_final_orientation, 0.0, TbotsProto::DribblerMode::OFF, + TbotsProto::BallCollisionType::AVOID, {AutoChipOrKickMode::OFF, 0}, + TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, + TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE, 0.0); tactics_to_run[0].emplace_back(align_wall_tactic); event.common.set_tactics(tactics_to_run); } -void BallPlacementPlayFSM::setPickOffDest(const BallPlacementPlayFSM::Update &event) { - Point ball_pos = event.common.world_ptr->ball().position(); - Rectangle field_boundary = event.common.world_ptr->field().fieldBoundary(); +void BallPlacementPlayFSM::setPickOffDest(const BallPlacementPlayFSM::Update &event) +{ + Point ball_pos = event.common.world_ptr->ball().position(); + Rectangle field_boundary = event.common.world_ptr->field().fieldBoundary(); - std::pair location = calculateWallPickOffLocation(ball_pos, field_boundary, ROBOT_MAX_RADIUS_METERS * 4); + std::pair location = calculateWallPickOffLocation( + ball_pos, field_boundary, ROBOT_MAX_RADIUS_METERS * 4); pickoff_final_orientation = location.first; - pickoff_destination = location.second - Vector::createFromAngle(pickoff_final_orientation).normalize(ROBOT_MAX_RADIUS_METERS * 5.5); + pickoff_destination = + location.second - Vector::createFromAngle(pickoff_final_orientation) + .normalize(ROBOT_MAX_RADIUS_METERS * 5.5); } -void BallPlacementPlayFSM::pickOffWall(const BallPlacementPlayFSM::Update &event) { +void BallPlacementPlayFSM::pickOffWall(const BallPlacementPlayFSM::Update &event) +{ PriorityTacticVector tactics_to_run = {{}}; // setup move tactics for robots away from ball placing robot @@ -54,14 +63,18 @@ void BallPlacementPlayFSM::pickOffWall(const BallPlacementPlayFSM::Update &event tactics_to_run[0].insert(tactics_to_run[0].end(), move_tactics.begin(), move_tactics.end()); - pickoff_wall_tactic->updateControlParams(pickoff_destination, pickoff_final_orientation, true, TbotsProto::MaxAllowedSpeedMode::BALL_PLACEMENT_WALL_DRIBBLE, TbotsProto::MaxAllowedSpeedMode::BALL_PLACEMENT_WALL_DRIBBLE); + pickoff_wall_tactic->updateControlParams( + pickoff_destination, pickoff_final_orientation, true, + TbotsProto::MaxAllowedSpeedMode::BALL_PLACEMENT_WALL_DRIBBLE, + TbotsProto::MaxAllowedSpeedMode::BALL_PLACEMENT_WALL_DRIBBLE); tactics_to_run[0].emplace_back(pickoff_wall_tactic); event.common.set_tactics(tactics_to_run); } -void BallPlacementPlayFSM::releaseBall(const BallPlacementPlayFSM::Update &event) { +void BallPlacementPlayFSM::releaseBall(const BallPlacementPlayFSM::Update &event) +{ PriorityTacticVector tactics_to_run = {{}}; // setup move tactics for robots away from ball placing robot @@ -70,20 +83,23 @@ void BallPlacementPlayFSM::releaseBall(const BallPlacementPlayFSM::Update &event move_tactics.end()); Point ball_pos = event.common.world_ptr->ball().position(); - std::optional robot_placing_ball = event.common.world_ptr->friendlyTeam().getNearestRobot(ball_pos); + std::optional robot_placing_ball = + event.common.world_ptr->friendlyTeam().getNearestRobot(ball_pos); if (!robot_placing_ball.has_value()) { return; } - Point release_point = ball_pos + (robot_placing_ball->position() - ball_pos).normalize(.5); + Point release_point = + ball_pos + (robot_placing_ball->position() - ball_pos).normalize(.5); - align_wall_tactic->updateControlParams(release_point, pickoff_final_orientation, - 0.0, TbotsProto::DribblerMode::RELEASE_BALL_SLOW, - TbotsProto::BallCollisionType::ALLOW, {AutoChipOrKickMode::OFF, 0}, - TbotsProto::MaxAllowedSpeedMode::BALL_PLACEMENT_RETREAT, - TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE, 0.0); + align_wall_tactic->updateControlParams( + release_point, pickoff_final_orientation, 0.0, + TbotsProto::DribblerMode::RELEASE_BALL_SLOW, TbotsProto::BallCollisionType::ALLOW, + {AutoChipOrKickMode::OFF, 0}, + TbotsProto::MaxAllowedSpeedMode::BALL_PLACEMENT_RETREAT, + TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE, 0.0); tactics_to_run[0].emplace_back(align_wall_tactic); event.common.set_tactics(tactics_to_run); @@ -127,7 +143,7 @@ void BallPlacementPlayFSM::alignPlacement(const Update &event) void BallPlacementPlayFSM::placeBall(const Update &event) { std::optional placement_point = - event.common.world_ptr->gameState().getBallPlacementPoint(); + event.common.world_ptr->gameState().getBallPlacementPoint(); if (!placement_point.has_value()) { @@ -142,7 +158,8 @@ void BallPlacementPlayFSM::placeBall(const Update &event) move_tactics.end()); Point ball_pos = event.common.world_ptr->ball().position(); - std::optional robot_placing_ball = event.common.world_ptr->friendlyTeam().getNearestRobot(ball_pos); + std::optional robot_placing_ball = + event.common.world_ptr->friendlyTeam().getNearestRobot(ball_pos); if (!robot_placing_ball.has_value()) { @@ -161,7 +178,8 @@ void BallPlacementPlayFSM::placeBall(const Update &event) // setup ball placement tactic for ball placing robot place_ball_tactic->updateControlParams( - event.common.world_ptr->gameState().getBallPlacementPoint(), final_angle, true, TbotsProto::MaxAllowedSpeedMode::DRIBBLE); + event.common.world_ptr->gameState().getBallPlacementPoint(), final_angle, true, + TbotsProto::MaxAllowedSpeedMode::DRIBBLE); tactics_to_run[0].emplace_back(place_ball_tactic); event.common.set_tactics(tactics_to_run); @@ -176,7 +194,7 @@ void BallPlacementPlayFSM::wait(const Update &event) { WorldPtr world_ptr = event.common.world_ptr; std::optional nearest_robot = - world_ptr->friendlyTeam().getNearestRobot(world_ptr->ball().position()); + world_ptr->friendlyTeam().getNearestRobot(world_ptr->ball().position()); if (nearest_robot.has_value()) { @@ -189,10 +207,11 @@ void BallPlacementPlayFSM::wait(const Update &event) wait_tactic->updateControlParams( - nearest_robot->position(), nearest_robot->orientation(), 0.0, TbotsProto::DribblerMode::RELEASE_BALL_SLOW, - TbotsProto::BallCollisionType::ALLOW, {AutoChipOrKickMode::OFF, 0}, - TbotsProto::MaxAllowedSpeedMode::BALL_PLACEMENT_RETREAT, - TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE, 0.0); + nearest_robot->position(), nearest_robot->orientation(), 0.0, + TbotsProto::DribblerMode::RELEASE_BALL_SLOW, + TbotsProto::BallCollisionType::ALLOW, {AutoChipOrKickMode::OFF, 0}, + TbotsProto::MaxAllowedSpeedMode::BALL_PLACEMENT_RETREAT, + TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE, 0.0); tactics_to_run[0].emplace_back(wait_tactic); event.common.set_tactics(tactics_to_run); @@ -251,13 +270,14 @@ void BallPlacementPlayFSM::retreat(const Update &event) } } -bool BallPlacementPlayFSM::shouldPickOffWall(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().fieldBoundary(); - double wiggle_room = std::abs(signedDistance(ball_pos, field_lines)); + double wiggle_room = std::abs(signedDistance(ball_pos, field_lines)); - return wiggle_room <= ROBOT_MAX_RADIUS_METERS * 4; // TODO: make this a constant + return wiggle_room <= ROBOT_MAX_RADIUS_METERS * 4; // TODO: make this a constant } bool BallPlacementPlayFSM::alignDone(const Update &event) @@ -275,10 +295,11 @@ bool BallPlacementPlayFSM::alignDone(const Update &event) } } -bool BallPlacementPlayFSM::wallAlignDone(const BallPlacementPlayFSM::Update &event) { +bool BallPlacementPlayFSM::wallAlignDone(const BallPlacementPlayFSM::Update &event) +{ std::optional nearest_robot = - event.common.world_ptr->friendlyTeam().getNearestRobot( - event.common.world_ptr->ball().position()); + event.common.world_ptr->friendlyTeam().getNearestRobot( + event.common.world_ptr->ball().position()); if (nearest_robot.has_value()) { return comparePoints(nearest_robot.value().position(), pickoff_point); @@ -289,7 +310,8 @@ bool BallPlacementPlayFSM::wallAlignDone(const BallPlacementPlayFSM::Update &eve } } -bool BallPlacementPlayFSM::wallPickOffDone(const Update &event) { +bool BallPlacementPlayFSM::wallPickOffDone(const Update &event) +{ return pickoff_wall_tactic->done(); } @@ -329,54 +351,57 @@ bool BallPlacementPlayFSM::retreatDone(const Update &event) ->position()) > RETREAT_DISTANCE_METERS; } -std::pair BallPlacementPlayFSM::calculateWallPickOffLocation(const Point &ball_pos, - const Rectangle &field_boundary, - const double max_dist) +std::pair BallPlacementPlayFSM::calculateWallPickOffLocation( + const Point &ball_pos, const Rectangle &field_boundary, const double max_dist) { Angle facing_angle; Point backoff_point; bool near_positive_y_boundary = (field_boundary.yMax() - ball_pos.y()) < max_dist; bool near_negative_y_boundary = (ball_pos.y() - field_boundary.yMin()) < max_dist; - bool near_positive_x_boundary = (field_boundary.xMax() - ball_pos.x()) < max_dist; - bool near_negative_x_boundary = (ball_pos.x() - field_boundary.xMin()) < max_dist; - if (near_positive_y_boundary && near_positive_x_boundary) // top right corner + bool near_positive_x_boundary = (field_boundary.xMax() - ball_pos.x()) < max_dist; + bool near_negative_x_boundary = (ball_pos.x() - field_boundary.xMin()) < max_dist; + if (near_positive_y_boundary && near_positive_x_boundary) // top right corner { - facing_angle = Angle::fromDegrees(45); - backoff_point = field_boundary.posXPosYCorner() - Vector::createFromAngle(facing_angle).normalize(0.9); + facing_angle = Angle::fromDegrees(45); + backoff_point = field_boundary.posXPosYCorner() - + Vector::createFromAngle(facing_angle).normalize(0.9); } - else if (near_positive_y_boundary && near_negative_x_boundary) // top left corner + else if (near_positive_y_boundary && near_negative_x_boundary) // top left corner { - facing_angle = Angle::fromDegrees(135); - backoff_point = field_boundary.negXPosYCorner() - Vector::createFromAngle(facing_angle).normalize(0.9); + facing_angle = Angle::fromDegrees(135); + backoff_point = field_boundary.negXPosYCorner() - + Vector::createFromAngle(facing_angle).normalize(0.9); } - else if (near_negative_y_boundary && near_positive_x_boundary) // bottom right corner + else if (near_negative_y_boundary && near_positive_x_boundary) // bottom right corner { - facing_angle = Angle::fromDegrees(-45); - backoff_point = field_boundary.posXNegYCorner() - Vector::createFromAngle(facing_angle).normalize(0.9); + facing_angle = Angle::fromDegrees(-45); + backoff_point = field_boundary.posXNegYCorner() - + Vector::createFromAngle(facing_angle).normalize(0.9); } - else if (near_negative_y_boundary && near_negative_x_boundary) // bottom left corner + else if (near_negative_y_boundary && near_negative_x_boundary) // bottom left corner { - facing_angle = Angle::fromDegrees(-135); - backoff_point = field_boundary.negXNegYCorner() - Vector::createFromAngle(facing_angle).normalize(0.9); + facing_angle = Angle::fromDegrees(-135); + backoff_point = field_boundary.negXNegYCorner() - + Vector::createFromAngle(facing_angle).normalize(0.9); } else if (near_positive_y_boundary) { - facing_angle = Angle::fromDegrees(90); + facing_angle = Angle::fromDegrees(90); backoff_point = Point(ball_pos.x(), field_boundary.yMax()); } else if (near_positive_x_boundary) { - facing_angle = Angle::fromDegrees(0); + facing_angle = Angle::fromDegrees(0); backoff_point = Point(field_boundary.xMax(), ball_pos.y()); } else if (near_negative_y_boundary) { - facing_angle = Angle::fromDegrees(-90); + facing_angle = Angle::fromDegrees(-90); backoff_point = Point(ball_pos.x(), field_boundary.yMin()); } else if (near_negative_x_boundary) { - facing_angle = Angle::fromDegrees(180); + facing_angle = Angle::fromDegrees(180); backoff_point = Point(field_boundary.xMin(), ball_pos.y()); } return std::make_pair(facing_angle, backoff_point); @@ -420,9 +445,11 @@ void BallPlacementPlayFSM::setupMoveTactics(const Update &event) } } -bool BallPlacementPlayFSM::ballReleased(const BallPlacementPlayFSM::Update &event) { +bool BallPlacementPlayFSM::ballReleased(const BallPlacementPlayFSM::Update &event) +{ Point ball_pos = event.common.world_ptr->ball().position(); - std::optional robot_placing_ball = event.common.world_ptr->friendlyTeam().getNearestRobot(ball_pos); + std::optional robot_placing_ball = + event.common.world_ptr->friendlyTeam().getNearestRobot(ball_pos); if (!robot_placing_ball.has_value()) { @@ -431,5 +458,3 @@ bool BallPlacementPlayFSM::ballReleased(const BallPlacementPlayFSM::Update &even return distance(ball_pos, robot_placing_ball->position()) > 0.4; } - - 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 697b0bf23f..982bb60704 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 @@ -6,8 +6,8 @@ #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" #include "software/ai/hl/stp/tactic/stop/stop_tactic.h" +#include "software/ai/passing/eighteen_zone_pitch_division.h" #include "software/geom/algorithms/closest_point.h" using Zones = std::unordered_set; @@ -45,7 +45,8 @@ struct BallPlacementPlayFSM void alignWall(const Update& event); /** - * The transition action into picking the ball off the wall, set the target destination. + * The transition action into picking the ball off the wall, set the target + * destination. * * @param event the BallPlacementPlayFSM Update event */ @@ -67,7 +68,6 @@ struct BallPlacementPlayFSM - /** * Action that finds a point where the ball's current position aligns with the ball * placement position, and moves the robot so that it is aligned to dribble straight @@ -107,7 +107,8 @@ struct BallPlacementPlayFSM void retreat(const Update& event); /** - * Guard on whether the ball is close enough to the wall that the robot cannot safely fit behind it + * 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 * @@ -177,14 +178,17 @@ struct BallPlacementPlayFSM /** - * Helper function for calculating the angle at which the robot must face towards to pick up ball + * Helper function for calculating the angle at which the robot must face towards to + * pick up ball * * @param ball_pos the ball position to use when calculating the kick angle * @param field_lines the field lines of the playing area * * @return the kick angle */ - std::pair calculateWallPickOffLocation(const Point& ball_pos, const Rectangle& field_lines, double max_dist); + std::pair calculateWallPickOffLocation(const Point& ball_pos, + const Rectangle& field_lines, + double max_dist); /** * Helper function that populates the move_tactics field with MoveTactics that @@ -231,23 +235,28 @@ struct BallPlacementPlayFSM return make_transition_table( // src_state + event [guard] / action = dest_state *StartState_S + Update_E[!shouldPickOffWall_G] / alignPlacement_A = - AlignPlacementState_S, + AlignPlacementState_S, StartState_S + Update_E[shouldPickOffWall_G] = AlignWallState_S, - AlignWallState_S + Update_E[!wallAlignDone_G && shouldPickOffWall_G] / alignWall_A = AlignWallState_S, - AlignWallState_S + Update_E[wallAlignDone_G] / setPickOffDest_A = PickOffWallState_S, - AlignWallState_S + Update_E[!shouldPickOffWall_G] = AlignPlacementState_S, - PickOffWallState_S + Update_E[!wallPickOffDone_G] / pickOffWall_A = PickOffWallState_S, - PickOffWallState_S + Update_E[wallPickOffDone_G] / startWait_A = PickOffWaitState_S, + AlignWallState_S + Update_E[!wallAlignDone_G && shouldPickOffWall_G] / + alignWall_A = AlignWallState_S, + AlignWallState_S + Update_E[wallAlignDone_G] / setPickOffDest_A = + PickOffWallState_S, + AlignWallState_S + Update_E[!shouldPickOffWall_G] = AlignPlacementState_S, + PickOffWallState_S + Update_E[!wallPickOffDone_G] / pickOffWall_A = + PickOffWallState_S, + PickOffWallState_S + Update_E[wallPickOffDone_G] / startWait_A = + PickOffWaitState_S, PickOffWaitState_S + Update_E[!waitDone_G] / wait_A = PickOffWaitState_S, PickOffWaitState_S + Update_E[waitDone_G] = ReleaseWallState_S, - ReleaseWallState_S + Update_E[!ballReleased_G] / releaseBall_A = ReleaseWallState_S, - ReleaseWallState_S + Update_E[ballReleased_G] = AlignPlacementState_S, + ReleaseWallState_S + Update_E[!ballReleased_G] / releaseBall_A = + ReleaseWallState_S, + ReleaseWallState_S + Update_E[ballReleased_G] = AlignPlacementState_S, AlignPlacementState_S + Update_E[shouldPickOffWall_G] = AlignWallState_S, AlignPlacementState_S + Update_E[!alignDone_G] / alignPlacement_A = - AlignPlacementState_S, - AlignPlacementState_S + Update_E[alignDone_G] = PlaceBallState_S, - PlaceBallState_S + Update_E[!ballPlaced_G] / placeBall_A = PlaceBallState_S, - PlaceBallState_S + Update_E[ballPlaced_G] / startWait_A = WaitState_S, + AlignPlacementState_S, + AlignPlacementState_S + Update_E[alignDone_G] = 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 && ballPlaced_G] / wait_A = WaitState_S, WaitState_S + Update_E[!ballPlaced_G] = StartState_S, WaitState_S + Update_E[waitDone_G] = RetreatState_S, 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 0d3dea5cb7..fe368ed3bc 100644 --- a/src/software/ai/hl/stp/tactic/dribble/dribble_fsm.h +++ b/src/software/ai/hl/stp/tactic/dribble/dribble_fsm.h @@ -37,8 +37,10 @@ struct DribbleFSM // 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; + TbotsProto::MaxAllowedSpeedMode max_speed_dribble = + TbotsProto::MaxAllowedSpeedMode::DRIBBLE; + TbotsProto::MaxAllowedSpeedMode max_speed_get_possession = + TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT; }; DEFINE_TACTIC_UPDATE_STRUCT_WITH_CONTROL_AND_COMMON_PARAMS 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 cfb83fa325..8658fa1730 100644 --- a/src/software/ai/hl/stp/tactic/dribble/dribble_tactic.cpp +++ b/src/software/ai/hl/stp/tactic/dribble/dribble_tactic.cpp @@ -5,11 +5,12 @@ DribbleTactic::DribbleTactic(TbotsProto::AiConfig ai_config) : Tactic({RobotCapability::Move, RobotCapability::Dribble, RobotCapability::Kick}), fsm_map(), - control_params{DribbleFSM::ControlParams{.dribble_destination = std::nullopt, - .final_dribble_orientation = std::nullopt, - .allow_excessive_dribbling = false, - .max_speed_dribble = TbotsProto::MaxAllowedSpeedMode::DRIBBLE, - .max_speed_get_possession = TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT}}, + control_params{DribbleFSM::ControlParams{ + .dribble_destination = std::nullopt, + .final_dribble_orientation = std::nullopt, + .allow_excessive_dribbling = false, + .max_speed_dribble = TbotsProto::MaxAllowedSpeedMode::DRIBBLE, + .max_speed_get_possession = TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT}}, ai_config(ai_config) { for (RobotId id = 0; id < MAX_ROBOT_IDS; id++) @@ -19,17 +20,17 @@ DribbleTactic::DribbleTactic(TbotsProto::AiConfig ai_config) } } -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) +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; + 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 d821748365..6d97191265 100644 --- a/src/software/ai/hl/stp/tactic/dribble/dribble_tactic.h +++ b/src/software/ai/hl/stp/tactic/dribble/dribble_tactic.h @@ -37,8 +37,10 @@ class DribbleTactic : public Tactic void updateControlParams(std::optional dribble_destination, std::optional final_dribble_orientation, bool allow_excessive_dribbling = false, - TbotsProto::MaxAllowedSpeedMode max_speed_dribble = TbotsProto::MaxAllowedSpeedMode::DRIBBLE, - TbotsProto::MaxAllowedSpeedMode max_speed_get_possession = TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT); + TbotsProto::MaxAllowedSpeedMode max_speed_dribble = + TbotsProto::MaxAllowedSpeedMode::DRIBBLE, + TbotsProto::MaxAllowedSpeedMode max_speed_get_possession = + TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT); void accept(TacticVisitor& visitor) const override; 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 b6f4d16488..d9c0cdf3d0 100644 --- a/src/software/ai/hl/stp/tactic/move/move_fsm.cpp +++ b/src/software/ai/hl/stp/tactic/move/move_fsm.cpp @@ -15,6 +15,7 @@ void MoveFSM::updateMove(const Update &event) bool MoveFSM::moveDone(const Update &event) { - return event.control_params.dribbler_mode == TbotsProto::DribblerMode::OFF && robotReachedDestination(event.common.robot, event.control_params.destination, + return event.control_params.dribbler_mode == TbotsProto::DribblerMode::OFF && + robotReachedDestination(event.common.robot, event.control_params.destination, event.control_params.final_orientation); } 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 18670f48d0..7ff8c8f7cb 100644 --- a/src/software/ai/navigator/obstacle/robot_navigation_obstacle_factory.cpp +++ b/src/software/ai/navigator/obstacle/robot_navigation_obstacle_factory.cpp @@ -158,7 +158,9 @@ RobotNavigationObstacleFactory::createObstaclesFromMotionConstraints( ObstaclePtr RobotNavigationObstacleFactory::createFromBallPosition( const Point &ball_position) const { - return createFromShape(Circle(ball_position, BALL_MAX_RADIUS_METERS + config.additional_ball_obstacle_inflation_meters())); + return createFromShape(Circle( + ball_position, + BALL_MAX_RADIUS_METERS + config.additional_ball_obstacle_inflation_meters())); } ObstaclePtr RobotNavigationObstacleFactory::createStadiumEnemyRobotObstacle( diff --git a/src/software/field_tests/ball_placement_field_test.py b/src/software/field_tests/ball_placement_field_test.py index 52e34c4692..20918f5a38 100644 --- a/src/software/field_tests/ball_placement_field_test.py +++ b/src/software/field_tests/ball_placement_field_test.py @@ -2,7 +2,6 @@ import software.python_bindings as tbots_cpp import sys -import math from proto.ssl_gc_common_pb2 import Team from proto.ssl_gc_state_pb2 import Command from proto.import_all_protos import * @@ -31,8 +30,9 @@ def test_ball_placement(field_test_runner): ] ) - field_test_runner.send_gamecontroller_command(Command.Type.BALL_PLACEMENT, proto.ssl_gc_common_pb2.Team.BLUE, - placement_point) + 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=[[]], @@ -46,4 +46,4 @@ def test_ball_placement(field_test_runner): if __name__ == "__main__": # Run the test, -s disables all capturing and -vv increases verbosity - sys.exit(pytest.main([__file__, "-svv"])) \ No newline at end of file + sys.exit(pytest.main([__file__, "-svv"])) diff --git a/src/software/field_tests/movement_robot_field_test.py b/src/software/field_tests/movement_robot_field_test.py index fa0548f341..eae0baf5cf 100644 --- a/src/software/field_tests/movement_robot_field_test.py +++ b/src/software/field_tests/movement_robot_field_test.py @@ -150,7 +150,6 @@ def test_one_robots_square(field_test_runner): ] ) - # field_test_runner.gamecontroller.send_gc_command( # gc_command=Command.Type.FORCE_START, team=Team.BLUE # ) diff --git a/src/software/jetson_nano/services/motor.cpp b/src/software/jetson_nano/services/motor.cpp index 4caf2a8fd0..4dcd136a35 100644 --- a/src/software/jetson_nano/services/motor.cpp +++ b/src/software/jetson_nano/services/motor.cpp @@ -837,8 +837,7 @@ void MotorService::writeToDriverOrDieTrying(uint8_t motor, uint8_t address, int3 // If we get here, we have failed to write to the driver. We reset // the chip to clear any bad values we just wrote and crash so everything stops. reset_gpio_.setValue(GpioState::LOW); - CHECK(read_value == value) << "Couldn't write " << value - << " to the TMC6100" + CHECK(read_value == value) << "Couldn't write " << value << " to the TMC6100" << " on motor " << static_cast(motor) << " at address " << static_cast(address) << " received: " << read_value; diff --git a/src/software/logger/logger.h b/src/software/logger/logger.h index b1b07eef97..2968734769 100644 --- a/src/software/logger/logger.h +++ b/src/software/logger/logger.h @@ -81,11 +81,11 @@ class LoggerSingleton // arg. Note: log locations are defaulted to the bazel-out folder due to Bazel's // hermetic build principles -// // if log dir doesn't exist, create it -// if (!std::experimental::filesystem::exists(runtime_dir)) -// { -// std::experimental::filesystem::create_directories(runtime_dir); -// } + // // if log dir doesn't exist, create it + // if (!std::experimental::filesystem::exists(runtime_dir)) + // { + // std::experimental::filesystem::create_directories(runtime_dir); + // } auto csv_sink_handle = logWorker->addSink(std::make_unique(runtime_dir), &CSVSink::appendToFile); diff --git a/src/software/thunderscope/thunderscope_config.py b/src/software/thunderscope/thunderscope_config.py index a381a56b1c..aabc102e7d 100644 --- a/src/software/thunderscope/thunderscope_config.py +++ b/src/software/thunderscope/thunderscope_config.py @@ -517,7 +517,9 @@ def configure_replay_view( replay_log=blue_replay_log, visualization_buffer_size=visualization_buffer_size, extra_widgets=[ - configure_robot_view_fullsystem(proto_unix_io_map[ProtoUnixIOTypes.BLUE]) + configure_robot_view_fullsystem( + proto_unix_io_map[ProtoUnixIOTypes.BLUE] + ) ], ), ) @@ -539,7 +541,9 @@ def configure_replay_view( replay_log=yellow_replay_log, visualization_buffer_size=visualization_buffer_size, extra_widgets=[ - configure_robot_view_fullsystem(proto_unix_io_map[ProtoUnixIOTypes.YELLOW]) + configure_robot_view_fullsystem( + proto_unix_io_map[ProtoUnixIOTypes.YELLOW] + ) ], ), ) From 4d83ab8b4efa4c76bfed9d7435f4012036c45369 Mon Sep 17 00:00:00 2001 From: Mikhael Date: Mon, 8 Jul 2024 16:12:34 -0700 Subject: [PATCH 13/29] Teehee --- .../ball_placement_play_fsm_test.cpp | 91 ------------------- ...robot_navigation_obstacle_factory_test.cpp | 2 +- 2 files changed, 1 insertion(+), 92 deletions(-) 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 2997eead09..499be0929c 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 @@ -33,94 +33,3 @@ TEST(BallPlacementPlayFSMTest, test_transitions) EXPECT_TRUE(fsm.is(boost::sml::state)); } - -TEST(BallPlacementPlayFSMTest, test_kick_off_wall_transitions) -{ - 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))); - - 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); - - TbotsProto::AiConfig ai_config; - FSM fsm(BallPlacementPlayFSM{ai_config}); - - 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) {}))); - - EXPECT_TRUE(fsm.is(boost::sml::state)); - - // 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))); - - 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)); -} - -TEST(BallPlacementPlayFSMTest, test_kick_off_wall_angle) -{ - TbotsProto::AiConfig ai_config; - BallPlacementPlayFSM fsm(ai_config); - - 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); -} 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); From d1a6d4e12492b5cf70b45562b14f7c335b98c6cd Mon Sep 17 00:00:00 2001 From: Mikhael Date: Mon, 15 Jul 2024 01:47:03 -0700 Subject: [PATCH 14/29] Addressed some more notes --- .../ball_placement_play_fsm.cpp | 8 ++-- .../ball_placement/ball_placement_play_fsm.h | 13 +++++-- .../ai/hl/stp/tactic/dribble/dribble_tactic.h | 2 +- .../ai/hl/stp/tactic/move/move_fsm.cpp | 1 + .../ai/hl/stp/tactic/move/move_tactic.h | 2 +- .../ai/hl/stp/tactic/tactic_visitor.h | 8 ++-- .../motion_constraint_visitor.cpp | 4 +- .../motion_constraint_visitor.h | 4 +- src/software/field_tests/BUILD | 12 ------ .../field_tests/ball_placement_field_test.py | 8 ++-- .../field_tests/movement_robot_field_test.py | 39 +++++++------------ src/software/jetson_nano/primitive_executor.h | 2 +- src/software/jetson_nano/services/motor.cpp | 2 +- src/software/logger/logger.h | 10 ++--- 14 files changed, 50 insertions(+), 65 deletions(-) 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 14a1ea667e..150c4447a8 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 @@ -4,11 +4,11 @@ BallPlacementPlayFSM::BallPlacementPlayFSM(TbotsProto::AiConfig ai_config) : ai_config(ai_config), align_wall_tactic(std::make_shared()), pickoff_wall_tactic(std::make_shared(ai_config)), - place_ball_tactic(std::make_shared(ai_config)), + place_ball_tactic(std::make_shared(ai_config)), align_placement_tactic(std::make_shared()), retreat_tactic(std::make_shared()), wait_tactic(std::make_shared()), - move_tactics(std::vector>()) + move_tactics(std::vector>()) { } @@ -417,9 +417,9 @@ void BallPlacementPlayFSM::setupMoveTactics(const Update &event) return; } - move_tactics = std::vector>(num_move_tactics); + move_tactics = std::vector>(num_move_tactics); std::generate(move_tactics.begin(), move_tactics.end(), - [this]() { return std::make_shared(); }); + [this]() { return std::make_shared(); }); // 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 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 982bb60704..db9d31cf19 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 @@ -237,29 +237,36 @@ struct BallPlacementPlayFSM *StartState_S + Update_E[!shouldPickOffWall_G] / alignPlacement_A = AlignPlacementState_S, StartState_S + Update_E[shouldPickOffWall_G] = AlignWallState_S, + AlignWallState_S + Update_E[!wallAlignDone_G && shouldPickOffWall_G] / alignWall_A = AlignWallState_S, AlignWallState_S + Update_E[wallAlignDone_G] / setPickOffDest_A = PickOffWallState_S, AlignWallState_S + Update_E[!shouldPickOffWall_G] = AlignPlacementState_S, + PickOffWallState_S + Update_E[!wallPickOffDone_G] / pickOffWall_A = PickOffWallState_S, PickOffWallState_S + Update_E[wallPickOffDone_G] / startWait_A = PickOffWaitState_S, PickOffWaitState_S + Update_E[!waitDone_G] / wait_A = PickOffWaitState_S, PickOffWaitState_S + Update_E[waitDone_G] = ReleaseWallState_S, + ReleaseWallState_S + Update_E[!ballReleased_G] / releaseBall_A = ReleaseWallState_S, ReleaseWallState_S + Update_E[ballReleased_G] = AlignPlacementState_S, + AlignPlacementState_S + Update_E[shouldPickOffWall_G] = AlignWallState_S, AlignPlacementState_S + Update_E[!alignDone_G] / alignPlacement_A = AlignPlacementState_S, AlignPlacementState_S + Update_E[alignDone_G] = 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 && ballPlaced_G] / wait_A = WaitState_S, WaitState_S + Update_E[!ballPlaced_G] = StartState_S, WaitState_S + Update_E[waitDone_G] = 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] = StartState_S); @@ -268,12 +275,12 @@ struct BallPlacementPlayFSM private: 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 pickoff_wall_tactic; + std::shared_ptr place_ball_tactic; std::shared_ptr align_placement_tactic; std::shared_ptr retreat_tactic; std::shared_ptr wait_tactic; - std::vector> move_tactics; + std::vector> move_tactics; Point setup_point; Point pickoff_point; Point pickoff_destination; 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 6d97191265..d0fb7bef47 100644 --- a/src/software/ai/hl/stp/tactic/dribble/dribble_tactic.h +++ b/src/software/ai/hl/stp/tactic/dribble/dribble_tactic.h @@ -54,4 +54,4 @@ class DribbleTactic : public Tactic TbotsProto::AiConfig ai_config; }; -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 d9c0cdf3d0..f4a34ba91b 100644 --- a/src/software/ai/hl/stp/tactic/move/move_fsm.cpp +++ b/src/software/ai/hl/stp/tactic/move/move_fsm.cpp @@ -15,6 +15,7 @@ void MoveFSM::updateMove(const Update &event) bool MoveFSM::moveDone(const Update &event) { + // only finish moving if not dribbling. Sometimes when dribbling we just want to hold the ball somewhere. return event.control_params.dribbler_mode == TbotsProto::DribblerMode::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_tactic.h b/src/software/ai/hl/stp/tactic/move/move_tactic.h index 7232304473..6f9185521a 100644 --- a/src/software/ai/hl/stp/tactic/move/move_tactic.h +++ b/src/software/ai/hl/stp/tactic/move/move_tactic.h @@ -75,5 +75,5 @@ class MoveTactic : public Tactic 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/tactic_visitor.h b/src/software/ai/hl/stp/tactic/tactic_visitor.h index 5ef7fc69a1..e13fae1580 100644 --- a/src/software/ai/hl/stp/tactic/tactic_visitor.h +++ b/src/software/ai/hl/stp/tactic/tactic_visitor.h @@ -27,8 +27,8 @@ class StopTactic; class StopTestTactic; class MoveGoalieToGoalLineTactic; class PrepareKickoffMoveTactic; -class PlaceBallTactic; -class PlaceBallMoveTactic; +class BallPlacementDribbleTactic; +class BallPlacementMoveTactic; class WallKickoffTactic; class AvoidInterferenceTactic; @@ -69,8 +69,8 @@ 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 BallPlacementDribbleTactic &tactic) = 0; + virtual void visit(const BallPlacementMoveTactic &tactic) = 0; virtual void visit(const WallKickoffTactic &tactic) = 0; virtual void visit(const AvoidInterferenceTactic &tactic) = 0; }; diff --git a/src/software/ai/motion_constraint/motion_constraint_visitor.cpp b/src/software/ai/motion_constraint/motion_constraint_visitor.cpp index 2e72672309..13c8e1bb5b 100644 --- a/src/software/ai/motion_constraint/motion_constraint_visitor.cpp +++ b/src/software/ai/motion_constraint/motion_constraint_visitor.cpp @@ -87,12 +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) +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 5aba45c2b5..1cb78a71e8 100644 --- a/src/software/ai/motion_constraint/motion_constraint_visitor.h +++ b/src/software/ai/motion_constraint/motion_constraint_visitor.h @@ -40,8 +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 BallPlacementDribbleTactic &tactic) override; + void visit(const BallPlacementMoveTactic &tactic) override; void visit(const WallKickoffTactic &tactic) override; void visit(const AvoidInterferenceTactic &tactic) override; void visit(const PassDefenderTactic &tactic) override; diff --git a/src/software/field_tests/BUILD b/src/software/field_tests/BUILD index 3d88f9e756..c90e49d527 100644 --- a/src/software/field_tests/BUILD +++ b/src/software/field_tests/BUILD @@ -33,10 +33,6 @@ py_test( srcs = [ "movement_robot_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", @@ -51,10 +47,6 @@ 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", @@ -69,10 +61,6 @@ py_test( srcs = [ "ball_placement_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", diff --git a/src/software/field_tests/ball_placement_field_test.py b/src/software/field_tests/ball_placement_field_test.py index 20918f5a38..44171dcff5 100644 --- a/src/software/field_tests/ball_placement_field_test.py +++ b/src/software/field_tests/ball_placement_field_test.py @@ -14,10 +14,8 @@ logger = createLogger(__name__) - +# this field test will start a ball placement referee command to the placement_point. def test_ball_placement(field_test_runner): - # robot ID - id = 2 # point to place ball placement_point = tbots_cpp.Point(0, 0) @@ -43,7 +41,7 @@ def test_ball_placement(field_test_runner): 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__, "-svv"])) + sys.exit(pytest_main(__file__)) diff --git a/src/software/field_tests/movement_robot_field_test.py b/src/software/field_tests/movement_robot_field_test.py index eae0baf5cf..aaa6872a59 100644 --- a/src/software/field_tests/movement_robot_field_test.py +++ b/src/software/field_tests/movement_robot_field_test.py @@ -1,7 +1,6 @@ import pytest import software.python_bindings as tbots_cpp -import sys from proto.ssl_gc_common_pb2 import Team from proto.import_all_protos import * from software.field_tests.field_test_fixture import * @@ -134,8 +133,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) @@ -150,17 +147,13 @@ def test_one_robots_square(field_test_runner): ] ) - # field_test_runner.gamecontroller.send_gc_command( - # gc_command=Command.Type.FORCE_START, team=Team.BLUE - # ) - id = world.friendly_team.team_robots[0].id print(f"Running test on robot {id}") - point1 = Point(x_meters=-0.75, y_meters=0.6) - point2 = Point(x_meters=-0.75, y_meters=-0.6) - point3 = Point(x_meters=0.75, y_meters=-0.6) - point4 = Point(x_meters=0.75, y_meters=0.6) + point1 = Point(x_meters=-0.3, y_meters=0.6) + point2 = Point(x_meters=-0.3, y_meters=-0.6) + point3 = Point(x_meters=-1.5, y_meters=-0.6) + point4 = Point(x_meters=-1.5, y_meters=0.6) tactic_0 = MoveTactic( destination=point1, @@ -208,18 +201,17 @@ def test_one_robots_square(field_test_runner): ) tactics = [tactic_0, tactic_1, tactic_2, tactic_3] - for i in range(3): - for tactic in tactics: - print(f"Going to {tactic.destination}") - params = AssignedTacticPlayControlParams() - params.assigned_tactics[id].move.CopyFrom(tactic) + for tactic in tactics: + print(f"Going to {tactic.destination}") + params = AssignedTacticPlayControlParams() + params.assigned_tactics[id].move.CopyFrom(tactic) - field_test_runner.set_tactics(params, True) - field_test_runner.run_test( - always_validation_sequence_set=[[]], - eventually_validation_sequence_set=[[]], - test_timeout_s=4, - ) + field_test_runner.set_tactics(params, True) + field_test_runner.run_test( + always_validation_sequence_set=[[]], + eventually_validation_sequence_set=[[]], + test_timeout_s=4, + ) # Send a stop tactic after the test finishes stop_tactic = StopTactic() @@ -228,5 +220,4 @@ def test_one_robots_square(field_test_runner): if __name__ == "__main__": - # Run the test, -s disables all capturing at -vv increases verbosity - sys.exit(pytest.main([__file__, "-svv"])) + pytest_main(__file__) \ No newline at end of file diff --git a/src/software/jetson_nano/primitive_executor.h b/src/software/jetson_nano/primitive_executor.h index f08773c6a4..c95b4638f2 100644 --- a/src/software/jetson_nano/primitive_executor.h +++ b/src/software/jetson_nano/primitive_executor.h @@ -90,7 +90,7 @@ class PrimitiveExecutor RobotId robot_id_; // Estimated delay between a vision frame to AI processing to robot executing - static constexpr double VISION_TO_ROBOT_DELAY_S = 0.06; + static constexpr double VISION_TO_ROBOT_DELAY_S = 0.03; // The distance away from the destination at which we start dampening the velocity // to avoid jittering around the destination. diff --git a/src/software/jetson_nano/services/motor.cpp b/src/software/jetson_nano/services/motor.cpp index 4dcd136a35..878c59edec 100644 --- a/src/software/jetson_nano/services/motor.cpp +++ b/src/software/jetson_nano/services/motor.cpp @@ -838,7 +838,7 @@ void MotorService::writeToDriverOrDieTrying(uint8_t motor, uint8_t address, int3 // the chip to clear any bad values we just wrote and crash so everything stops. reset_gpio_.setValue(GpioState::LOW); CHECK(read_value == value) << "Couldn't write " << value << " to the TMC6100" - << " on motor " << static_cast(motor) + << " on motor " << MOTOR_NAMES[motor] << " at address " << static_cast(address) << " received: " << read_value; } diff --git a/src/software/logger/logger.h b/src/software/logger/logger.h index 2968734769..36cb2d0265 100644 --- a/src/software/logger/logger.h +++ b/src/software/logger/logger.h @@ -3,7 +3,7 @@ #include #include -//#include +#include #include #include #include @@ -82,10 +82,10 @@ class LoggerSingleton // hermetic build principles // // if log dir doesn't exist, create it - // if (!std::experimental::filesystem::exists(runtime_dir)) - // { - // std::experimental::filesystem::create_directories(runtime_dir); - // } + if (!std::experimental::filesystem::exists(runtime_dir)) + { + std::experimental::filesystem::create_directories(runtime_dir); + } auto csv_sink_handle = logWorker->addSink(std::make_unique(runtime_dir), &CSVSink::appendToFile); From 4644b4d24362d5719966cf6109564bed6ec043cd Mon Sep 17 00:00:00 2001 From: Mikhael Date: Mon, 15 Jul 2024 02:37:33 -0700 Subject: [PATCH 15/29] Added constants --- .../ball_placement_play_fsm.cpp | 20 ++-- .../ball_placement/ball_placement_play_fsm.h | 4 + .../ball_placement_play_fsm_test.cpp | 113 ++++++++++++++++++ 3 files changed, 127 insertions(+), 10 deletions(-) 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 150c4447a8..56ebf6fc6a 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,9 +3,9 @@ BallPlacementPlayFSM::BallPlacementPlayFSM(TbotsProto::AiConfig ai_config) : ai_config(ai_config), align_wall_tactic(std::make_shared()), - pickoff_wall_tactic(std::make_shared(ai_config)), + pickoff_wall_tactic(std::make_shared(ai_config)), place_ball_tactic(std::make_shared(ai_config)), - align_placement_tactic(std::make_shared()), + align_placement_tactic(std::make_shared()), retreat_tactic(std::make_shared()), wait_tactic(std::make_shared()), move_tactics(std::vector>()) @@ -26,7 +26,7 @@ void BallPlacementPlayFSM::alignWall(const Update &event) Rectangle field_boundary = event.common.world_ptr->field().fieldBoundary(); pickoff_final_orientation = calculateWallPickOffLocation(ball_pos, field_boundary, - ROBOT_MAX_RADIUS_METERS * 4) + MINIMUM_DISTANCE_FROM_WALL_FOR_ALIGN_METERS) .first; pickoff_point = ball_pos - Vector::createFromAngle(pickoff_final_orientation).normalize(0.4); @@ -46,12 +46,12 @@ void BallPlacementPlayFSM::setPickOffDest(const BallPlacementPlayFSM::Update &ev Rectangle field_boundary = event.common.world_ptr->field().fieldBoundary(); std::pair location = calculateWallPickOffLocation( - ball_pos, field_boundary, ROBOT_MAX_RADIUS_METERS * 4); + ball_pos, field_boundary, MINIMUM_DISTANCE_FROM_WALL_FOR_ALIGN_METERS); pickoff_final_orientation = location.first; pickoff_destination = location.second - Vector::createFromAngle(pickoff_final_orientation) - .normalize(ROBOT_MAX_RADIUS_METERS * 5.5); + .normalize(BACK_AWAY_FROM_WALL_M); } void BallPlacementPlayFSM::pickOffWall(const BallPlacementPlayFSM::Update &event) @@ -277,7 +277,7 @@ bool BallPlacementPlayFSM::shouldPickOffWall(const Update &event) Rectangle field_lines = event.common.world_ptr->field().fieldBoundary(); double wiggle_room = std::abs(signedDistance(ball_pos, field_lines)); - return wiggle_room <= ROBOT_MAX_RADIUS_METERS * 4; // TODO: make this a constant + return wiggle_room <= MINIMUM_DISTANCE_FROM_WALL_FOR_ALIGN_METERS; } bool BallPlacementPlayFSM::alignDone(const Update &event) @@ -364,25 +364,25 @@ std::pair BallPlacementPlayFSM::calculateWallPickOffLocation( { facing_angle = Angle::fromDegrees(45); backoff_point = field_boundary.posXPosYCorner() - - Vector::createFromAngle(facing_angle).normalize(0.9); + Vector::createFromAngle(facing_angle).normalize(BACK_AWAY_FROM_CORNER_EXTRA_M); } else if (near_positive_y_boundary && near_negative_x_boundary) // top left corner { facing_angle = Angle::fromDegrees(135); backoff_point = field_boundary.negXPosYCorner() - - Vector::createFromAngle(facing_angle).normalize(0.9); + Vector::createFromAngle(facing_angle).normalize(BACK_AWAY_FROM_CORNER_EXTRA_M); } else if (near_negative_y_boundary && near_positive_x_boundary) // bottom right corner { facing_angle = Angle::fromDegrees(-45); backoff_point = field_boundary.posXNegYCorner() - - Vector::createFromAngle(facing_angle).normalize(0.9); + Vector::createFromAngle(facing_angle).normalize(BACK_AWAY_FROM_CORNER_EXTRA_M); } else if (near_negative_y_boundary && near_negative_x_boundary) // bottom left corner { facing_angle = Angle::fromDegrees(-135); backoff_point = field_boundary.negXNegYCorner() - - Vector::createFromAngle(facing_angle).normalize(0.9); + Vector::createFromAngle(facing_angle).normalize(BACK_AWAY_FROM_CORNER_EXTRA_M); } else if (near_positive_y_boundary) { 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 db9d31cf19..41431f0317 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 @@ -14,6 +14,10 @@ using Zones = std::unordered_set; struct BallPlacementPlayFSM { + double BACK_AWAY_FROM_CORNER_EXTRA_M = 0.9; + double BACK_AWAY_FROM_WALL_M = ROBOT_MAX_RADIUS_METERS * 5.5; + double MINIMUM_DISTANCE_FROM_WALL_FOR_ALIGN_METERS = ROBOT_MAX_RADIUS_METERS * 4.0; + class StartState; class PickOffWallState; class PickOffWaitState; 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 499be0929c..af6c74273f 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 @@ -33,3 +33,116 @@ TEST(BallPlacementPlayFSMTest, test_transitions) EXPECT_TRUE(fsm.is(boost::sml::state)); } + +TEST(BallPlacementPlayFSMTest, test_pick_off_wall_transitions) +{ + 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 try to align for a wall pick off + world_ptr->updateBall( + Ball(Point(0, 3.2), 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); + + TbotsProto::AiConfig ai_config; + FSM fsm(BallPlacementPlayFSM{ai_config}); + + 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) {}))); + + EXPECT_TRUE(fsm.is(boost::sml::state)); + + ::TestUtil::setFriendlyRobotPositions(world_ptr, {Point(0, 2.8)}, 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)); + + // After the robot has completed the pickoff, we expect it to be done waiting + world_ptr->updateBall(Ball(Point(-2.0, 0), Vector(0, 0), Timestamp::fromSeconds(0))); + ::TestUtil::setFriendlyRobotPositions(world_ptr, {Point(0, 0)}, 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)); + + // After the robot is done waiting, we want to be in align placement + + world_ptr->updateTimestamp(Timestamp::fromSeconds(4)); + + 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)); +} + +// TEST(BallPlacementPlayFSMTest, test_pick_off_wall_angle_position) +// { +// TbotsProto::AiConfig ai_config; +// BallPlacementPlayFSM fsm(ai_config); +// +// 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); +//} From 45c98d5eb3d135acfcbc615f6fef7866baef1a39 Mon Sep 17 00:00:00 2001 From: Mikhael Date: Mon, 15 Jul 2024 02:49:33 -0700 Subject: [PATCH 16/29] Changed constant names --- .../message_translation/tbots_protobuf.cpp | 6 +- src/shared/2021_robot_constants.cpp | 3 + src/shared/constants.h | 6 - src/shared/robot_constants.h | 11 ++ .../ball_placement_play_fsm_test.cpp | 115 +----------------- 5 files changed, 18 insertions(+), 123 deletions(-) diff --git a/src/proto/message_translation/tbots_protobuf.cpp b/src/proto/message_translation/tbots_protobuf.cpp index 55d69582fe..3e0da3317e 100644 --- a/src/proto/message_translation/tbots_protobuf.cpp +++ b/src/proto/message_translation/tbots_protobuf.cpp @@ -512,11 +512,11 @@ double convertMaxAllowedSpeedModeToMaxAllowedSpeed( case TbotsProto::MaxAllowedSpeedMode::COLLISIONS_ALLOWED: return COLLISION_ALLOWED_ROBOT_MAX_SPEED_METERS_PER_SECOND; case TbotsProto::MaxAllowedSpeedMode::BALL_PLACEMENT_RETREAT: - return BALL_PLACEMENT_RETREAT_SPEED_METERS_PER_SECOND; + return robot_constants.ball_placement_retreat_max_speed_m_per_s; case TbotsProto::MaxAllowedSpeedMode::BALL_PLACEMENT_WALL_DRIBBLE: - return BALL_PLACEMENT_WALL_SPEED_METERS_PER_SECOND; + return robot_constants.ball_placement_wall_max_speed_m_per_s; case TbotsProto::MaxAllowedSpeedMode::DRIBBLE: - return DRIBBLE_SPEED_METERS_PER_SECOND; + return robot_constants.dribble_speed_m_per_s; default: LOG(WARNING) << "MaxAllowedSpeedMode is invalid" << std::endl; return 0.0; diff --git a/src/shared/2021_robot_constants.cpp b/src/shared/2021_robot_constants.cpp index 1a0e3d4041..66d6752354 100644 --- a/src/shared/2021_robot_constants.cpp +++ b/src/shared/2021_robot_constants.cpp @@ -26,6 +26,9 @@ RobotConstants_t create2021RobotConstants(void) // Robot's linear movement constants .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, diff --git a/src/shared/constants.h b/src/shared/constants.h index a4d1803198..5c6f479885 100644 --- a/src/shared/constants.h +++ b/src/shared/constants.h @@ -74,12 +74,6 @@ static const double STOP_COMMAND_ROBOT_MAX_SPEED_METERS_PER_SECOND = 1.5; // The margin by which the speed of the robot should stay below the maximum allowed speed // when the stop command is issued static const double STOP_COMMAND_SPEED_SAFETY_MARGIN_METERS_PER_SECOND = 0.2; -// The max speed at which we will retreat away from the ball after placing it -static const double BALL_PLACEMENT_RETREAT_SPEED_METERS_PER_SECOND = 0.3; -// The max speed at which we will pick the ball off the wall -static const double BALL_PLACEMENT_WALL_SPEED_METERS_PER_SECOND = 0.3; -// The max speed at which we dribble the ball -static const double DRIBBLE_SPEED_METERS_PER_SECOND = 1.5; // The max allowed speed of the robot before collisions would incur a foul static const double COLLISION_ALLOWED_ROBOT_MAX_SPEED_METERS_PER_SECOND = 0.5; // The minimum distance from the ball all robots must be when the stop command is issued diff --git a/src/shared/robot_constants.h b/src/shared/robot_constants.h index 7e4c0006aa..8496b9d68c 100644 --- a/src/shared/robot_constants.h +++ b/src/shared/robot_constants.h @@ -83,6 +83,17 @@ typedef struct RobotConstants // 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/x] + 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_test.cpp b/src/software/ai/hl/stp/play/ball_placement/ball_placement_play_fsm_test.cpp index af6c74273f..9e6c9639a3 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 @@ -32,117 +32,4 @@ TEST(BallPlacementPlayFSMTest, test_transitions) InterPlayCommunication{}, [](InterPlayCommunication comm) {}))); EXPECT_TRUE(fsm.is(boost::sml::state)); -} - -TEST(BallPlacementPlayFSMTest, test_pick_off_wall_transitions) -{ - 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 try to align for a wall pick off - world_ptr->updateBall( - Ball(Point(0, 3.2), 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); - - TbotsProto::AiConfig ai_config; - FSM fsm(BallPlacementPlayFSM{ai_config}); - - 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) {}))); - - EXPECT_TRUE(fsm.is(boost::sml::state)); - - ::TestUtil::setFriendlyRobotPositions(world_ptr, {Point(0, 2.8)}, 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)); - - // After the robot has completed the pickoff, we expect it to be done waiting - world_ptr->updateBall(Ball(Point(-2.0, 0), Vector(0, 0), Timestamp::fromSeconds(0))); - ::TestUtil::setFriendlyRobotPositions(world_ptr, {Point(0, 0)}, 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)); - - // After the robot is done waiting, we want to be in align placement - - world_ptr->updateTimestamp(Timestamp::fromSeconds(4)); - - 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)); -} - -// TEST(BallPlacementPlayFSMTest, test_pick_off_wall_angle_position) -// { -// TbotsProto::AiConfig ai_config; -// BallPlacementPlayFSM fsm(ai_config); -// -// 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); -//} +} \ No newline at end of file From 9940e3ec294d1fb470ce5eaa1636d63c293232ee Mon Sep 17 00:00:00 2001 From: Mikhael Date: Mon, 15 Jul 2024 03:23:48 -0700 Subject: [PATCH 17/29] Fixed ball placement play test failing --- .../ai/hl/stp/play/ball_placement/ball_placement_play_fsm.cpp | 2 +- .../ai/hl/stp/play/ball_placement/ball_placement_play_fsm.h | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) 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 56ebf6fc6a..5e4bfb097f 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 @@ -2,7 +2,7 @@ BallPlacementPlayFSM::BallPlacementPlayFSM(TbotsProto::AiConfig ai_config) : ai_config(ai_config), - align_wall_tactic(std::make_shared()), + align_wall_tactic(std::make_shared()), pickoff_wall_tactic(std::make_shared(ai_config)), place_ball_tactic(std::make_shared(ai_config)), align_placement_tactic(std::make_shared()), 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 41431f0317..1e744bbad8 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 @@ -278,10 +278,10 @@ struct BallPlacementPlayFSM private: TbotsProto::AiConfig ai_config; - std::shared_ptr align_wall_tactic; + 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 align_placement_tactic; std::shared_ptr retreat_tactic; std::shared_ptr wait_tactic; std::vector> move_tactics; From 3e462dd2f7beeea9970d07cdb71facc9a02c4c58 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci-lite[bot]" <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> Date: Mon, 15 Jul 2024 11:30:37 +0000 Subject: [PATCH 18/29] [pre-commit.ci lite] apply automatic fixes --- src/shared/2021_robot_constants.cpp | 10 ++++----- .../ball_placement_play_fsm.cpp | 22 ++++++++++++------- .../ball_placement/ball_placement_play_fsm.h | 14 ++++++------ .../ball_placement_play_fsm_test.cpp | 2 +- .../ai/hl/stp/tactic/move/move_fsm.cpp | 3 ++- .../ai/hl/stp/tactic/tactic_visitor.h | 4 ++-- .../field_tests/ball_placement_field_test.py | 1 + .../field_tests/movement_robot_field_test.py | 2 +- src/software/jetson_nano/services/motor.cpp | 4 ++-- 9 files changed, 35 insertions(+), 27 deletions(-) diff --git a/src/shared/2021_robot_constants.cpp b/src/shared/2021_robot_constants.cpp index 66d6752354..da6021d790 100644 --- a/src/shared/2021_robot_constants.cpp +++ b/src/shared/2021_robot_constants.cpp @@ -25,12 +25,12 @@ RobotConstants_t create2021RobotConstants(void) .motor_max_acceleration_m_per_s_2 = 4.5f, // Robot's linear movement constants - .robot_max_speed_m_per_s = 3.000f, - .ball_placement_wall_max_speed_m_per_s = 0.3f, + .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, + .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/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 5e4bfb097f..e23b2ce276 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 @@ -25,9 +25,10 @@ void BallPlacementPlayFSM::alignWall(const Update &event) Point ball_pos = event.common.world_ptr->ball().position(); Rectangle field_boundary = event.common.world_ptr->field().fieldBoundary(); - pickoff_final_orientation = calculateWallPickOffLocation(ball_pos, field_boundary, - MINIMUM_DISTANCE_FROM_WALL_FOR_ALIGN_METERS) - .first; + pickoff_final_orientation = + calculateWallPickOffLocation(ball_pos, field_boundary, + MINIMUM_DISTANCE_FROM_WALL_FOR_ALIGN_METERS) + .first; pickoff_point = ball_pos - Vector::createFromAngle(pickoff_final_orientation).normalize(0.4); align_wall_tactic->updateControlParams( @@ -364,25 +365,29 @@ std::pair BallPlacementPlayFSM::calculateWallPickOffLocation( { facing_angle = Angle::fromDegrees(45); backoff_point = field_boundary.posXPosYCorner() - - Vector::createFromAngle(facing_angle).normalize(BACK_AWAY_FROM_CORNER_EXTRA_M); + Vector::createFromAngle(facing_angle) + .normalize(BACK_AWAY_FROM_CORNER_EXTRA_M); } else if (near_positive_y_boundary && near_negative_x_boundary) // top left corner { facing_angle = Angle::fromDegrees(135); backoff_point = field_boundary.negXPosYCorner() - - Vector::createFromAngle(facing_angle).normalize(BACK_AWAY_FROM_CORNER_EXTRA_M); + Vector::createFromAngle(facing_angle) + .normalize(BACK_AWAY_FROM_CORNER_EXTRA_M); } else if (near_negative_y_boundary && near_positive_x_boundary) // bottom right corner { facing_angle = Angle::fromDegrees(-45); backoff_point = field_boundary.posXNegYCorner() - - Vector::createFromAngle(facing_angle).normalize(BACK_AWAY_FROM_CORNER_EXTRA_M); + Vector::createFromAngle(facing_angle) + .normalize(BACK_AWAY_FROM_CORNER_EXTRA_M); } else if (near_negative_y_boundary && near_negative_x_boundary) // bottom left corner { facing_angle = Angle::fromDegrees(-135); backoff_point = field_boundary.negXNegYCorner() - - Vector::createFromAngle(facing_angle).normalize(BACK_AWAY_FROM_CORNER_EXTRA_M); + Vector::createFromAngle(facing_angle) + .normalize(BACK_AWAY_FROM_CORNER_EXTRA_M); } else if (near_positive_y_boundary) { @@ -417,7 +422,8 @@ void BallPlacementPlayFSM::setupMoveTactics(const Update &event) return; } - move_tactics = std::vector>(num_move_tactics); + move_tactics = + std::vector>(num_move_tactics); std::generate(move_tactics.begin(), move_tactics.end(), [this]() { return std::make_shared(); }); 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 1e744bbad8..045be41fdd 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 @@ -14,8 +14,8 @@ using Zones = std::unordered_set; struct BallPlacementPlayFSM { - double BACK_AWAY_FROM_CORNER_EXTRA_M = 0.9; - double BACK_AWAY_FROM_WALL_M = ROBOT_MAX_RADIUS_METERS * 5.5; + double BACK_AWAY_FROM_CORNER_EXTRA_M = 0.9; + double BACK_AWAY_FROM_WALL_M = ROBOT_MAX_RADIUS_METERS * 5.5; double MINIMUM_DISTANCE_FROM_WALL_FOR_ALIGN_METERS = ROBOT_MAX_RADIUS_METERS * 4.0; class StartState; @@ -257,12 +257,12 @@ struct BallPlacementPlayFSM ReleaseWallState_S + Update_E[!ballReleased_G] / releaseBall_A = ReleaseWallState_S, - ReleaseWallState_S + Update_E[ballReleased_G] = AlignPlacementState_S, + ReleaseWallState_S + Update_E[ballReleased_G] = AlignPlacementState_S, AlignPlacementState_S + Update_E[shouldPickOffWall_G] = 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] = PlaceBallState_S, PlaceBallState_S + Update_E[!ballPlaced_G] / placeBall_A = PlaceBallState_S, PlaceBallState_S + Update_E[ballPlaced_G] / startWait_A = WaitState_S, @@ -271,9 +271,9 @@ struct BallPlacementPlayFSM WaitState_S + Update_E[!ballPlaced_G] = StartState_S, WaitState_S + Update_E[waitDone_G] = 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] = StartState_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] = StartState_S); } private: 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 9e6c9639a3..499be0929c 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 @@ -32,4 +32,4 @@ TEST(BallPlacementPlayFSMTest, test_transitions) InterPlayCommunication{}, [](InterPlayCommunication comm) {}))); EXPECT_TRUE(fsm.is(boost::sml::state)); -} \ No newline at end of file +} 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 f4a34ba91b..d84e9d863a 100644 --- a/src/software/ai/hl/stp/tactic/move/move_fsm.cpp +++ b/src/software/ai/hl/stp/tactic/move/move_fsm.cpp @@ -15,7 +15,8 @@ void MoveFSM::updateMove(const Update &event) bool MoveFSM::moveDone(const Update &event) { - // only finish moving if not dribbling. Sometimes when dribbling we just want to hold the ball somewhere. + // only finish moving if not dribbling. Sometimes when dribbling we just want to hold + // the ball somewhere. return event.control_params.dribbler_mode == TbotsProto::DribblerMode::OFF && robotReachedDestination(event.common.robot, event.control_params.destination, event.control_params.final_orientation); diff --git a/src/software/ai/hl/stp/tactic/tactic_visitor.h b/src/software/ai/hl/stp/tactic/tactic_visitor.h index e13fae1580..9d8eefa570 100644 --- a/src/software/ai/hl/stp/tactic/tactic_visitor.h +++ b/src/software/ai/hl/stp/tactic/tactic_visitor.h @@ -69,8 +69,8 @@ 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 BallPlacementDribbleTactic &tactic) = 0; - virtual void visit(const BallPlacementMoveTactic &tactic) = 0; + virtual void visit(const BallPlacementDribbleTactic &tactic) = 0; + virtual void visit(const BallPlacementMoveTactic &tactic) = 0; virtual void visit(const WallKickoffTactic &tactic) = 0; virtual void visit(const AvoidInterferenceTactic &tactic) = 0; }; diff --git a/src/software/field_tests/ball_placement_field_test.py b/src/software/field_tests/ball_placement_field_test.py index 44171dcff5..5a444faac7 100644 --- a/src/software/field_tests/ball_placement_field_test.py +++ b/src/software/field_tests/ball_placement_field_test.py @@ -41,6 +41,7 @@ def test_ball_placement(field_test_runner): params = AssignedTacticPlayControlParams() params.assigned_tactics[id].stop.CopyFrom(stop_tactic) + _ if __name__ == "__main__": # Run the test, -s disables all capturing and -vv increases verbosity diff --git a/src/software/field_tests/movement_robot_field_test.py b/src/software/field_tests/movement_robot_field_test.py index aaa6872a59..7b53649a7d 100644 --- a/src/software/field_tests/movement_robot_field_test.py +++ b/src/software/field_tests/movement_robot_field_test.py @@ -220,4 +220,4 @@ def test_one_robots_square(field_test_runner): if __name__ == "__main__": - pytest_main(__file__) \ No newline at end of file + pytest_main(__file__) diff --git a/src/software/jetson_nano/services/motor.cpp b/src/software/jetson_nano/services/motor.cpp index 878c59edec..255adb6d72 100644 --- a/src/software/jetson_nano/services/motor.cpp +++ b/src/software/jetson_nano/services/motor.cpp @@ -838,8 +838,8 @@ void MotorService::writeToDriverOrDieTrying(uint8_t motor, uint8_t address, int3 // the chip to clear any bad values we just wrote and crash so everything stops. reset_gpio_.setValue(GpioState::LOW); CHECK(read_value == value) << "Couldn't write " << value << " to the TMC6100" - << " on motor " << MOTOR_NAMES[motor] - << " at address " << static_cast(address) + << " on motor " << MOTOR_NAMES[motor] << " at address " + << static_cast(address) << " received: " << read_value; } From 7966dd19519c5c6f6188fd964b812e8e9b911062 Mon Sep 17 00:00:00 2001 From: Mikhael Date: Mon, 15 Jul 2024 06:52:52 -0700 Subject: [PATCH 19/29] cleaned up fsm logic --- .../ball_placement_play_fsm.cpp | 48 +---------------- .../ball_placement/ball_placement_play_fsm.h | 52 ++++--------------- 2 files changed, 12 insertions(+), 88 deletions(-) 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 5e4bfb097f..ec45b8e23e 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 @@ -73,38 +73,6 @@ void BallPlacementPlayFSM::pickOffWall(const BallPlacementPlayFSM::Update &event event.common.set_tactics(tactics_to_run); } -void BallPlacementPlayFSM::releaseBall(const BallPlacementPlayFSM::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()); - - 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()) - { - return; - } - - Point release_point = - ball_pos + (robot_placing_ball->position() - ball_pos).normalize(.5); - - align_wall_tactic->updateControlParams( - release_point, pickoff_final_orientation, 0.0, - TbotsProto::DribblerMode::RELEASE_BALL_SLOW, TbotsProto::BallCollisionType::ALLOW, - {AutoChipOrKickMode::OFF, 0}, - TbotsProto::MaxAllowedSpeedMode::BALL_PLACEMENT_RETREAT, - TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE, 0.0); - tactics_to_run[0].emplace_back(align_wall_tactic); - - event.common.set_tactics(tactics_to_run); -} - void BallPlacementPlayFSM::alignPlacement(const Update &event) { std::optional placement_point = @@ -190,7 +158,7 @@ void BallPlacementPlayFSM::startWait(const Update &event) start_time = event.common.world_ptr->getMostRecentTimestamp(); } -void BallPlacementPlayFSM::wait(const Update &event) +void BallPlacementPlayFSM::releaseBall(const Update &event) { WorldPtr world_ptr = event.common.world_ptr; std::optional nearest_robot = @@ -444,17 +412,3 @@ void BallPlacementPlayFSM::setupMoveTactics(const Update &event) TbotsProto::ObstacleAvoidanceMode::SAFE); } } - -bool BallPlacementPlayFSM::ballReleased(const BallPlacementPlayFSM::Update &event) -{ - 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()) - { - return false; - } - - return distance(ball_pos, robot_placing_ball->position()) > 0.4; -} 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 1e744bbad8..53d4a9a0ee 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 @@ -20,12 +20,10 @@ struct BallPlacementPlayFSM class StartState; class PickOffWallState; - class PickOffWaitState; - class ReleaseWallState; class AlignWallState; class AlignPlacementState; class PlaceBallState; - class WaitState; + class ReleaseBallState; class RetreatState; struct ControlParams @@ -63,15 +61,6 @@ struct BallPlacementPlayFSM */ void pickOffWall(const Update& event); - /** - * Action that slowly drives away from the ball after picking it off the wall - * - * @param event the BallPlacementPlayFSM Update event - */ - void releaseBall(const Update& event); - - - /** * Action that finds a point where the ball's current position aligns with the ball * placement position, and moves the robot so that it is aligned to dribble straight @@ -97,11 +86,11 @@ struct BallPlacementPlayFSM void startWait(const Update& event); /** - * Action that stops the robot while waiting + * Action that stops the robot and spins the dribbler out while waiting * * @param event the BallPlacementPlayFSM Update event */ - void wait(const Update& event); + void releaseBall(const Update& event); /** * Action that has the placing robot retreat after placing the ball @@ -146,15 +135,6 @@ struct BallPlacementPlayFSM */ bool wallPickOffDone(const Update& event); - /** - * Guard on whether the placing robot has finished moving away from the ball - * - * @param event the BallPlacementPlayFSM Update event - * - * @return whether the robot has backed off from the ball - */ - bool ballReleased(const Update& event); - /** * Guard on whether the placing robot has finished placing the ball into the desired * position @@ -168,7 +148,7 @@ struct BallPlacementPlayFSM /** * Guard on whether the robot has waited sufficiently * @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); @@ -209,11 +189,9 @@ struct BallPlacementPlayFSM DEFINE_SML_STATE(StartState) DEFINE_SML_STATE(AlignWallState) DEFINE_SML_STATE(PickOffWallState) - DEFINE_SML_STATE(PickOffWaitState) - DEFINE_SML_STATE(ReleaseWallState) DEFINE_SML_STATE(AlignPlacementState) DEFINE_SML_STATE(PlaceBallState) - DEFINE_SML_STATE(WaitState) + DEFINE_SML_STATE(ReleaseBallState) DEFINE_SML_STATE(RetreatState) DEFINE_SML_EVENT(Update) @@ -222,16 +200,14 @@ struct BallPlacementPlayFSM DEFINE_SML_ACTION(setPickOffDest) DEFINE_SML_ACTION(alignWall) DEFINE_SML_ACTION(pickOffWall) - DEFINE_SML_ACTION(releaseBall) DEFINE_SML_ACTION(startWait) DEFINE_SML_ACTION(retreat) - DEFINE_SML_ACTION(wait) + DEFINE_SML_ACTION(releaseBall) DEFINE_SML_GUARD(shouldPickOffWall) DEFINE_SML_GUARD(alignDone) DEFINE_SML_GUARD(wallAlignDone) DEFINE_SML_GUARD(wallPickOffDone) - DEFINE_SML_GUARD(ballReleased) DEFINE_SML_GUARD(ballPlaced) DEFINE_SML_GUARD(waitDone) DEFINE_SML_GUARD(retreatDone) @@ -251,13 +227,7 @@ struct BallPlacementPlayFSM PickOffWallState_S + Update_E[!wallPickOffDone_G] / pickOffWall_A = PickOffWallState_S, PickOffWallState_S + Update_E[wallPickOffDone_G] / startWait_A = - PickOffWaitState_S, - PickOffWaitState_S + Update_E[!waitDone_G] / wait_A = PickOffWaitState_S, - PickOffWaitState_S + Update_E[waitDone_G] = ReleaseWallState_S, - - ReleaseWallState_S + Update_E[!ballReleased_G] / releaseBall_A = - ReleaseWallState_S, - ReleaseWallState_S + Update_E[ballReleased_G] = AlignPlacementState_S, + ReleaseBallState_S, AlignPlacementState_S + Update_E[shouldPickOffWall_G] = AlignWallState_S, AlignPlacementState_S + Update_E[!alignDone_G] / alignPlacement_A = @@ -265,11 +235,11 @@ struct BallPlacementPlayFSM AlignPlacementState_S + Update_E[alignDone_G] = PlaceBallState_S, PlaceBallState_S + Update_E[!ballPlaced_G] / placeBall_A = PlaceBallState_S, - PlaceBallState_S + Update_E[ballPlaced_G] / startWait_A = WaitState_S, + PlaceBallState_S + Update_E[ballPlaced_G] / startWait_A = ReleaseBallState_S, - WaitState_S + Update_E[!waitDone_G && ballPlaced_G] / wait_A = WaitState_S, - WaitState_S + Update_E[!ballPlaced_G] = StartState_S, - WaitState_S + Update_E[waitDone_G] = RetreatState_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] = RetreatState_S, RetreatState_S + Update_E[retreatDone_G && ballPlaced_G] = X, RetreatState_S + Update_E[ballPlaced_G] / retreat_A = RetreatState_S, From 5354608a82d028a6f5059fafee848481d6db76f4 Mon Sep 17 00:00:00 2001 From: Mikhael Date: Mon, 15 Jul 2024 06:54:14 -0700 Subject: [PATCH 20/29] Removed old constant --- .../ai/hl/stp/play/ball_placement/ball_placement_play_fsm.h | 1 - 1 file changed, 1 deletion(-) 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 53d4a9a0ee..a4dd897efb 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 @@ -260,7 +260,6 @@ struct BallPlacementPlayFSM 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 = 2.0; From 93141a1532ecb8ba0fa6378fdb750c1f2c1e202f Mon Sep 17 00:00:00 2001 From: "pre-commit-ci-lite[bot]" <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> Date: Mon, 15 Jul 2024 14:03:15 +0000 Subject: [PATCH 21/29] [pre-commit.ci lite] apply automatic fixes --- docs/fsm-diagrams.md | 14 +++++--------- .../ball_placement/ball_placement_play_fsm.h | 19 ++++++++++--------- 2 files changed, 15 insertions(+), 18 deletions(-) diff --git a/docs/fsm-diagrams.md b/docs/fsm-diagrams.md index e7e857ea45..786177c561 100644 --- a/docs/fsm-diagrams.md +++ b/docs/fsm-diagrams.md @@ -42,19 +42,15 @@ AlignWallState --> AlignWallState : [!wallAlignDone && shouldPickOffWall]\nal AlignWallState --> PickOffWallState : [wallAlignDone]\nsetPickOffDest AlignWallState --> AlignPlacementState : [!shouldPickOffWall] PickOffWallState --> PickOffWallState : [!wallPickOffDone]\npickOffWall -PickOffWallState --> PickOffWaitState : [wallPickOffDone]\nstartWait -PickOffWaitState --> PickOffWaitState : [!waitDone]\nwait -PickOffWaitState --> ReleaseWallState : [waitDone] -ReleaseWallState --> ReleaseWallState : [!ballReleased]\nreleaseBall -ReleaseWallState --> AlignPlacementState : [ballReleased] +PickOffWallState --> ReleaseBallState : [wallPickOffDone]\nstartWait AlignPlacementState --> AlignWallState : [shouldPickOffWall] AlignPlacementState --> AlignPlacementState : [!alignDone]\nalignPlacement AlignPlacementState --> PlaceBallState : [alignDone] PlaceBallState --> PlaceBallState : [!ballPlaced]\nplaceBall -PlaceBallState --> WaitState : [ballPlaced]\nstartWait -WaitState --> WaitState : [!waitDone && ballPlaced]\nwait -WaitState --> StartState : [!ballPlaced] -WaitState --> RetreatState : [waitDone] +PlaceBallState --> ReleaseBallState : [ballPlaced]\nstartWait +ReleaseBallState --> ReleaseBallState : [!waitDone && ballPlaced]\nreleaseBall +ReleaseBallState --> StartState : [!ballPlaced] +ReleaseBallState --> RetreatState : [waitDone] RetreatState --> Terminate:::terminate : [retreatDone && ballPlaced] RetreatState --> RetreatState : [ballPlaced]\nretreat RetreatState --> StartState : [!ballPlaced] 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 a4dd897efb..94f3b90491 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 @@ -14,8 +14,8 @@ using Zones = std::unordered_set; struct BallPlacementPlayFSM { - double BACK_AWAY_FROM_CORNER_EXTRA_M = 0.9; - double BACK_AWAY_FROM_WALL_M = ROBOT_MAX_RADIUS_METERS * 5.5; + double BACK_AWAY_FROM_CORNER_EXTRA_M = 0.9; + double BACK_AWAY_FROM_WALL_M = ROBOT_MAX_RADIUS_METERS * 5.5; double MINIMUM_DISTANCE_FROM_WALL_FOR_ALIGN_METERS = ROBOT_MAX_RADIUS_METERS * 4.0; class StartState; @@ -232,18 +232,19 @@ struct BallPlacementPlayFSM AlignPlacementState_S + Update_E[shouldPickOffWall_G] = 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] = PlaceBallState_S, PlaceBallState_S + Update_E[!ballPlaced_G] / placeBall_A = PlaceBallState_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] = RetreatState_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] = 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] = StartState_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] = StartState_S); } private: From 66fbd98fe355e4ba5d08cb8f461b164fad72f99f Mon Sep 17 00:00:00 2001 From: Mr-Anyone Date: Sat, 1 Mar 2025 15:11:02 -0800 Subject: [PATCH 22/29] Merge conflict fixes --- src/shared/robot_constants.h | 4 ++-- .../stp/play/ball_placement/ball_placement_play_fsm.cpp | 8 ++++---- .../hl/stp/play/ball_placement/ball_placement_play_fsm.h | 2 +- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/shared/robot_constants.h b/src/shared/robot_constants.h index ea8ac4d2bc..5f2390ec3b 100644 --- a/src/shared/robot_constants.h +++ b/src/shared/robot_constants.h @@ -75,7 +75,7 @@ typedef struct RobotConstants // Speed at which to spin the dribbler to release the ball right in front of a // stationary robot [rpm] - float release_ball_dribbler_speed_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; @@ -86,7 +86,7 @@ typedef struct RobotConstants // 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/x] + // 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 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 3e83475ab8..7ee5af89d2 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 @@ -32,10 +32,10 @@ void BallPlacementPlayFSM::alignWall(const Update &event) pickoff_point = ball_pos - Vector::createFromAngle(pickoff_final_orientation).normalize(0.4); align_wall_tactic->updateControlParams( - pickoff_point, pickoff_final_orientation, 0.0, TbotsProto::DribblerMode::OFF, + pickoff_point, pickoff_final_orientation, TbotsProto::DribblerMode::OFF, TbotsProto::BallCollisionType::AVOID, {AutoChipOrKickMode::OFF, 0}, TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, - TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE, 0.0); + TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE); tactics_to_run[0].emplace_back(align_wall_tactic); event.common.set_tactics(tactics_to_run); @@ -176,11 +176,11 @@ void BallPlacementPlayFSM::releaseBall(const Update &event) wait_tactic->updateControlParams( - nearest_robot->position(), nearest_robot->orientation(), 0.0, + 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, 0.0); + TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE); tactics_to_run[0].emplace_back(wait_tactic); event.common.set_tactics(tactics_to_run); 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 94f3b90491..1bd63eb4ed 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 @@ -6,7 +6,7 @@ #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/hl/stp/tactic/stop/stop_tactic.h" +#include "software/ai/hl/stp/tactic/halt/halt_tactic.h" #include "software/ai/passing/eighteen_zone_pitch_division.h" #include "software/geom/algorithms/closest_point.h" From ada9c31ac1ddf977c06c59326866e5225eac3377 Mon Sep 17 00:00:00 2001 From: Mr-Anyone Date: Sat, 1 Mar 2025 17:36:16 -0800 Subject: [PATCH 23/29] Fix problems with ball placement play --- .../ball_placement_play_fsm.cpp | 211 ++++++++---------- .../ball_placement/ball_placement_play_fsm.h | 6 +- .../ai/hl/stp/tactic/move/move_fsm.cpp | 5 +- src/software/ai/hl/stp/tactic/move/move_fsm.h | 2 +- .../ai/hl/stp/tactic/transition_conditions.h | 2 +- 5 files changed, 104 insertions(+), 122 deletions(-) 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 7ee5af89d2..68a0e4c120 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 @@ -14,14 +14,8 @@ BallPlacementPlayFSM::BallPlacementPlayFSM(TbotsProto::AiConfig ai_config) void BallPlacementPlayFSM::alignWall(const Update &event) { - PriorityTacticVector tactics_to_run = {{}}; + PriorityTacticVector tactics_to_run = {{align_wall_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()); - - // setup wall kickoff tactic for ball placing robot Point ball_pos = event.common.world_ptr->ball().position(); Rectangle field_boundary = event.common.world_ptr->field().fieldBoundary(); @@ -29,14 +23,19 @@ void BallPlacementPlayFSM::alignWall(const Update &event) calculateWallPickOffLocation(ball_pos, field_boundary, MINIMUM_DISTANCE_FROM_WALL_FOR_ALIGN_METERS) .first; + pickoff_point = ball_pos - Vector::createFromAngle(pickoff_final_orientation).normalize(0.4); + 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); - tactics_to_run[0].emplace_back(align_wall_tactic); + + setupMoveTactics(event, 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); } @@ -57,37 +56,29 @@ void BallPlacementPlayFSM::setPickOffDest(const BallPlacementPlayFSM::Update &ev void BallPlacementPlayFSM::pickOffWall(const BallPlacementPlayFSM::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()); + 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); - tactics_to_run[0].emplace_back(pickoff_wall_tactic); + setupMoveTactics(event, 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 = @@ -103,53 +94,51 @@ void BallPlacementPlayFSM::alignPlacement(const Update &event) TbotsProto::MaxAllowedSpeedMode::STOP_COMMAND, 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, 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) { - std::optional placement_point = - event.common.world_ptr->gameState().getBallPlacementPoint(); - - if (!placement_point.has_value()) - { - return; - } - 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()); - - Point ball_pos = event.common.world_ptr->ball().position(); - std::optional robot_placing_ball = - event.common.world_ptr->friendlyTeam().getNearestRobot(ball_pos); + std::optional placement_point = + event.common.world_ptr->gameState().getBallPlacementPoint(); - if (!robot_placing_ball.has_value()) + if (placement_point.has_value()) { - return; - } + Point ball_pos = event.common.world_ptr->ball().position(); + std::optional robot_placing_ball = + event.common.world_ptr->friendlyTeam().getNearestRobot(ball_pos); - Angle final_angle = robot_placing_ball->orientation(); + if (robot_placing_ball.has_value()) + { + Angle final_angle = robot_placing_ball->orientation(); + Vector placement_dribble_vector = + placement_point.value() - event.common.world_ptr->ball().position(); + if (placement_dribble_vector.length() > 0.3) + { + final_angle = placement_dribble_vector.orientation(); + } + + place_ball_tactic->updateControlParams( + event.common.world_ptr->gameState().getBallPlacementPoint(), final_angle, true, + TbotsProto::MaxAllowedSpeedMode::DRIBBLE); - Vector placement_dribble_vector = - placement_point.value() - event.common.world_ptr->ball().position(); - if (placement_dribble_vector.length() > 0.3) - { - final_angle = placement_dribble_vector.orientation(); + 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, - TbotsProto::MaxAllowedSpeedMode::DRIBBLE); - tactics_to_run[0].emplace_back(place_ball_tactic); + setupMoveTactics(event, 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); } @@ -161,47 +150,41 @@ void BallPlacementPlayFSM::startWait(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 = {{}}; - - // 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()); - - 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].emplace_back(wait_tactic); - event.common.set_tactics(tactics_to_run); + tactics_to_run[0].push_back(wait_tactic); } + + setupMoveTactics(event, 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()) { - 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()); - Point ball_pos = world_ptr->ball().position(); // robot will try to retreat backwards from wherever it is currently facing @@ -227,16 +210,20 @@ void BallPlacementPlayFSM::retreat(const Update &event) ROBOT_MAX_RADIUS_METERS); } - // setup ball placement tactic for ball placing robot retreat_tactic->updateControlParams( retreat_position, final_orientation, TbotsProto::DribblerMode::OFF, TbotsProto::BallCollisionType::AVOID, {AutoChipOrKickMode::OFF, 0}, 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, 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::shouldPickOffWall(const Update &event) @@ -254,14 +241,13 @@ 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); } - else - { - return false; - } + + return false; } bool BallPlacementPlayFSM::wallAlignDone(const BallPlacementPlayFSM::Update &event) @@ -269,14 +255,13 @@ bool BallPlacementPlayFSM::wallAlignDone(const BallPlacementPlayFSM::Update &eve 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(), pickoff_point); } - else - { - return false; - } + + return false; } bool BallPlacementPlayFSM::wallPickOffDone(const Update &event) @@ -286,35 +271,33 @@ bool BallPlacementPlayFSM::wallPickOffDone(const Update &event) 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()) { + // Check if the ball is at the placement destination return comparePoints(ball_pos, placement_point.value(), PLACEMENT_DIST_THRESHOLD_METERS) && - event.common.world_ptr->ball().velocity().length() < - this->ai_config.ai_parameter_config() - .ball_is_kicked_m_per_s_threshold(); - } - else - { - return true; + ball_speed < ai_config.ai_parameter_config().ball_is_kicked_m_per_s_threshold(); } + + 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(); + const 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; @@ -325,32 +308,34 @@ std::pair BallPlacementPlayFSM::calculateWallPickOffLocation( { Angle facing_angle; Point backoff_point; - bool near_positive_y_boundary = (field_boundary.yMax() - ball_pos.y()) < max_dist; - bool near_negative_y_boundary = (ball_pos.y() - field_boundary.yMin()) < max_dist; - bool near_positive_x_boundary = (field_boundary.xMax() - ball_pos.x()) < max_dist; - bool near_negative_x_boundary = (ball_pos.x() - field_boundary.xMin()) < max_dist; - if (near_positive_y_boundary && near_positive_x_boundary) // top right corner + + const bool near_positive_y_boundary = (field_boundary.yMax() - ball_pos.y()) < max_dist; + const bool near_negative_y_boundary = (ball_pos.y() - field_boundary.yMin()) < max_dist; + const bool near_positive_x_boundary = (field_boundary.xMax() - ball_pos.x()) < max_dist; + const bool near_negative_x_boundary = (ball_pos.x() - field_boundary.xMin()) < max_dist; + + if (near_positive_y_boundary && near_positive_x_boundary) { facing_angle = Angle::fromDegrees(45); backoff_point = field_boundary.posXPosYCorner() - Vector::createFromAngle(facing_angle) .normalize(BACK_AWAY_FROM_CORNER_EXTRA_M); } - else if (near_positive_y_boundary && near_negative_x_boundary) // top left corner + else if (near_positive_y_boundary && near_negative_x_boundary) { facing_angle = Angle::fromDegrees(135); backoff_point = field_boundary.negXPosYCorner() - Vector::createFromAngle(facing_angle) .normalize(BACK_AWAY_FROM_CORNER_EXTRA_M); } - else if (near_negative_y_boundary && near_positive_x_boundary) // bottom right corner + else if (near_negative_y_boundary && near_positive_x_boundary) { facing_angle = Angle::fromDegrees(-45); backoff_point = field_boundary.posXNegYCorner() - Vector::createFromAngle(facing_angle) .normalize(BACK_AWAY_FROM_CORNER_EXTRA_M); } - else if (near_negative_y_boundary && near_negative_x_boundary) // bottom left corner + else if (near_negative_y_boundary && near_negative_x_boundary) { facing_angle = Angle::fromDegrees(-135); backoff_point = field_boundary.negXNegYCorner() - @@ -377,29 +362,26 @@ std::pair BallPlacementPlayFSM::calculateWallPickOffLocation( facing_angle = Angle::fromDegrees(180); backoff_point = Point(field_boundary.xMin(), ball_pos.y()); } - return std::make_pair(facing_angle, backoff_point); + + 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(), + [this]() { return std::make_shared(); }); } - move_tactics = - std::vector>(num_move_tactics); - std::generate(move_tactics.begin(), move_tactics.end(), - [this]() { return std::make_shared(); }); - // 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, @@ -412,6 +394,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 1bd63eb4ed..fdaa71b83e 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 @@ -6,7 +6,6 @@ #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/hl/stp/tactic/halt/halt_tactic.h" #include "software/ai/passing/eighteen_zone_pitch_division.h" #include "software/geom/algorithms/closest_point.h" @@ -159,8 +158,6 @@ struct BallPlacementPlayFSM */ bool retreatDone(const Update& event); - - /** * Helper function for calculating the angle at which the robot must face towards to * pick up ball @@ -179,8 +176,9 @@ struct BallPlacementPlayFSM * organize the robots away from the ball placing robot * * @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()() { 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 d84e9d863a..2b0772e0d5 100644 --- a/src/software/ai/hl/stp/tactic/move/move_fsm.cpp +++ b/src/software/ai/hl/stp/tactic/move/move_fsm.cpp @@ -15,9 +15,10 @@ void MoveFSM::updateMove(const Update &event) bool MoveFSM::moveDone(const Update &event) { - // only finish moving if not dribbling. Sometimes when dribbling we just want to hold - // the ball somewhere. + // 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 34fd11e180..02eafeea26 100644 --- a/src/software/ai/hl/stp/tactic/move/move_fsm.h +++ b/src/software/ai/hl/stp/tactic/move/move_fsm.h @@ -66,6 +66,6 @@ struct MoveFSM *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/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 From f622aadb05485a453b6b19e2eba221037173b39e Mon Sep 17 00:00:00 2001 From: williamckha Date: Sat, 8 Mar 2025 13:57:28 -0800 Subject: [PATCH 24/29] Code cleanup --- src/shared/constants.h | 16 ++-- src/shared/robot_constants.h | 2 - .../ball_placement/ball_placement_play.cpp | 3 +- .../ball_placement_play_fsm.cpp | 94 +++++++++---------- .../ball_placement/ball_placement_play_fsm.h | 88 ++++++++++------- .../ai/hl/stp/tactic/move/move_fsm.cpp | 5 +- .../field_tests/ball_placement_field_test.py | 11 +-- 7 files changed, 115 insertions(+), 104 deletions(-) diff --git a/src/shared/constants.h b/src/shared/constants.h index 8030105b04..3eded34995 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 5f2390ec3b..afdc9deebf 100644 --- a/src/shared/robot_constants.h +++ b/src/shared/robot_constants.h @@ -92,8 +92,6 @@ typedef struct RobotConstants // 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.cpp b/src/software/ai/hl/stp/play/ball_placement/ball_placement_play.cpp index 51443d36d3..563dd977e5 100644 --- a/src/software/ai/hl/stp/play/ball_placement/ball_placement_play.cpp +++ b/src/software/ai/hl/stp/play/ball_placement/ball_placement_play.cpp @@ -18,11 +18,12 @@ void BallPlacementPlay::getNextTactics(TacticCoroutine::push_type &yield, void BallPlacementPlay::updateTactics(const PlayUpdate &play_update) { - auto event = BallPlacementPlayFSM::Update(control_params, play_update); + BallPlacementPlayFSM::Update event(control_params, play_update); fsm.process_event(event); std::optional placement_point = event.common.world_ptr->gameState().getBallPlacementPoint(); + if (placement_point.has_value()) { TbotsProto::BallPlacementVisualization ball_placement_vis_msg; 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 68a0e4c120..c1a4dada0a 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 @@ -19,10 +19,7 @@ void BallPlacementPlayFSM::alignWall(const Update &event) Point ball_pos = event.common.world_ptr->ball().position(); Rectangle field_boundary = event.common.world_ptr->field().fieldBoundary(); - pickoff_final_orientation = - calculateWallPickOffLocation(ball_pos, field_boundary, - MINIMUM_DISTANCE_FROM_WALL_FOR_ALIGN_METERS) - .first; + pickoff_final_orientation = calculateWallPickOffDest(ball_pos, field_boundary).first; pickoff_point = ball_pos - Vector::createFromAngle(pickoff_final_orientation).normalize(0.4); @@ -33,7 +30,8 @@ void BallPlacementPlayFSM::alignWall(const Update &event) TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE); - setupMoveTactics(event, event.common.num_tactics - static_cast(tactics_to_run[0].size())); + setupMoveTactics(event, 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()); @@ -45,8 +43,7 @@ void BallPlacementPlayFSM::setPickOffDest(const BallPlacementPlayFSM::Update &ev Point ball_pos = event.common.world_ptr->ball().position(); Rectangle field_boundary = event.common.world_ptr->field().fieldBoundary(); - std::pair location = calculateWallPickOffLocation( - ball_pos, field_boundary, MINIMUM_DISTANCE_FROM_WALL_FOR_ALIGN_METERS); + std::pair location = calculateWallPickOffDest(ball_pos, field_boundary); pickoff_final_orientation = location.first; pickoff_destination = @@ -63,7 +60,8 @@ void BallPlacementPlayFSM::pickOffWall(const BallPlacementPlayFSM::Update &event TbotsProto::MaxAllowedSpeedMode::BALL_PLACEMENT_WALL_DRIBBLE, TbotsProto::MaxAllowedSpeedMode::BALL_PLACEMENT_WALL_DRIBBLE); - setupMoveTactics(event, event.common.num_tactics - static_cast(tactics_to_run[0].size())); + setupMoveTactics(event, 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()); @@ -86,7 +84,7 @@ void BallPlacementPlayFSM::alignPlacement(const Update &event) .normalize(); Angle setup_angle = alignment_vector.orientation(); setup_point = event.common.world_ptr->ball().position() - - 2.5 * alignment_vector * ROBOT_MAX_RADIUS_METERS; + ALIGNMENT_VECTOR_LENGTH_FACTOR * alignment_vector; align_placement_tactic->updateControlParams( setup_point, setup_angle, TbotsProto::DribblerMode::OFF, @@ -97,7 +95,8 @@ void BallPlacementPlayFSM::alignPlacement(const Update &event) tactics_to_run[0].push_back(align_placement_tactic); } - setupMoveTactics(event, event.common.num_tactics - static_cast(tactics_to_run[0].size())); + setupMoveTactics(event, 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()); @@ -127,16 +126,17 @@ void BallPlacementPlayFSM::placeBall(const Update &event) { final_angle = placement_dribble_vector.orientation(); } - + place_ball_tactic->updateControlParams( - event.common.world_ptr->gameState().getBallPlacementPoint(), final_angle, true, - TbotsProto::MaxAllowedSpeedMode::DRIBBLE); + event.common.world_ptr->gameState().getBallPlacementPoint(), final_angle, + true, TbotsProto::MaxAllowedSpeedMode::DRIBBLE); tactics_to_run[0].push_back(place_ball_tactic); } } - setupMoveTactics(event, event.common.num_tactics - static_cast(tactics_to_run[0].size())); + setupMoveTactics(event, 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()); @@ -168,7 +168,8 @@ void BallPlacementPlayFSM::releaseBall(const Update &event) tactics_to_run[0].push_back(wait_tactic); } - setupMoveTactics(event, event.common.num_tactics - static_cast(tactics_to_run[0].size())); + setupMoveTactics(event, 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()); @@ -187,15 +188,15 @@ void BallPlacementPlayFSM::retreat(const Update &event) { Point ball_pos = world_ptr->ball().position(); - // robot will try to retreat backwards from wherever it is currently facing + // 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); + retreat_direction * (RETREAT_DISTANCE_M + 2 * ROBOT_MAX_RADIUS_METERS); - // 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)) { @@ -206,7 +207,7 @@ void BallPlacementPlayFSM::retreat(const Update &event) closer_goal = world_ptr->field().enemyGoalCenter(); } retreat_direction = (closer_goal - ball_pos).normalize(); - retreat_position = ball_pos + retreat_direction * (RETREAT_DISTANCE_METERS + + retreat_position = ball_pos + retreat_direction * (RETREAT_DISTANCE_M + ROBOT_MAX_RADIUS_METERS); } @@ -219,7 +220,8 @@ void BallPlacementPlayFSM::retreat(const Update &event) tactics_to_run[0].push_back(retreat_tactic); } - setupMoveTactics(event, event.common.num_tactics - static_cast(tactics_to_run[0].size())); + setupMoveTactics(event, 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()); @@ -228,12 +230,10 @@ void BallPlacementPlayFSM::retreat(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().fieldBoundary(); - double wiggle_room = std::abs(signedDistance(ball_pos, field_lines)); - - return wiggle_room <= MINIMUM_DISTANCE_FROM_WALL_FOR_ALIGN_METERS; + 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) @@ -246,7 +246,7 @@ bool BallPlacementPlayFSM::alignDone(const Update &event) { return comparePoints(nearest_robot.value().position(), setup_point); } - + return false; } @@ -255,7 +255,7 @@ bool BallPlacementPlayFSM::wallAlignDone(const BallPlacementPlayFSM::Update &eve 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(), pickoff_point); @@ -271,7 +271,7 @@ bool BallPlacementPlayFSM::wallPickOffDone(const Update &event) bool BallPlacementPlayFSM::ballPlaced(const Update &event) { - const 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 = @@ -282,8 +282,9 @@ bool BallPlacementPlayFSM::ballPlaced(const Update &event) { // Check if the ball is at the placement destination return comparePoints(ball_pos, placement_point.value(), - PLACEMENT_DIST_THRESHOLD_METERS) && - ball_speed < ai_config.ai_parameter_config().ball_is_kicked_m_per_s_threshold(); + PLACEMENT_DIST_THRESHOLD_M) && + ball_speed < + ai_config.ai_parameter_config().ball_is_kicked_m_per_s_threshold(); } return true; @@ -300,19 +301,23 @@ bool BallPlacementPlayFSM::retreatDone(const Update &event) const 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; + ->position()) > RETREAT_DISTANCE_M; } -std::pair BallPlacementPlayFSM::calculateWallPickOffLocation( - const Point &ball_pos, const Rectangle &field_boundary, const double max_dist) +std::pair BallPlacementPlayFSM::calculateWallPickOffDest( + const Point &ball_pos, const Rectangle &field_boundary) { Angle facing_angle; Point backoff_point; - - const bool near_positive_y_boundary = (field_boundary.yMax() - ball_pos.y()) < max_dist; - const bool near_negative_y_boundary = (ball_pos.y() - field_boundary.yMin()) < max_dist; - const bool near_positive_x_boundary = (field_boundary.xMax() - ball_pos.x()) < max_dist; - const bool near_negative_x_boundary = (ball_pos.x() - field_boundary.xMin()) < max_dist; + + 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) { @@ -370,23 +375,18 @@ void BallPlacementPlayFSM::setupMoveTactics(const Update &event, unsigned int nu { if (move_tactics.size() != num_tactics) { - move_tactics = - std::vector>(num_tactics); + move_tactics = std::vector>(num_tactics); std::generate(move_tactics.begin(), move_tactics.end(), - [this]() { return std::make_shared(); }); + [this]() { return std::make_shared(); }); } - // 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++) { 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 fdaa71b83e..50a23a32a5 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 @@ -9,14 +9,8 @@ #include "software/ai/passing/eighteen_zone_pitch_division.h" #include "software/geom/algorithms/closest_point.h" -using Zones = std::unordered_set; - struct BallPlacementPlayFSM { - double BACK_AWAY_FROM_CORNER_EXTRA_M = 0.9; - double BACK_AWAY_FROM_WALL_M = ROBOT_MAX_RADIUS_METERS * 5.5; - double MINIMUM_DISTANCE_FROM_WALL_FOR_ALIGN_METERS = ROBOT_MAX_RADIUS_METERS * 4.0; - class StartState; class PickOffWallState; class AlignWallState; @@ -32,22 +26,22 @@ struct BallPlacementPlayFSM DEFINE_PLAY_UPDATE_STRUCT_WITH_CONTROL_AND_COMMON_PARAMS /** - * Creates a ball placement play FSM + * Creates a BallPlacementPlayFSM * * @param ai_config the play config for this play FSM */ explicit BallPlacementPlayFSM(TbotsProto::AiConfig ai_config); /** - * Action that has the robot align with the wall in order to pick the ball off of it. + * 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); /** - * The transition action into picking the ball off the wall, set the target - * destination. + * Sets the target wall pickoff destination. + * This action is called one time before transitioning into PickOffWallState. * * @param event the BallPlacementPlayFSM Update event */ @@ -78,7 +72,8 @@ struct BallPlacementPlayFSM 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 */ @@ -112,14 +107,16 @@ struct BallPlacementPlayFSM * 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 wether the robot is in position to pick the ball off the wall + * 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); @@ -145,35 +142,42 @@ struct BallPlacementPlayFSM 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 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 at which the robot must face towards to - * pick up ball + * 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 */ - std::pair calculateWallPickOffLocation(const Point& ball_pos, - const Rectangle& field_lines, - double max_dist); + 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 @@ -214,31 +218,33 @@ struct BallPlacementPlayFSM // src_state + event [guard] / action = dest_state *StartState_S + Update_E[!shouldPickOffWall_G] / alignPlacement_A = AlignPlacementState_S, - StartState_S + Update_E[shouldPickOffWall_G] = AlignWallState_S, + StartState_S + Update_E[shouldPickOffWall_G] / alignWall_A = AlignWallState_S, - AlignWallState_S + Update_E[!wallAlignDone_G && shouldPickOffWall_G] / - alignWall_A = AlignWallState_S, - AlignWallState_S + Update_E[wallAlignDone_G] / setPickOffDest_A = - PickOffWallState_S, - AlignWallState_S + Update_E[!shouldPickOffWall_G] = AlignPlacementState_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, + PickOffWallState_S + Update_E[!wallPickOffDone_G] / pickOffWall_A, PickOffWallState_S + Update_E[wallPickOffDone_G] / startWait_A = ReleaseBallState_S, - AlignPlacementState_S + Update_E[shouldPickOffWall_G] = AlignWallState_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 = 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] = RetreatState_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, @@ -247,6 +253,7 @@ struct BallPlacementPlayFSM private: TbotsProto::AiConfig ai_config; + std::shared_ptr align_wall_tactic; std::shared_ptr pickoff_wall_tactic; std::shared_ptr place_ball_tactic; @@ -254,12 +261,21 @@ struct BallPlacementPlayFSM std::shared_ptr retreat_tactic; 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 RETREAT_DISTANCE_METERS = 0.6; - constexpr static double const PLACEMENT_DIST_THRESHOLD_METERS = 0.15; - constexpr static double const BALL_IS_PLACED_WAIT_S = 2.0; + + static constexpr double BACK_AWAY_FROM_CORNER_EXTRA_M = 0.9; + static constexpr double BACK_AWAY_FROM_WALL_M = ROBOT_MAX_RADIUS_METERS * 5.5; + static constexpr double MIN_DISTANCE_FROM_WALL_FOR_ALIGN_M = + ROBOT_MAX_RADIUS_METERS * 4; + static constexpr double RETREAT_DISTANCE_M = 0.6; + static constexpr double PLACEMENT_DIST_THRESHOLD_M = 0.15; + 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_FACTOR = + ROBOT_MAX_RADIUS_METERS * 2.5; }; 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 2b0772e0d5..27c7b62d28 100644 --- a/src/software/ai/hl/stp/tactic/move/move_fsm.cpp +++ b/src/software/ai/hl/stp/tactic/move/move_fsm.cpp @@ -15,10 +15,11 @@ void MoveFSM::updateMove(const Update &event) bool MoveFSM::moveDone(const Update &event) { - // Only finish moving if not dribbling/kicking/chipping. + // 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 && + 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/field_tests/ball_placement_field_test.py b/src/software/field_tests/ball_placement_field_test.py index 5a444faac7..4b5eff252d 100644 --- a/src/software/field_tests/ball_placement_field_test.py +++ b/src/software/field_tests/ball_placement_field_test.py @@ -1,22 +1,16 @@ -import pytest - import software.python_bindings as tbots_cpp import sys -from proto.ssl_gc_common_pb2 import Team 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 -from software.simulated_tests.robot_enters_region import RobotEventuallyEntersRegion -from proto.message_translation.tbots_protobuf import create_world_state logger = createLogger(__name__) -# this field test will start a ball placement referee command to the placement_point. + def test_ball_placement(field_test_runner): - # point to place ball placement_point = tbots_cpp.Point(0, 0) world = field_test_runner.world_buffer.get(block=True, timeout=WORLD_BUFFER_TIMEOUT) @@ -31,18 +25,19 @@ def test_ball_placement(field_test_runner): 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__)) From 6ac00452cca6ee43828f1fffd8c1c14ef8eb803a Mon Sep 17 00:00:00 2001 From: williamckha Date: Sat, 8 Mar 2025 14:13:51 -0800 Subject: [PATCH 25/29] Remove change to logger.h --- src/software/logger/logger.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/software/logger/logger.h b/src/software/logger/logger.h index e5a6d4a099..461ff8f732 100644 --- a/src/software/logger/logger.h +++ b/src/software/logger/logger.h @@ -89,7 +89,7 @@ class LoggerSingleton // arg. Note: log locations are defaulted to the bazel-out folder due to Bazel's // hermetic build principles - // // if log dir doesn't exist, create it + // if log dir doesn't exist, create it if (!std::experimental::filesystem::exists(runtime_dir)) { std::experimental::filesystem::create_directories(runtime_dir); From 1dd1bb5a721114aa852c26775935c99f2147772f Mon Sep 17 00:00:00 2001 From: "pre-commit-ci-lite[bot]" <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> Date: Sat, 8 Mar 2025 22:23:23 +0000 Subject: [PATCH 26/29] [pre-commit.ci lite] apply automatic fixes --- docs/fsm-diagrams.md | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/docs/fsm-diagrams.md b/docs/fsm-diagrams.md index 56c91d1f0c..d5f46c5a4b 100644 --- a/docs/fsm-diagrams.md +++ b/docs/fsm-diagrams.md @@ -34,20 +34,20 @@ classDef terminate fill:white,color:black,font-weight:bold direction LR [*] --> StartState StartState --> AlignPlacementState : [!shouldPickOffWall]\nalignPlacement -StartState --> AlignWallState : [shouldPickOffWall] +StartState --> AlignWallState : [shouldPickOffWall]\nalignWall AlignWallState --> AlignWallState : [!wallAlignDone && shouldPickOffWall]\nalignWall -AlignWallState --> PickOffWallState : [wallAlignDone]\nsetPickOffDest -AlignWallState --> AlignPlacementState : [!shouldPickOffWall] +AlignWallState --> PickOffWallState : [wallAlignDone]\nsetPickOffDest, pickOffWall +AlignWallState --> AlignPlacementState : [!shouldPickOffWall]\nalignPlacement PickOffWallState --> PickOffWallState : [!wallPickOffDone]\npickOffWall PickOffWallState --> ReleaseBallState : [wallPickOffDone]\nstartWait -AlignPlacementState --> AlignWallState : [shouldPickOffWall] +AlignPlacementState --> AlignWallState : [shouldPickOffWall]\nalignWall AlignPlacementState --> AlignPlacementState : [!alignDone]\nalignPlacement -AlignPlacementState --> PlaceBallState : [alignDone] +AlignPlacementState --> PlaceBallState : [alignDone]\nplaceBall PlaceBallState --> PlaceBallState : [!ballPlaced]\nplaceBall PlaceBallState --> ReleaseBallState : [ballPlaced]\nstartWait ReleaseBallState --> ReleaseBallState : [!waitDone && ballPlaced]\nreleaseBall ReleaseBallState --> StartState : [!ballPlaced] -ReleaseBallState --> RetreatState : [waitDone] +ReleaseBallState --> RetreatState : [waitDone]\nretreat RetreatState --> Terminate:::terminate : [retreatDone && ballPlaced] RetreatState --> RetreatState : [ballPlaced]\nretreat RetreatState --> StartState : [!ballPlaced] @@ -374,7 +374,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 ``` From 5a082fb52de86917ee7a3669e6b460a8cba2ddf5 Mon Sep 17 00:00:00 2001 From: williamckha Date: Fri, 9 Jan 2026 22:38:25 -0800 Subject: [PATCH 27/29] Addressed some comments + code cleanup --- src/cli/cli_params.py | 2 +- .../ball_placement/ball_placement_play.cpp | 3 +- .../ball_placement_play_fsm.cpp | 164 +++++++++--------- .../ball_placement/ball_placement_play_fsm.h | 20 +-- .../hl/stp/tactic/dribble/dribble_tactic.cpp | 8 +- .../ai/hl/stp/tactic/dribble/dribble_tactic.h | 4 +- .../stp/tactic/pivot_kick/pivot_kick_tactic.h | 2 - .../ai/hl/stp/tactic/tactic_visitor.h | 2 - .../motion_constraint_visitor.cpp | 5 - .../motion_constraint_visitor.h | 1 - .../thunderscope/thunderscope_config.py | 12 +- 11 files changed, 103 insertions(+), 120 deletions(-) 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/software/ai/hl/stp/play/ball_placement/ball_placement_play.cpp b/src/software/ai/hl/stp/play/ball_placement/ball_placement_play.cpp index 4ef7b10428..721749addd 100644 --- a/src/software/ai/hl/stp/play/ball_placement/ball_placement_play.cpp +++ b/src/software/ai/hl/stp/play/ball_placement/ball_placement_play.cpp @@ -19,12 +19,11 @@ void BallPlacementPlay::getNextTactics(TacticCoroutine::push_type &yield, void BallPlacementPlay::updateTactics(const PlayUpdate &play_update) { - BallPlacementPlayFSM::Update event(control_params, play_update); + auto event = BallPlacementPlayFSM::Update(control_params, play_update); fsm.process_event(event); std::optional placement_point = event.common.world_ptr->gameState().getBallPlacementPoint(); - if (placement_point.has_value()) { TbotsProto::BallPlacementVisualization ball_placement_vis_msg; 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 4d593b4cf9..420bb0cf52 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,12 +3,12 @@ BallPlacementPlayFSM::BallPlacementPlayFSM( std::shared_ptr ai_config_ptr) : PlayFSM(ai_config_ptr), - align_wall_tactic(std::make_shared()), + 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)), - wait_tactic(std::make_shared()), + wait_tactic(std::make_shared(ai_config_ptr)), move_tactics(std::vector>()) { } @@ -17,13 +17,7 @@ void BallPlacementPlayFSM::alignWall(const Update &event) { PriorityTacticVector tactics_to_run = {{align_wall_tactic}}; - Point ball_pos = event.common.world_ptr->ball().position(); - Rectangle field_boundary = event.common.world_ptr->field().fieldBoundary(); - - pickoff_final_orientation = calculateWallPickOffDest(ball_pos, field_boundary).first; - - pickoff_point = - ball_pos - Vector::createFromAngle(pickoff_final_orientation).normalize(0.4); + setPickOffDest(event); align_wall_tactic->updateControlParams( pickoff_point, pickoff_final_orientation, TbotsProto::DribblerMode::OFF, @@ -39,20 +33,17 @@ void BallPlacementPlayFSM::alignWall(const Update &event) event.common.set_tactics(tactics_to_run); } -void BallPlacementPlayFSM::setPickOffDest(const BallPlacementPlayFSM::Update &event) +void BallPlacementPlayFSM::setPickOffDest(const Update &event) { Point ball_pos = event.common.world_ptr->ball().position(); Rectangle field_boundary = event.common.world_ptr->field().fieldBoundary(); - std::pair location = calculateWallPickOffDest(ball_pos, field_boundary); - - pickoff_final_orientation = location.first; - pickoff_destination = - location.second - Vector::createFromAngle(pickoff_final_orientation) - .normalize(BACK_AWAY_FROM_WALL_M); + auto [orientation, dest] = calculateWallPickOffDest(ball_pos, field_boundary); + pickoff_final_orientation = orientation; + pickoff_destination = dest; } -void BallPlacementPlayFSM::pickOffWall(const BallPlacementPlayFSM::Update &event) +void BallPlacementPlayFSM::pickOffWall(const Update &event) { PriorityTacticVector tactics_to_run = {{pickoff_wall_tactic}}; @@ -82,15 +73,14 @@ void BallPlacementPlayFSM::alignPlacement(const Update &event) // 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() - - ALIGNMENT_VECTOR_LENGTH_FACTOR * alignment_vector; + 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::STOP_COMMAND, + TbotsProto::MaxAllowedSpeedMode::DRIBBLE, TbotsProto::ObstacleAvoidanceMode::SAFE); tactics_to_run[0].push_back(align_placement_tactic); @@ -119,13 +109,22 @@ void BallPlacementPlayFSM::placeBall(const Update &event) if (robot_placing_ball.has_value()) { - Angle final_angle = robot_placing_ball->orientation(); - - Vector placement_dribble_vector = + Vector placement_vector = placement_point.value() - event.common.world_ptr->ball().position(); - if (placement_dribble_vector.length() > 0.3) + + Angle final_angle; + if (placement_vector.length() > APPROACHING_PLACEMENT_DIST_THRESHOLD_M) { - final_angle = placement_dribble_vector.orientation(); + // 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( @@ -190,30 +189,30 @@ void BallPlacementPlayFSM::retreat(const Update &event) 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_M + 2 * ROBOT_MAX_RADIUS_METERS); + 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 // 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_M + - ROBOT_MAX_RADIUS_METERS); + + retreat_dir = closer_goal - ball_pos; + retreat_pos = ball_pos + retreat_dir.normalize(RETREAT_DISTANCE_M); } 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::BALL_PLACEMENT_RETREAT, TbotsProto::ObstacleAvoidanceMode::SAFE); @@ -245,13 +244,13 @@ bool BallPlacementPlayFSM::alignDone(const Update &event) if (nearest_robot.has_value()) { - return comparePoints(nearest_robot.value().position(), setup_point); + return comparePoints(nearest_robot->position(), setup_point); } return false; } -bool BallPlacementPlayFSM::wallAlignDone(const BallPlacementPlayFSM::Update &event) +bool BallPlacementPlayFSM::wallAlignDone(const Update &event) { std::optional nearest_robot = event.common.world_ptr->friendlyTeam().getNearestRobot( @@ -259,7 +258,7 @@ bool BallPlacementPlayFSM::wallAlignDone(const BallPlacementPlayFSM::Update &eve if (nearest_robot.has_value()) { - return comparePoints(nearest_robot.value().position(), pickoff_point); + return comparePoints(nearest_robot->position(), pickoff_point); } return false; @@ -281,11 +280,14 @@ bool BallPlacementPlayFSM::ballPlaced(const Update &event) if (placement_point.has_value()) { - // Check if the ball is at the placement destination - return comparePoints(ball_pos, placement_point.value(), - PLACEMENT_DIST_THRESHOLD_M) && - ball_speed < - ai_config.ai_parameter_config().ball_is_kicked_m_per_s_threshold(); + 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; @@ -299,10 +301,7 @@ bool BallPlacementPlayFSM::waitDone(const Update &event) bool BallPlacementPlayFSM::retreatDone(const Update &event) { - const Point ball_position = event.common.world_ptr->ball().position(); - return distance(ball_position, event.common.world_ptr->friendlyTeam() - .getNearestRobot(ball_position) - ->position()) > RETREAT_DISTANCE_M; + return retreat_tactic->done(); } std::pair BallPlacementPlayFSM::calculateWallPickOffDest( @@ -322,51 +321,59 @@ std::pair BallPlacementPlayFSM::calculateWallPickOffDest( if (near_positive_y_boundary && near_positive_x_boundary) { - facing_angle = Angle::fromDegrees(45); - backoff_point = field_boundary.posXPosYCorner() - - Vector::createFromAngle(facing_angle) - .normalize(BACK_AWAY_FROM_CORNER_EXTRA_M); + facing_angle = Angle::fromDegrees(45); + backoff_point = + field_boundary.posXPosYCorner() - + Vector::createFromAngle(facing_angle).normalize(BACK_AWAY_FROM_CORNER_M); } else if (near_positive_y_boundary && near_negative_x_boundary) { - facing_angle = Angle::fromDegrees(135); - backoff_point = field_boundary.negXPosYCorner() - - Vector::createFromAngle(facing_angle) - .normalize(BACK_AWAY_FROM_CORNER_EXTRA_M); + facing_angle = Angle::fromDegrees(135); + backoff_point = + field_boundary.negXPosYCorner() - + Vector::createFromAngle(facing_angle).normalize(BACK_AWAY_FROM_CORNER_M); } else if (near_negative_y_boundary && near_positive_x_boundary) { - facing_angle = Angle::fromDegrees(-45); - backoff_point = field_boundary.posXNegYCorner() - - Vector::createFromAngle(facing_angle) - .normalize(BACK_AWAY_FROM_CORNER_EXTRA_M); + facing_angle = Angle::fromDegrees(-45); + backoff_point = + field_boundary.posXNegYCorner() - + Vector::createFromAngle(facing_angle).normalize(BACK_AWAY_FROM_CORNER_M); } else if (near_negative_y_boundary && near_negative_x_boundary) { - facing_angle = Angle::fromDegrees(-135); - backoff_point = field_boundary.negXNegYCorner() - - Vector::createFromAngle(facing_angle) - .normalize(BACK_AWAY_FROM_CORNER_EXTRA_M); + 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()); + 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()); + 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()); + facing_angle = Angle::fromDegrees(-90); + backoff_point = + Point(ball_pos.x(), field_boundary.yMin()) - + Vector::createFromAngle(facing_angle).normalize(BACK_AWAY_FROM_WALL_M); } else if (near_negative_x_boundary) { - facing_angle = Angle::fromDegrees(180); - backoff_point = Point(field_boundary.xMin(), ball_pos.y()); + 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}; @@ -377,8 +384,9 @@ void BallPlacementPlayFSM::setupMoveTactics(const Update &event, unsigned int nu if (move_tactics.size() != num_tactics) { move_tactics = std::vector>(num_tactics); - std::generate(move_tactics.begin(), move_tactics.end(), - [this]() { return std::make_shared(); }); + std::generate( + move_tactics.begin(), move_tactics.end(), + [&] { return std::make_shared(ai_config_ptr); }); } Vector waiting_line_vector = 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 3191e7033e..c62b976a35 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 @@ -26,7 +26,6 @@ struct BallPlacementPlayFSM : public PlayFSM class ReleaseBallState; class RetreatState; - /** * Creates a BallPlacementPlayFSM * @@ -271,14 +270,13 @@ struct BallPlacementPlayFSM : public PlayFSM Angle pickoff_final_orientation; Timestamp start_time; - static constexpr double BACK_AWAY_FROM_CORNER_EXTRA_M = 0.9; - static constexpr double BACK_AWAY_FROM_WALL_M = ROBOT_MAX_RADIUS_METERS * 5.5; - static constexpr double MIN_DISTANCE_FROM_WALL_FOR_ALIGN_M = - ROBOT_MAX_RADIUS_METERS * 4; - static constexpr double RETREAT_DISTANCE_M = 0.6; - static constexpr double PLACEMENT_DIST_THRESHOLD_M = 0.15; - 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_FACTOR = - ROBOT_MAX_RADIUS_METERS * 2.5; + 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/tactic/dribble/dribble_tactic.cpp b/src/software/ai/hl/stp/tactic/dribble/dribble_tactic.cpp index 9d6906b06f..57b6ad0681 100644 --- a/src/software/ai/hl/stp/tactic/dribble/dribble_tactic.cpp +++ b/src/software/ai/hl/stp/tactic/dribble/dribble_tactic.cpp @@ -5,13 +5,7 @@ DribbleTactic::DribbleTactic(std::shared_ptr ai_config_ptr) : TacticBase( {RobotCapability::Move, RobotCapability::Dribble, RobotCapability::Kick}, - ai_config_ptr), - control_params{DribbleFSM::ControlParams{ - .dribble_destination = std::nullopt, - .final_dribble_orientation = std::nullopt, - .allow_excessive_dribbling = false, - .max_speed_dribble = TbotsProto::MaxAllowedSpeedMode::DRIBBLE, - .max_speed_get_possession = TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT}}, + ai_config_ptr) { } 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 63c7cb5e79..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,7 +32,9 @@ 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 attained while the ball is in possession + * @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, 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 1faabdea6d..dbc7e7d770 100644 --- a/src/software/ai/hl/stp/tactic/tactic_visitor.h +++ b/src/software/ai/hl/stp/tactic/tactic_visitor.h @@ -29,7 +29,6 @@ class MoveGoalieToGoalLineTactic; class PrepareKickoffMoveTactic; class BallPlacementDribbleTactic; class BallPlacementMoveTactic; -class WallKickoffTactic; class AvoidInterferenceTactic; /** @@ -71,6 +70,5 @@ class TacticVisitor virtual void visit(const PrepareKickoffMoveTactic &tactic) = 0; virtual void visit(const BallPlacementDribbleTactic &tactic) = 0; virtual void visit(const BallPlacementMoveTactic &tactic) = 0; - virtual void visit(const WallKickoffTactic &tactic) = 0; virtual void visit(const AvoidInterferenceTactic &tactic) = 0; }; diff --git a/src/software/ai/motion_constraint/motion_constraint_visitor.cpp b/src/software/ai/motion_constraint/motion_constraint_visitor.cpp index 0349b1ffcf..ecb765b0bc 100644 --- a/src/software/ai/motion_constraint/motion_constraint_visitor.cpp +++ b/src/software/ai/motion_constraint/motion_constraint_visitor.cpp @@ -97,11 +97,6 @@ void MotionConstraintVisitor::visit(const BallPlacementMoveTactic &tactic) current_motion_constraints.clear(); } -void MotionConstraintVisitor::visit(const WallKickoffTactic &tactic) -{ - current_motion_constraints.clear(); -} - void MotionConstraintVisitor::visit(const AvoidInterferenceTactic &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 abc905c940..b73fa7d428 100644 --- a/src/software/ai/motion_constraint/motion_constraint_visitor.h +++ b/src/software/ai/motion_constraint/motion_constraint_visitor.h @@ -42,7 +42,6 @@ class MotionConstraintVisitor : public TacticVisitor void visit(const PrepareKickoffMoveTactic &tactic) override; void visit(const BallPlacementDribbleTactic &tactic) override; void visit(const BallPlacementMoveTactic &tactic) override; - void visit(const WallKickoffTactic &tactic) override; void visit(const AvoidInterferenceTactic &tactic) override; void visit(const PassDefenderTactic &tactic) override; diff --git a/src/software/thunderscope/thunderscope_config.py b/src/software/thunderscope/thunderscope_config.py index ad4fdde584..f3c328aee6 100644 --- a/src/software/thunderscope/thunderscope_config.py +++ b/src/software/thunderscope/thunderscope_config.py @@ -467,11 +467,7 @@ def configure_replay_view( replay=True, replay_log=blue_replay_log, visualization_buffer_size=visualization_buffer_size, - extra_widgets=[ - configure_robot_view_fullsystem( - proto_unix_io_map[ProtoUnixIOTypes.BLUE] - ) - ], + extra_widgets=[], frame_swap_counter=FrameTimeCounter(), refresh_counter=blue_refresh_func_counter, ), @@ -494,11 +490,7 @@ def configure_replay_view( replay=True, replay_log=yellow_replay_log, visualization_buffer_size=visualization_buffer_size, - extra_widgets=[ - configure_robot_view_fullsystem( - proto_unix_io_map[ProtoUnixIOTypes.YELLOW] - ) - ], + extra_widgets=[], frame_swap_counter=FrameTimeCounter(), refresh_counter=yellow_refresh_func_counter, ), From ce857e1963cf711f488aeeafc8ba04089db23822 Mon Sep 17 00:00:00 2001 From: Apeiros-46B Date: Tue, 10 Feb 2026 19:12:15 -0800 Subject: [PATCH 28/29] Use saturating arithmetic in ball placement play to avoid underflow --- .../ball_placement_play_fsm.cpp | 37 +++++++++++++------ .../ball_placement/ball_placement_play_fsm.h | 3 -- 2 files changed, 25 insertions(+), 15 deletions(-) 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 420bb0cf52..f9e721d2eb 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 @@ -13,6 +13,19 @@ BallPlacementPlayFSM::BallPlacementPlayFSM( { } +// Subtract b from a, or return 0 if b is larger than a +static unsigned int subSat(unsigned int a, unsigned int b) +{ + if (b > a) + { + return 0; + } + else + { + return a - b; + } +} + void BallPlacementPlayFSM::alignWall(const Update &event) { PriorityTacticVector tactics_to_run = {{align_wall_tactic}}; @@ -25,8 +38,8 @@ void BallPlacementPlayFSM::alignWall(const Update &event) TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE); - setupMoveTactics(event, event.common.num_tactics - - static_cast(tactics_to_run[0].size())); + 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()); @@ -52,8 +65,8 @@ void BallPlacementPlayFSM::pickOffWall(const Update &event) TbotsProto::MaxAllowedSpeedMode::BALL_PLACEMENT_WALL_DRIBBLE, TbotsProto::MaxAllowedSpeedMode::BALL_PLACEMENT_WALL_DRIBBLE); - setupMoveTactics(event, event.common.num_tactics - - static_cast(tactics_to_run[0].size())); + 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()); @@ -86,8 +99,8 @@ void BallPlacementPlayFSM::alignPlacement(const Update &event) tactics_to_run[0].push_back(align_placement_tactic); } - setupMoveTactics(event, event.common.num_tactics - - static_cast(tactics_to_run[0].size())); + 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()); @@ -135,8 +148,8 @@ void BallPlacementPlayFSM::placeBall(const Update &event) } } - setupMoveTactics(event, event.common.num_tactics - - static_cast(tactics_to_run[0].size())); + 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()); @@ -168,8 +181,8 @@ void BallPlacementPlayFSM::releaseBall(const Update &event) tactics_to_run[0].push_back(wait_tactic); } - setupMoveTactics(event, event.common.num_tactics - - static_cast(tactics_to_run[0].size())); + 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()); @@ -220,8 +233,8 @@ void BallPlacementPlayFSM::retreat(const Update &event) tactics_to_run[0].push_back(retreat_tactic); } - setupMoveTactics(event, event.common.num_tactics - - static_cast(tactics_to_run[0].size())); + 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()); 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 c62b976a35..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,9 +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" -#include "software/geom/algorithms/closest_point.h" struct BallPlacementPlayFSM : public PlayFSM { From 0f77793ca86718affd3e29e7326c040d87322325 Mon Sep 17 00:00:00 2001 From: Apeiros-46B Date: Sun, 22 Feb 2026 21:59:56 -0500 Subject: [PATCH 29/29] Add some more tests --- .../ball_placement_play_fsm_test.cpp | 103 ++++++++++++++++-- 1 file changed, 95 insertions(+), 8 deletions(-) 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 ad7b71311f..93e1ea242a 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 @@ -1,18 +1,31 @@ #include "software/ai/hl/stp/play/ball_placement/ball_placement_play_fsm.h" #include +#include #include "proto/parameters.pb.h" -#include "software/test_util/equal_within_tolerance.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,11 +39,85 @@ 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)); + + // TODO: expose it from the class or hardcode? + 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)); +} + +TEST(BallPlacementPlayFSMTest, test_align_wall) +{ + 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); + + // TODO: place robot at mock pickoff_point + + EXPECT_TRUE(fsm.is(boost::sml::state)); + + update(fsm, world_ptr, num_tactics); + + // TODO: pickoff_point is never assigned anywhere? + + 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)); }