From 04dcc785ab70e8e13d1cefacc8c3d74ee621c571 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Thu, 6 Jun 2024 20:25:43 +0900 Subject: [PATCH 1/4] add prefix autoware_ to lane_departure_checker package Signed-off-by: kyoichi-sugahara --- .../lane_departure_checker_node.hpp | 2 +- .../lane_departure_checker_node.cpp | 10 +++++----- planning/behavior_path_planner/package.xml | 1 + 3 files changed, 7 insertions(+), 6 deletions(-) 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..88c0001cc47b2 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 @@ -117,7 +117,7 @@ class LaneDepartureCheckerNode : public rclcpp::Node // Core Input input_{}; Output output_{}; - std::unique_ptr lane_departure_checker_; + std::unique_ptr autoware_lane_departure_checker_; // Diagnostic Updater diagnostic_updater::Updater updater_{this}; 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..c35a07a1b788e 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 @@ -166,8 +166,8 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o add_on_set_parameters_callback(std::bind(&LaneDepartureCheckerNode::onParameter, this, _1)); // Core - lane_departure_checker_ = std::make_unique(); - lane_departure_checker_->setParam(param_, vehicle_info); + autoware_lane_departure_checker_ = std::make_unique(); + autoware_lane_departure_checker_->setParam(param_, vehicle_info); // Subscriber sub_odom_ = this->create_subscription( @@ -351,7 +351,7 @@ void LaneDepartureCheckerNode::onTimer() input_.boundary_types_to_detect = node_param_.boundary_types_to_detect; processing_time_map["Node: setInputData"] = stop_watch.toc(true); - output_ = lane_departure_checker_->update(input_); + output_ = autoware_lane_departure_checker_->update(input_); processing_time_map["Node: update"] = stop_watch.toc(true); updater_.force_update(); @@ -410,8 +410,8 @@ rcl_interfaces::msg::SetParametersResult LaneDepartureCheckerNode::onParameter( update_param(parameters, "delay_time", param_.delay_time); update_param(parameters, "min_braking_distance", param_.min_braking_distance); - if (lane_departure_checker_) { - lane_departure_checker_->setParam(param_); + if (autoware_lane_departure_checker_) { + autoware_lane_departure_checker_->setParam(param_); } } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { result.successful = false; diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index f51fa99578d7b..feaeb8fd3c640 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -41,6 +41,7 @@ autoware_frenet_planner autoware_lane_departure_checker autoware_path_sampler + autoware_lane_departure_checker autoware_perception_msgs autoware_planning_msgs autoware_planning_test_manager From 9c2d0d1915f438abded2165054cdcf51c685c37b Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Thu, 6 Jun 2024 21:26:04 +0900 Subject: [PATCH 2/4] revert variable name Signed-off-by: kyoichi-sugahara --- .../lane_departure_checker_node.hpp | 2 +- .../lane_departure_checker_node.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) 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 88c0001cc47b2..be7126944f767 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 @@ -117,7 +117,7 @@ class LaneDepartureCheckerNode : public rclcpp::Node // Core Input input_{}; Output output_{}; - std::unique_ptr autoware_lane_departure_checker_; + std::unique_ptr lane_departure_checker_; // Diagnostic Updater diagnostic_updater::Updater updater_{this}; 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 c35a07a1b788e..2b919f89e6e95 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 @@ -166,8 +166,8 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o add_on_set_parameters_callback(std::bind(&LaneDepartureCheckerNode::onParameter, this, _1)); // Core - autoware_lane_departure_checker_ = std::make_unique(); - autoware_lane_departure_checker_->setParam(param_, vehicle_info); + lane_departure_checker_ = std::make_unique(); + lane_departure_checker_->setParam(param_, vehicle_info); // Subscriber sub_odom_ = this->create_subscription( @@ -351,7 +351,7 @@ void LaneDepartureCheckerNode::onTimer() input_.boundary_types_to_detect = node_param_.boundary_types_to_detect; processing_time_map["Node: setInputData"] = stop_watch.toc(true); - output_ = autoware_lane_departure_checker_->update(input_); + output_ = lane_departure_checker_->update(input_); processing_time_map["Node: update"] = stop_watch.toc(true); updater_.force_update(); @@ -410,8 +410,8 @@ rcl_interfaces::msg::SetParametersResult LaneDepartureCheckerNode::onParameter( update_param(parameters, "delay_time", param_.delay_time); update_param(parameters, "min_braking_distance", param_.min_braking_distance); - if (autoware_lane_departure_checker_) { - autoware_lane_departure_checker_->setParam(param_); + if (lane_departure_checker_) { + lane_departure_checker_->setParam(param_); } } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { result.successful = false; From 961a4a6f226d7a92f1674deab9ae22085090b0a1 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 7 Jun 2024 04:39:34 +0000 Subject: [PATCH 3/4] style(pre-commit): autofix --- planning/behavior_path_planner/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index feaeb8fd3c640..3cf0cb03f0a87 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -42,6 +42,7 @@ autoware_lane_departure_checker autoware_path_sampler autoware_lane_departure_checker + autoware_path_sampler autoware_perception_msgs autoware_planning_msgs autoware_planning_test_manager From 30418769a003e5db59c856de274675e3d26f07ae Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Fri, 7 Jun 2024 14:53:13 +0900 Subject: [PATCH 4/4] use polling subscriber Signed-off-by: kyoichi-sugahara --- .../lane_departure_checker_node.hpp | 16 +++-- .../lane_departure_checker_node.cpp | 62 +++++-------------- planning/behavior_path_planner/package.xml | 2 - 3 files changed, 27 insertions(+), 53 deletions(-) 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; } diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index 3cf0cb03f0a87..f51fa99578d7b 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -41,8 +41,6 @@ autoware_frenet_planner autoware_lane_departure_checker autoware_path_sampler - autoware_lane_departure_checker - autoware_path_sampler autoware_perception_msgs autoware_planning_msgs autoware_planning_test_manager