From 1788787ae8b442eb471067bf6e571b596f501e06 Mon Sep 17 00:00:00 2001 From: Sanat Dhanyamraju <53443682+sanatd33@users.noreply.github.com> Date: Sun, 10 Nov 2024 21:28:40 -0500 Subject: [PATCH] Valid intermediate Points (#2307) * enforce points to be in range by giving field dimensions * Fix Code Style On valid-intermediate-points (#2308) automated style fixes Co-authored-by: sanatd33 * pass a reference --------- Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> Co-authored-by: sanatd33 --- soccer/src/soccer/planning/global_state.cpp | 4 ++ soccer/src/soccer/planning/global_state.hpp | 1 + .../planning/planner/collect_path_planner.cpp | 24 ++++++++---- .../planner/goalie_idle_path_planner.cpp | 3 +- .../planner/path_target_path_planner.cpp | 3 +- .../soccer/planning/planner/plan_request.hpp | 11 +++++- .../planning/planner/settle_path_planner.cpp | 33 ++++++++++------ .../src/soccer/planning/planner_for_robot.cpp | 1 + .../planning/primitives/create_path.cpp | 38 +++++++++++++------ .../planning/primitives/create_path.hpp | 8 +++- .../soccer/planning/primitives/replanner.cpp | 13 ++++--- .../soccer/planning/primitives/replanner.hpp | 3 ++ .../soccer/planning/tests/planner_test.cpp | 23 +++++++++++ 13 files changed, 125 insertions(+), 40 deletions(-) diff --git a/soccer/src/soccer/planning/global_state.cpp b/soccer/src/soccer/planning/global_state.cpp index e89a4acc979..663aa1e17c7 100644 --- a/soccer/src/soccer/planning/global_state.cpp +++ b/soccer/src/soccer/planning/global_state.cpp @@ -74,6 +74,10 @@ GlobalState::GlobalState(rclcpp::Node* node) { auto lock = std::lock_guard(last_world_state_mutex_); return &last_world_state_; } +[[nodiscard]] const FieldDimensions* GlobalState::field_dimensions() const { + auto lock = std::lock_guard(last_field_dimensions_mutex_); + return &last_field_dimensions_; +} rj_geometry::ShapeSet GlobalState::create_defense_area_obstacles() { // need field dimensions and to be initialized for this to diff --git a/soccer/src/soccer/planning/global_state.hpp b/soccer/src/soccer/planning/global_state.hpp index b5aac730ba2..ddb380f9af3 100644 --- a/soccer/src/soccer/planning/global_state.hpp +++ b/soccer/src/soccer/planning/global_state.hpp @@ -34,6 +34,7 @@ class GlobalState { [[nodiscard]] rj_geometry::ShapeSet global_obstacles() const; [[nodiscard]] rj_geometry::ShapeSet def_area_obstacles() const; [[nodiscard]] const WorldState* world_state() const; + [[nodiscard]] const FieldDimensions* field_dimensions() const; private: rclcpp::Subscription::SharedPtr play_state_sub_; diff --git a/soccer/src/soccer/planning/planner/collect_path_planner.cpp b/soccer/src/soccer/planning/planner/collect_path_planner.cpp index 1d98bca5acc..cd42ccdfe7f 100644 --- a/soccer/src/soccer/planning/planner/collect_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/collect_path_planner.cpp @@ -224,8 +224,10 @@ Trajectory CollectPathPlanner::coarse_approach( target_slow, static_obstacles, dynamic_obstacles, + plan_request.field_dimensions, plan_request.constraints, - AngleFns::face_point(ball.position)}; + AngleFns::face_point(ball.position), + plan_request.shell_id}; Trajectory coarse_path = Replanner::create_plan(params, previous_); if (plan_request.debug_drawer != nullptr) { @@ -282,8 +284,10 @@ Trajectory CollectPathPlanner::fine_approach( target_hit, static_obstacles, dynamic_obstacles, + plan_request.field_dimensions, plan_request.constraints, - AngleFns::face_point(ball.position)}; + AngleFns::face_point(ball.position), + plan_request.shell_id}; Trajectory path_hit = Replanner::create_plan(params, previous_); path_hit.set_debug_text("fine"); @@ -376,8 +380,10 @@ Trajectory CollectPathPlanner::control(const PlanRequest& plan_request, RobotIns target, static_obstacles, dynamic_obstacles, + plan_request.field_dimensions, plan_request.constraints, - AngleFns::face_point(ball.position)}; + AngleFns::face_point(ball.position), + plan_request.shell_id}; Trajectory path = Replanner::create_plan(params, previous_); @@ -419,10 +425,14 @@ Trajectory CollectPathPlanner::invalid(const PlanRequest& plan_request, // programmatically LinearMotionInstant target{plan_request.start.position(), Point()}; - Replanner::PlanParams params{ - plan_request.start, target, - static_obstacles, dynamic_obstacles, - plan_request.constraints, AngleFns::face_point(plan_request.world_state->ball.position)}; + Replanner::PlanParams params{plan_request.start, + target, + static_obstacles, + dynamic_obstacles, + plan_request.field_dimensions, + plan_request.constraints, + AngleFns::face_point(plan_request.world_state->ball.position), + plan_request.shell_id}; Trajectory path = Replanner::create_plan(params, previous_); path.set_debug_text("Invalid state in collect"); diff --git a/soccer/src/soccer/planning/planner/goalie_idle_path_planner.cpp b/soccer/src/soccer/planning/planner/goalie_idle_path_planner.cpp index 33fe917b7ad..a16f84f3912 100644 --- a/soccer/src/soccer/planning/planner/goalie_idle_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/goalie_idle_path_planner.cpp @@ -31,7 +31,8 @@ Trajectory GoalieIdlePathPlanner::plan(const PlanRequest& plan_request) { // call Replanner to generate a Trajectory Trajectory trajectory = Replanner::create_plan( Replanner::PlanParams{plan_request.start, target, static_obstacles, dynamic_obstacles, - plan_request.constraints, angle_function, RJ::Seconds(3.0)}, + plan_request.field_dimensions, plan_request.constraints, + angle_function, plan_request.shell_id, RJ::Seconds(3.0)}, std::move(previous_)); // Debug drawing diff --git a/soccer/src/soccer/planning/planner/path_target_path_planner.cpp b/soccer/src/soccer/planning/planner/path_target_path_planner.cpp index 1a4e7b6e7c4..f19fbbb9f8c 100644 --- a/soccer/src/soccer/planning/planner/path_target_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/path_target_path_planner.cpp @@ -41,7 +41,8 @@ Trajectory PathTargetPathPlanner::plan(const PlanRequest& request) { // Call into the sub-object to actually execute the plan. Trajectory trajectory = Replanner::create_plan( Replanner::PlanParams{request.start, target_instant, static_obstacles, dynamic_obstacles, - request.constraints, angle_function, RJ::Seconds(3.0)}, + request.field_dimensions, request.constraints, angle_function, + request.shell_id, RJ::Seconds(3.0)}, std::move(previous_)); previous_ = trajectory; diff --git a/soccer/src/soccer/planning/planner/plan_request.hpp b/soccer/src/soccer/planning/planner/plan_request.hpp index db0d566a7c5..307943d94ed 100644 --- a/soccer/src/soccer/planning/planner/plan_request.hpp +++ b/soccer/src/soccer/planning/planner/plan_request.hpp @@ -30,8 +30,9 @@ struct PlanRequest { RobotConstraints constraints, rj_geometry::ShapeSet field_obstacles, rj_geometry::ShapeSet virtual_obstacles, TrajectoryCollection* planned_trajectories, 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) + const FieldDimensions* field_dimensions, 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), @@ -42,6 +43,7 @@ struct PlanRequest { world_state(world_state), priority(priority), play_state(play_state), + field_dimensions(field_dimensions), debug_drawer(debug_drawer), ball_sense(ball_sense), min_dist_from_ball(min_dist_from_ball), @@ -102,6 +104,11 @@ struct PlanRequest { */ PlayState play_state; + /** + * the Field Dimensions + */ + const FieldDimensions* field_dimensions; + /** * Allows debug drawing in the world. If this is nullptr, no debug drawing * should be performed. diff --git a/soccer/src/soccer/planning/planner/settle_path_planner.cpp b/soccer/src/soccer/planning/planner/settle_path_planner.cpp index 6e4cf03258f..04c78245030 100644 --- a/soccer/src/soccer/planning/planner/settle_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/settle_path_planner.cpp @@ -223,7 +223,8 @@ Trajectory SettlePathPlanner::intercept(const PlanRequest& plan_request, RobotIn // test location Trajectory path = CreatePath::intermediate( start_instant.linear_motion(), target_robot_intersection, plan_request.constraints.mot, - start_instant.stamp, static_obstacles); + start_instant.stamp, static_obstacles, dynamic_obstacles, plan_request.field_dimensions, + plan_request.shell_id); // Calculate the RJ::Seconds buffer_duration = ball_time - path.duration(); @@ -332,9 +333,10 @@ Trajectory SettlePathPlanner::intercept(const PlanRequest& plan_request, RobotIn LinearMotionInstant target{closest_pt, settle::PARAM_ball_speed_percent_for_dampen * average_ball_vel_}; - Trajectory shortcut = CreatePath::intermediate(start_instant.linear_motion(), target, - plan_request.constraints.mot, - start_instant.stamp, static_obstacles); + Trajectory shortcut = CreatePath::intermediate( + start_instant.linear_motion(), target, plan_request.constraints.mot, + start_instant.stamp, static_obstacles, dynamic_obstacles, plan_request.field_dimensions, + plan_request.shell_id); if (!shortcut.empty()) { plan_angles(&shortcut, start_instant, AngleFns::face_point(face_pos), @@ -361,9 +363,14 @@ Trajectory SettlePathPlanner::intercept(const PlanRequest& plan_request, RobotIn LinearMotionInstant target_robot_intersection{ path_intercept_target_, settle::PARAM_ball_speed_percent_for_dampen * average_ball_vel_}; - Replanner::PlanParams params{ - start_instant, target_robot_intersection, static_obstacles, - dynamic_obstacles, plan_request.constraints, AngleFns::face_point(face_pos)}; + Replanner::PlanParams params{start_instant, + target_robot_intersection, + static_obstacles, + dynamic_obstacles, + plan_request.field_dimensions, + plan_request.constraints, + AngleFns::face_point(face_pos), + plan_request.shell_id}; Trajectory new_target_path = Replanner::create_plan(params, previous_); RJ::Seconds time_of_arrival = new_target_path.duration(); @@ -485,10 +492,14 @@ Trajectory SettlePathPlanner::invalid(const PlanRequest& plan_request, // programmatically LinearMotionInstant target{plan_request.start.position(), Point()}; - Replanner::PlanParams params{ - plan_request.start, target, - static_obstacles, dynamic_obstacles, - plan_request.constraints, AngleFns::face_point(plan_request.world_state->ball.position)}; + Replanner::PlanParams params{plan_request.start, + target, + static_obstacles, + dynamic_obstacles, + plan_request.field_dimensions, + plan_request.constraints, + AngleFns::face_point(plan_request.world_state->ball.position), + plan_request.shell_id}; Trajectory path = Replanner::create_plan(params, previous_); path.set_debug_text("Invalid state in settle"); return path; diff --git a/soccer/src/soccer/planning/planner_for_robot.cpp b/soccer/src/soccer/planning/planner_for_robot.cpp index 7a302773c2f..272945df268 100644 --- a/soccer/src/soccer/planning/planner_for_robot.cpp +++ b/soccer/src/soccer/planning/planner_for_robot.cpp @@ -217,6 +217,7 @@ PlanRequest PlannerForRobot::make_request(const RobotIntent& intent) { static_cast(robot_id_), world_state, play_state, + global_state_.field_dimensions(), intent.priority, &debug_draw_, had_break_beam_, diff --git a/soccer/src/soccer/planning/primitives/create_path.cpp b/soccer/src/soccer/planning/primitives/create_path.cpp index d510483a0ad..c37c901887e 100644 --- a/soccer/src/soccer/planning/primitives/create_path.cpp +++ b/soccer/src/soccer/planning/primitives/create_path.cpp @@ -75,11 +75,13 @@ Trajectory rrt(const LinearMotionInstant& start, const LinearMotionInstant& goal return path; } -static std::optional> cached_intermediate_tuple_{}; +static std::unordered_map> cached_intermediate_tuple_{}; Trajectory intermediate(const LinearMotionInstant& start, const LinearMotionInstant& goal, const MotionConstraints& motion_constraints, RJ::Time start_time, - const rj_geometry::ShapeSet& static_obstacles) { + const rj_geometry::ShapeSet& static_obstacles, + const std::vector& dynamic_obstacles, + const FieldDimensions* field_dimensions, unsigned int robot_id) { // if already on goal, no need to move if (start.position.dist_to(goal.position) < 1e-6) { return Trajectory{{RobotInstant{Pose(start.position, 0), Twist(), start_time}}}; @@ -97,7 +99,7 @@ Trajectory intermediate(const LinearMotionInstant& start, const LinearMotionInst } // Generate list of intermediate points - std::vector intermediates = get_intermediates(start, goal); + std::vector intermediates = get_intermediates(start, goal, robot_id); for (int i = 0; i < intermediate::PARAM_num_intermediates; i++) { rj_geometry::Point final_inter = intermediates[i]; @@ -108,25 +110,38 @@ Trajectory intermediate(const LinearMotionInstant& start, const LinearMotionInst t += intermediate::PARAM_step_size) { rj_geometry::Point intermediate = (final_inter - start.position).normalized(t) + start.position; + + auto offset = intermediate - field_dimensions->center_point(); + + // Ignore out-of-bounds intermediate points + // The offset 0.2m is chosen because the sim prevents you from moving + // more than 0.2m away from the border lines + if (abs(offset.x()) > field_dimensions->width() / 2 + 0.2 || + abs(offset.y()) > field_dimensions->length() / 2 + 0.2) { + continue; + } + Trajectory trajectory = CreatePath::simple(start, goal, motion_constraints, start_time, {intermediate}); // If the trajectory does not hit an obstacle, it is valid if ((!trajectory_hits_static(trajectory, static_obstacles, start_time, nullptr))) { auto angle = (final_inter - start.position).angle(); - cached_intermediate_tuple_ = {abs(angle), signbit(angle) ? -1 : 1, - (final_inter - start.position).mag()}; + cached_intermediate_tuple_[robot_id] = { + abs(angle), (final_inter - start.position).mag(), signbit(angle) ? -1 : 1}; return trajectory; } } } - // If all else fails, return the straight-line trajectory - return straight_trajectory; + // If all else fails, use rrt to ensure obstacle avoidance + return CreatePath::rrt(start, goal, motion_constraints, start_time, static_obstacles, + dynamic_obstacles); } std::vector get_intermediates(const LinearMotionInstant& start, - const LinearMotionInstant& goal) { + const LinearMotionInstant& goal, + unsigned int robot_id) { std::random_device rd; std::mt19937 gen(rd()); // Create a random distribution for the distance between the start @@ -152,15 +167,16 @@ std::vector get_intermediates(const LinearMotionInstant& sta inter_tuples.emplace_back(abs(angle), signbit(angle) ? -1 : 1, scale); } - if (cached_intermediate_tuple_) { - inter_tuples.push_back(*cached_intermediate_tuple_); - } // Sort the list of tuples by the magnitude of angle // This ensures that we take paths with // smaller offsets from the simple path sort(inter_tuples.begin(), inter_tuples.end()); + if (cached_intermediate_tuple_.find(robot_id) != cached_intermediate_tuple_.end()) { + inter_tuples.emplace(inter_tuples.begin(), cached_intermediate_tuple_[robot_id]); + } + for (int i = 0; i < intermediate::PARAM_num_intermediates; i++) { double angle = std::get<0>(inter_tuples[i]) * std::get<1>(inter_tuples[i]); double scale = std::get<2>(inter_tuples[i]); diff --git a/soccer/src/soccer/planning/primitives/create_path.hpp b/soccer/src/soccer/planning/primitives/create_path.hpp index d71cf5809f8..6921cb7bbed 100644 --- a/soccer/src/soccer/planning/primitives/create_path.hpp +++ b/soccer/src/soccer/planning/primitives/create_path.hpp @@ -6,6 +6,7 @@ #include "planning/motion_constraints.hpp" #include "planning/primitives/path_smoothing.hpp" #include "planning/trajectory.hpp" +#include "rj_common/field_dimensions.hpp" namespace planning::CreatePath { @@ -29,8 +30,11 @@ Trajectory simple( Trajectory intermediate(const LinearMotionInstant& start, const LinearMotionInstant& goal, const MotionConstraints& motion_constraints, RJ::Time start_time, - const rj_geometry::ShapeSet& static_obstacles); + const rj_geometry::ShapeSet& static_obstacles, + const std::vector& dynamic_obstacles, + const FieldDimensions* field_dimensions, unsigned int robot_id); std::vector get_intermediates(const LinearMotionInstant& start, - const LinearMotionInstant& goal); + const LinearMotionInstant& goal, + unsigned int robot_id); } // namespace planning::CreatePath \ No newline at end of file diff --git a/soccer/src/soccer/planning/primitives/replanner.cpp b/soccer/src/soccer/planning/primitives/replanner.cpp index 3c5725f5047..fc790bf7e35 100644 --- a/soccer/src/soccer/planning/primitives/replanner.cpp +++ b/soccer/src/soccer/planning/primitives/replanner.cpp @@ -33,7 +33,8 @@ Trajectory Replanner::partial_replan(const PlanParams& params, const Trajectory& Trajectory pre_trajectory = partial_path(previous, params.start.stamp); Trajectory post_trajectory = CreatePath::intermediate( pre_trajectory.last().linear_motion(), params.goal, params.constraints.mot, - pre_trajectory.end_time(), params.static_obstacles); + pre_trajectory.end_time(), params.static_obstacles, params.dynamic_obstacles, + params.field_dimensions, params.robot_id); // If we couldn't profile such that velocity at the end of the partial replan period is valid, // do a full replan. @@ -55,9 +56,10 @@ Trajectory Replanner::partial_replan(const PlanParams& params, const Trajectory& } Trajectory Replanner::full_replan(const Replanner::PlanParams& params) { - Trajectory path = - CreatePath::intermediate(params.start.linear_motion(), params.goal, params.constraints.mot, - params.start.stamp, params.static_obstacles); + Trajectory path = CreatePath::intermediate(params.start.linear_motion(), params.goal, + params.constraints.mot, params.start.stamp, + params.static_obstacles, params.dynamic_obstacles, + params.field_dimensions, params.robot_id); // if the initial path is empty, the goal must be blocked // try to shift the goal_point until it is no longer blocked @@ -77,7 +79,8 @@ Trajectory Replanner::full_replan(const Replanner::PlanParams& params) { path = CreatePath::intermediate(params.start.linear_motion(), almost_goal, params.constraints.mot, params.start.stamp, - params.static_obstacles); + params.static_obstacles, params.dynamic_obstacles, + params.field_dimensions, params.robot_id); } if (!path.empty()) { diff --git a/soccer/src/soccer/planning/primitives/replanner.hpp b/soccer/src/soccer/planning/primitives/replanner.hpp index 89d07984545..60df404f93b 100644 --- a/soccer/src/soccer/planning/primitives/replanner.hpp +++ b/soccer/src/soccer/planning/primitives/replanner.hpp @@ -7,6 +7,7 @@ #include "planning/primitives/angle_planning.hpp" #include "planning/robot_constraints.hpp" #include "planning/trajectory.hpp" +#include "rj_common/field_dimensions.hpp" #include "velocity_profiling.hpp" namespace planning { @@ -27,8 +28,10 @@ class Replanner { LinearMotionInstant goal; const rj_geometry::ShapeSet& static_obstacles; const std::vector& dynamic_obstacles; + const FieldDimensions* field_dimensions; RobotConstraints constraints; const AngleFunction& angle_function; + unsigned int robot_id; std::optional hold_time = std::nullopt; }; diff --git a/soccer/src/soccer/planning/tests/planner_test.cpp b/soccer/src/soccer/planning/tests/planner_test.cpp index 26fa3d07ace..42fcd170128 100644 --- a/soccer/src/soccer/planning/tests/planner_test.cpp +++ b/soccer/src/soccer/planning/tests/planner_test.cpp @@ -13,6 +13,7 @@ #include "planning/primitives/rrt_util.hpp" #include "planning/tests/testing_utils.hpp" #include "planning/trajectory.hpp" +#include "rj_common/field_dimensions.hpp" #include "rj_geometry/pose.hpp" /* @@ -29,6 +30,7 @@ TEST(Planning, path_target_random) { WorldState world_state; PathTargetPathPlanner planner; + FieldDimensions* field_dimensions = &FieldDimensions::current_dimensions; int failure_count = 0; for (int i = 0; i < 1000; i++) { @@ -58,6 +60,7 @@ TEST(Planning, path_target_random) { 0, &world_state, play_state, + field_dimensions, 2, nullptr}; Trajectory path = planner.plan(std::move(request)); @@ -87,6 +90,7 @@ TEST(Planning, path_target_random) { TEST(Planning, collect_basic) { WorldState world_state; + FieldDimensions* field_dimensions = &FieldDimensions::current_dimensions; world_state.ball.position = Point{1, 1}; world_state.ball.velocity = Point{0, 0}; world_state.ball.timestamp = RJ::now(); @@ -100,6 +104,7 @@ TEST(Planning, collect_basic) { 0, &world_state, play_state, + field_dimensions, 2, nullptr}; CollectPathPlanner planner; @@ -109,6 +114,7 @@ TEST(Planning, collect_basic) { TEST(Planning, collect_obstructed) { WorldState world_state; + FieldDimensions* field_dimensions = &FieldDimensions::current_dimensions; world_state.ball.position = Point{1, 1}; world_state.ball.velocity = Point{0, 0}; world_state.ball.timestamp = RJ::now(); @@ -124,6 +130,7 @@ TEST(Planning, collect_obstructed) { 0, &world_state, play_state, + field_dimensions, 2, nullptr}; CollectPathPlanner planner; @@ -133,6 +140,7 @@ TEST(Planning, collect_obstructed) { TEST(Planning, collect_pointless_obs) { WorldState world_state; + FieldDimensions* field_dimensions = &FieldDimensions::current_dimensions; world_state.ball.position = Point{1, 1}; world_state.ball.velocity = Point{0, 0}; world_state.ball.timestamp = RJ::now(); @@ -151,6 +159,7 @@ TEST(Planning, collect_pointless_obs) { 0, &world_state, play_state, + field_dimensions, 2, nullptr}; CollectPathPlanner planner; @@ -160,6 +169,7 @@ TEST(Planning, collect_pointless_obs) { TEST(Planning, collect_moving_ball_quick) { WorldState world_state; + FieldDimensions* field_dimensions = &FieldDimensions::current_dimensions; world_state.ball.position = Point{-1, 1}; world_state.ball.velocity = Point{-0.03, 0.3}; world_state.ball.timestamp = RJ::now(); @@ -175,6 +185,7 @@ TEST(Planning, collect_moving_ball_quick) { 0, &world_state, play_state, + field_dimensions, 2, nullptr}; CollectPathPlanner planner; @@ -184,6 +195,7 @@ TEST(Planning, collect_moving_ball_quick) { TEST(Planning, collect_moving_ball_slow) { WorldState world_state; + FieldDimensions* field_dimensions = &FieldDimensions::current_dimensions; world_state.ball.position = Point{-1, 1}; world_state.ball.velocity = Point{0, 0.1}; world_state.ball.timestamp = RJ::now(); @@ -199,6 +211,7 @@ TEST(Planning, collect_moving_ball_slow) { 0, &world_state, play_state, + field_dimensions, 2, nullptr}; CollectPathPlanner planner; @@ -208,6 +221,7 @@ TEST(Planning, collect_moving_ball_slow) { TEST(Planning, collect_moving_ball_slow_2) { WorldState world_state; + FieldDimensions* field_dimensions = &FieldDimensions::current_dimensions; world_state.ball.position = Point{-1, 1}; world_state.ball.velocity = Point{0.01, 0.05}; world_state.ball.timestamp = RJ::now(); @@ -223,6 +237,7 @@ TEST(Planning, collect_moving_ball_slow_2) { 0, &world_state, play_state, + field_dimensions, 2, nullptr}; CollectPathPlanner planner; @@ -233,6 +248,7 @@ TEST(Planning, collect_moving_ball_slow_2) { TEST(Planning, collect_random) { std::mt19937 gen(1337); WorldState world_state; + FieldDimensions* field_dimensions = &FieldDimensions::current_dimensions; int failure_count = 0; @@ -260,6 +276,7 @@ TEST(Planning, collect_random) { 0, &world_state, play_state, + field_dimensions, 2, nullptr}; CollectPathPlanner planner; @@ -278,6 +295,7 @@ TEST(Planning, collect_random) { TEST(Planning, settle_basic) { WorldState world_state; + FieldDimensions* field_dimensions = &FieldDimensions::current_dimensions; world_state.ball.position = Point{1, 1}; world_state.ball.velocity = Point{-1, -1.5}; world_state.ball.timestamp = RJ::now(); @@ -293,6 +311,7 @@ TEST(Planning, settle_basic) { 0, &world_state, play_state, + field_dimensions, 2, nullptr}; SettlePathPlanner planner; @@ -302,6 +321,7 @@ TEST(Planning, settle_basic) { TEST(Planning, settle_pointless_obs) { WorldState world_state; + FieldDimensions* field_dimensions = &FieldDimensions::current_dimensions; // Use some initial velocity, settle doesn't always work for non-moving // balls world_state.ball.position = Point{1, 3}; @@ -319,6 +339,7 @@ TEST(Planning, settle_pointless_obs) { 0, &world_state, play_state, + field_dimensions, 2, nullptr}; SettlePathPlanner planner; @@ -330,6 +351,7 @@ TEST(Planning, settle_pointless_obs) { TEST(Planning, settle_random) { std::mt19937 gen(1337); WorldState world_state; + FieldDimensions* field_dimensions = &FieldDimensions::current_dimensions; int failure_count = 0; @@ -357,6 +379,7 @@ TEST(Planning, settle_random) { 0, &world_state, play_state, + field_dimensions, 2, nullptr}; SettlePathPlanner planner;