Merge branch 'main' into 1121-detection_area_module_debug_markers_no_…

…obstacles_new

launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml

@@ -3,6 +3,7 @@
     enable_whole_load: true
     enable_downsampled_whole_load: false
     enable_partial_load: false
+    enable_differential_load: false
 
     # only used when downsample_whole_load enabled
     leaf_size: 3.0 # downsample leaf size [m]

map/map_loader/CMakeLists.txt

@@ -10,6 +10,7 @@ ament_auto_add_library(pointcloud_map_loader_node SHARED
   src/pointcloud_map_loader/pointcloud_map_loader_node.cpp
   src/pointcloud_map_loader/pointcloud_map_loader_module.cpp
   src/pointcloud_map_loader/partial_map_loader_module.cpp
+  src/pointcloud_map_loader/differential_map_loader_module.cpp
   src/pointcloud_map_loader/utils.cpp
 )
 target_link_libraries(pointcloud_map_loader_node ${PCL_LIBRARIES})

map/map_loader/README.md

@@ -12,6 +12,7 @@ Currently, it supports the following two types:
 - Publish raw pointcloud map
 - Publish downsampled pointcloud map
 - Send partial pointcloud map loading via ROS 2 service
+- Send differential pointcloud map loading via ROS 2 service
 
 #### Publish raw pointcloud map (ROS 2 topic)
 
