Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

added play state to plan request #2151

Merged
merged 3 commits into from
Jan 23, 2024
Merged

added play state to plan request #2151

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
12ec9a8
added play state to plan request
petergarud Nov 20, 2023
aa49966
Fix style
sid-parikh Jan 22, 2024
7f26c8c
Fix Code Style On add-play-state-to-plan-request (#2166)
github-actions[bot] Jan 23, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 10 additions & 3 deletions soccer/src/soccer/planning/planner/plan_request.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@
#include <planning/trajectory.hpp>

#include "context.hpp"
#include "../global_state.hpp"
#include "planning/dynamic_obstacle.hpp"
#include "planning/instant.hpp"
#include "planning/robot_constraints.hpp"
Expand All @@ -28,7 +29,7 @@ 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,
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),
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,8 +110,7 @@ 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.
*/
float min_dist_from_ball = 0;
Expand All @@ -113,6 +119,7 @@ struct PlanRequest {
* Dribbler Speed
*/
float dribbler_speed = 0;

};

/**
Expand Down
1 change: 1 addition & 0 deletions soccer/src/soccer/planning/planner_for_robot.cpp
View file
Open in desktop
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
Open in desktop
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