fix/move occupancy grid map outlier filter (autowarefoundation#143)

* fix package name

* add occupancy grid map launch

* update launcher

* rename filter package

perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py

@@ -16,17 +16,14 @@
 from ament_index_python.packages import get_package_share_directory
 import launch
 from launch.actions import DeclareLaunchArgument
-from launch.actions import IncludeLaunchDescription
 from launch.actions import OpaqueFunction
 from launch.actions import SetLaunchConfiguration
 from launch.conditions import IfCondition
 from launch.conditions import UnlessCondition
-from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import ComposableNodeContainer
 from launch_ros.actions import LoadComposableNodes
 from launch_ros.descriptions import ComposableNode
-from launch_ros.substitutions import FindPackageShare
 import yaml
 
 
@@ -302,33 +299,9 @@ def launch_setup(context, *args, **kwargs):
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
 
-    laserscan_to_occupancy_grid_map_loader = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            [
-                FindPackageShare("laserscan_to_occupancy_grid_map"),
-                "/launch/laserscan_to_occupancy_grid_map.launch.py",
-            ]
-        ),
-        launch_arguments={
-            "container": "/perception/obstacle_segmentation/ground_segmentation/perception_pipeline_container",
-            "input/obstacle_pointcloud": "no_ground/oneshot/pointcloud"
-            if bool(ground_segmentation_param["additional_lidars"])
-            else "no_ground/pointcloud",
-            "input/raw_pointcloud": "/sensing/lidar/concatenated/pointcloud",
-            "output": "/perception/occupancy_grid_map/map",
-            "use_intra_process": LaunchConfiguration("use_intra_process"),
-        }.items(),
-        condition=UnlessCondition(
-            LaunchConfiguration(
-                "use_compare_map_pipeline",
-                default=ground_segmentation_param["use_compare_map_pipeline"],
-            )
-        ),
-    )
-
     occupancy_outlier_filter_component = ComposableNode(
-        package="pointcloud_preprocessor",
-        plugin="pointcloud_preprocessor::OccupancyGridMapOutlierFilterComponent",
+        package="occupancy_grid_map_outlier_filter",
+        plugin="occupancy_grid_map_outlier_filter::OccupancyGridMapOutlierFilterComponent",
         name="occupancy_grid_map_outlier_filter",
         remappings=[
             ("~/input/occupancy_grid_map", "/perception/occupancy_grid_map/map"),
@@ -406,7 +379,6 @@ def launch_setup(context, *args, **kwargs):
         compare_map_component_loader,
         concat_data_component_loader,
         occupancy_grid_outlier_filter_component_loader,
-        laserscan_to_occupancy_grid_map_loader,
     ]
 
 

perception_launch/launch/occupancy_grid_map/occupancy_grid_map.launch.py

@@ -0,0 +1,139 @@
+# 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.
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.actions import SetLaunchConfiguration
+from launch.conditions import IfCondition
+from launch.conditions import LaunchConfigurationEquals
+from launch.conditions import LaunchConfigurationNotEquals
+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
+
+
+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")),
+    )
+
+    composable_nodes = [
+        ComposableNode(
+            package="pointcloud_to_laserscan",
+            plugin="pointcloud_to_laserscan::PointCloudToLaserScanNode",
+            name="pointcloud_to_laserscan_node",
+            remappings=[
+                ("~/input/pointcloud", LaunchConfiguration("input/obstacle_pointcloud")),
+                ("~/output/laserscan", LaunchConfiguration("output/laserscan")),
+                ("~/output/pointcloud", LaunchConfiguration("output/pointcloud")),
+                ("~/output/ray", LaunchConfiguration("output/ray")),
+                ("~/output/stixel", LaunchConfiguration("output/stixel")),
+            ],
+            parameters=[
+                {
+                    "target_frame": "base_link",  # Leave disabled to output scan in pointcloud frame
+                    "transform_tolerance": 0.01,
+                    "min_height": 0.0,
+                    "max_height": 2.0,
+                    "angle_min": -3.141592,  # -M_PI
+                    "angle_max": 3.141592,  # M_PI
+                    "angle_increment": 0.00436332222,  # 0.25*M_PI/180.0
+                    "scan_time": 0.0,
+                    "range_min": 1.0,
+                    "range_max": 200.0,
+                    "use_inf": True,
+                    "inf_epsilon": 1.0,
+                    # Concurrency level, affects number of pointclouds queued for processing
+                    # and number of threads used
+                    # 0 : Detect number of cores
+                    # 1 : Single threaded
+                    # 2->inf : Parallelism level
+                    "concurrency_level": 1,
+                }
+            ],
+            extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+        ),
+        ComposableNode(
+            package="laserscan_to_occupancy_grid_map",
+            plugin="occupancy_grid_map::OccupancyGridMapNode",
+            name="occupancy_grid_map_node",
+            remappings=[
+                ("~/input/laserscan", LaunchConfiguration("output/laserscan")),
+                ("~/input/obstacle_pointcloud", LaunchConfiguration("input/obstacle_pointcloud")),
+                ("~/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"
+                    ),
+                }
+            ],
+            extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+        ),
+    ]
+
+    occupancy_grid_map_container = ComposableNodeContainer(
+        condition=LaunchConfigurationEquals("container", ""),
+        name="occupancy_grid_map_container",
+        namespace="",
+        package="rclcpp_components",
+        executable=LaunchConfiguration("container_executable"),
+        composable_node_descriptions=composable_nodes,
+        output="screen",
+    )
+
+    load_composable_nodes = LoadComposableNodes(
+        condition=LaunchConfigurationNotEquals("container", ""),
+        composable_node_descriptions=composable_nodes,
+        target_container=LaunchConfiguration("container"),
+    )
+
+    return LaunchDescription(
+        [
+            add_launch_arg("container", ""),
+            add_launch_arg("use_multithread", "false"),
+            add_launch_arg("use_intra_process", "false"),
+            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("output/laserscan", "virtual_scan/laserscan"),
+            add_launch_arg("output/pointcloud", "virtual_scan/pointcloud"),
+            add_launch_arg("output/ray", "virtual_scan/ray"),
+            add_launch_arg("output/stixel", "virtual_scan/stixel"),
+            add_launch_arg("input_obstacle_pointcloud", "false"),
+            add_launch_arg("input_obstacle_and_raw_pointcloud", "true"),
+            set_container_executable,
+            set_container_mt_executable,
+            occupancy_grid_map_container,
+            load_composable_nodes,
+        ]
+    )

