feat: add map_launch package (autowarefoundation#170)

* release v0.4.0 * removed ROS1 package Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * Revert "removed ROS1 package" This reverts commit d8ee43e9455ea61642e27de183be1f7660bcc406. * add COLCON_IGNORE file to all ROS1 packages Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * rename *.launch files to *.launch.xml Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * port map_launch package Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * ROS2 Linting: map_launch (autowarefoundation#31) * Add linter tests and missing exec dependencies * Address PR comment: - Use ament_lint_common * add use_sim-time option (autowarefoundation#99) * Use set_parameter for use_sim_time (autowarefoundation#198) * Use set_parameter for use_sim_time Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Add default parameter for scenario simulator Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Add map.launch.py (autowarefoundation#212) * Add map.launch.py Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Update map.launch.xml Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Rename parameter for lanelet2 map path Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Format launch files (autowarefoundation#228) * Format launch files Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Format launch.py Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix lint Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * add description for map_launch autowarefoundation#333 * add README.md and svg files (autowarefoundation#328) * add md and svg * fix typo * fix typo * fix word * fix typo * add lack of things * Update README * fix depending packages * fix word * Add autoware api (autowarefoundation#376) * Add external api adaptor (autowarefoundation#267) * Add external api adaptor * Add api adaptor to logging simulator Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Add engage status output Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Add internal api adaptor (autowarefoundation#273) Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Add map hash generator (autowarefoundation#319) Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Add autoware api launch (autowarefoundation#326) * Add autoware api launch Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Apply autoware api launch Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Add deprecated comment Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Remove unused parameter (autowarefoundation#325) Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Add api parameter (autowarefoundation#341) Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Add start request API (autowarefoundation#321) * Add use start request option Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix lint Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Feature external cmd selector heartbeat (autowarefoundation#356) * Rename external command topic Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Modify command topic name Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix topic name Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Rename remote_cmd_converter Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Remove gate mode from external command Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix latest external command name (autowarefoundation#361) Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix merge conflict Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * change external traffic light topic name in behavior velocity planner (autowarefoundation#310) * fix topic * change internal topic name Co-authored-by: yabuta <makoto.yabuta@tier4.jp> * Change formatter to black (autowarefoundation#488) * Update pre-commit settings * Apply Black * Replace ament_lint_common with autoware_lint_common * Update build_depends.repos * Fix build_depends * input pointcloud_map_path to map_hash_generator (autowarefoundation#142) Co-authored-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> Co-authored-by: Jilada Eccleston <jilada.eccleston@tier4.jp> Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp> Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: yabuta <makoto.yabuta@tier4.jp> Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp> Co-authored-by: Taichi Higashide <taichi.higashide@tier4.jp>
badai-nguyen · Dec 13, 2021 · 20b4152 · 20b4152
1 parent 5ce9b56
commit 20b4152
Show file tree

Hide file tree

Showing 6 changed files with 486 additions and 0 deletions.
diff --git a/launch/map_launch/CMakeLists.txt b/launch/map_launch/CMakeLists.txt
@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 3.5)
+project(map_launch)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wno-unused-parameter -Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+)
diff --git a/launch/map_launch/README.md b/launch/map_launch/README.md
@@ -0,0 +1,28 @@
+# map_launch
+
+## Structure
+
+![map_launch](./map_launch.drawio.svg)
+
+## Package Dependencies
+
+Please see `<exec_depend>` in `package.xml`.
+
+## Usage
+
+You can include as follows in `*.launch.xml` to use `map.launch.py`.
+
+```xml
+<arg name="map_path" description="point cloud and lanelet2 map directory path"/>
+<arg name="lanelet2_map_file" default="lanelet2_map.osm" description="lanelet2 map file name"/>
+<arg name="pointcloud_map_file" default="pointcloud_map.pcd" description="pointcloud map file name"/>
+
+<include file="$(find-pkg-share map_launch)/launch/map.launch.py">
+  <arg name="lanelet2_map_path" value="$(var map_path)/$(var lanelet2_map_file)" />
+  <arg name="pointcloud_map_path" value="$(var map_path)/$(var pointcloud_map_file)"/>
+</include>
+```
+
+## Notes
+
+For reducing processing load, we use the [Component](https://docs.ros.org/en/galactic/Concepts/About-Composition.html) feature in ROS2 (similar to Nodelet in ROS1 )
diff --git a/launch/map_launch/launch/map.launch.py b/launch/map_launch/launch/map.launch.py
@@ -0,0 +1,142 @@
+# Copyright 2021 Tier IV, Inc. All rights reserved.
+#
+# 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.
+
+import launch
+from launch.actions import DeclareLaunchArgument
+from launch.actions import GroupAction
+from launch.actions import SetLaunchConfiguration
+from launch.conditions import IfCondition
+from launch.conditions import UnlessCondition
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.actions import Node
+from launch_ros.actions import PushRosNamespace
+from launch_ros.descriptions import ComposableNode
+
+
+def generate_launch_description():
+    map_hash_generator = Node(
+        package="map_loader",
+        executable="map_hash_generator",
+        name="map_hash_generator",
+        parameters=[
+            {
+                "lanelet2_map_path": LaunchConfiguration("lanelet2_map_path"),
+                "pointcloud_map_path": LaunchConfiguration("pointcloud_map_path"),
+            }
+        ],
+    )
+
+    lanelet2_map_loader = ComposableNode(
+        package="map_loader",
+        plugin="Lanelet2MapLoaderNode",
+        name="lanelet2_map_loader",
+        remappings=[("output/lanelet2_map", "vector_map")],
+        parameters=[
+            {
+                "center_line_resolution": 5.0,
+                "lanelet2_map_path": LaunchConfiguration("lanelet2_map_path"),
+            }
+        ],
+        extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+    )
+
+    lanelet2_map_visualization = ComposableNode(
+        package="map_loader",
+        plugin="Lanelet2MapVisualizationNode",
+        name="lanelet2_map_visualization",
+        remappings=[
+            ("input/lanelet2_map", "vector_map"),
+            ("output/lanelet2_map_marker", "vector_map_marker"),
+        ],
+        extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+    )
+
+    pointcloud_map_loader = ComposableNode(
+        package="map_loader",
+        plugin="PointCloudMapLoaderNode",
+        name="pointcloud_map_loader",
+        remappings=[("output/pointcloud_map", "pointcloud_map")],
+        parameters=[
+            {"pcd_paths_or_directory": ["[", LaunchConfiguration("pointcloud_map_path"), "]"]}
+        ],
+        extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+    )
+
+    map_tf_generator = ComposableNode(
+        package="map_tf_generator",
+        plugin="MapTFGeneratorNode",
+        name="map_tf_generator",
+        parameters=[
+            {
+                "map_frame": "map",
+                "viewer_frame": "viewer",
+            }
+        ],
+        extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+    )
+
+    container = ComposableNodeContainer(
+        name="map_container",
+        namespace="",
+        package="rclcpp_components",
+        executable=LaunchConfiguration("container_executable"),
+        composable_node_descriptions=[
+            lanelet2_map_loader,
+            lanelet2_map_visualization,
+            pointcloud_map_loader,
+            map_tf_generator,
+        ],
+        output="screen",
+    )
+
+    def add_launch_arg(name: str, default_value=None, description=None):
+        return DeclareLaunchArgument(name, default_value=default_value, description=description)
+
+    return launch.LaunchDescription(
+        [
+            add_launch_arg("map_path", "", "path to map directory"),
+            add_launch_arg(
+                "lanelet2_map_path",
+                [LaunchConfiguration("map_path"), "/lanelet2_map.osm"],
+                "path to lanelet2 map file",
+            ),
+            add_launch_arg(
+                "pointcloud_map_path",
+                [LaunchConfiguration("map_path"), "/pointcloud_map.pcd"],
+                "path to pointcloud map file",
+            ),
+            add_launch_arg(
+                "use_intra_process", "false", "use ROS2 component container communication"
+            ),
+            add_launch_arg("use_multithread", "false", "use multithread"),
+            SetLaunchConfiguration(
+                "container_executable",
+                "component_container",
+                condition=UnlessCondition(LaunchConfiguration("use_multithread")),
+            ),
+            SetLaunchConfiguration(
+                "container_executable",
+                "component_container_mt",
+                condition=IfCondition(LaunchConfiguration("use_multithread")),
+            ),
+            GroupAction(
+                [
+                    PushRosNamespace("map"),
+                    container,
+                    map_hash_generator,
+                ]
+            ),
+        ]
+    )
diff --git a/launch/map_launch/launch/map.launch.xml b/launch/map_launch/launch/map.launch.xml
@@ -0,0 +1,22 @@
+<launch>
+  <arg name="map_path" default="" />
+  <arg name="lanelet2_map_path" default="$(var map_path)/lanelet2_map.osm" />
+  <arg name="pointcloud_map_path" default="$(var map_path)/pointcloud_map.pcd" />
+
+  <group>
+    <push-ros-namespace namespace="map"/>
+    <include file="$(find-pkg-share map_loader)/launch/lanelet2_map_loader.launch.xml">
+      <arg name="lanelet2_map_path" value="$(var lanelet2_map_path)" />
+    </include>
+
+    <node pkg="map_loader" exec="pointcloud_map_loader" name="pointcloud_map_loader">
+      <remap from="output/pointcloud_map" to="/map/pointcloud_map" />
+      <param name="pcd_paths_or_directory" value="[$(var pointcloud_map_path)]" />
+    </node>
+
+    <include file="$(find-pkg-share map_tf_generator)/launch/map_tf_generator.launch.xml">
+      <arg name="input_map_points_topic" value="/map/pointcloud_map" />
+    </include>
+  </group>
+
+</launch>