Merge pull request #376 from tier4/sync-upstream

chore: sync upstream

common/autoware_ad_api_specs/include/autoware_ad_api_specs/routing.hpp

@@ -38,6 +38,18 @@ struct SetRoute
   static constexpr char name[] = "/api/routing/set_route";
 };
 
+struct ChangeRoutePoints
+{
+  using Service = autoware_adapi_v1_msgs::srv::SetRoutePoints;
+  static constexpr char name[] = "/api/routing/change_route_points";
+};
+
+struct ChangeRoute
+{
+  using Service = autoware_adapi_v1_msgs::srv::SetRoute;
+  static constexpr char name[] = "/api/routing/change_route";
+};
+
 struct ClearRoute
 {
   using Service = autoware_adapi_v1_msgs::srv::ClearRoute;

common/autoware_ad_api_specs/package.xml

@@ -7,7 +7,7 @@
   <maintainer email="isamu.takagi@tier4.jp">Takagi, Isamu</maintainer>
   <maintainer email="makoto.yabuta@tier4.jp">yabuta</maintainer>
   <maintainer email="kahhooi.tan@tier4.jp">Kah Hooi Tan</maintainer>
-  <maintainer email="kenji.miyake@tier4.jp">Kenji Miyake</maintainer>
+  <maintainer email="ryohsuke.mitsudome@tier4.jp">Ryohsuke Mitsudome</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>

common/component_interface_utils/package.xml

@@ -7,7 +7,7 @@
   <maintainer email="isamu.takagi@tier4.jp">Takagi, Isamu</maintainer>
   <maintainer email="makoto.yabuta@tier4.jp">yabuta</maintainer>
   <maintainer email="kahhooi.tan@tier4.jp">Kah Hooi Tan</maintainer>
-  <maintainer email="kenji.miyake@tier4.jp">Kenji Miyake</maintainer>
+  <maintainer email="yukihiro.saito@tier4.jp">Yukihiro Saito</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>

common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp

@@ -90,6 +90,9 @@ std::string getModuleName(const uint8_t module_type)
     case Module::OCCLUSION_SPOT: {
       return "occlusion_spot";
     }
+    case Module::INTERSECTION_OCCLUSION: {
+      return "intersection_occlusion";
+    }
   }
   return "NONE";
 }
