Merge branch 'main' into perf_occupancy_grid_map

autowarefoundation · Jul 9, 2024 · 9f334dc · 9f334dc
2 parents 5452685 + f716b5b
commit 9f334dc
Show file tree

Hide file tree

Showing 148 changed files with 1,956 additions and 1,540 deletions.
diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS
@@ -6,7 +6,7 @@ common/autoware_motion_utils/** [email protected] [email protected]
 common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/** [email protected]
 common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/** [email protected]
 common/autoware_perception_rviz_plugin/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
-common/autoware_point_types/** taichi.higashide@tier4.jp
+common/autoware_point_types/** [email protected] max.schmeller@tier4.jp
 common/autoware_test_utils/** [email protected] [email protected] [email protected] [email protected]
 common/autoware_testing/** [email protected] [email protected] [email protected] [email protected]
 common/autoware_universe_utils/** [email protected] [email protected] [email protected]
@@ -115,9 +115,9 @@ perception/elevation_map_loader/** [email protected] shintaro.tomie@tier4
 perception/euclidean_cluster/** [email protected] [email protected]
 perception/ground_segmentation/** [email protected] [email protected] [email protected] [email protected] [email protected]
 perception/image_projection_based_fusion/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
-perception/lidar_apollo_instance_segmentation/** [email protected]
-perception/lidar_apollo_segmentation_tvm/** [email protected] xinyu.wang@tier4.jp
-perception/lidar_apollo_segmentation_tvm_nodes/** [email protected] xinyu.wang@tier4.jp
+perception/lidar_apollo_instance_segmentation/** [email protected] [email protected]
+perception/lidar_apollo_segmentation_tvm/** [email protected] yoshi.ri@tier4.jp
+perception/lidar_apollo_segmentation_tvm_nodes/** [email protected] yoshi.ri@tier4.jp
 perception/lidar_centerpoint/** [email protected] [email protected]
 perception/lidar_transfusion/** [email protected] [email protected] [email protected]
 perception/multi_object_tracker/** [email protected] [email protected] [email protected]
@@ -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/** [email protected] [email protected] [email protected]
 planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/** [email protected] [email protected] [email protected] [email protected]
 planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/** [email protected] [email protected]
-planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/** [email protected] [email protected]
+planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/** [email protected]
 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/** [email protected] [email protected] [email protected] [email protected]
-planning/motion_velocity_planner/autoware_motion_velocity_planner_common/** [email protected] [email protected] [email protected]
-planning/motion_velocity_planner/autoware_motion_velocity_planner_node/** [email protected] [email protected] [email protected]
+planning/motion_velocity_planner/autoware_motion_velocity_planner_common/** [email protected]
+planning/motion_velocity_planner/autoware_motion_velocity_planner_node/** [email protected]
 planning/sampling_based_planner/autoware_bezier_sampler/** [email protected]
 planning/sampling_based_planner/autoware_frenet_planner/** [email protected]
 planning/sampling_based_planner/autoware_path_sampler/** [email protected]
@@ -204,7 +204,7 @@ sensing/gnss_poser/** [email protected] [email protected] koji.minod
 sensing/image_diagnostics/** [email protected] [email protected]
 sensing/image_transport_decompressor/** [email protected] [email protected]
 sensing/imu_corrector/** [email protected] [email protected] [email protected]
-sensing/livox/livox_tag_filter/** [email protected] [email protected]
+sensing/livox/autoware_livox_tag_filter/** [email protected] [email protected]
 sensing/pointcloud_preprocessor/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 sensing/radar_scan_to_pointcloud2/** [email protected] [email protected] [email protected] [email protected]
 sensing/radar_static_pointcloud_filter/** [email protected] [email protected] [email protected] [email protected]

diff --git 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
@@ -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 <typename... Reporters>
+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<ProcessingTimeDetail>::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<std_msgs::msg::String>::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 <rclcpp/rclcpp.hpp>
+
+#include <iostream>
+#include <memory>
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<rclcpp::Node>("time_keeper_example");
+
+  auto time_keeper = std::make_shared<autoware::universe_utils::TimeKeeper>();
+
+  time_keeper->add_reporter(&std::cout);
+
+  auto publisher =
+    node->create_publisher<autoware::universe_utils::ProcessingTimeDetail>("processing_time", 10);
+
+  time_keeper->add_reporter(publisher);
+
+  auto publisher_str = node->create_publisher<std_msgs::msg::String>("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 <rclcpp/rclcpp.hpp>
+
+#include <iostream>
+#include <memory>
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<rclcpp::Node>("scoped_time_track_example");
+
+  auto time_keeper = std::make_shared<autoware::universe_utils::TimeKeeper>();
+
+  time_keeper->add_reporter(&std::cout);
+
+  auto publisher =
+    node->create_publisher<autoware::universe_utils::ProcessingTimeDetail>("processing_time", 10);
+
+  time_keeper->add_reporter(publisher);
+
+  auto publisher_str = node->create_publisher<std_msgs::msg::String>("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)
+
+  ---
+  ```