From 49aa10b04de61c36706f6151d11bf17257ca54d1 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Tue, 13 Dec 2022 06:46:20 +0900 Subject: [PATCH 1/3] feat(default_ad_api): split parameters into file (#2488) * feat(default_ad_api): split parameters into file Signed-off-by: Takagi, Isamu * feat: remove old parameter Signed-off-by: Takagi, Isamu * fix: test Signed-off-by: Takagi, Isamu * feat: add default config Signed-off-by: Takagi, Isamu Signed-off-by: Takagi, Isamu --- system/default_ad_api/CMakeLists.txt | 2 +- .../config/default_ad_api.param.yaml | 8 ++++++++ .../launch/default_ad_api.launch.py | 18 +++++++++++++++--- .../src/compatibility/autoware_state.cpp | 3 +-- system/default_ad_api/src/motion.cpp | 4 ++-- 5 files changed, 27 insertions(+), 8 deletions(-) create mode 100644 system/default_ad_api/config/default_ad_api.param.yaml diff --git a/system/default_ad_api/CMakeLists.txt b/system/default_ad_api/CMakeLists.txt index 12b6eef559fe4..e0656a15b82f3 100644 --- a/system/default_ad_api/CMakeLists.txt +++ b/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) diff --git a/system/default_ad_api/config/default_ad_api.param.yaml b/system/default_ad_api/config/default_ad_api.param.yaml new file mode 100644 index 0000000000000..a15abe091764c --- /dev/null +++ b/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 diff --git a/system/default_ad_api/launch/default_ad_api.launch.py b/system/default_ad_api/launch/default_ad_api.launch.py index eab13c67ef42c..d3182a9070c2a 100644 --- a/system/default_ad_api/launch/default_ad_api.launch.py +++ b/system/default_ad_api/launch/default_ad_api.launch.py @@ -13,9 +13,14 @@ # limitations under the License. import launch +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.substitutions import PathJoinSubstitution from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import Node from launch_ros.descriptions import ComposableNode +from launch_ros.parameter_descriptions import ParameterFile +from launch_ros.substitutions import FindPackageShare def create_api_node(node_name, class_name, **kwargs): @@ -24,17 +29,23 @@ def create_api_node(node_name, class_name, **kwargs): name=node_name, package="default_ad_api", plugin="default_ad_api::" + class_name, - **kwargs, + parameters=[ParameterFile(LaunchConfiguration("config"))], ) +def get_default_config(): + path = FindPackageShare("default_ad_api") + path = PathJoinSubstitution([path, "config/default_ad_api.param.yaml"]) + return path + + def generate_launch_description(): components = [ create_api_node("autoware_state", "AutowareStateNode"), create_api_node("fail_safe", "FailSafeNode"), create_api_node("interface", "InterfaceNode"), create_api_node("localization", "LocalizationNode"), - create_api_node("motion", "MotionNode", parameters=[{"require_accept_start": False}]), + create_api_node("motion", "MotionNode"), create_api_node("operation_mode", "OperationModeNode"), create_api_node("routing", "RoutingNode"), ] @@ -51,4 +62,5 @@ def generate_launch_description(): name="web_server", executable="web_server.py", ) - return launch.LaunchDescription([container, web_server]) + argument = DeclareLaunchArgument("config", default_value=get_default_config()) + return launch.LaunchDescription([argument, container, web_server]) diff --git a/system/default_ad_api/src/compatibility/autoware_state.cpp b/system/default_ad_api/src/compatibility/autoware_state.cpp index 5778edf66559f..1aa49383aa48f 100644 --- a/system/default_ad_api/src/compatibility/autoware_state.cpp +++ b/system/default_ad_api/src/compatibility/autoware_state.cpp @@ -46,8 +46,7 @@ AutowareStateNode::AutowareStateNode(const rclcpp::NodeOptions & options) adaptor.init_sub(sub_routing_, this, &AutowareStateNode::on_routing); adaptor.init_sub(sub_operation_mode_, this, &AutowareStateNode::on_operation_mode); - // TODO(Takagi, Isamu): remove default value - const auto rate = rclcpp::Rate(declare_parameter("update_rate", 10.0)); + const auto rate = rclcpp::Rate(declare_parameter("update_rate")); timer_ = rclcpp::create_timer(this, get_clock(), rate.period(), [this]() { on_timer(); }); component_states_.resize(module_names.size()); diff --git a/system/default_ad_api/src/motion.cpp b/system/default_ad_api/src/motion.cpp index d99f72f437685..b02fd43e9be44 100644 --- a/system/default_ad_api/src/motion.cpp +++ b/system/default_ad_api/src/motion.cpp @@ -23,8 +23,8 @@ namespace default_ad_api MotionNode::MotionNode(const rclcpp::NodeOptions & options) : Node("motion", options), vehicle_stop_checker_(this) { - stop_check_duration_ = declare_parameter("stop_check_duration", 1.0); - require_accept_start_ = declare_parameter("require_accept_start", false); + stop_check_duration_ = declare_parameter("stop_check_duration"); + require_accept_start_ = declare_parameter("require_accept_start"); is_calling_set_pause_ = false; const auto adaptor = component_interface_utils::NodeAdaptor(this); From 9a3613bfcd3e36e522d0ea9130f6200ca7689e2b Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Tue, 13 Dec 2022 08:49:23 +0900 Subject: [PATCH 2/3] docs(default_ad_api): add readme (#2491) * docs(default_ad_api): add readme Signed-off-by: Takagi, Isamu * feat: update table Signed-off-by: Takagi, Isamu Signed-off-by: Takagi, Isamu --- system/default_ad_api/README.md | 11 + .../default_ad_api/document/autoware-state.md | 16 + system/default_ad_api/document/fail-safe.md | 5 + .../autoware-state-architecture.drawio.svg | 332 +++++++++++ .../images/autoware-state-table.drawio.svg | 457 +++++++++++++++ .../document/images/localization.drawio.svg | 553 ++++++++++++++++++ .../images/motion-architecture.drawio.svg | 255 ++++++++ .../motion-state.drawio.svg} | 75 ++- .../operation-mode-architecture.drawio.svg | 323 ++++++++++ .../images/operation-mode-state.drawio.svg | 418 +++++++++++++ .../images/operation-mode-table.drawio.svg | 317 ++++++++++ .../document/images/routing.drawio.svg | 398 +++++++++++++ system/default_ad_api/document/interface.md | 5 + .../default_ad_api/document/localization.md | 7 + system/default_ad_api/document/motion.md | 13 + .../default_ad_api/document/operation-mode.md | 24 + system/default_ad_api/document/routing.md | 7 + 17 files changed, 3213 insertions(+), 3 deletions(-) create mode 100644 system/default_ad_api/README.md create mode 100644 system/default_ad_api/document/autoware-state.md create mode 100644 system/default_ad_api/document/fail-safe.md create mode 100644 system/default_ad_api/document/images/autoware-state-architecture.drawio.svg create mode 100644 system/default_ad_api/document/images/autoware-state-table.drawio.svg create mode 100644 system/default_ad_api/document/images/localization.drawio.svg create mode 100644 system/default_ad_api/document/images/motion-architecture.drawio.svg rename system/default_ad_api/document/{motion.drawio.svg => images/motion-state.drawio.svg} (74%) create mode 100644 system/default_ad_api/document/images/operation-mode-architecture.drawio.svg create mode 100644 system/default_ad_api/document/images/operation-mode-state.drawio.svg create mode 100644 system/default_ad_api/document/images/operation-mode-table.drawio.svg create mode 100644 system/default_ad_api/document/images/routing.drawio.svg create mode 100644 system/default_ad_api/document/interface.md create mode 100644 system/default_ad_api/document/localization.md create mode 100644 system/default_ad_api/document/motion.md create mode 100644 system/default_ad_api/document/operation-mode.md create mode 100644 system/default_ad_api/document/routing.md diff --git a/system/default_ad_api/README.md b/system/default_ad_api/README.md new file mode 100644 index 0000000000000..0b6cdf30778a2 --- /dev/null +++ b/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) diff --git a/system/default_ad_api/document/autoware-state.md b/system/default_ad_api/document/autoware-state.md new file mode 100644 index 0000000000000..e26756de1f4ba --- /dev/null +++ b/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) diff --git a/system/default_ad_api/document/fail-safe.md b/system/default_ad_api/document/fail-safe.md new file mode 100644 index 0000000000000..b9967089c5e3a --- /dev/null +++ b/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. diff --git a/system/default_ad_api/document/images/autoware-state-architecture.drawio.svg b/system/default_ad_api/document/images/autoware-state-architecture.drawio.svg new file mode 100644 index 0000000000000..40aebd20ae43f --- /dev/null +++ b/system/default_ad_api/document/images/autoware-state-architecture.drawio.svg @@ -0,0 +1,332 @@ + + + + + + + + + + + +
+
+
+ component state monitor +
+
+
+ + component state monitor + + + + + + + + +
+
+
+ localization +
+ initialization state +
+
+
+ + localization... + + + + + + + + +
+
+
+ routing state +
+
+
+ + routing state + + + + + + + + +
+
+
+ operation mode +
+
+
+ + operation mode + + + + + + + + +
+
+
+ topic check status +
+ for startup +
+
+
+ + topic check status... + + + + + + + + +
+
+
+ topic check status +
+ for autonomous +
+
+
+ + topic check status... + + + + + + + + + + + + +
+
+
+ default_ad_api +
+ (localization, routing, operation mode) +
+
+
+ + default_ad_api... + + + + + + + + +
+
+
+ diagnostics +
+
+
+ + diagnostics + + + + + + + + +
+
+
+ topic state monitor +
+
+
+ + topic state monitor + + + + + + + + +
+
+
+ default_ad_api +
+ (autoware state) +
+
+
+ + default_ad_api... + + + + + + +
+
+
+ autoware +
+ state +
+
+
+ + autoware... + + + + + + + + +
+
+
+ autoware +
+ shutdown +
+
+
+ + autoware... + + + + + + + + +
+
+
+ topics that ad_service_state_monitor was checking before +
+
+
+ + topics that ad_service_state_monitor was checking before + + + + + + + +
+
+
+ node +
+
+
+ + node + + + + + + +
+
+
+ service +
+
+
+ + service + + + + + + +
+
+
+ topic +
+
+
+ + topic + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/system/default_ad_api/document/images/autoware-state-table.drawio.svg b/system/default_ad_api/document/images/autoware-state-table.drawio.svg new file mode 100644 index 0000000000000..ab21c1b865407 --- /dev/null +++ b/system/default_ad_api/document/images/autoware-state-table.drawio.svg @@ -0,0 +1,457 @@ + + + + + + + + +
+
+
+ WaitingForRoute +
+
+
+ + WaitingForRoute + + + + + + +
+
+
+ localization state +
+
+
+ + localization state + + + + + + +
+
+
+ routing state +
+
+
+ + routing state + + + + + + +
+
+
+ operation mode +
+
+
+ + operation mode + + + + + + +
+
+
+ auto mode available +
+
+
+ + auto mode available + + + + + + +
+
+
+ initializing +
+
+
+ + initializing + + + + + + +
+
+
+ running +
+
+
+ + running + + + + + + +
+
+
+ uninitialized +
+
+
+ + uninitialized + + + + + + +
+
+
+ initializing +
+
+
+ + initializing + + + + + + +
+
+
+ initialized +
+
+
+ + initialized + + + + + + +
+
+
+ unset, arrived +
+
+
+ + unset, arrived + + + + + + +
+
+
+ set +
+
+
+ + set + + + + + + +
+
+
+ arrived +
+
+
+ + arrived + + + + + + +
+
+
+ finalizing +
+
+
+ + finalizing + + + + + + +
+
+
+ stop +
+
+
+ + stop + + + + + + +
+
+
+ not stop +
+
+
+ + not stop + + + + + + +
+
+
+ false +
+
+
+ + false + + + + + + +
+
+
+ true +
+
+
+ + true + + + + + + +
+
+
+ Initializing +
+
+
+ + Initializing + + + + + + +
+
+
+ launch state +
+
+
+ + launch state + + + + + + +
+
+
+ Planning +
+
+
+ + Planning + + + + + + +
+
+
+ WaitingForEngage +
+
+
+ + WaitingForEngage + + + + + + +
+
+
+ Driving +
+
+
+ + Driving + + + + + + +
+
+
+ ArrivedGoal +
+
+
+ + ArrivedGoal + + + + + + +
+
+
+ Finalizing +
+
+
+ + Finalizing + + + + + + + + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/system/default_ad_api/document/images/localization.drawio.svg b/system/default_ad_api/document/images/localization.drawio.svg new file mode 100644 index 0000000000000..3c1347e8b4a3c --- /dev/null +++ b/system/default_ad_api/document/images/localization.drawio.svg @@ -0,0 +1,553 @@ + + + + + + + + + + + + + +
+
+
+ Applications (FMS, etc.) +
+
+
+ + Applications (FMS, etc.) + + + + + + + + +
+
+
+ /api/localization/initialize +
+
+
+ + /api/localization/initialize + + + + + + + + +
+
+
+ AD API +
+ (default implementation) +
+
+
+ + AD API... + + + + + + + + + + +
+
+
+ /localization/initialize +
+
+
+ + /localization/initialize + + + + + + +
+
+
+ component_state_monitor +
+
+
+ + component_state_monitor + + + + + + + + +
+
+
+ initialpose3d +
+
+
+ + initialpose3d + + + + + + + + + + + +
+
+
+ simple_planning_simulator +
+
+
+ + simple_planning_simulator + + + + + + +
+
+
+ NDT +
+
+
+ + NDT + + + + + + + + + +
+
+
+ ndt_align_srv +
+
+
+ + ndt_align_srv + + + + + + +
+
+
+ common +
+
+
+ + common + + + + + + +
+
+
+ psim +
+
+
+ + psim + + + + + + +
+
+
+ lsim/real +
+
+
+ + lsim/real + + + + + + +
+
+
+ node +
+
+
+ + node + + + + + + +
+
+
+ service +
+
+
+ + service + + + + + + +
+
+
+ topic +
+
+
+ + topic + + + + + + + + + + + + + +
+
+
+ + initial_pose_adaptor +
+ (Fix z position to fit map) + +
+
+
+ + initial_pose_adaptor... + + + + + + + + +
+
+
+ EKF +
+
+
+ + EKF + + + + + + + + +
+
+
+ gnss pose +
+
+
+ + gnss pose + + + + + + +
+
+
+ Module when using GNSS +
+ (Fix z position to fit map) +
+
+
+ + Module when using GNSS... + + + + + + + + +
+
+
+ automatic_pose_initializer +
+ (Call the API when state is uninitialized) +
+
+
+ + automatic_pose_initializer... + + + + + + + + +
+
+
+ initialpose (from rviz) +
+
+
+ + initialpose (from rviz) + + + + + + + + + + + + +
+
+
+ fit_map_height +
+
+
+ + fit_map_height + + + + + + +
+
+
+ map_height_fitter +
+
+
+ + map_height_fitter + + + + + + +
+
+
+ Module when using NDT +
+
+
+ + Module when using NDT + + + + + + + + +
+
+
+ /localization/initialization_state +
+
+
+ + /localization/initialization_state + + + + + + + + +
+
+
+ sensing twist +
+
+
+ + sensing twist + + + + + + +
+
+
+ pose_initializer +
+
+
+ + pose_initializer + + + + + + +
+
+
+ Module to check vehicle stops +
+
+
+ + Module to check vehicle stops + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/system/default_ad_api/document/images/motion-architecture.drawio.svg b/system/default_ad_api/document/images/motion-architecture.drawio.svg new file mode 100644 index 0000000000000..24271b1c65643 --- /dev/null +++ b/system/default_ad_api/document/images/motion-architecture.drawio.svg @@ -0,0 +1,255 @@ + + + + + + + + + + + +
+
+
+ AD API +
+
+
+ + AD API + + + + + + + + +
+
+
+ Applications +
+
+
+ + Applications + + + + + + + + + + +
+
+
+ vehicle_cmd_gate +
+
+
+ + vehicle_cmd_gate + + + + + + + + +
+
+
+ /localization/kinematic_state +
+
+
+ + /localization/kinematic_state + + + + + + + + +
+
+
+ is_paused +
+
+
+ + is_paused + + + + + + + + +
+
+
+ is_start_requested +
+
+
+ + is_start_requested + + + + + + + + +
+
+
+ set_pause +
+
+
+ + set_pause + + + + + + + + +
+
+
+ localization +
+
+
+ + localization + + + + + + + + +
+
+
+ accept_start +
+
+
+ + accept_start + + + + + + + + +
+
+
+ motion_state +
+
+
+ + motion_state + + + + + + + +
+
+
+ node +
+
+
+ + node + + + + + + +
+
+
+ service +
+
+
+ + service + + + + + + +
+
+
+ topic +
+
+
+ + topic + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/system/default_ad_api/document/motion.drawio.svg b/system/default_ad_api/document/images/motion-state.drawio.svg similarity index 74% rename from system/default_ad_api/document/motion.drawio.svg rename to system/default_ad_api/document/images/motion-state.drawio.svg index c6e2593ed0eb7..3387b8ed29bb7 100644 --- a/system/default_ad_api/document/motion.drawio.svg +++ b/system/default_ad_api/document/images/motion-state.drawio.svg @@ -3,13 +3,31 @@ xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" - width="342px" + width="521px" height="282px" - viewBox="-0.5 -0.5 342 282" - content="<mxfile><diagram id="Cz8Q8T6vesO4MC4sh1-d" name="Page-1">5VpNc5swEP01vmZAAhkf2zRpL5nJ1IeeZZCBqUCMkL/66ytAfIo4tgE7iXPIoGUl0Nu3T9LiGXyM9j85ToIX5hE6A4a3n8EfMwDmAMr/meFQGCwTFAafh15hMmvDMvxHlNFQ1k3okbTlKBijIkzaRpfFMXFFy4Y5Z7u225rR9lMT7BPNsHQx1a1/Qk8EymqiRX3jFwn9QD3aAfPiRoRLZzWTNMAe2zVM8GkGHzljoriK9o+EZtiVuBT9nt+4W70YJ7E4pQNUPVJxKCdHPDlX1WRcBMxnMaZPtfU7Z5vYI9kIhmwFIqLy0pSXqcBcfMsAlgaX4jQN3dL8HNLSrXhk9pw337p8LbbhrvJSFJFj+UR5wQoxyTTCIiL4QbpwQrEIt+3RsYq5X/nVsMgLhcwbKNmfGiVrIEx5V/nC+NBwSFgYi7Qx8mtmkA4qyYGhKK5yfNEh4lnu8qJ4fh2uaiInRVDBssV0o+b7ijepjE43rnXUshjsglCQZYJzdHdSy9qRXMtoPTLKeN4Xrh2XuEUoOftLGndWjm3ZRhXULeGC7I+HVY9X2QG1kTJLLdnVUuQoU9AQIcsYngjAOZoIMYvJEa6r202iGxcTHepEH8rzk+VAI9Nvkm6iMPZHpZNnE8ez+ujkgBVEaBw6QXBDOi0u0NUz2dWW4cu4ZulcQ7fQVNhJfYCOi+o7/oNVFcLJ1sVRtaInfvaVtMLq14qRV57bSEW1Zl9DKtCHWXlsnU3gSmyyNTZl25ixF57b7GOuySZoaIB9yIUHjS9co2zmwfy83XzHf/DCg7Q8eGHbz7n/0tLA0dOg4vzYeTDXcNTzIvY6RPdwGlSQNuA7leI6FI2p2j0zLW3ncVwjZRdoe/GA7EX9N28PWOShGuMI160u11EnMEX2agNdQHtHC1cqWCItecMQXJq78ZPMFF3xatJZBbXJfWXCNPRj2aRknY2QsTx0Mf2mzFHoebku9mVVn/gNSpNu8KzTFgswQpIsjqO+xjS9G9jR9WAvi713KU7mXIoTGkeQTGcyQTJNLUZJVle7R0kyr5kb4B3c70mUzJ4d01TAw+PH0A9T8YA9JY+hX0tGKVmZxnklq47/8JKVXpBZZoiPfoZer0H/GdpDK2RPdHjo2RVNdnaA+r4Iuy5JxASy40o8CL+58MD+j1RNuNFEwmPpHx10Jfqqu6HF4sGeDz+pVatCR1sm2BhZusy4kZfzj1aLtF6E+qIL9ImVvVHyRC+R5sAjHGVTjVdpksOTR6G0+iKf990E5IobJuuuSkxdhdFqQbeXKtmsf15UuNe/0YJP/wE=</diagram></mxfile>" + viewBox="-0.5 -0.5 521 282" + content="<mxfile><diagram id="Cz8Q8T6vesO4MC4sh1-d" name="Page-1">5Vpdc6IwFP01vnaAQMRHa223D906a2f3OUIEZoE4IWq7v36DhM+gxRq0rT44cEkgOffck5sLAzCJXh8oWvlPxMXhwNDc1wG4GxjGSDf4f2p4ywwW1DKDRwM3M+mlYR78w8KYN1sHLk5qDRkhIQtWdaND4hg7rGZDlJJtvdmShPWnrpCHJcPcQaFs/RO4zBdWHY7KCz9w4Pni0bYxzC5EKG8sZpL4yCXbiglMB2BCCWHZUfQ6wWGKXY5L1u9+z9ViYBTHrEsHU4x4g8K1mNycIYbTJjH/G9+lf7NHMVr2lkNAyTp2cXoXbQBut37A8HyFnPTqlvuc23wWhfxM54dLEjPhRe56cLvBlAUcznEYeDE3MpJ2ECPh1/Dr3unoBUicXJhEmNE33kR0ME2BqyCWLU63FS/l0PsVBxmaMCLBDK+4dQkePxD4tWMJdAkl7HLeiFNCmU88EqNwWlpv6zhWMEsYomyckpUbnBAlSeDk5vsgzJvtQYiPgqypI4Yhwo139bBoBTJTOsCDwFIcIhZs6sQ/CSXrK6FkqoZp15UPGL1VGqxIELOkcudZaihprdt1Wo8aQX1Uc36QPb90VzGRTh40JMmYoXXCvbNfI/QOGsG9NSEhobu+YGk72MlcSclfXLmysC3T0g45VZKQvXqhN/SiEIeKYNgtemEqkAvDPhgIMYnxAa6Ly1WiH8SkSnQgE105zzvLgUSmXzhZR0HsKaWTa2HbNdvoZBsLAKEaOgHtgnQafUBXj2RXXYY7cc2UuQY/haaCRugb8LCovtP+ZFUFoLd18RStaPGfdSmtMNu1QvHKcxmpKDLQc0gFvNTKY8lsMi7FJktiU5rGqF54LpPHnJNNQJMA+wwLDzyDcClJ5o3hcdl8o/3JCw+U4uCJbL5m/iWFQcv+v+C86jgYSjjKcRG7DaK7KPELSCvw7QXjXfJWpmq1zDS3HcdxiZRNoK3RDbRG5W9Yv2EWmOIeh5KsJtdhwzFZOEs3+gDtbcldSVqG4hl7eqIxys1N/3EasqZ4VeksnFrlvjAhUekK8ZK1FMCiwHV3utgWVR3Fr3uYNJ1ndlssDAVBIpcba6gvUZhcDezwfLDnhfOrFCd9yMUJqhGkYjFWL0i6LvloldbVrlGS9HPGhvEO7tckSnpLxtQX8ODwNvRSFQ/QUvJQ/rZESclK144rWTXan16ykgsy8xRx5Xvo5dJo30O7cAGtnjYPLVlRb3sHIOdFyHHwivUgOw6HCNOLC4/R/pKqCjfsSXhM+aWDrETfNRsajW6s4ek7tcI5DW3pITEyZZlxIndHtrBYpOUi1DddoDtW9pTEiVwi3QEPUZRONV4kqx08Oy/kVo/t5n01DjljwmReVYmpqTBSLegzSpVcVZq/jH+9PP58kDx1lozotK+prAZww5bPqbSeUqL82dW69PNv1UB2rksrBbJ4Z3IWIOW6wvzleTab3qmlZNcXXWopqfeHJD8tP8TMxKD8mhVM/wM=</diagram></mxfile>" > + + + + +
+
+
+ State in AD API +
+
+
+ + State in AD API + + @@ -265,6 +283,57 @@ + + + + +
+
+
+ STARTING +
+
+
+ + STARTING + + + + + + +
+
+
+ MOVING +
+
+
+ + MOVING + + + + + + +
+
+
+ STOPPED +
+
+
+ + STOPPED + + diff --git a/system/default_ad_api/document/images/operation-mode-architecture.drawio.svg b/system/default_ad_api/document/images/operation-mode-architecture.drawio.svg new file mode 100644 index 0000000000000..4a879e98ed09d --- /dev/null +++ b/system/default_ad_api/document/images/operation-mode-architecture.drawio.svg @@ -0,0 +1,323 @@ + + + + + + + + + +
+
+
+ external_cmd_selector +
+
+
+ + external_cmd_selector + + + + + + + + +
+
+
+ vehicle_cmd_gate +
+
+
+ + vehicle_cmd_gate + + + + + + + + + + + + +
+
+
+ vehicle_interface +
+
+
+ + vehicle_interface + + + + + + + + + + + + + + +
+
+
+ + operation mode +
+ transition manager + +
+
+
+ + operation mode... + + + + + + +
+
+
+ engage, gate_mode +
+
+
+ + engage, gate_mode + + + + + + +
+
+
+ control_mode +
+
+
+ + control_mode + + + + + + +
+
+
+ selector_mode +
+
+
+ + selector_mode + + + + + + +
+
+
+ AD API +
+
+
+ + AD API + + + + + + +
+
+
+ control command +
+ (joystick etc.) +
+
+
+ + control command... + + + + + + +
+
+
+ control command +
+ (remote) +
+
+
+ + control command... + + + + + + +
+
+
+ control command +
+ (autonomous) +
+
+
+ + control command... + + + + + + + + +
+
+
+ + component_state_monitor +
+ +
+
+
+ + component_state_monitor + + + + + + + + +
+
+
+ diagnostics +
+
+
+ + diagnostics + + + + + + + +
+
+
+ node +
+
+
+ + node + + + + + + +
+
+
+ service +
+
+
+ + service + + + + + + +
+
+
+ topic +
+
+
+ + topic + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/system/default_ad_api/document/images/operation-mode-state.drawio.svg b/system/default_ad_api/document/images/operation-mode-state.drawio.svg new file mode 100644 index 0000000000000..3a008fd8a64f3 --- /dev/null +++ b/system/default_ad_api/document/images/operation-mode-state.drawio.svg @@ -0,0 +1,418 @@ + + + + + + + + + + + + + +
+
+
+ stop +
+ (autoware control is false) +
+
+
+ + stop... + + + + + + + + + + +
+
+
+ auto +
+ (autoware control is false) +
+
+
+ + auto... + + + + + + + + + + +
+
+
+ auto +
+ (autoware control is true) +
+
+
+ + auto... + + + + + + + + + + +
+
+
+ stop +
+ (autoware control is true) +
+
+
+ + stop... + + + + + + + + + + + + +
+
+
+ auto +
+ (in transition) +
+
+
+ + auto... + + + + + + + + + + + + +
+
+
+ stop +
+ (in transition) +
+
+
+ + stop... + + + + + + + + +
+
+
+ disable autoware control +
+
+
+ + disable autoware control + + + + + + + + +
+
+
+ enable autoware control +
+
+
+ + enable autoware control + + + + + + + + +
+
+
+ change operation mode +
+
+
+ + change operation mode + + + + + + + + +
+
+
+ mode change completed +
+
+
+ + mode change completed + + + + + + + + + + +
+
+
+ remote +
+ (autoware control is false) +
+
+
+ + remote... + + + + + + + + +
+
+
+ local +
+ (autoware control is false) +
+
+
+ + local... + + + + + + + + + + + + +
+
+
+ remote +
+ (in transition) +
+
+
+ + remote... + + + + + + + + + + + + +
+
+
+ local +
+ (in transition) +
+
+
+ + local... + + + + + + + + + + +
+
+
+ remote +
+ (autoware control is true) +
+
+
+ + remote... + + + + + + + + + + +
+
+
+ local +
+ (autoware control is true) +
+
+
+ + local... + + + + + + + +
+
+
+ mode change failed +
+
+
+ + mode change failed + + + + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/system/default_ad_api/document/images/operation-mode-table.drawio.svg b/system/default_ad_api/document/images/operation-mode-table.drawio.svg new file mode 100644 index 0000000000000..19a3ea5bd1133 --- /dev/null +++ b/system/default_ad_api/document/images/operation-mode-table.drawio.svg @@ -0,0 +1,317 @@ + + + + + + + +
+
+
+ driver +
+
+
+ + driver + + + + + + +
+
+
+ autonomous +
+
+
+ + autonomous + + + + + + +
+
+
+ stop +
+
+
+ + stop + + + + + + +
+
+
+ remote +
+
+
+ + remote + + + + + + +
+
+
+ local +
+
+
+ + local + + + + + + +
+
+
+ control_mode +
+
+
+ + control_mode + + + + + + +
+
+
+ manual +
+
+
+ + manual + + + + + + +
+
+
+ autonomous +
+
+
+ + autonomous + + + + + + +
+
+
+ gate_mode +
+
+
+ + gate_mode + + + + + + + +
+
+
+ auto +
+
+
+ + auto + + + + + + +
+
+
+ external +
+
+
+ + external + + + + + + +
+
+
+ engage +
+
+
+ + engage + + + + + + + +
+
+
+ false +
+
+
+ + false + + + + + + +
+
+
+ true +
+
+
+ + true + + + + + + +
+
+
+ selector_mode +
+
+
+ + selector_mode + + + + + + + +
+
+
+ local +
+
+
+ + local + + + + + + +
+
+
+ remote +
+
+
+ + remote + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/system/default_ad_api/document/images/routing.drawio.svg b/system/default_ad_api/document/images/routing.drawio.svg new file mode 100644 index 0000000000000..6742bfe4c12b0 --- /dev/null +++ b/system/default_ad_api/document/images/routing.drawio.svg @@ -0,0 +1,398 @@ + + + + + + + +
+
+
+ mission_planner +
+
+
+ + mission_planner + + + + + + + + +
+
+
+ planner plugin +
+
+
+ + planner plugin + + + + + + + + + + +
+
+
+ routing_adaptor +
+
+
+ + routing_adaptor + + + + + + +
+
+
+ set route +
+
+
+ + set route + + + + + + +
+
+
+ set route points +
+
+
+ + set route points + + + + + + +
+
+
+ pose topic +
+
+
+ + pose topic + + + + + + +
+
+
+ planning modules +
+
+
+ + planning modules + + + + + + +
+
+
+ lanelet route +
+
+
+ + lanelet route + + + + + + +
+
+
+ set route points +
+
+
+ + set route points + + + + + + + + + + + + +
+
+
+ App +
+
+
+ + App + + + + + + +
+
+
+ RViz +
+
+
+ + RViz + + + + + + + + + + + + +
+
+
+ AD +
+ API +
+
+
+ + AD... + + + + + + + + + + + + + + +
+
+
+ main module +
+
+
+ + main module + + + + + + +
+
+
+ set lanelet route +
+
+
+ + set lanelet route + + + + + + +
+
+
+ + route, route state + +
+
+
+ + route, route state + + + + + + +
+
+
+ set route points +
+
+
+ + set route points + + + + + + +
+
+
+ route state +
+
+
+ + route state + + + + + + +
+
+
+ + clear route + +
+
+
+ + clear route + + + + + + +
+
+
+ pose array +
+
+
+ + pose array + + + + + + +
+
+
+ lanelet route +
+
+
+ + lanelet route + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/system/default_ad_api/document/interface.md b/system/default_ad_api/document/interface.md new file mode 100644 index 0000000000000..f3fc6a389c294 --- /dev/null +++ b/system/default_ad_api/document/interface.md @@ -0,0 +1,5 @@ +# Interface API + +## Overview + +The interface API simply returns a version number. See the [autoware-documentation](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/interface/) for AD API specifications. diff --git a/system/default_ad_api/document/localization.md b/system/default_ad_api/document/localization.md new file mode 100644 index 0000000000000..866322ed807cf --- /dev/null +++ b/system/default_ad_api/document/localization.md @@ -0,0 +1,7 @@ +# Localization API + +## Overview + +Unify the location initialization method to the service. The topic `/initialpose` from rviz is now only subscribed to by adapter node and converted to API call. This API call is forwarded to the pose initializer node so it can centralize the state of pose initialization. For other nodes that require initialpose, pose initializer node publishes as `/initialpose3d`. See the [autoware-documentation](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/localization/) for AD API specifications. + +![localization-architecture](images/localization.drawio.svg) diff --git a/system/default_ad_api/document/motion.md b/system/default_ad_api/document/motion.md new file mode 100644 index 0000000000000..fd01a8f56ed7f --- /dev/null +++ b/system/default_ad_api/document/motion.md @@ -0,0 +1,13 @@ +# Motion API + +## Overview + +Provides a hook for when the vehicle starts. It is typically used for announcements that call attention to the surroundings. Add a pause function to the vehicle_cmd_gate, and API will control it based on vehicle stopped and start requested. See the [autoware-documentation](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/motion/) for AD API specifications. + +![motion-architecture](images/motion-architecture.drawio.svg) + +## States + +The implementation has more detailed state transitions to manage pause state synchronization. The correspondence with the AD API state is as follows. + +![motion-state](images/motion-state.drawio.svg) diff --git a/system/default_ad_api/document/operation-mode.md b/system/default_ad_api/document/operation-mode.md new file mode 100644 index 0000000000000..703f6aa47b50d --- /dev/null +++ b/system/default_ad_api/document/operation-mode.md @@ -0,0 +1,24 @@ +# Operation mode API + +## Overview + +Introduce operation mode. It handles autoware engage, gate_mode, external_cmd_selector and control_mode abstractly. When the mode is changed, it will be in-transition state, and if the transition completion condition to that mode is not satisfied, it will be returned to the previous mode. Also, currently, the condition for mode change is only `WaitingForEngage` in `/autoware/state`, and the engage state is shared between modes. After introducing the operation mode, each mode will have a transition available flag. See the [autoware-documentation](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/operation_mode/) for AD API specifications. + +![operation-mode-architecture](images/operation-mode-architecture.drawio.svg) + +## States + +The operation mode has the following state transitions. Disabling autoware control and changing operation mode when autoware control is disabled can be done immediately. +Otherwise, enabling autoware control and changing operation mode when autoware control is enabled causes the state will be transition state. +If the mode change completion condition is not satisfied within the timeout in the transition state, it will return to the previous mode. + +![operation-mode-state](images/operation-mode-state.drawio.svg) + +## Compatibility + +Ideally, vehicle_cmd_gate and external_cmd_selector should be merged so that the operation mode can be handled directly. +However, currently the operation mode transition manager performs the following conversions to match the implementation. +The transition manager monitors each topic in the previous interface and synchronizes the operation mode when it changes. +When the operation mode is changed with the new interface, the transition manager disables synchronization and changes the operation mode using the previous interface. + +![operation-mode-table](images/operation-mode-table.drawio.svg) diff --git a/system/default_ad_api/document/routing.md b/system/default_ad_api/document/routing.md new file mode 100644 index 0000000000000..899136f2d9e50 --- /dev/null +++ b/system/default_ad_api/document/routing.md @@ -0,0 +1,7 @@ +# Routing API + +## Overview + +Unify the route setting method to the service. This API supports two waypoint formats, poses and lanelet segments. The goal and checkpoint topics from rviz is only subscribed to by adapter node and converted to API call. This API call is forwarded to the mission planner node so it can centralize the state of routing. For other nodes that require route, mission planner node publishes as `/planning/mission_planning/route`. See the [autoware-documentation](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/routing/) for AD API specifications. + +![routing-architecture](images/routing.drawio.svg) From c48b9cfa7074ecd46d96f6dc43679e17bde3a63d Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Tue, 13 Dec 2022 09:16:14 +0900 Subject: [PATCH 3/3] feat(map_loader): add differential map loading interface (#2417) * first commit Signed-off-by: kminoda * ci(pre-commit): autofix * added module load in _node.cpp Signed-off-by: kminoda * ci(pre-commit): autofix * create pcd metadata dict when either of the flag is true Signed-off-by: kminoda * ci(pre-commit): autofix * fix readme * ci(pre-commit): autofix Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../config/pointcloud_map_loader.param.yaml | 1 + map/map_loader/CMakeLists.txt | 1 + map/map_loader/README.md | 10 +++ .../config/pointcloud_map_loader.param.yaml | 1 + .../differential_map_loader_module.cpp | 85 +++++++++++++++++++ .../differential_map_loader_module.hpp | 59 +++++++++++++ .../pointcloud_map_loader_node.cpp | 11 ++- .../pointcloud_map_loader_node.hpp | 2 + 8 files changed, 169 insertions(+), 1 deletion(-) create mode 100644 map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp create mode 100644 map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp diff --git a/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml b/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml index fa94afbf12a0b..8f3ccbff00360 100644 --- a/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml +++ b/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] diff --git a/map/map_loader/CMakeLists.txt b/map/map_loader/CMakeLists.txt index 6bfcffde71d60..4c3b418239a5c 100644 --- a/map/map_loader/CMakeLists.txt +++ b/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}) diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 1683fd472e74a..c8f5bed19bb3d 100644 --- a/map/map_loader/README.md +++ b/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,6 +29,13 @@ 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 | @@ -35,6 +43,7 @@ Please see [the description of `GetPartialPointCloudMap.srv`](https://github.com | 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 @@ -42,6 +51,7 @@ Please see [the description of `GetPartialPointCloudMap.srv`](https://github.com - `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 --- diff --git a/map/map_loader/config/pointcloud_map_loader.param.yaml b/map/map_loader/config/pointcloud_map_loader.param.yaml index fa94afbf12a0b..8f3ccbff00360 100644 --- a/map/map_loader/config/pointcloud_map_loader.param.yaml +++ b/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] diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp new file mode 100644 index 0000000000000..b42b919edb59d --- /dev/null +++ b/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 & 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( + "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 & cached_ids, + GetDifferentialPointCloudMap::Response::SharedPtr & response) const +{ + // iterate over all the available pcd map grids + std::vector should_remove(static_cast(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(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 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; +} diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp new file mode 100644 index 0000000000000..5d6188c0b1a1f --- /dev/null +++ b/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 + +#include "autoware_map_msgs/srv/get_differential_point_cloud_map.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +class DifferentialMapLoaderModule +{ + using GetDifferentialPointCloudMap = autoware_map_msgs::srv::GetDifferentialPointCloudMap; + +public: + explicit DifferentialMapLoaderModule( + rclcpp::Node * node, const std::map & pcd_file_metadata_dict); + +private: + rclcpp::Logger logger_; + + std::map all_pcd_file_metadata_dict_; + rclcpp::Service::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 & 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_ diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp index b42be7308b67b..ecbe481345381 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp +++ b/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("enable_whole_load"); bool enable_downsample_whole_load = declare_parameter("enable_downsampled_whole_load"); bool enable_partial_load = declare_parameter("enable_partial_load"); + bool enable_differential_load = declare_parameter("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(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(this, pcd_metadata_dict_); } + + if (enable_differential_load) { + differential_map_loader_ = + std::make_unique(this, pcd_metadata_dict_); + } } std::vector PointCloudMapLoaderNode::getPcdPaths( diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp index a7c289dd0f9a8..d321902131a81 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp +++ b/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 pcd_map_loader_; std::unique_ptr downsampled_pcd_map_loader_; std::unique_ptr partial_map_loader_; + std::unique_ptr differential_map_loader_; std::vector getPcdPaths( const std::vector & pcd_paths_or_directory) const;