From b928ffd6e9f231290bedd05c0025a380a1a3427c Mon Sep 17 00:00:00 2001 From: a-maumau Date: Thu, 24 Apr 2025 16:43:04 +0900 Subject: [PATCH 1/2] fix autoware_utils header Signed-off-by: a-maumau --- .../behavior_velocity_planner/node.hpp | 30 +++++++++---------- .../package.xml | 5 +++- .../src/node.cpp | 10 +++---- 3 files changed, 24 insertions(+), 21 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp index 022df4c87b..d70ce9c0b7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp @@ -18,9 +18,9 @@ #include "autoware/behavior_velocity_planner/planner_manager.hpp" #include -#include -#include -#include +#include +#include +#include #include #include @@ -62,31 +62,31 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node trigger_sub_path_with_lane_id_; // polling subscribers - autoware_utils::InterProcessPollingSubscriber + autoware_utils_rclcpp::InterProcessPollingSubscriber sub_predicted_objects_{this, "~/input/dynamic_objects"}; - autoware_utils::InterProcessPollingSubscriber + autoware_utils_rclcpp::InterProcessPollingSubscriber sub_no_ground_pointcloud_{ - this, "~/input/no_ground_pointcloud", autoware_utils::single_depth_sensor_qos()}; + this, "~/input/no_ground_pointcloud", autoware_utils_rclcpp::single_depth_sensor_qos()}; - autoware_utils::InterProcessPollingSubscriber sub_vehicle_odometry_{ + autoware_utils_rclcpp::InterProcessPollingSubscriber sub_vehicle_odometry_{ this, "~/input/vehicle_odometry"}; - autoware_utils::InterProcessPollingSubscriber + autoware_utils_rclcpp::InterProcessPollingSubscriber sub_acceleration_{this, "~/input/accel"}; - autoware_utils::InterProcessPollingSubscriber< + autoware_utils_rclcpp::InterProcessPollingSubscriber< autoware_perception_msgs::msg::TrafficLightGroupArray> sub_traffic_signals_{this, "~/input/traffic_signals"}; - autoware_utils::InterProcessPollingSubscriber sub_occupancy_grid_{ + autoware_utils_rclcpp::InterProcessPollingSubscriber sub_occupancy_grid_{ this, "~/input/occupancy_grid"}; - autoware_utils::InterProcessPollingSubscriber< - LaneletMapBin, autoware_utils::polling_policy::Newest> + autoware_utils_rclcpp::InterProcessPollingSubscriber< + LaneletMapBin, autoware_utils_rclcpp::polling_policy::Newest> sub_lanelet_map_{this, "~/input/vector_map", rclcpp::QoS{1}.transient_local()}; - autoware_utils::InterProcessPollingSubscriber sub_external_velocity_limit_{ + autoware_utils_rclcpp::InterProcessPollingSubscriber sub_external_velocity_limit_{ this, "~/input/external_velocity_limit_mps", rclcpp::QoS{1}.transient_local()}; void onTrigger( @@ -134,9 +134,9 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node const autoware_internal_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, const PlannerData & planner_data); - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; static constexpr int logger_throttle_interval = 3000; }; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml index 2f6839dbdd..f662933f85 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml @@ -41,7 +41,10 @@ autoware_perception_msgs autoware_planning_msgs autoware_route_handler - autoware_utils + autoware_utils_debug + autoware_utils_logging + autoware_utils_pcl + autoware_utils_rclcpp autoware_velocity_smoother diagnostic_msgs eigen diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp index 709d5813a6..6a0c3b909b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp @@ -19,8 +19,8 @@ #include #include #include -#include -#include +#include +#include #include #include @@ -106,8 +106,8 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio planner_manager_.launchScenePlugin(*this, name); } - logger_configure_ = std::make_unique(this); - published_time_publisher_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); + published_time_publisher_ = std::make_unique(this); } void BehaviorVelocityPlannerNode::onLoadPlugin( @@ -154,7 +154,7 @@ void BehaviorVelocityPlannerNode::processNoGroundPointCloud( Eigen::Affine3f affine = tf2::transformToEigen(transform.transform).cast(); pcl::PointCloud::Ptr pc_transformed(new pcl::PointCloud); if (!pc.empty()) { - autoware_utils::transform_pointcloud(pc, *pc_transformed, affine); + autoware_utils_pcl::transform_pointcloud(pc, *pc_transformed, affine); } planner_data_.no_ground_pointcloud = pc_transformed; From 387f9b92128e8aa71dd3ff54ba62c6416d50f667 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 24 Apr 2025 07:47:03 +0000 Subject: [PATCH 2/2] style(pre-commit): autofix --- .../autoware/behavior_velocity_planner/node.hpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp index d70ce9c0b7..e69987a4da 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp @@ -62,25 +62,27 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node trigger_sub_path_with_lane_id_; // polling subscribers - autoware_utils_rclcpp::InterProcessPollingSubscriber + autoware_utils_rclcpp::InterProcessPollingSubscriber< + autoware_perception_msgs::msg::PredictedObjects> sub_predicted_objects_{this, "~/input/dynamic_objects"}; autoware_utils_rclcpp::InterProcessPollingSubscriber sub_no_ground_pointcloud_{ this, "~/input/no_ground_pointcloud", autoware_utils_rclcpp::single_depth_sensor_qos()}; - autoware_utils_rclcpp::InterProcessPollingSubscriber sub_vehicle_odometry_{ - this, "~/input/vehicle_odometry"}; + autoware_utils_rclcpp::InterProcessPollingSubscriber + sub_vehicle_odometry_{this, "~/input/vehicle_odometry"}; - autoware_utils_rclcpp::InterProcessPollingSubscriber + autoware_utils_rclcpp::InterProcessPollingSubscriber< + geometry_msgs::msg::AccelWithCovarianceStamped> sub_acceleration_{this, "~/input/accel"}; autoware_utils_rclcpp::InterProcessPollingSubscriber< autoware_perception_msgs::msg::TrafficLightGroupArray> sub_traffic_signals_{this, "~/input/traffic_signals"}; - autoware_utils_rclcpp::InterProcessPollingSubscriber sub_occupancy_grid_{ - this, "~/input/occupancy_grid"}; + autoware_utils_rclcpp::InterProcessPollingSubscriber + sub_occupancy_grid_{this, "~/input/occupancy_grid"}; autoware_utils_rclcpp::InterProcessPollingSubscriber< LaneletMapBin, autoware_utils_rclcpp::polling_policy::Newest>