diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp index 3677e6c5fb075..db17496288766 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp +++ b/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 @@ -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::SharedPtr traj_pub_; rclcpp::Publisher::SharedPtr virtual_wall_pub_; // interface subscriber rclcpp::Subscription::SharedPtr path_sub_; - rclcpp::Subscription::SharedPtr odom_sub_; + tier4_autoware_utils::InterProcessPollingSubscriber ego_odom_sub_{ + this, "~/input/odometry"}; // debug publisher rclcpp::Publisher::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 generateOptimizedTrajectory(const PlannerData & planner_data); std::vector extendTrajectory( const std::vector & traj_points, diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index c1da988eb5c25..b05d3f9da0c57 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -95,8 +95,6 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n // interface subscriber path_sub_ = create_subscription( "~/input/path", 1, std::bind(&ObstacleAvoidancePlanner::onPath, this, std::placeholders::_1)); - odom_sub_ = create_subscription( - "~/input/odometry", 1, [this](const Odometry::ConstSharedPtr msg) { ego_state_ptr_ = msg; }); // debug publisher debug_extended_traj_pub_ = create_publisher("~/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,7 +298,8 @@ 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; @@ -305,8 +307,8 @@ PlannerData ObstacleAvoidancePlanner::createPlannerData(const Path & path) const 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;