diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 863cb4e4955a8..b796a84f48be6 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -105,7 +105,9 @@ map/map_projection_loader/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp mas map/map_tf_generator/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp map/util/lanelet2_map_preprocessor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp perception/autoware_crosswalk_traffic_light_estimator/** satoshi.ota@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp +perception/autoware_lidar_transfusion/** amadeusz.szymko.2@tier4.jp kenzo.lobos@tier4.jp satoshi.tanaka@tier4.jp perception/autoware_map_based_prediction/** kotaro.uetake@tier4.jp kyoichi.sugahara@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yoshi.ri@tier4.jp +perception/autoware_multi_object_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/autoware_object_merger/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/autoware_object_range_splitter/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/autoware_object_velocity_splitter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp @@ -121,22 +123,20 @@ perception/detected_object_feature_remover/** tomoya.kimura@tier4.jp perception/detected_object_validation/** dai.nguyen@tier4.jp shintaro.tomie@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/autoware_detection_by_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/elevation_map_loader/** kosuke.takeuchi@tier4.jp shintaro.tomie@tier4.jp taichi.higashide@tier4.jp -perception/euclidean_cluster/** dai.nguyen@tier4.jp yukihiro.saito@tier4.jp +perception/eucliautoware_euclidean_clusterdean_cluster/** dai.nguyen@tier4.jp yukihiro.saito@tier4.jp perception/ground_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/image_projection_based_fusion/** dai.nguyen@tier4.jp koji.minoda@tier4.jp kotaro.uetake@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/lidar_apollo_instance_segmentation/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/lidar_apollo_segmentation_tvm/** ambroise.vincent@arm.com yoshi.ri@tier4.jp perception/lidar_apollo_segmentation_tvm_nodes/** ambroise.vincent@arm.com yoshi.ri@tier4.jp perception/lidar_centerpoint/** kenzo.lobos@tier4.jp koji.minoda@tier4.jp -perception/lidar_transfusion/** amadeusz.szymko.2@tier4.jp kenzo.lobos@tier4.jp satoshi.tanaka@tier4.jp -perception/autoware_multi_object_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/occupancy_grid_map_outlier_filter/** abrahammonrroy@yahoo.com yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/probabilistic_occupancy_grid_map/** mamoru.sobue@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/radar_tracks_msgs_converter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp perception/autoware_raindrop_cluster_filter/** dai.nguyen@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/shape_estimation/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/simple_object_merger/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp -perception/tensorrt_classifier/** kotaro.uetake@tier4.jp shunsuke.miura@tier4.jp +perception/autoware_tensorrt_classifier/** kotaro.uetake@tier4.jp shunsuke.miura@tier4.jp perception/tensorrt_yolox/** dan.umeda@tier4.jp manato.hirabayashi@tier4.jp perception/traffic_light_arbiter/** kenzo.lobos@tier4.jp shunsuke.miura@tier4.jp perception/traffic_light_classifier/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp yukihiro.saito@tier4.jp diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp index 5f5c9cee2da43..1e79505ea97c3 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp @@ -103,8 +103,7 @@ private Q_SLOTS: turn_signals_sub_; rclcpp::Subscription::SharedPtr hazard_lights_sub_; - rclcpp::Subscription::SharedPtr - traffic_sub_; + rclcpp::Subscription::SharedPtr traffic_sub_; rclcpp::Subscription::SharedPtr speed_limit_sub_; std::mutex property_mutex_; @@ -118,7 +117,7 @@ private Q_SLOTS: const autoware_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg); void updateSpeedLimitData(const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg); void updateTrafficLightData( - const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg); + const autoware_perception_msgs::msg::TrafficLightGroup::ConstSharedPtr msg); void drawWidget(QImage & hud); }; } // namespace autoware_overlay_rviz_plugin diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/traffic_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/traffic_display.hpp index fd15f542021f1..01545bcea6f4e 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/traffic_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/traffic_display.hpp @@ -25,7 +25,6 @@ #include #include -#include #include #include @@ -40,8 +39,8 @@ class TrafficDisplay TrafficDisplay(); void drawTrafficLightIndicator(QPainter & painter, const QRectF & backgroundRect); void updateTrafficLightData( - const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr & msg); - autoware_perception_msgs::msg::TrafficLightGroupArray current_traffic_; + const autoware_perception_msgs::msg::TrafficLightGroup::ConstSharedPtr & msg); + autoware_perception_msgs::msg::TrafficLightGroup current_traffic_; private: QImage traffic_light_image_; diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp index 1fd33a95fb34d..c61a6b47853ba 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp @@ -112,9 +112,10 @@ void SignalDisplay::onInitialize() speed_limit_topic_property_->initialize(rviz_ros_node); traffic_topic_property_ = std::make_unique( - "Traffic Topic", "/perception/traffic_light_recognition/traffic_signals", - "autoware_perception_msgs/msgs/msg/TrafficLightGroupArray", "Topic for Traffic Light Data", - this, SLOT(topic_updated_traffic())); + "Traffic Topic", + "/planning/scenario_planning/lane_driving/behavior_planning/debug/traffic_signal", + "autoware_perception_msgs/msgs/msg/TrafficLightGroup", "Topic for Traffic Light Data", this, + SLOT(topic_updated_traffic())); traffic_topic_property_->initialize(rviz_ros_node); } @@ -192,7 +193,7 @@ void SignalDisplay::onDisable() } void SignalDisplay::updateTrafficLightData( - const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg) + const autoware_perception_msgs::msg::TrafficLightGroup::ConstSharedPtr msg) { std::lock_guard lock(property_mutex_); @@ -458,14 +459,13 @@ void SignalDisplay::topic_updated_traffic() // resubscribe to the topic traffic_sub_.reset(); auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); - traffic_sub_ = - rviz_ros_node->get_raw_node() - ->create_subscription( - traffic_topic_property_->getTopicStd(), - rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), - [this](const autoware_perception_msgs::msg::TrafficLightGroupArray::SharedPtr msg) { - updateTrafficLightData(msg); - }); + traffic_sub_ = rviz_ros_node->get_raw_node() + ->create_subscription( + traffic_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_perception_msgs::msg::TrafficLightGroup::SharedPtr msg) { + updateTrafficLightData(msg); + }); } } // namespace autoware_overlay_rviz_plugin diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp index 2dc9c23583a52..c3f79f40e2ecd 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp @@ -48,7 +48,7 @@ TrafficDisplay::TrafficDisplay() } void TrafficDisplay::updateTrafficLightData( - const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr & msg) + const autoware_perception_msgs::msg::TrafficLightGroup::ConstSharedPtr & msg) { current_traffic_ = *msg; } @@ -68,8 +68,8 @@ void TrafficDisplay::drawTrafficLightIndicator(QPainter & painter, const QRectF backgroundRect.top() + circleRect.height() / 2)); painter.drawEllipse(circleRect); - if (!current_traffic_.traffic_light_groups.empty()) { - switch (current_traffic_.traffic_light_groups[0].elements[0].color) { + if (!current_traffic_.elements.empty()) { + switch (current_traffic_.elements[0].color) { case 1: painter.setBrush(QBrush(tl_red_)); painter.drawEllipse(circleRect); diff --git a/control/autoware_vehicle_cmd_gate/README.md b/control/autoware_vehicle_cmd_gate/README.md index 2d6f5c5f949af..e46db3c06cfeb 100644 --- a/control/autoware_vehicle_cmd_gate/README.md +++ b/control/autoware_vehicle_cmd_gate/README.md @@ -78,6 +78,10 @@ The limitation values are calculated based on the 1D interpolation of the limita Notation: this filter is not designed to enhance ride comfort. Its main purpose is to detect and remove abnormal values in the control outputs during the final stages of Autoware. If this filter is frequently active, it implies the control module may need tuning. If you're aiming to smoothen the signal via a low-pass filter or similar techniques, that should be handled in the control module. When the filter is activated, the topic `~/is_filter_activated` is published. +Notation 2: If you use vehicles in which the driving force is controlled by the accelerator/brake pedal, the jerk limit, denoting the pedal rate limit, must be sufficiently relaxed at low speeds. +Otherwise, quick pedal changes at start/stop will not be possible, resulting in slow starts and creep down on hills. +This functionality for starting/stopping was embedded in the source code but was removed because it was complex and could be achieved by parameters. + ## Assumptions / Known limits The parameter `check_external_emergency_heartbeat` (true by default) enables an emergency stop request from external modules. diff --git a/control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml b/control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml index 54c87f45b6a96..74affea696893 100644 --- a/control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml +++ b/control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml @@ -16,14 +16,14 @@ stop_check_duration: 1.0 nominal: vel_lim: 25.0 - reference_speed_points: [20.0, 30.0] - steer_lim: [1.0, 0.8] - steer_rate_lim: [1.0, 0.8] - lon_acc_lim: [5.0, 4.0] - lon_jerk_lim: [5.0, 4.0] - lat_acc_lim: [5.0, 4.0] - lat_jerk_lim: [7.0, 6.0] - actual_steer_diff_lim: [1.0, 0.8] + reference_speed_points: [0.1, 0.3, 20.0, 30.0] + steer_lim: [1.0, 1.0, 1.0, 0.8] + steer_rate_lim: [1.0, 1.0, 1.0, 0.8] + lon_acc_lim: [5.0, 5.0, 5.0, 4.0] + lon_jerk_lim: [80.0, 5.0, 5.0, 4.0] # The first element is required for quick pedal changes when stopping and starting. + lat_acc_lim: [5.0, 5.0, 5.0, 4.0] + lat_jerk_lim: [7.0, 7.0, 7.0, 6.0] + actual_steer_diff_lim: [1.0, 1.0, 1.0, 0.8] on_transition: vel_lim: 50.0 reference_speed_points: [20.0, 30.0] diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index 87e79f59bc356..dd6e2f0f54aea 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -607,7 +607,6 @@ Control VehicleCmdGate::filterControlCommand(const Control & in) const auto mode = current_operation_mode_; const auto current_status_cmd = getActualStatusAsCommand(); const auto ego_is_stopped = vehicle_stop_checker_->isVehicleStopped(stop_check_duration_); - const auto input_cmd_is_stopping = in.longitudinal.acceleration < 0.0; filter_.setCurrentSpeed(current_kinematics_.twist.twist.linear.x); filter_on_transition_.setCurrentSpeed(current_kinematics_.twist.twist.linear.x); @@ -618,23 +617,6 @@ Control VehicleCmdGate::filterControlCommand(const Control & in) if (mode.is_in_transition) { filter_on_transition_.filterAll(dt, current_steer_, out, is_filter_activated); } else { - // When ego is stopped and the input command is not stopping, - // use the higher of actual vehicle longitudinal state - // and previous longitudinal command for the filtering - // this is to prevent the jerk limits being applied and causing - // a delay when restarting the vehicle. - - if (ego_is_stopped && !input_cmd_is_stopping) { - auto prev_cmd = filter_.getPrevCmd(); - prev_cmd.longitudinal.acceleration = - std::max(prev_cmd.longitudinal.acceleration, current_status_cmd.longitudinal.acceleration); - // consider reverse driving - prev_cmd.longitudinal.velocity = std::fabs(prev_cmd.longitudinal.velocity) > - std::fabs(current_status_cmd.longitudinal.velocity) - ? prev_cmd.longitudinal.velocity - : current_status_cmd.longitudinal.velocity; - filter_.setPrevCmd(prev_cmd); - } filter_.filterAll(dt, current_steer_, out, is_filter_activated); } diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml index 9256e90ec3e35..73e0e34611fb3 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml @@ -112,7 +112,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml index 98e8ec7c8ed4b..f13edf5e52dff 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml @@ -17,10 +17,10 @@ - + - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml index 38ee946201f7e..f737d280e039f 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml @@ -10,7 +10,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py index e467aba5d4e98..6e77fa71cd553 100644 --- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py +++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py @@ -56,7 +56,7 @@ def create_parameter_dict(*args): composable_node_descriptions=[ ComposableNode( package="traffic_light_classifier", - plugin="traffic_light::TrafficLightClassifierNodelet", + plugin="autoware::traffic_light::TrafficLightClassifierNodelet", name="car_traffic_light_classifier", namespace="classification", parameters=[car_traffic_light_classifier_model_param], @@ -71,7 +71,7 @@ def create_parameter_dict(*args): ), ComposableNode( package="traffic_light_classifier", - plugin="traffic_light::TrafficLightClassifierNodelet", + plugin="autoware::traffic_light::TrafficLightClassifierNodelet", name="pedestrian_traffic_light_classifier", namespace="classification", parameters=[pedestrian_traffic_light_classifier_model_param], diff --git a/launch/tier4_perception_launch/package.xml b/launch/tier4_perception_launch/package.xml index 660725cf1c067..67394cb4526a9 100644 --- a/launch/tier4_perception_launch/package.xml +++ b/launch/tier4_perception_launch/package.xml @@ -16,6 +16,7 @@ autoware_cluster_merger autoware_crosswalk_traffic_light_estimator autoware_detection_by_tracker + autoware_euclidean_cluster autoware_map_based_prediction autoware_multi_object_tracker autoware_object_merger @@ -32,7 +33,6 @@ detected_object_feature_remover detected_object_validation elevation_map_loader - euclidean_cluster ground_segmentation image_projection_based_fusion image_transport_decompressor diff --git a/localization/autoware_pose_covariance_modifier/src/include/pose_covariance_modifier.hpp b/localization/autoware_pose_covariance_modifier/src/include/pose_covariance_modifier.hpp index ffafe41269468..3a253d785e8a7 100644 --- a/localization/autoware_pose_covariance_modifier/src/include/pose_covariance_modifier.hpp +++ b/localization/autoware_pose_covariance_modifier/src/include/pose_covariance_modifier.hpp @@ -65,7 +65,7 @@ class PoseCovarianceModifierNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_double_gnss_position_stddev_; void callback_gnss_pose_with_cov( - const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg); + const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg_pose_with_cov_in); void callback_ndt_pose_with_cov( const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg_pose_with_cov_in); diff --git a/perception/autoware_detection_by_tracker/package.xml b/perception/autoware_detection_by_tracker/package.xml index 837dfae1e2ef9..dd5a719e70270 100644 --- a/perception/autoware_detection_by_tracker/package.xml +++ b/perception/autoware_detection_by_tracker/package.xml @@ -13,9 +13,9 @@ autoware_cmake eigen3_cmake_module + autoware_euclidean_cluster autoware_universe_utils eigen - euclidean_cluster object_recognition_utils rclcpp rclcpp_components diff --git a/perception/euclidean_cluster/CMakeLists.txt b/perception/autoware_euclidean_cluster/CMakeLists.txt similarity index 97% rename from perception/euclidean_cluster/CMakeLists.txt rename to perception/autoware_euclidean_cluster/CMakeLists.txt index d27390ae7707a..55f664489a4d9 100644 --- a/perception/euclidean_cluster/CMakeLists.txt +++ b/perception/autoware_euclidean_cluster/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(euclidean_cluster) +project(autoware_euclidean_cluster) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/perception/euclidean_cluster/README.md b/perception/autoware_euclidean_cluster/README.md similarity index 97% rename from perception/euclidean_cluster/README.md rename to perception/autoware_euclidean_cluster/README.md index b57204c84cc3b..349d4211ee8e9 100644 --- a/perception/euclidean_cluster/README.md +++ b/perception/autoware_euclidean_cluster/README.md @@ -1,8 +1,8 @@ -# euclidean_cluster +# autoware_euclidean_cluster ## Purpose -euclidean_cluster is a package for clustering points into smaller parts to classify objects. +autoware_euclidean_cluster is a package for clustering points into smaller parts to classify objects. This package has two clustering methods: `euclidean_cluster` and `voxel_grid_based_euclidean_cluster`. diff --git a/perception/euclidean_cluster/config/euclidean_cluster.param.yaml b/perception/autoware_euclidean_cluster/config/euclidean_cluster.param.yaml similarity index 100% rename from perception/euclidean_cluster/config/euclidean_cluster.param.yaml rename to perception/autoware_euclidean_cluster/config/euclidean_cluster.param.yaml diff --git a/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml b/perception/autoware_euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml similarity index 100% rename from perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml rename to perception/autoware_euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml diff --git a/perception/euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster.hpp b/perception/autoware_euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster.hpp similarity index 100% rename from perception/euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster.hpp rename to perception/autoware_euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster.hpp diff --git a/perception/euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster_interface.hpp b/perception/autoware_euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster_interface.hpp similarity index 100% rename from perception/euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster_interface.hpp rename to perception/autoware_euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster_interface.hpp diff --git a/perception/euclidean_cluster/include/autoware/euclidean_cluster/utils.hpp b/perception/autoware_euclidean_cluster/include/autoware/euclidean_cluster/utils.hpp similarity index 100% rename from perception/euclidean_cluster/include/autoware/euclidean_cluster/utils.hpp rename to perception/autoware_euclidean_cluster/include/autoware/euclidean_cluster/utils.hpp diff --git a/perception/euclidean_cluster/include/autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp b/perception/autoware_euclidean_cluster/include/autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp similarity index 100% rename from perception/euclidean_cluster/include/autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp rename to perception/autoware_euclidean_cluster/include/autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp diff --git a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py b/perception/autoware_euclidean_cluster/launch/euclidean_cluster.launch.py similarity index 99% rename from perception/euclidean_cluster/launch/euclidean_cluster.launch.py rename to perception/autoware_euclidean_cluster/launch/euclidean_cluster.launch.py index ab23fb2b44916..fbbd70b966809 100644 --- a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py +++ b/perception/autoware_euclidean_cluster/launch/euclidean_cluster.launch.py @@ -33,7 +33,7 @@ def load_composable_node_param(param_path): return yaml.safe_load(f)["/**"]["ros__parameters"] ns = "" - pkg = "euclidean_cluster" + pkg = "autoware_euclidean_cluster" low_height_cropbox_filter_component = ComposableNode( package="autoware_pointcloud_preprocessor", diff --git a/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml b/perception/autoware_euclidean_cluster/launch/euclidean_cluster.launch.xml similarity index 80% rename from perception/euclidean_cluster/launch/euclidean_cluster.launch.xml rename to perception/autoware_euclidean_cluster/launch/euclidean_cluster.launch.xml index f4deeccf7b76c..22d62c4df375a 100644 --- a/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml +++ b/perception/autoware_euclidean_cluster/launch/euclidean_cluster.launch.xml @@ -4,11 +4,11 @@ - + - + diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py b/perception/autoware_euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py similarity index 99% rename from perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py rename to perception/autoware_euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py index 12d41bf52ff32..6a678cc4c7f59 100644 --- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py +++ b/perception/autoware_euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py @@ -32,7 +32,7 @@ def load_composable_node_param(param_path): return yaml.safe_load(f)["/**"]["ros__parameters"] ns = "" - pkg = "euclidean_cluster" + pkg = "autoware_euclidean_cluster" low_height_cropbox_filter_component = ComposableNode( package="autoware_pointcloud_preprocessor", diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml b/perception/autoware_euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml similarity index 82% rename from perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml rename to perception/autoware_euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml index 3a7c685d8f449..c9ece95e9a606 100644 --- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml +++ b/perception/autoware_euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml @@ -4,11 +4,11 @@ - + - + diff --git a/perception/euclidean_cluster/lib/euclidean_cluster.cpp b/perception/autoware_euclidean_cluster/lib/euclidean_cluster.cpp similarity index 100% rename from perception/euclidean_cluster/lib/euclidean_cluster.cpp rename to perception/autoware_euclidean_cluster/lib/euclidean_cluster.cpp diff --git a/perception/euclidean_cluster/lib/utils.cpp b/perception/autoware_euclidean_cluster/lib/utils.cpp similarity index 100% rename from perception/euclidean_cluster/lib/utils.cpp rename to perception/autoware_euclidean_cluster/lib/utils.cpp diff --git a/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp b/perception/autoware_euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp similarity index 100% rename from perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp rename to perception/autoware_euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp diff --git a/perception/euclidean_cluster/package.xml b/perception/autoware_euclidean_cluster/package.xml similarity index 90% rename from perception/euclidean_cluster/package.xml rename to perception/autoware_euclidean_cluster/package.xml index 45b5d05031c72..076406bd3980a 100644 --- a/perception/euclidean_cluster/package.xml +++ b/perception/autoware_euclidean_cluster/package.xml @@ -1,9 +1,9 @@ - euclidean_cluster + autoware_euclidean_cluster 0.1.0 - The euclidean_cluster package + The autoware_euclidean_cluster package Yukihiro Saito Dai Nguyen Apache License 2.0 diff --git a/perception/euclidean_cluster/src/euclidean_cluster_node.cpp b/perception/autoware_euclidean_cluster/src/euclidean_cluster_node.cpp similarity index 100% rename from perception/euclidean_cluster/src/euclidean_cluster_node.cpp rename to perception/autoware_euclidean_cluster/src/euclidean_cluster_node.cpp diff --git a/perception/euclidean_cluster/src/euclidean_cluster_node.hpp b/perception/autoware_euclidean_cluster/src/euclidean_cluster_node.hpp similarity index 100% rename from perception/euclidean_cluster/src/euclidean_cluster_node.hpp rename to perception/autoware_euclidean_cluster/src/euclidean_cluster_node.hpp diff --git a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp b/perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp similarity index 100% rename from perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp rename to perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp diff --git a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp b/perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp similarity index 100% rename from perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp rename to perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp diff --git a/perception/euclidean_cluster/test/test_voxel_grid_based_euclidean_cluster.cpp b/perception/autoware_euclidean_cluster/test/test_voxel_grid_based_euclidean_cluster.cpp similarity index 100% rename from perception/euclidean_cluster/test/test_voxel_grid_based_euclidean_cluster.cpp rename to perception/autoware_euclidean_cluster/test/test_voxel_grid_based_euclidean_cluster.cpp diff --git a/perception/lidar_transfusion/CMakeLists.txt b/perception/autoware_lidar_transfusion/CMakeLists.txt similarity index 88% rename from perception/lidar_transfusion/CMakeLists.txt rename to perception/autoware_lidar_transfusion/CMakeLists.txt index 15c89beb15d3e..c3a56f883fbe5 100644 --- a/perception/lidar_transfusion/CMakeLists.txt +++ b/perception/autoware_lidar_transfusion/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(lidar_transfusion) +project(autoware_lidar_transfusion) find_package(autoware_cmake REQUIRED) autoware_package() @@ -82,7 +82,7 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) set(CUDA_NVCC_FLAGS "-g -G") endif() - ament_auto_add_library(transfusion_lib SHARED + ament_auto_add_library(${PROJECT_NAME}_lib SHARED lib/detection_class_remapper.cpp lib/network/network_trt.cpp lib/postprocess/non_maximum_suppression.cpp @@ -92,23 +92,23 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) lib/transfusion_trt.cpp ) - cuda_add_library(transfusion_cuda_lib SHARED + cuda_add_library(${PROJECT_NAME}_cuda_lib SHARED lib/postprocess/circle_nms_kernel.cu lib/postprocess/postprocess_kernel.cu lib/preprocess/preprocess_kernel.cu ) - target_link_libraries(transfusion_lib + target_link_libraries(${PROJECT_NAME}_lib ${NVINFER} ${NVONNXPARSER} ${CUDA_LIBRARIES} ${CUBLAS_LIBRARIES} ${CUDA_curand_LIBRARY} ${CUDNN_LIBRARY} - transfusion_cuda_lib + ${PROJECT_NAME}_cuda_lib ) - target_include_directories(transfusion_lib + target_include_directories(${PROJECT_NAME}_lib PUBLIC $ $ @@ -116,26 +116,26 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) # To suppress unknown-pragmas error. The root-cause is CUB library in CUDA 11.6. # This issue was fixed by https://github.com/NVIDIA/cub/commit/7d608bf1dc14553e2fb219eabeed80b76621b6fe - target_include_directories(transfusion_lib + target_include_directories(${PROJECT_NAME}_lib SYSTEM PUBLIC ${CUDA_INCLUDE_DIRS} ) - ament_auto_add_library(lidar_transfusion_component SHARED + ament_auto_add_library(${PROJECT_NAME}_component SHARED src/lidar_transfusion_node.cpp ) - target_link_libraries(lidar_transfusion_component - transfusion_lib + target_link_libraries(${PROJECT_NAME}_component + ${PROJECT_NAME}_lib ) - rclcpp_components_register_node(lidar_transfusion_component - PLUGIN "lidar_transfusion::LidarTransfusionNode" - EXECUTABLE lidar_transfusion_node + rclcpp_components_register_node(${PROJECT_NAME}_component + PLUGIN "autoware::lidar_transfusion::LidarTransfusionNode" + EXECUTABLE ${PROJECT_NAME}_node ) install( - TARGETS transfusion_cuda_lib + TARGETS ${PROJECT_NAME}_cuda_lib DESTINATION lib ) @@ -168,7 +168,7 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) # ${test_preprocess_kernel_SOURCE_DIR} # ) # target_link_libraries(test_preprocess_kernel - # transfusion_cuda_lib + # ${PROJECT_NAME}_cuda_lib # gtest # gtest_main # ) @@ -185,7 +185,7 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) # ${test_postprocess_kernel_SOURCE_DIR} # ) # target_link_libraries(test_postprocess_kernel - # transfusion_cuda_lib + # ${PROJECT_NAME}_cuda_lib # gtest # gtest_main # ) diff --git a/perception/lidar_transfusion/README.md b/perception/autoware_lidar_transfusion/README.md similarity index 84% rename from perception/lidar_transfusion/README.md rename to perception/autoware_lidar_transfusion/README.md index 6745cc1f7d219..4a55babbcba9f 100644 --- a/perception/lidar_transfusion/README.md +++ b/perception/autoware_lidar_transfusion/README.md @@ -1,8 +1,8 @@ -# lidar_transfusion +# autoware_lidar_transfusion ## Purpose -The `lidar_transfusion` package is used for 3D object detection based on lidar data (x, y, z, intensity). +The `autoware_lidar_transfusion` package is used for 3D object detection based on lidar data (x, y, z, intensity). ## Inner-workings / Algorithms @@ -34,27 +34,27 @@ We trained the models using . ### TransFusion -{{ json_to_markdown("perception/lidar_transfusion/schema/transfusion.schema.json") }} +{{ json_to_markdown("perception/autoware_lidar_transfusion/schema/transfusion.schema.json") }} ### Detection class remapper -{{ json_to_markdown("perception/lidar_transfusion/schema/detection_class_remapper.schema.json") }} +{{ json_to_markdown("perception/autoware_lidar_transfusion/schema/detection_class_remapper.schema.json") }} ### The `build_only` option -The `lidar_transfusion` node has `build_only` option to build the TensorRT engine file from the ONNX file. +The `autoware_lidar_transfusion` node has `build_only` option to build the TensorRT engine file from the ONNX file. Although it is preferred to move all the ROS parameters in `.param.yaml` file in Autoware Universe, the `build_only` option is not moved to the `.param.yaml` file for now, because it may be used as a flag to execute the build as a pre-task. You can execute with the following command: ```bash -ros2 launch lidar_transfusion lidar_transfusion.launch.xml build_only:=true +ros2 launch autoware_lidar_transfusion lidar_transfusion.launch.xml build_only:=true ``` ### The `log_level` option -The default logging severity level for `lidar_transfusion` is `info`. For debugging purposes, the developer may decrease severity level using `log_level` parameter: +The default logging severity level for `autoware_lidar_transfusion` is `info`. For debugging purposes, the developer may decrease severity level using `log_level` parameter: ```bash -ros2 launch lidar_transfusion lidar_transfusion.launch.xml log_level:=debug +ros2 launch autoware_lidar_transfusion lidar_transfusion.launch.xml log_level:=debug ``` ## Assumptions / Known limits diff --git a/perception/lidar_transfusion/config/detection_class_remapper.param.yaml b/perception/autoware_lidar_transfusion/config/detection_class_remapper.param.yaml similarity index 100% rename from perception/lidar_transfusion/config/detection_class_remapper.param.yaml rename to perception/autoware_lidar_transfusion/config/detection_class_remapper.param.yaml diff --git a/perception/lidar_transfusion/config/transfusion.param.yaml b/perception/autoware_lidar_transfusion/config/transfusion.param.yaml similarity index 100% rename from perception/lidar_transfusion/config/transfusion.param.yaml rename to perception/autoware_lidar_transfusion/config/transfusion.param.yaml diff --git a/perception/lidar_transfusion/include/lidar_transfusion/cuda_utils.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/cuda_utils.hpp similarity index 96% rename from perception/lidar_transfusion/include/lidar_transfusion/cuda_utils.hpp rename to perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/cuda_utils.hpp index 5c25a936d5392..17a7c7ee9c165 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/cuda_utils.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/cuda_utils.hpp @@ -39,8 +39,8 @@ * https://creativecommons.org/publicdomain/zero/1.0/deed.en */ -#ifndef LIDAR_TRANSFUSION__CUDA_UTILS_HPP_ -#define LIDAR_TRANSFUSION__CUDA_UTILS_HPP_ +#ifndef AUTOWARE__LIDAR_TRANSFUSION__CUDA_UTILS_HPP_ +#define AUTOWARE__LIDAR_TRANSFUSION__CUDA_UTILS_HPP_ #include @@ -123,4 +123,4 @@ void clear_async(T * ptr, size_t num_elem, cudaStream_t stream) } // namespace cuda -#endif // LIDAR_TRANSFUSION__CUDA_UTILS_HPP_ +#endif // AUTOWARE__LIDAR_TRANSFUSION__CUDA_UTILS_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/detection_class_remapper.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/detection_class_remapper.hpp similarity index 82% rename from perception/lidar_transfusion/include/lidar_transfusion/detection_class_remapper.hpp rename to perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/detection_class_remapper.hpp index ace7dc5ff3405..6095472affe92 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/detection_class_remapper.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/detection_class_remapper.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_TRANSFUSION__DETECTION_CLASS_REMAPPER_HPP_ -#define LIDAR_TRANSFUSION__DETECTION_CLASS_REMAPPER_HPP_ +#ifndef AUTOWARE__LIDAR_TRANSFUSION__DETECTION_CLASS_REMAPPER_HPP_ +#define AUTOWARE__LIDAR_TRANSFUSION__DETECTION_CLASS_REMAPPER_HPP_ #include @@ -24,7 +24,7 @@ #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { class DetectionClassRemapper @@ -42,6 +42,6 @@ class DetectionClassRemapper int num_labels_; }; -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion -#endif // LIDAR_TRANSFUSION__DETECTION_CLASS_REMAPPER_HPP_ +#endif // AUTOWARE__LIDAR_TRANSFUSION__DETECTION_CLASS_REMAPPER_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/lidar_transfusion_node.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/lidar_transfusion_node.hpp similarity index 78% rename from perception/lidar_transfusion/include/lidar_transfusion/lidar_transfusion_node.hpp rename to perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/lidar_transfusion_node.hpp index a82013fac6ce7..e19d2d49af998 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/lidar_transfusion_node.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/lidar_transfusion_node.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_TRANSFUSION__LIDAR_TRANSFUSION_NODE_HPP_ -#define LIDAR_TRANSFUSION__LIDAR_TRANSFUSION_NODE_HPP_ +#ifndef AUTOWARE__LIDAR_TRANSFUSION__LIDAR_TRANSFUSION_NODE_HPP_ +#define AUTOWARE__LIDAR_TRANSFUSION__LIDAR_TRANSFUSION_NODE_HPP_ -#include "lidar_transfusion/detection_class_remapper.hpp" -#include "lidar_transfusion/postprocess/non_maximum_suppression.hpp" -#include "lidar_transfusion/preprocess/pointcloud_densification.hpp" -#include "lidar_transfusion/transfusion_trt.hpp" -#include "lidar_transfusion/visibility_control.hpp" +#include "autoware/lidar_transfusion/detection_class_remapper.hpp" +#include "autoware/lidar_transfusion/postprocess/non_maximum_suppression.hpp" +#include "autoware/lidar_transfusion/preprocess/pointcloud_densification.hpp" +#include "autoware/lidar_transfusion/transfusion_trt.hpp" +#include "autoware/lidar_transfusion/visibility_control.hpp" #include #include @@ -36,7 +36,7 @@ #include #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { class LIDAR_TRANSFUSION_PUBLIC LidarTransfusionNode : public rclcpp::Node @@ -68,6 +68,6 @@ class LIDAR_TRANSFUSION_PUBLIC LidarTransfusionNode : public rclcpp::Node std::unique_ptr debug_publisher_ptr_{nullptr}; std::unique_ptr published_time_pub_{nullptr}; }; -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion -#endif // LIDAR_TRANSFUSION__LIDAR_TRANSFUSION_NODE_HPP_ +#endif // AUTOWARE__LIDAR_TRANSFUSION__LIDAR_TRANSFUSION_NODE_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/network/network_trt.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/network/network_trt.hpp similarity index 86% rename from perception/lidar_transfusion/include/lidar_transfusion/network/network_trt.hpp rename to perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/network/network_trt.hpp index 3f8f32e346b0a..d237361a436e2 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/network/network_trt.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/network/network_trt.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_TRANSFUSION__NETWORK__NETWORK_TRT_HPP_ -#define LIDAR_TRANSFUSION__NETWORK__NETWORK_TRT_HPP_ +#ifndef AUTOWARE__LIDAR_TRANSFUSION__NETWORK__NETWORK_TRT_HPP_ +#define AUTOWARE__LIDAR_TRANSFUSION__NETWORK__NETWORK_TRT_HPP_ -#include "lidar_transfusion/transfusion_config.hpp" -#include "lidar_transfusion/utils.hpp" +#include "autoware/lidar_transfusion/transfusion_config.hpp" +#include "autoware/lidar_transfusion/utils.hpp" #include @@ -29,7 +29,7 @@ #include #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { struct ProfileDimension @@ -82,6 +82,6 @@ class NetworkTRT std::array in_profile_dims_; }; -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion -#endif // LIDAR_TRANSFUSION__NETWORK__NETWORK_TRT_HPP_ +#endif // AUTOWARE__LIDAR_TRANSFUSION__NETWORK__NETWORK_TRT_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/postprocess/circle_nms_kernel.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/postprocess/circle_nms_kernel.hpp similarity index 72% rename from perception/lidar_transfusion/include/lidar_transfusion/postprocess/circle_nms_kernel.hpp rename to perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/postprocess/circle_nms_kernel.hpp index e231482652b98..653bc281c7086 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/postprocess/circle_nms_kernel.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/postprocess/circle_nms_kernel.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_TRANSFUSION__POSTPROCESS__CIRCLE_NMS_KERNEL_HPP_ -#define LIDAR_TRANSFUSION__POSTPROCESS__CIRCLE_NMS_KERNEL_HPP_ +#ifndef AUTOWARE__LIDAR_TRANSFUSION__POSTPROCESS__CIRCLE_NMS_KERNEL_HPP_ +#define AUTOWARE__LIDAR_TRANSFUSION__POSTPROCESS__CIRCLE_NMS_KERNEL_HPP_ -#include "lidar_transfusion/utils.hpp" +#include "autoware/lidar_transfusion/utils.hpp" #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { // Non-maximum suppression (NMS) uses the distance on the xy plane instead of // intersection over union (IoU) to suppress overlapped objects. @@ -27,6 +27,6 @@ std::size_t circleNMS( thrust::device_vector & boxes3d, const float distance_threshold, thrust::device_vector & keep_mask, cudaStream_t stream); -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion -#endif // LIDAR_TRANSFUSION__POSTPROCESS__CIRCLE_NMS_KERNEL_HPP_ +#endif // AUTOWARE__LIDAR_TRANSFUSION__POSTPROCESS__CIRCLE_NMS_KERNEL_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/postprocess/non_maximum_suppression.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/postprocess/non_maximum_suppression.hpp similarity index 83% rename from perception/lidar_transfusion/include/lidar_transfusion/postprocess/non_maximum_suppression.hpp rename to perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/postprocess/non_maximum_suppression.hpp index daf2cc7c1fac1..9f07d2448fcae 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/postprocess/non_maximum_suppression.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/postprocess/non_maximum_suppression.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_TRANSFUSION__POSTPROCESS__NON_MAXIMUM_SUPPRESSION_HPP_ -#define LIDAR_TRANSFUSION__POSTPROCESS__NON_MAXIMUM_SUPPRESSION_HPP_ +#ifndef AUTOWARE__LIDAR_TRANSFUSION__POSTPROCESS__NON_MAXIMUM_SUPPRESSION_HPP_ +#define AUTOWARE__LIDAR_TRANSFUSION__POSTPROCESS__NON_MAXIMUM_SUPPRESSION_HPP_ -#include "lidar_transfusion/ros_utils.hpp" +#include "autoware/lidar_transfusion/ros_utils.hpp" #include @@ -24,7 +24,7 @@ #include #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { using autoware_perception_msgs::msg::DetectedObject; @@ -75,6 +75,6 @@ class NonMaximumSuppression std::vector target_class_mask_{}; }; -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion -#endif // LIDAR_TRANSFUSION__POSTPROCESS__NON_MAXIMUM_SUPPRESSION_HPP_ +#endif // AUTOWARE__LIDAR_TRANSFUSION__POSTPROCESS__NON_MAXIMUM_SUPPRESSION_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/postprocess/postprocess_kernel.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/postprocess/postprocess_kernel.hpp similarity index 74% rename from perception/lidar_transfusion/include/lidar_transfusion/postprocess/postprocess_kernel.hpp rename to perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/postprocess/postprocess_kernel.hpp index 01435424aa248..ed677bc84b867 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/postprocess/postprocess_kernel.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/postprocess/postprocess_kernel.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_TRANSFUSION__POSTPROCESS__POSTPROCESS_KERNEL_HPP_ -#define LIDAR_TRANSFUSION__POSTPROCESS__POSTPROCESS_KERNEL_HPP_ +#ifndef AUTOWARE__LIDAR_TRANSFUSION__POSTPROCESS__POSTPROCESS_KERNEL_HPP_ +#define AUTOWARE__LIDAR_TRANSFUSION__POSTPROCESS__POSTPROCESS_KERNEL_HPP_ -#include "lidar_transfusion/transfusion_config.hpp" -#include "lidar_transfusion/utils.hpp" +#include "autoware/lidar_transfusion/transfusion_config.hpp" +#include "autoware/lidar_transfusion/utils.hpp" #include #include @@ -24,7 +24,7 @@ #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { class PostprocessCuda @@ -45,6 +45,6 @@ class PostprocessCuda thrust::device_vector yaw_norm_thresholds_d_; }; -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion -#endif // LIDAR_TRANSFUSION__POSTPROCESS__POSTPROCESS_KERNEL_HPP_ +#endif // AUTOWARE__LIDAR_TRANSFUSION__POSTPROCESS__POSTPROCESS_KERNEL_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/preprocess/pointcloud_densification.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/preprocess/pointcloud_densification.hpp similarity index 88% rename from perception/lidar_transfusion/include/lidar_transfusion/preprocess/pointcloud_densification.hpp rename to perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/preprocess/pointcloud_densification.hpp index 6ac0a6544389f..2d03978497f72 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/preprocess/pointcloud_densification.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/preprocess/pointcloud_densification.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_TRANSFUSION__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_ -#define LIDAR_TRANSFUSION__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_ +#ifndef AUTOWARE__LIDAR_TRANSFUSION__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_ +#define AUTOWARE__LIDAR_TRANSFUSION__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_ -#include "lidar_transfusion/cuda_utils.hpp" +#include "autoware/lidar_transfusion/cuda_utils.hpp" #include #include @@ -29,7 +29,7 @@ #include #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { class DensificationParam @@ -96,6 +96,6 @@ class PointCloudDensification cudaStream_t stream_; }; -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion -#endif // LIDAR_TRANSFUSION__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_ +#endif // AUTOWARE__LIDAR_TRANSFUSION__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/preprocess/preprocess_kernel.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/preprocess/preprocess_kernel.hpp similarity index 83% rename from perception/lidar_transfusion/include/lidar_transfusion/preprocess/preprocess_kernel.hpp rename to perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/preprocess/preprocess_kernel.hpp index 592f09c2d288a..1f5fe3ae3907f 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/preprocess/preprocess_kernel.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/preprocess/preprocess_kernel.hpp @@ -28,16 +28,16 @@ * limitations under the License. */ -#ifndef LIDAR_TRANSFUSION__PREPROCESS__PREPROCESS_KERNEL_HPP_ -#define LIDAR_TRANSFUSION__PREPROCESS__PREPROCESS_KERNEL_HPP_ +#ifndef AUTOWARE__LIDAR_TRANSFUSION__PREPROCESS__PREPROCESS_KERNEL_HPP_ +#define AUTOWARE__LIDAR_TRANSFUSION__PREPROCESS__PREPROCESS_KERNEL_HPP_ -#include "lidar_transfusion/cuda_utils.hpp" -#include "lidar_transfusion/transfusion_config.hpp" -#include "lidar_transfusion/utils.hpp" +#include "autoware/lidar_transfusion/cuda_utils.hpp" +#include "autoware/lidar_transfusion/transfusion_config.hpp" +#include "autoware/lidar_transfusion/utils.hpp" #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { class PreprocessCuda @@ -68,6 +68,6 @@ class PreprocessCuda unsigned int mask_size_; unsigned int voxels_size_; }; -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion -#endif // LIDAR_TRANSFUSION__PREPROCESS__PREPROCESS_KERNEL_HPP_ +#endif // AUTOWARE__LIDAR_TRANSFUSION__PREPROCESS__PREPROCESS_KERNEL_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/preprocess/voxel_generator.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/preprocess/voxel_generator.hpp similarity index 76% rename from perception/lidar_transfusion/include/lidar_transfusion/preprocess/voxel_generator.hpp rename to perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/preprocess/voxel_generator.hpp index f0d253ee28755..19bd8c14787de 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/preprocess/voxel_generator.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/preprocess/voxel_generator.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_TRANSFUSION__PREPROCESS__VOXEL_GENERATOR_HPP_ -#define LIDAR_TRANSFUSION__PREPROCESS__VOXEL_GENERATOR_HPP_ +#ifndef AUTOWARE__LIDAR_TRANSFUSION__PREPROCESS__VOXEL_GENERATOR_HPP_ +#define AUTOWARE__LIDAR_TRANSFUSION__PREPROCESS__VOXEL_GENERATOR_HPP_ -#include "lidar_transfusion/cuda_utils.hpp" -#include "lidar_transfusion/preprocess/pointcloud_densification.hpp" -#include "lidar_transfusion/preprocess/preprocess_kernel.hpp" -#include "lidar_transfusion/ros_utils.hpp" -#include "lidar_transfusion/transfusion_config.hpp" +#include "autoware/lidar_transfusion/cuda_utils.hpp" +#include "autoware/lidar_transfusion/preprocess/pointcloud_densification.hpp" +#include "autoware/lidar_transfusion/preprocess/preprocess_kernel.hpp" +#include "autoware/lidar_transfusion/ros_utils.hpp" +#include "autoware/lidar_transfusion/transfusion_config.hpp" #ifdef ROS_DISTRO_GALACTIC #include @@ -36,7 +36,7 @@ #include #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { constexpr size_t AFF_MAT_SIZE = 16; // 4x4 matrix constexpr size_t MAX_CLOUD_STEP_SIZE = sizeof(autoware_point_types::PointXYZIRCAEDT); @@ -67,6 +67,6 @@ class VoxelGenerator bool is_initialized_{false}; }; -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion -#endif // LIDAR_TRANSFUSION__PREPROCESS__VOXEL_GENERATOR_HPP_ +#endif // AUTOWARE__LIDAR_TRANSFUSION__PREPROCESS__VOXEL_GENERATOR_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/ros_utils.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/ros_utils.hpp similarity index 93% rename from perception/lidar_transfusion/include/lidar_transfusion/ros_utils.hpp rename to perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/ros_utils.hpp index cbfc4e87b1610..37b364ecf8a9d 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/ros_utils.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/ros_utils.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_TRANSFUSION__ROS_UTILS_HPP_ -#define LIDAR_TRANSFUSION__ROS_UTILS_HPP_ +#ifndef AUTOWARE__LIDAR_TRANSFUSION__ROS_UTILS_HPP_ +#define AUTOWARE__LIDAR_TRANSFUSION__ROS_UTILS_HPP_ -#include "lidar_transfusion/utils.hpp" +#include "autoware/lidar_transfusion/utils.hpp" #include @@ -41,7 +41,7 @@ CHECK_OFFSET(offset, structure, field); \ CHECK_TYPE(type, structure, field) -namespace lidar_transfusion +namespace autoware::lidar_transfusion { using sensor_msgs::msg::PointField; @@ -102,6 +102,6 @@ void box3DToDetectedObject( uint8_t getSemanticType(const std::string & class_name); -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion -#endif // LIDAR_TRANSFUSION__ROS_UTILS_HPP_ +#endif // AUTOWARE__LIDAR_TRANSFUSION__ROS_UTILS_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/transfusion_config.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_config.hpp similarity index 95% rename from perception/lidar_transfusion/include/lidar_transfusion/transfusion_config.hpp rename to perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_config.hpp index 0d0148d2f6c17..363ee17a0d6e0 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/transfusion_config.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_config.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_TRANSFUSION__TRANSFUSION_CONFIG_HPP_ -#define LIDAR_TRANSFUSION__TRANSFUSION_CONFIG_HPP_ +#ifndef AUTOWARE__LIDAR_TRANSFUSION__TRANSFUSION_CONFIG_HPP_ +#define AUTOWARE__LIDAR_TRANSFUSION__TRANSFUSION_CONFIG_HPP_ #include #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { class TransfusionConfig @@ -165,6 +165,6 @@ class TransfusionConfig std::size_t max_coors_dim_size_{num_point_values_}; }; -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion -#endif // LIDAR_TRANSFUSION__TRANSFUSION_CONFIG_HPP_ +#endif // AUTOWARE__LIDAR_TRANSFUSION__TRANSFUSION_CONFIG_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/transfusion_trt.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_trt.hpp similarity index 80% rename from perception/lidar_transfusion/include/lidar_transfusion/transfusion_trt.hpp rename to perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_trt.hpp index 0a9ea413dc30d..4ba3474fc1d72 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/transfusion_trt.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_trt.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_TRANSFUSION__TRANSFUSION_TRT_HPP_ -#define LIDAR_TRANSFUSION__TRANSFUSION_TRT_HPP_ - -#include "lidar_transfusion/cuda_utils.hpp" -#include "lidar_transfusion/network/network_trt.hpp" -#include "lidar_transfusion/postprocess/postprocess_kernel.hpp" -#include "lidar_transfusion/preprocess/pointcloud_densification.hpp" -#include "lidar_transfusion/preprocess/preprocess_kernel.hpp" -#include "lidar_transfusion/preprocess/voxel_generator.hpp" -#include "lidar_transfusion/utils.hpp" -#include "lidar_transfusion/visibility_control.hpp" +#ifndef AUTOWARE__LIDAR_TRANSFUSION__TRANSFUSION_TRT_HPP_ +#define AUTOWARE__LIDAR_TRANSFUSION__TRANSFUSION_TRT_HPP_ + +#include "autoware/lidar_transfusion/cuda_utils.hpp" +#include "autoware/lidar_transfusion/network/network_trt.hpp" +#include "autoware/lidar_transfusion/postprocess/postprocess_kernel.hpp" +#include "autoware/lidar_transfusion/preprocess/pointcloud_densification.hpp" +#include "autoware/lidar_transfusion/preprocess/preprocess_kernel.hpp" +#include "autoware/lidar_transfusion/preprocess/voxel_generator.hpp" +#include "autoware/lidar_transfusion/utils.hpp" +#include "autoware/lidar_transfusion/visibility_control.hpp" #include @@ -37,7 +37,7 @@ #include #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { class NetworkParam @@ -109,6 +109,6 @@ class LIDAR_TRANSFUSION_PUBLIC TransfusionTRT cuda::unique_ptr dir_cls_output_d_{nullptr}; }; -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion -#endif // LIDAR_TRANSFUSION__TRANSFUSION_TRT_HPP_ +#endif // AUTOWARE__LIDAR_TRANSFUSION__TRANSFUSION_TRT_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/utils.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/utils.hpp similarity index 84% rename from perception/lidar_transfusion/include/lidar_transfusion/utils.hpp rename to perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/utils.hpp index cc40e55851738..feea2216c5f78 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/utils.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/utils.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_TRANSFUSION__UTILS_HPP_ -#define LIDAR_TRANSFUSION__UTILS_HPP_ +#ifndef AUTOWARE__LIDAR_TRANSFUSION__UTILS_HPP_ +#define AUTOWARE__LIDAR_TRANSFUSION__UTILS_HPP_ #include #include #include #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { struct Box3D @@ -52,6 +52,6 @@ unsigned int divup(const T1 a, const T2 b) return (a + b - 1) / b; } -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion -#endif // LIDAR_TRANSFUSION__UTILS_HPP_ +#endif // AUTOWARE__LIDAR_TRANSFUSION__UTILS_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/visibility_control.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/visibility_control.hpp similarity index 89% rename from perception/lidar_transfusion/include/lidar_transfusion/visibility_control.hpp rename to perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/visibility_control.hpp index aeab20906628a..fe8bea1ae1403 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/visibility_control.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/visibility_control.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_TRANSFUSION__VISIBILITY_CONTROL_HPP_ -#define LIDAR_TRANSFUSION__VISIBILITY_CONTROL_HPP_ +#ifndef AUTOWARE__LIDAR_TRANSFUSION__VISIBILITY_CONTROL_HPP_ +#define AUTOWARE__LIDAR_TRANSFUSION__VISIBILITY_CONTROL_HPP_ //////////////////////////////////////////////////////////////////////////////// #if defined(__WIN32) @@ -34,4 +34,4 @@ #error "Unsupported Build Configuration" #endif -#endif // LIDAR_TRANSFUSION__VISIBILITY_CONTROL_HPP_ +#endif // AUTOWARE__LIDAR_TRANSFUSION__VISIBILITY_CONTROL_HPP_ diff --git a/perception/lidar_transfusion/launch/lidar_transfusion.launch.xml b/perception/autoware_lidar_transfusion/launch/lidar_transfusion.launch.xml similarity index 79% rename from perception/lidar_transfusion/launch/lidar_transfusion.launch.xml rename to perception/autoware_lidar_transfusion/launch/lidar_transfusion.launch.xml index 492eb8c4d59b7..c14802dcddc55 100644 --- a/perception/lidar_transfusion/launch/lidar_transfusion.launch.xml +++ b/perception/autoware_lidar_transfusion/launch/lidar_transfusion.launch.xml @@ -4,8 +4,8 @@ - - + + @@ -14,7 +14,7 @@ - + @@ -26,7 +26,7 @@ - + diff --git a/perception/lidar_transfusion/lib/detection_class_remapper.cpp b/perception/autoware_lidar_transfusion/lib/detection_class_remapper.cpp similarity index 94% rename from perception/lidar_transfusion/lib/detection_class_remapper.cpp rename to perception/autoware_lidar_transfusion/lib/detection_class_remapper.cpp index e093637402448..08bdc65e666de 100644 --- a/perception/lidar_transfusion/lib/detection_class_remapper.cpp +++ b/perception/autoware_lidar_transfusion/lib/detection_class_remapper.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { void DetectionClassRemapper::setParameters( @@ -68,4 +68,4 @@ void DetectionClassRemapper::mapClasses(autoware_perception_msgs::msg::DetectedO } } -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion diff --git a/perception/lidar_transfusion/lib/network/network_trt.cpp b/perception/autoware_lidar_transfusion/lib/network/network_trt.cpp similarity index 98% rename from perception/lidar_transfusion/lib/network/network_trt.cpp rename to perception/autoware_lidar_transfusion/lib/network/network_trt.cpp index a36890facceec..3eacf005cff4e 100644 --- a/perception/lidar_transfusion/lib/network/network_trt.cpp +++ b/perception/autoware_lidar_transfusion/lib/network/network_trt.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_transfusion/network/network_trt.hpp" +#include "autoware/lidar_transfusion/network/network_trt.hpp" #include @@ -20,7 +20,7 @@ #include #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { std::ostream & operator<<(std::ostream & os, const ProfileDimension & profile) @@ -329,4 +329,4 @@ nvinfer1::Dims NetworkTRT::validateTensorShape(NetworkIO name, const std::vector return tensor_shape; } -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion diff --git a/perception/lidar_transfusion/lib/postprocess/circle_nms_kernel.cu b/perception/autoware_lidar_transfusion/lib/postprocess/circle_nms_kernel.cu similarity index 94% rename from perception/lidar_transfusion/lib/postprocess/circle_nms_kernel.cu rename to perception/autoware_lidar_transfusion/lib/postprocess/circle_nms_kernel.cu index dcb60831825bb..2328ca53af530 100644 --- a/perception/lidar_transfusion/lib/postprocess/circle_nms_kernel.cu +++ b/perception/autoware_lidar_transfusion/lib/postprocess/circle_nms_kernel.cu @@ -21,9 +21,9 @@ Written by Shaoshuai Shi All Rights Reserved 2019-2020. */ -#include "lidar_transfusion/cuda_utils.hpp" -#include "lidar_transfusion/postprocess/circle_nms_kernel.hpp" -#include "lidar_transfusion/utils.hpp" +#include "autoware/lidar_transfusion/cuda_utils.hpp" +#include "autoware/lidar_transfusion/postprocess/circle_nms_kernel.hpp" +#include "autoware/lidar_transfusion/utils.hpp" #include @@ -32,7 +32,7 @@ namespace const std::size_t THREADS_PER_BLOCK_NMS = 16; } // namespace -namespace lidar_transfusion +namespace autoware::lidar_transfusion { __device__ inline float dist2dPow(const Box3D * a, const Box3D * b) @@ -141,4 +141,4 @@ std::size_t circleNMS( return num_to_keep; } -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion diff --git a/perception/lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp b/perception/autoware_lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp similarity index 95% rename from perception/lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp rename to perception/autoware_lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp index dd12c52970d38..6e563f706e3b2 100644 --- a/perception/lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp +++ b/perception/autoware_lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_transfusion/postprocess/non_maximum_suppression.hpp" +#include "autoware/lidar_transfusion/postprocess/non_maximum_suppression.hpp" #include #include #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { void NonMaximumSuppression::setParameters(const NMSParams & params) @@ -101,4 +101,4 @@ std::vector NonMaximumSuppression::apply( return output_objects; } -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion diff --git a/perception/lidar_transfusion/lib/postprocess/postprocess_kernel.cu b/perception/autoware_lidar_transfusion/lib/postprocess/postprocess_kernel.cu similarity index 95% rename from perception/lidar_transfusion/lib/postprocess/postprocess_kernel.cu rename to perception/autoware_lidar_transfusion/lib/postprocess/postprocess_kernel.cu index bca23e9961b40..fbdb89d59b3d1 100644 --- a/perception/lidar_transfusion/lib/postprocess/postprocess_kernel.cu +++ b/perception/autoware_lidar_transfusion/lib/postprocess/postprocess_kernel.cu @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_transfusion/postprocess/circle_nms_kernel.hpp" -#include "lidar_transfusion/postprocess/postprocess_kernel.hpp" +#include "autoware/lidar_transfusion/postprocess/circle_nms_kernel.hpp" +#include "autoware/lidar_transfusion/postprocess/postprocess_kernel.hpp" #include #include #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { const size_t THREADS_PER_BLOCK = 256; @@ -142,4 +142,4 @@ cudaError_t PostprocessCuda::generateDetectedBoxes3D_launch( return cudaGetLastError(); } -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion diff --git a/perception/lidar_transfusion/lib/preprocess/pointcloud_densification.cpp b/perception/autoware_lidar_transfusion/lib/preprocess/pointcloud_densification.cpp similarity index 95% rename from perception/lidar_transfusion/lib/preprocess/pointcloud_densification.cpp rename to perception/autoware_lidar_transfusion/lib/preprocess/pointcloud_densification.cpp index c13423f2d24d8..17491ede058b4 100644 --- a/perception/lidar_transfusion/lib/preprocess/pointcloud_densification.cpp +++ b/perception/autoware_lidar_transfusion/lib/preprocess/pointcloud_densification.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_transfusion/preprocess/pointcloud_densification.hpp" +#include "autoware/lidar_transfusion/preprocess/pointcloud_densification.hpp" #include @@ -55,7 +55,7 @@ Eigen::Affine3f transformToEigen(const geometry_msgs::msg::Transform & t) } // namespace -namespace lidar_transfusion +namespace autoware::lidar_transfusion { PointCloudDensification::PointCloudDensification( @@ -113,4 +113,4 @@ void PointCloudDensification::dequeue() } } -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion diff --git a/perception/lidar_transfusion/lib/preprocess/preprocess_kernel.cu b/perception/autoware_lidar_transfusion/lib/preprocess/preprocess_kernel.cu similarity index 97% rename from perception/lidar_transfusion/lib/preprocess/preprocess_kernel.cu rename to perception/autoware_lidar_transfusion/lib/preprocess/preprocess_kernel.cu index 48bb4eb9332a8..1b18b3dbcc006 100644 --- a/perception/lidar_transfusion/lib/preprocess/preprocess_kernel.cu +++ b/perception/autoware_lidar_transfusion/lib/preprocess/preprocess_kernel.cu @@ -28,12 +28,12 @@ * limitations under the License. */ -#include "lidar_transfusion/cuda_utils.hpp" -#include "lidar_transfusion/preprocess/preprocess_kernel.hpp" +#include "autoware/lidar_transfusion/cuda_utils.hpp" +#include "autoware/lidar_transfusion/preprocess/preprocess_kernel.hpp" #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { PreprocessCuda::PreprocessCuda(const TransfusionConfig & config, cudaStream_t & stream) @@ -218,4 +218,4 @@ cudaError_t PreprocessCuda::generateSweepPoints_launch( return err; } -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion diff --git a/perception/lidar_transfusion/lib/preprocess/voxel_generator.cpp b/perception/autoware_lidar_transfusion/lib/preprocess/voxel_generator.cpp similarity index 95% rename from perception/lidar_transfusion/lib/preprocess/voxel_generator.cpp rename to perception/autoware_lidar_transfusion/lib/preprocess/voxel_generator.cpp index cb8bac984aef3..203a556d9057a 100644 --- a/perception/lidar_transfusion/lib/preprocess/voxel_generator.cpp +++ b/perception/autoware_lidar_transfusion/lib/preprocess/voxel_generator.cpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_transfusion/preprocess/voxel_generator.hpp" +#include "autoware/lidar_transfusion/preprocess/voxel_generator.hpp" -#include "lidar_transfusion/preprocess/preprocess_kernel.hpp" +#include "autoware/lidar_transfusion/preprocess/preprocess_kernel.hpp" #include #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { VoxelGenerator::VoxelGenerator( @@ -120,4 +120,4 @@ std::tuple VoxelGenerator::getFiel } throw std::runtime_error("Missing field: " + field_name); } -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion diff --git a/perception/lidar_transfusion/lib/ros_utils.cpp b/perception/autoware_lidar_transfusion/lib/ros_utils.cpp similarity index 95% rename from perception/lidar_transfusion/lib/ros_utils.cpp rename to perception/autoware_lidar_transfusion/lib/ros_utils.cpp index ef5c45c339b64..dce4028759de5 100644 --- a/perception/lidar_transfusion/lib/ros_utils.cpp +++ b/perception/autoware_lidar_transfusion/lib/ros_utils.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_transfusion/ros_utils.hpp" +#include "autoware/lidar_transfusion/ros_utils.hpp" #include #include #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { using Label = autoware_perception_msgs::msg::ObjectClassification; @@ -80,4 +80,4 @@ uint8_t getSemanticType(const std::string & class_name) } } -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion diff --git a/perception/lidar_transfusion/lib/transfusion_trt.cpp b/perception/autoware_lidar_transfusion/lib/transfusion_trt.cpp similarity index 96% rename from perception/lidar_transfusion/lib/transfusion_trt.cpp rename to perception/autoware_lidar_transfusion/lib/transfusion_trt.cpp index d940b83c12cb4..94760d4d91720 100644 --- a/perception/lidar_transfusion/lib/transfusion_trt.cpp +++ b/perception/autoware_lidar_transfusion/lib/transfusion_trt.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_transfusion/transfusion_trt.hpp" +#include "autoware/lidar_transfusion/transfusion_trt.hpp" -#include "lidar_transfusion/preprocess/preprocess_kernel.hpp" -#include "lidar_transfusion/transfusion_config.hpp" +#include "autoware/lidar_transfusion/preprocess/preprocess_kernel.hpp" +#include "autoware/lidar_transfusion/transfusion_config.hpp" #include @@ -24,7 +24,7 @@ #include #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { TransfusionTRT::TransfusionTRT( @@ -203,4 +203,4 @@ bool TransfusionTRT::postprocess(std::vector & det_boxes3d) return true; } -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion diff --git a/perception/lidar_transfusion/package.xml b/perception/autoware_lidar_transfusion/package.xml similarity index 96% rename from perception/lidar_transfusion/package.xml rename to perception/autoware_lidar_transfusion/package.xml index 3b495025b1c34..834afba1221ad 100644 --- a/perception/lidar_transfusion/package.xml +++ b/perception/autoware_lidar_transfusion/package.xml @@ -1,7 +1,7 @@ - lidar_transfusion + autoware_lidar_transfusion 1.0.0 The lidar_transfusion package Amadeusz Szymko diff --git a/perception/lidar_transfusion/schema/detection_class_remapper.schema.json b/perception/autoware_lidar_transfusion/schema/detection_class_remapper.schema.json similarity index 100% rename from perception/lidar_transfusion/schema/detection_class_remapper.schema.json rename to perception/autoware_lidar_transfusion/schema/detection_class_remapper.schema.json diff --git a/perception/lidar_transfusion/schema/transfusion.schema.json b/perception/autoware_lidar_transfusion/schema/transfusion.schema.json similarity index 100% rename from perception/lidar_transfusion/schema/transfusion.schema.json rename to perception/autoware_lidar_transfusion/schema/transfusion.schema.json diff --git a/perception/lidar_transfusion/src/lidar_transfusion_node.cpp b/perception/autoware_lidar_transfusion/src/lidar_transfusion_node.cpp similarity index 96% rename from perception/lidar_transfusion/src/lidar_transfusion_node.cpp rename to perception/autoware_lidar_transfusion/src/lidar_transfusion_node.cpp index a07e385208748..fae1cc97404da 100644 --- a/perception/lidar_transfusion/src/lidar_transfusion_node.cpp +++ b/perception/autoware_lidar_transfusion/src/lidar_transfusion_node.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_transfusion/lidar_transfusion_node.hpp" +#include "autoware/lidar_transfusion/lidar_transfusion_node.hpp" -#include "lidar_transfusion/utils.hpp" +#include "autoware/lidar_transfusion/utils.hpp" -namespace lidar_transfusion +namespace autoware::lidar_transfusion { LidarTransfusionNode::LidarTransfusionNode(const rclcpp::NodeOptions & options) @@ -175,8 +175,8 @@ void LidarTransfusionNode::cloudCallback(const sensor_msgs::msg::PointCloud2::Co } } -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(lidar_transfusion::LidarTransfusionNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::lidar_transfusion::LidarTransfusionNode) diff --git a/perception/lidar_transfusion/test/test_detection_class_remapper.cpp b/perception/autoware_lidar_transfusion/test/test_detection_class_remapper.cpp similarity index 96% rename from perception/lidar_transfusion/test/test_detection_class_remapper.cpp rename to perception/autoware_lidar_transfusion/test/test_detection_class_remapper.cpp index 6e7f896e44d2c..a9b5c8e83fbf7 100644 --- a/perception/lidar_transfusion/test/test_detection_class_remapper.cpp +++ b/perception/autoware_lidar_transfusion/test/test_detection_class_remapper.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include TEST(DetectionClassRemapperTest, MapClasses) { - lidar_transfusion::DetectionClassRemapper remapper; + autoware::lidar_transfusion::DetectionClassRemapper remapper; // Set up the parameters for the remapper // Labels: CAR, TRUCK, TRAILER diff --git a/perception/lidar_transfusion/test/test_nms.cpp b/perception/autoware_lidar_transfusion/test/test_nms.cpp similarity index 92% rename from perception/lidar_transfusion/test/test_nms.cpp rename to perception/autoware_lidar_transfusion/test/test_nms.cpp index b6f0ec8c31684..654cb56078c57 100644 --- a/perception/lidar_transfusion/test/test_nms.cpp +++ b/perception/autoware_lidar_transfusion/test/test_nms.cpp @@ -12,21 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_transfusion/postprocess/non_maximum_suppression.hpp" +#include "autoware/lidar_transfusion/postprocess/non_maximum_suppression.hpp" #include TEST(NonMaximumSuppressionTest, Apply) { - lidar_transfusion::NonMaximumSuppression nms; - lidar_transfusion::NMSParams params; + autoware::lidar_transfusion::NonMaximumSuppression nms; + autoware::lidar_transfusion::NMSParams params; params.search_distance_2d_ = 1.0; params.iou_threshold_ = 0.2; - params.nms_type_ = lidar_transfusion::NMS_TYPE::IoU_BEV; + params.nms_type_ = autoware::lidar_transfusion::NMS_TYPE::IoU_BEV; params.target_class_names_ = {"CAR"}; nms.setParameters(params); - std::vector input_objects(4); + std::vector input_objects(4); // Object 1 autoware_perception_msgs::msg::ObjectClassification obj1_classification; @@ -88,7 +88,8 @@ TEST(NonMaximumSuppressionTest, Apply) input_objects[3].shape.dimensions.x = 0.5; input_objects[3].shape.dimensions.y = 0.5; - std::vector output_objects = nms.apply(input_objects); + std::vector output_objects = + nms.apply(input_objects); // Assert the expected number of output objects EXPECT_EQ(output_objects.size(), 3); diff --git a/perception/lidar_transfusion/test/test_postprocess_kernel.cpp b/perception/autoware_lidar_transfusion/test/test_postprocess_kernel.cpp similarity index 99% rename from perception/lidar_transfusion/test/test_postprocess_kernel.cpp rename to perception/autoware_lidar_transfusion/test/test_postprocess_kernel.cpp index 1e01db5a8b3d7..85c408b93f4ca 100644 --- a/perception/lidar_transfusion/test/test_postprocess_kernel.cpp +++ b/perception/autoware_lidar_transfusion/test/test_postprocess_kernel.cpp @@ -19,7 +19,7 @@ #include #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { void PostprocessKernelTest::SetUp() @@ -290,7 +290,7 @@ TEST_F(PostprocessKernelTest, CircleNMSTest) EXPECT_EQ(1, det_boxes3d.size()); } -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion int main(int argc, char ** argv) { diff --git a/perception/lidar_transfusion/test/test_postprocess_kernel.hpp b/perception/autoware_lidar_transfusion/test/test_postprocess_kernel.hpp similarity index 85% rename from perception/lidar_transfusion/test/test_postprocess_kernel.hpp rename to perception/autoware_lidar_transfusion/test/test_postprocess_kernel.hpp index 60fa7882ecc11..104f42ad9e75d 100644 --- a/perception/lidar_transfusion/test/test_postprocess_kernel.hpp +++ b/perception/autoware_lidar_transfusion/test/test_postprocess_kernel.hpp @@ -15,14 +15,14 @@ #ifndef TEST_POSTPROCESS_KERNEL_HPP_ #define TEST_POSTPROCESS_KERNEL_HPP_ -#include -#include +#include +#include #include #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { class PostprocessKernelTest : public testing::Test @@ -43,6 +43,6 @@ class PostprocessKernelTest : public testing::Test cuda::unique_ptr dir_cls_output_d_{nullptr}; }; -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion #endif // TEST_POSTPROCESS_KERNEL_HPP_ diff --git a/perception/lidar_transfusion/test/test_preprocess_kernel.cpp b/perception/autoware_lidar_transfusion/test/test_preprocess_kernel.cpp similarity index 97% rename from perception/lidar_transfusion/test/test_preprocess_kernel.cpp rename to perception/autoware_lidar_transfusion/test/test_preprocess_kernel.cpp index c1c2a824f0f53..1dbbf27d4a127 100644 --- a/perception/lidar_transfusion/test/test_preprocess_kernel.cpp +++ b/perception/autoware_lidar_transfusion/test/test_preprocess_kernel.cpp @@ -14,9 +14,9 @@ #include "test_preprocess_kernel.hpp" -#include "lidar_transfusion/cuda_utils.hpp" +#include "autoware/lidar_transfusion/cuda_utils.hpp" -#include +#include #include #include @@ -25,7 +25,7 @@ #include #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { void PreprocessKernelTest::SetUp() @@ -256,7 +256,7 @@ TEST_F(PreprocessKernelTest, VoxelOverflowTest) EXPECT_EQ(config_ptr_->max_voxels_, params_input); } -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion int main(int argc, char ** argv) { diff --git a/perception/lidar_transfusion/test/test_preprocess_kernel.hpp b/perception/autoware_lidar_transfusion/test/test_preprocess_kernel.hpp similarity index 87% rename from perception/lidar_transfusion/test/test_preprocess_kernel.hpp rename to perception/autoware_lidar_transfusion/test/test_preprocess_kernel.hpp index 024fae5eae4b1..260e4261c3c42 100644 --- a/perception/lidar_transfusion/test/test_preprocess_kernel.hpp +++ b/perception/autoware_lidar_transfusion/test/test_preprocess_kernel.hpp @@ -15,14 +15,14 @@ #ifndef TEST_PREPROCESS_KERNEL_HPP_ #define TEST_PREPROCESS_KERNEL_HPP_ -#include -#include +#include +#include #include #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { class PreprocessKernelTest : public testing::Test @@ -45,6 +45,6 @@ class PreprocessKernelTest : public testing::Test cuda::unique_ptr voxel_idxs_d_{nullptr}; }; -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion #endif // TEST_PREPROCESS_KERNEL_HPP_ diff --git a/perception/lidar_transfusion/test/test_ros_utils.cpp b/perception/autoware_lidar_transfusion/test/test_ros_utils.cpp similarity index 79% rename from perception/lidar_transfusion/test/test_ros_utils.cpp rename to perception/autoware_lidar_transfusion/test/test_ros_utils.cpp index 15541e6353b42..ac9c4ca0378cb 100644 --- a/perception/lidar_transfusion/test/test_ros_utils.cpp +++ b/perception/autoware_lidar_transfusion/test/test_ros_utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_transfusion/ros_utils.hpp" +#include "autoware/lidar_transfusion/ros_utils.hpp" #include @@ -23,7 +23,7 @@ TEST(TestSuite, box3DToDetectedObject) // Test case 1: Test with valid label { - lidar_transfusion::Box3D box3d; + autoware::lidar_transfusion::Box3D box3d; box3d.label = 0; // CAR box3d.score = 0.8f; box3d.x = 1.0; @@ -35,7 +35,7 @@ TEST(TestSuite, box3DToDetectedObject) box3d.yaw = 0.5; autoware_perception_msgs::msg::DetectedObject obj; - lidar_transfusion::box3DToDetectedObject(box3d, class_names, obj); + autoware::lidar_transfusion::box3DToDetectedObject(box3d, class_names, obj); EXPECT_FLOAT_EQ(obj.existence_probability, 0.8f); EXPECT_EQ( @@ -53,12 +53,12 @@ TEST(TestSuite, box3DToDetectedObject) // Test case 2: Test with invalid label { - lidar_transfusion::Box3D box3d; + autoware::lidar_transfusion::Box3D box3d; box3d.score = 0.5f; box3d.label = 10; // Invalid autoware_perception_msgs::msg::DetectedObject obj; - lidar_transfusion::box3DToDetectedObject(box3d, class_names, obj); + autoware::lidar_transfusion::box3DToDetectedObject(box3d, class_names, obj); EXPECT_FLOAT_EQ(obj.existence_probability, 0.5f); EXPECT_EQ( @@ -72,28 +72,28 @@ TEST(TestSuite, box3DToDetectedObject) TEST(TestSuite, getSemanticType) { EXPECT_EQ( - lidar_transfusion::getSemanticType("CAR"), + autoware::lidar_transfusion::getSemanticType("CAR"), autoware_perception_msgs::msg::ObjectClassification::CAR); EXPECT_EQ( - lidar_transfusion::getSemanticType("TRUCK"), + autoware::lidar_transfusion::getSemanticType("TRUCK"), autoware_perception_msgs::msg::ObjectClassification::TRUCK); EXPECT_EQ( - lidar_transfusion::getSemanticType("BUS"), + autoware::lidar_transfusion::getSemanticType("BUS"), autoware_perception_msgs::msg::ObjectClassification::BUS); EXPECT_EQ( - lidar_transfusion::getSemanticType("TRAILER"), + autoware::lidar_transfusion::getSemanticType("TRAILER"), autoware_perception_msgs::msg::ObjectClassification::TRAILER); EXPECT_EQ( - lidar_transfusion::getSemanticType("MOTORCYCLE"), + autoware::lidar_transfusion::getSemanticType("MOTORCYCLE"), autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE); EXPECT_EQ( - lidar_transfusion::getSemanticType("BICYCLE"), + autoware::lidar_transfusion::getSemanticType("BICYCLE"), autoware_perception_msgs::msg::ObjectClassification::BICYCLE); EXPECT_EQ( - lidar_transfusion::getSemanticType("PEDESTRIAN"), + autoware::lidar_transfusion::getSemanticType("PEDESTRIAN"), autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN); EXPECT_EQ( - lidar_transfusion::getSemanticType("UNKNOWN"), + autoware::lidar_transfusion::getSemanticType("UNKNOWN"), autoware_perception_msgs::msg::ObjectClassification::UNKNOWN); } diff --git a/perception/lidar_transfusion/test/test_voxel_generator.cpp b/perception/autoware_lidar_transfusion/test/test_voxel_generator.cpp similarity index 98% rename from perception/lidar_transfusion/test/test_voxel_generator.cpp rename to perception/autoware_lidar_transfusion/test/test_voxel_generator.cpp index 2673a341b3721..d8d7db1302761 100644 --- a/perception/lidar_transfusion/test/test_voxel_generator.cpp +++ b/perception/autoware_lidar_transfusion/test/test_voxel_generator.cpp @@ -14,15 +14,15 @@ #include "test_voxel_generator.hpp" +#include "autoware/lidar_transfusion/transfusion_config.hpp" +#include "autoware/lidar_transfusion/utils.hpp" #include "gtest/gtest.h" -#include "lidar_transfusion/transfusion_config.hpp" -#include "lidar_transfusion/utils.hpp" #include #include "sensor_msgs/point_cloud2_iterator.hpp" -namespace lidar_transfusion +namespace autoware::lidar_transfusion { void VoxelGeneratorTest::SetUp() @@ -274,7 +274,7 @@ TEST_F(VoxelGeneratorTest, TwoFrames) EXPECT_TRUE(std::isnan(points[i])); } } -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion int main(int argc, char ** argv) { diff --git a/perception/lidar_transfusion/test/test_voxel_generator.hpp b/perception/autoware_lidar_transfusion/test/test_voxel_generator.hpp similarity index 92% rename from perception/lidar_transfusion/test/test_voxel_generator.hpp rename to perception/autoware_lidar_transfusion/test/test_voxel_generator.hpp index eccbe6412d183..641a42322a302 100644 --- a/perception/lidar_transfusion/test/test_voxel_generator.hpp +++ b/perception/autoware_lidar_transfusion/test/test_voxel_generator.hpp @@ -15,7 +15,7 @@ #ifndef TEST_VOXEL_GENERATOR_HPP_ #define TEST_VOXEL_GENERATOR_HPP_ -#include +#include #include #include @@ -26,7 +26,7 @@ #include #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { class VoxelGeneratorTest : public testing::Test @@ -60,6 +60,6 @@ class VoxelGeneratorTest : public testing::Test cudaStream_t stream_{}; }; -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion #endif // TEST_VOXEL_GENERATOR_HPP_ diff --git a/perception/autoware_multi_object_tracker/README.md b/perception/autoware_multi_object_tracker/README.md index 447c74d9b3450..afb1598645733 100644 --- a/perception/autoware_multi_object_tracker/README.md +++ b/perception/autoware_multi_object_tracker/README.md @@ -65,16 +65,16 @@ Multiple inputs are pre-defined in the input channel parameters (described below ### Input Channel parameters -{{ json_to_markdown("perception/multi_object_tracker/schema/input_channels.schema.json") }} +{{ json_to_markdown("perception/autoware_multi_object_tracker/schema/input_channels.schema.json") }} ### Core Parameters -{{ json_to_markdown("perception/multi_object_tracker/schema/multi_object_tracker_node.schema.json") }} -{{ json_to_markdown("perception/multi_object_tracker/schema/data_association_matrix.schema.json") }} +{{ json_to_markdown("perception/autoware_multi_object_tracker/schema/multi_object_tracker_node.schema.json") }} +{{ json_to_markdown("perception/autoware_multi_object_tracker/schema/data_association_matrix.schema.json") }} #### Simulation parameters -{{ json_to_markdown("perception/multi_object_tracker/schema/simulation_tracker.schema.json") }} +{{ json_to_markdown("perception/autoware_multi_object_tracker/schema/simulation_tracker.schema.json") }} ## Assumptions / Known limits diff --git a/perception/tensorrt_classifier/CMakeLists.txt b/perception/autoware_tensorrt_classifier/CMakeLists.txt similarity index 97% rename from perception/tensorrt_classifier/CMakeLists.txt rename to perception/autoware_tensorrt_classifier/CMakeLists.txt index 9f879a2c0c56d..88747a1e9240c 100644 --- a/perception/tensorrt_classifier/CMakeLists.txt +++ b/perception/autoware_tensorrt_classifier/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(tensorrt_classifier) +project(autoware_tensorrt_classifier) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17 -O3 -Wno-write-strings -fopenmp -Wall") diff --git a/perception/tensorrt_classifier/README.md b/perception/autoware_tensorrt_classifier/README.md similarity index 100% rename from perception/tensorrt_classifier/README.md rename to perception/autoware_tensorrt_classifier/README.md diff --git a/perception/tensorrt_classifier/include/tensorrt_classifier/calibrator.hpp b/perception/autoware_tensorrt_classifier/include/autoware/tensorrt_classifier/calibrator.hpp similarity index 98% rename from perception/tensorrt_classifier/include/tensorrt_classifier/calibrator.hpp rename to perception/autoware_tensorrt_classifier/include/autoware/tensorrt_classifier/calibrator.hpp index b4d687c3c4fd9..088297f5bf796 100644 --- a/perception/tensorrt_classifier/include/tensorrt_classifier/calibrator.hpp +++ b/perception/autoware_tensorrt_classifier/include/autoware/tensorrt_classifier/calibrator.hpp @@ -34,8 +34,8 @@ * DEALINGS IN THE SOFTWARE. */ -#ifndef TENSORRT_CLASSIFIER__CALIBRATOR_HPP_ -#define TENSORRT_CLASSIFIER__CALIBRATOR_HPP_ +#ifndef AUTOWARE__TENSORRT_CLASSIFIER__CALIBRATOR_HPP_ +#define AUTOWARE__TENSORRT_CLASSIFIER__CALIBRATOR_HPP_ #include "cuda_utils/cuda_check_error.hpp" #include "cuda_utils/cuda_unique_ptr.hpp" @@ -52,7 +52,7 @@ #include #include -namespace tensorrt_classifier +namespace autoware::tensorrt_classifier { class ImageStream { @@ -524,6 +524,6 @@ class Int8MinMaxCalibrator : public nvinfer1::IInt8MinMaxCalibrator // std for preprocessing std::vector m_std; }; -} // namespace tensorrt_classifier +} // namespace autoware::tensorrt_classifier -#endif // TENSORRT_CLASSIFIER__CALIBRATOR_HPP_ +#endif // AUTOWARE__TENSORRT_CLASSIFIER__CALIBRATOR_HPP_ diff --git a/perception/tensorrt_classifier/include/tensorrt_classifier/preprocess.h b/perception/autoware_tensorrt_classifier/include/autoware/tensorrt_classifier/preprocess.h similarity index 97% rename from perception/tensorrt_classifier/include/tensorrt_classifier/preprocess.h rename to perception/autoware_tensorrt_classifier/include/autoware/tensorrt_classifier/preprocess.h index 55295a57ad5b8..51f4e973ba7ca 100644 --- a/perception/tensorrt_classifier/include/tensorrt_classifier/preprocess.h +++ b/perception/autoware_tensorrt_classifier/include/autoware/tensorrt_classifier/preprocess.h @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TENSORRT_CLASSIFIER__PREPROCESS_H_ -#define TENSORRT_CLASSIFIER__PREPROCESS_H_ +#ifndef AUTOWARE__TENSORRT_CLASSIFIER__PREPROCESS_H_ +#define AUTOWARE__TENSORRT_CLASSIFIER__PREPROCESS_H_ #include #include @@ -178,4 +178,4 @@ extern void multi_scale_resize_bilinear_letterbox_nhwc_to_nchw32_batch_gpu( float * dst, unsigned char * src, int d_w, int d_h, int d_c, Roi * d_roi, int s_w, int s_h, int s_c, int batch, float norm, cudaStream_t stream); -#endif // TENSORRT_CLASSIFIER__PREPROCESS_H_ +#endif // AUTOWARE__TENSORRT_CLASSIFIER__PREPROCESS_H_ diff --git a/perception/tensorrt_classifier/include/tensorrt_classifier/tensorrt_classifier.hpp b/perception/autoware_tensorrt_classifier/include/autoware/tensorrt_classifier/tensorrt_classifier.hpp similarity index 93% rename from perception/tensorrt_classifier/include/tensorrt_classifier/tensorrt_classifier.hpp rename to perception/autoware_tensorrt_classifier/include/autoware/tensorrt_classifier/tensorrt_classifier.hpp index d95ee52e8fae9..9fb1beaac39de 100644 --- a/perception/tensorrt_classifier/include/tensorrt_classifier/tensorrt_classifier.hpp +++ b/perception/autoware_tensorrt_classifier/include/autoware/tensorrt_classifier/tensorrt_classifier.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TENSORRT_CLASSIFIER__TENSORRT_CLASSIFIER_HPP_ -#define TENSORRT_CLASSIFIER__TENSORRT_CLASSIFIER_HPP_ +#ifndef AUTOWARE__TENSORRT_CLASSIFIER__TENSORRT_CLASSIFIER_HPP_ +#define AUTOWARE__TENSORRT_CLASSIFIER__TENSORRT_CLASSIFIER_HPP_ #include #include @@ -25,7 +25,7 @@ #include #include -namespace tensorrt_classifier +namespace autoware::tensorrt_classifier { using cuda_utils::CudaUniquePtr; using cuda_utils::CudaUniquePtrHost; @@ -129,6 +129,6 @@ class TrtClassifier int batch_size_; CudaUniquePtrHost out_prob_h_; }; -} // namespace tensorrt_classifier +} // namespace autoware::tensorrt_classifier -#endif // TENSORRT_CLASSIFIER__TENSORRT_CLASSIFIER_HPP_ +#endif // AUTOWARE__TENSORRT_CLASSIFIER__TENSORRT_CLASSIFIER_HPP_ diff --git a/perception/tensorrt_classifier/package.xml b/perception/autoware_tensorrt_classifier/package.xml similarity index 91% rename from perception/tensorrt_classifier/package.xml rename to perception/autoware_tensorrt_classifier/package.xml index 439318c147e76..e41625886d8ae 100644 --- a/perception/tensorrt_classifier/package.xml +++ b/perception/autoware_tensorrt_classifier/package.xml @@ -1,11 +1,10 @@ - tensorrt_classifier + autoware_tensorrt_classifier 0.0.1 tensorrt classifier wrapper Dan Umeda - Mingyu Li Kotaro Uetake Shunsuke Miura diff --git a/perception/tensorrt_classifier/src/preprocess.cu b/perception/autoware_tensorrt_classifier/src/preprocess.cu similarity index 99% rename from perception/tensorrt_classifier/src/preprocess.cu rename to perception/autoware_tensorrt_classifier/src/preprocess.cu index fd3d758c2c280..07976281faafb 100644 --- a/perception/tensorrt_classifier/src/preprocess.cu +++ b/perception/autoware_tensorrt_classifier/src/preprocess.cu @@ -11,9 +11,9 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include -#include #include diff --git a/perception/tensorrt_classifier/src/tensorrt_classifier.cpp b/perception/autoware_tensorrt_classifier/src/tensorrt_classifier.cpp similarity index 94% rename from perception/tensorrt_classifier/src/tensorrt_classifier.cpp rename to perception/autoware_tensorrt_classifier/src/tensorrt_classifier.cpp index b7c4ce99fa2e9..a327e6395b2eb 100644 --- a/perception/tensorrt_classifier/src/tensorrt_classifier.cpp +++ b/perception/autoware_tensorrt_classifier/src/tensorrt_classifier.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include +#include +#include +#include #include -#include #include #include @@ -95,7 +95,7 @@ std::vector loadImageList(const std::string & filename, const std:: return fileList; } -namespace tensorrt_classifier +namespace autoware::tensorrt_classifier { TrtClassifier::TrtClassifier( const std::string & model_path, const std::string & precision, @@ -120,7 +120,8 @@ TrtClassifier::TrtClassifier( if (calibration_image_list_path != "") { calibration_images = loadImageList(calibration_image_list_path, ""); } - tensorrt_classifier::ImageStream stream(max_batch_size, input_dims, calibration_images); + autoware::tensorrt_classifier::ImageStream stream( + max_batch_size, input_dims, calibration_images); fs::path calibration_table{model_path}; std::string calibName = ""; std::string ext = ""; @@ -140,17 +141,17 @@ TrtClassifier::TrtClassifier( std::unique_ptr calibrator; if (build_config.calib_type_str == "Entropy") { - calibrator.reset( - new tensorrt_classifier::Int8EntropyCalibrator(stream, calibration_table, mean_, std_)); + calibrator.reset(new autoware::tensorrt_classifier::Int8EntropyCalibrator( + stream, calibration_table, mean_, std_)); } else if ( build_config.calib_type_str == "Legacy" || build_config.calib_type_str == "Percentile") { double quantile = 0.999999; double cutoff = 0.999999; - calibrator.reset(new tensorrt_classifier::Int8LegacyCalibrator( + calibrator.reset(new autoware::tensorrt_classifier::Int8LegacyCalibrator( stream, calibration_table, histogram_table, mean_, std_, true, quantile, cutoff)); } else { - calibrator.reset( - new tensorrt_classifier::Int8MinMaxCalibrator(stream, calibration_table, mean_, std_)); + calibrator.reset(new autoware::tensorrt_classifier::Int8MinMaxCalibrator( + stream, calibration_table, mean_, std_)); } trt_common_ = std::make_unique( model_path, precision, std::move(calibrator), batch_config, max_workspace_size, build_config); @@ -384,4 +385,4 @@ bool TrtClassifier::feedforwardAndDecode( } return true; } -} // namespace tensorrt_classifier +} // namespace autoware::tensorrt_classifier diff --git a/perception/detected_object_validation/src/lanelet_filter/lanelet_filter.cpp b/perception/detected_object_validation/src/lanelet_filter/lanelet_filter.cpp index 09440fedc1764..6437134bc98a5 100644 --- a/perception/detected_object_validation/src/lanelet_filter/lanelet_filter.cpp +++ b/perception/detected_object_validation/src/lanelet_filter/lanelet_filter.cpp @@ -23,6 +23,8 @@ #include #include +#include +#include #include namespace autoware::detected_object_validation @@ -76,9 +78,6 @@ void ObjectLaneletFilterNode::mapCallback( lanelet_frame_id_ = map_msg->header.frame_id; lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg(*map_msg, lanelet_map_ptr_); - const lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_); - road_lanelets_ = lanelet::utils::query::roadLanelets(all_lanelets); - shoulder_lanelets_ = lanelet::utils::query::shoulderLanelets(all_lanelets); } void ObjectLaneletFilterNode::objectCallback( @@ -103,19 +102,15 @@ void ObjectLaneletFilterNode::objectCallback( // calculate convex hull const auto convex_hull = getConvexHull(transformed_objects); + // get intersected lanelets - lanelet::ConstLanelets intersected_road_lanelets = - getIntersectedLanelets(convex_hull, road_lanelets_); - lanelet::ConstLanelets intersected_shoulder_lanelets = - getIntersectedLanelets(convex_hull, shoulder_lanelets_); + lanelet::ConstLanelets intersected_lanelets = getIntersectedLanelets(convex_hull); // filtering process for (size_t index = 0; index < transformed_objects.objects.size(); ++index) { const auto & transformed_object = transformed_objects.objects.at(index); const auto & input_object = input_msg->objects.at(index); - filterObject( - transformed_object, input_object, intersected_road_lanelets, intersected_shoulder_lanelets, - output_object_msg); + filterObject(transformed_object, input_object, intersected_lanelets, output_object_msg); } object_pub_->publish(output_object_msg); published_time_publisher_->publish_if_subscribed(object_pub_, output_object_msg.header.stamp); @@ -133,8 +128,7 @@ void ObjectLaneletFilterNode::objectCallback( bool ObjectLaneletFilterNode::filterObject( const autoware_perception_msgs::msg::DetectedObject & transformed_object, const autoware_perception_msgs::msg::DetectedObject & input_object, - const lanelet::ConstLanelets & intersected_road_lanelets, - const lanelet::ConstLanelets & intersected_shoulder_lanelets, + const lanelet::ConstLanelets & intersected_lanelets, autoware_perception_msgs::msg::DetectedObjects & output_object_msg) { const auto & label = transformed_object.classification.front().label; @@ -143,8 +137,7 @@ bool ObjectLaneletFilterNode::filterObject( // 1. is polygon overlap with road lanelets or shoulder lanelets if (filter_settings_.polygon_overlap_filter) { const bool is_polygon_overlap = - isObjectOverlapLanelets(transformed_object, intersected_road_lanelets) || - isObjectOverlapLanelets(transformed_object, intersected_shoulder_lanelets); + isObjectOverlapLanelets(transformed_object, intersected_lanelets); filter_pass = filter_pass && is_polygon_overlap; } @@ -154,8 +147,7 @@ bool ObjectLaneletFilterNode::filterObject( autoware_perception_msgs::msg::TrackedObjectKinematics::UNAVAILABLE; if (filter_settings_.lanelet_direction_filter && !orientation_not_available) { const bool is_same_direction = - isSameDirectionWithLanelets(intersected_road_lanelets, transformed_object) || - isSameDirectionWithLanelets(intersected_shoulder_lanelets, transformed_object); + isSameDirectionWithLanelets(intersected_lanelets, transformed_object); filter_pass = filter_pass && is_same_direction; } @@ -214,18 +206,40 @@ LinearRing2d ObjectLaneletFilterNode::getConvexHull( return convex_hull; } +// fetch the intersected candidate lanelets with bounding box and then +// check the intersections among the lanelets and the convex hull lanelet::ConstLanelets ObjectLaneletFilterNode::getIntersectedLanelets( - const LinearRing2d & convex_hull, const lanelet::ConstLanelets & road_lanelets) + const LinearRing2d & convex_hull) { + namespace bg = boost::geometry; + lanelet::ConstLanelets intersected_lanelets; - // WARNING: This implementation currently iterate all lanelets, which could degrade performance - // when handling large sized map. - for (const auto & road_lanelet : road_lanelets) { - if (boost::geometry::intersects(convex_hull, road_lanelet.polygon2d().basicPolygon())) { - intersected_lanelets.emplace_back(road_lanelet); + // convert convex_hull to a 2D bounding box for searching in the LaneletMap + bg::model::box> bbox2d_convex_hull; + bg::envelope(convex_hull, bbox2d_convex_hull); + lanelet::BoundingBox2d bbox2d( + lanelet::BasicPoint2d( + bg::get(bbox2d_convex_hull), + bg::get(bbox2d_convex_hull)), + lanelet::BasicPoint2d( + bg::get(bbox2d_convex_hull), + bg::get(bbox2d_convex_hull))); + + lanelet::Lanelets candidates_lanelets = lanelet_map_ptr_->laneletLayer.search(bbox2d); + for (const auto & lanelet : candidates_lanelets) { + // only check the road lanelets and road shoulder lanelets + if ( + lanelet.hasAttribute(lanelet::AttributeName::Subtype) && + (lanelet.attribute(lanelet::AttributeName::Subtype).value() == + lanelet::AttributeValueString::Road || + lanelet.attribute(lanelet::AttributeName::Subtype).value() == "road_shoulder")) { + if (boost::geometry::intersects(convex_hull, lanelet.polygon2d().basicPolygon())) { + intersected_lanelets.emplace_back(lanelet); + } } } + return intersected_lanelets; } diff --git a/perception/detected_object_validation/src/lanelet_filter/lanelet_filter.hpp b/perception/detected_object_validation/src/lanelet_filter/lanelet_filter.hpp index 25d78a160c246..3f7a53d0319be 100644 --- a/perception/detected_object_validation/src/lanelet_filter/lanelet_filter.hpp +++ b/perception/detected_object_validation/src/lanelet_filter/lanelet_filter.hpp @@ -57,8 +57,6 @@ class ObjectLaneletFilterNode : public rclcpp::Node std::unique_ptr debug_publisher_{nullptr}; lanelet::LaneletMapPtr lanelet_map_ptr_; - lanelet::ConstLanelets road_lanelets_; - lanelet::ConstLanelets shoulder_lanelets_; std::string lanelet_frame_id_; tf2_ros::Buffer tf_buffer_; @@ -76,12 +74,10 @@ class ObjectLaneletFilterNode : public rclcpp::Node bool filterObject( const autoware_perception_msgs::msg::DetectedObject & transformed_object, const autoware_perception_msgs::msg::DetectedObject & input_object, - const lanelet::ConstLanelets & intersected_road_lanelets, - const lanelet::ConstLanelets & intersected_shoulder_lanelets, + const lanelet::ConstLanelets & intersected_lanelets, autoware_perception_msgs::msg::DetectedObjects & output_object_msg); LinearRing2d getConvexHull(const autoware_perception_msgs::msg::DetectedObjects &); - lanelet::ConstLanelets getIntersectedLanelets( - const LinearRing2d &, const lanelet::ConstLanelets &); + lanelet::ConstLanelets getIntersectedLanelets(const LinearRing2d &); bool isObjectOverlapLanelets( const autoware_perception_msgs::msg::DetectedObject & object, const lanelet::ConstLanelets & intersected_lanelets); diff --git a/perception/image_projection_based_fusion/package.xml b/perception/image_projection_based_fusion/package.xml index f7a95ae4c70d6..924e4c2b446b6 100644 --- a/perception/image_projection_based_fusion/package.xml +++ b/perception/image_projection_based_fusion/package.xml @@ -16,11 +16,11 @@ ament_cmake_auto autoware_cmake + autoware_euclidean_cluster autoware_perception_msgs autoware_point_types autoware_universe_utils cv_bridge - euclidean_cluster image_geometry image_transport lidar_centerpoint diff --git a/perception/traffic_light_classifier/CMakeLists.txt b/perception/traffic_light_classifier/CMakeLists.txt index a81dc6ab1e6ce..c6c33da839c49 100644 --- a/perception/traffic_light_classifier/CMakeLists.txt +++ b/perception/traffic_light_classifier/CMakeLists.txt @@ -79,9 +79,9 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ) ament_auto_add_library(traffic_light_classifier_nodelet SHARED - src/color_classifier.cpp - src/cnn_classifier.cpp - src/nodelet.cpp + src/classifier/color_classifier.cpp + src/classifier/cnn_classifier.cpp + src/traffic_light_classifier_node.cpp ) target_link_libraries(traffic_light_classifier_nodelet ${OpenCV_LIBRARIES} @@ -94,14 +94,14 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) stdc++fs ) rclcpp_components_register_node(traffic_light_classifier_nodelet - PLUGIN "traffic_light::TrafficLightClassifierNodelet" + PLUGIN "autoware::traffic_light::TrafficLightClassifierNodelet" EXECUTABLE traffic_light_classifier_node ) ament_auto_add_library(single_image_debug_inference_node SHARED - src/cnn_classifier.cpp - src/color_classifier.cpp - src/nodelet.cpp + src/classifier/cnn_classifier.cpp + src/classifier/color_classifier.cpp + src/traffic_light_classifier_node.cpp src/single_image_debug_inference_node.cpp ) target_link_libraries(single_image_debug_inference_node @@ -117,7 +117,7 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) opencv_highgui ) rclcpp_components_register_node(single_image_debug_inference_node - PLUGIN "traffic_light::SingleImageDebugInferenceNode" + PLUGIN "autoware::traffic_light::SingleImageDebugInferenceNode" EXECUTABLE single_image_debug_inference ) @@ -133,16 +133,16 @@ else() ${OpenCV_INCLUDE_DIRS} ) - ament_auto_add_library(traffic_light_classifier_nodelet SHARED - src/color_classifier.cpp - src/nodelet.cpp + ament_auto_add_library(${PROJECT_NAME} SHARED + src/classifier/color_classifier.cpp + src/traffic_light_classifier_node.cpp ) - target_link_libraries(traffic_light_classifier_nodelet + target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES} ) - rclcpp_components_register_node(traffic_light_classifier_nodelet - PLUGIN "traffic_light::TrafficLightClassifierNodelet" + rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::traffic_light::TrafficLightClassifierNodelet" EXECUTABLE traffic_light_classifier_node ) diff --git a/perception/traffic_light_classifier/package.xml b/perception/traffic_light_classifier/package.xml index 9e460e8ad1a1d..aa48eff993132 100644 --- a/perception/traffic_light_classifier/package.xml +++ b/perception/traffic_light_classifier/package.xml @@ -14,6 +14,7 @@ autoware_cmake + autoware_tensorrt_classifier cuda_utils cv_bridge image_transport @@ -22,7 +23,6 @@ rclcpp rclcpp_components sensor_msgs - tensorrt_classifier tensorrt_common tier4_perception_msgs diff --git a/perception/traffic_light_classifier/include/traffic_light_classifier/classifier_interface.hpp b/perception/traffic_light_classifier/src/classifier/classifier_interface.hpp similarity index 79% rename from perception/traffic_light_classifier/include/traffic_light_classifier/classifier_interface.hpp rename to perception/traffic_light_classifier/src/classifier/classifier_interface.hpp index 563850da9d2f4..cff5828a5b00f 100644 --- a/perception/traffic_light_classifier/include/traffic_light_classifier/classifier_interface.hpp +++ b/perception/traffic_light_classifier/src/classifier/classifier_interface.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAFFIC_LIGHT_CLASSIFIER__CLASSIFIER_INTERFACE_HPP_ -#define TRAFFIC_LIGHT_CLASSIFIER__CLASSIFIER_INTERFACE_HPP_ +#ifndef CLASSIFIER__CLASSIFIER_INTERFACE_HPP_ +#define CLASSIFIER__CLASSIFIER_INTERFACE_HPP_ #include #include @@ -22,7 +22,7 @@ #include -namespace traffic_light +namespace autoware::traffic_light { class ClassifierInterface { @@ -31,6 +31,6 @@ class ClassifierInterface const std::vector & input_image, tier4_perception_msgs::msg::TrafficLightArray & traffic_signals) = 0; }; -} // namespace traffic_light +} // namespace autoware::traffic_light -#endif // TRAFFIC_LIGHT_CLASSIFIER__CLASSIFIER_INTERFACE_HPP_ +#endif // CLASSIFIER__CLASSIFIER_INTERFACE_HPP_ diff --git a/perception/traffic_light_classifier/src/cnn_classifier.cpp b/perception/traffic_light_classifier/src/classifier/cnn_classifier.cpp similarity index 97% rename from perception/traffic_light_classifier/src/cnn_classifier.cpp rename to perception/traffic_light_classifier/src/classifier/cnn_classifier.cpp index d6ba81041c6f8..6e3ae34297510 100644 --- a/perception/traffic_light_classifier/src/cnn_classifier.cpp +++ b/perception/traffic_light_classifier/src/classifier/cnn_classifier.cpp @@ -12,18 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "traffic_light_classifier/cnn_classifier.hpp" +#include "cnn_classifier.hpp" #include #include #include +#include #include #include #include -namespace traffic_light +namespace autoware::traffic_light { CNNClassifier::CNNClassifier(rclcpp::Node * node_ptr) : node_ptr_(node_ptr) { @@ -55,7 +56,7 @@ CNNClassifier::CNNClassifier(rclcpp::Node * node_ptr) : node_ptr_(node_ptr) batch_size_ = input_dim.d[0]; tensorrt_common::BatchConfig batch_config{batch_size_, batch_size_, batch_size_}; - classifier_ = std::make_unique( + classifier_ = std::make_unique( model_file_path, precision, batch_config, mean_, std_); if (node_ptr_->declare_parameter("build_only", false)) { RCLCPP_INFO(node_ptr_->get_logger(), "TensorRT engine is built and shutdown node."); @@ -207,4 +208,4 @@ bool CNNClassifier::isColorLabel(const std::string label) return false; } -} // namespace traffic_light +} // namespace autoware::traffic_light diff --git a/perception/traffic_light_classifier/include/traffic_light_classifier/cnn_classifier.hpp b/perception/traffic_light_classifier/src/classifier/cnn_classifier.hpp similarity index 92% rename from perception/traffic_light_classifier/include/traffic_light_classifier/cnn_classifier.hpp rename to perception/traffic_light_classifier/src/classifier/cnn_classifier.hpp index 545a080763ef4..348c9cb81728f 100644 --- a/perception/traffic_light_classifier/include/traffic_light_classifier/cnn_classifier.hpp +++ b/perception/traffic_light_classifier/src/classifier/cnn_classifier.hpp @@ -12,11 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAFFIC_LIGHT_CLASSIFIER__CNN_CLASSIFIER_HPP_ -#define TRAFFIC_LIGHT_CLASSIFIER__CNN_CLASSIFIER_HPP_ +#ifndef CLASSIFIER__CNN_CLASSIFIER_HPP_ +#define CLASSIFIER__CNN_CLASSIFIER_HPP_ -#include "traffic_light_classifier/classifier_interface.hpp" +#include "classifier_interface.hpp" +#include #include #include #include @@ -24,7 +25,6 @@ #include #include #include -#include #include #include @@ -41,7 +41,7 @@ #include #include -namespace traffic_light +namespace autoware::traffic_light { using cuda_utils::CudaUniquePtr; @@ -111,13 +111,13 @@ class CNNClassifier : public ClassifierInterface rclcpp::Node * node_ptr_; int batch_size_; - std::unique_ptr classifier_; + std::unique_ptr classifier_; image_transport::Publisher image_pub_; std::vector labels_; std::vector mean_; std::vector std_; }; -} // namespace traffic_light +} // namespace autoware::traffic_light -#endif // TRAFFIC_LIGHT_CLASSIFIER__CNN_CLASSIFIER_HPP_ +#endif // CLASSIFIER__CNN_CLASSIFIER_HPP_ diff --git a/perception/traffic_light_classifier/src/color_classifier.cpp b/perception/traffic_light_classifier/src/classifier/color_classifier.cpp similarity index 99% rename from perception/traffic_light_classifier/src/color_classifier.cpp rename to perception/traffic_light_classifier/src/classifier/color_classifier.cpp index 604568b585a63..6f9800683aa27 100644 --- a/perception/traffic_light_classifier/src/color_classifier.cpp +++ b/perception/traffic_light_classifier/src/classifier/color_classifier.cpp @@ -11,7 +11,7 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#include "traffic_light_classifier/color_classifier.hpp" +#include "color_classifier.hpp" #include @@ -19,7 +19,7 @@ #include #include -namespace traffic_light +namespace autoware::traffic_light { ColorClassifier::ColorClassifier(rclcpp::Node * node_ptr) : node_ptr_(node_ptr) { @@ -255,4 +255,4 @@ rcl_interfaces::msg::SetParametersResult ColorClassifier::parametersCallback( return result; } -} // namespace traffic_light +} // namespace autoware::traffic_light diff --git a/perception/traffic_light_classifier/include/traffic_light_classifier/color_classifier.hpp b/perception/traffic_light_classifier/src/classifier/color_classifier.hpp similarity index 88% rename from perception/traffic_light_classifier/include/traffic_light_classifier/color_classifier.hpp rename to perception/traffic_light_classifier/src/classifier/color_classifier.hpp index 38f2d7f6a48db..6e11a6f6f3d84 100644 --- a/perception/traffic_light_classifier/include/traffic_light_classifier/color_classifier.hpp +++ b/perception/traffic_light_classifier/src/classifier/color_classifier.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAFFIC_LIGHT_CLASSIFIER__COLOR_CLASSIFIER_HPP_ -#define TRAFFIC_LIGHT_CLASSIFIER__COLOR_CLASSIFIER_HPP_ +#ifndef CLASSIFIER__COLOR_CLASSIFIER_HPP_ +#define CLASSIFIER__COLOR_CLASSIFIER_HPP_ -#include "traffic_light_classifier/classifier_interface.hpp" +#include "classifier_interface.hpp" #include #include @@ -32,7 +32,7 @@ #include -namespace traffic_light +namespace autoware::traffic_light { struct HSVConfig { @@ -93,6 +93,6 @@ class ColorClassifier : public ClassifierInterface cv::Scalar max_hsv_red_; }; -} // namespace traffic_light +} // namespace autoware::traffic_light -#endif // TRAFFIC_LIGHT_CLASSIFIER__COLOR_CLASSIFIER_HPP_ +#endif // CLASSIFIER__COLOR_CLASSIFIER_HPP_ diff --git a/perception/traffic_light_classifier/src/single_image_debug_inference_node.cpp b/perception/traffic_light_classifier/src/single_image_debug_inference_node.cpp index f6f963086bdd7..245dc069af163 100644 --- a/perception/traffic_light_classifier/src/single_image_debug_inference_node.cpp +++ b/perception/traffic_light_classifier/src/single_image_debug_inference_node.cpp @@ -15,11 +15,11 @@ #include #if ENABLE_GPU -#include +#include "classifier/cnn_classifier.hpp" #endif -#include -#include +#include "classifier/color_classifier.hpp" +#include "traffic_light_classifier_node.hpp" #include #include @@ -60,7 +60,7 @@ std::string toString(const uint8_t state) } } // namespace -namespace traffic_light +namespace autoware::traffic_light { class SingleImageDebugInferenceNode : public rclcpp::Node { @@ -159,7 +159,7 @@ class SingleImageDebugInferenceNode : public rclcpp::Node cv::Mat image_; std::unique_ptr classifier_ptr_; }; -} // namespace traffic_light +} // namespace autoware::traffic_light #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(traffic_light::SingleImageDebugInferenceNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::traffic_light::SingleImageDebugInferenceNode) diff --git a/perception/traffic_light_classifier/src/nodelet.cpp b/perception/traffic_light_classifier/src/traffic_light_classifier_node.cpp similarity index 96% rename from perception/traffic_light_classifier/src/nodelet.cpp rename to perception/traffic_light_classifier/src/traffic_light_classifier_node.cpp index 6378a98fdc8cc..b42ded4cbffa3 100644 --- a/perception/traffic_light_classifier/src/nodelet.cpp +++ b/perception/traffic_light_classifier/src/traffic_light_classifier_node.cpp @@ -11,7 +11,7 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#include "traffic_light_classifier/nodelet.hpp" +#include "traffic_light_classifier_node.hpp" #include @@ -20,7 +20,7 @@ #include #include -namespace traffic_light +namespace autoware::traffic_light { TrafficLightClassifierNodelet::TrafficLightClassifierNodelet(const rclcpp::NodeOptions & options) : Node("traffic_light_classifier_node", options) @@ -169,8 +169,8 @@ bool TrafficLightClassifierNodelet::is_harsh_backlight(const cv::Mat & img) cons return backlight_threshold_ <= intensity; } -} // namespace traffic_light +} // namespace autoware::traffic_light #include -RCLCPP_COMPONENTS_REGISTER_NODE(traffic_light::TrafficLightClassifierNodelet) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::traffic_light::TrafficLightClassifierNodelet) diff --git a/perception/traffic_light_classifier/include/traffic_light_classifier/nodelet.hpp b/perception/traffic_light_classifier/src/traffic_light_classifier_node.hpp similarity index 88% rename from perception/traffic_light_classifier/include/traffic_light_classifier/nodelet.hpp rename to perception/traffic_light_classifier/src/traffic_light_classifier_node.hpp index d48dd8aba3b95..d86880744929f 100644 --- a/perception/traffic_light_classifier/include/traffic_light_classifier/nodelet.hpp +++ b/perception/traffic_light_classifier/src/traffic_light_classifier_node.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAFFIC_LIGHT_CLASSIFIER__NODELET_HPP_ -#define TRAFFIC_LIGHT_CLASSIFIER__NODELET_HPP_ +#ifndef TRAFFIC_LIGHT_CLASSIFIER_NODE_HPP_ +#define TRAFFIC_LIGHT_CLASSIFIER_NODE_HPP_ -#include "traffic_light_classifier/classifier_interface.hpp" +#include "classifier/classifier_interface.hpp" #include #include @@ -43,15 +43,15 @@ #include #if ENABLE_GPU -#include "traffic_light_classifier/cnn_classifier.hpp" +#include "classifier/cnn_classifier.hpp" #endif -#include "traffic_light_classifier/color_classifier.hpp" +#include "classifier/color_classifier.hpp" #include #include -namespace traffic_light +namespace autoware::traffic_light { class TrafficLightClassifierNodelet : public rclcpp::Node { @@ -93,6 +93,6 @@ class TrafficLightClassifierNodelet : public rclcpp::Node bool is_harsh_backlight(const cv::Mat & img) const; }; -} // namespace traffic_light +} // namespace autoware::traffic_light -#endif // TRAFFIC_LIGHT_CLASSIFIER__NODELET_HPP_ +#endif // TRAFFIC_LIGHT_CLASSIFIER_NODE_HPP_ diff --git a/perception/traffic_light_map_based_detector/CMakeLists.txt b/perception/traffic_light_map_based_detector/CMakeLists.txt index 75a4bf9064782..d3c1f1548937c 100644 --- a/perception/traffic_light_map_based_detector/CMakeLists.txt +++ b/perception/traffic_light_map_based_detector/CMakeLists.txt @@ -11,12 +11,12 @@ include_directories( ${EIGEN3_INCLUDE_DIR} ) -ament_auto_add_library(traffic_light_map_based_detector SHARED - src/node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED + src/traffic_light_map_based_detector_node.cpp ) -rclcpp_components_register_node(traffic_light_map_based_detector - PLUGIN "traffic_light::MapBasedDetector" +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::traffic_light::MapBasedDetector" EXECUTABLE traffic_light_map_based_detector_node ) diff --git a/perception/traffic_light_map_based_detector/package.xml b/perception/traffic_light_map_based_detector/package.xml index 7553b136d791a..c25773acd8914 100644 --- a/perception/traffic_light_map_based_detector/package.xml +++ b/perception/traffic_light_map_based_detector/package.xml @@ -15,7 +15,6 @@ autoware_lanelet2_extension autoware_map_msgs - autoware_planning_msgs autoware_universe_utils geometry_msgs image_geometry diff --git a/perception/traffic_light_map_based_detector/src/node.cpp b/perception/traffic_light_map_based_detector/src/traffic_light_map_based_detector_node.cpp similarity index 99% rename from perception/traffic_light_map_based_detector/src/node.cpp rename to perception/traffic_light_map_based_detector/src/traffic_light_map_based_detector_node.cpp index c1fbd4a4ad4f2..d133fe0b443d1 100644 --- a/perception/traffic_light_map_based_detector/src/node.cpp +++ b/perception/traffic_light_map_based_detector/src/traffic_light_map_based_detector_node.cpp @@ -12,8 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "traffic_light_map_based_detector/node.hpp" +#define EIGEN_MPL2_ONLY + +#include "traffic_light_map_based_detector_node.hpp" +#include +#include #include #include #include @@ -27,10 +31,6 @@ #include #include -#define EIGEN_MPL2_ONLY -#include -#include - #ifdef ROS_DISTRO_GALACTIC #include #else @@ -118,7 +118,7 @@ tf2::Vector3 getTrafficLightCenter(const lanelet::ConstLineString3d & traffic_li } // namespace -namespace traffic_light +namespace autoware::traffic_light { MapBasedDetector::MapBasedDetector(const rclcpp::NodeOptions & node_options) : Node("traffic_light_map_based_detector", node_options), @@ -605,7 +605,7 @@ void MapBasedDetector::publishVisibleTrafficLights( } pub->publish(output_msg); } -} // namespace traffic_light +} // namespace autoware::traffic_light #include -RCLCPP_COMPONENTS_REGISTER_NODE(traffic_light::MapBasedDetector) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::traffic_light::MapBasedDetector) diff --git a/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp b/perception/traffic_light_map_based_detector/src/traffic_light_map_based_detector_node.hpp similarity index 97% rename from perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp rename to perception/traffic_light_map_based_detector/src/traffic_light_map_based_detector_node.hpp index 3aeba4c4a913a..6a2d00874fda0 100644 --- a/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp +++ b/perception/traffic_light_map_based_detector/src/traffic_light_map_based_detector_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAFFIC_LIGHT_MAP_BASED_DETECTOR__NODE_HPP_ -#define TRAFFIC_LIGHT_MAP_BASED_DETECTOR__NODE_HPP_ +#ifndef TRAFFIC_LIGHT_MAP_BASED_DETECTOR_NODE_HPP_ +#define TRAFFIC_LIGHT_MAP_BASED_DETECTOR_NODE_HPP_ #include #include @@ -44,7 +44,7 @@ #include #include -namespace traffic_light +namespace autoware::traffic_light { class MapBasedDetector : public rclcpp::Node { @@ -196,5 +196,5 @@ class MapBasedDetector : public rclcpp::Node const std::vector & visible_traffic_lights, const rclcpp::Publisher::SharedPtr pub); }; -} // namespace traffic_light -#endif // TRAFFIC_LIGHT_MAP_BASED_DETECTOR__NODE_HPP_ +} // namespace autoware::traffic_light +#endif // TRAFFIC_LIGHT_MAP_BASED_DETECTOR_NODE_HPP_ diff --git a/perception/traffic_light_multi_camera_fusion/CMakeLists.txt b/perception/traffic_light_multi_camera_fusion/CMakeLists.txt index 5765ebf58fd45..a99a37aebdedc 100644 --- a/perception/traffic_light_multi_camera_fusion/CMakeLists.txt +++ b/perception/traffic_light_multi_camera_fusion/CMakeLists.txt @@ -8,12 +8,12 @@ include_directories( SYSTEM ) -ament_auto_add_library(traffic_light_multi_camera_fusion SHARED - src/node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED + src/traffic_light_multi_camera_fusion_node.cpp ) -rclcpp_components_register_node(traffic_light_multi_camera_fusion - PLUGIN "traffic_light::MultiCameraFusion" +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::traffic_light::MultiCameraFusion" EXECUTABLE traffic_light_multi_camera_fusion_node ) diff --git a/perception/traffic_light_multi_camera_fusion/src/node.cpp b/perception/traffic_light_multi_camera_fusion/src/traffic_light_multi_camera_fusion_node.cpp similarity index 96% rename from perception/traffic_light_multi_camera_fusion/src/node.cpp rename to perception/traffic_light_multi_camera_fusion/src/traffic_light_multi_camera_fusion_node.cpp index 0e10fec3dbb87..70841b936af37 100644 --- a/perception/traffic_light_multi_camera_fusion/src/node.cpp +++ b/perception/traffic_light_multi_camera_fusion/src/traffic_light_multi_camera_fusion_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "traffic_light_multi_camera_fusion/node.hpp" +#include "traffic_light_multi_camera_fusion_node.hpp" #include #include @@ -20,6 +20,7 @@ #include #include #include +#include #include namespace @@ -39,7 +40,7 @@ bool isUnknown(const tier4_perception_msgs::msg::TrafficLight & signal) * @param record fusion record * @return 0 if traffic light is truncated, otherwise 1 */ -int calVisibleScore(const traffic_light::FusionRecord & record) +int calVisibleScore(const autoware::traffic_light::FusionRecord & record) { const uint32_t boundary = 5; uint32_t x1 = record.roi.roi.x_offset; @@ -55,7 +56,9 @@ int calVisibleScore(const traffic_light::FusionRecord & record) } } -int compareRecord(const traffic_light::FusionRecord & r1, const traffic_light::FusionRecord & r2) +int compareRecord( + const autoware::traffic_light::FusionRecord & r1, + const autoware::traffic_light::FusionRecord & r2) { /* if both records are from the same sensor but different stamp, trust the latest one @@ -133,7 +136,7 @@ autoware_perception_msgs::msg::TrafficLightElement convert( } // namespace -namespace traffic_light +namespace autoware::traffic_light { MultiCameraFusion::MultiCameraFusion(const rclcpp::NodeOptions & node_options) @@ -319,7 +322,7 @@ void MultiCameraFusion::groupFusion( } } -} // namespace traffic_light +} // namespace autoware::traffic_light #include -RCLCPP_COMPONENTS_REGISTER_NODE(traffic_light::MultiCameraFusion) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::traffic_light::MultiCameraFusion) diff --git a/perception/traffic_light_multi_camera_fusion/include/traffic_light_multi_camera_fusion/node.hpp b/perception/traffic_light_multi_camera_fusion/src/traffic_light_multi_camera_fusion_node.hpp similarity index 95% rename from perception/traffic_light_multi_camera_fusion/include/traffic_light_multi_camera_fusion/node.hpp rename to perception/traffic_light_multi_camera_fusion/src/traffic_light_multi_camera_fusion_node.hpp index 4389b5d3c1aa1..23bc59c26b293 100644 --- a/perception/traffic_light_multi_camera_fusion/include/traffic_light_multi_camera_fusion/node.hpp +++ b/perception/traffic_light_multi_camera_fusion/src/traffic_light_multi_camera_fusion_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAFFIC_LIGHT_MULTI_CAMERA_FUSION__NODE_HPP_ -#define TRAFFIC_LIGHT_MULTI_CAMERA_FUSION__NODE_HPP_ +#ifndef TRAFFIC_LIGHT_MULTI_CAMERA_FUSION_NODE_HPP_ +#define TRAFFIC_LIGHT_MULTI_CAMERA_FUSION_NODE_HPP_ #include @@ -36,7 +36,7 @@ #include #include -namespace traffic_light +namespace autoware::traffic_light { namespace mf = message_filters; @@ -125,5 +125,5 @@ class MultiCameraFusion : public rclcpp::Node */ double message_lifespan_; }; -} // namespace traffic_light -#endif // TRAFFIC_LIGHT_MULTI_CAMERA_FUSION__NODE_HPP_ +} // namespace autoware::traffic_light +#endif // TRAFFIC_LIGHT_MULTI_CAMERA_FUSION_NODE_HPP_ diff --git a/planning/autoware_rtc_interface/src/rtc_interface.cpp b/planning/autoware_rtc_interface/src/rtc_interface.cpp index 7dcdc4d5984a6..1352ec10557f9 100644 --- a/planning/autoware_rtc_interface/src/rtc_interface.cpp +++ b/planning/autoware_rtc_interface/src/rtc_interface.cpp @@ -364,7 +364,8 @@ bool RTCInterface::isForceActivated(const UUID & uuid) const } RCLCPP_WARN_STREAM( - getLogger(), "[isForceActivated] uuid : " << to_string(uuid) << " is not found" << std::endl); + getLogger(), + "[isForceActivated] uuid : " << uuid_to_string(uuid) << " is not found" << std::endl); return false; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp index c4a05d171654d..09e1382799f01 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -340,7 +340,7 @@ class SceneModuleInterface } if (rtc.second->isTerminated(uuid_map_.at(rtc.first))) { - return true; + return false; } return rtc.second->isActivated(uuid_map_.at(rtc.first)); diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp index 6eea3e3576732..ebdb0869545ca 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp @@ -90,6 +90,9 @@ struct StartPlannerDebugData std::vector start_pose_candidates; size_t selected_start_pose_candidate_index; double margin_for_start_pose_candidate; + + // for isPreventingRearVehicleFromPassingThrough + std::optional estimated_stop_pose; }; struct StartPlannerParameters diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp index b07d6497463ef..bb70ae2638056 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp @@ -229,8 +229,31 @@ class StartPlannerModule : public SceneModuleInterface bool isModuleRunning() const; bool isCurrentPoseOnMiddleOfTheRoad() const; + + /** + * @brief Check if the ego vehicle is preventing the rear vehicle from passing through. + * + * This function just call isPreventingRearVehicleFromPassingThrough(const Pose & ego_pose) with + * two poses. If rear vehicle is obstructed by ego vehicle at either of the two poses, it returns + * true. + * + * @return true if the ego vehicle is preventing the rear vehicle from passing through with the + * current pose or the pose if it stops. + */ bool isPreventingRearVehicleFromPassingThrough() const; + /** + * @brief Check if the ego vehicle is preventing the rear vehicle from passing through. + * + * This function measures the distance to the lane boundary from the current pose and the pose if +it stops, and determines whether there is enough space for the rear vehicle to pass through. If + * it is obstructing at either of the two poses, it returns true. + * + * @return true if the ego vehicle is preventing the rear vehicle from passing through with given +ego pose. + */ + bool isPreventingRearVehicleFromPassingThrough(const Pose & ego_pose) const; + bool isCloseToOriginalStartPose() const; bool hasArrivedAtGoal() const; bool isMoving() const; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index 9b94d3505c405..909a9574faa43 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -368,7 +368,35 @@ bool StartPlannerModule::isCurrentPoseOnMiddleOfTheRoad() const bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const { + // prepare poses for preventing check + // - current_pose + // - estimated_stop_pose (The position assumed when stopped with the minimum stop distance, + // although it is NOT actually stopped) const auto & current_pose = planner_data_->self_odometry->pose.pose; + std::vector preventing_check_pose{current_pose}; + const auto min_stop_distance = + autoware::behavior_path_planner::utils::parking_departure::calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration_for_stop, parameters_->maximum_jerk_for_stop, + 0.0); + debug_data_.estimated_stop_pose.reset(); // debug + if (min_stop_distance.has_value()) { + const auto current_path = getCurrentPath(); + const auto estimated_stop_pose = calcLongitudinalOffsetPose( + current_path.points, current_pose.position, min_stop_distance.value()); + if (estimated_stop_pose.has_value()) { + preventing_check_pose.push_back(estimated_stop_pose.value()); + debug_data_.estimated_stop_pose = estimated_stop_pose.value(); // debug + } + } + + // check if any of the preventing check poses are preventing rear vehicle from passing through + return std::any_of( + preventing_check_pose.begin(), preventing_check_pose.end(), + [&](const auto & pose) { return isPreventingRearVehicleFromPassingThrough(pose); }); +} + +bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough(const Pose & ego_pose) const +{ const auto & dynamic_object = planner_data_->dynamic_object; const auto & route_handler = planner_data_->route_handler; const Pose start_pose = planner_data_->route_handler->getOriginalStartPose(); @@ -414,8 +442,8 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const geometry_msgs::msg::Pose & ego_overhang_point_as_pose, const bool ego_is_merging_from_the_left) -> std::optional> { const auto local_vehicle_footprint = vehicle_info_.createFootprint(); - const auto vehicle_footprint = transformVector( - local_vehicle_footprint, autoware::universe_utils::pose2transform(current_pose)); + const auto vehicle_footprint = + transformVector(local_vehicle_footprint, autoware::universe_utils::pose2transform(ego_pose)); double smallest_lateral_gap_between_ego_and_border = std::numeric_limits::max(); double corresponding_lateral_gap_with_other_lane_bound = std::numeric_limits::max(); @@ -481,7 +509,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const // Check backwards just in case the Vehicle behind ego is in a different lanelet constexpr double backwards_length = 200.0; const auto prev_lanes = autoware::behavior_path_planner::utils::getBackwardLanelets( - *route_handler, target_lanes, current_pose, backwards_length); + *route_handler, target_lanes, ego_pose, backwards_length); // return all the relevant lanelets lanelet::ConstLanelets relevant_lanelets{closest_lanelet_const}; relevant_lanelets.insert(relevant_lanelets.end(), prev_lanes.begin(), prev_lanes.end()); @@ -491,7 +519,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const // filtering objects with velocity, position and class const auto filtered_objects = utils::path_safety_checker::filterObjects( - dynamic_object, route_handler, relevant_lanelets.value(), current_pose.position, + dynamic_object, route_handler, relevant_lanelets.value(), ego_pose.position, objects_filtering_params_); if (filtered_objects.objects.empty()) return false; @@ -508,7 +536,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const target_objects_on_lane.on_current_lane.begin(), target_objects_on_lane.on_current_lane.end(), [&](const auto & o) { const auto arc_length = autoware::motion_utils::calcSignedArcLength( - centerline_path.points, current_pose.position, o.initial_pose.pose.position); + centerline_path.points, ego_pose.position, o.initial_pose.pose.position); if (arc_length > 0.0) return; if (std::abs(arc_length) >= std::abs(arc_length_to_closet_object)) return; arc_length_to_closet_object = arc_length; @@ -1719,6 +1747,16 @@ void StartPlannerModule::setDebugData() info_marker_); add(showPolygon(debug_data_.collision_check, "ego_and_target_polygon_relation"), info_marker_); + // visualize estimated_stop_pose for isPreventingRearVehicleFromPassingThrough() + if (debug_data_.estimated_stop_pose.has_value()) { + auto footprint_marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "estimated_stop_pose", 0, Marker::LINE_STRIP, + createMarkerScale(0.2, 0.2, 0.2), purple_color); + footprint_marker.lifetime = rclcpp::Duration::from_seconds(1.5); + addFootprintMarker(footprint_marker, debug_data_.estimated_stop_pose.value(), vehicle_info_); + debug_marker_.markers.push_back(footprint_marker); + } + // set objects of interest for (const auto & [uuid, data] : debug_data_.collision_check) { const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED; diff --git a/system/diagnostic_graph_aggregator/src/common/graph/config.cpp b/system/diagnostic_graph_aggregator/src/common/graph/config.cpp index 496c95722002a..6dab6f7d2f511 100644 --- a/system/diagnostic_graph_aggregator/src/common/graph/config.cpp +++ b/system/diagnostic_graph_aggregator/src/common/graph/config.cpp @@ -264,9 +264,9 @@ void topological_sort(FileConfig & config) for (const auto & unit : config.units) units.push_back(unit.get()); // Count degrees of each unit. - for (const auto & unit : units) { - if (const auto & link = unit->item) ++degrees[link->child]; - for (const auto & link : unit->list) ++degrees[link->child]; + for (const auto * const unit : units) { + if (const auto * const link = unit->item) ++degrees[link->child]; + for (const auto * const link : unit->list) ++degrees[link->child]; } // Find initial units that are zero degrees. diff --git a/system/diagnostic_graph_aggregator/src/common/graph/loader.cpp b/system/diagnostic_graph_aggregator/src/common/graph/loader.cpp index 74636adab7415..e4d2e470a0d32 100644 --- a/system/diagnostic_graph_aggregator/src/common/graph/loader.cpp +++ b/system/diagnostic_graph_aggregator/src/common/graph/loader.cpp @@ -59,7 +59,7 @@ UnitLink * UnitLoader::child() const std::vector UnitLoader::children() const { std::vector result; - for (const auto & config : config_->list) { + for (auto * const config : config_->list) { result.push_back(links_.config_links.at(config)); } return result; diff --git a/system/diagnostic_graph_aggregator/src/common/graph/units.cpp b/system/diagnostic_graph_aggregator/src/common/graph/units.cpp index d9cbdbaa64400..5ebd603964add 100644 --- a/system/diagnostic_graph_aggregator/src/common/graph/units.cpp +++ b/system/diagnostic_graph_aggregator/src/common/graph/units.cpp @@ -145,7 +145,7 @@ MaxUnit::MaxUnit(const UnitLoader & unit) : NodeUnit(unit) void MaxUnit::update_status() { DiagnosticLevel level = DiagnosticStatus::OK; - for (const auto & link : links_) { + for (const auto * const link : links_) { level = std::max(level, link->child()->level()); } status_.level = std::min(level, DiagnosticStatus::ERROR); @@ -155,7 +155,7 @@ void ShortCircuitMaxUnit::update_status() { // TODO(Takagi, Isamu): update link flags. DiagnosticLevel level = DiagnosticStatus::OK; - for (const auto & link : links_) { + for (const auto * const link : links_) { level = std::max(level, link->child()->level()); } status_.level = std::min(level, DiagnosticStatus::ERROR); @@ -171,7 +171,7 @@ void MinUnit::update_status() DiagnosticLevel level = DiagnosticStatus::OK; if (!links_.empty()) { level = DiagnosticStatus::STALE; - for (const auto & link : links_) { + for (const auto * const link : links_) { level = std::min(level, link->child()->level()); } } diff --git a/tools/reaction_analyzer/src/subscriber.cpp b/tools/reaction_analyzer/src/subscriber.cpp index 02d7a3872953a..a5d1be613ee5a 100644 --- a/tools/reaction_analyzer/src/subscriber.cpp +++ b/tools/reaction_analyzer/src/subscriber.cpp @@ -272,6 +272,7 @@ void SubscriberBase::on_trajectory( if (zero_vel_idx) { RCLCPP_INFO(node_->get_logger(), "%s reacted without published time", node_name.c_str()); // set header time + // cppcheck-suppress constVariableReference auto & buffer = std::get(variant); buffer->header.stamp = msg_ptr->header.stamp; buffer->published_stamp = msg_ptr->header.stamp; @@ -363,6 +364,7 @@ void SubscriberBase::on_pointcloud( if (search_pointcloud_near_pose(pcl_pointcloud, entity_pose_, entity_search_radius_)) { RCLCPP_INFO(node_->get_logger(), "%s reacted without published time", node_name.c_str()); // set header time + // cppcheck-suppress constVariableReference auto & buffer = std::get(variant); buffer->header.stamp = msg_ptr->header.stamp; buffer->published_stamp = msg_ptr->header.stamp; @@ -440,6 +442,7 @@ void SubscriberBase::on_predicted_objects( if (search_predicted_objects_near_pose(*msg_ptr, entity_pose_, entity_search_radius_)) { RCLCPP_INFO(node_->get_logger(), "%s reacted without published time", node_name.c_str()); // set header time + // cppcheck-suppress constVariableReference auto & buffer = std::get(variant); buffer->header.stamp = msg_ptr->header.stamp; buffer->published_stamp = msg_ptr->header.stamp; @@ -520,6 +523,7 @@ void SubscriberBase::on_detected_objects( if (search_detected_objects_near_pose(output_objs, entity_pose_, entity_search_radius_)) { RCLCPP_INFO(node_->get_logger(), "%s reacted without published time", node_name.c_str()); // set header time + // cppcheck-suppress constVariableReference auto & buffer = std::get(variant); buffer->header.stamp = msg_ptr->header.stamp; buffer->published_stamp = msg_ptr->header.stamp;