Skip to content

Fix Code Style On obstacle-rewrite #2478

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

Closed
github-actions wants to merge 1 commit into from
Closed

Fix Code Style On obstacle-rewrite #2478

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
4 changes: 2 additions & 2 deletions src/rj_planning/include/rj_planning/plan_request.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -202,7 +202,7 @@ inline std::shared_ptr<Obstacle> make_robot_obstacle(rj_geometry::Point pos) {
* @return Shared pointer to the created Obstacle
*/
inline std::shared_ptr<Obstacle> make_moving_robot_obstacle(rj_geometry::Point pos,
rj_geometry::Point vel) {
rj_geometry::Point vel) {
auto obs_circle = std::make_shared<rj_geometry::Circle>(pos, kRobotRadius);

// Create stadium-shaped padding based on velocity
Expand All @@ -222,7 +222,7 @@ inline std::shared_ptr<Obstacle> make_moving_robot_obstacle(rj_geometry::Point p
* @return Shared pointer to the created Obstacle
*/
inline std::shared_ptr<Obstacle> make_ball_obstacle(rj_geometry::Point pos,
float extra_radius = 0.0f) {
float extra_radius = 0.0f) {
float radius = kBallRadius + kAvoidBallDistance + extra_radius;
auto circle = std::make_shared<rj_geometry::Circle>(pos, radius);
return std::make_shared<Obstacle>(circle, circle);
Expand Down
3 changes: 1 addition & 2 deletions src/rj_planning/include/rj_planning/planners/collect_path_planner.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -64,8 +64,7 @@ class CollectPathPlanner : public PathPlanner {
Trajectory fine_approach(const PlanRequest& plan_request, RobotInstant start_instant,
const ObstacleSet& obstacles);

Trajectory invalid(const PlanRequest& plan_request,
const ObstacleSet& obstacles);
Trajectory invalid(const PlanRequest& plan_request, const ObstacleSet& obstacles);

Trajectory previous_;

Expand Down
14 changes: 6 additions & 8 deletions src/rj_planning/include/rj_planning/planners/settle_path_planner.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -75,18 +75,16 @@ class SettlePathPlanner : public PathPlanner {
double angle, rj_geometry::Point delta_pos);

// State functions
Trajectory intercept(const PlanRequest& plan_request,
RobotInstant start_instant,
const ObstacleSet& obstacles,
rj_geometry::Point delta_pos, rj_geometry::Point face_pos);
Trajectory intercept(const PlanRequest& plan_request, RobotInstant start_instant,
const ObstacleSet& obstacles, rj_geometry::Point delta_pos,
rj_geometry::Point face_pos);

// Dampen doesn't need to take obstacles into account.
Trajectory dampen(const PlanRequest& plan_request, RobotInstant start_instant,
const ObstacleSet& obstacles,
rj_geometry::Point delta_pos, rj_geometry::Point face_pos);
const ObstacleSet& obstacles, rj_geometry::Point delta_pos,
rj_geometry::Point face_pos);

Trajectory invalid(const PlanRequest& plan_request,
const ObstacleSet& obstacles);
Trajectory invalid(const PlanRequest& plan_request, const ObstacleSet& obstacles);

std::optional<rj_geometry::Point> target_bounce_direction_;

Expand Down
7 changes: 3 additions & 4 deletions src/rj_planning/include/rj_planning/primitives/create_path.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,8 +19,7 @@ namespace planning::CreatePath {
/**
* Generate a smooth path from start to goal avoiding obstacles.
*/
Trajectory rrt(const LinearMotionInstant& start,
const LinearMotionInstant& goal,
Trajectory rrt(const LinearMotionInstant& start, const LinearMotionInstant& goal,
const MotionConstraints& motion_constraints, RJ::Time start_time,
const ObstacleSet& obstacles,
const std::vector<rj_geometry::Point>& bias_waypoints = {});
Expand All @@ -35,8 +34,8 @@ Trajectory simple(

Trajectory intermediate(const LinearMotionInstant& start, const LinearMotionInstant& goal,
const MotionConstraints& motion_constraints, RJ::Time start_time,
const ObstacleSet& obstacles,
const FieldDimensions* field_dimensions, unsigned int robot_id);
const ObstacleSet& obstacles, const FieldDimensions* field_dimensions,
unsigned int robot_id);

std::vector<rj_geometry::Point> get_intermediates(const LinearMotionInstant& start,
const LinearMotionInstant& goal,
Expand Down
7 changes: 3 additions & 4 deletions src/rj_planning/include/rj_planning/primitives/rrt_util.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,9 +36,8 @@ void draw_bi_rrt(const RRT::BiRRT<rj_geometry::Point>& bi_rrt,
* @return A vector of points representing some clear path from the start to
* the end.
*/
std::vector<rj_geometry::Point> generate_rrt(
rj_geometry::Point start, rj_geometry::Point goal,
const ObstacleSet& obstacles,
const std::vector<rj_geometry::Point>& waypoints = {});
std::vector<rj_geometry::Point> generate_rrt(rj_geometry::Point start, rj_geometry::Point goal,
const ObstacleSet& obstacles,
const std::vector<rj_geometry::Point>& waypoints = {});

} // namespace planning
5 changes: 2 additions & 3 deletions src/rj_planning/include/rj_planning/trajectory_utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,8 +18,7 @@ namespace planning {
* @param hit_time The time of the collision (output parameter).
* @return Whether or not there is a collision.
*/
bool trajectory_hits_static(const Trajectory& trajectory,
const ObstacleSet& obstacles,
RJ::Time start_time, RJ::Time* hit_time);
bool trajectory_hits_static(const Trajectory& trajectory, const ObstacleSet& obstacles,
RJ::Time start_time, RJ::Time* hit_time);

} // namespace planning
9 changes: 4 additions & 5 deletions src/rj_planning/src/plan_request.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@ void fill_obstacles(const PlanRequest& in, ObstacleSet& out_obstacles, bool avoi

if (their_robot.visible) {
out_obstacles.add(make_moving_robot_obstacle(their_robot.pose.position(),
their_robot.velocity.linear()));
their_robot.velocity.linear()));
}
}

Expand All @@ -45,15 +45,14 @@ void fill_obstacles(const PlanRequest& in, ObstacleSet& out_obstacles, bool avoi
continue;
}

out_obstacles.add(make_moving_robot_obstacle(our_robot.pose.position(),
our_robot.velocity.linear()));
out_obstacles.add(
make_moving_robot_obstacle(our_robot.pose.position(), our_robot.velocity.linear()));
}

// Add ball as obstacle if needed
// Only added when STOP state is enabled
if (in.min_dist_from_ball > 0 || avoid_ball) {
out_obstacles.add(make_ball_obstacle(in.world_state->ball.position,
in.min_dist_from_ball));
out_obstacles.add(make_ball_obstacle(in.world_state->ball.position, in.min_dist_from_ball));
float ball_radius = kBallRadius + kAvoidBallDistance + in.min_dist_from_ball;

// Draw ball obstacle in simulator
Expand Down
13 changes: 5 additions & 8 deletions src/rj_planning/src/planners/collect_path_planner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -309,8 +309,7 @@ Trajectory CollectPathPlanner::intercept(const PlanRequest& plan_request,
// test location
Trajectory path = CreatePath::intermediate(
start_instant.linear_motion(), target_robot_intersection, plan_request.constraints.mot,
start_instant.stamp, obstacles, plan_request.field_dimensions,
plan_request.shell_id);
start_instant.stamp, obstacles, plan_request.field_dimensions, plan_request.shell_id);

// Calculate the
RJ::Seconds buffer_duration = ball_time - path.duration();
Expand Down Expand Up @@ -419,8 +418,7 @@ Trajectory CollectPathPlanner::intercept(const PlanRequest& plan_request,

Trajectory shortcut = CreatePath::intermediate(
start_instant.linear_motion(), target, plan_request.constraints.mot,
start_instant.stamp, obstacles, plan_request.field_dimensions,
plan_request.shell_id);
start_instant.stamp, obstacles, plan_request.field_dimensions, plan_request.shell_id);

if (!shortcut.empty()) {
plan_angles(&shortcut, start_instant, AngleFns::face_point(face_pos),
Expand Down Expand Up @@ -563,10 +561,9 @@ Trajectory CollectPathPlanner::dampen(const PlanRequest& plan_request, RobotInst
Trajectory dampen_end;

if (previous_.empty()) {
dampen_end = CreatePath::intermediate(start_instant.linear_motion(), final_stopping_motion,
plan_request.constraints.mot, start_instant.stamp,
obstacles, plan_request.field_dimensions,
plan_request.shell_id);
dampen_end = CreatePath::intermediate(
start_instant.linear_motion(), final_stopping_motion, plan_request.constraints.mot,
start_instant.stamp, obstacles, plan_request.field_dimensions, plan_request.shell_id);
} else {
dampen_end = CreatePath::intermediate(previous_.last().linear_motion(),
final_stopping_motion, plan_request.constraints.mot,
Expand Down
8 changes: 4 additions & 4 deletions src/rj_planning/src/planners/settle_path_planner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -156,8 +156,8 @@ void SettlePathPlanner::process_state_transition(BallState ball, RobotInstant* s
}

Trajectory SettlePathPlanner::intercept(const PlanRequest& plan_request, RobotInstant start_instant,
const ObstacleSet& obstacles,
rj_geometry::Point delta_pos, rj_geometry::Point face_pos) {
const ObstacleSet& obstacles, rj_geometry::Point delta_pos,
rj_geometry::Point face_pos) {
BallState ball = plan_request.world_state->ball;

// Try find best point to intercept using brute force method
Expand Down Expand Up @@ -368,8 +368,8 @@ Trajectory SettlePathPlanner::intercept(const PlanRequest& plan_request, RobotIn
}

Trajectory SettlePathPlanner::dampen(const PlanRequest& plan_request, RobotInstant start_instant,
const ObstacleSet& obstacles,
rj_geometry::Point delta_pos, rj_geometry::Point face_pos) {
const ObstacleSet& obstacles, rj_geometry::Point delta_pos,
rj_geometry::Point face_pos) {
// Only run once if we can

// Intercept ends with a % ball velocity in the direction of the ball
Expand Down
7 changes: 3 additions & 4 deletions src/rj_planning/src/primitives/create_path.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,8 +21,7 @@ Trajectory simple(const LinearMotionInstant& start, const LinearMotionInstant& g

Trajectory rrt(const LinearMotionInstant& start, const LinearMotionInstant& goal,
const MotionConstraints& motion_constraints, RJ::Time start_time,
const ObstacleSet& obstacles,
const std::vector<Point>& bias_waypoints) {
const ObstacleSet& obstacles, const std::vector<Point>& bias_waypoints) {
// 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}}};
Expand Down Expand Up @@ -54,8 +53,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 ObstacleSet& obstacles,
const FieldDimensions* field_dimensions, unsigned int robot_id) {
const ObstacleSet& 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}}};
Expand Down
13 changes: 6 additions & 7 deletions src/rj_planning/src/primitives/replanner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,10 +25,9 @@ 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.obstacles, params.field_dimensions, params.robot_id);
Trajectory post_trajectory = CreatePath::intermediate(
pre_trajectory.last().linear_motion(), params.goal, params.constraints.mot,
pre_trajectory.end_time(), params.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.
Expand Down Expand Up @@ -70,9 +69,9 @@ Trajectory Replanner::full_replan(const Replanner::PlanParams& params) {

almost_goal.position += shift_dir * shift_size;

path = CreatePath::intermediate(
params.start.linear_motion(), almost_goal, params.constraints.mot, params.start.stamp,
params.obstacles, params.field_dimensions, params.robot_id);
path = CreatePath::intermediate(params.start.linear_motion(), almost_goal,
params.constraints.mot, params.start.stamp,
params.obstacles, params.field_dimensions, params.robot_id);
}

if (!path.empty()) {
Expand Down