diff --git a/control/lane_departure_checker/include/autoware_lane_departure_checker/lane_departure_checker_node.hpp b/control/lane_departure_checker/include/autoware_lane_departure_checker/lane_departure_checker_node.hpp index be7126944f767..d37ef30d6b8e8 100644 --- a/control/lane_departure_checker/include/autoware_lane_departure_checker/lane_departure_checker_node.hpp +++ b/control/lane_departure_checker/include/autoware_lane_departure_checker/lane_departure_checker_node.hpp @@ -16,6 +16,7 @@ #define AUTOWARE_LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_ #include "autoware_lane_departure_checker/lane_departure_checker.hpp" +#include "tier4_autoware_utils/ros/polling_subscriber.hpp" #include #include @@ -66,11 +67,16 @@ class LaneDepartureCheckerNode : public rclcpp::Node private: // Subscriber - rclcpp::Subscription::SharedPtr sub_odom_; - rclcpp::Subscription::SharedPtr sub_lanelet_map_bin_; - rclcpp::Subscription::SharedPtr sub_route_; - rclcpp::Subscription::SharedPtr sub_reference_trajectory_; - rclcpp::Subscription::SharedPtr sub_predicted_trajectory_; + tier4_autoware_utils::InterProcessPollingSubscriber sub_odom_{ + this, "~/input/odometry"}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_lanelet_map_bin_{ + this, "~/input/lanelet_map_bin", rclcpp::QoS{1}.transient_local()}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_route_{ + this, "~/input/route"}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_reference_trajectory_{ + this, "~/input/reference_trajectory"}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_predicted_trajectory_{ + this, "~/input/predicted_trajectory"}; // Data Buffer nav_msgs::msg::Odometry::ConstSharedPtr current_odom_; diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index 2b919f89e6e95..7956410fdf898 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -169,22 +169,6 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o lane_departure_checker_ = std::make_unique(); lane_departure_checker_->setParam(param_, vehicle_info); - // Subscriber - sub_odom_ = this->create_subscription( - "~/input/odometry", 1, std::bind(&LaneDepartureCheckerNode::onOdometry, this, _1)); - sub_lanelet_map_bin_ = this->create_subscription( - "~/input/lanelet_map_bin", rclcpp::QoS{1}.transient_local(), - std::bind(&LaneDepartureCheckerNode::onLaneletMapBin, this, _1)); - sub_route_ = this->create_subscription( - "~/input/route", rclcpp::QoS{1}.transient_local(), - std::bind(&LaneDepartureCheckerNode::onRoute, this, _1)); - sub_reference_trajectory_ = this->create_subscription( - "~/input/reference_trajectory", 1, - std::bind(&LaneDepartureCheckerNode::onReferenceTrajectory, this, _1)); - sub_predicted_trajectory_ = this->create_subscription( - "~/input/predicted_trajectory", 1, - std::bind(&LaneDepartureCheckerNode::onPredictedTrajectory, this, _1)); - // Publisher // Nothing @@ -201,36 +185,6 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o this, get_clock(), period_ns, std::bind(&LaneDepartureCheckerNode::onTimer, this)); } -void LaneDepartureCheckerNode::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) -{ - current_odom_ = msg; -} - -void LaneDepartureCheckerNode::onLaneletMapBin(const LaneletMapBin::ConstSharedPtr msg) -{ - lanelet_map_ = std::make_shared(); - lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_, &traffic_rules_, &routing_graph_); - - // get all shoulder lanes - lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_); - shoulder_lanelets_ = lanelet::utils::query::shoulderLanelets(all_lanelets); -} - -void LaneDepartureCheckerNode::onRoute(const LaneletRoute::ConstSharedPtr msg) -{ - route_ = msg; -} - -void LaneDepartureCheckerNode::onReferenceTrajectory(const Trajectory::ConstSharedPtr msg) -{ - reference_trajectory_ = msg; -} - -void LaneDepartureCheckerNode::onPredictedTrajectory(const Trajectory::ConstSharedPtr msg) -{ - predicted_trajectory_ = msg; -} - bool LaneDepartureCheckerNode::isDataReady() { if (!current_odom_) { @@ -300,6 +254,22 @@ void LaneDepartureCheckerNode::onTimer() tier4_autoware_utils::StopWatch stop_watch; stop_watch.tic("Total"); + current_odom_ = sub_odom_.takeData(); + route_ = sub_route_.takeData(); + reference_trajectory_ = sub_reference_trajectory_.takeData(); + predicted_trajectory_ = sub_predicted_trajectory_.takeData(); + + const auto lanelet_map_bin_msg = sub_lanelet_map_bin_.takeData(); + if (lanelet_map_bin_msg) { + lanelet_map_ = std::make_shared(); + lanelet::utils::conversion::fromBinMsg( + *lanelet_map_bin_msg, lanelet_map_, &traffic_rules_, &routing_graph_); + + // get all shoulder lanes + lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_); + shoulder_lanelets_ = lanelet::utils::query::shoulderLanelets(all_lanelets); + } + if (!isDataReady()) { return; }