@@ -42,9 +42,9 @@ const double pi = 3.1415926;
42
42
const std::array<double , 3 > start_pose1{8 ., 7 ., pi * 0.5 };
43
43
const std::array<double , 3 > start_pose2{9 ., 12 ., pi * 0.25 };
44
44
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 };
46
46
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 };
48
48
const std::array<std::array<double , 3 >, 5 > start_poses{
49
49
start_pose1, start_pose2, start_pose3, start_pose4, start_pose5};
50
50
@@ -88,7 +88,7 @@ nav_msgs::msg::OccupancyGrid construct_cost_map(
88
88
const double x = j * resolution;
89
89
const double y = i * resolution;
90
90
91
- // road
91
+ // road
92
92
if (5.4 < x && x < 5.4 + 4.5 && 0.0 < y && y < 0.0 + 19.4 ) {
93
93
costmap_msg.data [i * width + j] = 40.0 ;
94
94
}
@@ -100,27 +100,27 @@ nav_msgs::msg::OccupancyGrid construct_cost_map(
100
100
double width_p = 3.0 ;
101
101
double height_p = 6.0 ;
102
102
103
- // parking 1 (16,9)
103
+ // parking 1 (16,9)
104
104
if (16.0 < x && x < 16.0 + width_p && 9.0 < y && y < 9.0 + height_p) {
105
105
costmap_msg.data [i * width + j] = 50.0 ;
106
106
}
107
107
108
- // parking 2
108
+ // parking 2
109
109
if (19.3 < x && x < 19.3 + width_p && 9.0 < y && y < 9.0 + height_p) {
110
110
costmap_msg.data [i * width + j] = 50.0 ;
111
111
}
112
112
113
- // parking 3
113
+ // parking 3
114
114
if (22.6 < x && x < 22.6 + width_p && 9.0 < y && y < 9.0 + height_p) {
115
115
costmap_msg.data [i * width + j] = 50.0 ;
116
116
}
117
117
118
- // parking 4
118
+ // parking 4
119
119
if (25.9 < x && x < 25.9 + width_p && 9.0 < y && y < 9.0 + height_p) {
120
120
costmap_msg.data [i * width + j] = 50.0 ;
121
121
}
122
122
123
- // parking 5
123
+ // parking 5
124
124
if (29.2 < x && x < 29.2 + width_p && 9.0 < y && y < 9.0 + height_p) {
125
125
costmap_msg.data [i * width + j] = 50.0 ;
126
126
}
@@ -148,7 +148,7 @@ nav_msgs::msg::OccupancyGrid construct_cost_map(
148
148
costmap_msg.data [j] = 100.0 ;
149
149
}
150
150
}
151
-
151
+
152
152
return costmap_msg;
153
153
}
154
154
@@ -187,7 +187,7 @@ fpa::PlannerCommonParam get_default_planner_params()
187
187
const double maximum_turning_radius = 9.0 ;
188
188
const int turning_radius_size = 1 ;
189
189
190
- // Transitions every 2.5 deg
190
+ // Transitions every 2.5 deg
191
191
const int theta_size = 144 ;
192
192
193
193
const double curve_weight = 1.5 ;
@@ -222,16 +222,17 @@ std::unique_ptr<fpa::AbstractPlanningAlgorithm> configure_astar(bool use_multi,
222
222
const bool use_curve_weight = false ;
223
223
const bool use_complete_astar = false ;
224
224
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
+
228
229
if (use_multi) {
229
230
planner_common_param.minimum_turning_radius = 5.0 ;
230
231
planner_common_param.maximum_turning_radius = 9.0 ;
231
232
planner_common_param.turning_radius_size = 3 ;
232
233
// 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 ;
235
236
}
236
237
astar_param.use_complete_astar = true ;
237
238
}
@@ -246,11 +247,11 @@ enum AlgorithmType {
246
247
ASTAR_MULTI_NOCW,
247
248
};
248
249
// 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
+ });
254
255
255
256
bool test_algorithm (enum AlgorithmType algo_type, bool dump_rosbag = false )
256
257
{
0 commit comments