feat(probablisitic_occupancy_grid_map): add scan_frame option for gri…

…dmap generation (#3032) * add scan_frame and raytrace center Signed-off-by: yoshiri <[email protected]> * add scan frame to laserscan based method Signed-off-by: yoshiri <[email protected]> * update readme Signed-off-by: yoshiri <[email protected]> * fix typo Signed-off-by: yoshiri <[email protected]> * update laucher in perception_launch Signed-off-by: yoshiri <[email protected]> * fix config and launch file Signed-off-by: yoshiri <[email protected]> * fixed laserscan based launcher Signed-off-by: yoshiri <[email protected]> --------- Signed-off-by: yoshiri <[email protected]>
autowarefoundation · Mar 14, 2023 · 7a0f84c · 7a0f84c · ahuazuipiaoliang · Mar 14, 2023
1 parent 07b7837
commit 7a0f84c
Show file tree

Hide file tree

Showing 16 changed files with 329 additions and 170 deletions.
diff --git a/..._perception_launch/launch/occupancy_grid_map/laserscan_based_occupancy_grid_map.launch.py b/..._perception_launch/launch/occupancy_grid_map/laserscan_based_occupancy_grid_map.launch.py
@@ -14,29 +14,27 @@
 
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
+from launch.actions import OpaqueFunction
 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 LoadComposableNodes
 from launch_ros.descriptions import ComposableNode
+import yaml
 
 
-def generate_launch_description():
-    def add_launch_arg(name: str, default_value=None):
-        return DeclareLaunchArgument(name, default_value=default_value)
-
-    set_container_executable = SetLaunchConfiguration(
-        "container_executable",
-        "component_container",
-        condition=UnlessCondition(LaunchConfiguration("use_multithread")),
+def launch_setup(context, *args, **kwargs):
+    # load parameter files
+    param_file = LaunchConfiguration("param_file").perform(context)
+    with open(param_file, "r") as f:
+        laserscan_based_occupancy_grid_map_node_params = yaml.safe_load(f)["/**"]["ros__parameters"]
+    laserscan_based_occupancy_grid_map_node_params["input_obstacle_pointcloud"] = bool(
+        LaunchConfiguration("input_obstacle_pointcloud").perform(context)
     )
-
-    set_container_mt_executable = SetLaunchConfiguration(
-        "container_executable",
-        "component_container_mt",
-        condition=IfCondition(LaunchConfiguration("use_multithread")),
+    laserscan_based_occupancy_grid_map_node_params["input_obstacle_and_raw_pointcloud"] = bool(
+        LaunchConfiguration("input_obstacle_and_raw_pointcloud").perform(context)
     )
 
     composable_nodes = [
@@ -53,7 +51,9 @@ def add_launch_arg(name: str, default_value=None):
             ],
             parameters=[
                 {
-                    "target_frame": "base_link",  # Leave disabled to output scan in pointcloud frame
+                    "target_frame": laserscan_based_occupancy_grid_map_node_params[
+                        "scan_origin_frame"
+                    ],  # Leave disabled to output scan in pointcloud frame
                     "transform_tolerance": 0.01,
                     "min_height": 0.0,
                     "max_height": 2.0,
@@ -85,16 +85,7 @@ def add_launch_arg(name: str, default_value=None):
                 ("~/input/raw_pointcloud", LaunchConfiguration("input/raw_pointcloud")),
                 ("~/output/occupancy_grid_map", LaunchConfiguration("output")),
             ],
-            parameters=[
-                {
-                    "map_resolution": 0.5,
-                    "use_height_filter": True,
-                    "input_obstacle_pointcloud": LaunchConfiguration("input_obstacle_pointcloud"),
-                    "input_obstacle_and_raw_pointcloud": LaunchConfiguration(
-                        "input_obstacle_and_raw_pointcloud"
-                    ),
-                }
-            ],
+            parameters=[laserscan_based_occupancy_grid_map_node_params],
             extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
         ),
     ]
@@ -115,6 +106,25 @@ def add_launch_arg(name: str, default_value=None):
         condition=IfCondition(LaunchConfiguration("use_pointcloud_container")),
     )
 
+    return [occupancy_grid_map_container, load_composable_nodes]
+
+
+def generate_launch_description():
+    def add_launch_arg(name: str, default_value=None):
+        return DeclareLaunchArgument(name, default_value=default_value)
+
+    set_container_executable = SetLaunchConfiguration(
+        "container_executable",
+        "component_container",
+        condition=UnlessCondition(LaunchConfiguration("use_multithread")),
+    )
+
+    set_container_mt_executable = SetLaunchConfiguration(
+        "container_executable",
+        "component_container_mt",
+        condition=IfCondition(LaunchConfiguration("use_multithread")),
+    )
+
     return LaunchDescription(
         [
             add_launch_arg("use_multithread", "false"),
@@ -128,11 +138,11 @@ def add_launch_arg(name: str, default_value=None):
             add_launch_arg("output/stixel", "virtual_scan/stixel"),
             add_launch_arg("input_obstacle_pointcloud", "false"),
             add_launch_arg("input_obstacle_and_raw_pointcloud", "true"),
-            add_launch_arg("use_pointcloud_container", "False"),
+            add_launch_arg("param_file", "config/laserscan_based_occupancy_grid_map.param.yaml"),
+            add_launch_arg("use_pointcloud_container", "false"),
             add_launch_arg("container_name", "occupancy_grid_map_container"),
             set_container_executable,
             set_container_mt_executable,
-            occupancy_grid_map_container,
-            load_composable_nodes,
         ]
+        + [OpaqueFunction(function=launch_setup)]
     )
diff --git a/...perception_launch/launch/occupancy_grid_map/pointcloud_based_occupancy_grid_map.launch.py b/...perception_launch/launch/occupancy_grid_map/pointcloud_based_occupancy_grid_map.launch.py
@@ -14,30 +14,24 @@
 
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
+from launch.actions import OpaqueFunction
 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 LoadComposableNodes
 from launch_ros.descriptions import ComposableNode
+import yaml
 
 
-def generate_launch_description():
-    def add_launch_arg(name: str, default_value=None):
-        return DeclareLaunchArgument(name, default_value=default_value)
-
-    set_container_executable = SetLaunchConfiguration(
-        "container_executable",
-        "component_container",
-        condition=UnlessCondition(LaunchConfiguration("use_multithread")),
-    )
-
-    set_container_mt_executable = SetLaunchConfiguration(
-        "container_executable",
-        "component_container_mt",
-        condition=IfCondition(LaunchConfiguration("use_multithread")),
-    )
+def launch_setup(context, *args, **kwargs):
+    # load parameter files
+    param_file = LaunchConfiguration("param_file").perform(context)
+    with open(param_file, "r") as f:
+        pointcloud_based_occupancy_grid_map_node_params = yaml.safe_load(f)["/**"][
+            "ros__parameters"
+        ]
 
     composable_nodes = [
         ComposableNode(
@@ -49,12 +43,7 @@ def add_launch_arg(name: str, default_value=None):
                 ("~/input/raw_pointcloud", LaunchConfiguration("input/raw_pointcloud")),
                 ("~/output/occupancy_grid_map", LaunchConfiguration("output")),
             ],
-            parameters=[
-                {
-                    "map_resolution": 0.5,
-                    "use_height_filter": True,
-                }
-            ],
+            parameters=[pointcloud_based_occupancy_grid_map_node_params],
             extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
         ),
     ]
