From 26c7f7b51cbed6b26e648ce63b224548a5a570f2 Mon Sep 17 00:00:00 2001 From: Peter Garud <79226125+petergarud@users.noreply.github.com> Date: Tue, 23 Jan 2024 12:09:50 -0500 Subject: [PATCH] added play state to plan request (#2151) * added play state to plan request * Fix style * Fix Code Style On add-play-state-to-plan-request (#2166) automated style fixes Co-authored-by: sid-parikh --------- Co-authored-by: Sid Parikh Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> Co-authored-by: sid-parikh --- .../soccer/planning/planner/plan_request.hpp | 14 +++++++---- .../src/soccer/planning/planner_for_robot.cpp | 1 + .../soccer/planning/tests/planner_test.cpp | 23 +++++++++++++++++++ 3 files changed, 34 insertions(+), 4 deletions(-) diff --git a/soccer/src/soccer/planning/planner/plan_request.hpp b/soccer/src/soccer/planning/planner/plan_request.hpp index 532ab073f92..a96f207ed5f 100644 --- a/soccer/src/soccer/planning/planner/plan_request.hpp +++ b/soccer/src/soccer/planning/planner/plan_request.hpp @@ -8,6 +8,7 @@ #include #include +#include "../global_state.hpp" #include "context.hpp" #include "planning/dynamic_obstacle.hpp" #include "planning/instant.hpp" @@ -28,9 +29,9 @@ struct PlanRequest { PlanRequest(RobotInstant start, MotionCommand command, // NOLINT RobotConstraints constraints, rj_geometry::ShapeSet field_obstacles, rj_geometry::ShapeSet virtual_obstacles, TrajectoryCollection* planned_trajectories, - unsigned shell_id, const WorldState* world_state, int8_t priority = 0, - rj_drawing::RosDebugDrawer* debug_drawer = nullptr, bool ball_sense = false, - float min_dist_from_ball = 0, float dribbler_speed = 0) + unsigned shell_id, const WorldState* world_state, PlayState play_state, + int8_t priority = 0, rj_drawing::RosDebugDrawer* debug_drawer = nullptr, + bool ball_sense = false, float min_dist_from_ball = 0, float dribbler_speed = 0) : start(start), motion_command(command), // NOLINT constraints(constraints), @@ -40,6 +41,7 @@ struct PlanRequest { shell_id(shell_id), priority(priority), world_state(world_state), + play_state(play_state), debug_drawer(debug_drawer), ball_sense(ball_sense), min_dist_from_ball(min_dist_from_ball), @@ -95,6 +97,11 @@ struct PlanRequest { */ int8_t priority; + /** + * the current PlayState + */ + PlayState play_state; + /** * Allows debug drawing in the world. If this is nullptr, no debug drawing * should be performed. @@ -103,7 +110,6 @@ struct PlanRequest { // Whether the robot has a ball bool ball_sense = false; - /** * How far away to stay from the ball, if the MotionCommand chooses to avoid the ball. */ diff --git a/soccer/src/soccer/planning/planner_for_robot.cpp b/soccer/src/soccer/planning/planner_for_robot.cpp index 18914483dc7..e96789561e1 100644 --- a/soccer/src/soccer/planning/planner_for_robot.cpp +++ b/soccer/src/soccer/planning/planner_for_robot.cpp @@ -205,6 +205,7 @@ PlanRequest PlannerForRobot::make_request(const RobotIntent& intent) { robot_trajectories_, static_cast(robot_id_), world_state, + play_state, intent.priority, &debug_draw_, had_break_beam_, diff --git a/soccer/src/soccer/planning/tests/planner_test.cpp b/soccer/src/soccer/planning/tests/planner_test.cpp index dd0d14fd826..838e264b5ea 100644 --- a/soccer/src/soccer/planning/tests/planner_test.cpp +++ b/soccer/src/soccer/planning/tests/planner_test.cpp @@ -2,6 +2,7 @@ #include +#include "game_state.hpp" #include "planning/instant.hpp" #include "planning/planner/collect_path_planner.hpp" #include "planning/planner/motion_command.hpp" @@ -47,6 +48,7 @@ TEST(Planning, path_target_random) { } LinearMotionInstant goal = random_instant(&gen).linear_motion(); + PlayState play_state = PlayState::halt(); PlanRequest request{start, MotionCommand{"path_target", goal}, RobotConstraints{}, @@ -55,6 +57,7 @@ TEST(Planning, path_target_random) { {}, 0, &world_state, + play_state, 2, nullptr}; Trajectory path = planner.plan(std::move(request)); @@ -87,6 +90,7 @@ TEST(Planning, collect_basic) { world_state.ball.position = Point{1, 1}; world_state.ball.velocity = Point{0, 0}; world_state.ball.timestamp = RJ::now(); + PlayState play_state = PlayState::halt(); PlanRequest request{RobotInstant{{}, {}, RJ::now()}, MotionCommand{"collect"}, RobotConstraints{}, @@ -95,6 +99,7 @@ TEST(Planning, collect_basic) { {}, 0, &world_state, + play_state, 2, nullptr}; CollectPathPlanner planner; @@ -109,6 +114,7 @@ TEST(Planning, collect_obstructed) { world_state.ball.timestamp = RJ::now(); ShapeSet obstacles; obstacles.add(std::make_shared(Point{.5, .5}, .2)); + PlayState play_state = PlayState::halt(); PlanRequest request{RobotInstant{{}, {}, RJ::now()}, MotionCommand{"collect"}, RobotConstraints{}, @@ -117,6 +123,7 @@ TEST(Planning, collect_obstructed) { {}, 0, &world_state, + play_state, 2, nullptr}; CollectPathPlanner planner; @@ -134,6 +141,7 @@ TEST(Planning, collect_pointless_obs) { obstacles.add(std::make_shared(Point{3, 3}, .2)); obstacles.add(std::make_shared(Point{-2, 3}, .2)); obstacles.add(std::make_shared(Point{0, 5}, .2)); + PlayState play_state = PlayState::halt(); PlanRequest request{RobotInstant{{}, {}, RJ::now()}, MotionCommand{"collect"}, RobotConstraints{}, @@ -142,6 +150,7 @@ TEST(Planning, collect_pointless_obs) { {}, 0, &world_state, + play_state, 2, nullptr}; CollectPathPlanner planner; @@ -156,6 +165,7 @@ TEST(Planning, collect_moving_ball_quick) { world_state.ball.timestamp = RJ::now(); ShapeSet obstacles; obstacles.add(std::make_shared(Point{0, .5}, .2)); + PlayState play_state = PlayState::halt(); PlanRequest request{RobotInstant{{}, {}, RJ::now()}, MotionCommand{"collect"}, RobotConstraints{}, @@ -164,6 +174,7 @@ TEST(Planning, collect_moving_ball_quick) { {}, 0, &world_state, + play_state, 2, nullptr}; CollectPathPlanner planner; @@ -178,6 +189,7 @@ TEST(Planning, collect_moving_ball_slow) { world_state.ball.timestamp = RJ::now(); ShapeSet obstacles; obstacles.add(std::make_shared(Point{-0.5, .5}, .2)); + PlayState play_state = PlayState::halt(); PlanRequest request{RobotInstant{{}, {}, RJ::now()}, MotionCommand{"collect"}, RobotConstraints{}, @@ -186,6 +198,7 @@ TEST(Planning, collect_moving_ball_slow) { {}, 0, &world_state, + play_state, 2, nullptr}; CollectPathPlanner planner; @@ -200,6 +213,7 @@ TEST(Planning, collect_moving_ball_slow_2) { world_state.ball.timestamp = RJ::now(); ShapeSet obstacles; obstacles.add(std::make_shared(Point{0, .5}, .2)); + PlayState play_state = PlayState::halt(); PlanRequest request{RobotInstant{{}, {}, RJ::now()}, MotionCommand{"collect"}, RobotConstraints{}, @@ -208,6 +222,7 @@ TEST(Planning, collect_moving_ball_slow_2) { {}, 0, &world_state, + play_state, 2, nullptr}; CollectPathPlanner planner; @@ -234,6 +249,7 @@ TEST(Planning, collect_random) { Point{TestingUtils::random(&gen, -2.0, 2.0), TestingUtils::random(&gen, 0.5, 1.5)}, .2)); } + PlayState play_state = PlayState::halt(); PlanRequest request{RobotInstant{{}, {}, RJ::now()}, MotionCommand{"collect"}, RobotConstraints{}, @@ -242,6 +258,7 @@ TEST(Planning, collect_random) { {}, 0, &world_state, + play_state, 2, nullptr}; CollectPathPlanner planner; @@ -265,6 +282,7 @@ TEST(Planning, settle_basic) { world_state.ball.timestamp = RJ::now(); ShapeSet obstacles; obstacles.add(std::make_shared(Point{.5, .5}, .2)); + PlayState play_state = PlayState::halt(); PlanRequest request{RobotInstant{{}, {}, RJ::now()}, MotionCommand{"settle"}, RobotConstraints{}, @@ -273,6 +291,7 @@ TEST(Planning, settle_basic) { {}, 0, &world_state, + play_state, 2, nullptr}; SettlePathPlanner planner; @@ -289,6 +308,7 @@ TEST(Planning, settle_pointless_obs) { world_state.ball.timestamp = RJ::now(); ShapeSet obstacles; obstacles.add(std::make_shared(Point{-1, 1.0}, .2)); + PlayState play_state = PlayState::halt(); PlanRequest request{RobotInstant{{}, {}, RJ::now()}, MotionCommand{"settle"}, RobotConstraints{}, @@ -297,6 +317,7 @@ TEST(Planning, settle_pointless_obs) { {}, 0, &world_state, + play_state, 2, nullptr}; SettlePathPlanner planner; @@ -324,6 +345,7 @@ TEST(Planning, settle_random) { Point{TestingUtils::random(&gen, -2.0, 2.0), TestingUtils::random(&gen, .5, 1.5)}, .2)); } + PlayState play_state = PlayState::halt(); PlanRequest request{RobotInstant{{}, {}, RJ::now()}, MotionCommand{"settle"}, RobotConstraints{}, @@ -332,6 +354,7 @@ TEST(Planning, settle_random) { {}, 0, &world_state, + play_state, 2, nullptr}; SettlePathPlanner planner;