feat(obstacle_avoidance_planner): use polling subscriber

Signed-off-by: Takayuki Murooka <[email protected]>

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp

@@ -22,6 +22,7 @@
 #include "obstacle_avoidance_planner/type_alias.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "tier4_autoware_utils/ros/logger_level_configure.hpp"
+#include "tier4_autoware_utils/ros/polling_subscriber.hpp"
 #include "vehicle_info_util/vehicle_info_util.hpp"
 
 #include <tier4_autoware_utils/ros/published_time_publisher.hpp>
@@ -83,16 +84,14 @@ class ObstacleAvoidancePlanner : public rclcpp::Node
   TrajectoryParam traj_param_{};
   EgoNearestParam ego_nearest_param_{};
 
-  // variables for subscribers
-  Odometry::ConstSharedPtr ego_state_ptr_;
-
   // interface publisher
   rclcpp::Publisher<Trajectory>::SharedPtr traj_pub_;
   rclcpp::Publisher<MarkerArray>::SharedPtr virtual_wall_pub_;
 
   // interface subscriber
   rclcpp::Subscription<Path>::SharedPtr path_sub_;
-  rclcpp::Subscription<Odometry>::SharedPtr odom_sub_;
+  tier4_autoware_utils::InterProcessPollingSubscriber<Odometry> ego_odom_sub_{
+    this, "~/input/odometry"};
 
   // debug publisher
   rclcpp::Publisher<Trajectory>::SharedPtr debug_extended_traj_pub_;
@@ -113,8 +112,9 @@ class ObstacleAvoidancePlanner : public rclcpp::Node
   void resetPreviousData();
 
   // main functions
-  bool isDataReady(const Path & path, rclcpp::Clock clock) const;
-  PlannerData createPlannerData(const Path & path) const;
+  bool checkInputPath(const Path & path, rclcpp::Clock clock) const;
+  PlannerData createPlannerData(
+    const Path & path, const Odometry::ConstSharedPtr ego_odom_ptr) const;
   std::vector<TrajectoryPoint> generateOptimizedTrajectory(const PlannerData & planner_data);
   std::vector<TrajectoryPoint> extendTrajectory(
     const std::vector<TrajectoryPoint> & traj_points,

planning/obstacle_avoidance_planner/src/node.cpp

@@ -95,8 +95,6 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n
   // interface subscriber
   path_sub_ = create_subscription<Path>(
     "~/input/path", 1, std::bind(&ObstacleAvoidancePlanner::onPath, this, std::placeholders::_1));
-  odom_sub_ = create_subscription<Odometry>(
-    "~/input/odometry", 1, [this](const Odometry::ConstSharedPtr msg) { ego_state_ptr_ = msg; });
 
   // debug publisher
   debug_extended_traj_pub_ = create_publisher<Trajectory>("~/debug/extended_traj", 1);
@@ -224,8 +222,16 @@ void ObstacleAvoidancePlanner::onPath(const Path::ConstSharedPtr path_ptr)
   time_keeper_ptr_->init();
   time_keeper_ptr_->tic(__func__);
 
-  // check if data is ready and valid
-  if (!isDataReady(*path_ptr, *get_clock())) {
+  // check if input path is valid
+  if (!checkInputPath(*path_ptr, *get_clock())) {
+    return;
+  }
+
+  // check if ego's odometry is valid
+  const auto ego_odom_ptr = ego_odom_sub_.takeData();
+  if (!ego_odom_ptr) {
+    RCLCPP_INFO_SKIPFIRST_THROTTLE(
+      get_logger(), *get_clock(), 5000, "Waiting for ego pose and twist.");
     return;
   }
 
@@ -245,7 +251,7 @@ void ObstacleAvoidancePlanner::onPath(const Path::ConstSharedPtr path_ptr)
   }
 
   // 1. create planner data
-  const auto planner_data = createPlannerData(*path_ptr);
+  const auto planner_data = createPlannerData(*path_ptr, ego_odom_ptr);
 
   // 2. generate optimized trajectory
   const auto optimized_traj_points = generateOptimizedTrajectory(planner_data);
@@ -276,13 +282,8 @@ void ObstacleAvoidancePlanner::onPath(const Path::ConstSharedPtr path_ptr)
   published_time_publisher_->publish_if_subscribed(traj_pub_, output_traj_msg.header.stamp);
 }
 
-bool ObstacleAvoidancePlanner::isDataReady(const Path & path, rclcpp::Clock clock) const
+bool ObstacleAvoidancePlanner::checkInputPath(const Path & path, rclcpp::Clock clock) const
 {
-  if (!ego_state_ptr_) {
-    RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), clock, 5000, "Waiting for ego pose and twist.");
-    return false;
-  }
-
   if (path.points.size() < 2) {
     RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), clock, 5000, "Path points size is less than 1.");
     return false;
@@ -297,16 +298,17 @@ bool ObstacleAvoidancePlanner::isDataReady(const Path & path, rclcpp::Clock cloc
   return true;
 }
 
-PlannerData ObstacleAvoidancePlanner::createPlannerData(const Path & path) const
+PlannerData ObstacleAvoidancePlanner::createPlannerData(
+  const Path & path, const Odometry::ConstSharedPtr ego_odom_ptr) const
 {
   // create planner data
   PlannerData planner_data;
   planner_data.header = path.header;
   planner_data.traj_points = trajectory_utils::convertToTrajectoryPoints(path.points);
   planner_data.left_bound = path.left_bound;
   planner_data.right_bound = path.right_bound;
-  planner_data.ego_pose = ego_state_ptr_->pose.pose;
-  planner_data.ego_vel = ego_state_ptr_->twist.twist.linear.x;
+  planner_data.ego_pose = ego_odom_ptr->pose.pose;
+  planner_data.ego_vel = ego_odom_ptr->twist.twist.linear.x;
 
   debug_data_ptr_->ego_pose = planner_data.ego_pose;
   return planner_data;