@@ -75,18 +64,37 @@ def add_launch_arg(name: str, default_value=None):
         condition=IfCondition(LaunchConfiguration("use_pointcloud_container")),
     )
 
+    return [occupancy_grid_map_container, load_composable_nodes]
+
+
+def generate_launch_description():
+    def add_launch_arg(name: str, default_value=None):
+        return DeclareLaunchArgument(name, default_value=default_value)
+
+    set_container_executable = SetLaunchConfiguration(
+        "container_executable",
+        "component_container",
+        condition=UnlessCondition(LaunchConfiguration("use_multithread")),
+    )
+
+    set_container_mt_executable = SetLaunchConfiguration(
+        "container_executable",
+        "component_container_mt",
+        condition=IfCondition(LaunchConfiguration("use_multithread")),
+    )
+
     return LaunchDescription(
         [
-            add_launch_arg("use_multithread", "False"),
-            add_launch_arg("use_intra_process", "True"),
-            add_launch_arg("use_pointcloud_container", "False"),
+            add_launch_arg("use_multithread", "false"),
+            add_launch_arg("use_intra_process", "true"),
+            add_launch_arg("use_pointcloud_container", "false"),
             add_launch_arg("container_name", "occupancy_grid_map_container"),
             add_launch_arg("input/obstacle_pointcloud", "no_ground/oneshot/pointcloud"),
             add_launch_arg("input/raw_pointcloud", "concatenated/pointcloud"),
             add_launch_arg("output", "occupancy_grid"),
+            add_launch_arg("param_file", "config/pointcloud_based_occupancy_grid_map.param.yaml"),
             set_container_executable,
             set_container_mt_executable,
-            occupancy_grid_map_container,
-            load_composable_nodes,
         ]
+        + [OpaqueFunction(function=launch_setup)]
     )
diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml
@@ -76,6 +76,7 @@
         <arg name="use_multithread" value="true"/>
         <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
         <arg name="container_name" value="$(var pointcloud_container_name)"/>
