27
27
#include < lanelet2_extension/utility/message_conversion.hpp>
28
28
#include < lanelet2_extension/utility/query.hpp>
29
29
#include < lanelet2_extension/utility/utilities.hpp>
30
- #include < tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
31
30
32
31
#include < boost/format.hpp>
33
- #include < boost/geometry/algorithms/correct.hpp>
34
32
#include < boost/geometry/algorithms/intersects.hpp>
33
+ #include < boost/geometry/algorithms/within.hpp>
35
34
36
35
#include < lanelet2_core/geometry/LineString.h>
37
36
#include < lanelet2_core/geometry/Polygon.h>
38
37
39
38
#include < algorithm>
40
39
#include < chrono>
41
40
42
- #define debug (var ) \
43
- do { \
44
- std::cerr << __LINE__ << " , " << __func__ << " , " << #var << " : " ; \
45
- view (var); \
46
- } while (0 )
47
- template <typename T>
48
- void view (T e)
49
- {
50
- std::cerr << e << std::endl;
51
- }
52
- template <typename T>
53
- void view (const std::vector<T> & v)
54
- {
55
- for (const auto & e : v) {
56
- std::cerr << e << " " ;
57
- }
58
- std::cerr << std::endl;
59
- }
60
- template <typename T>
61
- void view (const std::vector<std::vector<T>> & vv)
62
- {
63
- for (const auto & v : vv) {
64
- view (v);
65
- }
66
- }
67
- #define line () \
68
- { \
69
- std::cerr << __LINE__ << " , " << __func__ << std::endl; \
70
- }
71
- #define line_with_file () \
72
- { \
73
- std::cerr << " (" << __FILE__ << " ) " << __func__ << " : " << __LINE__ << std::endl; \
74
- }
75
-
76
41
namespace
77
42
{
78
43
VelocityLimitClearCommand createVelocityLimitClearCommandMessage (
@@ -320,13 +285,13 @@ ObstacleCruisePlannerNode::BehaviorDeterminationParam::BehaviorDeterminationPara
320
285
" behavior_determination.consider_current_pose.enable_to_consider_current_pose" );
321
286
time_to_convergence = node.declare_parameter <double >(
322
287
" behavior_determination.consider_current_pose.time_to_convergence" );
323
- work_as_pseudo_occulusion = node.declare_parameter <bool >(
324
- " behavior_determination.slow_down.pseudo_occulusion .enable_function" );
325
- if (work_as_pseudo_occulusion ) {
326
- max_obj_vel_for_pseudo_occulusion = node.declare_parameter <double >(
327
- " behavior_determination.slow_down.pseudo_occulusion .max_obj_vel" );
328
- focus_intersections_for_pseudo_occulusion = node.declare_parameter <std::vector<lanelet::Id>>(
329
- " behavior_determination.slow_down.pseudo_occulusion .focus_intersections" );
288
+ work_as_pseudo_occlusion = node.declare_parameter <bool >(
289
+ " behavior_determination.slow_down.pseudo_occlusion .enable_function" );
290
+ if (work_as_pseudo_occlusion ) {
291
+ max_obj_vel_for_pseudo_occlusion = node.declare_parameter <double >(
292
+ " behavior_determination.slow_down.pseudo_occlusion .max_obj_vel" );
293
+ focus_intersections_for_pseudo_occlusion = node.declare_parameter <std::vector<lanelet::Id>>(
294
+ " behavior_determination.slow_down.pseudo_occlusion .focus_intersections" );
330
295
}
331
296
}
332
297
@@ -389,15 +354,15 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam(
389
354
parameters, " behavior_determination.consider_current_pose.time_to_convergence" ,
390
355
time_to_convergence);
391
356
tier4_autoware_utils::updateParam<bool >(
392
- parameters, " behavior_determination.slow_down.pseudo_occulusion .enable_function" ,
393
- work_as_pseudo_occulusion );
394
- if (work_as_pseudo_occulusion ) {
357
+ parameters, " behavior_determination.slow_down.pseudo_occlusion .enable_function" ,
358
+ work_as_pseudo_occlusion );
359
+ if (work_as_pseudo_occlusion ) {
395
360
tier4_autoware_utils::updateParam<double >(
396
- parameters, " behavior_determination.slow_down.pseudo_occulusion .max_obj_vel" ,
397
- max_obj_vel_for_pseudo_occulusion );
361
+ parameters, " behavior_determination.slow_down.pseudo_occlusion .max_obj_vel" ,
362
+ max_obj_vel_for_pseudo_occlusion );
398
363
tier4_autoware_utils::updateParam<std::vector<lanelet::Id>>(
399
- parameters, " behavior_determination.slow_down.pseudo_occulusion .focus_intersections" ,
400
- focus_intersections_for_pseudo_occulusion );
364
+ parameters, " behavior_determination.slow_down.pseudo_occlusion .focus_intersections" ,
365
+ focus_intersections_for_pseudo_occlusion );
401
366
}
402
367
}
403
368
@@ -1199,21 +1164,18 @@ std::optional<SlowDownObstacle> ObstacleCruisePlannerNode::createSlowDownObstacl
1199
1164
return std::nullopt;
1200
1165
}
1201
1166
1202
- const auto is_occulusion_object = [&]() {
1167
+ const auto is_occlusion_object = [&]() {
1203
1168
if (
1204
1169
std::hypot (obstacle.twist .linear .x , obstacle.twist .linear .y ) >
1205
- behavior_determination_param_.max_obj_vel_for_pseudo_occulusion + 1e-6 ) {
1206
- line ();
1170
+ behavior_determination_param_.max_obj_vel_for_pseudo_occlusion + 1e-6 ) {
1207
1171
return false ;
1208
1172
}
1209
1173
1210
1174
if (motion_utils::calcLateralOffset (traj_points, obstacle.pose .position ) > 0.0 ) {
1211
- line ();
1212
1175
return true ;
1213
1176
}
1214
1177
1215
- for (const auto & id :
1216
- behavior_determination_param_.focus_intersections_for_pseudo_occulusion ) {
1178
+ for (const auto & id : behavior_determination_param_.focus_intersections_for_pseudo_occlusion ) {
1217
1179
if (id == 0 ) {
1218
1180
continue ;
1219
1181
}
@@ -1223,15 +1185,13 @@ std::optional<SlowDownObstacle> ObstacleCruisePlannerNode::createSlowDownObstacl
1223
1185
if (
1224
1186
boost::geometry::within (obstacle_poly, intersection_poly) ||
1225
1187
boost::geometry::intersects (obstacle_poly, intersection_poly)) {
1226
- line ();
1227
1188
return true ;
1228
1189
}
1229
1190
}
1230
- line ();
1231
1191
return false ;
1232
1192
};
1233
1193
1234
- if (behavior_determination_param_.work_as_pseudo_occulusion && !is_occulusion_object ()) {
1194
+ if (behavior_determination_param_.work_as_pseudo_occlusion && !is_occlusion_object ()) {
1235
1195
return std::nullopt;
1236
1196
}
1237
1197
0 commit comments