Skip to content

Commit

Permalink
added play state to plan request (#2151)
Browse files Browse the repository at this point in the history 
* 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 <[email protected]>

---------

Co-authored-by: Sid Parikh <[email protected]>
Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>
Co-authored-by: sid-parikh <[email protected]>
  • Loading branch information
@petergarud @sid-parikh
@github-actions @sanatd33
4 people authored and sanatd33 committed Jan 29, 2024
1 parent 7d8bebe commit 26c7f7b
Show file tree
Hide file tree
Showing 3 changed files with 34 additions and 4 deletions.
14 changes: 10 additions & 4 deletions soccer/src/soccer/planning/planner/plan_request.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
#include <planning/planner/motion_command.hpp>
#include <planning/trajectory.hpp>

#include "../global_state.hpp"
#include "context.hpp"
#include "planning/dynamic_obstacle.hpp"
#include "planning/instant.hpp"
Expand All @@ -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),
Expand All @@ -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),
Expand Down Expand Up @@ -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.
Expand All @@ -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.
*/
Expand Down
1 change: 1 addition & 0 deletions soccer/src/soccer/planning/planner_for_robot.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -205,6 +205,7 @@ PlanRequest PlannerForRobot::make_request(const RobotIntent& intent) {
robot_trajectories_,
static_cast<unsigned int>(robot_id_),
world_state,
play_state,
intent.priority,
&debug_draw_,
had_break_beam_,
Expand Down
23 changes: 23 additions & 0 deletions soccer/src/soccer/planning/tests/planner_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@

#include <gtest/gtest.h>

#include "game_state.hpp"
#include "planning/instant.hpp"
#include "planning/planner/collect_path_planner.hpp"
#include "planning/planner/motion_command.hpp"
Expand Down Expand Up @@ -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{},
Expand All @@ -55,6 +57,7 @@ TEST(Planning, path_target_random) {
{},
0,
&world_state,
play_state,
2,
nullptr};
Trajectory path = planner.plan(std::move(request));
Expand Down Expand Up @@ -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{},
Expand All @@ -95,6 +99,7 @@ TEST(Planning, collect_basic) {
{},
0,
&world_state,
play_state,
2,
nullptr};
CollectPathPlanner planner;
Expand All @@ -109,6 +114,7 @@ TEST(Planning, collect_obstructed) {
world_state.ball.timestamp = RJ::now();
ShapeSet obstacles;
obstacles.add(std::make_shared<Circle>(Point{.5, .5}, .2));
PlayState play_state = PlayState::halt();
PlanRequest request{RobotInstant{{}, {}, RJ::now()},
MotionCommand{"collect"},
RobotConstraints{},
Expand All @@ -117,6 +123,7 @@ TEST(Planning, collect_obstructed) {
{},
0,
&world_state,
play_state,
2,
nullptr};
CollectPathPlanner planner;
Expand All @@ -134,6 +141,7 @@ TEST(Planning, collect_pointless_obs) {
obstacles.add(std::make_shared<Circle>(Point{3, 3}, .2));
obstacles.add(std::make_shared<Circle>(Point{-2, 3}, .2));
obstacles.add(std::make_shared<Circle>(Point{0, 5}, .2));
PlayState play_state = PlayState::halt();
PlanRequest request{RobotInstant{{}, {}, RJ::now()},
MotionCommand{"collect"},
RobotConstraints{},
Expand All @@ -142,6 +150,7 @@ TEST(Planning, collect_pointless_obs) {
{},
0,
&world_state,
play_state,
2,
nullptr};
CollectPathPlanner planner;
Expand All @@ -156,6 +165,7 @@ TEST(Planning, collect_moving_ball_quick) {
world_state.ball.timestamp = RJ::now();
ShapeSet obstacles;
obstacles.add(std::make_shared<Circle>(Point{0, .5}, .2));
PlayState play_state = PlayState::halt();
PlanRequest request{RobotInstant{{}, {}, RJ::now()},
MotionCommand{"collect"},
RobotConstraints{},
Expand All @@ -164,6 +174,7 @@ TEST(Planning, collect_moving_ball_quick) {
{},
0,
&world_state,
play_state,
2,
nullptr};
CollectPathPlanner planner;
Expand All @@ -178,6 +189,7 @@ TEST(Planning, collect_moving_ball_slow) {
world_state.ball.timestamp = RJ::now();
ShapeSet obstacles;
obstacles.add(std::make_shared<Circle>(Point{-0.5, .5}, .2));
PlayState play_state = PlayState::halt();
PlanRequest request{RobotInstant{{}, {}, RJ::now()},
MotionCommand{"collect"},
RobotConstraints{},
Expand All @@ -186,6 +198,7 @@ TEST(Planning, collect_moving_ball_slow) {
{},
0,
&world_state,
play_state,
2,
nullptr};
CollectPathPlanner planner;
Expand All @@ -200,6 +213,7 @@ TEST(Planning, collect_moving_ball_slow_2) {
world_state.ball.timestamp = RJ::now();
ShapeSet obstacles;
obstacles.add(std::make_shared<Circle>(Point{0, .5}, .2));
PlayState play_state = PlayState::halt();
PlanRequest request{RobotInstant{{}, {}, RJ::now()},
MotionCommand{"collect"},
RobotConstraints{},
Expand All @@ -208,6 +222,7 @@ TEST(Planning, collect_moving_ball_slow_2) {
{},
0,
&world_state,
play_state,
2,
nullptr};
CollectPathPlanner planner;
Expand All @@ -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{},
Expand All @@ -242,6 +258,7 @@ TEST(Planning, collect_random) {
{},
0,
&world_state,
play_state,
2,
nullptr};
CollectPathPlanner planner;
Expand All @@ -265,6 +282,7 @@ TEST(Planning, settle_basic) {
world_state.ball.timestamp = RJ::now();
ShapeSet obstacles;
obstacles.add(std::make_shared<Circle>(Point{.5, .5}, .2));
PlayState play_state = PlayState::halt();
PlanRequest request{RobotInstant{{}, {}, RJ::now()},
MotionCommand{"settle"},
RobotConstraints{},
Expand All @@ -273,6 +291,7 @@ TEST(Planning, settle_basic) {
{},
0,
&world_state,
play_state,
2,
nullptr};
SettlePathPlanner planner;
Expand All @@ -289,6 +308,7 @@ TEST(Planning, settle_pointless_obs) {
world_state.ball.timestamp = RJ::now();
ShapeSet obstacles;
obstacles.add(std::make_shared<Circle>(Point{-1, 1.0}, .2));
PlayState play_state = PlayState::halt();
PlanRequest request{RobotInstant{{}, {}, RJ::now()},
MotionCommand{"settle"},
RobotConstraints{},
Expand All @@ -297,6 +317,7 @@ TEST(Planning, settle_pointless_obs) {
{},
0,
&world_state,
play_state,
2,
nullptr};
SettlePathPlanner planner;
Expand Down Expand Up @@ -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{},
Expand All @@ -332,6 +354,7 @@ TEST(Planning, settle_random) {
{},
0,
&world_state,
play_state,
2,
nullptr};
SettlePathPlanner planner;
Expand Down

0 comments on commit 26c7f7b

Please sign in to comment.