+        <arg name="param_file" value="$(find-pkg-share probabilistic_occupancy_grid_map)/config/pointcloud_based_occupancy_grid_map.param.yaml"/>
       </include>
     </group>
 

diff --git a/perception/probabilistic_occupancy_grid_map/CMakeLists.txt b/perception/probabilistic_occupancy_grid_map/CMakeLists.txt
@@ -48,4 +48,5 @@ rclcpp_components_register_node(laserscan_based_occupancy_grid_map
 ament_auto_package(
   INSTALL_TO_SHARE
     launch
+    config
 )
diff --git a/perception/probabilistic_occupancy_grid_map/README.md b/perception/probabilistic_occupancy_grid_map/README.md
@@ -9,7 +9,9 @@ This package outputs the probability of having an obstacle as occupancy grid map
 
 Occupancy grid map is generated on `map_frame`, and grid orientation is fixed.
 
-You may need to choose `output_frame` which means grid map origin. Default is `base_link`, but your main LiDAR sensor frame (e.g. `velodyne_top` in sample_vehicle) would be the better choice.
+You may need to choose `scan_origin_frame` and `gridmap_origin_frame` which means sensor origin and gridmap origin respectively. Especially, set your main LiDAR sensor frame (e.g. `velodyne_top` in sample_vehicle) as a `scan_origin_frame` would result in better performance.
+
+![image_for_frame_parameter_visualization](./image/gridmap_frame_settings.drawio.svg)
 
 ### Each config paramters
 
@@ -21,7 +23,8 @@ Config parameters are managed in `config/*.yaml` and here shows its outline.
 | --------------------------------- | ------------- |
 | map_frame                         | "map"         |
 | base_link_frame                   | "base_link"   |
-| output_frame                      | "base_link"   |
+| scan_origin_frame                 | "base_link"   |
+| gridmap_origin_frame              | "base_link"   |
 | use_height_filter                 | true          |
 | enable_single_frame_mode          | false         |
 | map_length                        | 100.0 [m]     |
@@ -40,7 +43,8 @@ Config parameters are managed in `config/*.yaml` and here shows its outline.
 | enable_single_frame_mode | false         |
 | map_frame                | "map"         |
 | base_link_frame          | "base_link"   |
-| output_frame             | "base_link"   |
+| scan_origin_frame        | "base_link"   |
+| gridmap_origin_frame     | "base_link"   |
 
 ## References/External links
 

diff --git a/...ion/probabilistic_occupancy_grid_map/config/laserscan_based_occupancy_grid_map.param.yaml b/...ion/probabilistic_occupancy_grid_map/config/laserscan_based_occupancy_grid_map.param.yaml
@@ -2,8 +2,10 @@
   ros__parameters:
     map_frame: "map"
     base_link_frame: "base_link"
-    output_frame: "base_link"
-    # using top of the main lidar frame is appropriate. ex) velodyne_top
+    # center of the grid map
+    gridmap_origin_frame: "base_link"
+    # ray-tracing center: main sensor frame is preferable like: "velodyne_top"
+    scan_origin_frame: "base_link"
 
     use_height_filter: true
     enable_single_frame_mode: false

diff --git a/...on/probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml b/...on/probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml
@@ -1,6 +1,6 @@
 /**:
   ros__parameters:
-    map_length: 100     # [m]
+    map_length: 100.0     # [m]
     map_resolution: 0.5 # [m]
 
     use_height_filter: true
@@ -9,5 +9,7 @@
     # grid map coordinate
     map_frame: "map"
     base_link_frame: "base_link"
-    output_frame: "base_link"
-    # center of grid_map. Main LiDAR frame is preferable like: "velodyne_top"
+    # center of the grid map
+    gridmap_origin_frame: "base_link"
+    # ray-tracing center: main sensor frame is preferable like: "velodyne_top"
+    scan_origin_frame: "base_link"
diff --git a/...eption/probabilistic_occupancy_grid_map/image/gridmap_frame_settings.drawio.svg b/...eption/probabilistic_occupancy_grid_map/image/gridmap_frame_settings.drawio.svg
diff --git a/...ap/include/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp b/...ap/include/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp
@@ -92,7 +92,8 @@ class LaserscanBasedOccupancyGridMapNode : public rclcpp::Node
   // ROS Parameters
   std::string map_frame_;
   std::string base_link_frame_;
-  std::string output_frame_;
+  std::string gridmap_origin_frame_;
+  std::string scan_origin_frame_;
   bool use_height_filter_;
   bool enable_single_frame_mode_;
 };
-Original file line number
+Diff line change
@@ Expand Up @@
     ament_auto_package(
       INSTALL_TO_SHARE
         launch
+        config
     )