perception_launch/launch/perception.launch.xml

@@ -43,6 +43,16 @@
       </group>
     </group>
 
+    <!-- occupancy grid map module -->
+    <group>
+      <push-ros-namespace namespace="occupancy_grid_map"/>
+      <include file="$(find-pkg-share perception_launch)/launch/occupancy_grid_map/occupancy_grid_map.launch.py">
+        <arg name="input/obstacle_pointcloud" value="/perception/obstacle_segmentation/ground_segmentation/no_ground/pointcloud" />
+        <arg name="input/raw_pointcloud" value="/sensing/lidar/concatenated/pointcloud" />
+        <arg name="output" value="/perception/occupancy_grid_map/map" />
+      </include>
+    </group>
+
     <!-- object recognition module -->
     <group unless="$(var use_empty_dynamic_object_publisher)">
       <push-ros-namespace namespace="object_recognition"/>

perception_launch/package.xml

@@ -19,6 +19,8 @@
   <exec_depend>multi_object_tracker</exec_depend>
   <exec_depend>object_merger</exec_depend>
   <exec_depend>object_range_splitter</exec_depend>
+  <exec_depend>occupancy_grid_map_outlier_filter</exec_depend>
+  <exec_depend>pointcloud_to_laserscan</exec_depend>
   <exec_depend>roi_cluster_fusion</exec_depend>
   <exec_depend>shape_estimation</exec_depend>
   <exec_depend>tensorrt_yolo</exec_depend>