Merge pull request autowarefoundation#358 from tier4/sync-upstream

chore: sync upstream

common/tensorrt_common/CMakeLists.txt

@@ -1,57 +1,81 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.17)
 project(tensorrt_common)
 
-find_package(autoware_cmake REQUIRED)
-autoware_package()
-
-# TODO(tensorrt_common): Remove once upgrading to TensorRT 8.5 is complete
-add_compile_options(-Wno-deprecated-declarations)
+find_package(ament_cmake REQUIRED)
+find_package(cudnn_cmake_module REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(tensorrt_cmake_module REQUIRED)
 
 find_package(CUDA)
+find_package(CUDAToolkit)
 find_package(CUDNN)
 find_package(TENSORRT)
 
-if(NOT (${CUDA_FOUND} AND ${CUDNN_FOUND} AND ${TENSORRT_FOUND}))
+if(NOT (CUDAToolkit_FOUND AND CUDNN_FOUND AND TENSORRT_FOUND))
   message(WARNING "cuda, cudnn, tensorrt libraries are not found")
   return()
 endif()
 
-cuda_add_library(${PROJECT_NAME} SHARED
+add_library(${PROJECT_NAME} SHARED
   src/tensorrt_common.cpp
 )
 
-ament_target_dependencies(${PROJECT_NAME}
-  rclcpp
+target_link_libraries(${PROJECT_NAME}
+  CUDA::cudart
+  rclcpp::rclcpp
+  stdc++fs
+  ${TENSORRT_LIBRARIES}
 )
 
 target_include_directories(${PROJECT_NAME}
   PUBLIC
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-    $<INSTALL_INTERFACE:include>
+    $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
     ${TENSORRT_INCLUDE_DIRS}
 )
 
-target_link_libraries(${PROJECT_NAME}
-  ${TENSORRT_LIBRARIES}
-  stdc++fs
+set_target_properties(${PROJECT_NAME}
+  PROPERTIES
+    CXX_STANDARD 17
+    CXX_STANDARD_REQUIRED YES
+    CXX_EXTENSIONS NO
+)
+
+# TODO(tensorrt_common): Remove -Wno-deprecated-declarations once upgrading to TensorRT 8.5 is complete
+target_compile_options(${PROJECT_NAME} PRIVATE
+  -Wall -Wextra -Wpedantic -Werror -Wno-deprecated-declarations
 )
 
 target_compile_definitions(${PROJECT_NAME} PRIVATE
   TENSORRT_VERSION_MAJOR=${TENSORRT_VERSION_MAJOR}
 )
 
-list(APPEND ${PROJECT_NAME}_LIBRARIES "${PROJECT_NAME}")
-
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
+
+  # These don't pass yet, disable them for now
+  set(ament_cmake_copyright_FOUND TRUE)
+  set(ament_cmake_flake8_FOUND TRUE)
+  set(ament_cmake_pep257_FOUND TRUE)
+  set(ament_cmake_uncrustify_FOUND TRUE)
+
   ament_lint_auto_find_test_dependencies()
 endif()
 
-ament_export_include_directories(include)
-ament_export_dependencies(CUDA)
-ament_export_dependencies(cudnn_cmake_module)
-ament_export_dependencies(CUDNN)
-ament_export_dependencies(tensorrt_cmake_module)
-ament_export_dependencies(TENSORRT)
+install(TARGETS ${PROJECT_NAME} EXPORT export_${PROJECT_NAME})
+install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME})
+
+ament_export_include_directories("include/${PROJECT_NAME}")
+ament_export_targets(export_${PROJECT_NAME})
+
+ament_export_dependencies(
+  "CUDA"
+  "CUDAToolkit"
+  "cudnn_cmake_module"
+  "CUDNN"
+  "rclcpp"
+  "tensorrt_cmake_module"
+  "TENSORRT"
+)
 
-ament_auto_package()
+ament_package()

common/tensorrt_common/package.xml

@@ -11,16 +11,14 @@
 
   <license>Apache License 2.0</license>
 
-  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
   <buildtool_depend>cudnn_cmake_module</buildtool_depend>
   <buildtool_depend>tensorrt_cmake_module</buildtool_depend>
 
-  <build_depend>autoware_cmake</build_depend>
-
   <depend>rclcpp</depend>
 
   <test_depend>ament_lint_auto</test_depend>
-  <test_depend>autoware_lint_common</test_depend>
+  <test_depend>ament_lint_common</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

control/mpc_lateral_controller/src/mpc.cpp

@@ -24,9 +24,6 @@
 #include <utility>
 #include <vector>
 
-#define DEG2RAD 3.1415926535 / 180.0
-#define RAD2DEG 180.0 / 3.1415926535
-
 namespace autoware::motion::control::mpc_lateral_controller
 {
 using namespace std::literals::chrono_literals;
@@ -297,7 +294,8 @@ bool MPC::getData(
   if (std::fabs(data->yaw_err) > m_admissible_yaw_error_rad) {
     RCLCPP_WARN_SKIPFIRST_THROTTLE(
       m_logger, *m_clock, duration, "yaw error is over limit. error = %f deg, limit %f deg",
-      RAD2DEG * data->yaw_err, RAD2DEG * m_admissible_yaw_error_rad);
+      tier4_autoware_utils::rad2deg(data->yaw_err),
+      tier4_autoware_utils::rad2deg(m_admissible_yaw_error_rad));
     return false;
   }
 
@@ -594,7 +592,7 @@ MPCMatrix MPC::generateMPCMatrix(
     /* get reference input (feed-forward) */
     m_vehicle_model_ptr->setCurvature(ref_smooth_k);
     m_vehicle_model_ptr->calculateReferenceInput(Uref);
-    if (std::fabs(Uref(0, 0)) < DEG2RAD * m_param.zero_ff_steer_deg) {
+    if (std::fabs(Uref(0, 0)) < tier4_autoware_utils::deg2rad(m_param.zero_ff_steer_deg)) {
       Uref(0, 0) = 0.0;  // ignore curvature noise
     }
     m.Uref_ex.block(i * DIM_U, 0, DIM_U, 1) = Uref;

launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py

@@ -321,10 +321,14 @@ def create_single_frame_outlier_filter_components(input_topic, output_topic, con
                     ("output/elevation_map", "map"),
                     ("input/pointcloud_map", "/map/pointcloud_map"),
                     ("input/vector_map", "/map/vector_map"),
+                    ("input/pointcloud_map_metadata", "/map/pointcloud_map_metadata"),
+                    ("service/get_selected_pointcloud_map", "/map/get_selected_pointcloud_map"),
                 ],
                 parameters=[
                     {
                         "use_lane_filter": False,
+                        "use_sequential_load": False,
+                        "sequential_map_load_num": 1,
                         "use_inpaint": True,
                         "inpaint_radius": 1.0,
                         "lane_margin": 2.0,

perception/elevation_map_loader/README.md

@@ -20,10 +20,11 @@ Cells with No elevation value can be inpainted using the values of neighboring c
 
 ### Input
 
-| Name                   | Type                                         | Description                                |
-| ---------------------- | -------------------------------------------- | ------------------------------------------ |
-| `input/pointcloud_map` | `sensor_msgs::msg::PointCloud2`              | The point cloud map                        |
-| `input/vector_map`     | `autoware_auto_mapping_msgs::msg::HADMapBin` | (Optional) The binary data of lanelet2 map |
+| Name                            | Type                                            | Description                                |
+| ------------------------------- | ----------------------------------------------- | ------------------------------------------ |
+| `input/pointcloud_map`          | `sensor_msgs::msg::PointCloud2`                 | The point cloud map                        |
+| `input/vector_map`              | `autoware_auto_mapping_msgs::msg::HADMapBin`    | (Optional) The binary data of lanelet2 map |
+| `input/pointcloud_map_metadata` | `autoware_map_msgs::msg::PointCloudMapMetaData` | (Optional) The metadata of point cloud map |
 
 ### Output
 
@@ -32,21 +33,29 @@ Cells with No elevation value can be inpainted using the values of neighboring c
 | `output/elevation_map`       | `grid_map_msgs::msg::GridMap`   | The elevation map                                                    |
 | `output/elevation_map_cloud` | `sensor_msgs::msg::PointCloud2` | (Optional) The point cloud generated from the value of elevation map |
 
+### Service
+
+| Name                           | Type                                               | Description                                                                                                                               |
+| ------------------------------ | -------------------------------------------------- | ----------------------------------------------------------------------------------------------------------------------------------------- |
+| `service/get_selected_pcd_map` | `autoware_map_msgs::srv::GetSelectedPointCloudMap` | (Optional) service to request point cloud map. If pointcloud_map_loader uses selected pointcloud map loading via ROS 2 service, use this. |
+
 ## Parameters
 
 ### Node parameters
 
-| Name                              | Type        | Description                                                                                                                       | Default value |
-| :-------------------------------- | :---------- | :-------------------------------------------------------------------------------------------------------------------------------- | :------------ |
-| map_layer_name                    | std::string | elevation_map layer name                                                                                                          | elevation     |
-| param_file_path                   | std::string | GridMap parameters config                                                                                                         | path_default  |
-| elevation_map_directory           | std::string | elevation_map file (bag2)                                                                                                         | path_default  |
-| map_frame                         | std::string | map_frame when loading elevation_map file                                                                                         | map           |
-| use_inpaint                       | bool        | Whether to inpaint empty cells                                                                                                    | true          |
-| inpaint_radius                    | float       | Radius of a circular neighborhood of each point inpainted that is considered by the algorithm [m]                                 | 0.3           |
-| use_elevation_map_cloud_publisher | bool        | Whether to publish `output/elevation_map_cloud`                                                                                   | false         |
-| use_lane_filter                   | bool        | Whether to filter elevation_map with vector_map                                                                                   | false         |
-| lane_margin                       | float       | Margin distance from the lane polygon of the area to be included in the inpainting mask [m]. Used only when use_lane_filter=True. | 0.0           |
+| Name                              | Type        | Description                                                                                                                                                          | Default value |
+| :-------------------------------- | :---------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
+| map_layer_name                    | std::string | elevation_map layer name                                                                                                                                             | elevation     |
+| param_file_path                   | std::string | GridMap parameters config                                                                                                                                            | path_default  |
+| elevation_map_directory           | std::string | elevation_map file (bag2)                                                                                                                                            | path_default  |
+| map_frame                         | std::string | map_frame when loading elevation_map file                                                                                                                            | map           |
+| use_inpaint                       | bool        | Whether to inpaint empty cells                                                                                                                                       | true          |
+| inpaint_radius                    | float       | Radius of a circular neighborhood of each point inpainted that is considered by the algorithm [m]                                                                    | 0.3           |
+| use_elevation_map_cloud_publisher | bool        | Whether to publish `output/elevation_map_cloud`                                                                                                                      | false         |
+| use_lane_filter                   | bool        | Whether to filter elevation_map with vector_map                                                                                                                      | false         |
+| lane_margin                       | float       | Margin distance from the lane polygon of the area to be included in the inpainting mask [m]. Used only when use_lane_filter=True.                                    | 0.0           |
+| use_sequential_load               | bool        | Whether to get point cloud map by service                                                                                                                            | false         |
+| sequential_map_load_num           | int         | The number of point cloud maps to load at once (only used when use_sequential_load is set true). This should not be larger than number of all point cloud map cells. | 1             |
 
 ### GridMap parameters
 

perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp

@@ -26,6 +26,8 @@
 
 #include "tier4_external_api_msgs/msg/map_hash.hpp"
 #include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
+#include <autoware_map_msgs/msg/point_cloud_map_meta_data.hpp>
+#include <autoware_map_msgs/srv/get_selected_point_cloud_map.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 
 #include <pcl/pcl_base.h>
@@ -53,6 +55,7 @@ class DataManager
   pcl::PointCloud<pcl::PointXYZ>::Ptr map_pcl_ptr_;
   lanelet::LaneletMapPtr lanelet_map_ptr_;
   bool use_lane_filter_ = false;
+  std::vector<std::string> pointcloud_map_ids_;
 };
 
 class ElevationMapLoaderNode : public rclcpp::Node
@@ -64,12 +67,25 @@ class ElevationMapLoaderNode : public rclcpp::Node
   rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_pointcloud_map_;
   rclcpp::Subscription<autoware_auto_mapping_msgs::msg::HADMapBin>::SharedPtr sub_vector_map_;
   rclcpp::Subscription<tier4_external_api_msgs::msg::MapHash>::SharedPtr sub_map_hash_;
+  rclcpp::Subscription<autoware_map_msgs::msg::PointCloudMapMetaData>::SharedPtr
+    sub_pointcloud_metadata_;
   rclcpp::Publisher<grid_map_msgs::msg::GridMap>::SharedPtr pub_elevation_map_;
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_elevation_map_cloud_;
+  rclcpp::Client<autoware_map_msgs::srv::GetSelectedPointCloudMap>::SharedPtr pcd_loader_client_;
+  rclcpp::CallbackGroup::SharedPtr group_;
+  rclcpp::TimerBase::SharedPtr timer_;
   void onPointcloudMap(const sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud_map);
   void onMapHash(const tier4_external_api_msgs::msg::MapHash::ConstSharedPtr map_hash);
+  void timerCallback();
   void onVectorMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr vector_map);
-
+  void onPointCloudMapMetaData(
+    const autoware_map_msgs::msg::PointCloudMapMetaData pointcloud_map_metadata);
+  void receiveMap();
+  void concatenatePointCloudMaps(
+    sensor_msgs::msg::PointCloud2 & pointcloud_map,
+    const std::vector<autoware_map_msgs::msg::PointCloudMapCellWithID> & new_pointcloud_with_ids)
+    const;
+  std::vector<std::string> getRequestIDs(const unsigned int map_id_counter) const;
   void publish();
   void createElevationMap();
   void setVerbosityLevelToDebugIfFlagSet();
@@ -85,8 +101,12 @@ class ElevationMapLoaderNode : public rclcpp::Node
   std::string elevation_map_directory_;
   bool use_inpaint_;
   float inpaint_radius_;
+  unsigned int sequential_map_load_num_;
   bool use_elevation_map_cloud_publisher_;
   std::string param_file_path_;
+  bool is_map_metadata_received_ = false;
+  bool is_map_received_ = false;
+  bool is_elevation_map_published_ = false;
 
   DataManager data_manager_;
   struct LaneFilter

perception/elevation_map_loader/launch/elevation_map_loader.launch.xml

@@ -2,16 +2,22 @@
   <arg name="elevation_map_directory" default="$(find-pkg-share elevation_map_loader)/data/elevation_maps"/>
   <arg name="param_file_path" default="$(find-pkg-share elevation_map_loader)/config/elevation_map_parameters.yaml"/>
   <arg name="use_lane_filter" default="false"/>
+  <arg name="use_sequential_load" default="false"/>
+  <arg name="sequential_map_load_num" default="1"/>
   <arg name="use_inpaint" default="true"/>
   <arg name="inpaint_radius" default="1.0"/>
 
   <node pkg="elevation_map_loader" exec="elevation_map_loader" name="elevation_map_loader" output="screen">
     <remap from="output/elevation_map" to="/map/elevation_map"/>
     <remap from="input/pointcloud_map" to="/map/pointcloud_map"/>
+    <remap from="input/pointcloud_map_metadata" to="/map/pointcloud_map_metadata"/>
     <remap from="input/vector_map" to="/map/vector_map"/>
+    <remap from="service/get_selected_pcd_map" to="/map/get_selected_pointcloud_map"/>
 
     <param name="elevation_map_directory" value="$(var elevation_map_directory)"/>
     <param name="param_file_path" value="$(var param_file_path)"/>
     <param name="use_lane_filter" value="$(var use_lane_filter)"/>
+    <param name="use_sequential_load" value="$(var use_sequential_load)"/>
+    <param name="sequential_map_load_num" value="$(var sequential_map_load_num)"/>
   </node>
 </launch>

perception/elevation_map_loader/package.xml

@@ -13,6 +13,7 @@
   <build_depend>autoware_cmake</build_depend>
 
   <depend>autoware_auto_mapping_msgs</depend>
+  <depend>autoware_map_msgs</depend>
   <depend>grid_map_cv</depend>
   <depend>grid_map_pcl</depend>
   <depend>grid_map_ros</depend>