Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(tier4_autoware_utils): default QoS setting of polling subscriber #6976

Merged
merged 2 commits into from
May 16, 2024
Merged

feat(tier4_autoware_utils): default QoS setting of polling subscriber #6976

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
d1b62e2
feat(tier4_autoware_utils): default QoS setting of polling subscriber
soblin May 10, 2024
f005883
change API
soblin May 16, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
34 changes: 18 additions & 16 deletions common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,8 @@

#include <rclcpp/rclcpp.hpp>

#include <memory>
#include <stdexcept>
#include <string>

namespace tier4_autoware_utils
Expand All @@ -27,37 +29,37 @@ class InterProcessPollingSubscriber
{
private:
typename rclcpp::Subscription<T>::SharedPtr subscriber_;
std::optional<T> data_;
typename T::SharedPtr 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);
auto noexec_subscription_options = rclcpp::SubscriptionOptions();
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);
};
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)) {
data_ = tmp;
if (qos.get_rmw_qos_profile().depth > 1) {
throw std::invalid_argument(
"InterProcessPollingSubscriber will be used with depth > 1, which may cause inefficient "
"serialization while updateLatestData()");
}
return data_.has_value();
};
const T & getData() const
typename T::ConstSharedPtr takeData()
{
if (!data_.has_value()) {
throw std::runtime_error("Bad_optional_access in class InterProcessPollingSubscriber");
auto new_data = std::make_shared<T>();
rclcpp::MessageInfo message_info;
const bool success = subscriber_->take(*new_data, message_info);
if (success) {
data_ = new_data;
}
return data_.value();

return data_;
};
};

Expand Down
22 changes: 14 additions & 8 deletions planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,15 +55,19 @@
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,

Check warning on line 63 in planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp

View workflow job for this annotation

GitHub Actions / spell-check-partial 

Unknown word (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;
Expand All @@ -88,12 +92,14 @@
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,
Expand Down
71 changes: 39 additions & 32 deletions planning/obstacle_cruise_planner/src/node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2022 TIER IV, Inc.

Check notice on line 1 in planning/obstacle_cruise_planner/src/node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Lines of Code in a Single File 

The lines of code increases from 1235 to 1241, improve code health by reducing it to 1000. The number of Lines of Code in a single file. More Lines of Code lowers the code health.

Check notice on line 1 in planning/obstacle_cruise_planner/src/node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Primitive Obsession 

The ratio of primitive types in function arguments decreases from 32.69% to 30.91%, threshold = 30.0%. The functions in this file have too many primitive types (e.g. int, double, float) in their function argument lists. Using many primitive types lead to the code smell Primitive Obsession. Avoid adding more primitive arguments.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -484,12 +484,17 @@

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()) {
Expand All @@ -506,14 +511,14 @@
// (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);
Expand Down Expand Up @@ -629,15 +634,16 @@
}

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 =
Expand All @@ -655,8 +661,7 @@
}

// 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) {
Expand Down Expand Up @@ -745,14 +750,15 @@

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
Expand All @@ -777,14 +783,14 @@
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;
Expand All @@ -810,7 +816,7 @@
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;
Expand All @@ -826,13 +832,12 @@
}

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());
Expand Down Expand Up @@ -1151,8 +1156,9 @@
}

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);
Expand Down Expand Up @@ -1203,7 +1209,7 @@
}

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_,
Expand All @@ -1216,7 +1222,7 @@

// 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_);
Expand All @@ -1231,8 +1237,8 @@
}

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_;
Expand Down Expand Up @@ -1286,7 +1292,7 @@
// 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;
Expand Down Expand Up @@ -1446,11 +1452,11 @@
}

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
Expand All @@ -1477,14 +1483,15 @@
}

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;
}
Expand Down
Loading