@@ -111,7 +114,7 @@ bool isPathChangeModule(const uint8_t module_type)
 RTCManagerPanel::RTCManagerPanel(QWidget * parent) : rviz_common::Panel(parent)
 {
   // TODO(tanaka): replace this magic number to Module::SIZE
-  const size_t module_size = 18;
+  const size_t module_size = 19;
   auto_modes_.reserve(module_size);
   auto * v_layout = new QVBoxLayout;
   auto vertical_header = new QHeaderView(Qt::Vertical);

perception/compare_map_segmentation/README.md

@@ -16,11 +16,11 @@ Compare the z of the input points with the value of elevation_map. The height di
 
 ### Distance Based Compare Map Filter
 
-WIP
+This filter compares the input pointcloud with the map pointcloud using the `nearestKSearch` function of `kdtree` and removes points that are close to the map point cloud. The map pointcloud can be loaded statically at once at the beginning or dynamically as the vehicle moves.
 
 ### Voxel Based Approximate Compare Map Filter
 
-WIP
+The filter loads the map point cloud, which can be loaded statically at the beginning or dynamically during vehicle movement, and creates a voxel grid of the map point cloud. The filter uses the getCentroidIndexAt function in combination with the getGridCoordinates function from the VoxelGrid class to find input points that are inside the voxel grid and removes them.
 
 ### Voxel Based Compare Map Filter
 
@@ -30,7 +30,7 @@ For each point of input pointcloud, the filter use `getCentroidIndexAt` combine
 
 ### Voxel Distance based Compare Map Filter
 
-WIP
+This filter is a combination of the distance_based_compare_map_filter and voxel_based_approximate_compare_map_filter. The filter loads the map point cloud, which can be loaded statically at the beginning or dynamically during vehicle movement, and creates a voxel grid and a k-d tree of the map point cloud. The filter uses the getCentroidIndexAt function in combination with the getGridCoordinates function from the VoxelGrid class to find input points that are inside the voxel grid and removes them. For points that do not belong to any voxel grid, they are compared again with the map point cloud using the radiusSearch function of the k-d tree and are removed if they are close enough to the map.
 
 ## Inputs / Outputs
 

perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp

@@ -16,33 +16,99 @@
 #define COMPARE_MAP_SEGMENTATION__DISTANCE_BASED_COMPARE_MAP_FILTER_NODELET_HPP_
 
 #include "pointcloud_preprocessor/filter.hpp"
+#include "voxel_grid_map_loader.hpp"
 
+#include <pcl/common/point_tests.h>  // for pcl::isFinite
 #include <pcl/filters/voxel_grid.h>
 #include <pcl/search/pcl_search.h>
 
+#include <memory>
+#include <string>
 #include <vector>
 
 namespace compare_map_segmentation
 {
+
+typedef typename pcl::Filter<pcl::PointXYZ>::PointCloud PointCloud;
+typedef typename PointCloud::Ptr PointCloudPtr;
+typedef typename PointCloud::ConstPtr PointCloudConstPtr;
+
+class DistanceBasedStaticMapLoader : public VoxelGridStaticMapLoader
+{
+private:
+  PointCloudConstPtr map_ptr_;
+  pcl::search::Search<pcl::PointXYZ>::Ptr tree_;
+
+public:
+  DistanceBasedStaticMapLoader(
+    rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex)
+  : VoxelGridStaticMapLoader(node, leaf_size, tf_map_input_frame, mutex)
+  {
+    RCLCPP_INFO(logger_, "DistanceBasedStaticMapLoader initialized.\n");
+  }
+
+  void onMapCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr map) override;
+  bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) override;
+};
+
+class DistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader
+{
+public:
+  DistanceBasedDynamicMapLoader(
+    rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex,
+    rclcpp::CallbackGroup::SharedPtr main_callback_group)
+  : VoxelGridDynamicMapLoader(node, leaf_size, tf_map_input_frame, mutex, main_callback_group)
+  {
+    RCLCPP_INFO(logger_, "DistanceBasedDynamicMapLoader initialized.\n");
+  }
+  bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) override;
+
+  inline void addMapCellAndFilter(
+    const autoware_map_msgs::msg::PointCloudMapCellWithID & map_cell_to_add) override
+  {
+    map_grid_size_x_ = map_cell_to_add.metadata.max_x - map_cell_to_add.metadata.min_x;
+    map_grid_size_y_ = map_cell_to_add.metadata.max_y - map_cell_to_add.metadata.min_y;
+
+    pcl::PointCloud<pcl::PointXYZ> map_cell_pc_tmp;
+    pcl::fromROSMsg(map_cell_to_add.pointcloud, map_cell_pc_tmp);
+
+    auto map_cell_voxel_input_tmp_ptr =
+      std::make_shared<pcl::PointCloud<pcl::PointXYZ>>(map_cell_pc_tmp);
+
+    MapGridVoxelInfo current_voxel_grid_list_item;
+    current_voxel_grid_list_item.min_b_x = map_cell_to_add.metadata.min_x;
+    current_voxel_grid_list_item.min_b_y = map_cell_to_add.metadata.min_y;
+    current_voxel_grid_list_item.max_b_x = map_cell_to_add.metadata.max_x;
+    current_voxel_grid_list_item.max_b_y = map_cell_to_add.metadata.max_y;
+
+    // add kdtree
+    pcl::search::Search<pcl::PointXYZ>::Ptr tree_tmp;
+    if (!tree_tmp) {
+      if (map_cell_voxel_input_tmp_ptr->isOrganized()) {
+        tree_tmp.reset(new pcl::search::OrganizedNeighbor<pcl::PointXYZ>());
+      } else {
+        tree_tmp.reset(new pcl::search::KdTree<pcl::PointXYZ>(false));
+      }
+    }
+    tree_tmp->setInputCloud(map_cell_voxel_input_tmp_ptr);
+    current_voxel_grid_list_item.map_cell_kdtree = tree_tmp;
+
+    // add
+    (*mutex_ptr_).lock();
+    current_voxel_grid_dict_.insert({map_cell_to_add.cell_id, current_voxel_grid_list_item});
+    (*mutex_ptr_).unlock();
+  }
+};
+
 class DistanceBasedCompareMapFilterComponent : public pointcloud_preprocessor::Filter
 {
 protected:
   virtual void filter(
     const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output);
 
-  void input_target_callback(const PointCloud2ConstPtr map);
-
 private:
-  rclcpp::Subscription<PointCloud2>::SharedPtr sub_map_;
-  PointCloudConstPtr map_ptr_;
   double distance_threshold_;
-  pcl::search::Search<pcl::PointXYZ>::Ptr tree_;
-
-  /** \brief Parameter service callback result : needed to be hold */
-  OnSetParametersCallbackHandle::SharedPtr set_param_res_;
-
-  /** \brief Parameter service callback */
-  rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector<rclcpp::Parameter> & p);
+  std::unique_ptr<VoxelGridMapLoader> distance_based_map_loader_;
 
 public:
   PCL_MAKE_ALIGNED_OPERATOR_NEW

perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp

