Skip to content

Commit 12f600a

Browse files
soblindanielsanchezaran
authored andcommitted
feat(tier4_autoware_utils): default QoS setting of polling subscriber (autowarefoundation#6976)
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
1 parent e07c3d4 commit 12f600a

File tree

3 files changed

+71
-56
lines changed

3 files changed

+71
-56
lines changed

common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp

+18-16
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,8 @@
1717

1818
#include <rclcpp/rclcpp.hpp>
1919

20+
#include <memory>
21+
#include <stdexcept>
2022
#include <string>
2123

2224
namespace tier4_autoware_utils
@@ -27,37 +29,37 @@ class InterProcessPollingSubscriber
2729
{
2830
private:
2931
typename rclcpp::Subscription<T>::SharedPtr subscriber_;
30-
std::optional<T> data_;
32+
typename T::SharedPtr data_;
3133

3234
public:
33-
explicit InterProcessPollingSubscriber(rclcpp::Node * node, const std::string & topic_name)
35+
explicit InterProcessPollingSubscriber(
36+
rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos = rclcpp::QoS{1})
3437
{
3538
auto noexec_callback_group =
3639
node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
3740
auto noexec_subscription_options = rclcpp::SubscriptionOptions();
3841
noexec_subscription_options.callback_group = noexec_callback_group;
3942

4043
subscriber_ = node->create_subscription<T>(
41-
topic_name, rclcpp::QoS{1},
44+
topic_name, qos,
4245
[node]([[maybe_unused]] const typename T::ConstSharedPtr msg) { assert(false); },
4346
noexec_subscription_options);
44-
};
45-
bool updateLatestData()
46-
{
47-
rclcpp::MessageInfo message_info;
48-
T tmp;
49-
// The queue size (QoS) must be 1 to get the last message data.
50-
if (subscriber_->take(tmp, message_info)) {
51-
data_ = tmp;
47+
if (qos.get_rmw_qos_profile().depth > 1) {
48+
throw std::invalid_argument(
49+
"InterProcessPollingSubscriber will be used with depth > 1, which may cause inefficient "
50+
"serialization while updateLatestData()");
5251
}
53-
return data_.has_value();
5452
};
55-
const T & getData() const
53+
typename T::ConstSharedPtr takeData()
5654
{
57-
if (!data_.has_value()) {
58-
throw std::runtime_error("Bad_optional_access in class InterProcessPollingSubscriber");
55+
auto new_data = std::make_shared<T>();
56+
rclcpp::MessageInfo message_info;
57+
const bool success = subscriber_->take(*new_data, message_info);
58+
if (success) {
59+
data_ = new_data;
5960
}
60-
return data_.value();
61+
62+
return data_;
6163
};
6264
};
6365

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp

+14-8
Original file line numberDiff line numberDiff line change
@@ -54,15 +54,19 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
5454
const std::vector<TrajectoryPoint> & traj_points,
5555
const vehicle_info_util::VehicleInfo & vehicle_info,
5656
const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin = 0.0) const;
57-
std::vector<Obstacle> convertToObstacles(const std::vector<TrajectoryPoint> & traj_points) const;
57+
std::vector<Obstacle> convertToObstacles(
58+
const Odometry & odometry, const PredictedObjects & objects,
59+
const std::vector<TrajectoryPoint> & traj_points) const;
5860
std::tuple<std::vector<StopObstacle>, std::vector<CruiseObstacle>, std::vector<SlowDownObstacle>>
5961
determineEgoBehaviorAgainstObstacles(
62+
const Odometry & odometry, const PredictedObjects & objecrts,
6063
const std::vector<TrajectoryPoint> & traj_points, const std::vector<Obstacle> & obstacles);
6164
std::vector<TrajectoryPoint> decimateTrajectoryPoints(
62-
const std::vector<TrajectoryPoint> & traj_points) const;
65+
const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points) const;
6366
std::optional<StopObstacle> createStopObstacle(
64-
const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polys,
65-
const Obstacle & obstacle, const double precise_lateral_dist) const;
67+
const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points,
68+
const std::vector<Polygon2d> & traj_polys, const Obstacle & obstacle,
69+
const double precise_lateral_dist) const;
6670
bool isStopObstacle(const uint8_t label) const;
6771
bool isInsideCruiseObstacle(const uint8_t label) const;
6872
bool isOutsideCruiseObstacle(const uint8_t label) const;
@@ -87,12 +91,14 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
8791
bool isObstacleCrossing(
8892
const std::vector<TrajectoryPoint> & traj_points, const Obstacle & obstacle) const;
8993
double calcCollisionTimeMargin(
90-
const std::vector<PointWithStamp> & collision_points,
94+
const Odometry & odometry, const std::vector<PointWithStamp> & collision_points,
9195
const std::vector<TrajectoryPoint> & traj_points, const bool is_driving_forward) const;
9296
std::optional<SlowDownObstacle> createSlowDownObstacle(
93-
const std::vector<TrajectoryPoint> & traj_points, const Obstacle & obstacle,
94-
const double precise_lat_dist);
95-
PlannerData createPlannerData(const std::vector<TrajectoryPoint> & traj_points) const;
97+
const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points,
98+
const Obstacle & obstacle, const double precise_lat_dist);
99+
PlannerData createPlannerData(
100+
const Odometry & odometry, const AccelWithCovarianceStamped & acc,
101+
const std::vector<TrajectoryPoint> & traj_points) const;
96102