@@ -28,20 +29,29 @@ Here, we assume that the pointcloud maps are divided into grids.
 Given a query from a client node, the node sends a set of pointcloud maps that overlaps with the queried area.
 Please see [the description of `GetPartialPointCloudMap.srv`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_map_msgs#getpartialpointcloudmapsrv) for details.
 
+#### Send differential pointcloud map (ROS 2 service)
+
+Here, we assume that the pointcloud maps are divided into grids.
+
+Given a query and set of map IDs, the node sends a set of pointcloud maps that overlap with the queried area and are not included in the set of map IDs.
+Please see [the description of `GetDifferentialPointCloudMap.srv`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_map_msgs#getdifferentialpointcloudmapsrv) for details.
+
 ### Parameters
 
 | Name                          | Type  | Description                                                                       | Default value |
 | :---------------------------- | :---- | :-------------------------------------------------------------------------------- | :------------ |
 | enable_whole_load             | bool  | A flag to enable raw pointcloud map publishing                                    | true          |
 | enable_downsampled_whole_load | bool  | A flag to enable downsampled pointcloud map publishing                            | false         |
 | enable_partial_load           | bool  | A flag to enable partial pointcloud map server                                    | false         |
+| enable_differential_load      | bool  | A flag to enable differential pointcloud map server                               | false         |
 | leaf_size                     | float | Downsampling leaf size (only used when enable_downsampled_whole_load is set true) | 3.0           |
 
 ### Interfaces
 
 - `output/pointcloud_map` (sensor_msgs/msg/PointCloud2) : Raw pointcloud map
 - `output/debug/downsampled_pointcloud_map` (sensor_msgs/msg/PointCloud2) : Downsampled pointcloud map
 - `service/get_partial_pcd_map` (autoware_map_msgs/srv/GetPartialPointCloudMap) : Partial pointcloud map
+- `service/get_differential_pcd_map` (autoware_map_msgs/srv/GetDifferentialPointCloudMap) : Differential pointcloud map
 
 ---
 

map/map_loader/config/pointcloud_map_loader.param.yaml

@@ -3,6 +3,7 @@
     enable_whole_load: true
     enable_downsampled_whole_load: false
     enable_partial_load: false
+    enable_differential_load: false
 
     # only used when downsample_whole_load enabled
     leaf_size: 3.0 # downsample leaf size [m]

map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp

@@ -0,0 +1,85 @@
+// Copyright 2022 The Autoware Contributors
+//
+// 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 "differential_map_loader_module.hpp"
+
+DifferentialMapLoaderModule::DifferentialMapLoaderModule(
+  rclcpp::Node * node, const std::map<std::string, PCDFileMetadata> & pcd_file_metadata_dict)
+: logger_(node->get_logger()), all_pcd_file_metadata_dict_(pcd_file_metadata_dict)
+{
+  get_differential_pcd_maps_service_ = node->create_service<GetDifferentialPointCloudMap>(
+    "service/get_differential_pcd_map",
+    std::bind(
+      &DifferentialMapLoaderModule::onServiceGetDifferentialPointCloudMap, this,
+      std::placeholders::_1, std::placeholders::_2));
+}
+
+void DifferentialMapLoaderModule::differentialAreaLoad(
+  const autoware_map_msgs::msg::AreaInfo area, const std::vector<std::string> & cached_ids,
+  GetDifferentialPointCloudMap::Response::SharedPtr & response) const
+{
+  // iterate over all the available pcd map grids
+  std::vector<bool> should_remove(static_cast<int>(cached_ids.size()), true);
+  for (const auto & ele : all_pcd_file_metadata_dict_) {
+    std::string path = ele.first;
+    PCDFileMetadata metadata = ele.second;
+
+    // assume that the map ID = map path (for now)
+    std::string map_id = path;
+
+    // skip if the pcd file is not within the queried area
+    if (!isGridWithinQueriedArea(area, metadata)) continue;
+
+    auto id_in_cached_list = std::find(cached_ids.begin(), cached_ids.end(), map_id);
+    if (id_in_cached_list != cached_ids.end()) {
+      int index = id_in_cached_list - cached_ids.begin();
+      should_remove[index] = false;
+    } else {
+      autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id =
+        loadPointCloudMapCellWithID(path, map_id);
+      response->new_pointcloud_with_ids.push_back(pointcloud_map_cell_with_id);
+    }
+  }
+
+  for (int i = 0; i < static_cast<int>(cached_ids.size()); ++i) {
+    if (should_remove[i]) {
+      response->ids_to_remove.push_back(cached_ids[i]);
+    }
+  }
+}
+
+bool DifferentialMapLoaderModule::onServiceGetDifferentialPointCloudMap(
+  GetDifferentialPointCloudMap::Request::SharedPtr req,
+  GetDifferentialPointCloudMap::Response::SharedPtr res)
+{
+  auto area = req->area;
+  std::vector<std::string> cached_ids = req->cached_ids;
+  differentialAreaLoad(area, cached_ids, res);
+  res->header.frame_id = "map";
+  return true;
+}
+
+autoware_map_msgs::msg::PointCloudMapCellWithID
+DifferentialMapLoaderModule::loadPointCloudMapCellWithID(
+  const std::string path, const std::string map_id) const
+{
+  sensor_msgs::msg::PointCloud2 pcd;
+  if (pcl::io::loadPCDFile(path, pcd) == -1) {
+    RCLCPP_ERROR_STREAM(logger_, "PCD load failed: " << path);
+  }
+  autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id;
+  pointcloud_map_cell_with_id.pointcloud = pcd;
+  pointcloud_map_cell_with_id.cell_id = map_id;
+  return pointcloud_map_cell_with_id;
+}

map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp

@@ -0,0 +1,59 @@
+// Copyright 2022 The Autoware Contributors
+//
+// 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 POINTCLOUD_MAP_LOADER__DIFFERENTIAL_MAP_LOADER_MODULE_HPP_
+#define POINTCLOUD_MAP_LOADER__DIFFERENTIAL_MAP_LOADER_MODULE_HPP_
+
+#include "utils.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include "autoware_map_msgs/srv/get_differential_point_cloud_map.hpp"
+
+#include <pcl/common/common.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+#include <map>
+#include <string>
+#include <vector>
+
+class DifferentialMapLoaderModule
+{
+  using GetDifferentialPointCloudMap = autoware_map_msgs::srv::GetDifferentialPointCloudMap;
+
+public:
+  explicit DifferentialMapLoaderModule(
+    rclcpp::Node * node, const std::map<std::string, PCDFileMetadata> & pcd_file_metadata_dict);
+
+private:
+  rclcpp::Logger logger_;
+
+  std::map<std::string, PCDFileMetadata> all_pcd_file_metadata_dict_;
+  rclcpp::Service<GetDifferentialPointCloudMap>::SharedPtr get_differential_pcd_maps_service_;
+
+  bool onServiceGetDifferentialPointCloudMap(
+    GetDifferentialPointCloudMap::Request::SharedPtr req,
+    GetDifferentialPointCloudMap::Response::SharedPtr res);
+  void differentialAreaLoad(
+    const autoware_map_msgs::msg::AreaInfo area_info, const std::vector<std::string> & cached_ids,
+    GetDifferentialPointCloudMap::Response::SharedPtr & response) const;
+  autoware_map_msgs::msg::PointCloudMapCellWithID loadPointCloudMapCellWithID(
+    const std::string path, const std::string map_id) const;
+};
+
+#endif  // POINTCLOUD_MAP_LOADER__DIFFERENTIAL_MAP_LOADER_MODULE_HPP_

map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp

@@ -52,6 +52,7 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt
   bool enable_whole_load = declare_parameter<bool>("enable_whole_load");
   bool enable_downsample_whole_load = declare_parameter<bool>("enable_downsampled_whole_load");
   bool enable_partial_load = declare_parameter<bool>("enable_partial_load");
+  bool enable_differential_load = declare_parameter<bool>("enable_differential_load");
 
   if (enable_whole_load) {
     std::string publisher_name = "output/pointcloud_map";
@@ -65,10 +66,18 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt
       std::make_unique<PointcloudMapLoaderModule>(this, pcd_paths, publisher_name, true);
   }
 
-  if (enable_partial_load) {
+  if (enable_partial_load | enable_differential_load) {
     pcd_metadata_dict_ = generatePCDMetadata(pcd_paths);
+  }
+
+  if (enable_partial_load) {
     partial_map_loader_ = std::make_unique<PartialMapLoaderModule>(this, pcd_metadata_dict_);
   }
+
+  if (enable_differential_load) {
+    differential_map_loader_ =
+      std::make_unique<DifferentialMapLoaderModule>(this, pcd_metadata_dict_);
+  }
 }
 
 std::vector<std::string> PointCloudMapLoaderNode::getPcdPaths(

map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp

@@ -15,6 +15,7 @@
 #ifndef POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_
 #define POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_
 
+#include "differential_map_loader_module.hpp"
 #include "partial_map_loader_module.hpp"
 #include "pointcloud_map_loader_module.hpp"
 
@@ -43,6 +44,7 @@ class PointCloudMapLoaderNode : public rclcpp::Node
   std::unique_ptr<PointcloudMapLoaderModule> pcd_map_loader_;
   std::unique_ptr<PointcloudMapLoaderModule> downsampled_pcd_map_loader_;
   std::unique_ptr<PartialMapLoaderModule> partial_map_loader_;
+  std::unique_ptr<DifferentialMapLoaderModule> differential_map_loader_;
 
   std::vector<std::string> getPcdPaths(
     const std::vector<std::string> & pcd_paths_or_directory) const;

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -172,6 +172,13 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   bool skipSmoothGoalConnection(
     const std::vector<std::shared_ptr<SceneModuleStatus>> & statuses) const;
 
+  /**
+   * @brief skip smooth goal connection
+   */
+  void computeTurnSignal(
+    const std::shared_ptr<PlannerData> planner_data, const PathWithLaneId & path,
+    const BehaviorModuleOutput & output);
+
   // debug
   rclcpp::Publisher<MarkerArray>::SharedPtr debug_drivable_area_lanelets_publisher_;
   rclcpp::Publisher<AvoidanceDebugMsgArray>::SharedPtr debug_avoidance_msg_array_publisher_;

planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp

@@ -288,7 +288,7 @@ std::vector<DrivableLanes> generateDrivableLanes(const lanelet::ConstLanelets &
 std::vector<DrivableLanes> generateDrivableLanesWithShoulderLanes(
   const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & shoulder_lanes);
 
-size_t getOverlappedLaneletId(const std::vector<DrivableLanes> & lanes);
+boost::optional<size_t> getOverlappedLaneletId(const std::vector<DrivableLanes> & lanes);
 std::vector<DrivableLanes> cutOverlappedLanes(
   PathWithLaneId & path, const std::vector<DrivableLanes> & lanes);
 

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -627,6 +627,11 @@ void BehaviorPathPlannerNode::run()
 
   // update planner data
   planner_data_->prev_output_path = path;
+
+  // compute turn signal
+  computeTurnSignal(planner_data, *path, output);
+
+  // unlock planner data
   mutex_pd_.unlock();
 
   // publish drivable bounds
@@ -645,26 +650,6 @@ void BehaviorPathPlannerNode::run()
   const auto path_candidate = getPathCandidate(output, planner_data);
   path_candidate_publisher_->publish(util::toPath(*path_candidate));
 
-  // for turn signal
-  {
-    TurnIndicatorsCommand turn_signal;
-    HazardLightsCommand hazard_signal;
-    if (output.turn_signal_info.hazard_signal.command == HazardLightsCommand::ENABLE) {
-      turn_signal.command = TurnIndicatorsCommand::DISABLE;
-      hazard_signal.command = output.turn_signal_info.hazard_signal.command;
-    } else {
-      turn_signal =
-        turn_signal_decider_.getTurnSignal(planner_data, *path, output.turn_signal_info);
-      hazard_signal.command = HazardLightsCommand::DISABLE;
-    }
-    turn_signal.stamp = get_clock()->now();
-    hazard_signal.stamp = get_clock()->now();
-    turn_signal_publisher_->publish(turn_signal);
-    hazard_signal_publisher_->publish(hazard_signal);
-
-    publish_steering_factor(turn_signal);
-  }
-
   publishSceneModuleDebugMsg();
 
   if (planner_data->parameters.visualize_drivable_area_for_shared_linestrings_lanelet) {
@@ -677,6 +662,27 @@ void BehaviorPathPlannerNode::run()
   RCLCPP_DEBUG(get_logger(), "----- behavior path planner end -----\n\n");
 }
 
+void BehaviorPathPlannerNode::computeTurnSignal(
+  const std::shared_ptr<PlannerData> planner_data, const PathWithLaneId & path,
+  const BehaviorModuleOutput & output)
+{
+  TurnIndicatorsCommand turn_signal;
+  HazardLightsCommand hazard_signal;
+  if (output.turn_signal_info.hazard_signal.command == HazardLightsCommand::ENABLE) {
+    turn_signal.command = TurnIndicatorsCommand::DISABLE;
+    hazard_signal.command = output.turn_signal_info.hazard_signal.command;
+  } else {
+    turn_signal = turn_signal_decider_.getTurnSignal(planner_data, path, output.turn_signal_info);
+    hazard_signal.command = HazardLightsCommand::DISABLE;
+  }
+  turn_signal.stamp = get_clock()->now();
+  hazard_signal.stamp = get_clock()->now();
+  turn_signal_publisher_->publish(turn_signal);
+  hazard_signal_publisher_->publish(hazard_signal);
+
+  publish_steering_factor(turn_signal);
+}
+
 void BehaviorPathPlannerNode::publish_steering_factor(const TurnIndicatorsCommand & turn_signal)
 {
   const auto [intersection_flag, approaching_intersection_flag] =