@@ -16,35 +16,52 @@
 #define COMPARE_MAP_SEGMENTATION__VOXEL_BASED_APPROXIMATE_COMPARE_MAP_FILTER_NODELET_HPP_  // NOLINT
 
 #include "pointcloud_preprocessor/filter.hpp"
+#include "voxel_grid_map_loader.hpp"
 
 #include <pcl/filters/voxel_grid.h>
 #include <pcl/search/pcl_search.h>
 
+#include <memory>
+#include <string>
 #include <vector>
 
 namespace compare_map_segmentation
 {
+
+class VoxelBasedApproximateStaticMapLoader : public VoxelGridStaticMapLoader
+{
+public:
+  explicit VoxelBasedApproximateStaticMapLoader(
+    rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex)
+  : VoxelGridStaticMapLoader(node, leaf_size, tf_map_input_frame, mutex)
+  {
+    RCLCPP_INFO(logger_, "VoxelBasedApproximateStaticMapLoader initialized.\n");
+  }
+  bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) override;
+};
+
+class VoxelBasedApproximateDynamicMapLoader : public VoxelGridDynamicMapLoader
+{
+public:
+  VoxelBasedApproximateDynamicMapLoader(
+    rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex,
+    rclcpp::CallbackGroup::SharedPtr main_callback_group)
+  : VoxelGridDynamicMapLoader(node, leaf_size, tf_map_input_frame, mutex, main_callback_group)
+  {
+    RCLCPP_INFO(logger_, "VoxelBasedApproximateDynamicMapLoader initialized.\n");
+  }
+  bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) override;
+};
+
 class VoxelBasedApproximateCompareMapFilterComponent : public pointcloud_preprocessor::Filter
 {
 protected:
   virtual void filter(
     const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output);
 
-  void input_target_callback(const PointCloud2ConstPtr map);
-
 private:
-  // pcl::SegmentDifferences<pcl::PointXYZ> impl_;
-  rclcpp::Subscription<PointCloud2>::SharedPtr sub_map_;
-  PointCloudPtr voxel_map_ptr_;
   double distance_threshold_;
-  pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_;
-  bool set_map_in_voxel_grid_;
-
-  /** \brief Parameter service callback result : needed to be hold */
-  OnSetParametersCallbackHandle::SharedPtr set_param_res_;
-
-  /** \brief Parameter service callback */
-  rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector<rclcpp::Parameter> & p);
+  std::unique_ptr<VoxelGridMapLoader> voxel_based_approximate_map_loader_;
 
 public:
   PCL_MAKE_ALIGNED_OPERATOR_NEW

perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp

@@ -16,14 +16,110 @@
 #define COMPARE_MAP_SEGMENTATION__VOXEL_DISTANCE_BASED_COMPARE_MAP_FILTER_NODELET_HPP_  // NOLINT
 
 #include "pointcloud_preprocessor/filter.hpp"
+#include "voxel_grid_map_loader.hpp"
 
 #include <pcl/filters/voxel_grid.h>
 #include <pcl/search/pcl_search.h>
 