97103
void checkConsistency(
98104
const rclcpp::Time & current_time, const PredictedObjects & predicted_objects,

planning/obstacle_cruise_planner/src/node.cpp

+39-32
Original file line numberDiff line numberDiff line change
@@ -483,12 +483,17 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam(
483483

484484
void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr msg)
485485
{
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) {
489490
return;
490491
}
491492

493+
const auto & ego_odom = *ego_odom_ptr;
494+
const auto & objects = *objects_ptr;
495+
const auto & acc = *acc_ptr;
496+
492497
const auto traj_points = motion_utils::convertToTrajectoryPointArray(*msg);
493498
// check if subscribed variables are ready
494499
if (traj_points.empty()) {
@@ -505,14 +510,14 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms
505510
// (1) with a proper label
506511
// (2) in front of ego
507512
// (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);
509514

510515
// 2. Determine ego's behavior against each obstacle from stop, cruise and slow down.
511516
const auto & [stop_obstacles, cruise_obstacles, slow_down_obstacles] =
512-
determineEgoBehaviorAgainstObstacles(traj_points, target_obstacles);
517+
determineEgoBehaviorAgainstObstacles(ego_odom, objects, traj_points, target_obstacles);
513518

514519
// 3. Create data for planning
515-
const auto planner_data = createPlannerData(traj_points);
520+
const auto planner_data = createPlannerData(ego_odom, acc, traj_points);
516521

517522
// 4. Stop planning
518523
const auto stop_traj_points = planner_ptr_->generateStopTrajectory(planner_data, stop_obstacles);
@@ -599,15 +604,16 @@ std::vector<Polygon2d> ObstacleCruisePlannerNode::createOneStepPolygons(
599604
}
600605

601606
std::vector<Obstacle> ObstacleCruisePlannerNode::convertToObstacles(
607+
const Odometry & odometry, const PredictedObjects & objects,
602608
const std::vector<TrajectoryPoint> & traj_points) const
603609
{
604610
stop_watch_.tic(__func__);
605611

606-
const auto obj_stamp = rclcpp::Time(objects_sub_.getData().header.stamp);
612+
const auto obj_stamp = rclcpp::Time(objects.header.stamp);
607613
const auto & p = behavior_determination_param_;
608614

609615
std::vector<Obstacle> target_obstacles;
610-
for (const auto & predicted_object : objects_sub_.getData().objects) {
616+
for (const auto & predicted_object : objects.objects) {
611617
const auto & object_id =
612618
tier4_autoware_utils::toHexString(predicted_object.object_id).substr(0, 4);
613619
const auto & current_obstacle_pose =
@@ -625,8 +631,7 @@ std::vector<Obstacle> ObstacleCruisePlannerNode::convertToObstacles(
625631
}
626632

627633
// 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);
630635
const auto ego_to_obstacle_distance =
631636
calcDistanceToFrontVehicle(traj_points, ego_idx, current_obstacle_pose.pose.position);
632637
if (!ego_to_obstacle_distance) {
@@ -715,14 +720,15 @@ bool ObstacleCruisePlannerNode::isFrontCollideObstacle(
715720

716721
std::tuple<std::vector<StopObstacle>, std::vector<CruiseObstacle>, std::vector<SlowDownObstacle>>
717722
ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles(
723+
const Odometry & odometry, const PredictedObjects & objects,
718724
const std::vector<TrajectoryPoint> & traj_points, const std::vector<Obstacle> & obstacles)
719725
{
720726
stop_watch_.tic(__func__);
721727

722728
// 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);
724730
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);
726732
debug_data_ptr_->detection_polygons = decimated_traj_polys;
727733

728734
// determine ego's behavior from stop, cruise and slow down
@@ -747,14 +753,14 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles(
747753
cruise_obstacles.push_back(*cruise_obstacle);
748754
continue;
749755
}
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);
752758
if (stop_obstacle) {
753759
stop_obstacles.push_back(*stop_obstacle);
754760
continue;
755761
}
756762
const auto slow_down_obstacle =
757-
createSlowDownObstacle(decimated_traj_points, obstacle, precise_lat_dist);
763+
createSlowDownObstacle(odometry, decimated_traj_points, obstacle, precise_lat_dist);
758764
if (slow_down_obstacle) {
759765
slow_down_obstacles.push_back(*slow_down_obstacle);
760766
continue;
@@ -780,7 +786,7 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles(
780786
slow_down_condition_counter_.removeCounterUnlessUpdated();
781787

782788
// Check target obstacles' consistency
783-
checkConsistency(objects_sub_.getData().header.stamp, objects_sub_.getData(), stop_obstacles);
789+
checkConsistency(objects.header.stamp, objects, stop_obstacles);
784790

785791
// update previous obstacles
786792
prev_stop_obstacles_ = stop_obstacles;
@@ -796,13 +802,12 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles(
796802
}
797803

798804
std::vector<TrajectoryPoint> ObstacleCruisePlannerNode::decimateTrajectoryPoints(
799-
const std::vector<TrajectoryPoint> & traj_points) const
805+
const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points) const
800806
{
801807
const auto & p = behavior_determination_param_;
802808

803809
// 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);
806811
const size_t traj_start_point_idx = ego_seg_idx;
807812
const auto trimmed_traj_points =
808813
std::vector<TrajectoryPoint>(traj_points.begin() + traj_start_point_idx, traj_points.end());
@@ -1121,8 +1126,9 @@ ObstacleCruisePlannerNode::createCollisionPointsForOutsideCruiseObstacle(
11211126
}
11221127

11231128
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
11261132
{
11271133
const auto & p = behavior_determination_param_;
11281134
const auto & object_id = obstacle.uuid.substr(0, 4);
@@ -1173,7 +1179,7 @@ std::optional<StopObstacle> ObstacleCruisePlannerNode::createStopObstacle(
11731179
}
11741180

11751181
const double collision_time_margin =
1176-
calcCollisionTimeMargin(collision_points, traj_points, is_driving_forward_);
1182+
calcCollisionTimeMargin(odometry, collision_points, traj_points, is_driving_forward_);
11771183
if (p.collision_time_margin < collision_time_margin) {
11781184
RCLCPP_INFO_EXPRESSION(
11791185
get_logger(), enable_debug_info_,
@@ -1186,7 +1192,7 @@ std::optional<StopObstacle> ObstacleCruisePlannerNode::createStopObstacle(
11861192

11871193
// calculate collision points with trajectory with lateral stop margin
11881194
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);
11901196

11911197
const auto collision_point = polygon_utils::getCollisionPoint(
11921198
traj_points, traj_polys_with_lat_margin, obstacle, is_driving_forward_, vehicle_info_);
@@ -1201,8 +1207,8 @@ std::optional<StopObstacle> ObstacleCruisePlannerNode::createStopObstacle(
12011207
}
12021208

12031209
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)
12061212
{
12071213
const auto & object_id = obstacle.uuid.substr(0, 4);
12081214
const auto & p = behavior_determination_param_;
@@ -1256,7 +1262,7 @@ std::optional<SlowDownObstacle> ObstacleCruisePlannerNode::createSlowDownObstacl
12561262
// calculate collision points with trajectory with lateral stop margin
12571263
// NOTE: For additional margin, hysteresis is not divided by two.
12581264
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,
12601266
p.max_lat_margin_for_slow_down + p.lat_hysteresis_margin_for_slow_down);
12611267

12621268
std::vector<Polygon2d> front_collision_polygons;
@@ -1416,11 +1422,11 @@ bool ObstacleCruisePlannerNode::isObstacleCrossing(
14161422
}
14171423

14181424
double ObstacleCruisePlannerNode::calcCollisionTimeMargin(
1419-
const std::vector<PointWithStamp> & collision_points,
1425+
const Odometry & odometry, const std::vector<PointWithStamp> & collision_points,
14201426
const std::vector<TrajectoryPoint> & traj_points, const bool is_driving_forward) const
14211427
{
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;
14241430

14251431
const double time_to_reach_collision_point = [&]() {
14261432
const double abs_ego_offset = is_driving_forward
@@ -1447,14 +1453,15 @@ double ObstacleCruisePlannerNode::calcCollisionTimeMargin(
14471453
}
14481454

14491455
PlannerData ObstacleCruisePlannerNode::createPlannerData(
1456+
const Odometry & odometry, const AccelWithCovarianceStamped & acc,
14501457
const std::vector<TrajectoryPoint> & traj_points) const
14511458
{
14521459
PlannerData planner_data;
14531460
planner_data.current_time = now();
14541461
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;
14581465
planner_data.is_driving_forward = is_driving_forward_;
14591466
return planner_data;
14601467
}

0 commit comments

Comments
 (0)