@@ -483,12 +483,17 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam(
483
483
484
484
void ObstacleCruisePlannerNode::onTrajectory (const Trajectory::ConstSharedPtr msg)
485
485
{
486
- if (
487
- !ego_odom_sub_.updateLatestData () || !objects_sub_.updateLatestData () ||
488
- !acc_sub_.updateLatestData ()) {
486
+ const auto ego_odom_ptr = ego_odom_sub_.takeData ();
487
+ const auto objects_ptr = objects_sub_.takeData ();
488
+ const auto acc_ptr = acc_sub_.takeData ();
489
+ if (!ego_odom_ptr || !objects_ptr || !acc_ptr) {
489
490
return ;
490
491
}
491
492
493
+ const auto & ego_odom = *ego_odom_ptr;
494
+ const auto & objects = *objects_ptr;
495
+ const auto & acc = *acc_ptr;
496
+
492
497
const auto traj_points = motion_utils::convertToTrajectoryPointArray (*msg);
493
498
// check if subscribed variables are ready
494
499
if (traj_points.empty ()) {
@@ -505,14 +510,14 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms
505
510
// (1) with a proper label
506
511
// (2) in front of ego
507
512
// (3) not too far from trajectory
508
- const auto target_obstacles = convertToObstacles (traj_points);
513
+ const auto target_obstacles = convertToObstacles (ego_odom, objects, traj_points);
509
514
510
515
// 2. Determine ego's behavior against each obstacle from stop, cruise and slow down.
511
516
const auto & [stop_obstacles, cruise_obstacles, slow_down_obstacles] =
512
- determineEgoBehaviorAgainstObstacles (traj_points, target_obstacles);
517
+ determineEgoBehaviorAgainstObstacles (ego_odom, objects, traj_points, target_obstacles);
513
518
514
519
// 3. Create data for planning
515
- const auto planner_data = createPlannerData (traj_points);
520
+ const auto planner_data = createPlannerData (ego_odom, acc, traj_points);
516
521
517
522
// 4. Stop planning
518
523
const auto stop_traj_points = planner_ptr_->generateStopTrajectory (planner_data, stop_obstacles);
@@ -599,15 +604,16 @@ std::vector<Polygon2d> ObstacleCruisePlannerNode::createOneStepPolygons(
599
604
}
600
605
601
606
std::vector<Obstacle> ObstacleCruisePlannerNode::convertToObstacles (
607
+ const Odometry & odometry, const PredictedObjects & objects,
602
608
const std::vector<TrajectoryPoint> & traj_points) const
603
609
{
604
610
stop_watch_.tic (__func__);
605
611
606
- const auto obj_stamp = rclcpp::Time (objects_sub_. getData () .header .stamp );
612
+ const auto obj_stamp = rclcpp::Time (objects .header .stamp );
607
613
const auto & p = behavior_determination_param_;
608
614
609
615
std::vector<Obstacle> target_obstacles;
610
- for (const auto & predicted_object : objects_sub_. getData () .objects ) {
616
+ for (const auto & predicted_object : objects .objects ) {
611
617
const auto & object_id =
612
618
tier4_autoware_utils::toHexString (predicted_object.object_id ).substr (0 , 4 );
613
619
const auto & current_obstacle_pose =
@@ -625,8 +631,7 @@ std::vector<Obstacle> ObstacleCruisePlannerNode::convertToObstacles(
625
631
}
626
632
627
633
// 2. Check if the obstacle is in front of the ego.
628
- const size_t ego_idx =
629
- ego_nearest_param_.findIndex (traj_points, ego_odom_sub_.getData ().pose .pose );
634
+ const size_t ego_idx = ego_nearest_param_.findIndex (traj_points, odometry.pose .pose );
630
635
const auto ego_to_obstacle_distance =
631
636
calcDistanceToFrontVehicle (traj_points, ego_idx, current_obstacle_pose.pose .position );
632
637
if (!ego_to_obstacle_distance) {
@@ -715,14 +720,15 @@ bool ObstacleCruisePlannerNode::isFrontCollideObstacle(
715
720
716
721
std::tuple<std::vector<StopObstacle>, std::vector<CruiseObstacle>, std::vector<SlowDownObstacle>>
717
722
ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles (
723
+ const Odometry & odometry, const PredictedObjects & objects,
718
724
const std::vector<TrajectoryPoint> & traj_points, const std::vector<Obstacle> & obstacles)
719
725
{
720
726
stop_watch_.tic (__func__);
721
727
722
728
// calculated decimated trajectory points and trajectory polygon
723
- const auto decimated_traj_points = decimateTrajectoryPoints (traj_points);
729
+ const auto decimated_traj_points = decimateTrajectoryPoints (odometry, traj_points);
724
730
const auto decimated_traj_polys =
725
- createOneStepPolygons (decimated_traj_points, vehicle_info_, ego_odom_sub_. getData () .pose .pose );
731
+ createOneStepPolygons (decimated_traj_points, vehicle_info_, odometry .pose .pose );
726
732
debug_data_ptr_->detection_polygons = decimated_traj_polys;
727
733
728
734
// determine ego's behavior from stop, cruise and slow down
@@ -747,14 +753,14 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles(
747
753
cruise_obstacles.push_back (*cruise_obstacle);
748
754
continue ;
749
755
}
750
- const auto stop_obstacle =
751
- createStopObstacle ( decimated_traj_points, decimated_traj_polys, obstacle, precise_lat_dist);
756
+ const auto stop_obstacle = createStopObstacle (
757
+ odometry, decimated_traj_points, decimated_traj_polys, obstacle, precise_lat_dist);
752
758
if (stop_obstacle) {
753
759
stop_obstacles.push_back (*stop_obstacle);
754
760
continue ;
755
761
}
756
762
const auto slow_down_obstacle =
757
- createSlowDownObstacle (decimated_traj_points, obstacle, precise_lat_dist);
763
+ createSlowDownObstacle (odometry, decimated_traj_points, obstacle, precise_lat_dist);
758
764
if (slow_down_obstacle) {
759
765
slow_down_obstacles.push_back (*slow_down_obstacle);
760
766
continue ;
@@ -780,7 +786,7 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles(
780
786
slow_down_condition_counter_.removeCounterUnlessUpdated ();
781
787
782
788
// Check target obstacles' consistency
783
- checkConsistency (objects_sub_. getData (). header .stamp , objects_sub_. getData () , stop_obstacles);
789
+ checkConsistency (objects. header .stamp , objects , stop_obstacles);
784
790
785
791
// update previous obstacles
786
792
prev_stop_obstacles_ = stop_obstacles;
@@ -796,13 +802,12 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles(
796
802
}
797
803
798
804
std::vector<TrajectoryPoint> ObstacleCruisePlannerNode::decimateTrajectoryPoints (
799
- const std::vector<TrajectoryPoint> & traj_points) const
805
+ const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points) const
800
806
{
801
807
const auto & p = behavior_determination_param_;
802
808
803
809
// trim trajectory
804
- const size_t ego_seg_idx =
805
- ego_nearest_param_.findSegmentIndex (traj_points, ego_odom_sub_.getData ().pose .pose );
810
+ const size_t ego_seg_idx = ego_nearest_param_.findSegmentIndex (traj_points, odometry.pose .pose );
806
811
const size_t traj_start_point_idx = ego_seg_idx;
807
812
const auto trimmed_traj_points =
808
813
std::vector<TrajectoryPoint>(traj_points.begin () + traj_start_point_idx, traj_points.end ());
@@ -1121,8 +1126,9 @@ ObstacleCruisePlannerNode::createCollisionPointsForOutsideCruiseObstacle(
1121
1126
}
1122
1127
1123
1128
std::optional<StopObstacle> ObstacleCruisePlannerNode::createStopObstacle (
1124
- const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polys,
1125
- const Obstacle & obstacle, const double precise_lat_dist) const
1129
+ const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points,
1130
+ const std::vector<Polygon2d> & traj_polys, const Obstacle & obstacle,
1131
+ const double precise_lat_dist) const
1126
1132
{
1127
1133
const auto & p = behavior_determination_param_;
1128
1134
const auto & object_id = obstacle.uuid .substr (0 , 4 );
@@ -1173,7 +1179,7 @@ std::optional<StopObstacle> ObstacleCruisePlannerNode::createStopObstacle(
1173
1179
}
1174
1180
1175
1181
const double collision_time_margin =
1176
- calcCollisionTimeMargin (collision_points, traj_points, is_driving_forward_);
1182
+ calcCollisionTimeMargin (odometry, collision_points, traj_points, is_driving_forward_);
1177
1183
if (p.collision_time_margin < collision_time_margin) {
1178
1184
RCLCPP_INFO_EXPRESSION (
1179
1185
get_logger (), enable_debug_info_,
@@ -1186,7 +1192,7 @@ std::optional<StopObstacle> ObstacleCruisePlannerNode::createStopObstacle(
1186
1192
1187
1193
// calculate collision points with trajectory with lateral stop margin
1188
1194
const auto traj_polys_with_lat_margin = createOneStepPolygons (
1189
- traj_points, vehicle_info_, ego_odom_sub_. getData () .pose .pose , p.max_lat_margin_for_stop );
1195
+ traj_points, vehicle_info_, odometry .pose .pose , p.max_lat_margin_for_stop );
1190
1196
1191
1197
const auto collision_point = polygon_utils::getCollisionPoint (
1192
1198
traj_points, traj_polys_with_lat_margin, obstacle, is_driving_forward_, vehicle_info_);
@@ -1201,8 +1207,8 @@ std::optional<StopObstacle> ObstacleCruisePlannerNode::createStopObstacle(
1201
1207
}
1202
1208
1203
1209
std::optional<SlowDownObstacle> ObstacleCruisePlannerNode::createSlowDownObstacle (
1204
- const std::vector<TrajectoryPoint> & traj_points, const Obstacle & obstacle ,
1205
- const double precise_lat_dist)
1210
+ const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points,
1211
+ const Obstacle & obstacle, const double precise_lat_dist)
1206
1212
{
1207
1213
const auto & object_id = obstacle.uuid .substr (0 , 4 );
1208
1214
const auto & p = behavior_determination_param_;
@@ -1256,7 +1262,7 @@ std::optional<SlowDownObstacle> ObstacleCruisePlannerNode::createSlowDownObstacl
1256
1262
// calculate collision points with trajectory with lateral stop margin
1257
1263
// NOTE: For additional margin, hysteresis is not divided by two.
1258
1264
const auto traj_polys_with_lat_margin = createOneStepPolygons (
1259
- traj_points, vehicle_info_, ego_odom_sub_. getData () .pose .pose ,
1265
+ traj_points, vehicle_info_, odometry .pose .pose ,
1260
1266
p.max_lat_margin_for_slow_down + p.lat_hysteresis_margin_for_slow_down );
1261
1267
1262
1268
std::vector<Polygon2d> front_collision_polygons;
@@ -1416,11 +1422,11 @@ bool ObstacleCruisePlannerNode::isObstacleCrossing(
1416
1422
}
1417
1423
1418
1424
double ObstacleCruisePlannerNode::calcCollisionTimeMargin (
1419
- const std::vector<PointWithStamp> & collision_points,
1425
+ const Odometry & odometry, const std::vector<PointWithStamp> & collision_points,
1420
1426
const std::vector<TrajectoryPoint> & traj_points, const bool is_driving_forward) const
1421
1427
{
1422
- const auto & ego_pose = ego_odom_sub_. getData () .pose .pose ;
1423
- const double ego_vel = ego_odom_sub_. getData () .twist .twist .linear .x ;
1428
+ const auto & ego_pose = odometry .pose .pose ;
1429
+ const double ego_vel = odometry .twist .twist .linear .x ;
1424
1430
1425
1431
const double time_to_reach_collision_point = [&]() {
1426
1432
const double abs_ego_offset = is_driving_forward
@@ -1447,14 +1453,15 @@ double ObstacleCruisePlannerNode::calcCollisionTimeMargin(
1447
1453
}
1448
1454
1449
1455
PlannerData ObstacleCruisePlannerNode::createPlannerData (
1456
+ const Odometry & odometry, const AccelWithCovarianceStamped & acc,
1450
1457
const std::vector<TrajectoryPoint> & traj_points) const
1451
1458
{
1452
1459
PlannerData planner_data;
1453
1460
planner_data.current_time = now ();
1454
1461
planner_data.traj_points = traj_points;
1455
- planner_data.ego_pose = ego_odom_sub_. getData () .pose .pose ;
1456
- planner_data.ego_vel = ego_odom_sub_. getData () .twist .twist .linear .x ;
1457
- planner_data.ego_acc = acc_sub_. getData () .accel .accel .linear .x ;
1462
+ planner_data.ego_pose = odometry .pose .pose ;
1463
+ planner_data.ego_vel = odometry .twist .twist .linear .x ;
1464
+ planner_data.ego_acc = acc .accel .accel .linear .x ;
1458
1465
planner_data.is_driving_forward = is_driving_forward_;
1459
1466
return planner_data;
1460
1467
}
0 commit comments