diff --git a/soccer/src/soccer/planning/planning_params.cpp b/soccer/src/soccer/planning/planning_params.cpp index 525c930545e..e94b3abba75 100644 --- a/soccer/src/soccer/planning/planning_params.cpp +++ b/soccer/src/soccer/planning/planning_params.cpp @@ -41,14 +41,18 @@ DEFINE_NS_INT64(kPlanningParamModule, rrt, min_iterations, 50, DEFINE_NS_INT64(kPlanningParamModule, rrt, max_iterations, 500, "Maximum number of RRT iterations to run before giving up"); - -DEFINE_NS_FLOAT64(kPlanningParamModule, intermediate, min_scale, 0.5, "Minimum length for intermediate point (m)"); -DEFINE_NS_FLOAT64(kPlanningParamModule, intermediate, max_scale, 1.5, "Maximum length for intermediate point (m)"); -DEFINE_NS_FLOAT64(kPlanningParamModule, intermediate, min_angle, 20, "Minimum angle for intermediate point (deg)"); -DEFINE_NS_FLOAT64(kPlanningParamModule, intermediate, max_angle, 140, "Maximum angle for intermediate point (deg)"); -DEFINE_NS_INT64(kPlanningParamModule, intermediate, num_intermediates, 5, "Number of intermediate points used (unitless)"); -DEFINE_NS_FLOAT64(kPlanningParamModule, intermediate, step_size, 0.1, "Step size for testing intermediates (m)"); - +DEFINE_NS_FLOAT64(kPlanningParamModule, intermediate, min_scale, 0.5, + "Minimum length for intermediate point (m)"); +DEFINE_NS_FLOAT64(kPlanningParamModule, intermediate, max_scale, 1.5, + "Maximum length for intermediate point (m)"); +DEFINE_NS_FLOAT64(kPlanningParamModule, intermediate, min_angle, 20, + "Minimum angle for intermediate point (deg)"); +DEFINE_NS_FLOAT64(kPlanningParamModule, intermediate, max_angle, 140, + "Maximum angle for intermediate point (deg)"); +DEFINE_NS_INT64(kPlanningParamModule, intermediate, num_intermediates, 5, + "Number of intermediate points used (unitless)"); +DEFINE_NS_FLOAT64(kPlanningParamModule, intermediate, step_size, 0.1, + "Step size for testing intermediates (m)"); DEFINE_NS_FLOAT64( kPlanningParamModule, escape, step_size, 0.1, diff --git a/soccer/src/soccer/planning/primitives/create_path.cpp b/soccer/src/soccer/planning/primitives/create_path.cpp index 666128ab4d8..a673fdc5eef 100644 --- a/soccer/src/soccer/planning/primitives/create_path.cpp +++ b/soccer/src/soccer/planning/primitives/create_path.cpp @@ -77,7 +77,6 @@ Trajectory rrt(const LinearMotionInstant& start, const LinearMotionInstant& goal static std::optional> cached_intermediate_pair_{}; - Trajectory intermediate(const LinearMotionInstant& start, const LinearMotionInstant& goal, const MotionConstraints& motion_constraints, RJ::Time start_time, const rj_geometry::ShapeSet& static_obstacles) { @@ -105,7 +104,8 @@ Trajectory intermediate(const LinearMotionInstant& start, const LinearMotionInst // Step through the path from the robot to the final intermediate point // and test each point on that path as an intermediate point - for (double t = intermediate::PARAM_step_size; t < final_inter.dist_to(start.position); t += intermediate::PARAM_step_size) { + for (double t = intermediate::PARAM_step_size; t < final_inter.dist_to(start.position); + t += intermediate::PARAM_step_size) { rj_geometry::Point intermediate = (final_inter - start.position).normalized(t) + start.position; Trajectory trajectory = @@ -113,7 +113,8 @@ 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))) { - cached_intermediate_pair_ = {(final_inter - start.position).angle(), (final_inter - start.position).mag()}; + cached_intermediate_pair_ = {(final_inter - start.position).angle(), + (final_inter - start.position).mag()}; return trajectory; } } @@ -129,7 +130,8 @@ std::vector get_intermediates(const LinearMotionInstant& sta std::mt19937 gen(rd()); // Create a random distribution for the distance between the start // and the intermediate points - std::uniform_real_distribution<> scale_dist(intermediate::PARAM_min_scale, intermediate::PARAM_max_scale); + std::uniform_real_distribution<> scale_dist(intermediate::PARAM_min_scale, + intermediate::PARAM_max_scale); double angle_range = intermediate::PARAM_max_angle - intermediate::PARAM_min_angle; // Create a random distribution for the angle between the start diff --git a/soccer/src/soccer/planning/primitives/create_path.hpp b/soccer/src/soccer/planning/primitives/create_path.hpp index 400a372c649..d71cf5809f8 100644 --- a/soccer/src/soccer/planning/primitives/create_path.hpp +++ b/soccer/src/soccer/planning/primitives/create_path.hpp @@ -1,7 +1,8 @@ #pragma once -#include #include +#include + #include "planning/motion_constraints.hpp" #include "planning/primitives/path_smoothing.hpp" #include "planning/trajectory.hpp"