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

Fix Code Style On valid-intermediate-points #2308

Merged
merged 1 commit into from
Nov 4, 2024
Merged

Fix Code Style On valid-intermediate-points #2308

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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: 8 additions & 5 deletions soccer/src/soccer/planning/planner/collect_path_planner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -425,11 +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.field_dimensions,
plan_request.constraints, AngleFns::face_point(plan_request.world_state->ball.position),
plan_request.shell_id};
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");

Expand Down
5 changes: 3 additions & 2 deletions soccer/src/soccer/planning/planner/goalie_idle_path_planner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,8 +30,9 @@ 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.field_dimensions,
plan_request.constraints, angle_function, plan_request.shell_id, RJ::Seconds(3.0)},
Replanner::PlanParams{plan_request.start, target, static_obstacles, dynamic_obstacles,
plan_request.field_dimensions, plan_request.constraints,
angle_function, plan_request.shell_id, RJ::Seconds(3.0)},
std::move(previous_));

// Debug drawing
Expand Down
4 changes: 2 additions & 2 deletions soccer/src/soccer/planning/planner/path_target_path_planner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,8 +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.field_dimensions, request.constraints,
angle_function, request.shell_id, RJ::Seconds(3.0)},
request.field_dimensions, request.constraints, angle_function,
request.shell_id, RJ::Seconds(3.0)},
std::move(previous_));

previous_ = trajectory;
Expand Down
7 changes: 4 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 @@ -29,9 +29,10 @@ 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, PlayState play_state, 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)
unsigned shell_id, const WorldState* world_state, PlayState play_state,
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),
Expand Down
36 changes: 22 additions & 14 deletions soccer/src/soccer/planning/planner/settle_path_planner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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, dynamic_obstacles, plan_request.field_dimensions, plan_request.shell_id);
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();
Expand Down Expand Up @@ -332,10 +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, dynamic_obstacles,
plan_request.field_dimensions, plan_request.shell_id);
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),
Expand All @@ -362,10 +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.field_dimensions,
plan_request.constraints, AngleFns::face_point(face_pos), plan_request.shell_id};
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();
Expand Down Expand Up @@ -487,11 +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.field_dimensions,
plan_request.constraints, AngleFns::face_point(plan_request.world_state->ball.position),
plan_request.shell_id};
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;
Expand Down
15 changes: 10 additions & 5 deletions soccer/src/soccer/planning/primitives/create_path.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -79,7 +79,8 @@ static std::unordered_map<uint8_t, std::tuple<double, double, double>> cached_in

Trajectory intermediate(const LinearMotionInstant& start, const LinearMotionInstant& goal,
const MotionConstraints& motion_constraints, RJ::Time start_time,
const rj_geometry::ShapeSet& static_obstacles, const std::vector<DynamicObstacle>& dynamic_obstacles,
const rj_geometry::ShapeSet& static_obstacles,
const std::vector<DynamicObstacle>& 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) {
Expand Down Expand Up @@ -115,7 +116,8 @@ Trajectory intermediate(const LinearMotionInstant& start, const LinearMotionInst
// 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) {
if (abs(offset.x()) > field_dimensions.width() / 2 + 0.2 ||
abs(offset.y()) > field_dimensions.length() / 2 + 0.2) {
continue;
}

Expand All @@ -125,18 +127,21 @@ Trajectory intermediate(const LinearMotionInstant& start, const LinearMotionInst
// 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_[robot_id] = {abs(angle), (final_inter - start.position).mag(), signbit(angle) ? -1 : 1};
cached_intermediate_tuple_[robot_id] = {
abs(angle), (final_inter - start.position).mag(), signbit(angle) ? -1 : 1};
return trajectory;
}
}
}

// If all else fails, use rrt to ensure obstacle avoidance
return CreatePath::rrt(start, goal, motion_constraints, start_time, static_obstacles, dynamic_obstacles);
return CreatePath::rrt(start, goal, motion_constraints, start_time, static_obstacles,
dynamic_obstacles);
}

std::vector<rj_geometry::Point> get_intermediates(const LinearMotionInstant& start,
const LinearMotionInstant& goal, unsigned int robot_id) {
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
Expand Down
6 changes: 4 additions & 2 deletions soccer/src/soccer/planning/primitives/create_path.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,9 +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 std::vector<DynamicObstacle>& dynamic_obstacles,
const rj_geometry::ShapeSet& static_obstacles,
const std::vector<DynamicObstacle>& dynamic_obstacles,
const FieldDimensions field_dimensions, unsigned int robot_id);

std::vector<rj_geometry::Point> get_intermediates(const LinearMotionInstant& start,
const LinearMotionInstant& goal, unsigned int robot_id);
const LinearMotionInstant& goal,
unsigned int robot_id);
} // namespace planning::CreatePath
12 changes: 6 additions & 6 deletions soccer/src/soccer/planning/primitives/replanner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@ 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, params.dynamic_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,
Expand All @@ -56,10 +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, params.dynamic_obstacles,
params.field_dimensions, params.robot_id);
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
Expand All @@ -79,7 +79,7 @@ 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.dynamic_obstacles,
params.static_obstacles, params.dynamic_obstacles,
params.field_dimensions, params.robot_id);
}

Expand Down
2 changes: 1 addition & 1 deletion soccer/src/soccer/planning/primitives/replanner.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,8 +7,8 @@
#include "planning/primitives/angle_planning.hpp"
#include "planning/robot_constraints.hpp"
#include "planning/trajectory.hpp"
#include "velocity_profiling.hpp"
#include "rj_common/field_dimensions.hpp"
#include "velocity_profiling.hpp"

namespace planning {

Expand Down
4 changes: 2 additions & 2 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 @@ -13,8 +13,8 @@
#include "planning/primitives/rrt_util.hpp"
#include "planning/tests/testing_utils.hpp"
#include "planning/trajectory.hpp"
#include "rj_geometry/pose.hpp"
#include "rj_common/field_dimensions.hpp"
#include "rj_geometry/pose.hpp"

/*
* If these tests are failing, run again with the flag --gtest_break_on_failure
Expand Down Expand Up @@ -221,7 +221,7 @@ TEST(Planning, collect_moving_ball_slow) {

TEST(Planning, collect_moving_ball_slow_2) {
WorldState world_state;
FieldDimensions field_dimensions = FieldDimensions::current_dimensions;
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();
Expand Down