[pull] main from autowarefoundation:main

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;

system/default_ad_api/CMakeLists.txt

@@ -34,4 +34,4 @@ install(
   DESTINATION lib/${PROJECT_NAME}
 )
 
-ament_auto_package(INSTALL_TO_SHARE launch test)
+ament_auto_package(INSTALL_TO_SHARE config launch test)

system/default_ad_api/README.md

@@ -0,0 +1,11 @@
+# default_ad_api
+
+This package is a default implementation AD API.
+
+- [autoware state (backward compatibility)](document/autoware-state.md)
+- [fail-safe](document/fail-safe.md)
+- [interface](document/interface.md)
+- [localization](document/localization.md)
+- [motion](document/motion.md)
+- [operation mode](document/operation-mode.md)
+- [routing](document/routing.md)

system/default_ad_api/config/default_ad_api.param.yaml

@@ -0,0 +1,8 @@
+/default_ad_api/node/autoware_state:
+  ros__parameters:
+    update_rate: 10.0
+
+/default_ad_api/node/motion:
+  ros__parameters:
+    require_accept_start: false
+    stop_check_duration: 1.0

system/default_ad_api/document/autoware-state.md

@@ -0,0 +1,16 @@
+# Autoware state compatibility
+
+## Overview
+
+Since `/autoware/state` was so widely used, default_ad_api creates it from the states of AD API for backwards compatibility.
+The diagnostic checks that ad_service_state_monitor used to perform have been replaced by component_state_monitor.
+The service `/autoware/shutdown` to change autoware state to finalizing is also supported for compatibility.
+
+![autoware-state-architecture](images/autoware-state-architecture.drawio.svg)
+
+## Conversion
+
+This is the correspondence between AD API states and autoware states.
+The launch state is the data that default_ad_api node holds internally.
+
+![autoware-state-table](images/autoware-state-table.drawio.svg)

system/default_ad_api/document/fail-safe.md

@@ -0,0 +1,5 @@
+# Fail-safe API
+
+## Overview
+
+The fail-safe API simply relays the MRM state. See the [autoware-documentation](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/fail_safe/) for AD API specifications.