From fa6d8c583539cfd30d3eb537e4bcdd5fc5ef03c4 Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Mon, 8 Jul 2024 13:47:05 +0900 Subject: [PATCH 01/19] chore: update CODEOWNERS (#7819) Signed-off-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> Co-authored-by: github-actions Co-authored-by: Kenzo Lobos Tsunekawa --- .github/CODEOWNERS | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 8f947ce2d2a13..0fe324f84f81a 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -6,7 +6,7 @@ common/autoware_motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.j common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/** ahmed.ebrahim@leodrive.ai common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/** khalil@leodrive.ai common/autoware_perception_rviz_plugin/** opensource@apex.ai satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp yoshi.ri@tier4.jp -common/autoware_point_types/** taichi.higashide@tier4.jp +common/autoware_point_types/** david.wong@tier4.jp max.schmeller@tier4.jp common/autoware_test_utils/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp zulfaqar.azmi@tier4.jp common/autoware_testing/** adam.dabrowski@robotec.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/autoware_universe_utils/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @@ -115,9 +115,9 @@ perception/elevation_map_loader/** kosuke.takeuchi@tier4.jp shintaro.tomie@tier4 perception/euclidean_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/** yukihiro.saito@tier4.jp -perception/lidar_apollo_segmentation_tvm/** ambroise.vincent@arm.com xinyu.wang@tier4.jp -perception/lidar_apollo_segmentation_tvm_nodes/** ambroise.vincent@arm.com xinyu.wang@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/multi_object_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp @@ -192,10 +192,10 @@ planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_modu planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/** mamoru.sobue@tier4.jp maxime.clement@tier4.jp -planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/** maxime.clement@tier4.jp daniel.sanchez@tier4.jp +planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/** maxime.clement@tier4.jp planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/** maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -planning/motion_velocity_planner/autoware_motion_velocity_planner_common/** maxime.clement@tier4.jp daniel.sanchez@tier4.jp alqudah.mohammad@tier4.jp -planning/motion_velocity_planner/autoware_motion_velocity_planner_node/** maxime.clement@tier4.jp daniel.sanchez@tier4.jp alqudah.mohammad@tier4.jp +planning/motion_velocity_planner/autoware_motion_velocity_planner_common/** maxime.clement@tier4.jp +planning/motion_velocity_planner/autoware_motion_velocity_planner_node/** maxime.clement@tier4.jp planning/sampling_based_planner/autoware_bezier_sampler/** maxime.clement@tier4.jp planning/sampling_based_planner/autoware_frenet_planner/** maxime.clement@tier4.jp planning/sampling_based_planner/autoware_path_sampler/** maxime.clement@tier4.jp @@ -204,7 +204,7 @@ sensing/gnss_poser/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minod sensing/image_diagnostics/** dai.nguyen@tier4.jp yoshi.ri@tier4.jp sensing/image_transport_decompressor/** kenzo.lobos@tier4.jp yukihiro.saito@tier4.jp sensing/imu_corrector/** koji.minoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -sensing/livox/livox_tag_filter/** kenzo.lobos@tier4.jp ryohsuke.mitsudome@tier4.jp +sensing/livox/autoware_livox_tag_filter/** kenzo.lobos@tier4.jp ryohsuke.mitsudome@tier4.jp sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp david.wong@tier4.jp kenzo.lobos@tier4.jp kyoichi.sugahara@tier4.jp melike@leodrive.ai shunsuke.miura@tier4.jp yihsiang.fang@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp sensing/radar_scan_to_pointcloud2/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp sensing/radar_static_pointcloud_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp From 598d9375e915758711c7b63e0e9446a81efc0430 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Mon, 8 Jul 2024 14:49:05 +0900 Subject: [PATCH 02/19] fix(motion_planning): fix processing time topic names (#7885) Signed-off-by: Maxime CLEMENT --- .../autoware_motion_velocity_planner_node/src/node.cpp | 4 ++-- .../autoware_motion_velocity_planner_node/src/node.hpp | 3 ++- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp index f0100f58074ba..f181b0c76b51e 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -83,8 +83,8 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & velocity_factor_publisher_ = this->create_publisher( "~/output/velocity_factors", 1); - processing_time_publisher_ = this->create_publisher( - "~/debug/total_time/processing_time_ms", 1); + processing_time_publisher_ = + this->create_publisher("~/debug/processing_time_ms", 1); // Parameters smooth_velocity_before_planning_ = declare_parameter("smooth_velocity_before_planning"); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp index 8debb9cbedf00..4bf24cadb36f2 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp @@ -97,7 +97,8 @@ class MotionVelocityPlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_viz_pub_; rclcpp::Publisher::SharedPtr velocity_factor_publisher_; - autoware::universe_utils::ProcessingTimePublisher processing_diag_publisher_{this}; + autoware::universe_utils::ProcessingTimePublisher processing_diag_publisher_{ + this, "~/debug/processing_time_ms_diag"}; rclcpp::Publisher::SharedPtr processing_time_publisher_; autoware::universe_utils::PublishedTimePublisher published_time_publisher_{this}; From 45a0c3abd0a7436e650b937c5b8da8833f0ba535 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 8 Jul 2024 15:53:24 +0900 Subject: [PATCH 03/19] refactor(probabilistic_occupancy_grid_map)!: fix namespace and directory structure (#7872) * refactor: update include paths for probabilistic occupancy grid map Signed-off-by: Taekjin LEE * refactor: update include paths for probabilistic occupancy grid map Signed-off-by: Taekjin LEE * refactor: update namespace for nodes Signed-off-by: Taekjin LEE * refactor: update namespace for lib Signed-off-by: Taekjin LEE * refactor: remove unused dependency Signed-off-by: Taekjin LEE * refactor: use const pointer for occupancy grid map data Signed-off-by: Taekjin LEE --------- Signed-off-by: Taekjin LEE --- .../CMakeLists.txt | 35 ++++++++++--------- .../cost_value}/cost_value.hpp | 14 +++++--- .../costmap_2d}/occupancy_grid_map.hpp | 9 +++-- .../costmap_2d}/occupancy_grid_map_base.hpp | 9 +++-- .../costmap_2d}/occupancy_grid_map_fixed.hpp | 11 +++--- .../occupancy_grid_map_projective.hpp | 11 +++--- .../fusion_policy/fusion_policy.hpp} | 12 ++++--- .../updater/binary_bayes_filter_updater.hpp} | 11 +++--- .../log_odds_bayes_filter_updater.hpp} | 15 ++++---- .../updater/ogm_updater_interface.hpp} | 14 ++++---- .../utils/utils.hpp | 12 ++++--- ...serscan_based_occupancy_grid_map.launch.py | 2 +- ...ntcloud_based_occupancy_grid_map.launch.py | 2 +- ...ntcloud_based_occupancy_grid_map.launch.py | 2 +- .../costmap_2d}/occupancy_grid_map.cpp | 17 +++++---- .../costmap_2d}/occupancy_grid_map_base.cpp | 12 ++++--- .../costmap_2d}/occupancy_grid_map_fixed.cpp | 29 ++++++++------- .../occupancy_grid_map_projective.cpp | 26 ++++++++------ .../fusion_policy/fusion_policy.cpp} | 9 +++-- .../updater/binary_bayes_filter_updater.cpp} | 13 ++++--- .../log_odds_bayes_filter_updater.cpp} | 10 ++++-- .../{src => lib}/utils/utils.cpp | 15 ++++---- .../package.xml | 5 +-- .../synchronized_grid_map_fusion_node.cpp | 20 +++++------ .../synchronized_grid_map_fusion_node.hpp | 18 +++++----- ...aserscan_based_occupancy_grid_map_node.cpp | 14 ++++---- ...aserscan_based_occupancy_grid_map_node.hpp | 16 ++++----- ...intcloud_based_occupancy_grid_map_node.cpp | 18 +++++----- ...intcloud_based_occupancy_grid_map_node.hpp | 16 ++++----- .../test/cost_value_test.cpp | 6 ++-- .../test/fusion_policy_test.cpp | 9 ++--- .../test/test_utils.cpp | 12 +++---- 32 files changed, 241 insertions(+), 183 deletions(-) rename perception/probabilistic_occupancy_grid_map/include/{probabilistic_occupancy_grid_map => autoware/probabilistic_occupancy_grid_map/cost_value}/cost_value.hpp (90%) rename perception/probabilistic_occupancy_grid_map/include/{probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map => autoware/probabilistic_occupancy_grid_map/costmap_2d}/occupancy_grid_map.hpp (91%) rename perception/probabilistic_occupancy_grid_map/include/{probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map => autoware/probabilistic_occupancy_grid_map/costmap_2d}/occupancy_grid_map_base.hpp (91%) rename perception/probabilistic_occupancy_grid_map/include/{probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map => autoware/probabilistic_occupancy_grid_map/costmap_2d}/occupancy_grid_map_fixed.hpp (74%) rename perception/probabilistic_occupancy_grid_map/include/{probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map => autoware/probabilistic_occupancy_grid_map/costmap_2d}/occupancy_grid_map_projective.hpp (77%) rename perception/probabilistic_occupancy_grid_map/include/{probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp => autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp} (84%) rename perception/probabilistic_occupancy_grid_map/include/{probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp => autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp} (72%) rename perception/probabilistic_occupancy_grid_map/include/{probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp => autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater.hpp} (69%) rename perception/probabilistic_occupancy_grid_map/include/{probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp => autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp} (68%) rename perception/probabilistic_occupancy_grid_map/include/{ => autoware}/probabilistic_occupancy_grid_map/utils/utils.hpp (90%) rename perception/probabilistic_occupancy_grid_map/{src/laserscan_based_occupancy_grid_map => lib/costmap_2d}/occupancy_grid_map.cpp (93%) rename perception/probabilistic_occupancy_grid_map/{src/pointcloud_based_occupancy_grid_map => lib/costmap_2d}/occupancy_grid_map_base.cpp (95%) rename perception/probabilistic_occupancy_grid_map/{src/pointcloud_based_occupancy_grid_map => lib/costmap_2d}/occupancy_grid_map_fixed.cpp (90%) rename perception/probabilistic_occupancy_grid_map/{src/pointcloud_based_occupancy_grid_map => lib/costmap_2d}/occupancy_grid_map_projective.cpp (93%) rename perception/probabilistic_occupancy_grid_map/{src/fusion/single_frame_fusion_policy.cpp => lib/fusion_policy/fusion_policy.cpp} (97%) rename perception/probabilistic_occupancy_grid_map/{src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp => lib/updater/binary_bayes_filter_updater.cpp} (88%) rename perception/probabilistic_occupancy_grid_map/{src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp => lib/updater/log_odds_bayes_filter_updater.cpp} (88%) rename perception/probabilistic_occupancy_grid_map/{src => lib}/utils/utils.cpp (95%) rename perception/probabilistic_occupancy_grid_map/{include/probabilistic_occupancy_grid_map => src}/fusion/synchronized_grid_map_fusion_node.hpp (85%) rename perception/probabilistic_occupancy_grid_map/{include/probabilistic_occupancy_grid_map => src}/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp (82%) rename perception/probabilistic_occupancy_grid_map/{include/probabilistic_occupancy_grid_map => src}/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp (81%) diff --git a/perception/probabilistic_occupancy_grid_map/CMakeLists.txt b/perception/probabilistic_occupancy_grid_map/CMakeLists.txt index 928532f928e38..d01ef157e21b4 100644 --- a/perception/probabilistic_occupancy_grid_map/CMakeLists.txt +++ b/perception/probabilistic_occupancy_grid_map/CMakeLists.txt @@ -14,8 +14,8 @@ include_directories( ) ament_auto_add_library(${PROJECT_NAME}_common SHARED - src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp - src/utils/utils.cpp + lib/updater/binary_bayes_filter_updater.cpp + lib/utils/utils.cpp ) target_link_libraries(${PROJECT_NAME}_common ${PCL_LIBRARIES} @@ -24,9 +24,9 @@ target_link_libraries(${PROJECT_NAME}_common # PointcloudBasedOccupancyGridMap ament_auto_add_library(pointcloud_based_occupancy_grid_map SHARED src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp - src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp - src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp - src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp + lib/costmap_2d/occupancy_grid_map_base.cpp + lib/costmap_2d/occupancy_grid_map_fixed.cpp + lib/costmap_2d/occupancy_grid_map_projective.cpp ) target_link_libraries(pointcloud_based_occupancy_grid_map @@ -35,14 +35,14 @@ target_link_libraries(pointcloud_based_occupancy_grid_map ) rclcpp_components_register_node(pointcloud_based_occupancy_grid_map - PLUGIN "occupancy_grid_map::PointcloudBasedOccupancyGridMapNode" + PLUGIN "autoware::occupancy_grid_map::PointcloudBasedOccupancyGridMapNode" EXECUTABLE pointcloud_based_occupancy_grid_map_node ) # LaserscanBasedOccupancyGridMap ament_auto_add_library(laserscan_based_occupancy_grid_map SHARED src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp - src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp + lib/costmap_2d/occupancy_grid_map.cpp ) target_link_libraries(laserscan_based_occupancy_grid_map @@ -51,17 +51,17 @@ target_link_libraries(laserscan_based_occupancy_grid_map ) rclcpp_components_register_node(laserscan_based_occupancy_grid_map - PLUGIN "occupancy_grid_map::LaserscanBasedOccupancyGridMapNode" + PLUGIN "autoware::occupancy_grid_map::LaserscanBasedOccupancyGridMapNode" EXECUTABLE laserscan_based_occupancy_grid_map_node ) # GridMapFusionNode ament_auto_add_library(synchronized_grid_map_fusion SHARED src/fusion/synchronized_grid_map_fusion_node.cpp - src/fusion/single_frame_fusion_policy.cpp - src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp - src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp - src/utils/utils.cpp + lib/fusion_policy/fusion_policy.cpp + lib/costmap_2d/occupancy_grid_map_fixed.cpp + lib/updater/log_odds_bayes_filter_updater.cpp + lib/utils/utils.cpp ) target_link_libraries(synchronized_grid_map_fusion @@ -69,7 +69,7 @@ target_link_libraries(synchronized_grid_map_fusion ) rclcpp_components_register_node(synchronized_grid_map_fusion - PLUGIN "synchronized_grid_map_fusion::GridMapFusionNode" + PLUGIN "autoware::occupancy_grid_map::GridMapFusionNode" EXECUTABLE synchronized_grid_map_fusion_node ) @@ -88,13 +88,14 @@ if(BUILD_TESTING) # gtest ament_add_gtest(test_utils - test/test_utils.cpp + test/test_utils.cpp ) ament_add_gtest(costmap_unit_tests - test/cost_value_test.cpp) + test/cost_value_test.cpp + ) ament_add_gtest(fusion_policy_unit_tests - test/fusion_policy_test.cpp - src/fusion/single_frame_fusion_policy.cpp + test/fusion_policy_test.cpp + lib/fusion_policy/fusion_policy.cpp ) target_link_libraries(test_utils ${PCL_LIBRARIES} diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/cost_value.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp similarity index 90% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/cost_value.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp index 880297a1210ed..d470d206c41c6 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/cost_value.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp @@ -48,12 +48,14 @@ * Author: Eitan Marder-Eppstein *********************************************************************/ -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE__COST_VALUE_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE__COST_VALUE_HPP_ #include -namespace occupancy_cost_value +namespace autoware::occupancy_grid_map +{ +namespace cost_value { static const unsigned char NO_INFORMATION = 128; // 0.5 * 255 static const unsigned char LETHAL_OBSTACLE = 255; @@ -101,6 +103,8 @@ struct InverseCostTranslationTable static const CostTranslationTable cost_translation_table; static const InverseCostTranslationTable inverse_cost_translation_table; -} // namespace occupancy_cost_value -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE_HPP_ +} // namespace cost_value +} // namespace autoware::occupancy_grid_map + +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE__COST_VALUE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map.hpp similarity index 91% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map.hpp index d2210cf9c348a..93e768c0f6b4e 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map.hpp @@ -49,8 +49,8 @@ * David V. Lu!! *********************************************************************/ -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_HPP_ #include #include @@ -59,6 +59,8 @@ #include #include +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using geometry_msgs::msg::Pose; @@ -90,5 +92,6 @@ class OccupancyGridMap : public nav2_costmap_2d::Costmap2D }; } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp similarity index 91% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp index f01b2d74e160b..2b2057af9cc12 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp @@ -49,8 +49,8 @@ * David V. Lu!! *********************************************************************/ -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_BASE_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_BASE_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_BASE_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_BASE_HPP_ #include #include @@ -59,6 +59,8 @@ #include #include +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using geometry_msgs::msg::Pose; @@ -91,5 +93,6 @@ class OccupancyGridMapInterface : public nav2_costmap_2d::Costmap2D }; } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_BASE_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_BASE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp similarity index 74% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp index 298f327d632d7..9dfd8b8689123 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp @@ -12,11 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_FIXED_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_FIXED_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_FIXED_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_FIXED_HPP_ -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp" +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using geometry_msgs::msg::Pose; @@ -43,5 +45,6 @@ class OccupancyGridMapFixedBlindSpot : public OccupancyGridMapInterface }; } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_FIXED_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_FIXED_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp similarity index 77% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp index 67b51d6184c8c..1f0f92d933ff3 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp @@ -12,15 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp" #include #include +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using geometry_msgs::msg::Pose; @@ -51,5 +53,6 @@ class OccupancyGridMapProjectiveBlindSpot : public OccupancyGridMapInterface }; } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp similarity index 84% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp index 1f0553878ef5a..a2d7c90a023a5 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION_POLICY__FUSION_POLICY_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION_POLICY__FUSION_POLICY_HPP_ -#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include #include @@ -25,6 +25,8 @@ /*min and max prob threshold to prevent log-odds to diverge*/ #define EPSILON_PROB 0.03 +namespace autoware::occupancy_grid_map +{ namespace fusion_policy { enum class FusionMethod { OVERWRITE, LOG_ODDS, DEMPSTER_SHAFER }; @@ -61,6 +63,8 @@ unsigned char singleFrameOccupancyFusion( unsigned char singleFrameOccupancyFusion( const std::vector & occupancy, FusionMethod method, const std::vector & reliability); + } // namespace fusion_policy +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION_POLICY__FUSION_POLICY_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp similarity index 72% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp index 3a921035ef219..af28b7962b3bc 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp @@ -12,14 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__BINARY_BAYES_FILTER_UPDATER_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__BINARY_BAYES_FILTER_UPDATER_HPP_ -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp" #include #include +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { class OccupancyGridMapBBFUpdater : public OccupancyGridMapUpdaterInterface @@ -38,5 +40,6 @@ class OccupancyGridMapBBFUpdater : public OccupancyGridMapUpdaterInterface }; } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__BINARY_BAYES_FILTER_UPDATER_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater.hpp similarity index 69% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater.hpp index 245484e442609..d7bb1184c06d6 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ -#include "probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp" -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include @@ -25,6 +25,8 @@ // LOBF means: Log Odds Bayes Filter // cspell: ignore LOBF +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { class OccupancyGridMapLOBFUpdater : public OccupancyGridMapUpdaterInterface @@ -45,5 +47,6 @@ class OccupancyGridMapLOBFUpdater : public OccupancyGridMapUpdaterInterface }; } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp similarity index 68% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp index e85edf2245ef3..75231089ac33c 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp @@ -12,14 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OGM_UPDATER_INTERFACE_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OGM_UPDATER_INTERFACE_HPP_ -#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include #include +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { class OccupancyGridMapUpdaterInterface : public nav2_costmap_2d::Costmap2D @@ -27,8 +29,7 @@ class OccupancyGridMapUpdaterInterface : public nav2_costmap_2d::Costmap2D public: OccupancyGridMapUpdaterInterface( const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution) - : Costmap2D( - cells_size_x, cells_size_y, resolution, 0.f, 0.f, occupancy_cost_value::NO_INFORMATION) + : Costmap2D(cells_size_x, cells_size_y, resolution, 0.f, 0.f, cost_value::NO_INFORMATION) { } virtual ~OccupancyGridMapUpdaterInterface() = default; @@ -37,5 +38,6 @@ class OccupancyGridMapUpdaterInterface : public nav2_costmap_2d::Costmap2D }; } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OGM_UPDATER_INTERFACE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/utils/utils.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp similarity index 90% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/utils/utils.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp index 0950272dac470..50b470d904ef6 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/utils/utils.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ -#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include #include @@ -45,6 +45,8 @@ #include #include +namespace autoware::occupancy_grid_map +{ namespace utils { @@ -117,6 +119,8 @@ bool extractCommonPointCloud( sensor_msgs::msg::PointCloud2 & output_obstacle_pc); unsigned char getApproximateOccupancyState(const unsigned char & value); + } // namespace utils +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py index 62cfa4bcb5228..1edc538b3de3f 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py +++ b/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py @@ -85,7 +85,7 @@ def launch_setup(context, *args, **kwargs): ), ComposableNode( package="probabilistic_occupancy_grid_map", - plugin="occupancy_grid_map::LaserscanBasedOccupancyGridMapNode", + plugin="autoware::occupancy_grid_map::LaserscanBasedOccupancyGridMapNode", name="occupancy_grid_map_node", remappings=[ ("~/input/laserscan", LaunchConfiguration("output/laserscan")), diff --git a/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py index b112dd0a84b83..ef40839d2a5aa 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py +++ b/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py @@ -196,7 +196,7 @@ def launch_setup(context, *args, **kwargs): # generate composable node node = ComposableNode( package="probabilistic_occupancy_grid_map", - plugin="occupancy_grid_map::PointcloudBasedOccupancyGridMapNode", + plugin="autoware::occupancy_grid_map::PointcloudBasedOccupancyGridMapNode", name="occupancy_grid_map_node", namespace=frame_name, remappings=[ diff --git a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py index 0e84e4860fdf3..bb5ef025a7e47 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py +++ b/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py @@ -96,7 +96,7 @@ def launch_setup(context, *args, **kwargs): composable_nodes.append( ComposableNode( package="probabilistic_occupancy_grid_map", - plugin="occupancy_grid_map::PointcloudBasedOccupancyGridMapNode", + plugin="autoware::occupancy_grid_map::PointcloudBasedOccupancyGridMapNode", name="occupancy_grid_map_node", remappings=[ ( diff --git a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map.cpp similarity index 93% rename from perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp rename to perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map.cpp index 3d02be9ca7156..25eafcd564e2d 100644 --- a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map.cpp @@ -49,21 +49,23 @@ * David V. Lu!! *********************************************************************/ -#include "probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include #include +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using sensor_msgs::PointCloud2ConstIterator; OccupancyGridMap::OccupancyGridMap( const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution) -: Costmap2D(cells_size_x, cells_size_y, resolution, 0.f, 0.f, occupancy_cost_value::NO_INFORMATION) +: Costmap2D(cells_size_x, cells_size_y, resolution, 0.f, 0.f, cost_value::NO_INFORMATION) { } @@ -140,7 +142,7 @@ void OccupancyGridMap::raytrace2D(const PointCloud2 & pointcloud, const Pose & r raytraceFreespace(pointcloud, robot_pose); // occupied - MarkCell marker(costmap_, occupancy_cost_value::LETHAL_OBSTACLE); + MarkCell marker(costmap_, cost_value::LETHAL_OBSTACLE); for (PointCloud2ConstIterator iter_x(pointcloud, "x"), iter_y(pointcloud, "y"); iter_x != iter_x.end(); ++iter_x, ++iter_y) { unsigned int mx, my; @@ -155,12 +157,12 @@ void OccupancyGridMap::raytrace2D(const PointCloud2 & pointcloud, const Pose & r void OccupancyGridMap::updateFreespaceCells(const PointCloud2 & pointcloud) { - updateCellsByPointCloud(pointcloud, occupancy_cost_value::FREE_SPACE); + updateCellsByPointCloud(pointcloud, cost_value::FREE_SPACE); } void OccupancyGridMap::updateOccupiedCells(const PointCloud2 & pointcloud) { - updateCellsByPointCloud(pointcloud, occupancy_cost_value::LETHAL_OBSTACLE); + updateCellsByPointCloud(pointcloud, cost_value::LETHAL_OBSTACLE); } void OccupancyGridMap::updateCellsByPointCloud( @@ -244,9 +246,10 @@ void OccupancyGridMap::raytraceFreespace(const PointCloud2 & pointcloud, const P } constexpr unsigned int cell_raytrace_range = 10000; // large number to ignore range threshold - MarkCell marker(costmap_, occupancy_cost_value::FREE_SPACE); + MarkCell marker(costmap_, cost_value::FREE_SPACE); raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range); } } } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_base.cpp similarity index 95% rename from perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp rename to perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_base.cpp index 3d176e41583a0..5e9b5598c8f63 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_base.cpp @@ -49,10 +49,10 @@ * David V. Lu!! *********************************************************************/ -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include @@ -69,13 +69,16 @@ #endif #include + +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using sensor_msgs::PointCloud2ConstIterator; OccupancyGridMapInterface::OccupancyGridMapInterface( const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution) -: Costmap2D(cells_size_x, cells_size_y, resolution, 0.f, 0.f, occupancy_cost_value::NO_INFORMATION) +: Costmap2D(cells_size_x, cells_size_y, resolution, 0.f, 0.f, cost_value::NO_INFORMATION) { } @@ -230,3 +233,4 @@ void OccupancyGridMapInterface::raytrace( } } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp similarity index 90% rename from perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp rename to perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp index d4e177209f99d..f25009d47bdcc 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include @@ -33,6 +33,9 @@ #endif #include + +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using sensor_msgs::PointCloud2ConstIterator; @@ -136,7 +139,7 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( } raytrace( scan_origin.position.x, scan_origin.position.y, end_distance.wx, end_distance.wy, - occupancy_cost_value::FREE_SPACE); + cost_value::FREE_SPACE); } // Second step: Add unknown cell @@ -162,9 +165,8 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); if (!no_freespace_point) { const auto & target = *raw_distance_iter; - raytrace( - source.wx, source.wy, target.wx, target.wy, occupancy_cost_value::NO_INFORMATION); - setCellValue(target.wx, target.wy, occupancy_cost_value::FREE_SPACE); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::NO_INFORMATION); + setCellValue(target.wx, target.wy, cost_value::FREE_SPACE); } continue; } @@ -177,7 +179,7 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( } else if (no_freespace_point) { const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); const auto & target = obstacle_pointcloud_angle_bin.at(dist_index + 1); - raytrace(source.wx, source.wy, target.wx, target.wy, occupancy_cost_value::NO_INFORMATION); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::NO_INFORMATION); continue; } @@ -186,13 +188,13 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( if (next_raw_distance < next_obstacle_point_distance) { const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); const auto & target = *raw_distance_iter; - raytrace(source.wx, source.wy, target.wx, target.wy, occupancy_cost_value::NO_INFORMATION); - setCellValue(target.wx, target.wy, occupancy_cost_value::FREE_SPACE); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::NO_INFORMATION); + setCellValue(target.wx, target.wy, cost_value::FREE_SPACE); continue; } else { const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); const auto & target = obstacle_pointcloud_angle_bin.at(dist_index + 1); - raytrace(source.wx, source.wy, target.wx, target.wy, occupancy_cost_value::NO_INFORMATION); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::NO_INFORMATION); continue; } } @@ -203,7 +205,7 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( auto & obstacle_pointcloud_angle_bin = obstacle_pointcloud_angle_bins.at(bin_index); for (size_t dist_index = 0; dist_index < obstacle_pointcloud_angle_bin.size(); ++dist_index) { const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); - setCellValue(source.wx, source.wy, occupancy_cost_value::LETHAL_OBSTACLE); + setCellValue(source.wx, source.wy, cost_value::LETHAL_OBSTACLE); if (dist_index + 1 == obstacle_pointcloud_angle_bin.size()) { continue; @@ -215,7 +217,7 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( if (next_obstacle_point_distance <= distance_margin_) { const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); const auto & target = obstacle_pointcloud_angle_bin.at(dist_index + 1); - raytrace(source.wx, source.wy, target.wx, target.wy, occupancy_cost_value::LETHAL_OBSTACLE); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::LETHAL_OBSTACLE); continue; } } @@ -229,3 +231,4 @@ void OccupancyGridMapFixedBlindSpot::initRosParam(rclcpp::Node & node) } } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp similarity index 93% rename from perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp rename to perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp index 03d7a42ae7043..a2400fb3c81b2 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include @@ -34,6 +34,9 @@ #endif #include + +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using sensor_msgs::PointCloud2ConstIterator; @@ -192,7 +195,7 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( } raytrace( scan_origin.position.x, scan_origin.position.y, ray_end.wx, ray_end.wy, - occupancy_cost_value::FREE_SPACE); + cost_value::FREE_SPACE); } if (pub_debug_grid_) @@ -219,7 +222,7 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); raytrace( source.wx, source.wy, source.projected_wx, source.projected_wy, - occupancy_cost_value::NO_INFORMATION); + cost_value::NO_INFORMATION); break; } @@ -227,7 +230,7 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); raytrace( source.wx, source.wy, source.projected_wx, source.projected_wy, - occupancy_cost_value::NO_INFORMATION); + cost_value::NO_INFORMATION); continue; } @@ -243,13 +246,13 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( if (next_raw_distance < next_obstacle_point_distance) { const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); const auto & target = *raw_distance_iter; - raytrace(source.wx, source.wy, target.wx, target.wy, occupancy_cost_value::NO_INFORMATION); - setCellValue(target.wx, target.wy, occupancy_cost_value::FREE_SPACE); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::NO_INFORMATION); + setCellValue(target.wx, target.wy, cost_value::FREE_SPACE); continue; } else { const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); const auto & target = obstacle_pointcloud_angle_bin.at(dist_index + 1); - raytrace(source.wx, source.wy, target.wx, target.wy, occupancy_cost_value::NO_INFORMATION); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::NO_INFORMATION); continue; } } @@ -262,7 +265,7 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( auto & obstacle_pointcloud_angle_bin = obstacle_pointcloud_angle_bins.at(bin_index); for (size_t dist_index = 0; dist_index < obstacle_pointcloud_angle_bin.size(); ++dist_index) { const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); - setCellValue(source.wx, source.wy, occupancy_cost_value::LETHAL_OBSTACLE); + setCellValue(source.wx, source.wy, cost_value::LETHAL_OBSTACLE); if (dist_index + 1 == obstacle_pointcloud_angle_bin.size()) { continue; @@ -274,7 +277,7 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( if (next_obstacle_point_distance <= obstacle_separation_threshold_) { const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); const auto & target = obstacle_pointcloud_angle_bin.at(dist_index + 1); - raytrace(source.wx, source.wy, target.wx, target.wy, occupancy_cost_value::LETHAL_OBSTACLE); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::LETHAL_OBSTACLE); continue; } } @@ -299,3 +302,4 @@ void OccupancyGridMapProjectiveBlindSpot::initRosParam(rclcpp::Node & node) } } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/src/fusion/single_frame_fusion_policy.cpp b/perception/probabilistic_occupancy_grid_map/lib/fusion_policy/fusion_policy.cpp similarity index 97% rename from perception/probabilistic_occupancy_grid_map/src/fusion/single_frame_fusion_policy.cpp rename to perception/probabilistic_occupancy_grid_map/lib/fusion_policy/fusion_policy.cpp index 7e2c3f7ba304a..1e88b7a39fd14 100644 --- a/perception/probabilistic_occupancy_grid_map/src/fusion/single_frame_fusion_policy.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/fusion_policy/fusion_policy.cpp @@ -12,8 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp" +#include "autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp" +namespace autoware::occupancy_grid_map +{ namespace fusion_policy { @@ -61,9 +63,9 @@ namespace overwrite_fusion */ State getApproximateState(const unsigned char & occupancy) { - if (occupancy >= occupancy_cost_value::OCCUPIED_THRESHOLD) { + if (occupancy >= cost_value::OCCUPIED_THRESHOLD) { return State::OCCUPIED; - } else if (occupancy <= occupancy_cost_value::FREE_THRESHOLD) { + } else if (occupancy <= cost_value::FREE_THRESHOLD) { return State::FREE; } else { return State::UNKNOWN; @@ -320,3 +322,4 @@ unsigned char singleFrameOccupancyFusion( } } // namespace fusion_policy +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp b/perception/probabilistic_occupancy_grid_map/lib/updater/binary_bayes_filter_updater.cpp similarity index 88% rename from perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp rename to perception/probabilistic_occupancy_grid_map/lib/updater/binary_bayes_filter_updater.cpp index 40f538a64f6e9..74506d095b36d 100644 --- a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/updater/binary_bayes_filter_updater.cpp @@ -12,12 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { @@ -48,15 +50,15 @@ inline unsigned char OccupancyGridMapBBFUpdater::applyBBF( float pz{}; float not_pz{}; float po_hat{}; - if (z == occupancy_cost_value::LETHAL_OBSTACLE) { + if (z == cost_value::LETHAL_OBSTACLE) { pz = probability_matrix_(Index::OCCUPIED, Index::OCCUPIED); not_pz = probability_matrix_(Index::FREE, Index::OCCUPIED); po_hat = ((po * pz) / ((po * pz) + ((1.f - po) * not_pz))); - } else if (z == occupancy_cost_value::FREE_SPACE) { + } else if (z == cost_value::FREE_SPACE) { pz = 1.f - probability_matrix_(Index::FREE, Index::FREE); not_pz = 1.f - probability_matrix_(Index::OCCUPIED, Index::FREE); po_hat = ((po * pz) / ((po * pz) + ((1.f - po) * not_pz))); - } else if (z == occupancy_cost_value::NO_INFORMATION) { + } else if (z == cost_value::NO_INFORMATION) { const float inv_v_ratio = 1.f / v_ratio_; po_hat = ((po + (0.5f * inv_v_ratio)) / ((1.f * inv_v_ratio) + 1.f)); } @@ -79,3 +81,4 @@ bool OccupancyGridMapBBFUpdater::update(const Costmap2D & single_frame_occupancy } } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp b/perception/probabilistic_occupancy_grid_map/lib/updater/log_odds_bayes_filter_updater.cpp similarity index 88% rename from perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp rename to perception/probabilistic_occupancy_grid_map/lib/updater/log_odds_bayes_filter_updater.cpp index 607694d6da571..9f3c3c7e40eaf 100644 --- a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/updater/log_odds_bayes_filter_updater.cpp @@ -12,14 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include // cspell: ignore LOBF +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { @@ -35,7 +37,7 @@ inline unsigned char OccupancyGridMapLOBFUpdater::applyLOBF( using fusion_policy::convertProbabilityToChar; using fusion_policy::log_odds_fusion::logOddsFusion; - constexpr unsigned char unknown = occupancy_cost_value::NO_INFORMATION; + constexpr unsigned char unknown = cost_value::NO_INFORMATION; constexpr unsigned char unknown_margin = 1; /* Tau and ST decides how fast the observation decay to the unknown status*/ constexpr double tau = 0.75; @@ -68,4 +70,6 @@ bool OccupancyGridMapLOBFUpdater::update(const Costmap2D & single_frame_occupanc } return true; } + } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp b/perception/probabilistic_occupancy_grid_map/lib/utils/utils.cpp similarity index 95% rename from perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp rename to perception/probabilistic_occupancy_grid_map/lib/utils/utils.cpp index 209207fe10f34..e8c2cdb2617df 100644 --- a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/utils/utils.cpp @@ -12,12 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include +namespace autoware::occupancy_grid_map +{ namespace utils { @@ -189,13 +191,14 @@ bool extractCommonPointCloud( */ unsigned char getApproximateOccupancyState(const unsigned char & value) { - if (value >= occupancy_cost_value::OCCUPIED_THRESHOLD) { - return occupancy_cost_value::LETHAL_OBSTACLE; - } else if (value <= occupancy_cost_value::FREE_THRESHOLD) { - return occupancy_cost_value::FREE_SPACE; + if (value >= cost_value::OCCUPIED_THRESHOLD) { + return cost_value::LETHAL_OBSTACLE; + } else if (value <= cost_value::FREE_THRESHOLD) { + return cost_value::FREE_SPACE; } else { - return occupancy_cost_value::NO_INFORMATION; + return cost_value::NO_INFORMATION; } } } // namespace utils +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/package.xml b/perception/probabilistic_occupancy_grid_map/package.xml index d8ae702ef60ca..d8647ae2ba744 100644 --- a/perception/probabilistic_occupancy_grid_map/package.xml +++ b/perception/probabilistic_occupancy_grid_map/package.xml @@ -13,9 +13,9 @@ ament_cmake_auto autoware_cmake + eigen3_cmake_module autoware_universe_utils - eigen3_cmake_module grid_map_costmap_2d grid_map_msgs grid_map_ros @@ -27,12 +27,9 @@ rclcpp rclcpp_components sensor_msgs - tf2 tf2_eigen - tf2_geometry_msgs tf2_ros tf2_sensor_msgs - visualization_msgs pointcloud_preprocessor pointcloud_to_laserscan diff --git a/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp index fa93db802b0ce..a271e17a1e566 100644 --- a/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp" +#include "synchronized_grid_map_fusion_node.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" // cspell: ignore LOBF -namespace synchronized_grid_map_fusion +namespace autoware::occupancy_grid_map { using costmap_2d::OccupancyGridMapFixedBlindSpot; using costmap_2d::OccupancyGridMapLOBFUpdater; @@ -404,7 +404,7 @@ nav2_costmap_2d::Costmap2D GridMapFusionNode::OccupancyGridMsgToCostmap2D( for (unsigned int j = 0; j < occupancy_grid_map.info.height; j++) { const unsigned int index = i + j * occupancy_grid_map.info.width; costmap2d.setCost( - i, j, occupancy_cost_value::inverse_cost_translation_table[occupancy_grid_map.data[index]]); + i, j, cost_value::inverse_cost_translation_table[occupancy_grid_map.data[index]]); } } @@ -424,7 +424,7 @@ OccupancyGridMapFixedBlindSpot GridMapFusionNode::OccupancyGridMsgToGridMap( for (unsigned int j = 0; j < occupancy_grid_map.info.height; j++) { const unsigned int index = i + j * occupancy_grid_map.info.width; gridmap.setCost( - i, j, occupancy_cost_value::inverse_cost_translation_table[occupancy_grid_map.data[index]]); + i, j, cost_value::inverse_cost_translation_table[occupancy_grid_map.data[index]]); } } return gridmap; @@ -453,14 +453,14 @@ nav_msgs::msg::OccupancyGrid::UniquePtr GridMapFusionNode::OccupancyGridMapToMsg msg_ptr->data.resize(msg_ptr->info.width * msg_ptr->info.height); - unsigned char * data = occupancy_grid_map.getCharMap(); + const unsigned char * data = occupancy_grid_map.getCharMap(); for (unsigned int i = 0; i < msg_ptr->data.size(); ++i) { - msg_ptr->data[i] = occupancy_cost_value::cost_translation_table[data[i]]; + msg_ptr->data[i] = cost_value::cost_translation_table[data[i]]; } return msg_ptr; } -} // namespace synchronized_grid_map_fusion +} // namespace autoware::occupancy_grid_map #include -RCLCPP_COMPONENTS_REGISTER_NODE(synchronized_grid_map_fusion::GridMapFusionNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::occupancy_grid_map::GridMapFusionNode) diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.hpp similarity index 85% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp rename to perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.hpp index 84ca13c8b1881..c839f160aab9b 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp +++ b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ +#ifndef FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ +#define FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ -#include "probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp" -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp" -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp" -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp" +#include "autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp" #include #include @@ -45,7 +45,7 @@ // cspell: ignore LOBF -namespace synchronized_grid_map_fusion +namespace autoware::occupancy_grid_map { using costmap_2d::OccupancyGridMapFixedBlindSpot; @@ -124,6 +124,6 @@ class GridMapFusionNode : public rclcpp::Node fusion_policy::FusionMethod fusion_method_; }; -} // namespace synchronized_grid_map_fusion +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ +#endif // FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp index 777c180fe1a3a..6c4ad8135b37d 100644 --- a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp" +#include "laserscan_based_occupancy_grid_map_node.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include @@ -35,7 +35,7 @@ #include #include -namespace occupancy_grid_map +namespace autoware::occupancy_grid_map { using costmap_2d::OccupancyGridMap; using costmap_2d::OccupancyGridMapBBFUpdater; @@ -229,12 +229,12 @@ OccupancyGrid::UniquePtr LaserscanBasedOccupancyGridMapNode::OccupancyGridMapToM unsigned char * data = occupancy_grid_map.getCharMap(); for (unsigned int i = 0; i < msg_ptr->data.size(); ++i) { - msg_ptr->data[i] = occupancy_cost_value::cost_translation_table[data[i]]; + msg_ptr->data[i] = cost_value::cost_translation_table[data[i]]; } return msg_ptr; } -} // namespace occupancy_grid_map +} // namespace autoware::occupancy_grid_map #include -RCLCPP_COMPONENTS_REGISTER_NODE(occupancy_grid_map::LaserscanBasedOccupancyGridMapNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::occupancy_grid_map::LaserscanBasedOccupancyGridMapNode) diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp similarity index 82% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp rename to perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp index a8adea6e700e5..a599f7b564c8b 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp +++ b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#ifndef LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#define LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ -#include "probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp" -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp" #include #include @@ -36,7 +36,7 @@ #include #include -namespace occupancy_grid_map +namespace autoware::occupancy_grid_map { using builtin_interfaces::msg::Time; using costmap_2d::OccupancyGridMapUpdaterInterface; @@ -100,6 +100,6 @@ class LaserscanBasedOccupancyGridMapNode : public rclcpp::Node bool enable_single_frame_mode_; }; -} // namespace occupancy_grid_map +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#endif // LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp index d019280aefda0..3fe75263b3f28 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp" +#include "pointcloud_based_occupancy_grid_map_node.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp" -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp" -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include @@ -40,7 +40,7 @@ #include #include -namespace occupancy_grid_map +namespace autoware::occupancy_grid_map { using costmap_2d::OccupancyGridMapBBFUpdater; using costmap_2d::OccupancyGridMapFixedBlindSpot; @@ -250,12 +250,12 @@ OccupancyGrid::UniquePtr PointcloudBasedOccupancyGridMapNode::OccupancyGridMapTo unsigned char * data = occupancy_grid_map.getCharMap(); for (unsigned int i = 0; i < msg_ptr->data.size(); ++i) { - msg_ptr->data[i] = occupancy_cost_value::cost_translation_table[data[i]]; + msg_ptr->data[i] = cost_value::cost_translation_table[data[i]]; } return msg_ptr; } -} // namespace occupancy_grid_map +} // namespace autoware::occupancy_grid_map #include -RCLCPP_COMPONENTS_REGISTER_NODE(occupancy_grid_map::PointcloudBasedOccupancyGridMapNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::occupancy_grid_map::PointcloudBasedOccupancyGridMapNode) diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp similarity index 81% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp rename to perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp index 1d119102dc28d..e0c7ef74988f4 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#ifndef POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#define POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp" #include #include @@ -38,7 +38,7 @@ #include #include -namespace occupancy_grid_map +namespace autoware::occupancy_grid_map { using builtin_interfaces::msg::Time; using costmap_2d::OccupancyGridMapInterface; @@ -93,6 +93,6 @@ class PointcloudBasedOccupancyGridMapNode : public rclcpp::Node bool filter_obstacle_pointcloud_by_raw_pointcloud_; }; -} // namespace occupancy_grid_map +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#endif // POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp b/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp index c41c809a92b0c..f03852562f7ca 100644 --- a/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp +++ b/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include // Test the CostTranslationTable and InverseCostTranslationTable functions -using occupancy_cost_value::cost_translation_table; -using occupancy_cost_value::inverse_cost_translation_table; +using autoware::occupancy_grid_map::cost_value::cost_translation_table; +using autoware::occupancy_grid_map::cost_value::inverse_cost_translation_table; TEST(CostTranslationTableTest, TestRange) { diff --git a/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp b/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp index 6b3dc8a2ebcef..78137371c720e 100644 --- a/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp +++ b/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp @@ -12,15 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/cost_value.hpp" -#include "probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp" +#include "autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp" + +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include // Test the log-odds update rule TEST(FusionPolicyTest, TestLogOddsUpdateRule) { - using fusion_policy::log_odds_fusion::logOddsFusion; + using autoware::occupancy_grid_map::fusion_policy::log_odds_fusion::logOddsFusion; const double MARGIN = 0.03; const double OCCUPIED = 1.0 - MARGIN; const double FREE = 0.0 + MARGIN; @@ -49,7 +50,7 @@ TEST(FusionPolicyTest, TestLogOddsUpdateRule) // Test the dempster-shafer update rule TEST(FusionPolicyTest, TestDempsterShaferUpdateRule) { - using fusion_policy::dempster_shafer_fusion::dempsterShaferFusion; + using autoware::occupancy_grid_map::fusion_policy::dempster_shafer_fusion::dempsterShaferFusion; const double MARGIN = 0.03; const double OCCUPIED = 1.0 - MARGIN; const double FREE = 0.0 + MARGIN; diff --git a/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp b/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp index 4bc3dca0a67bd..17df981d849d4 100644 --- a/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp +++ b/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp @@ -13,7 +13,7 @@ // limitations under the License. // autoware -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include @@ -77,14 +77,14 @@ TEST(TestUtils, TestCropPointcloudByHeight) sensor_msgs::msg::PointCloud2 test1_output, test2_output, test3_output; // case1: normal input, normal output - EXPECT_NO_THROW( - utils::cropPointcloudByHeight(ros_cloud_10, mock_buffer, "base_link", 0.0, 10.0, test1_output)); + EXPECT_NO_THROW(autoware::occupancy_grid_map::utils::cropPointcloudByHeight( + ros_cloud_10, mock_buffer, "base_link", 0.0, 10.0, test1_output)); // case2: normal input, empty output - EXPECT_NO_THROW(utils::cropPointcloudByHeight( + EXPECT_NO_THROW(autoware::occupancy_grid_map::utils::cropPointcloudByHeight( ros_cloud_10, mock_buffer, "base_link", -2.0, -0.1, test2_output)); // case3: empty input, normal output - EXPECT_NO_THROW( - utils::cropPointcloudByHeight(ros_cloud_0, mock_buffer, "base_link", 0.0, 10.0, test3_output)); + EXPECT_NO_THROW(autoware::occupancy_grid_map::utils::cropPointcloudByHeight( + ros_cloud_0, mock_buffer, "base_link", 0.0, 10.0, test3_output)); } From 0c2045904535f837cc5b2d01170adc68b6bb820c Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Mon, 8 Jul 2024 15:59:06 +0900 Subject: [PATCH 04/19] refactor(lanelet2_map_preprocessor): apply static analysis (#7870) * refactor based on linter Signed-off-by: a-maumau * style(pre-commit): autofix * apply suggestion Signed-off-by: a-maumau --------- Signed-off-by: a-maumau Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/fix_lane_change_tags.cpp | 33 +++---- .../src/fix_z_value_by_pcd.cpp | 43 ++++---- .../src/merge_close_lines.cpp | 98 ++++++++----------- .../src/merge_close_points.cpp | 23 ++--- .../src/remove_unreferenced_geometry.cpp | 27 ++--- .../src/transform_maps.cpp | 27 ++--- 6 files changed, 116 insertions(+), 135 deletions(-) diff --git a/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp b/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp index cbd279af2b46f..571c2e91c53a5 100644 --- a/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp +++ b/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp @@ -27,10 +27,7 @@ #include #include -using lanelet::utils::getId; -using lanelet::utils::to2D; - -bool loadLaneletMap( +bool load_lanelet_map( const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr, lanelet::Projector & projector) { @@ -48,32 +45,32 @@ bool loadLaneletMap( return true; } -lanelet::Lanelets convertToVector(lanelet::LaneletMapPtr & lanelet_map_ptr) +lanelet::Lanelets convert_to_vector(const lanelet::LaneletMapPtr & lanelet_map_ptr) { lanelet::Lanelets lanelets; - for (lanelet::Lanelet lanelet : lanelet_map_ptr->laneletLayer) { - lanelets.push_back(lanelet); - } + std::copy( + lanelet_map_ptr->laneletLayer.begin(), lanelet_map_ptr->laneletLayer.end(), + std::back_inserter(lanelets)); return lanelets; } -void fixTags(lanelet::LaneletMapPtr & lanelet_map_ptr) +void fix_tags(lanelet::LaneletMapPtr & lanelet_map_ptr) { - auto lanelets = convertToVector(lanelet_map_ptr); - lanelet::traffic_rules::TrafficRulesPtr trafficRules = + auto lanelets = convert_to_vector(lanelet_map_ptr); + lanelet::traffic_rules::TrafficRulesPtr traffic_rules = lanelet::traffic_rules::TrafficRulesFactory::create( lanelet::Locations::Germany, lanelet::Participants::Vehicle); - lanelet::routing::RoutingGraphUPtr routingGraph = - lanelet::routing::RoutingGraph::build(*lanelet_map_ptr, *trafficRules); + lanelet::routing::RoutingGraphUPtr routing_graph = + lanelet::routing::RoutingGraph::build(*lanelet_map_ptr, *traffic_rules); for (auto & llt : lanelets) { - if (!routingGraph->conflicting(llt).empty()) { + if (!routing_graph->conflicting(llt).empty()) { continue; } llt.attributes().erase("turn_direction"); - if (!!routingGraph->adjacentRight(llt)) { + if (!!routing_graph->adjacentRight(llt)) { llt.rightBound().attributes()["lane_change"] = "yes"; } - if (!!routingGraph->adjacentLeft(llt)) { + if (!!routing_graph->adjacentLeft(llt)) { llt.leftBound().attributes()["lane_change"] = "yes"; } } @@ -91,11 +88,11 @@ int main(int argc, char * argv[]) lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap); lanelet::projection::MGRSProjector projector; - if (!loadLaneletMap(llt_map_path, llt_map_ptr, projector)) { + if (!load_lanelet_map(llt_map_path, llt_map_ptr, projector)) { return EXIT_FAILURE; } - fixTags(llt_map_ptr); + fix_tags(llt_map_ptr); lanelet::write(output_path, *llt_map_ptr, projector); rclcpp::shutdown(); diff --git a/map/util/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp b/map/util/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp index 14a33b01beee0..af565208c5f71 100644 --- a/map/util/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp +++ b/map/util/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp @@ -27,7 +27,7 @@ #include #include -bool loadLaneletMap( +bool load_lanelet_map( const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr, lanelet::Projector & projector) { @@ -45,7 +45,8 @@ bool loadLaneletMap( return true; } -bool loadPCDMap(const std::string & pcd_map_path, pcl::PointCloud::Ptr & pcd_map_ptr) +bool load_pcd_map( + const std::string & pcd_map_path, pcl::PointCloud::Ptr & pcd_map_ptr) { if (pcl::io::loadPCDFile(pcd_map_path, *pcd_map_ptr) == -1) { //* load the file RCLCPP_ERROR_STREAM(rclcpp::get_logger("loadPCDMap"), "Couldn't read file: " << pcd_map_path); @@ -56,16 +57,16 @@ bool loadPCDMap(const std::string & pcd_map_path, pcl::PointCloud return true; } -double getMinHeightAroundPoint( +double get_min_height_around_point( const pcl::PointCloud::Ptr & pcd_map_ptr, const pcl::KdTreeFLANN & kdtree, const pcl::PointXYZ & search_pt, const double search_radius3d, const double search_radius2d) { - std::vector pointIdxRadiusSearch; - std::vector pointRadiusSquaredDistance; + std::vector point_idx_radius_search; + std::vector point_radius_squared_distance; if ( kdtree.radiusSearch( - search_pt, search_radius3d, pointIdxRadiusSearch, pointRadiusSquaredDistance) <= 0) { + search_pt, search_radius3d, point_idx_radius_search, point_radius_squared_distance) <= 0) { std::cout << "no points found within 3d radius " << search_radius3d << std::endl; return search_pt.z; } @@ -73,8 +74,7 @@ double getMinHeightAroundPoint( double min_height = std::numeric_limits::max(); bool found = false; - for (std::size_t i = 0; i < pointIdxRadiusSearch.size(); i++) { - std::size_t pt_idx = pointIdxRadiusSearch.at(i); + for (auto pt_idx : point_idx_radius_search) { const auto pt = pcd_map_ptr->points.at(pt_idx); if (pt.z > min_height) { continue; @@ -91,8 +91,9 @@ double getMinHeightAroundPoint( return min_height; } -void adjustHeight( - const pcl::PointCloud::Ptr & pcd_map_ptr, lanelet::LaneletMapPtr & lanelet_map_ptr) +void adjust_height( + const pcl::PointCloud::Ptr & pcd_map_ptr, + const lanelet::LaneletMapPtr & lanelet_map_ptr) { std::unordered_set done; double search_radius2d = 0.5; @@ -107,11 +108,11 @@ void adjustHeight( continue; } pcl::PointXYZ pcl_pt; - pcl_pt.x = pt.x(); - pcl_pt.y = pt.y(); - pcl_pt.z = pt.z(); + pcl_pt.x = static_cast(pt.x()); + pcl_pt.y = static_cast(pt.y()); + pcl_pt.z = static_cast(pt.z()); double min_height = - getMinHeightAroundPoint(pcd_map_ptr, kdtree, pcl_pt, search_radius3d, search_radius2d); + get_min_height_around_point(pcd_map_ptr, kdtree, pcl_pt, search_radius3d, search_radius2d); std::cout << "moving from " << pt.z() << " to " << min_height << std::endl; pt.z() = min_height; done.insert(pt.id()); @@ -121,11 +122,11 @@ void adjustHeight( continue; } pcl::PointXYZ pcl_pt; - pcl_pt.x = pt.x(); - pcl_pt.y = pt.y(); - pcl_pt.z = pt.z(); + pcl_pt.x = static_cast(pt.x()); + pcl_pt.y = static_cast(pt.y()); + pcl_pt.z = static_cast(pt.z()); double min_height = - getMinHeightAroundPoint(pcd_map_ptr, kdtree, pcl_pt, search_radius3d, search_radius2d); + get_min_height_around_point(pcd_map_ptr, kdtree, pcl_pt, search_radius3d, search_radius2d); std::cout << "moving from " << pt.z() << " to " << min_height << std::endl; pt.z() = min_height; done.insert(pt.id()); @@ -148,14 +149,14 @@ int main(int argc, char * argv[]) pcl::PointCloud::Ptr pcd_map_ptr(new pcl::PointCloud); - if (!loadLaneletMap(llt_map_path, llt_map_ptr, projector)) { + if (!load_lanelet_map(llt_map_path, llt_map_ptr, projector)) { return EXIT_FAILURE; } - if (!loadPCDMap(pcd_map_path, pcd_map_ptr)) { + if (!load_pcd_map(pcd_map_path, pcd_map_ptr)) { return EXIT_FAILURE; } - adjustHeight(pcd_map_ptr, llt_map_ptr); + adjust_height(pcd_map_ptr, llt_map_ptr); lanelet::write(llt_output_path, *llt_map_ptr, projector); rclcpp::shutdown(); diff --git a/map/util/lanelet2_map_preprocessor/src/merge_close_lines.cpp b/map/util/lanelet2_map_preprocessor/src/merge_close_lines.cpp index e0d071ea11dc1..0e40d04a2cec3 100644 --- a/map/util/lanelet2_map_preprocessor/src/merge_close_lines.cpp +++ b/map/util/lanelet2_map_preprocessor/src/merge_close_lines.cpp @@ -26,10 +26,7 @@ #include #include -using lanelet::utils::getId; -using lanelet::utils::to2D; - -bool loadLaneletMap( +bool load_lanelet_map( const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr, lanelet::Projector & projector) { @@ -47,21 +44,17 @@ bool loadLaneletMap( return true; } -bool exists(std::unordered_set & set, lanelet::Id element) -{ - return set.find(element) != set.end(); -} - -lanelet::LineStrings3d convertLineLayerToLineStrings(lanelet::LaneletMapPtr & lanelet_map_ptr) +lanelet::LineStrings3d convert_line_layer_to_line_strings( + const lanelet::LaneletMapPtr & lanelet_map_ptr) { lanelet::LineStrings3d lines; - for (const lanelet::LineString3d & line : lanelet_map_ptr->lineStringLayer) { - lines.push_back(line); - } + std::copy( + lanelet_map_ptr->lineStringLayer.begin(), lanelet_map_ptr->lineStringLayer.end(), + std::back_inserter(lines)); return lines; } -lanelet::ConstPoint3d get3DPointFrom2DArcLength( +lanelet::ConstPoint3d get3d_point_from2d_arc_length( const lanelet::ConstLineString3d & line, const double s) { double accumulated_distance2d = 0; @@ -71,27 +64,23 @@ lanelet::ConstPoint3d get3DPointFrom2DArcLength( auto prev_pt = line.front(); for (size_t i = 1; i < line.size(); i++) { const auto & pt = line[i]; - double distance2d = lanelet::geometry::distance2d(to2D(prev_pt), to2D(pt)); + double distance2d = + lanelet::geometry::distance2d(lanelet::utils::to2D(prev_pt), lanelet::utils::to2D(pt)); if (accumulated_distance2d + distance2d >= s) { double ratio = (s - accumulated_distance2d) / distance2d; auto interpolated_pt = prev_pt.basicPoint() * (1 - ratio) + pt.basicPoint() * ratio; std::cout << interpolated_pt << std::endl; - return lanelet::ConstPoint3d( - lanelet::utils::getId(), interpolated_pt.x(), interpolated_pt.y(), interpolated_pt.z()); + return lanelet::ConstPoint3d{ + lanelet::utils::getId(), interpolated_pt.x(), interpolated_pt.y(), interpolated_pt.z()}; } accumulated_distance2d += distance2d; prev_pt = pt; } RCLCPP_ERROR(rclcpp::get_logger("merge_close_lines"), "interpolation failed"); - return lanelet::ConstPoint3d(); -} - -double getLineLength(const lanelet::ConstLineString3d & line) -{ - return boost::geometry::length(line.basicLineString()); + return {}; } -bool areLinesSame( +bool are_lines_same( const lanelet::ConstLineString3d & line1, const lanelet::ConstLineString3d & line2) { bool same_ends = false; @@ -105,66 +94,63 @@ bool areLinesSame( return false; } - double sum_distance = 0; - for (const auto & pt : line1) { - sum_distance += boost::geometry::distance(pt.basicPoint(), line2); - } - for (const auto & pt : line2) { - sum_distance += boost::geometry::distance(pt.basicPoint(), line1); - } + double sum_distance = + std::accumulate(line1.begin(), line1.end(), 0.0, [&line2](double sum, const auto & pt) { + return sum + boost::geometry::distance(pt.basicPoint(), line2); + }); + sum_distance += + std::accumulate(line2.begin(), line2.end(), 0.0, [&line1](double sum, const auto & pt) { + return sum + boost::geometry::distance(pt.basicPoint(), line1); + }); - double avg_distance = sum_distance / (line1.size() + line2.size()); + double avg_distance = sum_distance / static_cast(line1.size() + line2.size()); std::cout << line1 << " " << line2 << " " << avg_distance << std::endl; - if (avg_distance < 1.0) { - return true; - } else { - return false; - } + return avg_distance < 1.0; } -lanelet::BasicPoint3d getClosestPointOnLine( +lanelet::BasicPoint3d get_closest_point_on_line( const lanelet::BasicPoint3d & search_point, const lanelet::ConstLineString3d & line) { - auto arc_coordinate = lanelet::geometry::toArcCoordinates(to2D(line), to2D(search_point)); + auto arc_coordinate = lanelet::geometry::toArcCoordinates( + lanelet::utils::to2D(line), lanelet::utils::to2D(search_point)); std::cout << arc_coordinate.length << " " << arc_coordinate.distance << std::endl; - return get3DPointFrom2DArcLength(line, arc_coordinate.length).basicPoint(); + return get3d_point_from2d_arc_length(line, arc_coordinate.length).basicPoint(); } -lanelet::LineString3d mergeTwoLines( +lanelet::LineString3d merge_two_lines( const lanelet::LineString3d & line1, const lanelet::ConstLineString3d & line2) { lanelet::Points3d new_points; for (const auto & p1 : line1) { - lanelet::BasicPoint3d p1_basic_point = p1.basicPoint(); - lanelet::BasicPoint3d p2_basic_point = getClosestPointOnLine(p1, line2); + const lanelet::BasicPoint3d & p1_basic_point = p1.basicPoint(); + lanelet::BasicPoint3d p2_basic_point = get_closest_point_on_line(p1, line2); lanelet::BasicPoint3d new_basic_point = (p1_basic_point + p2_basic_point) / 2; lanelet::Point3d new_point(lanelet::utils::getId(), new_basic_point); new_points.push_back(new_point); } - return lanelet::LineString3d(lanelet::utils::getId(), new_points); + return lanelet::LineString3d{lanelet::utils::getId(), new_points}; } -void copyData(lanelet::LineString3d & dst, lanelet::LineString3d & src) +void copy_data(lanelet::LineString3d & dst, const lanelet::LineString3d & src) { - lanelet::Points3d points; dst.clear(); - for (lanelet::Point3d & pt : src) { - dst.push_back(pt); + for (const lanelet::ConstPoint3d & pt : src) { + dst.push_back(static_cast(pt)); } } -void mergeLines(lanelet::LaneletMapPtr & lanelet_map_ptr) +void merge_lines(lanelet::LaneletMapPtr & lanelet_map_ptr) { - auto lines = convertLineLayerToLineStrings(lanelet_map_ptr); + auto lines = convert_line_layer_to_line_strings(lanelet_map_ptr); for (size_t i = 0; i < lines.size(); i++) { auto line_i = lines.at(i); for (size_t j = 0; j < i; j++) { auto line_j = lines.at(j); - if (areLinesSame(line_i, line_j)) { - auto merged_line = mergeTwoLines(line_i, line_j); - copyData(line_i, merged_line); - copyData(line_j, merged_line); + if (are_lines_same(line_i, line_j)) { + auto merged_line = merge_two_lines(line_i, line_j); + copy_data(line_i, merged_line); + copy_data(line_j, merged_line); line_i.setId(line_j.id()); std::cout << line_j << " " << line_i << std::endl; // lanelet_map_ptr->add(merged_line); @@ -189,11 +175,11 @@ int main(int argc, char * argv[]) lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap); lanelet::projection::MGRSProjector projector; - if (!loadLaneletMap(llt_map_path, llt_map_ptr, projector)) { + if (!load_lanelet_map(llt_map_path, llt_map_ptr, projector)) { return EXIT_FAILURE; } - mergeLines(llt_map_ptr); + merge_lines(llt_map_ptr); lanelet::write(output_path, *llt_map_ptr, projector); rclcpp::shutdown(); diff --git a/map/util/lanelet2_map_preprocessor/src/merge_close_points.cpp b/map/util/lanelet2_map_preprocessor/src/merge_close_points.cpp index e18a366003e10..3cbbffc702019 100644 --- a/map/util/lanelet2_map_preprocessor/src/merge_close_points.cpp +++ b/map/util/lanelet2_map_preprocessor/src/merge_close_points.cpp @@ -25,7 +25,7 @@ #include #include -bool loadLaneletMap( +bool load_lanelet_map( const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr, lanelet::Projector & projector) { @@ -43,17 +43,12 @@ bool loadLaneletMap( return true; } -bool exists(std::unordered_set & set, lanelet::Id element) -{ - return set.find(element) != set.end(); -} - -lanelet::Points3d convertPointsLayerToPoints(lanelet::LaneletMapPtr & lanelet_map_ptr) +lanelet::Points3d convert_points_layer_to_points(const lanelet::LaneletMapPtr & lanelet_map_ptr) { lanelet::Points3d points; - for (const lanelet::Point3d & pt : lanelet_map_ptr->pointLayer) { - points.push_back(pt); - } + std::copy( + lanelet_map_ptr->pointLayer.begin(), lanelet_map_ptr->pointLayer.end(), + std::back_inserter(points)); return points; } @@ -72,9 +67,9 @@ lanelet::Points3d convertPointsLayerToPoints(lanelet::LaneletMapPtr & lanelet_ma // return lanelet::LineString3d(lanelet::utils::getId(), new_points); // } -void mergePoints(lanelet::LaneletMapPtr & lanelet_map_ptr) +void merge_points(const lanelet::LaneletMapPtr & lanelet_map_ptr) { - const auto & points = convertPointsLayerToPoints(lanelet_map_ptr); + const auto & points = convert_points_layer_to_points(lanelet_map_ptr); for (size_t i = 0; i < points.size(); i++) { auto point_i = points.at(i); @@ -109,11 +104,11 @@ int main(int argc, char * argv[]) lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap); lanelet::projection::MGRSProjector projector; - if (!loadLaneletMap(llt_map_path, llt_map_ptr, projector)) { + if (!load_lanelet_map(llt_map_path, llt_map_ptr, projector)) { return EXIT_FAILURE; } - mergePoints(llt_map_ptr); + merge_points(llt_map_ptr); lanelet::write(output_path, *llt_map_ptr, projector); rclcpp::shutdown(); diff --git a/map/util/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp b/map/util/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp index 633dae4e20c1d..3ab10551e36f9 100644 --- a/map/util/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp +++ b/map/util/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp @@ -25,7 +25,7 @@ #include #include -bool loadLaneletMap( +bool load_lanelet_map( const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr, lanelet::Projector & projector) { @@ -43,28 +43,29 @@ bool loadLaneletMap( return true; } -lanelet::Points3d convertPointsLayerToPoints(lanelet::LaneletMapPtr & lanelet_map_ptr) +lanelet::Points3d convert_points_layer_to_points(const lanelet::LaneletMapPtr & lanelet_map_ptr) { lanelet::Points3d points; - for (const lanelet::Point3d & pt : lanelet_map_ptr->pointLayer) { - points.push_back(pt); - } + std::copy( + lanelet_map_ptr->pointLayer.begin(), lanelet_map_ptr->pointLayer.end(), + std::back_inserter(points)); return points; } -lanelet::LineStrings3d convertLineLayerToLineStrings(lanelet::LaneletMapPtr & lanelet_map_ptr) +lanelet::LineStrings3d convert_line_layer_to_line_strings( + const lanelet::LaneletMapPtr & lanelet_map_ptr) { lanelet::LineStrings3d lines; - for (const lanelet::LineString3d & line : lanelet_map_ptr->lineStringLayer) { - lines.push_back(line); - } + std::copy( + lanelet_map_ptr->lineStringLayer.begin(), lanelet_map_ptr->lineStringLayer.end(), + std::back_inserter(lines)); return lines; } -void removeUnreferencedGeometry(lanelet::LaneletMapPtr & lanelet_map_ptr) +void remove_unreferenced_geometry(lanelet::LaneletMapPtr & lanelet_map_ptr) { lanelet::LaneletMapPtr new_map(new lanelet::LaneletMap); - for (auto llt : lanelet_map_ptr->laneletLayer) { + for (const auto & llt : lanelet_map_ptr->laneletLayer) { new_map->add(llt); } lanelet_map_ptr = new_map; @@ -82,10 +83,10 @@ int main(int argc, char * argv[]) lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap); lanelet::projection::MGRSProjector projector; - if (!loadLaneletMap(llt_map_path, llt_map_ptr, projector)) { + if (!load_lanelet_map(llt_map_path, llt_map_ptr, projector)) { return EXIT_FAILURE; } - removeUnreferencedGeometry(llt_map_ptr); + remove_unreferenced_geometry(llt_map_ptr); lanelet::write(output_path, *llt_map_ptr, projector); rclcpp::shutdown(); diff --git a/map/util/lanelet2_map_preprocessor/src/transform_maps.cpp b/map/util/lanelet2_map_preprocessor/src/transform_maps.cpp index d29df716b11e7..d55e03c7faee5 100644 --- a/map/util/lanelet2_map_preprocessor/src/transform_maps.cpp +++ b/map/util/lanelet2_map_preprocessor/src/transform_maps.cpp @@ -27,7 +27,7 @@ #include #include -bool loadLaneletMap( +bool load_lanelet_map( const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr, lanelet::Projector & projector) { @@ -45,7 +45,8 @@ bool loadLaneletMap( return true; } -bool loadPCDMap(const std::string & pcd_map_path, pcl::PointCloud::Ptr & pcd_map_ptr) +bool load_pcd_map( + const std::string & pcd_map_path, pcl::PointCloud::Ptr & pcd_map_ptr) { if (pcl::io::loadPCDFile(pcd_map_path, *pcd_map_ptr) == -1) { //* load the file RCLCPP_ERROR_STREAM(rclcpp::get_logger("loadPCDMap"), "Couldn't read file: " << pcd_map_path); @@ -56,9 +57,9 @@ bool loadPCDMap(const std::string & pcd_map_path, pcl::PointCloud return true; } -void transformMaps( - pcl::PointCloud::Ptr & pcd_map_ptr, lanelet::LaneletMapPtr & lanelet_map_ptr, - const Eigen::Affine3d affine) +void transform_maps( + const pcl::PointCloud::Ptr & pcd_map_ptr, + const lanelet::LaneletMapPtr & lanelet_map_ptr, const Eigen::Affine3d & affine) { { for (lanelet::Point3d & pt : lanelet_map_ptr->pointLayer) { @@ -74,14 +75,14 @@ void transformMaps( for (auto & pt : pcd_map_ptr->points) { Eigen::Vector3d eigen_pt(pt.x, pt.y, pt.z); auto transformed_pt = affine * eigen_pt; - pt.x = transformed_pt.x(); - pt.y = transformed_pt.y(); - pt.z = transformed_pt.z(); + pt.x = static_cast(transformed_pt.x()); + pt.y = static_cast(transformed_pt.y()); + pt.z = static_cast(transformed_pt.z()); } } } -Eigen::Affine3d createAffineMatrixFromXYZRPY( +Eigen::Affine3d create_affine_matrix_from_xyzrpy( const double x, const double y, const double z, const double roll, const double pitch, const double yaw) { @@ -127,19 +128,19 @@ int main(int argc, char * argv[]) pcl::PointCloud::Ptr pcd_map_ptr(new pcl::PointCloud); - if (!loadLaneletMap(llt_map_path, llt_map_ptr, projector)) { + if (!load_lanelet_map(llt_map_path, llt_map_ptr, projector)) { return EXIT_FAILURE; } - if (!loadPCDMap(pcd_map_path, pcd_map_ptr)) { + if (!load_pcd_map(pcd_map_path, pcd_map_ptr)) { return EXIT_FAILURE; } - Eigen::Affine3d affine = createAffineMatrixFromXYZRPY(x, y, z, roll, pitch, yaw); + Eigen::Affine3d affine = create_affine_matrix_from_xyzrpy(x, y, z, roll, pitch, yaw); const auto mgrs_grid = node->declare_parameter("mgrs_grid", projector.getProjectedMGRSGrid()); std::cout << "using mgrs grid: " << mgrs_grid << std::endl; - transformMaps(pcd_map_ptr, llt_map_ptr, affine); + transform_maps(pcd_map_ptr, llt_map_ptr, affine); lanelet::write(llt_output_path, *llt_map_ptr, projector); pcl::io::savePCDFileBinary(pcd_output_path, *pcd_map_ptr); From deef7c4a484e62828e9e8ee2a3092db95fed9640 Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Mon, 8 Jul 2024 16:22:41 +0900 Subject: [PATCH 05/19] fix(pointcloud_preprocessor): fix the bug where the geometry message was not saved correctly. (#7886) * fix: fix bug that geometry message didn't save correctly Signed-off-by: vividf * chore: change some functions from public to protected Signed-off-by: vividf --------- Signed-off-by: vividf Co-authored-by: Kenzo Lobos Tsunekawa --- .../distortion_corrector.hpp | 36 +++++++++---------- .../distortion_corrector.cpp | 19 +++++----- 2 files changed, 25 insertions(+), 30 deletions(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index 6f372f0e74646..8a7b7c90113da 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -61,7 +61,8 @@ class DistortionCorrectorBase template class DistortionCorrector : public DistortionCorrectorBase { -public: +protected: + geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr_; bool pointcloud_transform_needed_{false}; bool pointcloud_transform_exists_{false}; bool imu_transform_exists_{false}; @@ -72,28 +73,12 @@ class DistortionCorrector : public DistortionCorrectorBase std::deque twist_queue_; std::deque angular_velocity_queue_; - explicit DistortionCorrector(rclcpp::Node * node) - : node_(node), tf_buffer_(node_->get_clock()), tf_listener_(tf_buffer_) - { - } - void processTwistMessage( - const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) override; - - void processIMUMessage( - const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) override; - void getIMUTransformation( - const std::string & base_frame, const std::string & imu_frame, - geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr); - void enqueueIMU( - const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg, - geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr); - - bool isInputValid(sensor_msgs::msg::PointCloud2 & pointcloud); + void getIMUTransformation(const std::string & base_frame, const std::string & imu_frame); + void enqueueIMU(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg); void getTwistAndIMUIterator( bool use_imu, double first_point_time_stamp_sec, std::deque::iterator & it_twist, std::deque::iterator & it_imu); - void undistortPointCloud(bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) override; void warnIfTimestampIsTooLate(bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_too_late); void undistortPoint( sensor_msgs::PointCloud2Iterator & it_x, sensor_msgs::PointCloud2Iterator & it_y, @@ -105,6 +90,19 @@ class DistortionCorrector : public DistortionCorrectorBase static_cast(this)->undistortPointImplementation( it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid); }; + +public: + explicit DistortionCorrector(rclcpp::Node * node) + : node_(node), tf_buffer_(node_->get_clock()), tf_listener_(tf_buffer_) + { + } + void processTwistMessage( + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) override; + + void processIMUMessage( + const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) override; + void undistortPointCloud(bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) override; + bool isInputValid(sensor_msgs::msg::PointCloud2 & pointcloud); }; class DistortionCorrector2D : public DistortionCorrector diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index 81c29a9f6202a..8c8752c7e278e 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -47,21 +47,20 @@ template void DistortionCorrector::processIMUMessage( const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) { - geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr = - std::make_shared(); - getIMUTransformation(base_frame, imu_msg->header.frame_id, geometry_imu_to_base_link_ptr); - enqueueIMU(imu_msg, geometry_imu_to_base_link_ptr); + getIMUTransformation(base_frame, imu_msg->header.frame_id); + enqueueIMU(imu_msg); } template void DistortionCorrector::getIMUTransformation( - const std::string & base_frame, const std::string & imu_frame, - geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr) + const std::string & base_frame, const std::string & imu_frame) { if (imu_transform_exists_) { return; } + geometry_imu_to_base_link_ptr_ = std::make_shared(); + tf2::Transform tf2_imu_to_base_link; if (base_frame == imu_frame) { tf2_imu_to_base_link.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); @@ -83,20 +82,18 @@ void DistortionCorrector::getIMUTransformation( } } - geometry_imu_to_base_link_ptr->transform.rotation = + geometry_imu_to_base_link_ptr_->transform.rotation = tf2::toMsg(tf2_imu_to_base_link.getRotation()); } template -void DistortionCorrector::enqueueIMU( - const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg, - geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr) +void DistortionCorrector::enqueueIMU(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) { geometry_msgs::msg::Vector3Stamped angular_velocity; angular_velocity.vector = imu_msg->angular_velocity; geometry_msgs::msg::Vector3Stamped transformed_angular_velocity; - tf2::doTransform(angular_velocity, transformed_angular_velocity, *geometry_imu_to_base_link_ptr); + tf2::doTransform(angular_velocity, transformed_angular_velocity, *geometry_imu_to_base_link_ptr_); transformed_angular_velocity.header = imu_msg->header; angular_velocity_queue_.push_back(transformed_angular_velocity); From 094f2e2a7ba4db09b591f526ef6d60df4937ff95 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 8 Jul 2024 16:30:07 +0900 Subject: [PATCH 06/19] refactor(multi_object_tracker)!: fix namespace and directory structure (#7863) * refactor: update include paths for debugger and processor modules Signed-off-by: Taekjin LEE * refactor: update include paths for debugger and processor modules Signed-off-by: Taekjin LEE * refactor: move include files into 'autoware' Signed-off-by: Taekjin LEE * refactor: set namespace Signed-off-by: Taekjin LEE * refactor: refine logics to pass cppcheck Signed-off-by: Taekjin LEE --------- Signed-off-by: Taekjin LEE --- .../multi_object_tracker/CMakeLists.txt | 44 ++++++++++--------- .../association/association.hpp} | 14 +++--- .../association}/solver/gnn_solver.hpp | 12 ++--- .../solver/gnn_solver_interface.hpp | 10 +++-- .../association/solver/mu_ssp.hpp} | 12 +++-- .../association/solver/ssp.hpp} | 12 +++-- .../tracker/model/bicycle_tracker.hpp | 17 ++++--- .../tracker/model/big_vehicle_tracker.hpp | 17 ++++--- .../model/multiple_vehicle_tracker.hpp | 17 ++++--- .../tracker/model/normal_vehicle_tracker.hpp | 17 ++++--- .../tracker/model/pass_through_tracker.hpp | 11 +++-- .../model/pedestrian_and_bicycle_tracker.hpp | 17 ++++--- .../tracker/model/pedestrian_tracker.hpp | 19 ++++---- .../tracker/model/tracker_base.hpp | 13 ++++-- .../tracker/model/unknown_tracker.hpp | 15 ++++--- .../motion_model/bicycle_motion_model.hpp | 13 ++++-- .../motion_model/ctrv_motion_model.hpp | 14 +++--- .../tracker/motion_model/cv_motion_model.hpp | 13 ++++-- .../motion_model/motion_model_base.hpp | 11 +++-- .../tracker/object_model/object_model.hpp | 10 +++-- .../multi_object_tracker/tracker/tracker.hpp | 6 +-- .../multi_object_tracker/utils/utils.hpp | 6 +-- .../association/association.cpp} | 11 +++-- .../mu_successive_shortest_path/mu_ssp.cpp} | 6 ++- .../successive_shortest_path/ssp.cpp} | 6 ++- .../tracker/model/bicycle_tracker.cpp | 9 +++- .../tracker/model/big_vehicle_tracker.cpp | 8 +++- .../model/multiple_vehicle_tracker.cpp | 7 ++- .../tracker/model/normal_vehicle_tracker.cpp | 9 +++- .../tracker/model/pass_through_tracker.cpp | 9 +++- .../model/pedestrian_and_bicycle_tracker.cpp | 7 ++- .../tracker/model/pedestrian_tracker.cpp | 9 +++- .../tracker/model/tracker_base.cpp | 9 +++- .../tracker/model/unknown_tracker.cpp | 9 +++- .../motion_model/bicycle_motion_model.cpp | 11 +++-- .../motion_model/ctrv_motion_model.cpp | 10 +++-- .../tracker/motion_model/cv_motion_model.cpp | 10 +++-- .../motion_model/motion_model_base.cpp | 7 ++- .../src/debugger/debug_object.cpp | 9 +++- .../debugger/debug_object.hpp | 13 ++++-- .../src/debugger/debugger.cpp | 6 ++- .../debugger/debugger.hpp | 13 ++++-- ...core.cpp => multi_object_tracker_node.cpp} | 19 ++++---- .../multi_object_tracker_node.hpp} | 22 +++++----- .../src/processor/input_manager.cpp | 32 ++++++++------ .../processor/input_manager.hpp | 10 ++--- .../src/processor/processor.cpp | 9 +++- .../processor/processor.hpp | 12 +++-- 48 files changed, 398 insertions(+), 204 deletions(-) rename perception/multi_object_tracker/include/{multi_object_tracker/data_association/data_association.hpp => autoware/multi_object_tracker/association/association.hpp} (81%) rename perception/multi_object_tracker/include/{multi_object_tracker/data_association => autoware/multi_object_tracker/association}/solver/gnn_solver.hpp (55%) rename perception/multi_object_tracker/include/{multi_object_tracker/data_association => autoware/multi_object_tracker/association}/solver/gnn_solver_interface.hpp (73%) rename perception/multi_object_tracker/include/{multi_object_tracker/data_association/solver/mu_successive_shortest_path.hpp => autoware/multi_object_tracker/association/solver/mu_ssp.hpp} (71%) rename perception/multi_object_tracker/include/{multi_object_tracker/data_association/solver/successive_shortest_path.hpp => autoware/multi_object_tracker/association/solver/ssp.hpp} (71%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/model/bicycle_tracker.hpp (78%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp (78%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp (71%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp (78%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/model/pass_through_tracker.hpp (81%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp (71%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/model/pedestrian_tracker.hpp (78%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/model/tracker_base.hpp (91%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/model/unknown_tracker.hpp (80%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp (87%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp (86%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp (84%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/motion_model/motion_model_base.hpp (85%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/object_model/object_model.hpp (96%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/tracker.hpp (84%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/utils/utils.hpp (98%) rename perception/multi_object_tracker/{src/data_association/data_association.cpp => lib/association/association.cpp} (96%) rename perception/multi_object_tracker/{src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp => lib/association/mu_successive_shortest_path/mu_ssp.cpp} (88%) rename perception/multi_object_tracker/{src/data_association/successive_shortest_path/successive_shortest_path.cpp => lib/association/successive_shortest_path/ssp.cpp} (98%) rename perception/multi_object_tracker/{src => lib}/tracker/model/bicycle_tracker.cpp (98%) rename perception/multi_object_tracker/{src => lib}/tracker/model/big_vehicle_tracker.cpp (98%) rename perception/multi_object_tracker/{src => lib}/tracker/model/multiple_vehicle_tracker.cpp (93%) rename perception/multi_object_tracker/{src => lib}/tracker/model/normal_vehicle_tracker.cpp (98%) rename perception/multi_object_tracker/{src => lib}/tracker/model/pass_through_tracker.cpp (95%) rename perception/multi_object_tracker/{src => lib}/tracker/model/pedestrian_and_bicycle_tracker.cpp (93%) rename perception/multi_object_tracker/{src => lib}/tracker/model/pedestrian_tracker.cpp (98%) rename perception/multi_object_tracker/{src => lib}/tracker/model/tracker_base.cpp (96%) rename perception/multi_object_tracker/{src => lib}/tracker/model/unknown_tracker.cpp (97%) rename perception/multi_object_tracker/{src => lib}/tracker/motion_model/bicycle_motion_model.cpp (98%) rename perception/multi_object_tracker/{src => lib}/tracker/motion_model/ctrv_motion_model.cpp (97%) rename perception/multi_object_tracker/{src => lib}/tracker/motion_model/cv_motion_model.cpp (97%) rename perception/multi_object_tracker/{src => lib}/tracker/motion_model/motion_model_base.cpp (93%) rename perception/multi_object_tracker/{include/multi_object_tracker => src}/debugger/debug_object.hpp (91%) rename perception/multi_object_tracker/{include/multi_object_tracker => src}/debugger/debugger.hpp (94%) rename perception/multi_object_tracker/src/{multi_object_tracker_core.cpp => multi_object_tracker_node.cpp} (96%) rename perception/multi_object_tracker/{include/multi_object_tracker/multi_object_tracker_core.hpp => src/multi_object_tracker_node.hpp} (82%) rename perception/multi_object_tracker/{include/multi_object_tracker => src}/processor/input_manager.hpp (95%) rename perception/multi_object_tracker/{include/multi_object_tracker => src}/processor/processor.hpp (91%) diff --git a/perception/multi_object_tracker/CMakeLists.txt b/perception/multi_object_tracker/CMakeLists.txt index 4f9268583dd34..370e5bb0b9161 100644 --- a/perception/multi_object_tracker/CMakeLists.txt +++ b/perception/multi_object_tracker/CMakeLists.txt @@ -20,32 +20,34 @@ include_directories( ) # Generate exe file -set(MULTI_OBJECT_TRACKER_SRC - src/multi_object_tracker_core.cpp +set(${PROJECT_NAME}_src + src/multi_object_tracker_node.cpp src/debugger/debugger.cpp src/debugger/debug_object.cpp src/processor/processor.cpp src/processor/input_manager.cpp - src/data_association/data_association.cpp - src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp - src/tracker/motion_model/motion_model_base.cpp - src/tracker/motion_model/bicycle_motion_model.cpp +) +set(${PROJECT_NAME}_lib + lib/association/association.cpp + lib/association/mu_successive_shortest_path/mu_ssp.cpp + lib/tracker/motion_model/motion_model_base.cpp + lib/tracker/motion_model/bicycle_motion_model.cpp # cspell: ignore ctrv - src/tracker/motion_model/ctrv_motion_model.cpp - src/tracker/motion_model/cv_motion_model.cpp - src/tracker/model/tracker_base.cpp - src/tracker/model/big_vehicle_tracker.cpp - src/tracker/model/normal_vehicle_tracker.cpp - src/tracker/model/multiple_vehicle_tracker.cpp - src/tracker/model/bicycle_tracker.cpp - src/tracker/model/pedestrian_tracker.cpp - src/tracker/model/pedestrian_and_bicycle_tracker.cpp - src/tracker/model/unknown_tracker.cpp - src/tracker/model/pass_through_tracker.cpp + lib/tracker/motion_model/ctrv_motion_model.cpp + lib/tracker/motion_model/cv_motion_model.cpp + lib/tracker/model/tracker_base.cpp + lib/tracker/model/big_vehicle_tracker.cpp + lib/tracker/model/normal_vehicle_tracker.cpp + lib/tracker/model/multiple_vehicle_tracker.cpp + lib/tracker/model/bicycle_tracker.cpp + lib/tracker/model/pedestrian_tracker.cpp + lib/tracker/model/pedestrian_and_bicycle_tracker.cpp + lib/tracker/model/unknown_tracker.cpp + lib/tracker/model/pass_through_tracker.cpp ) - ament_auto_add_library(${PROJECT_NAME} SHARED - ${MULTI_OBJECT_TRACKER_SRC} + ${${PROJECT_NAME}_src} + ${${PROJECT_NAME}_lib} ) target_link_libraries(${PROJECT_NAME} @@ -54,8 +56,8 @@ target_link_libraries(${PROJECT_NAME} ) rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "multi_object_tracker::MultiObjectTracker" - EXECUTABLE ${PROJECT_NAME}_node + PLUGIN "autoware::multi_object_tracker::MultiObjectTracker" + EXECUTABLE multi_object_tracker_node ) ament_auto_package(INSTALL_TO_SHARE diff --git a/perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp similarity index 81% rename from perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp index c30423db02c26..2c12341d0aa67 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp @@ -16,13 +16,13 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ -#define MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__ASSOCIATION_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__ASSOCIATION_HPP_ #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/data_association/solver/gnn_solver.hpp" -#include "multi_object_tracker/tracker/tracker.hpp" +#include "autoware/multi_object_tracker/association/solver/gnn_solver.hpp" +#include "autoware/multi_object_tracker/tracker/tracker.hpp" #include #include @@ -34,6 +34,8 @@ #include #include +namespace autoware::multi_object_tracker +{ class DataAssociation { private: @@ -61,4 +63,6 @@ class DataAssociation virtual ~DataAssociation() {} }; -#endif // MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__ASSOCIATION_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/gnn_solver.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/gnn_solver.hpp similarity index 55% rename from perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/gnn_solver.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/gnn_solver.hpp index 631fd73f8583d..3b72b800e86bc 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/gnn_solver.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/gnn_solver.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ -#define MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ -#include "multi_object_tracker/data_association/solver/gnn_solver_interface.hpp" -#include "multi_object_tracker/data_association/solver/mu_successive_shortest_path.hpp" -#include "multi_object_tracker/data_association/solver/successive_shortest_path.hpp" +#include "autoware/multi_object_tracker/association/solver/gnn_solver_interface.hpp" +#include "autoware/multi_object_tracker/association/solver/mu_ssp.hpp" +#include "autoware/multi_object_tracker/association/solver/ssp.hpp" -#endif // MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/gnn_solver_interface.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/gnn_solver_interface.hpp similarity index 73% rename from perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/gnn_solver_interface.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/gnn_solver_interface.hpp index 1fb508bcf12b2..88f9181d96d9b 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/gnn_solver_interface.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/gnn_solver_interface.hpp @@ -12,12 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ -#define MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ #include #include +namespace autoware::multi_object_tracker +{ namespace gnn_solver { class GnnSolverInterface @@ -30,6 +32,8 @@ class GnnSolverInterface const std::vector> & cost, std::unordered_map * direct_assignment, std::unordered_map * reverse_assignment) = 0; }; + } // namespace gnn_solver +} // namespace autoware::multi_object_tracker -#endif // MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/mu_successive_shortest_path.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/mu_ssp.hpp similarity index 71% rename from perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/mu_successive_shortest_path.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/mu_ssp.hpp index a4d9d727e6cc5..024670c3389c6 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/mu_successive_shortest_path.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/mu_ssp.hpp @@ -12,14 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ -#define MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__MU_SSP_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__MU_SSP_HPP_ -#include "multi_object_tracker/data_association/solver/gnn_solver_interface.hpp" +#include "autoware/multi_object_tracker/association/solver/gnn_solver_interface.hpp" #include #include +namespace autoware::multi_object_tracker +{ namespace gnn_solver { class MuSSP : public GnnSolverInterface @@ -32,6 +34,8 @@ class MuSSP : public GnnSolverInterface const std::vector> & cost, std::unordered_map * direct_assignment, std::unordered_map * reverse_assignment) override; }; + } // namespace gnn_solver +} // namespace autoware::multi_object_tracker -#endif // MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__MU_SSP_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/successive_shortest_path.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/ssp.hpp similarity index 71% rename from perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/successive_shortest_path.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/ssp.hpp index 1c4b15cd68b59..0f987fc49ee7c 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/successive_shortest_path.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/ssp.hpp @@ -12,14 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ -#define MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__SSP_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__SSP_HPP_ -#include "multi_object_tracker/data_association/solver/gnn_solver_interface.hpp" +#include "autoware/multi_object_tracker/association/solver/gnn_solver_interface.hpp" #include #include +namespace autoware::multi_object_tracker +{ namespace gnn_solver { class SSP : public GnnSolverInterface @@ -32,6 +34,8 @@ class SSP : public GnnSolverInterface const std::vector> & cost, std::unordered_map * direct_assignment, std::unordered_map * reverse_assignment) override; }; + } // namespace gnn_solver +} // namespace autoware::multi_object_tracker -#endif // MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__SSP_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp similarity index 78% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp index 63ad496b70ed9..5374e28d5f9cf 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -16,13 +16,16 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/object_model/object_model.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/model/tracker_base.hpp" -#include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" -#include "multi_object_tracker/tracker/object_model/object_model.hpp" + +namespace autoware::multi_object_tracker +{ class BicycleTracker : public Tracker { @@ -67,4 +70,6 @@ class BicycleTracker : public Tracker const geometry_msgs::msg::Transform & self_transform) const; }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp similarity index 78% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp index 7112e6a08ade1..c3f824aff35b4 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp @@ -16,13 +16,16 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/object_model/object_model.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/model/tracker_base.hpp" -#include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" -#include "multi_object_tracker/tracker/object_model/object_model.hpp" + +namespace autoware::multi_object_tracker +{ class BigVehicleTracker : public Tracker { @@ -70,4 +73,6 @@ class BigVehicleTracker : public Tracker const geometry_msgs::msg::Transform & self_transform); }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp similarity index 71% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp index 75aec0b06d6b8..a0a6bd7781761 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp @@ -16,16 +16,19 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ +#include "autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/model/big_vehicle_tracker.hpp" -#include "multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp" -#include "multi_object_tracker/tracker/model/tracker_base.hpp" #include +namespace autoware::multi_object_tracker +{ + class MultipleVehicleTracker : public Tracker { private: @@ -48,4 +51,6 @@ class MultipleVehicleTracker : public Tracker virtual ~MultipleVehicleTracker() {} }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp similarity index 78% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index 1165cecab258b..f7edebfc31378 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp @@ -16,13 +16,16 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/object_model/object_model.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/model/tracker_base.hpp" -#include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" -#include "multi_object_tracker/tracker/object_model/object_model.hpp" + +namespace autoware::multi_object_tracker +{ class NormalVehicleTracker : public Tracker { @@ -70,4 +73,6 @@ class NormalVehicleTracker : public Tracker const geometry_msgs::msg::Transform & self_transform); }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp similarity index 81% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp index cd661dab52c6e..e7f0a5fd699e1 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp @@ -16,12 +16,15 @@ // Author: v1.0 Yutaka Shimizu // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ #include "kalman_filter/kalman_filter.hpp" #include "tracker_base.hpp" +namespace autoware::multi_object_tracker +{ + class PassThroughTracker : public Tracker { private: @@ -44,4 +47,6 @@ class PassThroughTracker : public Tracker autoware_perception_msgs::msg::TrackedObject & object) const override; }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp similarity index 71% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp index 3c3ac038b085e..f593280c2e183 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp @@ -16,13 +16,16 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ +#include "autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/model/bicycle_tracker.hpp" -#include "multi_object_tracker/tracker/model/pedestrian_tracker.hpp" -#include "multi_object_tracker/tracker/model/tracker_base.hpp" + +namespace autoware::multi_object_tracker +{ class PedestrianAndBicycleTracker : public Tracker { @@ -46,4 +49,6 @@ class PedestrianAndBicycleTracker : public Tracker virtual ~PedestrianAndBicycleTracker() {} }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp similarity index 78% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp index c5be57f656eb5..1e0f778a69137 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp @@ -16,15 +16,16 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/object_model/object_model.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/model/tracker_base.hpp" -#include "multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" -#include "multi_object_tracker/tracker/object_model/object_model.hpp" -// cspell: ignore CTRV +namespace autoware::multi_object_tracker +{ class PedestrianTracker : public Tracker { @@ -49,7 +50,7 @@ class PedestrianTracker : public Tracker }; BoundingBox bounding_box_; Cylinder cylinder_; - + // cspell: ignore CTRV CTRVMotionModel motion_model_; using IDX = CTRVMotionModel::IDX; @@ -75,4 +76,6 @@ class PedestrianTracker : public Tracker const geometry_msgs::msg::Transform & self_transform) const; }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp similarity index 91% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp index 44b3884c392e6..bc52e49f763fd 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp @@ -16,11 +16,11 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/utils/utils.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include @@ -33,6 +33,9 @@ #include +namespace autoware::multi_object_tracker +{ + class Tracker { private: @@ -114,4 +117,6 @@ class Tracker virtual bool predict(const rclcpp::Time & time) = 0; }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp similarity index 80% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp index ca8ecba160bd8..4bc03f439ffc2 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp @@ -16,12 +16,15 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/model/tracker_base.hpp" -#include "multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" + +namespace autoware::multi_object_tracker +{ class UnknownTracker : public Tracker { @@ -62,4 +65,6 @@ class UnknownTracker : public Tracker autoware_perception_msgs::msg::TrackedObject & object) const override; }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp similarity index 87% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp index f6ce2842388c6..6e20d31aad168 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp @@ -16,11 +16,11 @@ // Author: v1.0 Taekjin Lee // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ +#include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include #include @@ -32,6 +32,9 @@ #endif #include +namespace autoware::multi_object_tracker +{ + class BicycleMotionModel : public MotionModel { private: @@ -107,4 +110,6 @@ class BicycleMotionModel : public MotionModel geometry_msgs::msg::Twist & twist, std::array & twist_cov) const override; }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp similarity index 86% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp index 657048079c46c..3cca786d9f65b 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp @@ -16,11 +16,11 @@ // Author: v1.0 Taekjin Lee // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ +#include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include #include @@ -32,8 +32,10 @@ #endif #include -// cspell: ignore CTRV +namespace autoware::multi_object_tracker +{ +// cspell: ignore CTRV class CTRVMotionModel : public MotionModel { private: @@ -92,4 +94,6 @@ class CTRVMotionModel : public MotionModel geometry_msgs::msg::Twist & twist, std::array & twist_cov) const override; }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp similarity index 84% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp index 421e413d7aab7..ad3061285a80c 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp @@ -16,11 +16,11 @@ // Author: v1.0 Taekjin Lee // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ +#include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include #include @@ -32,6 +32,9 @@ #endif #include +namespace autoware::multi_object_tracker +{ + class CVMotionModel : public MotionModel { private: @@ -85,4 +88,6 @@ class CVMotionModel : public MotionModel geometry_msgs::msg::Twist & twist, std::array & twist_cov) const override; }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/motion_model_base.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp similarity index 85% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/motion_model_base.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp index e740612e96d4f..130053fafd2ed 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/motion_model_base.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp @@ -16,8 +16,8 @@ // Author: v1.0 Taekjin Lee // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ #include "kalman_filter/kalman_filter.hpp" @@ -31,6 +31,9 @@ #endif #include +namespace autoware::multi_object_tracker +{ + class MotionModel { private: @@ -65,4 +68,6 @@ class MotionModel geometry_msgs::msg::Twist & twist, std::array & twist_cov) const = 0; }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/object_model/object_model.hpp similarity index 96% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/object_model/object_model.hpp index 6a771344bb36b..b49464109eec8 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/object_model/object_model.hpp @@ -16,8 +16,8 @@ // Author: v1.0 Taekjin Lee // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ #include @@ -46,6 +46,9 @@ constexpr T kmph2mps(const T kmph) } // namespace +namespace autoware::multi_object_tracker +{ + namespace object_model { @@ -301,5 +304,6 @@ static const ObjectModel bicycle(ObjectModelType::Bicycle); static const ObjectModel pedestrian(ObjectModelType::Pedestrian); } // namespace object_model +} // namespace autoware::multi_object_tracker -#endif // MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/tracker.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/tracker.hpp similarity index 84% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/tracker.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/tracker.hpp index a8bc58e254fc2..ea1e60d4c7e17 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/tracker.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/tracker.hpp @@ -16,8 +16,8 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ #include "model/bicycle_tracker.hpp" #include "model/big_vehicle_tracker.hpp" @@ -29,4 +29,4 @@ #include "model/tracker_base.hpp" #include "model/unknown_tracker.hpp" -#endif // MULTI_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp similarity index 98% rename from perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp index 0bda7870ae2b9..833f768e171f4 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp @@ -16,8 +16,8 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ -#define MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ #include #include @@ -276,4 +276,4 @@ inline bool getMeasurementYaw( } // namespace utils -#endif // MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ diff --git a/perception/multi_object_tracker/src/data_association/data_association.cpp b/perception/multi_object_tracker/lib/association/association.cpp similarity index 96% rename from perception/multi_object_tracker/src/data_association/data_association.cpp rename to perception/multi_object_tracker/lib/association/association.cpp index f7b40cd178528..ac31cad4cc13c 100644 --- a/perception/multi_object_tracker/src/data_association/data_association.cpp +++ b/perception/multi_object_tracker/lib/association/association.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "multi_object_tracker/data_association/data_association.hpp" +#include "autoware/multi_object_tracker/association/association.hpp" -#include "multi_object_tracker/data_association/solver/gnn_solver.hpp" -#include "multi_object_tracker/utils/utils.hpp" +#include "autoware/multi_object_tracker/association/solver/gnn_solver.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include @@ -68,6 +68,9 @@ double getFormedYawAngle( } } // namespace +namespace autoware::multi_object_tracker +{ + DataAssociation::DataAssociation( std::vector can_assign_vector, std::vector max_dist_vector, std::vector max_area_vector, std::vector min_area_vector, @@ -224,3 +227,5 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( return score_matrix; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp b/perception/multi_object_tracker/lib/association/mu_successive_shortest_path/mu_ssp.cpp similarity index 88% rename from perception/multi_object_tracker/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp rename to perception/multi_object_tracker/lib/association/mu_successive_shortest_path/mu_ssp.cpp index 18779a7fffbdf..9ae6e300b2b9b 100644 --- a/perception/multi_object_tracker/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp +++ b/perception/multi_object_tracker/lib/association/mu_successive_shortest_path/mu_ssp.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "multi_object_tracker/data_association/solver/mu_successive_shortest_path.hpp" +#include "autoware/multi_object_tracker/association/solver/mu_ssp.hpp" #include @@ -24,6 +24,8 @@ #include #include +namespace autoware::multi_object_tracker +{ namespace gnn_solver { void MuSSP::maximizeLinearAssignment( @@ -39,3 +41,5 @@ void MuSSP::maximizeLinearAssignment( solve_muSSP(cost, direct_assignment, reverse_assignment); } } // namespace gnn_solver + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/data_association/successive_shortest_path/successive_shortest_path.cpp b/perception/multi_object_tracker/lib/association/successive_shortest_path/ssp.cpp similarity index 98% rename from perception/multi_object_tracker/src/data_association/successive_shortest_path/successive_shortest_path.cpp rename to perception/multi_object_tracker/lib/association/successive_shortest_path/ssp.cpp index 0b52c09cb54b8..d20992c7d183b 100644 --- a/perception/multi_object_tracker/src/data_association/successive_shortest_path/successive_shortest_path.cpp +++ b/perception/multi_object_tracker/lib/association/successive_shortest_path/ssp.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "multi_object_tracker/data_association/solver/successive_shortest_path.hpp" +#include "autoware/multi_object_tracker/association/solver/ssp.hpp" #include #include @@ -22,6 +22,8 @@ #include #include +namespace autoware::multi_object_tracker +{ namespace gnn_solver { struct ResidualEdge @@ -368,3 +370,5 @@ void SSP::maximizeLinearAssignment( #endif } } // namespace gnn_solver + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp similarity index 98% rename from perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp rename to perception/multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp index a438c830596d7..ba53ccec1ad43 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp @@ -17,13 +17,13 @@ // #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/bicycle_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include @@ -40,6 +40,9 @@ #include #endif +namespace autoware::multi_object_tracker +{ + using Label = autoware_perception_msgs::msg::ObjectClassification; BicycleTracker::BicycleTracker( @@ -334,3 +337,5 @@ bool BicycleTracker::getTrackedObject( return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/lib/tracker/model/big_vehicle_tracker.cpp similarity index 98% rename from perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp rename to perception/multi_object_tracker/lib/tracker/model/big_vehicle_tracker.cpp index 90d33e65b46bd..0593b7fc9dc12 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/lib/tracker/model/big_vehicle_tracker.cpp @@ -17,13 +17,13 @@ // #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/big_vehicle_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include @@ -40,6 +40,8 @@ #include #endif +namespace autoware::multi_object_tracker +{ using Label = autoware_perception_msgs::msg::ObjectClassification; BigVehicleTracker::BigVehicleTracker( @@ -398,3 +400,5 @@ bool BigVehicleTracker::getTrackedObject( return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp b/perception/multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp similarity index 93% rename from perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp rename to perception/multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp index 67445e5b0daa2..a6e5021e4fe7d 100644 --- a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp @@ -16,7 +16,10 @@ // Author: v1.0 Yukihiro Saito // -#include "multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp" + +namespace autoware::multi_object_tracker +{ using Label = autoware_perception_msgs::msg::ObjectClassification; @@ -65,3 +68,5 @@ bool MultipleVehicleTracker::getTrackedObject( object.classification = getClassification(); return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/lib/tracker/model/normal_vehicle_tracker.cpp similarity index 98% rename from perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp rename to perception/multi_object_tracker/lib/tracker/model/normal_vehicle_tracker.cpp index 67282893083c1..ef345692b32ca 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/lib/tracker/model/normal_vehicle_tracker.cpp @@ -17,13 +17,13 @@ // #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include @@ -40,6 +40,9 @@ #include #endif +namespace autoware::multi_object_tracker +{ + using Label = autoware_perception_msgs::msg::ObjectClassification; NormalVehicleTracker::NormalVehicleTracker( @@ -399,3 +402,5 @@ bool NormalVehicleTracker::getTrackedObject( return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp b/perception/multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp similarity index 95% rename from perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp rename to perception/multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp index 17be415480360..c872944fab3d7 100644 --- a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp +++ b/perception/multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp @@ -16,10 +16,10 @@ // Author: v1.0 Yutaka Shimizu // #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/pass_through_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include @@ -36,6 +36,9 @@ #include #endif +namespace autoware::multi_object_tracker +{ + PassThroughTracker::PassThroughTracker( const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, @@ -117,3 +120,5 @@ bool PassThroughTracker::getTrackedObject( return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp b/perception/multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp similarity index 93% rename from perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp rename to perception/multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp index 00609934990a3..8c665e6078a2b 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp +++ b/perception/multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp @@ -16,7 +16,10 @@ // Author: v1.0 Yukihiro Saito // -#include "multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp" + +namespace autoware::multi_object_tracker +{ using Label = autoware_perception_msgs::msg::ObjectClassification; @@ -65,3 +68,5 @@ bool PedestrianAndBicycleTracker::getTrackedObject( object.classification = getClassification(); return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp similarity index 98% rename from perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp rename to perception/multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp index d9412c3b5739c..ee50c2e5449ed 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp @@ -17,13 +17,13 @@ // #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/pedestrian_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include @@ -40,6 +40,9 @@ #include #endif +namespace autoware::multi_object_tracker +{ + using Label = autoware_perception_msgs::msg::ObjectClassification; PedestrianTracker::PedestrianTracker( @@ -338,3 +341,5 @@ bool PedestrianTracker::getTrackedObject( return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp b/perception/multi_object_tracker/lib/tracker/model/tracker_base.cpp similarity index 96% rename from perception/multi_object_tracker/src/tracker/model/tracker_base.cpp rename to perception/multi_object_tracker/lib/tracker/model/tracker_base.cpp index f27bad172a4b9..daa8db5db91e6 100644 --- a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp +++ b/perception/multi_object_tracker/lib/tracker/model/tracker_base.cpp @@ -14,9 +14,9 @@ // // -#include "multi_object_tracker/tracker/model/tracker_base.hpp" +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" -#include "multi_object_tracker/utils/utils.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include #include @@ -40,6 +40,9 @@ float decayProbability(const float & prior, const float & delta_time) } } // namespace +namespace autoware::multi_object_tracker +{ + Tracker::Tracker( const rclcpp::Time & time, const std::vector & classification, @@ -202,3 +205,5 @@ geometry_msgs::msg::PoseWithCovariance Tracker::getPoseWithCovariance( getTrackedObject(time, object); return object.kinematics.pose_with_covariance; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp b/perception/multi_object_tracker/lib/tracker/model/unknown_tracker.cpp similarity index 97% rename from perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp rename to perception/multi_object_tracker/lib/tracker/model/unknown_tracker.cpp index 6905d7c3b8ced..0648007e0b807 100644 --- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp +++ b/perception/multi_object_tracker/lib/tracker/model/unknown_tracker.cpp @@ -13,13 +13,13 @@ // limitations under the License. #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/unknown_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include #include @@ -36,6 +36,9 @@ #endif #include "object_recognition_utils/object_recognition_utils.hpp" +namespace autoware::multi_object_tracker +{ + UnknownTracker::UnknownTracker( const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, @@ -234,3 +237,5 @@ bool UnknownTracker::getTrackedObject( return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp b/perception/multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp similarity index 98% rename from perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp rename to perception/multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp index 466108a2bef0e..2e325d18579a6 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp +++ b/perception/multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp @@ -17,17 +17,20 @@ // #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include #include +namespace autoware::multi_object_tracker +{ + // cspell: ignore CTRV // Bicycle CTRV motion model // CTRV : Constant Turn Rate and constant Velocity @@ -487,3 +490,5 @@ bool BicycleMotionModel::getPredictedState( return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp b/perception/multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp similarity index 97% rename from perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp rename to perception/multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp index b19af1d26d162..b1c5a36f9ad5b 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp +++ b/perception/multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp @@ -17,17 +17,19 @@ // #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include #include +namespace autoware::multi_object_tracker +{ // cspell: ignore CTRV // Constant Turn Rate and constant Velocity (CTRV) motion model using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; @@ -386,3 +388,5 @@ bool CTRVMotionModel::getPredictedState( return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp b/perception/multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp similarity index 97% rename from perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp rename to perception/multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp index 231ba73796ed9..2e2ba660a6e0d 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp +++ b/perception/multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp @@ -17,19 +17,21 @@ // #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include #include #include +namespace autoware::multi_object_tracker +{ // cspell: ignore CV // Constant Velocity (CV) motion model using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; @@ -306,3 +308,5 @@ bool CVMotionModel::getPredictedState( return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp b/perception/multi_object_tracker/lib/tracker/motion_model/motion_model_base.cpp similarity index 93% rename from perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp rename to perception/multi_object_tracker/lib/tracker/motion_model/motion_model_base.cpp index 4d51021de634b..ffbb452815fa7 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp +++ b/perception/multi_object_tracker/lib/tracker/motion_model/motion_model_base.cpp @@ -16,7 +16,10 @@ // Author: v1.0 Taekjin Lee // -#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" + +namespace autoware::multi_object_tracker +{ MotionModel::MotionModel() : last_update_time_(rclcpp::Time(0, 0)) { @@ -90,3 +93,5 @@ bool MotionModel::getPredictedState( tmp_ekf_for_no_update.getP(P); return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/debugger/debug_object.cpp b/perception/multi_object_tracker/src/debugger/debug_object.cpp index fe6d3d5c75564..58b7dfc5cef48 100644 --- a/perception/multi_object_tracker/src/debugger/debug_object.cpp +++ b/perception/multi_object_tracker/src/debugger/debug_object.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "multi_object_tracker/debugger/debug_object.hpp" +#include "debug_object.hpp" #include "autoware_perception_msgs/msg/tracked_object.hpp" @@ -48,6 +48,9 @@ int32_t uuidToInt(const boost::uuids::uuid & uuid) } } // namespace +namespace autoware::multi_object_tracker +{ + TrackerObjectDebugger::TrackerObjectDebugger(std::string frame_id) { // set frame id @@ -79,7 +82,7 @@ void TrackerObjectDebugger::collect( const std::unordered_map & direct_assignment, const std::unordered_map & /*reverse_assignment*/) { - if (!is_initialized_) is_initialized_ = true; + is_initialized_ = true; message_time_ = message_time; @@ -391,3 +394,5 @@ void TrackerObjectDebugger::getMessage(visualization_msgs::msg::MarkerArray & ma return; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp b/perception/multi_object_tracker/src/debugger/debug_object.hpp similarity index 91% rename from perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp rename to perception/multi_object_tracker/src/debugger/debug_object.hpp index 08c9eb975d8e6..ed4c556959861 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp +++ b/perception/multi_object_tracker/src/debugger/debug_object.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_ -#define MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_ +#ifndef DEBUGGER__DEBUG_OBJECT_HPP_ +#define DEBUGGER__DEBUG_OBJECT_HPP_ +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/universe_utils/ros/uuid_helper.hpp" -#include "multi_object_tracker/tracker/model/tracker_base.hpp" #include @@ -37,6 +37,9 @@ #include #include +namespace autoware::multi_object_tracker +{ + struct ObjectData { rclcpp::Time time; @@ -99,4 +102,6 @@ class TrackerObjectDebugger void getMessage(visualization_msgs::msg::MarkerArray & marker_array) const; }; -#endif // MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // DEBUGGER__DEBUG_OBJECT_HPP_ diff --git a/perception/multi_object_tracker/src/debugger/debugger.cpp b/perception/multi_object_tracker/src/debugger/debugger.cpp index e692ae76468e4..3e47bbe9bed8d 100644 --- a/perception/multi_object_tracker/src/debugger/debugger.cpp +++ b/perception/multi_object_tracker/src/debugger/debugger.cpp @@ -12,10 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "multi_object_tracker/debugger/debugger.hpp" +#include "debugger.hpp" #include +namespace autoware::multi_object_tracker +{ TrackerDebugger::TrackerDebugger(rclcpp::Node & node, const std::string & frame_id) : node_(node), diagnostic_updater_(&node), object_debugger_(frame_id) { @@ -207,3 +209,5 @@ void TrackerDebugger::publishObjectsMarkers() // reset object data object_debugger_.reset(); } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp b/perception/multi_object_tracker/src/debugger/debugger.hpp similarity index 94% rename from perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp rename to perception/multi_object_tracker/src/debugger/debugger.hpp index a1c516147b220..77618e1882be7 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp +++ b/perception/multi_object_tracker/src/debugger/debugger.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_ -#define MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_ +#ifndef DEBUGGER__DEBUGGER_HPP_ +#define DEBUGGER__DEBUGGER_HPP_ #include "autoware/universe_utils/ros/debug_publisher.hpp" #include "autoware/universe_utils/ros/published_time_publisher.hpp" -#include "multi_object_tracker/debugger/debug_object.hpp" +#include "debug_object.hpp" #include #include @@ -33,6 +33,9 @@ #include #include +namespace autoware::multi_object_tracker +{ + /** * @brief Debugger class for multi object tracker * @details This class is used to publish debug information of multi object tracker @@ -104,4 +107,6 @@ class TrackerDebugger void publishObjectsMarkers(); }; -#endif // MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // DEBUGGER__DEBUGGER_HPP_ diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_node.cpp similarity index 96% rename from perception/multi_object_tracker/src/multi_object_tracker_core.cpp rename to perception/multi_object_tracker/src/multi_object_tracker_node.cpp index 97c3d93d191b2..d3e1c9686b69b 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_node.cpp @@ -15,13 +15,12 @@ // #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/multi_object_tracker_core.hpp" +#include "multi_object_tracker_node.hpp" -#include "multi_object_tracker/utils/utils.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include #include -#include #include @@ -66,7 +65,7 @@ boost::optional getTransformAnonymous( } // namespace -namespace multi_object_tracker +namespace autoware::multi_object_tracker { using Label = autoware_perception_msgs::msg::ObjectClassification; @@ -192,7 +191,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) const auto max_rad_matrix = this->declare_parameter>("max_rad_matrix"); const auto min_iou_matrix = this->declare_parameter>("min_iou_matrix"); - data_association_ = std::make_unique( + association_ = std::make_unique( can_assign_matrix, max_dist_matrix, max_area_matrix, min_area_matrix, max_rad_matrix, min_iou_matrix); } @@ -295,9 +294,9 @@ void MultiObjectTracker::runProcess( const auto & list_tracker = processor_->getListTracker(); const auto & detected_objects = transformed_objects; // global nearest neighbor - Eigen::MatrixXd score_matrix = data_association_->calcScoreMatrix( + Eigen::MatrixXd score_matrix = association_->calcScoreMatrix( detected_objects, list_tracker); // row : tracker, col : measurement - data_association_->assign(score_matrix, direct_assignment, reverse_assignment); + association_->assign(score_matrix, direct_assignment, reverse_assignment); // Collect debug information - tracker list, existence probabilities, association results debugger_->collectObjectInfo( @@ -358,6 +357,8 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) const debugger_->publishObjectsMarkers(); } -} // namespace multi_object_tracker +} // namespace autoware::multi_object_tracker + +#include -RCLCPP_COMPONENTS_REGISTER_NODE(multi_object_tracker::MultiObjectTracker) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::multi_object_tracker::MultiObjectTracker) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp b/perception/multi_object_tracker/src/multi_object_tracker_node.hpp similarity index 82% rename from perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp rename to perception/multi_object_tracker/src/multi_object_tracker_node.hpp index aff3cbd00eabe..db5eaaa88ca8c 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_node.hpp @@ -16,14 +16,14 @@ // Author: v1.0 Yukihiro Saito /// -#ifndef MULTI_OBJECT_TRACKER__MULTI_OBJECT_TRACKER_CORE_HPP_ -#define MULTI_OBJECT_TRACKER__MULTI_OBJECT_TRACKER_CORE_HPP_ +#ifndef MULTI_OBJECT_TRACKER_NODE_HPP_ +#define MULTI_OBJECT_TRACKER_NODE_HPP_ -#include "multi_object_tracker/data_association/data_association.hpp" -#include "multi_object_tracker/debugger/debugger.hpp" -#include "multi_object_tracker/processor/input_manager.hpp" -#include "multi_object_tracker/processor/processor.hpp" -#include "multi_object_tracker/tracker/model/tracker_base.hpp" +#include "autoware/multi_object_tracker/association/association.hpp" +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" +#include "debugger/debugger.hpp" +#include "processor/input_manager.hpp" +#include "processor/processor.hpp" #include @@ -52,7 +52,7 @@ #include #include -namespace multi_object_tracker +namespace autoware::multi_object_tracker { using DetectedObject = autoware_perception_msgs::msg::DetectedObject; @@ -82,7 +82,7 @@ class MultiObjectTracker : public rclcpp::Node // internal states std::string world_frame_id_; // tracking frame - std::unique_ptr data_association_; + std::unique_ptr association_; std::unique_ptr processor_; // input manager @@ -103,6 +103,6 @@ class MultiObjectTracker : public rclcpp::Node inline bool shouldTrackerPublish(const std::shared_ptr tracker) const; }; -} // namespace multi_object_tracker +} // namespace autoware::multi_object_tracker -#endif // MULTI_OBJECT_TRACKER__MULTI_OBJECT_TRACKER_CORE_HPP_ +#endif // MULTI_OBJECT_TRACKER_NODE_HPP_ diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index bee76e6c05940..5172cc5062450 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "multi_object_tracker/processor/input_manager.hpp" +#include "input_manager.hpp" #include -namespace multi_object_tracker +namespace autoware::multi_object_tracker { /////////////////////////// /////// InputStream /////// @@ -62,7 +62,7 @@ void InputStream::onMessage( { const DetectedObjects objects = *msg; objects_que_.push_back(objects); - if (objects_que_.size() > que_size_) { + while (objects_que_.size() > que_size_) { objects_que_.pop_front(); } @@ -160,21 +160,27 @@ void InputStream::getObjectsOlderThan( return; } - for (const auto & object : objects_que_) { - const rclcpp::Time object_time = rclcpp::Time(object.header.stamp); - - // remove objects older than the specified duration + for (const auto & objects : objects_que_) { + const rclcpp::Time object_time = rclcpp::Time(objects.header.stamp); + // ignore objects older than the specified duration if (object_time < object_oldest_time) { - objects_que_.pop_front(); continue; } // Add the object if the object is older than the specified latest time - if (object_latest_time >= object_time) { - std::pair object_pair(index_, object); - objects_list.push_back(object_pair); - // remove the object from the queue + if (object_time <= object_latest_time) { + std::pair objects_pair(index_, objects); + objects_list.push_back(objects_pair); + } + } + + // remove objects older than 'object_latest_time' + while (!objects_que_.empty()) { + const rclcpp::Time object_time = rclcpp::Time(objects_que_.front().header.stamp); + if (object_time < object_latest_time) { objects_que_.pop_front(); + } else { + break; } } } @@ -368,4 +374,4 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li return is_any_object; } -} // namespace multi_object_tracker +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp b/perception/multi_object_tracker/src/processor/input_manager.hpp similarity index 95% rename from perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp rename to perception/multi_object_tracker/src/processor/input_manager.hpp index 29b0c18ae766f..432a4105d815f 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp +++ b/perception/multi_object_tracker/src/processor/input_manager.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MULTI_OBJECT_TRACKER__PROCESSOR__INPUT_MANAGER_HPP_ -#define MULTI_OBJECT_TRACKER__PROCESSOR__INPUT_MANAGER_HPP_ +#ifndef PROCESSOR__INPUT_MANAGER_HPP_ +#define PROCESSOR__INPUT_MANAGER_HPP_ #include "rclcpp/rclcpp.hpp" @@ -26,7 +26,7 @@ #include #include -namespace multi_object_tracker +namespace autoware::multi_object_tracker { using DetectedObjects = autoware_perception_msgs::msg::DetectedObjects; using ObjectsList = std::vector>; @@ -155,6 +155,6 @@ class InputManager void optimizeTimings(); }; -} // namespace multi_object_tracker +} // namespace autoware::multi_object_tracker -#endif // MULTI_OBJECT_TRACKER__PROCESSOR__INPUT_MANAGER_HPP_ +#endif // PROCESSOR__INPUT_MANAGER_HPP_ diff --git a/perception/multi_object_tracker/src/processor/processor.cpp b/perception/multi_object_tracker/src/processor/processor.cpp index 50d1a021c5838..17871c89b5258 100644 --- a/perception/multi_object_tracker/src/processor/processor.cpp +++ b/perception/multi_object_tracker/src/processor/processor.cpp @@ -12,15 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "multi_object_tracker/processor/processor.hpp" +#include "processor.hpp" -#include "multi_object_tracker/tracker/tracker.hpp" +#include "autoware/multi_object_tracker/tracker/tracker.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include "autoware_perception_msgs/msg/tracked_objects.hpp" #include +namespace autoware::multi_object_tracker +{ + using Label = autoware_perception_msgs::msg::ObjectClassification; TrackerProcessor::TrackerProcessor( @@ -248,3 +251,5 @@ void TrackerProcessor::getTentativeObjects( } } } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp b/perception/multi_object_tracker/src/processor/processor.hpp similarity index 91% rename from perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp rename to perception/multi_object_tracker/src/processor/processor.hpp index 6d0dbcd036e53..5eac113b3974c 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp +++ b/perception/multi_object_tracker/src/processor/processor.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MULTI_OBJECT_TRACKER__PROCESSOR__PROCESSOR_HPP_ -#define MULTI_OBJECT_TRACKER__PROCESSOR__PROCESSOR_HPP_ +#ifndef PROCESSOR__PROCESSOR_HPP_ +#define PROCESSOR__PROCESSOR_HPP_ -#include "multi_object_tracker/tracker/model/tracker_base.hpp" +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include @@ -29,6 +29,8 @@ #include #include +namespace autoware::multi_object_tracker +{ class TrackerProcessor { public: @@ -78,4 +80,6 @@ class TrackerProcessor const geometry_msgs::msg::Transform & self_transform, const uint & channel_index) const; }; -#endif // MULTI_OBJECT_TRACKER__PROCESSOR__PROCESSOR_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // PROCESSOR__PROCESSOR_HPP_ From f28ccd834427d2c7a07f3beef0fc3bc6edb2eb3b Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 8 Jul 2024 16:34:56 +0900 Subject: [PATCH 07/19] refactor(detected_object_validation)!: fix namespace and directory structure (#7866) * refactor: update include paths for detected object filters Signed-off-by: Taekjin LEE * refactor: set proper namespace Signed-off-by: Taekjin LEE * refactor: move utils to be shared Signed-off-by: Taekjin LEE * refactor: remove unnecessary dependencies in detected_object_validation/package.xml Signed-off-by: Taekjin LEE * chore: Override functions in pcl_validator.hpp Signed-off-by: Taekjin LEE * style(pre-commit): autofix * refactor: pcl_validator to obstacle_pointcloud_validator, ogm_validator to occupancy_grid_map_validator Signed-off-by: Taekjin LEE --------- Signed-off-by: Taekjin LEE Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../detected_object_validation/CMakeLists.txt | 28 +++++------- .../utils/utils.hpp | 10 +++-- .../{src => lib/utils}/utils.cpp | 6 ++- .../detected_object_validation/package.xml | 3 -- .../lanelet_filter.cpp} | 20 +++++---- .../lanelet_filter/lanelet_filter.hpp} | 27 ++++++----- .../obstacle_pointcloud}/debugger.hpp | 16 ++++--- .../obstacle_pointcloud_validator.cpp} | 14 +++--- .../obstacle_pointcloud_validator.hpp} | 45 ++++++++++++------- .../occupancy_grid_map_validator.cpp} | 21 +++++---- .../occupancy_grid_map_validator.hpp} | 19 +++++--- .../position_filter.cpp} | 12 +++-- .../position_filter/position_filter.hpp} | 23 +++++----- .../test_object_position_filter.cpp | 4 +- .../test/test_utils.cpp | 13 +++--- .../src/low_intensity_cluster_filter_node.hpp | 4 +- 16 files changed, 153 insertions(+), 112 deletions(-) rename perception/detected_object_validation/include/{ => autoware}/detected_object_validation/utils/utils.hpp (82%) rename perception/detected_object_validation/{src => lib/utils}/utils.cpp (87%) rename perception/detected_object_validation/src/{object_lanelet_filter.cpp => lanelet_filter/lanelet_filter.cpp} (95%) rename perception/detected_object_validation/{include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp => src/lanelet_filter/lanelet_filter.hpp} (81%) rename perception/detected_object_validation/{include/detected_object_validation/obstacle_pointcloud_based_validator => src/obstacle_pointcloud}/debugger.hpp (89%) rename perception/detected_object_validation/src/{obstacle_pointcloud_based_validator.cpp => obstacle_pointcloud/obstacle_pointcloud_validator.cpp} (97%) rename perception/detected_object_validation/{include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp => src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp} (79%) rename perception/detected_object_validation/src/{occupancy_grid_based_validator.cpp => occupancy_grid_map/occupancy_grid_map_validator.cpp} (92%) rename perception/detected_object_validation/{include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp => src/occupancy_grid_map/occupancy_grid_map_validator.hpp} (83%) rename perception/detected_object_validation/src/{object_position_filter.cpp => position_filter/position_filter.cpp} (91%) rename perception/detected_object_validation/{include/detected_object_validation/detected_object_filter/object_position_filter.hpp => src/position_filter/position_filter.hpp} (71%) diff --git a/perception/detected_object_validation/CMakeLists.txt b/perception/detected_object_validation/CMakeLists.txt index c416209d7d55d..6d93e7b426ad2 100644 --- a/perception/detected_object_validation/CMakeLists.txt +++ b/perception/detected_object_validation/CMakeLists.txt @@ -28,12 +28,8 @@ include_directories( ) # Generate occupancy grid based validator exe file -set(OCCUPANCY_GRID_BASED_VALIDATOR_SRC - src/occupancy_grid_based_validator.cpp -) - ament_auto_add_library(occupancy_grid_based_validator SHARED - ${OCCUPANCY_GRID_BASED_VALIDATOR_SRC} + src/occupancy_grid_map/occupancy_grid_map_validator.cpp ) target_link_libraries(occupancy_grid_based_validator @@ -42,12 +38,8 @@ target_link_libraries(occupancy_grid_based_validator ) # Generate obstacle pointcloud based validator exe file -set(OBSTACLE_POINTCLOUD_BASED_VALIDATOR_SRC - src/obstacle_pointcloud_based_validator.cpp -) - ament_auto_add_library(obstacle_pointcloud_based_validator SHARED - ${OBSTACLE_POINTCLOUD_BASED_VALIDATOR_SRC} + src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp ) target_link_libraries(obstacle_pointcloud_based_validator @@ -56,32 +48,32 @@ target_link_libraries(obstacle_pointcloud_based_validator ) ament_auto_add_library(object_lanelet_filter SHARED - src/object_lanelet_filter.cpp - src/utils.cpp + src/lanelet_filter/lanelet_filter.cpp + lib/utils/utils.cpp ) ament_auto_add_library(object_position_filter SHARED - src/object_position_filter.cpp - src/utils.cpp + src/position_filter/position_filter.cpp + lib/utils/utils.cpp ) rclcpp_components_register_node(obstacle_pointcloud_based_validator - PLUGIN "obstacle_pointcloud_based_validator::ObstaclePointCloudBasedValidator" + PLUGIN "autoware::detected_object_validation::obstacle_pointcloud::ObstaclePointCloudBasedValidator" EXECUTABLE obstacle_pointcloud_based_validator_node ) rclcpp_components_register_node(object_lanelet_filter - PLUGIN "object_lanelet_filter::ObjectLaneletFilterNode" + PLUGIN "autoware::detected_object_validation::lanelet_filter::ObjectLaneletFilterNode" EXECUTABLE object_lanelet_filter_node ) rclcpp_components_register_node(object_position_filter - PLUGIN "object_position_filter::ObjectPositionFilterNode" + PLUGIN "autoware::detected_object_validation::position_filter::ObjectPositionFilterNode" EXECUTABLE object_position_filter_node ) rclcpp_components_register_node(occupancy_grid_based_validator - PLUGIN "occupancy_grid_based_validator::OccupancyGridBasedValidator" + PLUGIN "autoware::detected_object_validation::occupancy_grid_map::OccupancyGridBasedValidator" EXECUTABLE occupancy_grid_based_validator_node ) diff --git a/perception/detected_object_validation/include/detected_object_validation/utils/utils.hpp b/perception/detected_object_validation/include/autoware/detected_object_validation/utils/utils.hpp similarity index 82% rename from perception/detected_object_validation/include/detected_object_validation/utils/utils.hpp rename to perception/detected_object_validation/include/autoware/detected_object_validation/utils/utils.hpp index c55e992213eb5..3ec119d2a48c5 100644 --- a/perception/detected_object_validation/include/detected_object_validation/utils/utils.hpp +++ b/perception/detected_object_validation/include/autoware/detected_object_validation/utils/utils.hpp @@ -12,12 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ -#define DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ +#ifndef AUTOWARE__DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ +#define AUTOWARE__DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ #include #include + +namespace autoware::detected_object_validation +{ namespace utils { struct FilterTargetLabel @@ -48,5 +51,6 @@ inline bool hasBoundingBox(const autoware_perception_msgs::msg::DetectedObject & } } // namespace utils +} // namespace autoware::detected_object_validation -#endif // DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ +#endif // AUTOWARE__DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ diff --git a/perception/detected_object_validation/src/utils.cpp b/perception/detected_object_validation/lib/utils/utils.cpp similarity index 87% rename from perception/detected_object_validation/src/utils.cpp rename to perception/detected_object_validation/lib/utils/utils.cpp index f6e2157cb885f..327f65a0c833e 100644 --- a/perception/detected_object_validation/src/utils.cpp +++ b/perception/detected_object_validation/lib/utils/utils.cpp @@ -12,10 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "detected_object_validation/utils/utils.hpp" +#include "autoware/detected_object_validation/utils/utils.hpp" #include +namespace autoware::detected_object_validation +{ namespace utils { using Label = autoware_perception_msgs::msg::ObjectClassification; @@ -27,4 +29,6 @@ bool FilterTargetLabel::isTarget(const uint8_t label) const (label == Label::TRAILER && TRAILER) || (label == Label::MOTORCYCLE && MOTORCYCLE) || (label == Label::BICYCLE && BICYCLE) || (label == Label::PEDESTRIAN && PEDESTRIAN); } + } // namespace utils +} // namespace autoware::detected_object_validation diff --git a/perception/detected_object_validation/package.xml b/perception/detected_object_validation/package.xml index d8eb1257ba451..09440c9f6358d 100644 --- a/perception/detected_object_validation/package.xml +++ b/perception/detected_object_validation/package.xml @@ -22,17 +22,14 @@ autoware_perception_msgs autoware_test_utils autoware_universe_utils - geometry_msgs message_filters nav_msgs object_recognition_utils pcl_conversions rclcpp rclcpp_components - tf2 tf2_geometry_msgs tf2_ros - tier4_perception_msgs ament_lint_auto autoware_lint_common diff --git a/perception/detected_object_validation/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/lanelet_filter/lanelet_filter.cpp similarity index 95% rename from perception/detected_object_validation/src/object_lanelet_filter.cpp rename to perception/detected_object_validation/src/lanelet_filter/lanelet_filter.cpp index d1ba34f4a61b2..09440fedc1764 100644 --- a/perception/detected_object_validation/src/object_lanelet_filter.cpp +++ b/perception/detected_object_validation/src/lanelet_filter/lanelet_filter.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "detected_object_validation/detected_object_filter/object_lanelet_filter.hpp" +#include "lanelet_filter.hpp" -#include -#include -#include -#include +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_lanelet2_extension/utility/message_conversion.hpp" +#include "autoware_lanelet2_extension/utility/query.hpp" +#include "object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -25,7 +25,9 @@ #include -namespace object_lanelet_filter +namespace autoware::detected_object_validation +{ +namespace lanelet_filter { ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & node_options) : Node("object_lanelet_filter_node", node_options), @@ -309,7 +311,9 @@ bool ObjectLaneletFilterNode::isSameDirectionWithLanelets( return false; } -} // namespace object_lanelet_filter +} // namespace lanelet_filter +} // namespace autoware::detected_object_validation #include -RCLCPP_COMPONENTS_REGISTER_NODE(object_lanelet_filter::ObjectLaneletFilterNode) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::detected_object_validation::lanelet_filter::ObjectLaneletFilterNode) diff --git a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp b/perception/detected_object_validation/src/lanelet_filter/lanelet_filter.hpp similarity index 81% rename from perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp rename to perception/detected_object_validation/src/lanelet_filter/lanelet_filter.hpp index 3ffd8d18c9a67..25d78a160c246 100644 --- a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp +++ b/perception/detected_object_validation/src/lanelet_filter/lanelet_filter.hpp @@ -12,19 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_LANELET_FILTER_HPP_ -#define DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_LANELET_FILTER_HPP_ +#ifndef LANELET_FILTER__LANELET_FILTER_HPP_ +#define LANELET_FILTER__LANELET_FILTER_HPP_ -#include "detected_object_validation/utils/utils.hpp" +#include "autoware/detected_object_validation/utils/utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/ros/debug_publisher.hpp" +#include "autoware/universe_utils/ros/published_time_publisher.hpp" +#include "autoware_lanelet2_extension/utility/utilities.hpp" -#include -#include -#include -#include #include -#include -#include +#include "autoware_map_msgs/msg/lanelet_map_bin.hpp" +#include "autoware_perception_msgs/msg/detected_objects.hpp" #include #include @@ -33,7 +33,9 @@ #include #include -namespace object_lanelet_filter +namespace autoware::detected_object_validation +{ +namespace lanelet_filter { using autoware::universe_utils::LinearRing2d; using autoware::universe_utils::MultiPoint2d; @@ -92,6 +94,7 @@ class ObjectLaneletFilterNode : public rclcpp::Node std::unique_ptr published_time_publisher_; }; -} // namespace object_lanelet_filter +} // namespace lanelet_filter +} // namespace autoware::detected_object_validation -#endif // DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_LANELET_FILTER_HPP_ +#endif // LANELET_FILTER__LANELET_FILTER_HPP_ diff --git a/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/debugger.hpp b/perception/detected_object_validation/src/obstacle_pointcloud/debugger.hpp similarity index 89% rename from perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/debugger.hpp rename to perception/detected_object_validation/src/obstacle_pointcloud/debugger.hpp index d338580d95418..5886474d51b34 100644 --- a/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/debugger.hpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud/debugger.hpp @@ -12,19 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__DEBUGGER_HPP_ -#define DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__DEBUGGER_HPP_ +#ifndef OBSTACLE_POINTCLOUD__DEBUGGER_HPP_ +#define OBSTACLE_POINTCLOUD__DEBUGGER_HPP_ #include -#include +#include "autoware_perception_msgs/msg/detected_objects.hpp" #include #include #include #include -namespace obstacle_pointcloud_based_validator +namespace autoware::detected_object_validation +{ +namespace obstacle_pointcloud { class Debugger { @@ -106,6 +108,8 @@ class Debugger return pointcloud_xyz; } }; -} // namespace obstacle_pointcloud_based_validator -#endif // DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__DEBUGGER_HPP_ +} // namespace obstacle_pointcloud +} // namespace autoware::detected_object_validation + +#endif // OBSTACLE_POINTCLOUD__DEBUGGER_HPP_ diff --git a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp b/perception/detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp similarity index 97% rename from perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp rename to perception/detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp index fbb3f863be7c4..3fe527935c485 100644 --- a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp @@ -12,7 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp" +#define EIGEN_MPL2_ONLY + +#include "obstacle_pointcloud_validator.hpp" #include #include @@ -25,11 +27,12 @@ #include #endif -#define EIGEN_MPL2_ONLY #include #include -namespace obstacle_pointcloud_based_validator +namespace autoware::detected_object_validation +{ +namespace obstacle_pointcloud { namespace bg = boost::geometry; using Shape = autoware_perception_msgs::msg::Shape; @@ -369,8 +372,9 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( "debug/pipeline_latency_ms", pipeline_latency); } -} // namespace obstacle_pointcloud_based_validator +} // namespace obstacle_pointcloud +} // namespace autoware::detected_object_validation #include RCLCPP_COMPONENTS_REGISTER_NODE( - obstacle_pointcloud_based_validator::ObstaclePointCloudBasedValidator) + autoware::detected_object_validation::obstacle_pointcloud::ObstaclePointCloudBasedValidator) diff --git a/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp b/perception/detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp similarity index 79% rename from perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp rename to perception/detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp index a16bb63fd0c87..19b42ecf9055b 100644 --- a/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp @@ -13,17 +13,17 @@ // limitations under the License. // NOLINTNEXTLINE(whitespace/line_length) -#ifndef DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__OBSTACLE_POINTCLOUD_BASED_VALIDATOR_HPP_ +#ifndef OBSTACLE_POINTCLOUD__OBSTACLE_POINTCLOUD_VALIDATOR_HPP_ // NOLINTNEXTLINE(whitespace/line_length) -#define DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__OBSTACLE_POINTCLOUD_BASED_VALIDATOR_HPP_ +#define OBSTACLE_POINTCLOUD__OBSTACLE_POINTCLOUD_VALIDATOR_HPP_ -#include "detected_object_validation/obstacle_pointcloud_based_validator/debugger.hpp" +#include "autoware/universe_utils/ros/debug_publisher.hpp" +#include "autoware/universe_utils/ros/published_time_publisher.hpp" +#include "debugger.hpp" -#include -#include #include -#include +#include "autoware_perception_msgs/msg/detected_objects.hpp" #include #include @@ -42,7 +42,10 @@ #include #include #include -namespace obstacle_pointcloud_based_validator + +namespace autoware::detected_object_validation +{ +namespace obstacle_pointcloud { struct PointsNumThresholdParam @@ -91,14 +94,17 @@ class Validator2D : public Validator pcl::PointCloud::Ptr convertToXYZ( const pcl::PointCloud::Ptr & pointcloud_xy); - inline pcl::PointCloud::Ptr getDebugNeighborPointCloud() + inline pcl::PointCloud::Ptr getDebugNeighborPointCloud() override { return convertToXYZ(neighbor_pointcloud_); } - bool setKdtreeInputCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_cloud); - bool validate_object(const autoware_perception_msgs::msg::DetectedObject & transformed_object); - std::optional getMaxRadius(const autoware_perception_msgs::msg::DetectedObject & object); + bool setKdtreeInputCloud( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_cloud) override; + bool validate_object( + const autoware_perception_msgs::msg::DetectedObject & transformed_object) override; + std::optional getMaxRadius( + const autoware_perception_msgs::msg::DetectedObject & object) override; std::optional getPointCloudWithinObject( const autoware_perception_msgs::msg::DetectedObject & object, const pcl::PointCloud::Ptr neighbor_pointcloud); @@ -112,13 +118,16 @@ class Validator3D : public Validator public: explicit Validator3D(PointsNumThresholdParam & points_num_threshold_param); - inline pcl::PointCloud::Ptr getDebugNeighborPointCloud() + inline pcl::PointCloud::Ptr getDebugNeighborPointCloud() override { return neighbor_pointcloud_; } - bool setKdtreeInputCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_cloud); - bool validate_object(const autoware_perception_msgs::msg::DetectedObject & transformed_object); - std::optional getMaxRadius(const autoware_perception_msgs::msg::DetectedObject & object); + bool setKdtreeInputCloud( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_cloud) override; + bool validate_object( + const autoware_perception_msgs::msg::DetectedObject & transformed_object) override; + std::optional getMaxRadius( + const autoware_perception_msgs::msg::DetectedObject & object) override; std::optional getPointCloudWithinObject( const autoware_perception_msgs::msg::DetectedObject & object, const pcl::PointCloud::Ptr neighbor_pointcloud); @@ -154,7 +163,9 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects, const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_obstacle_pointcloud); }; -} // namespace obstacle_pointcloud_based_validator + +} // namespace obstacle_pointcloud +} // namespace autoware::detected_object_validation // NOLINTNEXTLINE(whitespace/line_length) -#endif // DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__OBSTACLE_POINTCLOUD_BASED_VALIDATOR_HPP_ +#endif // OBSTACLE_POINTCLOUD__OBSTACLE_POINTCLOUD_VALIDATOR_HPP_ diff --git a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp b/perception/detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp similarity index 92% rename from perception/detected_object_validation/src/occupancy_grid_based_validator.cpp rename to perception/detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp index b8682a6e56b3b..ce900f30f4255 100644 --- a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp +++ b/perception/detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp @@ -12,11 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp" +#define EIGEN_MPL2_ONLY + +#include "occupancy_grid_map_validator.hpp" -#include -#include -#include +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "object_recognition_utils/object_classification.hpp" +#include "object_recognition_utils/object_recognition_utils.hpp" #include @@ -26,11 +28,12 @@ #include #endif -#define EIGEN_MPL2_ONLY #include #include -namespace occupancy_grid_based_validator +namespace autoware::detected_object_validation +{ +namespace occupancy_grid_map { using Shape = autoware_perception_msgs::msg::Shape; using Polygon2d = autoware::universe_utils::Polygon2d; @@ -173,7 +176,9 @@ void OccupancyGridBasedValidator::showDebugImage( cv::waitKey(2); } -} // namespace occupancy_grid_based_validator +} // namespace occupancy_grid_map +} // namespace autoware::detected_object_validation #include -RCLCPP_COMPONENTS_REGISTER_NODE(occupancy_grid_based_validator::OccupancyGridBasedValidator) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::detected_object_validation::occupancy_grid_map::OccupancyGridBasedValidator) diff --git a/perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp b/perception/detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.hpp similarity index 83% rename from perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp rename to perception/detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.hpp index beb8faf5db1a3..52b6f79e5e20a 100644 --- a/perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp +++ b/perception/detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.hpp @@ -12,16 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DETECTED_OBJECT_VALIDATION__OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_ -#define DETECTED_OBJECT_VALIDATION__OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_ +#ifndef OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_VALIDATOR_HPP_ +#define OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_VALIDATOR_HPP_ + +#include "autoware/universe_utils/ros/published_time_publisher.hpp" -#include #include #include #include #include -#include +#include "autoware_perception_msgs/msg/detected_objects.hpp" #include #include @@ -30,7 +31,9 @@ #include #include -namespace occupancy_grid_based_validator +namespace autoware::detected_object_validation +{ +namespace occupancy_grid_map { class OccupancyGridBasedValidator : public rclcpp::Node { @@ -68,6 +71,8 @@ class OccupancyGridBasedValidator : public rclcpp::Node const nav_msgs::msg::OccupancyGrid & ros_occ_grid, const autoware_perception_msgs::msg::DetectedObjects & objects, const cv::Mat & occ_grid); }; -} // namespace occupancy_grid_based_validator -#endif // DETECTED_OBJECT_VALIDATION__OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_ +} // namespace occupancy_grid_map +} // namespace autoware::detected_object_validation + +#endif // OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_VALIDATOR_HPP_ diff --git a/perception/detected_object_validation/src/object_position_filter.cpp b/perception/detected_object_validation/src/position_filter/position_filter.cpp similarity index 91% rename from perception/detected_object_validation/src/object_position_filter.cpp rename to perception/detected_object_validation/src/position_filter/position_filter.cpp index dccff8a6ccc67..ac0bab404c476 100644 --- a/perception/detected_object_validation/src/object_position_filter.cpp +++ b/perception/detected_object_validation/src/position_filter/position_filter.cpp @@ -12,9 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "detected_object_validation/detected_object_filter/object_position_filter.hpp" +#include "position_filter.hpp" -namespace object_position_filter +namespace autoware::detected_object_validation +{ +namespace position_filter { ObjectPositionFilterNode::ObjectPositionFilterNode(const rclcpp::NodeOptions & node_options) : Node("object_position_filter_node", node_options), @@ -78,7 +80,9 @@ bool ObjectPositionFilterNode::isObjectInBounds( position.y > lower_bound_y_ && position.y < upper_bound_y_; } -} // namespace object_position_filter +} // namespace position_filter +} // namespace autoware::detected_object_validation #include -RCLCPP_COMPONENTS_REGISTER_NODE(object_position_filter::ObjectPositionFilterNode) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::detected_object_validation::position_filter::ObjectPositionFilterNode) diff --git a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp b/perception/detected_object_validation/src/position_filter/position_filter.hpp similarity index 71% rename from perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp rename to perception/detected_object_validation/src/position_filter/position_filter.hpp index ea70d62fd952d..30a5d7fc9dfb1 100644 --- a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp +++ b/perception/detected_object_validation/src/position_filter/position_filter.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_POSITION_FILTER_HPP_ -#define DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_POSITION_FILTER_HPP_ +#ifndef POSITION_FILTER__POSITION_FILTER_HPP_ +#define POSITION_FILTER__POSITION_FILTER_HPP_ -#include "detected_object_validation/utils/utils.hpp" +#include "autoware/detected_object_validation/utils/utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/ros/published_time_publisher.hpp" -#include -#include #include -#include -#include +#include "autoware_map_msgs/msg/lanelet_map_bin.hpp" +#include "autoware_perception_msgs/msg/detected_objects.hpp" #include #include @@ -30,7 +30,9 @@ #include #include -namespace object_position_filter +namespace autoware::detected_object_validation +{ +namespace position_filter { class ObjectPositionFilterNode : public rclcpp::Node @@ -57,6 +59,7 @@ class ObjectPositionFilterNode : public rclcpp::Node std::unique_ptr published_time_publisher_; }; -} // namespace object_position_filter +} // namespace position_filter +} // namespace autoware::detected_object_validation -#endif // DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_POSITION_FILTER_HPP_ +#endif // POSITION_FILTER__POSITION_FILTER_HPP_ diff --git a/perception/detected_object_validation/test/object_position_filter/test_object_position_filter.cpp b/perception/detected_object_validation/test/object_position_filter/test_object_position_filter.cpp index 03cebf91418ce..5bd174d5a9e17 100644 --- a/perception/detected_object_validation/test/object_position_filter/test_object_position_filter.cpp +++ b/perception/detected_object_validation/test/object_position_filter/test_object_position_filter.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "detected_object_validation/detected_object_filter/object_position_filter.hpp" +#include "../../src/position_filter/position_filter.hpp" #include #include @@ -21,10 +21,10 @@ #include +using autoware::detected_object_validation::position_filter::ObjectPositionFilterNode; using autoware_perception_msgs::msg::DetectedObject; using autoware_perception_msgs::msg::DetectedObjects; using autoware_perception_msgs::msg::ObjectClassification; -using object_position_filter::ObjectPositionFilterNode; std::shared_ptr generateTestManager() { diff --git a/perception/detected_object_validation/test/test_utils.cpp b/perception/detected_object_validation/test/test_utils.cpp index f7077dadb1ef2..7865f928f23a8 100644 --- a/perception/detected_object_validation/test/test_utils.cpp +++ b/perception/detected_object_validation/test/test_utils.cpp @@ -12,17 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "detected_object_validation/utils/utils.hpp" +#include "autoware/detected_object_validation/utils/utils.hpp" -#include +#include "autoware_perception_msgs/msg/object_classification.hpp" #include +using autoware::detected_object_validation::utils::FilterTargetLabel; using AutowareLabel = autoware_perception_msgs::msg::ObjectClassification; -utils::FilterTargetLabel createFilterTargetAll() +FilterTargetLabel createFilterTargetAll() { - utils::FilterTargetLabel filter; + FilterTargetLabel filter; filter.UNKNOWN = true; filter.CAR = true; filter.TRUCK = true; @@ -34,9 +35,9 @@ utils::FilterTargetLabel createFilterTargetAll() return filter; } -utils::FilterTargetLabel createFilterTargetVehicle() +FilterTargetLabel createFilterTargetVehicle() { - utils::FilterTargetLabel filter; + FilterTargetLabel filter; filter.UNKNOWN = false; filter.CAR = true; filter.TRUCK = true; diff --git a/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter_node.hpp b/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter_node.hpp index 8e21306218ab2..61e23860cd195 100644 --- a/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter_node.hpp +++ b/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter_node.hpp @@ -15,9 +15,9 @@ #ifndef LOW_INTENSITY_CLUSTER_FILTER_NODE_HPP_ #define LOW_INTENSITY_CLUSTER_FILTER_NODE_HPP_ +#include "autoware/detected_object_validation/utils/utils.hpp" #include "autoware/universe_utils/ros/debug_publisher.hpp" #include "autoware/universe_utils/system/stop_watch.hpp" -#include "detected_object_validation/utils/utils.hpp" #include #include @@ -64,7 +64,7 @@ class LowIntensityClusterFilter : public rclcpp::Node // Eigen::Vector4f max_boundary_transformed_; bool is_validation_range_transformed_ = false; const std::string base_link_frame_id_ = "base_link"; - utils::FilterTargetLabel filter_target_; + autoware::detected_object_validation::utils::FilterTargetLabel filter_target_; // debugger std::unique_ptr> stop_watch_ptr_{ From 1cbd11d789d12ba0bd771b2222d22476747d86a5 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 8 Jul 2024 22:55:42 +0900 Subject: [PATCH 08/19] feat(autoware_universe_utils): add TimeKeeper to track function's processing time (#7754) Signed-off-by: Takayuki Murooka --- common/autoware_universe_utils/CMakeLists.txt | 25 ++ common/autoware_universe_utils/README.md | 249 ++++++++++++++++++ .../examples/example_scoped_time_track.cpp | 61 +++++ .../examples/example_time_keeper.cpp | 64 +++++ .../universe_utils/system/time_keeper.hpp | 202 ++++++++++++++ .../src/system/time_keeper.cpp | 175 ++++++++++++ .../test/src/system/test_time_keeper.cpp | 53 ++++ .../path_optimizer/common_structs.hpp | 52 ---- .../autoware/path_optimizer/mpt_optimizer.hpp | 5 +- .../include/autoware/path_optimizer/node.hpp | 7 +- .../state_equation_generator.hpp | 7 +- .../src/mpt_optimizer.cpp | 63 ++--- planning/autoware_path_optimizer/src/node.cpp | 71 +++-- .../src/state_equation_generator.cpp | 3 +- 14 files changed, 893 insertions(+), 144 deletions(-) create mode 100644 common/autoware_universe_utils/examples/example_scoped_time_track.cpp create mode 100644 common/autoware_universe_utils/examples/example_time_keeper.cpp create mode 100644 common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp create mode 100644 common/autoware_universe_utils/src/system/time_keeper.cpp create mode 100644 common/autoware_universe_utils/test/src/system/test_time_keeper.cpp diff --git a/common/autoware_universe_utils/CMakeLists.txt b/common/autoware_universe_utils/CMakeLists.txt index 9e86ddeb60692..2fdeef2119ab8 100644 --- a/common/autoware_universe_utils/CMakeLists.txt +++ b/common/autoware_universe_utils/CMakeLists.txt @@ -1,11 +1,15 @@ cmake_minimum_required(VERSION 3.14) project(autoware_universe_utils) +option(BUILD_EXAMPLES "Build examples" OFF) + find_package(autoware_cmake REQUIRED) autoware_package() find_package(Boost REQUIRED) +find_package(fmt REQUIRED) + ament_auto_add_library(autoware_universe_utils SHARED src/geometry/geometry.cpp src/geometry/pose_deviation.cpp @@ -16,6 +20,11 @@ ament_auto_add_library(autoware_universe_utils SHARED src/ros/marker_helper.cpp src/ros/logger_level_configure.cpp src/system/backtrace.cpp + src/system/time_keeper.cpp +) + +target_link_libraries(autoware_universe_utils + fmt::fmt ) if(BUILD_TESTING) @@ -30,4 +39,20 @@ if(BUILD_TESTING) ) endif() +if(BUILD_EXAMPLES) + message(STATUS "Building examples") + file(GLOB_RECURSE example_files examples/*.cpp) + + foreach(example_file ${example_files}) + get_filename_component(example_name ${example_file} NAME_WE) + add_executable(${example_name} ${example_file}) + target_link_libraries(${example_name} + autoware_universe_utils + ) + install(TARGETS ${example_name} + DESTINATION lib/${PROJECT_NAME} + ) + endforeach() +endif() + ament_auto_package() diff --git a/common/autoware_universe_utils/README.md b/common/autoware_universe_utils/README.md index 22b9355b09635..2d24f84423299 100644 --- a/common/autoware_universe_utils/README.md +++ b/common/autoware_universe_utils/README.md @@ -7,3 +7,252 @@ This package contains many common functions used by other packages, so please re ## For developers `autoware_universe_utils.hpp` header file was removed because the source files that directly/indirectly include this file took a long time for preprocessing. + +## `autoware::universe_utils` + +### `systems` + +#### `autoware::universe_utils::TimeKeeper` + +##### Constructor + +```cpp +template +explicit TimeKeeper(Reporters... reporters); +``` + +- Initializes the `TimeKeeper` with a list of reporters. + +##### Methods + +- `void add_reporter(std::ostream * os);` + + - Adds a reporter to output processing times to an `ostream`. + - `os`: Pointer to the `ostream` object. + +- `void add_reporter(rclcpp::Publisher::SharedPtr publisher);` + + - Adds a reporter to publish processing times to an `rclcpp` publisher. + - `publisher`: Shared pointer to the `rclcpp` publisher. + +- `void add_reporter(rclcpp::Publisher::SharedPtr publisher);` + + - Adds a reporter to publish processing times to an `rclcpp` publisher with `std_msgs::msg::String`. + - `publisher`: Shared pointer to the `rclcpp` publisher. + +- `void start_track(const std::string & func_name);` + + - Starts tracking the processing time of a function. + - `func_name`: Name of the function to be tracked. + +- `void end_track(const std::string & func_name);` + - Ends tracking the processing time of a function. + - `func_name`: Name of the function to end tracking. + +##### Example + +```cpp +#include "autoware/universe_utils/system/time_keeper.hpp" + +#include + +#include +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared("time_keeper_example"); + + auto time_keeper = std::make_shared(); + + time_keeper->add_reporter(&std::cout); + + auto publisher = + node->create_publisher("processing_time", 10); + + time_keeper->add_reporter(publisher); + + auto publisher_str = node->create_publisher("processing_time_str", 10); + + time_keeper->add_reporter(publisher_str); + + auto funcA = [&time_keeper]() { + time_keeper->start_track("funcA"); + std::this_thread::sleep_for(std::chrono::seconds(1)); + time_keeper->end_track("funcA"); + }; + + auto funcB = [&time_keeper, &funcA]() { + time_keeper->start_track("funcB"); + std::this_thread::sleep_for(std::chrono::seconds(2)); + funcA(); + time_keeper->end_track("funcB"); + }; + + auto funcC = [&time_keeper, &funcB]() { + time_keeper->start_track("funcC"); + std::this_thread::sleep_for(std::chrono::seconds(3)); + funcB(); + time_keeper->end_track("funcC"); + }; + + funcC(); + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} +``` + +- Output (console) + + ```text + ========================== + funcC (6000.7ms) + └── funcB (3000.44ms) + └── funcA (1000.19ms) + ``` + +- Output (`ros2 topic echo /processing_time`) + + ```text + nodes: + - id: 1 + name: funcC + processing_time: 6000.659 + parent_id: 0 + - id: 2 + name: funcB + processing_time: 3000.415 + parent_id: 1 + - id: 3 + name: funcA + processing_time: 1000.181 + parent_id: 2 + --- + ``` + +- Output (`ros2 topic echo /processing_time_str --field data`) + + ```text + funcC (6000.67ms) + └── funcB (3000.42ms) + └── funcA (1000.19ms) + + --- + ``` + +#### `autoware::universe_utils::ScopedTimeTrack` + +##### Description + +Class for automatically tracking the processing time of a function within a scope. + +##### Constructor + +```cpp +ScopedTimeTrack(const std::string & func_name, TimeKeeper & time_keeper); +``` + +- `func_name`: Name of the function to be tracked. +- `time_keeper`: Reference to the `TimeKeeper` object. + +##### Destructor + +```cpp +~ScopedTimeTrack(); +``` + +- Destroys the `ScopedTimeTrack` object, ending the tracking of the function. + +##### Example + +```cpp +#include "autoware/universe_utils/system/time_keeper.hpp" + +#include + +#include +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared("scoped_time_track_example"); + + auto time_keeper = std::make_shared(); + + time_keeper->add_reporter(&std::cout); + + auto publisher = + node->create_publisher("processing_time", 10); + + time_keeper->add_reporter(publisher); + + auto publisher_str = node->create_publisher("processing_time_str", 10); + + time_keeper->add_reporter(publisher_str); + + auto funcA = [&time_keeper]() { + autoware::universe_utils::ScopedTimeTrack st("funcA", *time_keeper); + std::this_thread::sleep_for(std::chrono::seconds(1)); + }; + + auto funcB = [&time_keeper, &funcA]() { + autoware::universe_utils::ScopedTimeTrack st("funcB", *time_keeper); + std::this_thread::sleep_for(std::chrono::seconds(2)); + funcA(); + }; + + auto funcC = [&time_keeper, &funcB]() { + autoware::universe_utils::ScopedTimeTrack st("funcC", *time_keeper); + std::this_thread::sleep_for(std::chrono::seconds(3)); + funcB(); + }; + + funcC(); + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} +``` + +- Output (console) + + ```text + ========================== + funcC (6000.7ms) + └── funcB (3000.44ms) + └── funcA (1000.19ms) + ``` + +- Output (`ros2 topic echo /processing_time`) + + ```text + nodes: + - id: 1 + name: funcC + processing_time: 6000.659 + parent_id: 0 + - id: 2 + name: funcB + processing_time: 3000.415 + parent_id: 1 + - id: 3 + name: funcA + processing_time: 1000.181 + parent_id: 2 + --- + ``` + +- Output (`ros2 topic echo /processing_time_str --field data`) + + ```text + funcC (6000.67ms) + └── funcB (3000.42ms) + └── funcA (1000.19ms) + + --- + ``` diff --git a/common/autoware_universe_utils/examples/example_scoped_time_track.cpp b/common/autoware_universe_utils/examples/example_scoped_time_track.cpp new file mode 100644 index 0000000000000..010cc58aba8ae --- /dev/null +++ b/common/autoware_universe_utils/examples/example_scoped_time_track.cpp @@ -0,0 +1,61 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// 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 "autoware/universe_utils/system/time_keeper.hpp" + +#include + +#include +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared("scoped_time_track_example"); + + auto time_keeper = std::make_shared(); + + time_keeper->add_reporter(&std::cout); + + auto publisher = + node->create_publisher("processing_time", 10); + + time_keeper->add_reporter(publisher); + + auto publisher_str = node->create_publisher("processing_time_str", 10); + + time_keeper->add_reporter(publisher_str); + + auto funcA = [&time_keeper]() { + autoware::universe_utils::ScopedTimeTrack st("funcA", *time_keeper); + std::this_thread::sleep_for(std::chrono::seconds(1)); + }; + + auto funcB = [&time_keeper, &funcA]() { + autoware::universe_utils::ScopedTimeTrack st("funcB", *time_keeper); + std::this_thread::sleep_for(std::chrono::seconds(2)); + funcA(); + }; + + auto funcC = [&time_keeper, &funcB]() { + autoware::universe_utils::ScopedTimeTrack st("funcC", *time_keeper); + std::this_thread::sleep_for(std::chrono::seconds(3)); + funcB(); + }; + + funcC(); + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/common/autoware_universe_utils/examples/example_time_keeper.cpp b/common/autoware_universe_utils/examples/example_time_keeper.cpp new file mode 100644 index 0000000000000..3f6037e68daac --- /dev/null +++ b/common/autoware_universe_utils/examples/example_time_keeper.cpp @@ -0,0 +1,64 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// 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 "autoware/universe_utils/system/time_keeper.hpp" + +#include + +#include +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared("time_keeper_example"); + + auto time_keeper = std::make_shared(); + + time_keeper->add_reporter(&std::cout); + + auto publisher = + node->create_publisher("processing_time", 10); + + time_keeper->add_reporter(publisher); + + auto publisher_str = node->create_publisher("processing_time_str", 10); + + time_keeper->add_reporter(publisher_str); + + auto funcA = [&time_keeper]() { + time_keeper->start_track("funcA"); + std::this_thread::sleep_for(std::chrono::seconds(1)); + time_keeper->end_track("funcA"); + }; + + auto funcB = [&time_keeper, &funcA]() { + time_keeper->start_track("funcB"); + std::this_thread::sleep_for(std::chrono::seconds(2)); + funcA(); + time_keeper->end_track("funcB"); + }; + + auto funcC = [&time_keeper, &funcB]() { + time_keeper->start_track("funcC"); + std::this_thread::sleep_for(std::chrono::seconds(3)); + funcB(); + time_keeper->end_track("funcC"); + }; + + funcC(); + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp new file mode 100644 index 0000000000000..96070f0f30b77 --- /dev/null +++ b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp @@ -0,0 +1,202 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// 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. +#ifndef AUTOWARE__UNIVERSE_UTILS__SYSTEM__TIME_KEEPER_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__SYSTEM__TIME_KEEPER_HPP_ + +#include "autoware/universe_utils/system/stop_watch.hpp" + +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include +#include + +namespace autoware::universe_utils +{ +/** + * @brief Class representing a node in the time tracking tree + */ +class ProcessingTimeNode : public std::enable_shared_from_this +{ +public: + /** + * @brief Construct a new ProcessingTimeNode object + * + * @param name Name of the node + */ + explicit ProcessingTimeNode(const std::string & name); + + /** + * @brief Add a child node + * + * @param name Name of the child node + * @return std::shared_ptr Shared pointer to the newly added child node + */ + std::shared_ptr add_child(const std::string & name); + + /** + * @brief Get the result string representing the node and its children in a tree structure + * + * @return std::string Result string representing the node and its children + */ + std::string to_string() const; + + /** + * @brief Construct a ProcessingTimeTree message from the node and its children + * + * @return tier4_debug_msgs::msg::ProcessingTimeTree Constructed ProcessingTimeTree message + */ + tier4_debug_msgs::msg::ProcessingTimeTree to_msg() const; + + /** + * @brief Get the parent node + * + * @return std::shared_ptr Shared pointer to the parent node + */ + std::shared_ptr get_parent_node() const; + + /** + * @brief Get the child nodes + * + * @return std::vector> Vector of shared pointers to the child + * nodes + */ + std::vector> get_child_nodes() const; + + /** + * @brief Set the processing time for the node + * + * @param processing_time Processing time to be set + */ + void set_time(const double processing_time); + + /** + * @brief Get the name of the node + * + * @return std::string Name of the node + */ + std::string get_name() const; + +private: + const std::string name_; //!< Name of the node + double processing_time_{0.0}; //!< Processing time of the node + std::shared_ptr parent_node_{nullptr}; //!< Shared pointer to the parent node + std::vector> + child_nodes_; //!< Vector of shared pointers to the child nodes +}; + +using ProcessingTimeDetail = + tier4_debug_msgs::msg::ProcessingTimeTree; //!< Alias for the ProcessingTimeTree message + +/** + * @brief Class for tracking and reporting the processing time of various functions + */ +class TimeKeeper +{ +public: + template + explicit TimeKeeper(Reporters... reporters) : current_time_node_(nullptr), root_node_(nullptr) + { + reporters_.reserve(sizeof...(Reporters)); + (add_reporter(reporters), ...); + } + + /** + * @brief Add a reporter to output processing times to an ostream + * + * @param os Pointer to the ostream object + */ + void add_reporter(std::ostream * os); + + /** + * @brief Add a reporter to publish processing times to an rclcpp publisher + * + * @param publisher Shared pointer to the rclcpp publisher + */ + void add_reporter(rclcpp::Publisher::SharedPtr publisher); + + /** + * @brief Add a reporter to publish processing times to an rclcpp publisher with + * std_msgs::msg::String + * + * @param publisher Shared pointer to the rclcpp publisher + */ + void add_reporter(rclcpp::Publisher::SharedPtr publisher); + + /** + * @brief Start tracking the processing time of a function + * + * @param func_name Name of the function to be tracked + */ + void start_track(const std::string & func_name); + + /** + * @brief End tracking the processing time of a function + * + * @param func_name Name of the function to end tracking + */ + void end_track(const std::string & func_name); + +private: + /** + * @brief Report the processing times to all registered reporters + */ + void report(); + + std::shared_ptr + current_time_node_; //!< Shared pointer to the current time node + std::shared_ptr root_node_; //!< Shared pointer to the root time node + autoware::universe_utils::StopWatch< + std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> + stop_watch_; //!< StopWatch object for tracking the processing time + + std::vector &)>> + reporters_; //!< Vector of functions for reporting the processing times +}; + +/** + * @brief Class for automatically tracking the processing time of a function within a scope + */ +class ScopedTimeTrack +{ +public: + /** + * @brief Construct a new ScopedTimeTrack object + * + * @param func_name Name of the function to be tracked + * @param time_keeper Reference to the TimeKeeper object + */ + ScopedTimeTrack(const std::string & func_name, TimeKeeper & time_keeper); + + /** + * @brief Destroy the ScopedTimeTrack object, ending the tracking of the function + */ + ~ScopedTimeTrack(); + +private: + const std::string func_name_; //!< Name of the function being tracked + TimeKeeper & time_keeper_; //!< Reference to the TimeKeeper object +}; + +} // namespace autoware::universe_utils + +#endif // AUTOWARE__UNIVERSE_UTILS__SYSTEM__TIME_KEEPER_HPP_ diff --git a/common/autoware_universe_utils/src/system/time_keeper.cpp b/common/autoware_universe_utils/src/system/time_keeper.cpp new file mode 100644 index 0000000000000..58bed6677227c --- /dev/null +++ b/common/autoware_universe_utils/src/system/time_keeper.cpp @@ -0,0 +1,175 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// 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 "autoware/universe_utils/system/time_keeper.hpp" + +#include +#include + +namespace autoware::universe_utils +{ + +ProcessingTimeNode::ProcessingTimeNode(const std::string & name) : name_(name) +{ +} + +std::shared_ptr ProcessingTimeNode::add_child(const std::string & name) +{ + auto new_child_node = std::make_shared(name); + new_child_node->parent_node_ = shared_from_this(); + child_nodes_.push_back(new_child_node); + return new_child_node; +} + +std::string ProcessingTimeNode::to_string() const +{ + std::function + construct_string = [&]( + const ProcessingTimeNode & node, std::ostringstream & oss, + const std::string & prefix, bool is_last, bool is_root) { + if (!is_root) { + oss << prefix << (is_last ? "└── " : "├── "); + } + oss << node.name_ << " (" << node.processing_time_ << "ms)\n"; + for (size_t i = 0; i < node.child_nodes_.size(); ++i) { + const auto & child = node.child_nodes_[i]; + construct_string( + *child, oss, prefix + (is_last ? " " : "│ "), i == node.child_nodes_.size() - 1, + false); + } + }; + + std::ostringstream oss; + construct_string(*this, oss, "", true, true); + return oss.str(); +} + +tier4_debug_msgs::msg::ProcessingTimeTree ProcessingTimeNode::to_msg() const +{ + tier4_debug_msgs::msg::ProcessingTimeTree time_tree_msg; + + std::function + construct_msg = [&]( + const ProcessingTimeNode & node, + tier4_debug_msgs::msg::ProcessingTimeTree & tree_msg, int parent_id) { + tier4_debug_msgs::msg::ProcessingTimeNode time_node_msg; + time_node_msg.name = node.name_; + time_node_msg.processing_time = node.processing_time_; + time_node_msg.id = tree_msg.nodes.size() + 1; + time_node_msg.parent_id = parent_id; + tree_msg.nodes.emplace_back(time_node_msg); + + for (const auto & child : node.child_nodes_) { + construct_msg(*child, tree_msg, time_node_msg.id); + } + }; + construct_msg(*this, time_tree_msg, 0); + + return time_tree_msg; +} + +std::shared_ptr ProcessingTimeNode::get_parent_node() const +{ + return parent_node_; +} +std::vector> ProcessingTimeNode::get_child_nodes() const +{ + return child_nodes_; +} +void ProcessingTimeNode::set_time(const double processing_time) +{ + processing_time_ = processing_time; +} +std::string ProcessingTimeNode::get_name() const +{ + return name_; +} + +void TimeKeeper::add_reporter(std::ostream * os) +{ + reporters_.emplace_back([os](const std::shared_ptr & node) { + *os << "==========================" << std::endl; + *os << node->to_string() << std::endl; + }); +} + +void TimeKeeper::add_reporter(rclcpp::Publisher::SharedPtr publisher) +{ + reporters_.emplace_back([publisher](const std::shared_ptr & node) { + publisher->publish(node->to_msg()); + }); +} + +void TimeKeeper::add_reporter(rclcpp::Publisher::SharedPtr publisher) +{ + reporters_.emplace_back([publisher](const std::shared_ptr & node) { + std_msgs::msg::String msg; + msg.data = node->to_string(); + publisher->publish(msg); + }); +} + +void TimeKeeper::start_track(const std::string & func_name) +{ + if (current_time_node_ == nullptr) { + current_time_node_ = std::make_shared(func_name); + root_node_ = current_time_node_; + } else { + current_time_node_ = current_time_node_->add_child(func_name); + } + stop_watch_.tic(func_name); +} + +void TimeKeeper::end_track(const std::string & func_name) +{ + if (current_time_node_->get_name() != func_name) { + throw std::runtime_error(fmt::format( + "You must call end_track({}) first, but end_track({}) is called", + current_time_node_->get_name(), func_name)); + } + const double processing_time = stop_watch_.toc(func_name); + current_time_node_->set_time(processing_time); + current_time_node_ = current_time_node_->get_parent_node(); + + if (current_time_node_ == nullptr) { + report(); + } +} + +void TimeKeeper::report() +{ + if (current_time_node_ != nullptr) { + throw std::runtime_error(fmt::format( + "You must call end_track({}) first, but report() is called", current_time_node_->get_name())); + } + for (const auto & reporter : reporters_) { + reporter(root_node_); + } + current_time_node_.reset(); + root_node_.reset(); +} + +ScopedTimeTrack::ScopedTimeTrack(const std::string & func_name, TimeKeeper & time_keeper) +: func_name_(func_name), time_keeper_(time_keeper) +{ + time_keeper_.start_track(func_name_); +} + +ScopedTimeTrack::~ScopedTimeTrack() +{ + time_keeper_.end_track(func_name_); +} + +} // namespace autoware::universe_utils diff --git a/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp b/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp new file mode 100644 index 0000000000000..71ca7cc74bec5 --- /dev/null +++ b/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp @@ -0,0 +1,53 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// 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 "autoware/universe_utils/system/time_keeper.hpp" + +#include +#include + +#include + +#include +#include +#include + +TEST(system, TimeKeeper) +{ + using autoware::universe_utils::ScopedTimeTrack; + using autoware::universe_utils::TimeKeeper; + + rclcpp::Node node{"sample_node"}; + + auto publisher = node.create_publisher( + "~/debug/processing_time_tree", 1); + + TimeKeeper time_keeper(&std::cerr, publisher); + + ScopedTimeTrack st{"main_func", time_keeper}; + + { // funcA + ScopedTimeTrack st{"funcA", time_keeper}; + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + + { // funcB + ScopedTimeTrack st{"funcB", time_keeper}; + std::this_thread::sleep_for(std::chrono::seconds(1)); + { // funcC + ScopedTimeTrack st{"funcC", time_keeper}; + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + } +} diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp index 7f2688ffaad95..399262f93e19d 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp @@ -17,7 +17,6 @@ #include "autoware/path_optimizer/type_alias.hpp" #include "autoware/universe_utils/ros/update_param.hpp" -#include "autoware/universe_utils/system/stop_watch.hpp" #include "rclcpp/rclcpp.hpp" #include @@ -43,57 +42,6 @@ struct PlannerData double ego_vel{}; }; -struct TimeKeeper -{ - void init() - { - accumulated_msg = "\n"; - accumulated_time = 0.0; - } - - template - TimeKeeper & operator<<(const T & msg) - { - latest_stream << msg; - return *this; - } - - void endLine() - { - const auto msg = latest_stream.str(); - accumulated_msg += msg + "\n"; - latest_stream.str(""); - - if (enable_calculation_time_info) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("autoware_path_optimizer.time"), msg); - } - } - - void tic(const std::string & func_name) { stop_watch_.tic(func_name); } - - void toc(const std::string & func_name, const std::string & white_spaces) - { - const double elapsed_time = stop_watch_.toc(func_name); - *this << white_spaces << func_name << ":= " << elapsed_time << " [ms]"; - accumulated_time = elapsed_time; - endLine(); - } - - std::string getLog() const { return accumulated_msg; } - - bool enable_calculation_time_info; - std::string accumulated_msg = "\n"; - std::stringstream latest_stream; - - double getAccumulatedTime() const { return accumulated_time; } - - double accumulated_time{0.0}; - - autoware::universe_utils::StopWatch< - std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> - stop_watch_; -}; - struct DebugData { // settting diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp index 4303e5c7e05b4..3443ab663b642 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp @@ -19,6 +19,7 @@ #include "autoware/path_optimizer/state_equation_generator.hpp" #include "autoware/path_optimizer/type_alias.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "gtest/gtest.h" #include "interpolation/linear_interpolation.hpp" @@ -107,7 +108,7 @@ class MPTOptimizer rclcpp::Node * node, const bool enable_debug_info, const EgoNearestParam ego_nearest_param, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const TrajectoryParam & traj_param, const std::shared_ptr debug_data_ptr, - const std::shared_ptr time_keeper_ptr); + const std::shared_ptr time_keeper_); std::vector optimizeTrajectory(const PlannerData & planner_data); std::optional> getPrevOptimizedTrajectoryPoints() const; @@ -222,7 +223,7 @@ class MPTOptimizer autoware::vehicle_info_utils::VehicleInfo vehicle_info_; TrajectoryParam traj_param_; mutable std::shared_ptr debug_data_ptr_; - mutable std::shared_ptr time_keeper_ptr_; + mutable std::shared_ptr time_keeper_; rclcpp::Logger logger_; MPTParam mpt_param_; diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp index 359c20f2a4d29..6f75c649e02b2 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp @@ -22,10 +22,11 @@ #include "autoware/path_optimizer/type_alias.hpp" #include "autoware/universe_utils/ros/logger_level_configure.hpp" #include "autoware/universe_utils/ros/polling_subscriber.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" -#include "rclcpp/rclcpp.hpp" #include +#include #include #include @@ -65,7 +66,7 @@ class PathOptimizer : public rclcpp::Node // argument variables autoware::vehicle_info_utils::VehicleInfo vehicle_info_{}; mutable std::shared_ptr debug_data_ptr_{nullptr}; - mutable std::shared_ptr time_keeper_ptr_{nullptr}; + mutable std::shared_ptr time_keeper_{nullptr}; // flags for some functions bool enable_pub_debug_marker_; @@ -98,6 +99,8 @@ class PathOptimizer : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_markers_pub_; rclcpp::Publisher::SharedPtr debug_calculation_time_str_pub_; rclcpp::Publisher::SharedPtr debug_calculation_time_float_pub_; + rclcpp::Publisher::SharedPtr + debug_processing_time_detail_pub_; // parameter callback rcl_interfaces::msg::SetParametersResult onParam( diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp index b8162f5ef2d32..1d23d7788dca1 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp @@ -18,6 +18,7 @@ #include "autoware/path_optimizer/common_structs.hpp" #include "autoware/path_optimizer/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include "autoware/path_optimizer/vehicle_model/vehicle_model_interface.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include #include @@ -39,9 +40,9 @@ class StateEquationGenerator StateEquationGenerator() = default; StateEquationGenerator( const double wheel_base, const double max_steer_rad, - const std::shared_ptr time_keeper_ptr) + const std::shared_ptr time_keeper) : vehicle_model_ptr_(std::make_unique(wheel_base, max_steer_rad)), - time_keeper_ptr_(time_keeper_ptr) + time_keeper_(time_keeper) { } @@ -56,7 +57,7 @@ class StateEquationGenerator private: std::unique_ptr vehicle_model_ptr_; - mutable std::shared_ptr time_keeper_ptr_; + mutable std::shared_ptr time_keeper_; }; } // namespace autoware::path_optimizer #endif // AUTOWARE__PATH_OPTIMIZER__STATE_EQUATION_GENERATOR_HPP_ diff --git a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp index b5fd266006a48..45943d7deec09 100644 --- a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp +++ b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp @@ -395,13 +395,13 @@ MPTOptimizer::MPTOptimizer( rclcpp::Node * node, const bool enable_debug_info, const EgoNearestParam ego_nearest_param, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const TrajectoryParam & traj_param, const std::shared_ptr debug_data_ptr, - const std::shared_ptr time_keeper_ptr) + const std::shared_ptr time_keeper) : enable_debug_info_(enable_debug_info), ego_nearest_param_(ego_nearest_param), vehicle_info_(vehicle_info), traj_param_(traj_param), debug_data_ptr_(debug_data_ptr), - time_keeper_ptr_(time_keeper_ptr), + time_keeper_(time_keeper), logger_(node->get_logger().get_child("mpt_optimizer")) { // initialize mpt param @@ -411,7 +411,7 @@ MPTOptimizer::MPTOptimizer( // state equation generator state_equation_generator_ = - StateEquationGenerator(vehicle_info_.wheel_base_m, mpt_param_.max_steer_rad, time_keeper_ptr_); + StateEquationGenerator(vehicle_info_.wheel_base_m, mpt_param_.max_steer_rad, time_keeper_); // osqp solver osqp_solver_ptr_ = std::make_unique(osqp_epsilon_); @@ -470,7 +470,7 @@ void MPTOptimizer::onParam(const std::vector & parameters) std::vector MPTOptimizer::optimizeTrajectory(const PlannerData & planner_data) { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & p = planner_data; const auto & traj_points = p.traj_points; @@ -520,8 +520,6 @@ std::vector MPTOptimizer::optimizeTrajectory(const PlannerData // 8. publish trajectories for debug publishDebugTrajectories(p.header, ref_points, *mpt_traj_points); - time_keeper_ptr_->toc(__func__, " "); - debug_data_ptr_->ref_points = ref_points; prev_ref_points_ptr_ = std::make_shared>(ref_points); prev_optimized_traj_points_ptr_ = @@ -541,7 +539,7 @@ std::optional> MPTOptimizer::getPrevOptimizedTrajec std::vector MPTOptimizer::calcReferencePoints( const PlannerData & planner_data, const std::vector & smoothed_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & p = planner_data; @@ -549,14 +547,14 @@ std::vector MPTOptimizer::calcReferencePoints( const double backward_traj_length = traj_param_.output_backward_traj_length; // 1. resample and convert smoothed points type from trajectory points to reference points - time_keeper_ptr_->tic("resampleReferencePoints"); + time_keeper_->start_track("resampleReferencePoints"); auto ref_points = [&]() { const auto resampled_smoothed_points = trajectory_utils::resampleTrajectoryPointsWithoutStopPoint( smoothed_points, mpt_param_.delta_arc_length); return trajectory_utils::convertToReferencePoints(resampled_smoothed_points); }(); - time_keeper_ptr_->toc("resampleReferencePoints", " "); + time_keeper_->end_track("resampleReferencePoints"); // 2. crop forward and backward with margin, and calculate spline interpolation // NOTE: Margin is added to calculate orientation, curvature, etc precisely. @@ -611,8 +609,6 @@ std::vector MPTOptimizer::calcReferencePoints( ref_points.resize(mpt_param_.num_points); } - time_keeper_ptr_->toc(__func__, " "); - return ref_points; } @@ -639,7 +635,7 @@ void MPTOptimizer::updateCurvature( void MPTOptimizer::updateFixedPoint(std::vector & ref_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (!prev_ref_points_ptr_) { // no fixed point @@ -681,8 +677,6 @@ void MPTOptimizer::updateFixedPoint(std::vector & ref_points) co ref_points.front().curvature = front_point.curvature; ref_points.front().fixed_kinematic_state = front_point.optimized_kinematic_state; } - - time_keeper_ptr_->toc(__func__, " "); } void MPTOptimizer::updateDeltaArcLength(std::vector & ref_points) const @@ -697,7 +691,7 @@ void MPTOptimizer::updateDeltaArcLength(std::vector & ref_points void MPTOptimizer::updateExtraPoints(std::vector & ref_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // alpha for (size_t i = 0; i < ref_points.size(); ++i) { @@ -786,8 +780,6 @@ void MPTOptimizer::updateExtraPoints(std::vector & ref_points) c } } } - - time_keeper_ptr_->toc(__func__, " "); } void MPTOptimizer::updateBounds( @@ -796,7 +788,7 @@ void MPTOptimizer::updateBounds( const std::vector & right_bound, const geometry_msgs::msg::Pose & ego_pose, const double ego_vel) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const double soft_road_clearance = mpt_param_.soft_clearance_from_road + vehicle_info_.vehicle_width_m / 2.0; @@ -844,8 +836,6 @@ void MPTOptimizer::updateBounds( } } */ - - time_keeper_ptr_->toc(__func__, " "); return; } @@ -1104,7 +1094,7 @@ void MPTOptimizer::updateVehicleBounds( std::vector & ref_points, const SplineInterpolationPoints2d & ref_points_spline) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); for (size_t p_idx = 0; p_idx < ref_points.size(); ++p_idx) { const auto & ref_point = ref_points.at(p_idx); @@ -1160,8 +1150,6 @@ void MPTOptimizer::updateVehicleBounds( ref_points.at(p_idx).pose_on_constraints.push_back(vehicle_bounds_pose); } } - - time_keeper_ptr_->toc(__func__, " "); } // cost function: J = x' Q x + u' R u @@ -1169,7 +1157,7 @@ MPTOptimizer::ValueMatrix MPTOptimizer::calcValueMatrix( const std::vector & ref_points, const std::vector & traj_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1228,7 +1216,6 @@ MPTOptimizer::ValueMatrix MPTOptimizer::calcValueMatrix( R_sparse_mat.setFromTriplets(R_triplet_vec.begin(), R_triplet_vec.end()); - time_keeper_ptr_->toc(__func__, " "); return ValueMatrix{Q_sparse_mat, R_sparse_mat}; } @@ -1236,7 +1223,7 @@ MPTOptimizer::ObjectiveMatrix MPTOptimizer::calcObjectiveMatrix( [[maybe_unused]] const StateEquationGenerator::Matrix & mpt_mat, const ValueMatrix & val_mat, const std::vector & ref_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1286,7 +1273,6 @@ MPTOptimizer::ObjectiveMatrix MPTOptimizer::calcObjectiveMatrix( obj_matrix.hessian = H; obj_matrix.gradient = g; - time_keeper_ptr_->toc(__func__, " "); return obj_matrix; } @@ -1297,7 +1283,7 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::calcConstraintMatrix( const StateEquationGenerator::Matrix & mpt_mat, const std::vector & ref_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1451,7 +1437,6 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::calcConstraintMatrix( A_rows_end += N_u; } - time_keeper_ptr_->toc(__func__, " "); return ConstraintMatrix{A, lb, ub}; } @@ -1477,7 +1462,7 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( const std::vector & ref_points, const ObjectiveMatrix & obj_mat, const ConstraintMatrix & const_mat) { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1510,7 +1495,7 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( const auto lower_bound = toStdVector(updated_const_mat.lower_bound); // initialize or update solver according to warm start - time_keeper_ptr_->tic("initOsqp"); + time_keeper_->start_track("initOsqp"); const autoware::common::osqp::CSC_Matrix P_csc = autoware::common::osqp::calCSCMatrixTrapezoidal(H); @@ -1530,13 +1515,12 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( } prev_mat_n_ = H.rows(); prev_mat_m_ = A.rows(); - - time_keeper_ptr_->toc("initOsqp", " "); + time_keeper_->end_track("initOsqp"); // solve qp - time_keeper_ptr_->tic("solveOsqp"); + time_keeper_->start_track("solveOsqp"); const auto result = osqp_solver_ptr_->optimize(); - time_keeper_ptr_->toc("solveOsqp", " "); + time_keeper_->end_track("solveOsqp"); // check solution status const int solution_status = std::get<3>(result); @@ -1563,8 +1547,6 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( const Eigen::VectorXd optimized_variables = Eigen::Map(&optimization_result[0], N_v); - time_keeper_ptr_->toc(__func__, " "); - if (u0) { // manual warm start return static_cast(optimized_variables + *u0); } @@ -1647,7 +1629,7 @@ std::optional> MPTOptimizer::calcMPTPoints( std::vector & ref_points, const Eigen::VectorXd & optimized_variables, [[maybe_unused]] const StateEquationGenerator::Matrix & mpt_mat) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1700,7 +1682,6 @@ std::optional> MPTOptimizer::calcMPTPoints( traj_points.push_back(traj_point); } - time_keeper_ptr_->toc(__func__, " "); return traj_points; } @@ -1708,7 +1689,7 @@ void MPTOptimizer::publishDebugTrajectories( const std_msgs::msg::Header & header, const std::vector & ref_points, const std::vector & mpt_traj_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // reference points const auto ref_traj = autoware::motion_utils::convertToTrajectory( @@ -1723,8 +1704,6 @@ void MPTOptimizer::publishDebugTrajectories( // mpt points const auto mpt_traj = autoware::motion_utils::convertToTrajectory(mpt_traj_points, header); debug_mpt_traj_pub_->publish(mpt_traj); - - time_keeper_ptr_->toc(__func__, " "); } std::vector MPTOptimizer::extractFixedPoints( diff --git a/planning/autoware_path_optimizer/src/node.cpp b/planning/autoware_path_optimizer/src/node.cpp index 3899867a9dcce..7c73d1ad1fee3 100644 --- a/planning/autoware_path_optimizer/src/node.cpp +++ b/planning/autoware_path_optimizer/src/node.cpp @@ -23,6 +23,7 @@ #include "rclcpp/time.hpp" #include +#include #include namespace autoware::path_optimizer @@ -38,7 +39,8 @@ std::vector concatVectors(const std::vector & prev_vector, const std::vect return concatenated_vector; } -StringStamped createStringStamped(const rclcpp::Time & now, const std::string & data) +[[maybe_unused]] StringStamped createStringStamped( + const rclcpp::Time & now, const std::string & data) { StringStamped msg; msg.stamp = now; @@ -46,7 +48,7 @@ StringStamped createStringStamped(const rclcpp::Time & now, const std::string & return msg; } -Float64Stamped createFloat64Stamped(const rclcpp::Time & now, const float & data) +[[maybe_unused]] Float64Stamped createFloat64Stamped(const rclcpp::Time & now, const float & data) { Float64Stamped msg; msg.stamp = now; @@ -85,8 +87,7 @@ std::vector calcSegmentLengthVector(const std::vector & PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options) : Node("path_optimizer", node_options), vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo()), - debug_data_ptr_(std::make_shared()), - time_keeper_ptr_(std::make_shared()) + debug_data_ptr_(std::make_shared()) { // interface publisher traj_pub_ = create_publisher("~/output/path", 1); @@ -102,6 +103,9 @@ PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options) debug_calculation_time_str_pub_ = create_publisher("~/debug/calculation_time", 1); debug_calculation_time_float_pub_ = create_publisher("~/debug/processing_time_ms", 1); + debug_processing_time_detail_pub_ = + create_publisher( + "~/debug/processing_time_detail_ms", 1); { // parameters // parameter for option @@ -120,8 +124,6 @@ PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options) // parameter for debug info enable_debug_info_ = declare_parameter("option.debug.enable_debug_info"); - time_keeper_ptr_->enable_calculation_time_info = - declare_parameter("option.debug.enable_calculation_time_info"); vehicle_stop_margin_outside_drivable_area_ = declare_parameter("common.vehicle_stop_margin_outside_drivable_area"); @@ -133,11 +135,14 @@ PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options) traj_param_ = TrajectoryParam(this); } + time_keeper_ = + std::make_shared(debug_processing_time_detail_pub_); + // create core algorithm pointers with parameter declaration replan_checker_ptr_ = std::make_shared(this, ego_nearest_param_); mpt_optimizer_ptr_ = std::make_shared( this, enable_debug_info_, ego_nearest_param_, vehicle_info_, traj_param_, debug_data_ptr_, - time_keeper_ptr_); + time_keeper_); // reset planners // NOTE: This function must be called after core algorithms (e.g. mpt_optimizer_) have been @@ -177,9 +182,6 @@ rcl_interfaces::msg::SetParametersResult PathOptimizer::onParam( // parameters for debug info updateParam(parameters, "option.debug.enable_debug_info", enable_debug_info_); - updateParam( - parameters, "option.debug.enable_calculation_time_info", - time_keeper_ptr_->enable_calculation_time_info); updateParam( parameters, "common.vehicle_stop_margin_outside_drivable_area", @@ -220,8 +222,7 @@ void PathOptimizer::resetPreviousData() void PathOptimizer::onPath(const Path::ConstSharedPtr path_ptr) { - time_keeper_ptr_->init(); - time_keeper_ptr_->tic(__func__); + time_keeper_->start_track(__func__); // check if input path is valid if (!checkInputPath(*path_ptr, *get_clock())) { @@ -267,21 +268,21 @@ void PathOptimizer::onPath(const Path::ConstSharedPtr path_ptr) // 5. publish debug data publishDebugData(planner_data.header); - time_keeper_ptr_->toc(__func__, ""); - *time_keeper_ptr_ << "========================================"; - time_keeper_ptr_->endLine(); - // publish calculation_time // NOTE: This function must be called after measuring onPath calculation time + /* const auto calculation_time_msg = createStringStamped(now(), time_keeper_ptr_->getLog()); debug_calculation_time_str_pub_->publish(calculation_time_msg); debug_calculation_time_float_pub_->publish( createFloat64Stamped(now(), time_keeper_ptr_->getAccumulatedTime())); + */ const auto output_traj_msg = autoware::motion_utils::convertToTrajectory(full_traj_points, path_ptr->header); traj_pub_->publish(output_traj_msg); published_time_publisher_->publish_if_subscribed(traj_pub_, output_traj_msg.header.stamp); + + time_keeper_->end_track(__func__); } bool PathOptimizer::checkInputPath(const Path & path, rclcpp::Clock clock) const @@ -319,7 +320,7 @@ PlannerData PathOptimizer::createPlannerData( std::vector PathOptimizer::generateOptimizedTrajectory( const PlannerData & planner_data) { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & input_traj_points = planner_data.traj_points; @@ -339,13 +340,12 @@ std::vector PathOptimizer::generateOptimizedTrajectory( // 4. publish debug marker publishDebugMarkerOfOptimization(optimized_traj_points); - time_keeper_ptr_->toc(__func__, " "); return optimized_traj_points; } std::vector PathOptimizer::optimizeTrajectory(const PlannerData & planner_data) { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & p = planner_data; // 1. check if replan (= optimization) is required @@ -372,7 +372,6 @@ std::vector PathOptimizer::optimizeTrajectory(const PlannerData // with model predictive trajectory const auto mpt_traj = mpt_optimizer_ptr_->optimizeTrajectory(planner_data); - time_keeper_ptr_->toc(__func__, " "); return mpt_traj; } @@ -391,7 +390,7 @@ void PathOptimizer::applyInputVelocity( const std::vector & input_traj_points, const geometry_msgs::msg::Pose & ego_pose) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // crop forward for faster calculation const auto forward_cropped_input_traj_points = [&]() { @@ -486,14 +485,12 @@ void PathOptimizer::applyInputVelocity( trajectory_utils::insertStopPoint(output_traj_points, input_stop_pose, *stop_seg_idx); } } - - time_keeper_ptr_->toc(__func__, " "); } void PathOptimizer::insertZeroVelocityOutsideDrivableArea( const PlannerData & planner_data, std::vector & optimized_traj_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (optimized_traj_points.empty()) { return; @@ -553,13 +550,11 @@ void PathOptimizer::insertZeroVelocityOutsideDrivableArea( } else { debug_data_ptr_->stop_pose_by_drivable_area = std::nullopt; } - - time_keeper_ptr_->toc(__func__, " "); } void PathOptimizer::publishVirtualWall(const geometry_msgs::msg::Pose & stop_pose) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); auto virtual_wall_marker = autoware::motion_utils::createStopVirtualWallMarker( stop_pose, "outside drivable area", now(), 0, vehicle_info_.max_longitudinal_offset_m); @@ -569,36 +564,33 @@ void PathOptimizer::publishVirtualWall(const geometry_msgs::msg::Pose & stop_pos } virtual_wall_pub_->publish(virtual_wall_marker); - time_keeper_ptr_->toc(__func__, " "); } void PathOptimizer::publishDebugMarkerOfOptimization( const std::vector & traj_points) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (!enable_pub_debug_marker_) { return; } - time_keeper_ptr_->tic(__func__); - // debug marker - time_keeper_ptr_->tic("getDebugMarker"); + time_keeper_->start_track("getDebugMarker"); const auto debug_marker = getDebugMarker(*debug_data_ptr_, traj_points, vehicle_info_, enable_pub_extra_debug_marker_); - time_keeper_ptr_->toc("getDebugMarker", " "); + time_keeper_->end_track("getDebugMarker"); - time_keeper_ptr_->tic("publishDebugMarker"); + time_keeper_->start_track("publishDebugMarker"); debug_markers_pub_->publish(debug_marker); - time_keeper_ptr_->toc("publishDebugMarker", " "); - - time_keeper_ptr_->toc(__func__, " "); + time_keeper_->end_track("publishDebugMarker"); } std::vector PathOptimizer::extendTrajectory( const std::vector & traj_points, const std::vector & optimized_traj_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & joint_start_pose = optimized_traj_points.back().pose; @@ -654,20 +646,17 @@ std::vector PathOptimizer::extendTrajectory( // debug_data_ptr_->extended_traj_points = // extended_traj_points ? *extended_traj_points : std::vector(); - time_keeper_ptr_->toc(__func__, " "); return resampled_traj_points; } void PathOptimizer::publishDebugData(const Header & header) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // publish trajectories const auto debug_extended_traj = autoware::motion_utils::convertToTrajectory(debug_data_ptr_->extended_traj_points, header); debug_extended_traj_pub_->publish(debug_extended_traj); - - time_keeper_ptr_->toc(__func__, " "); } } // namespace autoware::path_optimizer diff --git a/planning/autoware_path_optimizer/src/state_equation_generator.cpp b/planning/autoware_path_optimizer/src/state_equation_generator.cpp index 5aa2ab4ffbfa8..74033c5834db2 100644 --- a/planning/autoware_path_optimizer/src/state_equation_generator.cpp +++ b/planning/autoware_path_optimizer/src/state_equation_generator.cpp @@ -23,7 +23,7 @@ namespace autoware::path_optimizer StateEquationGenerator::Matrix StateEquationGenerator::calcMatrix( const std::vector & ref_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const size_t D_x = vehicle_model_ptr_->getDimX(); const size_t D_u = vehicle_model_ptr_->getDimU(); @@ -60,7 +60,6 @@ StateEquationGenerator::Matrix StateEquationGenerator::calcMatrix( W.segment(i * D_x, D_x) = Wd; } - time_keeper_ptr_->toc(__func__, " "); return Matrix{A, B, W}; } From a70aa0e55d8debc49b4d39f9118ef72c6697fe3b Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 9 Jul 2024 09:29:58 +0900 Subject: [PATCH 09/19] fix(multi_object_tracker): fix publish interval adjust timing (#7904) refactor: optimize publish time check in multi_object_tracker_node.cpp Signed-off-by: Taekjin LEE --- .../multi_object_tracker/src/multi_object_tracker_node.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/perception/multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/multi_object_tracker/src/multi_object_tracker_node.cpp index d3e1c9686b69b..198d9e2238566 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_node.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_node.cpp @@ -221,8 +221,9 @@ void MultiObjectTracker::onTrigger() } else { // Publish if the next publish time is close const double minimum_publish_interval = publisher_period_ * 0.70; // 70% of the period - if ((current_time - last_published_time_).seconds() > minimum_publish_interval) { - checkAndPublish(current_time); + const rclcpp::Time publish_time = this->now(); + if ((publish_time - last_published_time_).seconds() > minimum_publish_interval) { + checkAndPublish(publish_time); } } } From 7dcdd676f55f4c24893a589da0e543a2d7a72a97 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 9 Jul 2024 09:55:53 +0900 Subject: [PATCH 10/19] refactor(tier4_perception_launch): add maintainer to tier4_perception_launch (#7893) refactor: add maintainer to tier4_perception_launch Signed-off-by: Taekjin LEE --- launch/tier4_perception_launch/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/launch/tier4_perception_launch/package.xml b/launch/tier4_perception_launch/package.xml index 57d4b209efeef..5c15a937304ad 100644 --- a/launch/tier4_perception_launch/package.xml +++ b/launch/tier4_perception_launch/package.xml @@ -7,6 +7,7 @@ Yukihiro Saito Shunsuke Miura Yoshi Ri + Taekjin Lee Apache License 2.0 ament_cmake_auto From 6d0247e078d2e944c482b78a95eac9025f5863c2 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 9 Jul 2024 11:24:09 +0900 Subject: [PATCH 11/19] refactor(euclidean_cluster)!: fix namespace and directory structure (#7887) * refactor: update include paths for euclidean_cluster code Signed-off-by: Taekjin LEE refactor: fix namespace Signed-off-by: Taekjin LEE chore: update include paths for euclidean_cluster code Signed-off-by: Taekjin LEE * style(pre-commit): autofix --------- Signed-off-by: Taekjin LEE Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/debugger/debugger.hpp | 3 -- .../src/detection_by_tracker_node.cpp | 4 +-- .../src/detection_by_tracker_node.hpp | 8 +++--- perception/euclidean_cluster/CMakeLists.txt | 28 +++++++++---------- .../euclidean_cluster/euclidean_cluster.hpp | 8 +++--- .../euclidean_cluster_interface.hpp | 4 +-- .../euclidean_cluster/utils.hpp | 4 +-- .../voxel_grid_based_euclidean_cluster.hpp | 8 +++--- .../launch/euclidean_cluster.launch.py | 4 +-- ...xel_grid_based_euclidean_cluster.launch.py | 4 +-- .../lib/euclidean_cluster.cpp | 6 ++-- perception/euclidean_cluster/lib/utils.cpp | 6 ++-- .../voxel_grid_based_euclidean_cluster.cpp | 6 ++-- .../src/euclidean_cluster_node.cpp | 8 +++--- .../src/euclidean_cluster_node.hpp | 7 +++-- ...oxel_grid_based_euclidean_cluster_node.cpp | 8 +++--- ...oxel_grid_based_euclidean_cluster_node.hpp | 6 ++-- .../src/roi_pointcloud_fusion/node.cpp | 4 +-- 18 files changed, 62 insertions(+), 64 deletions(-) rename perception/euclidean_cluster/include/{ => autoware}/euclidean_cluster/euclidean_cluster.hpp (87%) rename perception/euclidean_cluster/include/{ => autoware}/euclidean_cluster/euclidean_cluster_interface.hpp (95%) rename perception/euclidean_cluster/include/{ => autoware}/euclidean_cluster/utils.hpp (94%) rename perception/euclidean_cluster/include/{ => autoware}/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp (90%) diff --git a/perception/detection_by_tracker/src/debugger/debugger.hpp b/perception/detection_by_tracker/src/debugger/debugger.hpp index 56f37fb10043d..bda42aa5f0b55 100644 --- a/perception/detection_by_tracker/src/debugger/debugger.hpp +++ b/perception/detection_by_tracker/src/debugger/debugger.hpp @@ -17,9 +17,6 @@ #include "autoware/universe_utils/ros/debug_publisher.hpp" #include "autoware/universe_utils/system/stop_watch.hpp" -#include "euclidean_cluster/euclidean_cluster.hpp" -#include "euclidean_cluster/utils.hpp" -#include "euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" #include diff --git a/perception/detection_by_tracker/src/detection_by_tracker_node.cpp b/perception/detection_by_tracker/src/detection_by_tracker_node.cpp index c22d015aa11a4..4c65500d96aaa 100644 --- a/perception/detection_by_tracker/src/detection_by_tracker_node.cpp +++ b/perception/detection_by_tracker/src/detection_by_tracker_node.cpp @@ -115,7 +115,7 @@ DetectionByTracker::DetectionByTracker(const rclcpp::NodeOptions & node_options) setMaxSearchRange(); shape_estimator_ = std::make_shared(true, true); - cluster_ = std::make_shared( + cluster_ = std::make_shared( false, 10, 10000, 0.7, 0.3, 0); debugger_ = std::make_shared(this); published_time_publisher_ = @@ -277,7 +277,7 @@ float DetectionByTracker::optimizeUnderSegmentedObject( const auto & label = target_object.classification.front().label; // initialize clustering parameters - euclidean_cluster::VoxelGridBasedEuclideanCluster cluster( + autoware::euclidean_cluster::VoxelGridBasedEuclideanCluster cluster( false, 4, 10000, initial_cluster_range, initial_voxel_size, 0); // convert to pcl diff --git a/perception/detection_by_tracker/src/detection_by_tracker_node.hpp b/perception/detection_by_tracker/src/detection_by_tracker_node.hpp index 095708fb4b51f..9c974d72cfdca 100644 --- a/perception/detection_by_tracker/src/detection_by_tracker_node.hpp +++ b/perception/detection_by_tracker/src/detection_by_tracker_node.hpp @@ -15,12 +15,12 @@ #ifndef DETECTION_BY_TRACKER_NODE_HPP_ #define DETECTION_BY_TRACKER_NODE_HPP_ +#include "autoware/euclidean_cluster/euclidean_cluster.hpp" +#include "autoware/euclidean_cluster/utils.hpp" +#include "autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" #include "autoware/shape_estimation/shape_estimator.hpp" #include "autoware/universe_utils/ros/published_time_publisher.hpp" #include "debugger/debugger.hpp" -#include "euclidean_cluster/euclidean_cluster.hpp" -#include "euclidean_cluster/utils.hpp" -#include "euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" #include "tracker/tracker_handler.hpp" #include "utils/utils.hpp" @@ -69,7 +69,7 @@ class DetectionByTracker : public rclcpp::Node TrackerHandler tracker_handler_; std::shared_ptr shape_estimator_; - std::shared_ptr cluster_; + std::shared_ptr cluster_; std::shared_ptr debugger_; std::map max_search_distance_for_merger_; std::map max_search_distance_for_divider_; diff --git a/perception/euclidean_cluster/CMakeLists.txt b/perception/euclidean_cluster/CMakeLists.txt index fe1fa9fda5fca..dacbcf460a14a 100644 --- a/perception/euclidean_cluster/CMakeLists.txt +++ b/perception/euclidean_cluster/CMakeLists.txt @@ -13,45 +13,45 @@ include_directories( ${PCL_INCLUDE_DIRS} ) -ament_auto_add_library(cluster_lib SHARED - lib/utils.cpp +ament_auto_add_library(${PROJECT_NAME}_lib SHARED lib/euclidean_cluster.cpp lib/voxel_grid_based_euclidean_cluster.cpp + lib/utils.cpp ) -target_link_libraries(cluster_lib +target_link_libraries(${PROJECT_NAME}_lib ${PCL_LIBRARIES} ) -target_include_directories(cluster_lib +target_include_directories(${PROJECT_NAME}_lib PUBLIC $ $ ) -ament_auto_add_library(euclidean_cluster_node_core SHARED +ament_auto_add_library(${PROJECT_NAME}_node_core SHARED src/euclidean_cluster_node.cpp ) -target_link_libraries(euclidean_cluster_node_core +target_link_libraries(${PROJECT_NAME}_node_core ${PCL_LIBRARIES} - cluster_lib + ${PROJECT_NAME}_lib ) -rclcpp_components_register_node(euclidean_cluster_node_core - PLUGIN "euclidean_cluster::EuclideanClusterNode" +rclcpp_components_register_node(${PROJECT_NAME}_node_core + PLUGIN "autoware::euclidean_cluster::EuclideanClusterNode" EXECUTABLE euclidean_cluster_node ) -ament_auto_add_library(voxel_grid_based_euclidean_cluster_node_core SHARED +ament_auto_add_library(${PROJECT_NAME}_voxel_grid_node_core SHARED src/voxel_grid_based_euclidean_cluster_node.cpp ) -target_link_libraries(voxel_grid_based_euclidean_cluster_node_core +target_link_libraries(${PROJECT_NAME}_voxel_grid_node_core ${PCL_LIBRARIES} - cluster_lib + ${PROJECT_NAME}_lib ) -rclcpp_components_register_node(voxel_grid_based_euclidean_cluster_node_core - PLUGIN "euclidean_cluster::VoxelGridBasedEuclideanClusterNode" +rclcpp_components_register_node(${PROJECT_NAME}_voxel_grid_node_core + PLUGIN "autoware::euclidean_cluster::VoxelGridBasedEuclideanClusterNode" EXECUTABLE voxel_grid_based_euclidean_cluster_node ) diff --git a/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster.hpp b/perception/euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster.hpp similarity index 87% rename from perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster.hpp rename to perception/euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster.hpp index aec3f56936828..8c4aad537cc43 100644 --- a/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster.hpp +++ b/perception/euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster.hpp @@ -14,14 +14,14 @@ #pragma once -#include "euclidean_cluster/euclidean_cluster_interface.hpp" -#include "euclidean_cluster/utils.hpp" +#include "autoware/euclidean_cluster/euclidean_cluster_interface.hpp" +#include "autoware/euclidean_cluster/utils.hpp" #include #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { class EuclideanCluster : public EuclideanClusterInterface { @@ -42,4 +42,4 @@ class EuclideanCluster : public EuclideanClusterInterface float tolerance_; }; -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster_interface.hpp b/perception/euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster_interface.hpp similarity index 95% rename from perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster_interface.hpp rename to perception/euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster_interface.hpp index 65b907f747666..461d75d4931db 100644 --- a/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster_interface.hpp +++ b/perception/euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster_interface.hpp @@ -24,7 +24,7 @@ #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { class EuclideanClusterInterface { @@ -54,4 +54,4 @@ class EuclideanClusterInterface int max_cluster_size_; }; -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/include/euclidean_cluster/utils.hpp b/perception/euclidean_cluster/include/autoware/euclidean_cluster/utils.hpp similarity index 94% rename from perception/euclidean_cluster/include/euclidean_cluster/utils.hpp rename to perception/euclidean_cluster/include/autoware/euclidean_cluster/utils.hpp index 50d2306d48f72..a52c0840c4d41 100644 --- a/perception/euclidean_cluster/include/euclidean_cluster/utils.hpp +++ b/perception/euclidean_cluster/include/autoware/euclidean_cluster/utils.hpp @@ -24,7 +24,7 @@ #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud); void convertPointCloudClusters2Msg( @@ -37,4 +37,4 @@ void convertPointCloudClusters2Msg( void convertObjectMsg2SensorMsg( const tier4_perception_msgs::msg::DetectedObjectsWithFeature & input, sensor_msgs::msg::PointCloud2 & output); -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/include/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp b/perception/euclidean_cluster/include/autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp similarity index 90% rename from perception/euclidean_cluster/include/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp rename to perception/euclidean_cluster/include/autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp index 375cc2d19a01f..5c51bb85ce177 100644 --- a/perception/euclidean_cluster/include/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp +++ b/perception/euclidean_cluster/include/autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp @@ -14,15 +14,15 @@ #pragma once -#include "euclidean_cluster/euclidean_cluster_interface.hpp" -#include "euclidean_cluster/utils.hpp" +#include "autoware/euclidean_cluster/euclidean_cluster_interface.hpp" +#include "autoware/euclidean_cluster/utils.hpp" #include #include #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { class VoxelGridBasedEuclideanCluster : public EuclideanClusterInterface { @@ -52,4 +52,4 @@ class VoxelGridBasedEuclideanCluster : public EuclideanClusterInterface int min_points_number_per_voxel_; }; -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py index b8d4f61a9cf28..053438a4352b0 100644 --- a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py +++ b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py @@ -49,7 +49,7 @@ def load_composable_node_param(param_path): use_low_height_euclidean_component = ComposableNode( package=pkg, - plugin="euclidean_cluster::EuclideanClusterNode", + plugin="autoware::euclidean_cluster::EuclideanClusterNode", name=AnonName("euclidean_cluster"), remappings=[ ("input", "low_height/pointcloud"), @@ -60,7 +60,7 @@ def load_composable_node_param(param_path): disuse_low_height_euclidean_component = ComposableNode( package=pkg, - plugin="euclidean_cluster::EuclideanClusterNode", + plugin="autoware::euclidean_cluster::EuclideanClusterNode", name=AnonName("euclidean_cluster"), remappings=[ ("input", LaunchConfiguration("input_pointcloud")), diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py index 607e1bf30ccaa..0b502b1c43d67 100644 --- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py +++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py @@ -49,7 +49,7 @@ def load_composable_node_param(param_path): use_low_height_euclidean_component = ComposableNode( package=pkg, namespace=ns, - plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", + plugin="autoware::euclidean_cluster::VoxelGridBasedEuclideanClusterNode", name="euclidean_cluster", remappings=[ ("input", "low_height/pointcloud"), @@ -61,7 +61,7 @@ def load_composable_node_param(param_path): disuse_low_height_euclidean_component = ComposableNode( package=pkg, namespace=ns, - plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", + plugin="autoware::euclidean_cluster::VoxelGridBasedEuclideanClusterNode", name="euclidean_cluster", remappings=[ ("input", LaunchConfiguration("input_pointcloud")), diff --git a/perception/euclidean_cluster/lib/euclidean_cluster.cpp b/perception/euclidean_cluster/lib/euclidean_cluster.cpp index 5ba1b99403280..0481b7e1b742d 100644 --- a/perception/euclidean_cluster/lib/euclidean_cluster.cpp +++ b/perception/euclidean_cluster/lib/euclidean_cluster.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "euclidean_cluster/euclidean_cluster.hpp" +#include "autoware/euclidean_cluster/euclidean_cluster.hpp" #include #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { EuclideanCluster::EuclideanCluster() { @@ -93,4 +93,4 @@ bool EuclideanCluster::cluster( return true; } -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/lib/utils.cpp b/perception/euclidean_cluster/lib/utils.cpp index 6d119ef3d01aa..624c838ef0647 100644 --- a/perception/euclidean_cluster/lib/utils.cpp +++ b/perception/euclidean_cluster/lib/utils.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 "euclidean_cluster/utils.hpp" +#include "autoware/euclidean_cluster/utils.hpp" #include #include @@ -19,7 +19,7 @@ #include #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud) { @@ -128,4 +128,4 @@ void convertObjectMsg2SensorMsg( output.height = 1; output.is_dense = false; } -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp b/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp index 126f877afddb0..096de3dc1b296 100644 --- a/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp +++ b/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" +#include "autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" #include #include #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { VoxelGridBasedEuclideanCluster::VoxelGridBasedEuclideanCluster() { @@ -166,4 +166,4 @@ bool VoxelGridBasedEuclideanCluster::cluster( return true; } -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/src/euclidean_cluster_node.cpp b/perception/euclidean_cluster/src/euclidean_cluster_node.cpp index f986b0334935f..b0b9448dfc0c4 100644 --- a/perception/euclidean_cluster/src/euclidean_cluster_node.cpp +++ b/perception/euclidean_cluster/src/euclidean_cluster_node.cpp @@ -14,11 +14,11 @@ #include "euclidean_cluster_node.hpp" -#include "euclidean_cluster/utils.hpp" +#include "autoware/euclidean_cluster/utils.hpp" #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { EuclideanClusterNode::EuclideanClusterNode(const rclcpp::NodeOptions & options) : Node("euclidean_cluster_node", options) @@ -86,8 +86,8 @@ void EuclideanClusterNode::onPointCloud( } } -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster #include -RCLCPP_COMPONENTS_REGISTER_NODE(euclidean_cluster::EuclideanClusterNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::euclidean_cluster::EuclideanClusterNode) diff --git a/perception/euclidean_cluster/src/euclidean_cluster_node.hpp b/perception/euclidean_cluster/src/euclidean_cluster_node.hpp index 92f10696d17dc..5ab48abb4f042 100644 --- a/perception/euclidean_cluster/src/euclidean_cluster_node.hpp +++ b/perception/euclidean_cluster/src/euclidean_cluster_node.hpp @@ -14,7 +14,7 @@ #pragma once -#include "euclidean_cluster/euclidean_cluster.hpp" +#include "autoware/euclidean_cluster/euclidean_cluster.hpp" #include #include @@ -26,7 +26,8 @@ #include #include -namespace euclidean_cluster + +namespace autoware::euclidean_cluster { class EuclideanClusterNode : public rclcpp::Node { @@ -45,4 +46,4 @@ class EuclideanClusterNode : public rclcpp::Node std::unique_ptr debug_publisher_; }; -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp index e9425095a3b0d..e54c55e4873ee 100644 --- a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp +++ b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp @@ -14,11 +14,11 @@ #include "voxel_grid_based_euclidean_cluster_node.hpp" -#include "euclidean_cluster/utils.hpp" +#include "autoware/euclidean_cluster/utils.hpp" #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { VoxelGridBasedEuclideanClusterNode::VoxelGridBasedEuclideanClusterNode( const rclcpp::NodeOptions & options) @@ -89,8 +89,8 @@ void VoxelGridBasedEuclideanClusterNode::onPointCloud( } } -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster #include -RCLCPP_COMPONENTS_REGISTER_NODE(euclidean_cluster::VoxelGridBasedEuclideanClusterNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::euclidean_cluster::VoxelGridBasedEuclideanClusterNode) diff --git a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp index b0c5d0e5a7fbf..b48e30b37de04 100644 --- a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp +++ b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp @@ -14,7 +14,7 @@ #pragma once -#include "euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" +#include "autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" #include #include @@ -26,7 +26,7 @@ #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { class VoxelGridBasedEuclideanClusterNode : public rclcpp::Node { @@ -45,4 +45,4 @@ class VoxelGridBasedEuclideanClusterNode : public rclcpp::Node std::unique_ptr debug_publisher_; }; -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp index 7f19402d9e565..904e66cb53298 100644 --- a/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp @@ -25,7 +25,7 @@ #include #endif -#include "euclidean_cluster/utils.hpp" +#include "autoware/euclidean_cluster/utils.hpp" namespace image_projection_based_fusion { @@ -68,7 +68,7 @@ void RoiPointCloudFusionNode::postprocess( // publish debug cluster if (cluster_debug_pub_->get_subscription_count() > 0) { sensor_msgs::msg::PointCloud2 debug_cluster_msg; - euclidean_cluster::convertObjectMsg2SensorMsg(output_msg, debug_cluster_msg); + autoware::euclidean_cluster::convertObjectMsg2SensorMsg(output_msg, debug_cluster_msg); cluster_debug_pub_->publish(debug_cluster_msg); } } From 03877b072f7d58f5c42fc06a33d2b330941acd3b Mon Sep 17 00:00:00 2001 From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> Date: Tue, 9 Jul 2024 13:42:29 +0900 Subject: [PATCH 12/19] chore(radar_object_tracker): delete maintainer (#7898) Signed-off-by: scepter914 --- perception/radar_object_tracker/package.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/perception/radar_object_tracker/package.xml b/perception/radar_object_tracker/package.xml index 79ae0385d4b5e..f104e2d7ea456 100644 --- a/perception/radar_object_tracker/package.xml +++ b/perception/radar_object_tracker/package.xml @@ -6,7 +6,6 @@ Do tracking radar object Yoshi Ri Yukihiro Saito - Satoshi Tanaka Taekjin Lee Apache License 2.0 From 458dbe5c5445fe95a62d9a4804509acbc024d703 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Tue, 9 Jul 2024 07:45:46 +0200 Subject: [PATCH 13/19] chore(autoware_behavior_velocity_planner): remove no_prefix function from tests (#7589) Signed-off-by: Esteve Fernandez --- .../test/src/test_node_interface.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp index 3482d7be8ec48..cc2f4bf3b96b4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp @@ -54,7 +54,6 @@ std::shared_ptr generateNode() const auto velocity_smoother_dir = ament_index_cpp::get_package_share_directory("autoware_velocity_smoother"); - // TODO(esteve): delete when all the modules are migrated to autoware_behavior_velocity_* const auto get_behavior_velocity_module_config = [](const std::string & module) { const auto package_name = "autoware_behavior_velocity_" + module + "_module"; const auto package_path = ament_index_cpp::get_package_share_directory(package_name); From f716b5b0ac4678d5d75ae06936d66e340292930d Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 9 Jul 2024 15:02:25 +0900 Subject: [PATCH 14/19] fix(euclidean_cluster): include autoware_universe_utils headers (#7908) fix: include autoware_universe_utils headers instead of compare_map_segmentation package Signed-off-by: Taekjin LEE --- perception/euclidean_cluster/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/euclidean_cluster/package.xml b/perception/euclidean_cluster/package.xml index c5cdd9c54b988..2644786ead061 100644 --- a/perception/euclidean_cluster/package.xml +++ b/perception/euclidean_cluster/package.xml @@ -12,7 +12,7 @@ autoware_cmake autoware_perception_msgs - compare_map_segmentation + autoware_universe_utils geometry_msgs libpcl-all-dev pcl_conversions From d2f2fd9c1ab4e505758646bfce3d48ffc8192fb9 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Tue, 9 Jul 2024 18:46:05 +0900 Subject: [PATCH 15/19] feat(autonomous_emergency_braking): add virtual stop wall to aeb (#7894) * add virtual stop wall to aeb Signed-off-by: Daniel Sanchez * add maintainer Signed-off-by: Daniel Sanchez * add uppercase Signed-off-by: Daniel Sanchez * use motion utils function instead of shiftPose Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- .github/CODEOWNERS | 2 +- .../README.md | 1 + .../autonomous_emergency_braking.param.yaml | 1 + .../autonomous_emergency_braking/node.hpp | 1 + .../autonomous_emergency_braking/utils.hpp | 2 - .../package.xml | 1 + .../src/node.cpp | 38 ++++++++++++++++++- 7 files changed, 41 insertions(+), 5 deletions(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 0fe324f84f81a..ed496d7b7ba74 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -45,7 +45,7 @@ common/time_utils/** christopherj.ho@gmail.com shumpei.wakabayashi@tier4.jp tomo common/traffic_light_recognition_marker_publisher/** shumpei.wakabayashi@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp common/traffic_light_utils/** kotaro.uetake@tier4.jp satoshi.ota@tier4.jp shunsuke.miura@tier4.jp common/tvm_utility/** ambroise.vincent@arm.com xinyu.wang@tier4.jp -control/autoware_autonomous_emergency_braking/** daniel.sanchez@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp +control/autoware_autonomous_emergency_braking/** daniel.sanchez@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp kyoichi.sugahara@tier4.jp control/autoware_control_validator/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/autoware_external_cmd_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp control/autoware_joy_controller/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp diff --git a/control/autoware_autonomous_emergency_braking/README.md b/control/autoware_autonomous_emergency_braking/README.md index f040235f3f052..db27ca1b4f699 100644 --- a/control/autoware_autonomous_emergency_braking/README.md +++ b/control/autoware_autonomous_emergency_braking/README.md @@ -192,6 +192,7 @@ When vehicle odometry information is faulty, it is possible that the MPC fails t | Name | Unit | Type | Description | Default value | | :-------------------------------- | :----- | :----- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| publish_debug_markers | [-] | bool | flag to publish debug markers | true | | publish_debug_pointcloud | [-] | bool | flag to publish the point cloud used for debugging | false | | use_predicted_trajectory | [-] | bool | flag to use the predicted path from the control module | true | | use_imu_path | [-] | bool | flag to use the predicted path generated by sensor data | true | diff --git a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml index 482ddc50766f8..25db9448f758b 100644 --- a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml +++ b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml @@ -14,6 +14,7 @@ # Debug publish_debug_pointcloud: false + publish_debug_markers: true # Point cloud partitioning detection_range_min_height: 0.0 diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp index f6df46440d841..9fd6d0192439c 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp @@ -329,6 +329,7 @@ class AEB : public rclcpp::Node // member variables bool publish_debug_pointcloud_; + bool publish_debug_markers_; bool use_predicted_trajectory_; bool use_imu_path_; bool use_pointcloud_data_; diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp index 4a6bc0ce2c11f..d2ec1763ca4dd 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp @@ -33,8 +33,6 @@ #include #endif -#include - namespace autoware::motion::control::autonomous_emergency_braking::utils { using autoware::universe_utils::Polygon2d; diff --git a/control/autoware_autonomous_emergency_braking/package.xml b/control/autoware_autonomous_emergency_braking/package.xml index c822adb510805..0c64b9498486a 100644 --- a/control/autoware_autonomous_emergency_braking/package.xml +++ b/control/autoware_autonomous_emergency_braking/package.xml @@ -8,6 +8,7 @@ Tomoya Kimura Mamoru Sobue Daniel Sanchez + Kyoichi Sugahara Apache License 2.0 diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index fb0fc2d043cc5..69bedd02d201b 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include #include @@ -41,7 +42,9 @@ #include #include +#include #include +#include #ifdef ROS_DISTRO_GALACTIC #include #include @@ -131,6 +134,7 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) } // parameter publish_debug_pointcloud_ = declare_parameter("publish_debug_pointcloud"); + publish_debug_markers_ = declare_parameter("publish_debug_markers"); use_predicted_trajectory_ = declare_parameter("use_predicted_trajectory"); use_imu_path_ = declare_parameter("use_imu_path"); use_pointcloud_data_ = declare_parameter("use_pointcloud_data"); @@ -182,6 +186,7 @@ rcl_interfaces::msg::SetParametersResult AEB::onParameter( { using autoware::universe_utils::updateParam; updateParam(parameters, "publish_debug_pointcloud", publish_debug_pointcloud_); + updateParam(parameters, "publish_debug_markers", publish_debug_markers_); updateParam(parameters, "use_predicted_trajectory", use_predicted_trajectory_); updateParam(parameters, "use_imu_path", use_imu_path_); updateParam(parameters, "use_pointcloud_data", use_pointcloud_data_); @@ -378,7 +383,9 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) stat.addf("RSS", "%.2f", data.rss); stat.addf("Distance", "%.2f", data.distance_to_object); stat.addf("Object Speed", "%.2f", data.velocity); - addCollisionMarker(data, debug_markers); + if (publish_debug_markers_) { + addCollisionMarker(data, debug_markers); + } } else { const std::string error_msg = "[AEB]: No Collision"; const auto diag_level = DiagnosticStatus::OK; @@ -455,7 +462,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers) }); // Add debug markers - { + if (publish_debug_markers_) { const auto [color_r, color_g, color_b, color_a] = debug_colors; addMarker( this->get_clock()->now(), path, ego_polys, objects, closest_object_point, color_r, color_g, @@ -896,6 +903,33 @@ void AEB::addCollisionMarker(const ObjectData & data, MarkerArray & debug_marker autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.3)); point_marker.pose.position = data.position; debug_markers.markers.push_back(point_marker); + + const auto ego_map_pose = std::invoke([this]() -> std::optional { + geometry_msgs::msg::TransformStamped tf_current_pose; + geometry_msgs::msg::Pose p; + try { + tf_current_pose = tf_buffer_.lookupTransform( + "map", "base_link", rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR(get_logger(), "%s", ex.what()); + return std::nullopt; + } + + p.orientation = tf_current_pose.transform.rotation; + p.position.x = tf_current_pose.transform.translation.x; + p.position.y = tf_current_pose.transform.translation.y; + p.position.z = tf_current_pose.transform.translation.z; + return std::make_optional(p); + }); + + if (ego_map_pose.has_value()) { + const double base_to_front_offset = vehicle_info_.max_longitudinal_offset_m; + const auto ego_front_pose = autoware::universe_utils::calcOffsetPose( + ego_map_pose.value(), base_to_front_offset, 0.0, 0.0, 0.0); + const auto virtual_stop_wall = autoware::motion_utils::createStopVirtualWallMarker( + ego_front_pose, "autonomous_emergency_braking", this->now(), 0); + autoware::universe_utils::appendMarkerArray(virtual_stop_wall, &debug_markers); + } } } // namespace autoware::motion::control::autonomous_emergency_braking From 48b7cee5f5e3fc3d07294d529758867231d113b0 Mon Sep 17 00:00:00 2001 From: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Date: Tue, 9 Jul 2024 18:47:11 +0900 Subject: [PATCH 16/19] docs(goal_planner): update parameter description (#7889) * docs(goal_planner): update parameter description Signed-off-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> * style(pre-commit): autofix Signed-off-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> --------- Signed-off-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../README.md | 27 +++++++++---------- 1 file changed, 13 insertions(+), 14 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md index 3e155aba0af2e..014aaf6db6657 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md @@ -173,20 +173,19 @@ The gray numbers represent objects to avoid, and you can see that the goal in fr ### Parameters for goal search -| Name | Unit | Type | Description | Default value | -| :------------------------------ | :--- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | :-------------------------- | -| goal_priority | [-] | string | In case `minimum_longitudinal_distance`, sort with smaller longitudinal distances taking precedence over smaller lateral distances. In case `minimum_weighted_distance`, sort with the sum of weighted lateral distance and longitudinal distance | `minimum_weighted_distance` | -| lateral_weight | [-] | double | Weight for lateral distance used when `minimum_weighted_distance` | 40.0 | -| prioritize_goals_before_objects | [-] | bool | If there are objects that may need to be avoided, prioritize the goal in front of them | true | -| forward_goal_search_length | [m] | double | length of forward range to be explored from the original goal | 20.0 | -| backward_goal_search_length | [m] | double | length of backward range to be explored from the original goal | 20.0 | -| goal_search_interval | [m] | double | distance interval for goal search | 2.0 | -| longitudinal_margin | [m] | double | margin between ego-vehicle at the goal position and obstacles | 3.0 | -| max_lateral_offset | [m] | double | maximum offset of goal search in the lateral direction | 0.5 | -| lateral_offset_interval | [m] | double | distance interval of goal search in the lateral direction | 0.25 | -| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | -| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | -| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | +| Name | Unit | Type | Description | Default value | +| :------------------------------ | :--- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :-------------------------- | +| goal_priority | [-] | string | In case `minimum_longitudinal_distance`, sort with smaller longitudinal distances taking precedence over smaller lateral distances. In case `minimum_weighted_distance`, sort with the sum of weighted lateral distance and longitudinal distance | `minimum_weighted_distance` | +| lateral_weight | [-] | double | Weight for lateral distance used when `minimum_weighted_distance` | 40.0 | +| prioritize_goals_before_objects | [-] | bool | If there are objects that may need to be avoided, prioritize the goal in front of them | true | +| forward_goal_search_length | [m] | double | length of forward range to be explored from the original goal | 20.0 | +| backward_goal_search_length | [m] | double | length of backward range to be explored from the original goal | 20.0 | +| goal_search_interval | [m] | double | distance interval for goal search | 2.0 | +| longitudinal_margin | [m] | double | margin between ego-vehicle at the goal position and obstacles | 3.0 | +| max_lateral_offset | [m] | double | maximum offset of goal search in the lateral direction | 0.5 | +| lateral_offset_interval | [m] | double | distance interval of goal search in the lateral direction | 0.25 | +| ignore_distance_from_lane_start | [m] | double | This parameter ensures that the distance between the start of the shoulder lane and the goal is not less than the specified value. It's used to prevent setting goals too close to the beginning of the shoulder lane, which might lead to unsafe or impractical pull-over maneuvers. Increasing this value will force the system to ignore potential goal positions near the start of the shoulder lane, potentially leading to safer and more comfortable pull-over locations. | 0.0 | +| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | ## **Pull Over** From f386fa8e611ec685c1703bed89e45a5d525c2282 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 9 Jul 2024 19:07:17 +0900 Subject: [PATCH 17/19] fix(start/goal_planner): fix addition of duplicate segments in calcBeforeShiftedArcLength (#7902) * fix(start/goal_planner): fix addition of duplicate segments in calcBeforeShiftedArcLength Signed-off-by: kosuke55 * Update trajectory.hpp Co-authored-by: Kyoichi Sugahara * Update trajectory.hpp Co-authored-by: Kyoichi Sugahara --------- Signed-off-by: kosuke55 Co-authored-by: Kyoichi Sugahara --- .../motion_utils/trajectory/trajectory.hpp | 49 +++++++++++++------ .../src/trajectory/trajectory.cpp | 12 ++--- .../src/shift_pull_over.cpp | 15 +++++- .../src/shift_pull_out.cpp | 13 ++++- 4 files changed, 63 insertions(+), 26 deletions(-) diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp index da37d04550f5e..126e66772920a 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp @@ -991,34 +991,51 @@ calcCurvature>( * curvature calculation */ template -std::vector> calcCurvatureAndArcLength(const T & points) +std::vector>> calcCurvatureAndSegmentLength( + const T & points) { - // Note that arclength is for the segment, not the sum. - std::vector> curvature_arc_length_vec; - curvature_arc_length_vec.emplace_back(0.0, 0.0); + // segment length is pair of segment length between {p1, p2} and {p2, p3} + std::vector>> curvature_and_segment_length_vec; + curvature_and_segment_length_vec.reserve(points.size()); + curvature_and_segment_length_vec.emplace_back(0.0, std::make_pair(0.0, 0.0)); for (size_t i = 1; i < points.size() - 1; ++i) { const auto p1 = autoware::universe_utils::getPoint(points.at(i - 1)); const auto p2 = autoware::universe_utils::getPoint(points.at(i)); const auto p3 = autoware::universe_utils::getPoint(points.at(i + 1)); const double curvature = autoware::universe_utils::calcCurvature(p1, p2, p3); - const double arc_length = - autoware::universe_utils::calcDistance2d(points.at(i - 1), points.at(i)) + - autoware::universe_utils::calcDistance2d(points.at(i), points.at(i + 1)); - curvature_arc_length_vec.emplace_back(curvature, arc_length); + + // The first point has only the next point, so put the distance to that point. + // In other words, assign the first segment length at the second point to the + // second_segment_length at the first point. + if (i == 1) { + curvature_and_segment_length_vec.at(0).second.second = + autoware::universe_utils::calcDistance2d(p1, p2); + } + + // The second_segment_length of the previous point and the first segment length of the current + // point are equal. + const std::pair arc_length{ + curvature_and_segment_length_vec.back().second.second, + autoware::universe_utils::calcDistance2d(p2, p3)}; + + curvature_and_segment_length_vec.emplace_back(curvature, arc_length); } - curvature_arc_length_vec.emplace_back(0.0, 0.0); - return curvature_arc_length_vec; + // set to the last point + curvature_and_segment_length_vec.emplace_back( + 0.0, std::make_pair(curvature_and_segment_length_vec.back().second.second, 0.0)); + + return curvature_and_segment_length_vec; } -extern template std::vector> -calcCurvatureAndArcLength>( +extern template std::vector>> +calcCurvatureAndSegmentLength>( const std::vector & points); -extern template std::vector> -calcCurvatureAndArcLength>( +extern template std::vector>> +calcCurvatureAndSegmentLength>( const std::vector & points); -extern template std::vector> -calcCurvatureAndArcLength>( +extern template std::vector>> +calcCurvatureAndSegmentLength>( const std::vector & points); /** diff --git a/common/autoware_motion_utils/src/trajectory/trajectory.cpp b/common/autoware_motion_utils/src/trajectory/trajectory.cpp index 5d536c0772fea..ce26952c3d634 100644 --- a/common/autoware_motion_utils/src/trajectory/trajectory.cpp +++ b/common/autoware_motion_utils/src/trajectory/trajectory.cpp @@ -238,14 +238,14 @@ calcCurvature>( const std::vector & points); // -template std::vector> -calcCurvatureAndArcLength>( +template std::vector>> +calcCurvatureAndSegmentLength>( const std::vector & points); -template std::vector> -calcCurvatureAndArcLength>( +template std::vector>> +calcCurvatureAndSegmentLength>( const std::vector & points); -template std::vector> -calcCurvatureAndArcLength>( +template std::vector>> +calcCurvatureAndSegmentLength>( const std::vector & points); // diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp index 3d2bf9500bc78..9570621f0f717 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp @@ -308,8 +308,19 @@ double ShiftPullOver::calcBeforeShiftedArcLength( double before_arc_length{0.0}; double after_arc_length{0.0}; - for (const auto & [k, segment_length] : - autoware::motion_utils::calcCurvatureAndArcLength(reversed_path.points)) { + + const auto curvature_and_segment_length = + autoware::motion_utils::calcCurvatureAndSegmentLength(reversed_path.points); + + for (size_t i = 0; i < curvature_and_segment_length.size(); ++i) { + const auto & [k, segment_length_pair] = curvature_and_segment_length[i]; + + // If it is the last point, add the lengths of the previous and next segments. + // For other points, only add the length of the previous segment. + const double segment_length = i == curvature_and_segment_length.size() - 1 + ? segment_length_pair.first + : segment_length_pair.first + segment_length_pair.second; + // after shifted segment length const double after_segment_length = k > 0 ? segment_length * (1 + k * dr) : segment_length / (1 - k * dr); diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp index 7c78f62192105..f1e0f8abb2395 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -423,8 +423,17 @@ double ShiftPullOut::calcBeforeShiftedArcLength( double before_arc_length{0.0}; double after_arc_length{0.0}; - for (const auto & [k, segment_length] : - autoware::motion_utils::calcCurvatureAndArcLength(path.points)) { + const auto curvatures_and_segment_lengths = + autoware::motion_utils::calcCurvatureAndSegmentLength(path.points); + for (size_t i = 0; i < curvatures_and_segment_lengths.size(); ++i) { + const auto & [k, segment_length_pair] = curvatures_and_segment_lengths.at(i); + + // If it is the last point, add the lengths of the previous and next segments. + // For other points, only add the length of the previous segment. + const double segment_length = i == curvatures_and_segment_lengths.size() - 1 + ? segment_length_pair.first + segment_length_pair.second + : segment_length_pair.first; + // after shifted segment length const double after_segment_length = k < 0 ? segment_length * (1 - k * dr) : segment_length / (1 + k * dr); From 1654a578acec1d033746200d5e052ba0d0da7355 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Tue, 9 Jul 2024 19:55:42 +0900 Subject: [PATCH 18/19] fix(frenet_planner): fix mistake in the curvature calculation (#7920) Signed-off-by: Maxime CLEMENT --- .../lane_driving/motion_planning/motion_planning.launch.xml | 1 + .../src/frenet_planner/frenet_planner.cpp | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml index d0c34ea01b2c6..63353459fa16c 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml @@ -90,6 +90,7 @@ + diff --git a/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp b/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp index 1f05fc93138a2..1e04db2a17eee 100644 --- a/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp +++ b/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// Copyright 2023-2024 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -186,7 +186,7 @@ void calculateCartesian( for (size_t i = 1; i < path.yaws.size(); ++i) { const auto dyaw = autoware::common::helper_functions::wrap_angle(path.yaws[i] - path.yaws[i - 1]); - path.curvatures.push_back(dyaw / (path.lengths[i - 1], path.lengths[i])); + path.curvatures.push_back(dyaw / (path.lengths[i] - path.lengths[i - 1])); } path.curvatures.push_back(path.curvatures.back()); } From 9b20ced4b6d976bc30e28e997269a0e0dea4bbfa Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Tue, 9 Jul 2024 21:02:02 +0900 Subject: [PATCH 19/19] fix(pointcloud_preprocessor): fix distortion corrector unit test (#7833) * fix: use pointcloud iterator instead of memcpy, remove reinterpret cast Signed-off-by: vividf * chore: set default debugging parameter to false Signed-off-by: vividf * style(pre-commit): autofix * chore: run precommit Signed-off-by: vividf * chore: fix TIER IV name Signed-off-by: vividf * chore: changed public variables to protected and add getters Signed-off-by: vividf * chore: add tolerance Signed-off-by: vividf * feat: add two tests for pure linear and rotation Signed-off-by: vividf * chore: change naming, fix tolerance value Signed-off-by: vividf * chore: add comment for quat Signed-off-by: vividf * fix: remove node_->get_clock() and use self-defined timestamp Signed-off-by: vividf * chore: remove redundant parameters Signed-off-by: vividf * chore: fix spell error and add tests in cmake Signed-off-by: vividf * chore: move all clock->now() to self-defined time Signed-off-by: vividf * chore: change function names Signed-off-by: vividf * chore: remove irrelevant variable Signed-off-by: vividf * chore: fix variables naming Signed-off-by: vividf * chore: change boolen naming: generate_points Signed-off-by: vividf * chore: add assert to make sure ms is not larger than a second Signed-off-by: vividf * chore: add note Signed-off-by: vividf * chore: add unifore initialization and semantic meaning for magic number Signed-off-by: vividf * chore: change vector to Eigen Signed-off-by: vividf * chore: fix explanation Signed-off-by: vividf * chore: fix more eigen Signed-off-by: vividf * chore: fix more magic numbers Signed-off-by: vividf * chore: add unit Signed-off-by: vividf * fix: use assert from gtest Signed-off-by: vividf --------- Signed-off-by: vividf Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Kenzo Lobos Tsunekawa --- .../pointcloud_preprocessor/CMakeLists.txt | 4 + .../distortion_corrector.hpp | 9 + .../distortion_corrector.cpp | 29 +- .../test/test_distortion_corrector_node.cpp | 904 ++++++++++++++++++ 4 files changed, 945 insertions(+), 1 deletion(-) create mode 100644 sensing/pointcloud_preprocessor/test/test_distortion_corrector_node.cpp diff --git a/sensing/pointcloud_preprocessor/CMakeLists.txt b/sensing/pointcloud_preprocessor/CMakeLists.txt index 53b6aae55d47b..2d0649fe43954 100644 --- a/sensing/pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/pointcloud_preprocessor/CMakeLists.txt @@ -231,8 +231,12 @@ if(BUILD_TESTING) ament_add_gtest(test_utilities test/test_utilities.cpp ) + ament_add_gtest(distortion_corrector_node_tests + test/test_distortion_corrector_node.cpp + ) target_link_libraries(test_utilities pointcloud_preprocessor_filter) + target_link_libraries(distortion_corrector_node_tests pointcloud_preprocessor_filter) endif() diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index 8a7b7c90113da..e37329a9a4cc3 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -48,6 +48,11 @@ namespace pointcloud_preprocessor class DistortionCorrectorBase { public: + virtual bool pointcloud_transform_exists() = 0; + virtual bool pointcloud_transform_needed() = 0; + virtual std::deque get_twist_queue() = 0; + virtual std::deque get_angular_velocity_queue() = 0; + virtual void processTwistMessage( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) = 0; virtual void processIMUMessage( @@ -96,6 +101,10 @@ class DistortionCorrector : public DistortionCorrectorBase : node_(node), tf_buffer_(node_->get_clock()), tf_listener_(tf_buffer_) { } + bool pointcloud_transform_exists(); + bool pointcloud_transform_needed(); + std::deque get_twist_queue(); + std::deque get_angular_velocity_queue(); void processTwistMessage( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) override; diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index 8c8752c7e278e..f933d4c6f75d9 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -20,6 +20,31 @@ namespace pointcloud_preprocessor { + +template +bool DistortionCorrector::pointcloud_transform_exists() +{ + return pointcloud_transform_exists_; +} + +template +bool DistortionCorrector::pointcloud_transform_needed() +{ + return pointcloud_transform_needed_; +} + +template +std::deque DistortionCorrector::get_twist_queue() +{ + return twist_queue_; +} + +template +std::deque DistortionCorrector::get_angular_velocity_queue() +{ + return angular_velocity_queue_; +} + template void DistortionCorrector::processTwistMessage( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) @@ -349,7 +374,9 @@ inline void DistortionCorrector2D::undistortPointImplementation( theta_ += w * time_offset; baselink_quat_.setValue( 0, 0, autoware::universe_utils::sin(theta_ * 0.5f), - autoware::universe_utils::cos(theta_ * 0.5f)); // baselink_quat.setRPY(0.0, 0.0, theta); + autoware::universe_utils::cos( + theta_ * + 0.5f)); // baselink_quat.setRPY(0.0, 0.0, theta); (Note that the value is slightly different) const float dis = v * time_offset; x_ += dis * autoware::universe_utils::cos(theta_); y_ += dis * autoware::universe_utils::sin(theta_); diff --git a/sensing/pointcloud_preprocessor/test/test_distortion_corrector_node.cpp b/sensing/pointcloud_preprocessor/test/test_distortion_corrector_node.cpp new file mode 100644 index 0000000000000..7997aaf43c202 --- /dev/null +++ b/sensing/pointcloud_preprocessor/test/test_distortion_corrector_node.cpp @@ -0,0 +1,904 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// 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. + +// Note: To regenerate the ground truth (GT) for the expected undistorted point cloud values, +// set the "debug_" value to true to display the point cloud values. Then, +// replace the expected values with the newly displayed undistorted point cloud values. +// +// Also, make sure the point stamp, twist stamp, and imu stamp are not identical. +// In the current hardcoded design, timestamp of pointcloud, twist, and imu msg are listed below +// pointcloud (1 msgs, 10points): +// 10.10, 10.11, 10.12, 10.13, 10.14, 10.15, 10.16, 10.17, 10.18, 10.19 +// twist (6msgs): +// 10.095, 10.119, 10.143, 10.167, 10.191, 10.215 +// imu (6msgs): +// 10.09, 10.117, 10.144, 10.171, 10.198, 10.225 + +#include "autoware/universe_utils/math/trigonometry.hpp" +#include "pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp" + +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include + +class DistortionCorrectorTest : public ::testing::Test +{ +protected: + void SetUp() override + { + node_ = std::make_shared("test_node"); + distortion_corrector_2d_ = + std::make_shared(node_.get()); + distortion_corrector_3d_ = + std::make_shared(node_.get()); + + // Setup TF + tf_broadcaster_ = std::make_shared(node_); + tf_broadcaster_->sendTransform(generateStaticTransformMsg()); + + // Spin the node for a while to ensure transforms are published + auto start = std::chrono::steady_clock::now(); + auto timeout = std::chrono::seconds(1); + while (std::chrono::steady_clock::now() - start < timeout) { + rclcpp::spin_some(node_); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + + void TearDown() override {} + + void checkInput(int ms) { ASSERT_LT(ms, 1000) << "ms should be less than a second."; } + + rclcpp::Time addMilliseconds(rclcpp::Time stamp, int ms) + { + checkInput(ms); + auto ms_in_ns = rclcpp::Duration(0, ms * 1000000); + return stamp + ms_in_ns; + } + + rclcpp::Time subtractMilliseconds(rclcpp::Time stamp, int ms) + { + checkInput(ms); + auto ms_in_ns = rclcpp::Duration(0, ms * 1000000); + return stamp - ms_in_ns; + } + + geometry_msgs::msg::TransformStamped generateTransformMsg( + const std::string & parent_frame, const std::string & child_frame, double x, double y, double z, + double qx, double qy, double qz, double qw) + { + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); + geometry_msgs::msg::TransformStamped tf_msg; + tf_msg.header.stamp = timestamp; + tf_msg.header.frame_id = parent_frame; + tf_msg.child_frame_id = child_frame; + tf_msg.transform.translation.x = x; + tf_msg.transform.translation.y = y; + tf_msg.transform.translation.z = z; + tf_msg.transform.rotation.x = qx; + tf_msg.transform.rotation.y = qy; + tf_msg.transform.rotation.z = qz; + tf_msg.transform.rotation.w = qw; + return tf_msg; + } + + std::vector generateStaticTransformMsg() + { + // generate defined transformations + return { + generateTransformMsg("base_link", "lidar_top", 5.0, 5.0, 5.0, 0.683, 0.5, 0.183, 0.499), + generateTransformMsg("base_link", "imu_link", 1.0, 1.0, 3.0, 0.278, 0.717, 0.441, 0.453)}; + } + + std::shared_ptr generateTwistMsg( + double linear_x, double angular_z, rclcpp::Time stamp) + { + auto twist_msg = std::make_shared(); + twist_msg->header.stamp = stamp; + twist_msg->header.frame_id = "base_link"; + twist_msg->twist.twist.linear.x = linear_x; + twist_msg->twist.twist.angular.z = angular_z; + return twist_msg; + } + + std::shared_ptr generateImuMsg( + double angular_vel_x, double angular_vel_y, double angular_vel_z, rclcpp::Time stamp) + { + auto imu_msg = std::make_shared(); + imu_msg->header.stamp = stamp; + imu_msg->header.frame_id = "imu_link"; + imu_msg->angular_velocity.x = angular_vel_x; + imu_msg->angular_velocity.y = angular_vel_y; + imu_msg->angular_velocity.z = angular_vel_z; + return imu_msg; + } + + std::vector> generateTwistMsgs( + rclcpp::Time pointcloud_timestamp) + { + std::vector> twist_msgs; + rclcpp::Time twist_stamp = subtractMilliseconds(pointcloud_timestamp, 5); + + for (int i = 0; i < number_of_twist_msgs_; ++i) { + auto twist_msg = generateTwistMsg( + twist_linear_x_ + i * twist_linear_x_increment_, + twist_angular_z_ + i * twist_angular_z_increment_, twist_stamp); + twist_msgs.push_back(twist_msg); + + twist_stamp = addMilliseconds(twist_stamp, twist_msgs_interval_ms_); + } + + return twist_msgs; + } + + std::vector> generateImuMsgs( + rclcpp::Time pointcloud_timestamp) + { + std::vector> imu_msgs; + rclcpp::Time imu_stamp = subtractMilliseconds(pointcloud_timestamp, 10); + + for (int i = 0; i < number_of_imu_msgs_; ++i) { + auto imu_msg = generateImuMsg( + imu_angular_x_ + i * imu_angular_x_increment_, + imu_angular_y_ + i * imu_angular_y_increment_, + imu_angular_z_ + i * imu_angular_z_increment_, imu_stamp); + imu_msgs.push_back(imu_msg); + imu_stamp = addMilliseconds(imu_stamp, imu_msgs_interval_ms_); + } + + return imu_msgs; + } + + sensor_msgs::msg::PointCloud2 generatePointCloudMsg( + bool generate_points, bool is_lidar_frame, rclcpp::Time stamp) + { + sensor_msgs::msg::PointCloud2 pointcloud_msg; + pointcloud_msg.header.stamp = stamp; + pointcloud_msg.header.frame_id = is_lidar_frame ? "lidar_top" : "base_link"; + pointcloud_msg.height = 1; + pointcloud_msg.is_dense = true; + pointcloud_msg.is_bigendian = false; + + if (generate_points) { + std::array points = {{ + Eigen::Vector3f(10.0f, 0.0f, 0.0f), // point 1 + Eigen::Vector3f(0.0f, 10.0f, 0.0f), // point 2 + Eigen::Vector3f(0.0f, 0.0f, 10.0f), // point 3 + Eigen::Vector3f(20.0f, 0.0f, 0.0f), // point 4 + Eigen::Vector3f(0.0f, 20.0f, 0.0f), // point 5 + Eigen::Vector3f(0.0f, 0.0f, 20.0f), // point 6 + Eigen::Vector3f(30.0f, 0.0f, 0.0f), // point 7 + Eigen::Vector3f(0.0f, 30.0f, 0.0f), // point 8 + Eigen::Vector3f(0.0f, 0.0f, 30.0f), // point 9 + Eigen::Vector3f(10.0f, 10.0f, 10.0f) // point 10 + }}; + + // Generate timestamps for the points + std::vector timestamps = generatePointTimestamps(stamp, number_of_points_); + + sensor_msgs::PointCloud2Modifier modifier(pointcloud_msg); + modifier.setPointCloud2Fields( + 4, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1, + sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32, + "time_stamp", 1, sensor_msgs::msg::PointField::FLOAT64); + modifier.resize(number_of_points_); + + sensor_msgs::PointCloud2Iterator iter_x(pointcloud_msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y(pointcloud_msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z(pointcloud_msg, "z"); + sensor_msgs::PointCloud2Iterator iter_t(pointcloud_msg, "time_stamp"); + + for (size_t i = 0; i < number_of_points_; ++i) { + *iter_x = points[i].x(); + *iter_y = points[i].y(); + *iter_z = points[i].z(); + *iter_t = timestamps[i]; + ++iter_x; + ++iter_y; + ++iter_z; + ++iter_t; + } + } else { + pointcloud_msg.width = 0; + pointcloud_msg.row_step = 0; + } + + return pointcloud_msg; + } + + std::vector generatePointTimestamps( + rclcpp::Time pointcloud_timestamp, size_t number_of_points) + { + std::vector timestamps; + rclcpp::Time point_stamp = pointcloud_timestamp; + for (size_t i = 0; i < number_of_points; ++i) { + double timestamp = point_stamp.seconds(); + timestamps.push_back(timestamp); + point_stamp = addMilliseconds(point_stamp, points_interval_ms_); + } + + return timestamps; + } + + std::shared_ptr node_; + std::shared_ptr distortion_corrector_2d_; + std::shared_ptr distortion_corrector_3d_; + std::shared_ptr tf_broadcaster_; + + static constexpr float standard_tolerance_{1e-4}; + static constexpr float coarse_tolerance_{5e-3}; + static constexpr int number_of_twist_msgs_{6}; + static constexpr int number_of_imu_msgs_{6}; + static constexpr size_t number_of_points_{10}; + static constexpr int32_t timestamp_seconds_{10}; + static constexpr uint32_t timestamp_nanoseconds_{100000000}; + + static constexpr double twist_linear_x_{10.0}; + static constexpr double twist_angular_z_{0.02}; + static constexpr double twist_linear_x_increment_{2.0}; + static constexpr double twist_angular_z_increment_{0.01}; + + static constexpr double imu_angular_x_{0.01}; + static constexpr double imu_angular_y_{-0.02}; + static constexpr double imu_angular_z_{0.05}; + static constexpr double imu_angular_x_increment_{0.005}; + static constexpr double imu_angular_y_increment_{0.005}; + static constexpr double imu_angular_z_increment_{0.005}; + + static constexpr int points_interval_ms_{10}; + static constexpr int twist_msgs_interval_ms_{24}; + static constexpr int imu_msgs_interval_ms_{27}; + + // for debugging or regenerating the ground truth point cloud + bool debug_{false}; +}; + +TEST_F(DistortionCorrectorTest, TestProcessTwistMessage) +{ + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); + auto twist_msg = generateTwistMsg(twist_linear_x_, twist_angular_z_, timestamp); + distortion_corrector_2d_->processTwistMessage(twist_msg); + + ASSERT_FALSE(distortion_corrector_2d_->get_twist_queue().empty()); + EXPECT_EQ(distortion_corrector_2d_->get_twist_queue().front().twist.linear.x, twist_linear_x_); + EXPECT_EQ(distortion_corrector_2d_->get_twist_queue().front().twist.angular.z, twist_angular_z_); +} + +TEST_F(DistortionCorrectorTest, TestProcessIMUMessage) +{ + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); + auto imu_msg = generateImuMsg(imu_angular_x_, imu_angular_y_, imu_angular_z_, timestamp); + distortion_corrector_2d_->processIMUMessage("base_link", imu_msg); + + ASSERT_FALSE(distortion_corrector_2d_->get_angular_velocity_queue().empty()); + EXPECT_NEAR( + distortion_corrector_2d_->get_angular_velocity_queue().front().vector.z, -0.03159, + standard_tolerance_); +} + +TEST_F(DistortionCorrectorTest, TestIsInputValid) +{ + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); + + // input normal pointcloud without twist + sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp); + bool result = distortion_corrector_2d_->isInputValid(pointcloud); + EXPECT_FALSE(result); + + // input normal pointcloud with valid twist + auto twist_msg = generateTwistMsg(twist_linear_x_, twist_angular_z_, timestamp); + distortion_corrector_2d_->processTwistMessage(twist_msg); + + pointcloud = generatePointCloudMsg(true, false, timestamp); + result = distortion_corrector_2d_->isInputValid(pointcloud); + EXPECT_TRUE(result); + + // input empty pointcloud + pointcloud = generatePointCloudMsg(false, false, timestamp); + result = distortion_corrector_2d_->isInputValid(pointcloud); + EXPECT_FALSE(result); +} + +TEST_F(DistortionCorrectorTest, TestSetPointCloudTransformWithBaseLink) +{ + distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link"); + EXPECT_TRUE(distortion_corrector_2d_->pointcloud_transform_exists()); + EXPECT_FALSE(distortion_corrector_2d_->pointcloud_transform_needed()); +} + +TEST_F(DistortionCorrectorTest, TestSetPointCloudTransformWithLidarFrame) +{ + distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top"); + EXPECT_TRUE(distortion_corrector_2d_->pointcloud_transform_exists()); + EXPECT_TRUE(distortion_corrector_2d_->pointcloud_transform_needed()); +} + +TEST_F(DistortionCorrectorTest, TestSetPointCloudTransformWithMissingFrame) +{ + distortion_corrector_2d_->setPointCloudTransform("base_link", "missing_lidar_frame"); + EXPECT_FALSE(distortion_corrector_2d_->pointcloud_transform_exists()); + EXPECT_FALSE(distortion_corrector_2d_->pointcloud_transform_needed()); +} + +TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithEmptyTwist) +{ + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); + // Generate the point cloud message + sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp); + + // Process empty twist queue + distortion_corrector_2d_->initialize(); + distortion_corrector_2d_->undistortPointCloud(false, pointcloud); + + // Verify the point cloud is not changed + sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(pointcloud, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(pointcloud, "z"); + + std::array expected_pointcloud = { + {Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.0f, 10.0f, 0.0f), + Eigen::Vector3f(0.0f, 0.0f, 10.0f), Eigen::Vector3f(20.0f, 0.0f, 0.0f), + Eigen::Vector3f(0.0f, 20.0f, 0.0f), Eigen::Vector3f(0.0f, 0.0f, 20.0f), + Eigen::Vector3f(30.0f, 0.0f, 0.0f), Eigen::Vector3f(0.0f, 30.0f, 0.0f), + Eigen::Vector3f(0.0f, 0.0f, 30.0f), Eigen::Vector3f(10.0f, 10.0f, 10.0f)}}; + + size_t i = 0; + std::ostringstream oss; + oss << "Expected pointcloud:\n"; + + for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++i) { + oss << "Point " << i << ": (" << *iter_x << ", " << *iter_y << ", " << *iter_z << ")\n"; + EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance_); + EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance_); + EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance_); + } + + if (debug_) { + RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str()); + } +} + +TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithEmptyPointCloud) +{ + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); + // Generate and process multiple twist messages + auto twist_msgs = generateTwistMsgs(timestamp); + for (const auto & twist_msg : twist_msgs) { + distortion_corrector_2d_->processTwistMessage(twist_msg); + } + // Generate an empty point cloud message + sensor_msgs::msg::PointCloud2 empty_pointcloud = generatePointCloudMsg(false, false, timestamp); + + // Process empty point cloud + distortion_corrector_2d_->initialize(); + distortion_corrector_2d_->undistortPointCloud(true, empty_pointcloud); + + // Verify the point cloud is still empty + EXPECT_EQ(empty_pointcloud.width, 0); + EXPECT_EQ(empty_pointcloud.row_step, 0); +} + +TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithoutImuInBaseLink) +{ + // Generate the point cloud message + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); + sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp); + + // Generate and process multiple twist messages + auto twist_msgs = generateTwistMsgs(timestamp); + for (const auto & twist_msg : twist_msgs) { + distortion_corrector_2d_->processTwistMessage(twist_msg); + } + + // Test using only twist + distortion_corrector_2d_->initialize(); + distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link"); + distortion_corrector_2d_->undistortPointCloud(false, pointcloud); + + sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(pointcloud, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(pointcloud, "z"); + + // Expected undistorted point cloud values + std::array expected_pointcloud = { + {Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.117124f, 10.0f, 0.0f), + Eigen::Vector3f(0.26f, 0.000135182f, 10.0f), Eigen::Vector3f(20.4f, 0.0213818f, 0.0f), + Eigen::Vector3f(0.50932f, 20.0005f, 0.0f), Eigen::Vector3f(0.699999f, 0.000819721f, 20.0f), + Eigen::Vector3f(30.8599f, 0.076f, 0.0f), Eigen::Vector3f(0.947959f, 30.0016f, 0.0f), + Eigen::Vector3f(1.22f, 0.00244382f, 30.0f), Eigen::Vector3f(11.3568f, 10.0463f, 10.0f)}}; + + // Verify each point in the undistorted point cloud + size_t i = 0; + std::ostringstream oss; + oss << "Expected pointcloud:\n"; + + for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++i) { + oss << "Point " << i << ": (" << *iter_x << ", " << *iter_y << ", " << *iter_z << ")\n"; + EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance_); + EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance_); + EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance_); + } + + if (debug_) { + RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str()); + } +} + +TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInBaseLink) +{ + // Generate the point cloud message + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); + sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp); + + // Generate and process multiple twist messages + auto twist_msgs = generateTwistMsgs(timestamp); + for (const auto & twist_msg : twist_msgs) { + distortion_corrector_2d_->processTwistMessage(twist_msg); + } + + // Generate and process multiple IMU messages + auto imu_msgs = generateImuMsgs(timestamp); + for (const auto & imu_msg : imu_msgs) { + distortion_corrector_2d_->processIMUMessage("base_link", imu_msg); + } + + distortion_corrector_2d_->initialize(); + distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link"); + distortion_corrector_2d_->undistortPointCloud(true, pointcloud); + + sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(pointcloud, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(pointcloud, "z"); + + // Expected undistorted point cloud values + std::array expected_pointcloud = { + {Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.122876f, 9.99996f, 0.0f), + Eigen::Vector3f(0.26f, -0.000115049f, 10.0f), Eigen::Vector3f(20.4f, -0.0174931f, 0.0f), + Eigen::Vector3f(0.56301f, 19.9996f, 0.0f), Eigen::Vector3f(0.7f, -0.000627014f, 20.0f), + Eigen::Vector3f(30.86f, -0.052675f, 0.0f), Eigen::Vector3f(1.1004f, 29.9987f, 0.0f), + Eigen::Vector3f(1.22f, -0.00166245f, 30.0f), Eigen::Vector3f(11.4249f, 9.97293f, 10.0f)}}; + + // Verify each point in the undistorted point cloud + size_t i = 0; + std::ostringstream oss; + oss << "Expected pointcloud:\n"; + + for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++i) { + oss << "Point " << i << ": (" << *iter_x << ", " << *iter_y << ", " << *iter_z << ")\n"; + EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance_); + EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance_); + EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance_); + } + + if (debug_) { + RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str()); + } +} + +TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInLidarFrame) +{ + // Generate the point cloud message + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); + sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, true, timestamp); + + // Generate and process multiple twist messages + auto twist_msgs = generateTwistMsgs(timestamp); + for (const auto & twist_msg : twist_msgs) { + distortion_corrector_2d_->processTwistMessage(twist_msg); + } + + // Generate and process multiple IMU messages + auto imu_msgs = generateImuMsgs(timestamp); + for (const auto & imu_msg : imu_msgs) { + distortion_corrector_2d_->processIMUMessage("base_link", imu_msg); + } + + distortion_corrector_2d_->initialize(); + distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top"); + distortion_corrector_2d_->undistortPointCloud(true, pointcloud); + + sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(pointcloud, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(pointcloud, "z"); + + // Expected undistorted point cloud values + std::array expected_pointcloud = { + {Eigen::Vector3f(10.0f, -1.77636e-15f, -4.44089e-16f), + Eigen::Vector3f(0.049989f, 10.0608f, 0.0924992f), + Eigen::Vector3f(0.106107f, 0.130237f, 10.1986f), + Eigen::Vector3f(20.1709f, 0.210011f, 0.32034f), + Eigen::Vector3f(0.220674f, 20.2734f, 0.417974f), + Eigen::Vector3f(0.274146f, 0.347043f, 20.5341f), + Eigen::Vector3f(30.3673f, 0.457564f, 0.700818f), + Eigen::Vector3f(0.418014f, 30.5259f, 0.807963f), + Eigen::Vector3f(0.464088f, 0.600081f, 30.9292f), + Eigen::Vector3f(10.5657f, 10.7121f, 11.094f)}}; + + // Verify each point in the undistorted point cloud + size_t i = 0; + std::ostringstream oss; + oss << "Expected pointcloud:\n"; + + for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++i) { + oss << "Point " << i << ": (" << *iter_x << ", " << *iter_y << ", " << *iter_z << ")\n"; + EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance_); + EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance_); + EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance_); + } + + if (debug_) { + RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str()); + } +} + +TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithoutImuInBaseLink) +{ + // Generate the point cloud message + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); + sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp); + + // Generate and process multiple twist messages + auto twist_msgs = generateTwistMsgs(timestamp); + for (const auto & twist_msg : twist_msgs) { + distortion_corrector_3d_->processTwistMessage(twist_msg); + } + + // Test using only twist + distortion_corrector_3d_->initialize(); + distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link"); + distortion_corrector_3d_->undistortPointCloud(false, pointcloud); + + sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(pointcloud, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(pointcloud, "z"); + + // Expected undistorted point cloud values + std::array expected_pointcloud = { + {Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.117f, 10.0f, 0.0f), + Eigen::Vector3f(0.26f, 9.27035e-05f, 10.0f), Eigen::Vector3f(20.4f, 0.0222176f, 0.0f), + Eigen::Vector3f(0.51f, 20.0004f, 0.0f), Eigen::Vector3f(0.7f, 0.000706573f, 20.0f), + Eigen::Vector3f(30.8599f, 0.0760946f, 0.0f), Eigen::Vector3f(0.946998f, 30.0015f, 0.0f), + Eigen::Vector3f(1.22f, 0.00234201f, 30.0f), Eigen::Vector3f(11.3569f, 10.046f, 10.0f)}}; + + // Verify each point in the undistorted point cloud + size_t i = 0; + std::ostringstream oss; + oss << "Expected pointcloud:\n"; + + for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++i) { + oss << "Point " << i << ": (" << *iter_x << ", " << *iter_y << ", " << *iter_z << ")\n"; + EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance_); + EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance_); + EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance_); + } + + if (debug_) { + RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str()); + } +} + +TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInBaseLink) +{ + // Generate the point cloud message + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); + sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp); + + // Generate and process multiple twist messages + auto twist_msgs = generateTwistMsgs(timestamp); + for (const auto & twist_msg : twist_msgs) { + distortion_corrector_3d_->processTwistMessage(twist_msg); + } + + // Generate and process multiple IMU messages + auto imu_msgs = generateImuMsgs(timestamp); + for (const auto & imu_msg : imu_msgs) { + distortion_corrector_3d_->processIMUMessage("base_link", imu_msg); + } + + distortion_corrector_3d_->initialize(); + distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link"); + distortion_corrector_3d_->undistortPointCloud(true, pointcloud); + + sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(pointcloud, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(pointcloud, "z"); + + // Expected undistorted point cloud values + std::array expected_pointcloud = { + {Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.123015f, 9.99998f, 0.00430552f), + Eigen::Vector3f(0.266103f, -0.00895269f, 9.99992f), + Eigen::Vector3f(20.4f, -0.0176539f, -0.0193392f), + Eigen::Vector3f(0.563265f, 19.9997f, 0.035628f), + Eigen::Vector3f(0.734597f, -0.046068f, 19.9993f), + Eigen::Vector3f(30.8599f, -0.0517931f, -0.0658165f), + Eigen::Vector3f(1.0995f, 29.9989f, 0.0956997f), + Eigen::Vector3f(1.31283f, -0.113544f, 29.9977f), + Eigen::Vector3f(11.461f, 9.93096f, 10.0035f)}}; + + // Verify each point in the undistorted point cloud + size_t i = 0; + std::ostringstream oss; + oss << "Expected pointcloud:\n"; + + for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++i) { + oss << "Point " << i << ": (" << *iter_x << ", " << *iter_y << ", " << *iter_z << ")\n"; + EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance_); + EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance_); + EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance_); + } + + if (debug_) { + RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str()); + } +} + +TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInLidarFrame) +{ + // Generate the point cloud message + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); + sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, true, timestamp); + + // Generate and process multiple twist messages + auto twist_msgs = generateTwistMsgs(timestamp); + for (const auto & twist_msg : twist_msgs) { + distortion_corrector_3d_->processTwistMessage(twist_msg); + } + + // Generate and process multiple IMU messages + auto imu_msgs = generateImuMsgs(timestamp); + for (const auto & imu_msg : imu_msgs) { + distortion_corrector_3d_->processIMUMessage("base_link", imu_msg); + } + + distortion_corrector_3d_->initialize(); + distortion_corrector_3d_->setPointCloudTransform("base_link", "lidar_top"); + distortion_corrector_3d_->undistortPointCloud(true, pointcloud); + + sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(pointcloud, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(pointcloud, "z"); + + // Expected undistorted point cloud values + std::array expected_pointcloud = { + {Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.046484f, 10.0622f, 0.098484f), + Eigen::Vector3f(0.107595f, 0.123767f, 10.2026f), + Eigen::Vector3f(20.1667f, 0.22465f, 0.313351f), + Eigen::Vector3f(0.201149f, 20.2784f, 0.464665f), + Eigen::Vector3f(0.290531f, 0.303489f, 20.5452f), + Eigen::Vector3f(30.3598f, 0.494116f, 0.672914f), + Eigen::Vector3f(0.375848f, 30.5336f, 0.933633f), + Eigen::Vector3f(0.510001f, 0.479651f, 30.9493f), + Eigen::Vector3f(10.5629f, 10.6855f, 11.1461f)}}; + + // Verify each point in the undistorted point cloud + size_t i = 0; + std::ostringstream oss; + oss << "Expected pointcloud:\n"; + + for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++i) { + oss << "Point " << i << ": (" << *iter_x << ", " << *iter_y << ", " << *iter_z << ")\n"; + EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance_); + EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance_); + EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance_); + } + + if (debug_) { + RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str()); + } +} + +TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureLinearMotion) +{ + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); + sensor_msgs::msg::PointCloud2 test2d_pointcloud = generatePointCloudMsg(true, false, timestamp); + sensor_msgs::msg::PointCloud2 test3d_pointcloud = generatePointCloudMsg(true, false, timestamp); + + // Generate and process a single twist message with constant linear velocity + auto twist_msg = generateTwistMsg(1.0, 0.0, timestamp); + + distortion_corrector_2d_->processTwistMessage(twist_msg); + distortion_corrector_2d_->initialize(); + distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link"); + distortion_corrector_2d_->undistortPointCloud(false, test2d_pointcloud); + + distortion_corrector_3d_->processTwistMessage(twist_msg); + distortion_corrector_3d_->initialize(); + distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link"); + distortion_corrector_3d_->undistortPointCloud(false, test3d_pointcloud); + + // Generate expected point cloud for testing + sensor_msgs::msg::PointCloud2 expected_pointcloud_msg = + generatePointCloudMsg(true, false, timestamp); + + // Calculate expected point cloud values based on constant linear motion + double velocity = 1.0; // 1 m/s linear velocity + sensor_msgs::PointCloud2Iterator iter_x(expected_pointcloud_msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y(expected_pointcloud_msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z(expected_pointcloud_msg, "z"); + sensor_msgs::PointCloud2Iterator iter_t(expected_pointcloud_msg, "time_stamp"); + + std::vector expected_points; + for (; iter_t != iter_t.end(); ++iter_t, ++iter_x, ++iter_y, ++iter_z) { + double time_offset = *iter_t - timestamp.seconds(); + expected_points.emplace_back( + *iter_x + static_cast(velocity * time_offset), *iter_y, *iter_z); + } + + // Verify each point in the undistorted point cloud + sensor_msgs::PointCloud2Iterator test2d_iter_x(test2d_pointcloud, "x"); + sensor_msgs::PointCloud2Iterator test2d_iter_y(test2d_pointcloud, "y"); + sensor_msgs::PointCloud2Iterator test2d_iter_z(test2d_pointcloud, "z"); + size_t i = 0; + std::ostringstream oss; + oss << "Expected pointcloud:\n"; + + for (; test2d_iter_x != test2d_iter_x.end(); + ++test2d_iter_x, ++test2d_iter_y, ++test2d_iter_z, ++i) { + oss << "Point " << i << ": (" << *test2d_iter_x << ", " << *test2d_iter_y << ", " + << *test2d_iter_z << ")\n"; + EXPECT_FLOAT_EQ(*test2d_iter_x, expected_points[i].x()); + EXPECT_FLOAT_EQ(*test2d_iter_y, expected_points[i].y()); + EXPECT_FLOAT_EQ(*test2d_iter_z, expected_points[i].z()); + } + + if (debug_) { + RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str()); + } + + // Verify each point in the undistorted 2d point cloud with undistorted 3d point cloud + test2d_iter_x = sensor_msgs::PointCloud2Iterator(test2d_pointcloud, "x"); + test2d_iter_y = sensor_msgs::PointCloud2Iterator(test2d_pointcloud, "y"); + test2d_iter_z = sensor_msgs::PointCloud2Iterator(test2d_pointcloud, "z"); + sensor_msgs::PointCloud2Iterator test3d_iter_x(test3d_pointcloud, "x"); + sensor_msgs::PointCloud2Iterator test3d_iter_y(test3d_pointcloud, "y"); + sensor_msgs::PointCloud2Iterator test3d_iter_z(test3d_pointcloud, "z"); + + i = 0; + oss.str(""); + oss.clear(); + + oss << "Expected pointcloud:\n"; + for (; test2d_iter_x != test2d_iter_x.end(); ++test2d_iter_x, ++test2d_iter_y, ++test2d_iter_z, + ++test3d_iter_x, ++test3d_iter_y, ++test3d_iter_z, + ++i) { + oss << "Point " << i << " - 2D: (" << *test2d_iter_x << ", " << *test2d_iter_y << ", " + << *test2d_iter_z << ")" + << " vs 3D: (" << *test3d_iter_x << ", " << *test3d_iter_y << ", " << *test3d_iter_z + << ")\n"; + EXPECT_FLOAT_EQ(*test2d_iter_x, *test3d_iter_x); + EXPECT_FLOAT_EQ(*test2d_iter_y, *test3d_iter_y); + EXPECT_FLOAT_EQ(*test2d_iter_z, *test3d_iter_z); + } + if (debug_) { + RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str()); + } +} + +TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureRotationalMotion) +{ + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); + sensor_msgs::msg::PointCloud2 test2d_pointcloud = generatePointCloudMsg(true, false, timestamp); + sensor_msgs::msg::PointCloud2 test3d_pointcloud = generatePointCloudMsg(true, false, timestamp); + + // Generate and process a single twist message with constant angular velocity + auto twist_msg = generateTwistMsg(0.0, 0.1, timestamp); + + distortion_corrector_2d_->processTwistMessage(twist_msg); + distortion_corrector_2d_->initialize(); + distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link"); + distortion_corrector_2d_->undistortPointCloud(false, test2d_pointcloud); + + distortion_corrector_3d_->processTwistMessage(twist_msg); + distortion_corrector_3d_->initialize(); + distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link"); + distortion_corrector_3d_->undistortPointCloud(false, test3d_pointcloud); + + // Generate expected point cloud for testing + sensor_msgs::msg::PointCloud2 expected_pointcloud_msg = + generatePointCloudMsg(true, false, timestamp); + + // Calculate expected point cloud values based on constant rotational motion + double angular_velocity = 0.1; // 0.1 rad/s rotational velocity + sensor_msgs::PointCloud2Iterator iter_x(expected_pointcloud_msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y(expected_pointcloud_msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z(expected_pointcloud_msg, "z"); + sensor_msgs::PointCloud2Iterator iter_t(expected_pointcloud_msg, "time_stamp"); + + std::vector expected_pointcloud; + for (; iter_t != iter_t.end(); ++iter_t, ++iter_x, ++iter_y, ++iter_z) { + double time_offset = *iter_t - timestamp.seconds(); + float angle = angular_velocity * time_offset; + + // Set the quaternion for the current angle + tf2::Quaternion quaternion; + quaternion.setValue( + 0, 0, autoware::universe_utils::sin(angle * 0.5f), + autoware::universe_utils::cos(angle * 0.5f)); + + tf2::Vector3 point(*iter_x, *iter_y, *iter_z); + tf2::Vector3 rotated_point = tf2::quatRotate(quaternion, point); + expected_pointcloud.emplace_back( + static_cast(rotated_point.x()), static_cast(rotated_point.y()), + static_cast(rotated_point.z())); + } + + // Verify each point in the undistorted 2D point cloud + sensor_msgs::PointCloud2Iterator test2d_iter_x(test2d_pointcloud, "x"); + sensor_msgs::PointCloud2Iterator test2d_iter_y(test2d_pointcloud, "y"); + sensor_msgs::PointCloud2Iterator test2d_iter_z(test2d_pointcloud, "z"); + + size_t i = 0; + std::ostringstream oss; + oss << "Expected pointcloud:\n"; + + for (; test2d_iter_x != test2d_iter_x.end(); + ++test2d_iter_x, ++test2d_iter_y, ++test2d_iter_z, ++i) { + oss << "Point " << i << ": (" << *test2d_iter_x << ", " << *test2d_iter_y << ", " + << *test2d_iter_z << ")\n"; + EXPECT_FLOAT_EQ(*test2d_iter_x, expected_pointcloud[i].x()); + EXPECT_FLOAT_EQ(*test2d_iter_y, expected_pointcloud[i].y()); + EXPECT_FLOAT_EQ(*test2d_iter_z, expected_pointcloud[i].z()); + } + + if (debug_) { + RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str()); + } + + // Verify each point in the undistorted 2D point cloud with undistorted 3D point cloud + test2d_iter_x = sensor_msgs::PointCloud2Iterator(test2d_pointcloud, "x"); + test2d_iter_y = sensor_msgs::PointCloud2Iterator(test2d_pointcloud, "y"); + test2d_iter_z = sensor_msgs::PointCloud2Iterator(test2d_pointcloud, "z"); + sensor_msgs::PointCloud2Iterator test3d_iter_x(test3d_pointcloud, "x"); + sensor_msgs::PointCloud2Iterator test3d_iter_y(test3d_pointcloud, "y"); + sensor_msgs::PointCloud2Iterator test3d_iter_z(test3d_pointcloud, "z"); + + i = 0; + oss.str(""); + oss.clear(); + + oss << "Expected pointcloud:\n"; + for (; test2d_iter_x != test2d_iter_x.end(); ++test2d_iter_x, ++test2d_iter_y, ++test2d_iter_z, + ++test3d_iter_x, ++test3d_iter_y, ++test3d_iter_z, + ++i) { + oss << "Point " << i << " - 2D: (" << *test2d_iter_x << ", " << *test2d_iter_y << ", " + << *test2d_iter_z << ")" + << " vs 3D: (" << *test3d_iter_x << ", " << *test3d_iter_y << ", " << *test3d_iter_z + << ")\n"; + EXPECT_NEAR(*test2d_iter_x, *test3d_iter_x, coarse_tolerance_); + EXPECT_NEAR(*test2d_iter_y, *test3d_iter_y, coarse_tolerance_); + EXPECT_NEAR(*test2d_iter_z, *test3d_iter_z, coarse_tolerance_); + } + + if (debug_) { + RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str()); + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int ret = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return ret; +}