Skip to content

Commit 7699ff2

Browse files
style(pre-commit): autofix
1 parent c3ae35d commit 7699ff2

File tree

1 file changed

+21
-20
lines changed

1 file changed

+21
-20
lines changed

planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms_astar.cpp

+21-20
Original file line numberDiff line numberDiff line change
@@ -42,9 +42,9 @@ const double pi = 3.1415926;
4242
const std::array<double, 3> start_pose1{8., 7., pi * 0.5};
4343
const std::array<double, 3> start_pose2{9., 12., pi * 0.25};
4444
const std::array<double, 3> start_pose3{13.3, 9., pi * 0.5};
45-
const std::array<double, 3> start_pose4{25.0, 16., - pi * 0.25};
45+
const std::array<double, 3> start_pose4{25.0, 16., -pi * 0.25};
4646
const std::array<double, 3> start_pose5{20.5, 8.5, 0.0};
47-
const std::array<double, 3> goal_pose{18.0, 10.5, pi * 0.5};
47+
const std::array<double, 3> goal_pose{18.0, 10.5, pi * 0.5};
4848
const std::array<std::array<double, 3>, 5> start_poses{
4949
start_pose1, start_pose2, start_pose3, start_pose4, start_pose5};
5050

@@ -88,7 +88,7 @@ nav_msgs::msg::OccupancyGrid construct_cost_map(
8888
const double x = j * resolution;
8989
const double y = i * resolution;
9090

91-
//road
91+
// road
9292
if (5.4 < x && x < 5.4 + 4.5 && 0.0 < y && y < 0.0 + 19.4) {
9393
costmap_msg.data[i * width + j] = 40.0;
9494
}
@@ -100,27 +100,27 @@ nav_msgs::msg::OccupancyGrid construct_cost_map(
100100
double width_p = 3.0;
101101
double height_p = 6.0;
102102

103-
//parking 1 (16,9)
103+
// parking 1 (16,9)
104104
if (16.0 < x && x < 16.0 + width_p && 9.0 < y && y < 9.0 + height_p) {
105105
costmap_msg.data[i * width + j] = 50.0;
106106
}
107107

108-
//parking 2
108+
// parking 2
109109
if (19.3 < x && x < 19.3 + width_p && 9.0 < y && y < 9.0 + height_p) {
110110
costmap_msg.data[i * width + j] = 50.0;
111111
}
112112

113-
//parking 3
113+
// parking 3
114114
if (22.6 < x && x < 22.6 + width_p && 9.0 < y && y < 9.0 + height_p) {
115115
costmap_msg.data[i * width + j] = 50.0;
116116
}
117117

118-
//parking 4
118+
// parking 4
119119
if (25.9 < x && x < 25.9 + width_p && 9.0 < y && y < 9.0 + height_p) {
120120
costmap_msg.data[i * width + j] = 50.0;
121121
}
122122

123-
//parking 5
123+
// parking 5
124124
if (29.2 < x && x < 29.2 + width_p && 9.0 < y && y < 9.0 + height_p) {
125125
costmap_msg.data[i * width + j] = 50.0;
126126
}
@@ -148,7 +148,7 @@ nav_msgs::msg::OccupancyGrid construct_cost_map(
148148
costmap_msg.data[j] = 100.0;
149149
}
150150
}
151-
151+
152152
return costmap_msg;
153153
}
154154

@@ -187,7 +187,7 @@ fpa::PlannerCommonParam get_default_planner_params()
187187
const double maximum_turning_radius = 9.0;
188188
const int turning_radius_size = 1;
189189

190-
// Transitions every 2.5 deg
190+
// Transitions every 2.5 deg
191191
const int theta_size = 144;
192192

193193
const double curve_weight = 1.5;
@@ -222,16 +222,17 @@ std::unique_ptr<fpa::AbstractPlanningAlgorithm> configure_astar(bool use_multi,
222222
const bool use_curve_weight = false;
223223
const bool use_complete_astar = false;
224224
const double distance_heuristic_weight = 1.0;
225-
auto astar_param =
226-
fpa::AstarParam{only_behind_solutions, use_back, use_curve_weight, use_complete_astar, distance_heuristic_weight};
227-
225+
auto astar_param = fpa::AstarParam{
226+
only_behind_solutions, use_back, use_curve_weight, use_complete_astar,
227+
distance_heuristic_weight};
228+
228229
if (use_multi) {
229230
planner_common_param.minimum_turning_radius = 5.0;
230231
planner_common_param.maximum_turning_radius = 9.0;
231232
planner_common_param.turning_radius_size = 3;
232233
// if multi-curvature and using curve weight
233-
if (cw){
234-
astar_param.use_curve_weight = true;
234+
if (cw) {
235+
astar_param.use_curve_weight = true;
235236
}
236237
astar_param.use_complete_astar = true;
237238
}
@@ -246,11 +247,11 @@ enum AlgorithmType {
246247
ASTAR_MULTI_NOCW,
247248
};
248249
// cspell: ignore fpalgos
249-
std::unordered_map<AlgorithmType, std::string> rosbag_dir_prefix_table(
250-
{{ASTAR_SINGLE, "fpalgos-astar_single"},
251-
{ASTAR_MULTI, "fpalgos-astar_multi"},
252-
{ASTAR_MULTI_NOCW, "fpalgos-astar_multi_nocw"},
253-
});
250+
std::unordered_map<AlgorithmType, std::string> rosbag_dir_prefix_table({
251+
{ASTAR_SINGLE, "fpalgos-astar_single"},
252+
{ASTAR_MULTI, "fpalgos-astar_multi"},
253+
{ASTAR_MULTI_NOCW, "fpalgos-astar_multi_nocw"},
254+
});
254255

255256
bool test_algorithm(enum AlgorithmType algo_type, bool dump_rosbag = false)
256257
{

0 commit comments

Comments
 (0)