From 951b90231597a745be855145a2d61a64b1ef7145 Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Wed, 3 Apr 2024 21:00:30 +0900 Subject: [PATCH] feat(tier4_autoware_utils, obstacle_cruise): change to read topic by polling (#6702) change to read topic by polling Signed-off-by: Yuki Takagi --- .../ros/polling_subscriber.hpp | 65 +++++++++++++++++++ .../include/obstacle_cruise_planner/node.hpp | 15 ++--- planning/obstacle_cruise_planner/src/node.cpp | 45 ++++++------- 3 files changed, 93 insertions(+), 32 deletions(-) create mode 100644 common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp 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 new file mode 100644 index 0000000000000..f4d230e15dcf1 --- /dev/null +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp @@ -0,0 +1,65 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TIER4_AUTOWARE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_ +#define TIER4_AUTOWARE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_ + +#include + +#include + +namespace tier4_autoware_utils +{ + +template +class InterProcessPollingSubscriber +{ +private: + typename rclcpp::Subscription::SharedPtr subscriber_; + std::optional data_; + +public: + explicit InterProcessPollingSubscriber(rclcpp::Node * node, const std::string & topic_name) + { + 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( + topic_name, rclcpp::QoS{1}, [node](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; + } + 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(); + }; +}; + +} // namespace tier4_autoware_utils + +#endif // TIER4_AUTOWARE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_ 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 d1a9d0ff56b52..c368265ea0f66 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -21,6 +21,7 @@ #include "obstacle_cruise_planner/type_alias.hpp" #include "signal_processing/lowpass_filter_1d.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" +#include "tier4_autoware_utils/ros/polling_subscriber.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" #include @@ -138,14 +139,12 @@ class ObstacleCruisePlannerNode : public rclcpp::Node // subscriber rclcpp::Subscription::SharedPtr traj_sub_; - rclcpp::Subscription::SharedPtr objects_sub_; - rclcpp::Subscription::SharedPtr odom_sub_; - rclcpp::Subscription::SharedPtr acc_sub_; - - // data for callback functions - PredictedObjects::ConstSharedPtr objects_ptr_{nullptr}; - Odometry::ConstSharedPtr ego_odom_ptr_{nullptr}; - AccelWithCovarianceStamped::ConstSharedPtr ego_accel_ptr_{nullptr}; + tier4_autoware_utils::InterProcessPollingSubscriber ego_odom_sub_{ + this, "~/input/odometry"}; + tier4_autoware_utils::InterProcessPollingSubscriber objects_sub_{ + this, "~/input/objects"}; + tier4_autoware_utils::InterProcessPollingSubscriber acc_sub_{ + this, "~/input/acceleration"}; // Vehicle Parameters VehicleInfo vehicle_info_; diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 2d1b70d1745f9..a069b4b6f9395 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -357,15 +357,6 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & traj_sub_ = create_subscription( "~/input/trajectory", rclcpp::QoS{1}, std::bind(&ObstacleCruisePlannerNode::onTrajectory, this, _1)); - objects_sub_ = create_subscription( - "~/input/objects", rclcpp::QoS{1}, - [this](const PredictedObjects::ConstSharedPtr msg) { objects_ptr_ = msg; }); - odom_sub_ = create_subscription( - "~/input/odometry", rclcpp::QoS{1}, - [this](const Odometry::ConstSharedPtr msg) { ego_odom_ptr_ = msg; }); - acc_sub_ = create_subscription( - "~/input/acceleration", rclcpp::QoS{1}, - [this](const AccelWithCovarianceStamped::ConstSharedPtr msg) { ego_accel_ptr_ = msg; }); // publisher trajectory_pub_ = create_publisher("~/output/trajectory", 1); @@ -493,10 +484,15 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr msg) { - const auto traj_points = motion_utils::convertToTrajectoryPointArray(*msg); + if ( + !ego_odom_sub_.updateLatestData() || !objects_sub_.updateLatestData() || + !acc_sub_.updateLatestData()) { + return; + } + const auto traj_points = motion_utils::convertToTrajectoryPointArray(*msg); // check if subscribed variables are ready - if (traj_points.empty() || !ego_odom_ptr_ || !ego_accel_ptr_ || !objects_ptr_) { + if (traj_points.empty()) { return; } @@ -609,11 +605,11 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( { stop_watch_.tic(__func__); - const auto obj_stamp = rclcpp::Time(objects_ptr_->header.stamp); + const auto obj_stamp = rclcpp::Time(objects_sub_.getData().header.stamp); const auto & p = behavior_determination_param_; std::vector target_obstacles; - for (const auto & predicted_object : objects_ptr_->objects) { + for (const auto & predicted_object : objects_sub_.getData().objects) { const auto & object_id = tier4_autoware_utils::toHexString(predicted_object.object_id).substr(0, 4); const auto & current_obstacle_pose = @@ -631,7 +627,8 @@ std::vector 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_ptr_->pose.pose); + const size_t ego_idx = + ego_nearest_param_.findIndex(traj_points, ego_odom_sub_.getData().pose.pose); const auto ego_to_obstacle_distance = calcDistanceToFrontVehicle(traj_points, ego_idx, current_obstacle_pose.pose.position); if (!ego_to_obstacle_distance) { @@ -727,7 +724,7 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( // calculated decimated trajectory points and trajectory polygon const auto decimated_traj_points = decimateTrajectoryPoints(traj_points); const auto decimated_traj_polys = - createOneStepPolygons(decimated_traj_points, vehicle_info_, ego_odom_ptr_->pose.pose); + createOneStepPolygons(decimated_traj_points, vehicle_info_, ego_odom_sub_.getData().pose.pose); debug_data_ptr_->detection_polygons = decimated_traj_polys; // determine ego's behavior from stop, cruise and slow down @@ -785,7 +782,7 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( slow_down_condition_counter_.removeCounterUnlessUpdated(); // Check target obstacles' consistency - checkConsistency(objects_ptr_->header.stamp, *objects_ptr_, stop_obstacles); + checkConsistency(objects_sub_.getData().header.stamp, objects_sub_.getData(), stop_obstacles); // update previous obstacles prev_stop_obstacles_ = stop_obstacles; @@ -807,7 +804,7 @@ std::vector ObstacleCruisePlannerNode::decimateTrajectoryPoints // trim trajectory const size_t ego_seg_idx = - ego_nearest_param_.findSegmentIndex(traj_points, ego_odom_ptr_->pose.pose); + ego_nearest_param_.findSegmentIndex(traj_points, ego_odom_sub_.getData().pose.pose); const size_t traj_start_point_idx = ego_seg_idx; const auto trimmed_traj_points = std::vector(traj_points.begin() + traj_start_point_idx, traj_points.end()); @@ -1191,7 +1188,7 @@ std::optional 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_ptr_->pose.pose, p.max_lat_margin_for_stop); + traj_points, vehicle_info_, ego_odom_sub_.getData().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_); @@ -1261,7 +1258,7 @@ std::optional 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_ptr_->pose.pose, + traj_points, vehicle_info_, ego_odom_sub_.getData().pose.pose, p.max_lat_margin_for_slow_down + p.lat_hysteresis_margin_for_slow_down); std::vector front_collision_polygons; @@ -1424,8 +1421,8 @@ double ObstacleCruisePlannerNode::calcCollisionTimeMargin( const std::vector & collision_points, const std::vector & traj_points, const bool is_driving_forward) const { - const auto & ego_pose = ego_odom_ptr_->pose.pose; - const double ego_vel = ego_odom_ptr_->twist.twist.linear.x; + const auto & ego_pose = ego_odom_sub_.getData().pose.pose; + const double ego_vel = ego_odom_sub_.getData().twist.twist.linear.x; const double time_to_reach_collision_point = [&]() { const double abs_ego_offset = is_driving_forward @@ -1457,9 +1454,9 @@ PlannerData ObstacleCruisePlannerNode::createPlannerData( PlannerData planner_data; planner_data.current_time = now(); planner_data.traj_points = traj_points; - planner_data.ego_pose = ego_odom_ptr_->pose.pose; - planner_data.ego_vel = ego_odom_ptr_->twist.twist.linear.x; - planner_data.ego_acc = ego_accel_ptr_->accel.accel.linear.x; + 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.is_driving_forward = is_driving_forward_; return planner_data; }