From 1bd727df9ca2fc9dcf943ef564aab1148c0b846a Mon Sep 17 00:00:00 2001 From: sanatd33 Date: Mon, 4 Nov 2024 01:35:31 +0000 Subject: [PATCH] automated style fixes --- .../planning/planner/collect_path_planner.cpp | 13 ++++--- .../planner/goalie_idle_path_planner.cpp | 5 +-- .../planner/path_target_path_planner.cpp | 4 +-- .../soccer/planning/planner/plan_request.hpp | 7 ++-- .../planning/planner/settle_path_planner.cpp | 36 +++++++++++-------- .../planning/primitives/create_path.cpp | 15 +++++--- .../planning/primitives/create_path.hpp | 6 ++-- .../soccer/planning/primitives/replanner.cpp | 12 +++---- .../soccer/planning/primitives/replanner.hpp | 2 +- .../soccer/planning/tests/planner_test.cpp | 4 +-- 10 files changed, 62 insertions(+), 42 deletions(-) diff --git a/soccer/src/soccer/planning/planner/collect_path_planner.cpp b/soccer/src/soccer/planning/planner/collect_path_planner.cpp index 240796060a5..cd42ccdfe7f 100644 --- a/soccer/src/soccer/planning/planner/collect_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/collect_path_planner.cpp @@ -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"); 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 5761c79bcb1..a16f84f3912 100644 --- a/soccer/src/soccer/planning/planner/goalie_idle_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/goalie_idle_path_planner.cpp @@ -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 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 07f5c0c08f6..b145bdd85f0 100644 --- a/soccer/src/soccer/planning/planner/path_target_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/path_target_path_planner.cpp @@ -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; diff --git a/soccer/src/soccer/planning/planner/plan_request.hpp b/soccer/src/soccer/planning/planner/plan_request.hpp index fea3be70e62..927bf949713 100644 --- a/soccer/src/soccer/planning/planner/plan_request.hpp +++ b/soccer/src/soccer/planning/planner/plan_request.hpp @@ -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), diff --git a/soccer/src/soccer/planning/planner/settle_path_planner.cpp b/soccer/src/soccer/planning/planner/settle_path_planner.cpp index a2e044caff8..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, 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(); @@ -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), @@ -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(); @@ -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; diff --git a/soccer/src/soccer/planning/primitives/create_path.cpp b/soccer/src/soccer/planning/primitives/create_path.cpp index 0feaef2be34..e63612a9c7e 100644 --- a/soccer/src/soccer/planning/primitives/create_path.cpp +++ b/soccer/src/soccer/planning/primitives/create_path.cpp @@ -79,7 +79,8 @@ static std::unordered_map> 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& dynamic_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) { @@ -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; } @@ -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 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 diff --git a/soccer/src/soccer/planning/primitives/create_path.hpp b/soccer/src/soccer/planning/primitives/create_path.hpp index 06bab6a8547..bed7c2d810b 100644 --- a/soccer/src/soccer/planning/primitives/create_path.hpp +++ b/soccer/src/soccer/planning/primitives/create_path.hpp @@ -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& dynamic_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, unsigned int robot_id); + 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 0edc76987ff..fc790bf7e35 100644 --- a/soccer/src/soccer/planning/primitives/replanner.cpp +++ b/soccer/src/soccer/planning/primitives/replanner.cpp @@ -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, @@ -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 @@ -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); } diff --git a/soccer/src/soccer/planning/primitives/replanner.hpp b/soccer/src/soccer/planning/primitives/replanner.hpp index 519a02dd012..433592fcc01 100644 --- a/soccer/src/soccer/planning/primitives/replanner.hpp +++ b/soccer/src/soccer/planning/primitives/replanner.hpp @@ -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 { diff --git a/soccer/src/soccer/planning/tests/planner_test.cpp b/soccer/src/soccer/planning/tests/planner_test.cpp index f7cbb059475..e1af54f5f23 100644 --- a/soccer/src/soccer/planning/tests/planner_test.cpp +++ b/soccer/src/soccer/planning/tests/planner_test.cpp @@ -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 @@ -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();