+#include <map>
+#include <memory>
+#include <string>
+#include <utility>
 #include <vector>
 
 namespace compare_map_segmentation
 {
+
+typedef typename pcl::Filter<pcl::PointXYZ>::PointCloud PointCloud;
+typedef typename PointCloud::Ptr PointCloudPtr;
+typedef typename PointCloud::ConstPtr PointCloudConstPtr;
+
+class VoxelDistanceBasedStaticMapLoader : public VoxelGridStaticMapLoader
+{
+private:
+  PointCloudConstPtr map_ptr_;
+  pcl::search::Search<pcl::PointXYZ>::Ptr tree_;
+
+public:
+  explicit VoxelDistanceBasedStaticMapLoader(
+    rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex)
+  : VoxelGridStaticMapLoader(node, leaf_size, tf_map_input_frame, mutex)
+  {
+    RCLCPP_INFO(logger_, "VoxelDistanceBasedStaticMapLoader initialized.\n");
+  }
+  bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) override;
+  void onMapCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr map) override;
+};
+
+class VoxelDistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader
+{
+protected:
+private:
+  PointCloudConstPtr map_ptr_;
+  /* data */
+public:
+  explicit VoxelDistanceBasedDynamicMapLoader(
+    rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex,
+    rclcpp::CallbackGroup::SharedPtr main_callback_group)
+  : VoxelGridDynamicMapLoader(node, leaf_size, tf_map_input_frame, mutex, main_callback_group)
+  {
+    RCLCPP_INFO(logger_, "VoxelDistanceBasedDynamicMapLoader initialized.\n");
+  }
+  bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) override;
+
+  inline void addMapCellAndFilter(
+    const autoware_map_msgs::msg::PointCloudMapCellWithID & map_cell_to_add) override
+  {
+    map_grid_size_x_ = map_cell_to_add.metadata.max_x - map_cell_to_add.metadata.min_x;
+    map_grid_size_y_ = map_cell_to_add.metadata.max_y - map_cell_to_add.metadata.min_y;
+
+    pcl::PointCloud<pcl::PointXYZ> map_cell_pc_tmp;
+    pcl::fromROSMsg(map_cell_to_add.pointcloud, map_cell_pc_tmp);
+
+    VoxelGridPointXYZ map_cell_voxel_grid_tmp;
+    PointCloudPtr map_cell_downsampled_pc_ptr_tmp;
+
+    auto map_cell_voxel_input_tmp_ptr =
+      std::make_shared<pcl::PointCloud<pcl::PointXYZ>>(map_cell_pc_tmp);
+    map_cell_voxel_grid_tmp.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, voxel_leaf_size_);
+    map_cell_downsampled_pc_ptr_tmp.reset(new pcl::PointCloud<pcl::PointXYZ>);
+    map_cell_voxel_grid_tmp.setInputCloud(map_cell_voxel_input_tmp_ptr);
+    map_cell_voxel_grid_tmp.setSaveLeafLayout(true);
+    map_cell_voxel_grid_tmp.filter(*map_cell_downsampled_pc_ptr_tmp);
+
+    MapGridVoxelInfo current_voxel_grid_list_item;
+    current_voxel_grid_list_item.min_b_x = map_cell_to_add.metadata.min_x;
+    current_voxel_grid_list_item.min_b_y = map_cell_to_add.metadata.min_y;
+    current_voxel_grid_list_item.max_b_x = map_cell_to_add.metadata.max_x;
+    current_voxel_grid_list_item.max_b_y = map_cell_to_add.metadata.max_y;
+
+    current_voxel_grid_list_item.map_cell_voxel_grid.set_voxel_grid(
+      &(map_cell_voxel_grid_tmp.leaf_layout_), map_cell_voxel_grid_tmp.get_min_b(),
+      map_cell_voxel_grid_tmp.get_max_b(), map_cell_voxel_grid_tmp.get_div_b(),
+      map_cell_voxel_grid_tmp.get_divb_mul(), map_cell_voxel_grid_tmp.get_inverse_leaf_size());
+
+    current_voxel_grid_list_item.map_cell_pc_ptr.reset(new pcl::PointCloud<pcl::PointXYZ>);
+    current_voxel_grid_list_item.map_cell_pc_ptr = std::move(map_cell_downsampled_pc_ptr_tmp);
+
+    // add kdtree
+    pcl::search::Search<pcl::PointXYZ>::Ptr tree_tmp;
+    if (!tree_tmp) {
+      if (map_cell_voxel_input_tmp_ptr->isOrganized()) {
+        tree_tmp.reset(new pcl::search::OrganizedNeighbor<pcl::PointXYZ>());
+      } else {
+        tree_tmp.reset(new pcl::search::KdTree<pcl::PointXYZ>(false));
+      }
+    }
+    tree_tmp->setInputCloud(map_cell_voxel_input_tmp_ptr);
+    current_voxel_grid_list_item.map_cell_kdtree = tree_tmp;
+
+    // add
+    (*mutex_ptr_).lock();
+    current_voxel_grid_dict_.insert({map_cell_to_add.cell_id, current_voxel_grid_list_item});
+    (*mutex_ptr_).unlock();
+  }
+};
+
 class VoxelDistanceBasedCompareMapFilterComponent : public pointcloud_preprocessor::Filter
 {
 protected:
@@ -33,20 +129,8 @@ class VoxelDistanceBasedCompareMapFilterComponent : public pointcloud_preprocess
   void input_target_callback(const PointCloud2ConstPtr map);
 
 private:
-  // pcl::SegmentDifferences<pcl::PointXYZ> impl_;
-  rclcpp::Subscription<PointCloud2>::SharedPtr sub_map_;
-  PointCloudPtr voxel_map_ptr_;
-  PointCloudConstPtr map_ptr_;
+  std::unique_ptr<VoxelGridMapLoader> voxel_distance_based_map_loader_;
   double distance_threshold_;
-  pcl::search::Search<pcl::PointXYZ>::Ptr tree_;
-  pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_;
-  bool set_map_in_voxel_grid_;
-
-  /** \brief Parameter service callback result : needed to be hold */
-  OnSetParametersCallbackHandle::SharedPtr set_param_res_;
-
-  /** \brief Parameter service callback */
-  rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector<rclcpp::Parameter> & p);
 
 public:
   PCL_MAKE_ALIGNED_OPERATOR_NEW