From d1b62e278c3364a295368b2e694180b2a481c85a Mon Sep 17 00:00:00 2001
From: Mamoru Sobue <mamoru.sobue@tier4.jp>
Date: Fri, 10 May 2024 18:37:47 +0900
Subject: [PATCH 1/2] feat(tier4_autoware_utils): default QoS setting of
polling subscriber
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
---
.../ros/polling_subscriber.hpp | 18 +++++++++++++++---
1 file changed, 15 insertions(+), 3 deletions(-)
diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp
index f8d5baaf02777..5dd17ddc69744 100644
--- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp
+++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp
@@ -30,7 +30,8 @@ class InterProcessPollingSubscriber
std::optional<T> data_;
public:
- explicit InterProcessPollingSubscriber(rclcpp::Node * node, const std::string & topic_name)
+ explicit InterProcessPollingSubscriber(
+ rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos = rclcpp::QoS{1})
{
auto noexec_callback_group =
node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
@@ -38,16 +39,27 @@ class InterProcessPollingSubscriber
noexec_subscription_options.callback_group = noexec_callback_group;
subscriber_ = node->create_subscription<T>(
- topic_name, rclcpp::QoS{1},
+ topic_name, qos,
[node]([[maybe_unused]] const typename T::ConstSharedPtr msg) { assert(false); },
noexec_subscription_options);
+ if (qos.get_rmw_qos_profile().depth > 1) {
+ RCLCPP_WARN(
+ node->get_logger(),
+ "InterProcessPollingSubscriber will be used with depth > 1, which may cause inefficient "
+ "serialization while updateLatestData()");
+ }
};
bool updateLatestData()
{
rclcpp::MessageInfo message_info;
T tmp;
// The queue size (QoS) must be 1 to get the last message data.
- if (subscriber_->take(tmp, message_info)) {
+ bool is_latest_message_consumed = false;
+ // pop the queue until latest data
+ while (subscriber_->take(tmp, message_info)) {
+ is_latest_message_consumed = true;
+ }
+ if (is_latest_message_consumed) {
data_ = tmp;
}
return data_.has_value();
From f0058836be04bce1064561ba488eb87b0a42c10e Mon Sep 17 00:00:00 2001
From: Mamoru Sobue <mamoru.sobue@tier4.jp>
Date: Thu, 16 May 2024 16:30:39 +0900
Subject: [PATCH 2/2] change API
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
---
.../ros/polling_subscriber.hpp | 32 +++------
.../include/obstacle_cruise_planner/node.hpp | 22 +++---
planning/obstacle_cruise_planner/src/node.cpp | 71 ++++++++++---------
3 files changed, 64 insertions(+), 61 deletions(-)
diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp
index 5dd17ddc69744..b842261d56cfa 100644
--- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp
+++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp
@@ -17,6 +17,8 @@
#include <rclcpp/rclcpp.hpp>
+#include <memory>
+#include <stdexcept>
#include <string>
namespace tier4_autoware_utils
@@ -27,7 +29,7 @@ class InterProcessPollingSubscriber
{
private:
typename rclcpp::Subscription<T>::SharedPtr subscriber_;
- std::optional<T> data_;
+ typename T::SharedPtr data_;
public:
explicit InterProcessPollingSubscriber(
@@ -43,33 +45,21 @@ class InterProcessPollingSubscriber
[node]([[maybe_unused]] const typename T::ConstSharedPtr msg) { assert(false); },
noexec_subscription_options);
if (qos.get_rmw_qos_profile().depth > 1) {
- RCLCPP_WARN(
- node->get_logger(),
+ throw std::invalid_argument(
"InterProcessPollingSubscriber will be used with depth > 1, which may cause inefficient "
"serialization while updateLatestData()");
}
};
- bool updateLatestData()
+ typename T::ConstSharedPtr takeData()
{
+ auto new_data = std::make_shared<T>();
rclcpp::MessageInfo message_info;
- T tmp;
- // The queue size (QoS) must be 1 to get the last message data.
- bool is_latest_message_consumed = false;
- // pop the queue until latest data
- while (subscriber_->take(tmp, message_info)) {
- is_latest_message_consumed = true;
+ const bool success = subscriber_->take(*new_data, message_info);
+ if (success) {
+ data_ = new_data;
}
- if (is_latest_message_consumed) {
- data_ = tmp;
- }
- return data_.has_value();
- };
- const T & getData() const
- {
- if (!data_.has_value()) {
- throw std::runtime_error("Bad_optional_access in class InterProcessPollingSubscriber");
- }
- return data_.value();
+
+ return data_;
};
};
diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp
index c368265ea0f66..a819f000412b1 100644
--- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp
+++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp
@@ -55,15 +55,19 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
const std::vector<TrajectoryPoint> & traj_points,
const vehicle_info_util::VehicleInfo & vehicle_info,
const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin = 0.0) const;
- std::vector<Obstacle> convertToObstacles(const std::vector<TrajectoryPoint> & traj_points) const;
+ std::vector<Obstacle> convertToObstacles(
+ const Odometry & odometry, const PredictedObjects & objects,
+ const std::vector<TrajectoryPoint> & traj_points) const;
std::tuple<std::vector<StopObstacle>, std::vector<CruiseObstacle>, std::vector<SlowDownObstacle>>
determineEgoBehaviorAgainstObstacles(
+ const Odometry & odometry, const PredictedObjects & objecrts,
const std::vector<TrajectoryPoint> & traj_points, const std::vector<Obstacle> & obstacles);
std::vector<TrajectoryPoint> decimateTrajectoryPoints(
- const std::vector<TrajectoryPoint> & traj_points) const;
+ const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points) const;
std::optional<StopObstacle> createStopObstacle(
- const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polys,
- const Obstacle & obstacle, const double precise_lateral_dist) const;
+ const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points,
+ const std::vector<Polygon2d> & traj_polys, const Obstacle & obstacle,
+ const double precise_lateral_dist) const;
bool isStopObstacle(const uint8_t label) const;
bool isInsideCruiseObstacle(const uint8_t label) const;
bool isOutsideCruiseObstacle(const uint8_t label) const;
@@ -88,12 +92,14 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
bool isObstacleCrossing(
const std::vector<TrajectoryPoint> & traj_points, const Obstacle & obstacle) const;
double calcCollisionTimeMargin(
- const std::vector<PointWithStamp> & collision_points,
+ const Odometry & odometry, const std::vector<PointWithStamp> & collision_points,
const std::vector<TrajectoryPoint> & traj_points, const bool is_driving_forward) const;
std::optional<SlowDownObstacle> createSlowDownObstacle(
- const std::vector<TrajectoryPoint> & traj_points, const Obstacle & obstacle,
- const double precise_lat_dist);
- PlannerData createPlannerData(const std::vector<TrajectoryPoint> & traj_points) const;
+ const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points,
+ const Obstacle & obstacle, const double precise_lat_dist);
+ PlannerData createPlannerData(
+ const Odometry & odometry, const AccelWithCovarianceStamped & acc,
+ const std::vector<TrajectoryPoint> & traj_points) const;
void checkConsistency(
const rclcpp::Time & current_time, const PredictedObjects & predicted_objects,
diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp
index 43400acb86bd9..94e9707d55c4f 100644
--- a/planning/obstacle_cruise_planner/src/node.cpp
+++ b/planning/obstacle_cruise_planner/src/node.cpp
@@ -484,12 +484,17 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam(
void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr msg)
{
- if (
- !ego_odom_sub_.updateLatestData() || !objects_sub_.updateLatestData() ||
- !acc_sub_.updateLatestData()) {
+ const auto ego_odom_ptr = ego_odom_sub_.takeData();
+ const auto objects_ptr = objects_sub_.takeData();
+ const auto acc_ptr = acc_sub_.takeData();
+ if (!ego_odom_ptr || !objects_ptr || !acc_ptr) {
return;
}
+ const auto & ego_odom = *ego_odom_ptr;
+ const auto & objects = *objects_ptr;
+ const auto & acc = *acc_ptr;
+
const auto traj_points = motion_utils::convertToTrajectoryPointArray(*msg);
// check if subscribed variables are ready
if (traj_points.empty()) {
@@ -506,14 +511,14 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms
// (1) with a proper label
// (2) in front of ego
// (3) not too far from trajectory
- const auto target_obstacles = convertToObstacles(traj_points);
+ const auto target_obstacles = convertToObstacles(ego_odom, objects, traj_points);
// 2. Determine ego's behavior against each obstacle from stop, cruise and slow down.
const auto & [stop_obstacles, cruise_obstacles, slow_down_obstacles] =
- determineEgoBehaviorAgainstObstacles(traj_points, target_obstacles);
+ determineEgoBehaviorAgainstObstacles(ego_odom, objects, traj_points, target_obstacles);
// 3. Create data for planning
- const auto planner_data = createPlannerData(traj_points);
+ const auto planner_data = createPlannerData(ego_odom, acc, traj_points);
// 4. Stop planning
const auto stop_traj_points = planner_ptr_->generateStopTrajectory(planner_data, stop_obstacles);
@@ -629,15 +634,16 @@ std::vector<Polygon2d> ObstacleCruisePlannerNode::createOneStepPolygons(
}
std::vector<Obstacle> ObstacleCruisePlannerNode::convertToObstacles(
+ const Odometry & odometry, const PredictedObjects & objects,
const std::vector<TrajectoryPoint> & traj_points) const
{
stop_watch_.tic(__func__);
- const auto obj_stamp = rclcpp::Time(objects_sub_.getData().header.stamp);
+ const auto obj_stamp = rclcpp::Time(objects.header.stamp);
const auto & p = behavior_determination_param_;
std::vector<Obstacle> target_obstacles;
- for (const auto & predicted_object : objects_sub_.getData().objects) {
+ for (const auto & predicted_object : objects.objects) {
const auto & object_id =
tier4_autoware_utils::toHexString(predicted_object.object_id).substr(0, 4);
const auto & current_obstacle_pose =
@@ -655,8 +661,7 @@ std::vector<Obstacle> ObstacleCruisePlannerNode::convertToObstacles(
}
// 2. Check if the obstacle is in front of the ego.
- const size_t ego_idx =
- ego_nearest_param_.findIndex(traj_points, ego_odom_sub_.getData().pose.pose);
+ const size_t ego_idx = ego_nearest_param_.findIndex(traj_points, odometry.pose.pose);
const auto ego_to_obstacle_distance =
calcDistanceToFrontVehicle(traj_points, ego_idx, current_obstacle_pose.pose.position);
if (!ego_to_obstacle_distance) {
@@ -745,14 +750,15 @@ bool ObstacleCruisePlannerNode::isFrontCollideObstacle(
std::tuple<std::vector<StopObstacle>, std::vector<CruiseObstacle>, std::vector<SlowDownObstacle>>
ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles(
+ const Odometry & odometry, const PredictedObjects & objects,
const std::vector<TrajectoryPoint> & traj_points, const std::vector<Obstacle> & obstacles)
{
stop_watch_.tic(__func__);
// calculated decimated trajectory points and trajectory polygon
- const auto decimated_traj_points = decimateTrajectoryPoints(traj_points);
+ const auto decimated_traj_points = decimateTrajectoryPoints(odometry, traj_points);
const auto decimated_traj_polys =
- createOneStepPolygons(decimated_traj_points, vehicle_info_, ego_odom_sub_.getData().pose.pose);
+ createOneStepPolygons(decimated_traj_points, vehicle_info_, odometry.pose.pose);
debug_data_ptr_->detection_polygons = decimated_traj_polys;
// determine ego's behavior from stop, cruise and slow down
@@ -777,14 +783,14 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles(
cruise_obstacles.push_back(*cruise_obstacle);
continue;
}
- const auto stop_obstacle =
- createStopObstacle(decimated_traj_points, decimated_traj_polys, obstacle, precise_lat_dist);
+ const auto stop_obstacle = createStopObstacle(
+ odometry, decimated_traj_points, decimated_traj_polys, obstacle, precise_lat_dist);
if (stop_obstacle) {
stop_obstacles.push_back(*stop_obstacle);
continue;
}
const auto slow_down_obstacle =
- createSlowDownObstacle(decimated_traj_points, obstacle, precise_lat_dist);
+ createSlowDownObstacle(odometry, decimated_traj_points, obstacle, precise_lat_dist);
if (slow_down_obstacle) {
slow_down_obstacles.push_back(*slow_down_obstacle);
continue;
@@ -810,7 +816,7 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles(
slow_down_condition_counter_.removeCounterUnlessUpdated();
// Check target obstacles' consistency
- checkConsistency(objects_sub_.getData().header.stamp, objects_sub_.getData(), stop_obstacles);
+ checkConsistency(objects.header.stamp, objects, stop_obstacles);
// update previous obstacles
prev_stop_obstacles_ = stop_obstacles;
@@ -826,13 +832,12 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles(
}
std::vector<TrajectoryPoint> ObstacleCruisePlannerNode::decimateTrajectoryPoints(
- const std::vector<TrajectoryPoint> & traj_points) const
+ const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points) const
{
const auto & p = behavior_determination_param_;
// trim trajectory
- const size_t ego_seg_idx =
- ego_nearest_param_.findSegmentIndex(traj_points, ego_odom_sub_.getData().pose.pose);
+ const size_t ego_seg_idx = ego_nearest_param_.findSegmentIndex(traj_points, odometry.pose.pose);
const size_t traj_start_point_idx = ego_seg_idx;
const auto trimmed_traj_points =
std::vector<TrajectoryPoint>(traj_points.begin() + traj_start_point_idx, traj_points.end());
@@ -1151,8 +1156,9 @@ ObstacleCruisePlannerNode::createCollisionPointsForOutsideCruiseObstacle(
}
std::optional<StopObstacle> ObstacleCruisePlannerNode::createStopObstacle(
- const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polys,
- const Obstacle & obstacle, const double precise_lat_dist) const
+ const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points,
+ const std::vector<Polygon2d> & traj_polys, const Obstacle & obstacle,
+ const double precise_lat_dist) const
{
const auto & p = behavior_determination_param_;
const auto & object_id = obstacle.uuid.substr(0, 4);
@@ -1203,7 +1209,7 @@ std::optional<StopObstacle> ObstacleCruisePlannerNode::createStopObstacle(
}
const double collision_time_margin =
- calcCollisionTimeMargin(collision_points, traj_points, is_driving_forward_);
+ calcCollisionTimeMargin(odometry, collision_points, traj_points, is_driving_forward_);
if (p.collision_time_margin < collision_time_margin) {
RCLCPP_INFO_EXPRESSION(
get_logger(), enable_debug_info_,
@@ -1216,7 +1222,7 @@ std::optional<StopObstacle> ObstacleCruisePlannerNode::createStopObstacle(
// calculate collision points with trajectory with lateral stop margin
const auto traj_polys_with_lat_margin = createOneStepPolygons(
- traj_points, vehicle_info_, ego_odom_sub_.getData().pose.pose, p.max_lat_margin_for_stop);
+ traj_points, vehicle_info_, odometry.pose.pose, p.max_lat_margin_for_stop);
const auto collision_point = polygon_utils::getCollisionPoint(
traj_points, traj_polys_with_lat_margin, obstacle, is_driving_forward_, vehicle_info_);
@@ -1231,8 +1237,8 @@ std::optional<StopObstacle> ObstacleCruisePlannerNode::createStopObstacle(
}
std::optional<SlowDownObstacle> ObstacleCruisePlannerNode::createSlowDownObstacle(
- const std::vector<TrajectoryPoint> & traj_points, const Obstacle & obstacle,
- const double precise_lat_dist)
+ const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points,
+ const Obstacle & obstacle, const double precise_lat_dist)
{
const auto & object_id = obstacle.uuid.substr(0, 4);
const auto & p = behavior_determination_param_;
@@ -1286,7 +1292,7 @@ std::optional<SlowDownObstacle> ObstacleCruisePlannerNode::createSlowDownObstacl
// calculate collision points with trajectory with lateral stop margin
// NOTE: For additional margin, hysteresis is not divided by two.
const auto traj_polys_with_lat_margin = createOneStepPolygons(
- traj_points, vehicle_info_, ego_odom_sub_.getData().pose.pose,
+ traj_points, vehicle_info_, odometry.pose.pose,
p.max_lat_margin_for_slow_down + p.lat_hysteresis_margin_for_slow_down);
std::vector<Polygon2d> front_collision_polygons;
@@ -1446,11 +1452,11 @@ bool ObstacleCruisePlannerNode::isObstacleCrossing(
}
double ObstacleCruisePlannerNode::calcCollisionTimeMargin(
- const std::vector<PointWithStamp> & collision_points,
+ const Odometry & odometry, const std::vector<PointWithStamp> & collision_points,
const std::vector<TrajectoryPoint> & traj_points, const bool is_driving_forward) const
{
- const auto & ego_pose = ego_odom_sub_.getData().pose.pose;
- const double ego_vel = ego_odom_sub_.getData().twist.twist.linear.x;
+ const auto & ego_pose = odometry.pose.pose;
+ const double ego_vel = odometry.twist.twist.linear.x;
const double time_to_reach_collision_point = [&]() {
const double abs_ego_offset = is_driving_forward
@@ -1477,14 +1483,15 @@ double ObstacleCruisePlannerNode::calcCollisionTimeMargin(
}
PlannerData ObstacleCruisePlannerNode::createPlannerData(
+ const Odometry & odometry, const AccelWithCovarianceStamped & acc,
const std::vector<TrajectoryPoint> & traj_points) const
{
PlannerData planner_data;
planner_data.current_time = now();
planner_data.traj_points = traj_points;
- planner_data.ego_pose = ego_odom_sub_.getData().pose.pose;
- planner_data.ego_vel = ego_odom_sub_.getData().twist.twist.linear.x;
- planner_data.ego_acc = acc_sub_.getData().accel.accel.linear.x;
+ planner_data.ego_pose = odometry.pose.pose;
+ planner_data.ego_vel = odometry.twist.twist.linear.x;
+ planner_data.ego_acc = acc.accel.accel.linear.x;
planner_data.is_driving_forward = is_driving_forward_;
return planner_data;
}