Merge pull request autowarefoundation#11 from leo-drive/add_multı_cam…

…era_launch

kc / add camera pipeline

launch/tier4_perception_launch/launch/object_recognition/detection/all_camera.launch.xml

@@ -0,0 +1,149 @@
+<launch>
+
+<!--    common parameters -->
+    <arg name="image_0" default="front_right_camera" description="image raw topic name"/>
+    <arg name="camera_info0" default="/lucid_vision/front_right_camera/camera_info" description="camera info topic name"/>
+    <arg name="gpu_id_image_0" default="2"/>
+    <arg name="image_1" default="front_camera"/>
+    <arg name="camera_info1" default="/lucid_vision/front_camera/camera_info"/>
+    <arg name="gpu_id_image_1" default="2"/>
+    <arg name="image_2" default="front_left_camera"/>
+    <arg name="camera_info2" default="/lucid_vision/front_left_camera/camera_info"/>
+    <arg name="gpu_id_image_2" default="2"/>
+    <arg name="image_3" default="middle_left_camera"/>
+    <arg name="camera_info3" default="/lucid_vision/middle_left_camera/camera_info"/>
+    <arg name="gpu_id_image_3" default="2"/>
+    <arg name="image_4" default="middle_right_camera"/>
+    <arg name="camera_info4" default="/lucid_vision/middle_right_camera/camera_info"/>
+    <arg name="gpu_id_image_4" default="3"/>
+    <arg name="image_5" default="rear_left_camera"/>
+    <arg name="camera_info5" default="/lucid_vision/rear_left_camera/camera_info"/>
+    <arg name="gpu_id_image_5" default="3"/>
+    <arg name="image_6" default="rear_right_camera"/>
+    <arg name="camera_info6" default="/lucid_vision/rear_right_camera/camera_info"/>
+    <arg name="gpu_id_image_6" default="3"/>
+    <arg name="image_7" default="rear_camera"/>
+    <arg name="camera_info7" default="/lucid_vision/rear_camera/camera_info"/>
+    <arg name="gpu_id_image_7" default="3"/>
+    <arg name="image_number" default="8" description="choose image raw number(0-7)"/>
+
+<!--    tensorrt params -->
+    <arg name="mode" default="FP32"/>
+    <arg name="yolo_type" default="yolov3" description="choose image raw number(0-7)"/>
+    <arg name="label_file" default="coco.names" description="choose image raw number(0-7)"/>
+    <arg name="gpu_id" default="0" description="choose image raw number(0-7)"/>
+    <arg name="use_intra_process" default="true"/>
+    <arg name="use_multithread" default="true"/>
+
+
+    <include if="$(eval &quot;'$(var image_number)'>='1'&quot;)" file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/base_camera_container.launch.py">
+        <arg name="mode" value="$(var mode)"/>
+        <arg name="camera_container_name" value="front_right_camera_container"/>
+        <arg name="input_image" value="$(var image_0)"/>
+        <arg name="input_camera_info" value="$(var camera_info0)"/>
+        <arg name="yolo_type" value="$(var yolo_type)"/>
+        <arg name="label_file" value="$(var label_file)"/>
+        <arg name="gpu_id" value="$(var gpu_id_image_0)"/>
+        <arg name="use_intra_process" value="$(var use_intra_process)"/>
+        <arg name="use_multithread" value="$(var use_multithread)"/>
+        <arg name="output_topic" value="front_right_camera/rois0"/>
+    </include>
+
+    <include if="$(eval &quot;'$(var image_number)'>='2'&quot;)" file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/base_camera_container.launch.py">
+        <arg name="mode" value="$(var mode)"/>
+        <arg name="input_image" value="$(var image_1)"/>
+        <arg name="camera_container_name" value="front_camera_container"/>
+        <arg name="input_camera_info" value="$(var camera_info1)"/>
+        <arg name="yolo_type" value="$(var yolo_type)"/>
+        <arg name="label_file" value="$(var label_file)"/>
+        <arg name="gpu_id" value="$(var gpu_id_image_1)"/>
+        <arg name="use_intra_process" value="$(var use_intra_process)"/>
+        <arg name="use_multithread" value="$(var use_multithread)"/>
+        <arg name="output_topic" value="front_camera/rois1"/>
+    </include>
+
+    <include if="$(eval &quot;'$(var image_number)'>='3'&quot;)" file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/base_camera_container.launch.py">
+        <arg name="mode" value="$(var mode)"/>
+        <arg name="input_image" value="$(var image_2)"/>
+        <arg name="camera_container_name" value="front_left_camera_container"/>
+        <arg name="input_camera_info" value="$(var camera_info2)"/>
+        <arg name="yolo_type" value="$(var yolo_type)"/>
+        <arg name="label_file" value="$(var label_file)"/>
+        <arg name="gpu_id" value="$(var gpu_id_image_2)"/>
+        <arg name="use_intra_process" value="$(var use_intra_process)"/>
+        <arg name="use_multithread" value="$(var use_multithread)"/>
+        <arg name="output_topic" value="front_left_camera/rois2"/>
+    </include>
+
+    <include if="$(eval &quot;'$(var image_number)'>='4'&quot;)" file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/base_camera_container.launch.py">
+        <arg name="mode" value="$(var mode)"/>
+        <arg name="input_image" value="$(var image_3)"/>
+        <arg name="camera_container_name" value="middle_left_camera_container"/>
+        <arg name="input_camera_info" value="$(var camera_info3)"/>
+        <arg name="yolo_type" value="$(var yolo_type)"/>
+        <arg name="label_file" value="$(var label_file)"/>
+        <arg name="gpu_id" value="$(var gpu_id_image_3)"/>
+        <arg name="use_intra_process" value="$(var use_intra_process)"/>
+        <arg name="use_multithread" value="$(var use_multithread)"/>
+        <arg name="output_topic" value="middle_left_camera/rois3"/>
+    </include>
+
+
+    <include if="$(eval &quot;'$(var image_number)'>='5'&quot;)" file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/base_camera_container.launch.py">
+        <arg name="mode" value="$(var mode)"/>
+        <arg name="input_image" value="$(var image_4)"/>
+        <arg name="camera_container_name" value="middle_right_camera_container"/>
+        <arg name="input_camera_info" value="$(var camera_info4)"/>
+        <arg name="yolo_type" value="$(var yolo_type)"/>
+        <arg name="label_file" value="$(var label_file)"/>
+        <arg name="gpu_id" value="$(var gpu_id_image_4)"/>
+        <arg name="use_intra_process" value="$(var use_intra_process)"/>
+        <arg name="use_multithread" value="$(var use_multithread)"/>
+        <arg name="output_topic" value="middle_right_camera/rois4"/>
+    </include>
+
+
+    <include if="$(eval &quot;'$(var image_number)'>='6'&quot;)" file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/base_camera_container.launch.py">
+        <arg name="mode" value="$(var mode)"/>
+        <arg name="input_image" value="$(var image_5)"/>
+        <arg name="camera_container_name" value="rear_left_camera_container"/>
+        <arg name="input_camera_info" value="$(var camera_info5)"/>
+        <arg name="yolo_type" value="$(var yolo_type)"/>
+        <arg name="label_file" value="$(var label_file)"/>
+        <arg name="gpu_id" value="$(var gpu_id_image_5)"/>
+        <arg name="use_intra_process" value="$(var use_intra_process)"/>
+        <arg name="use_multithread" value="$(var use_multithread)"/>
+        <arg name="output_topic" value="rear_left_camera/rois5"/>
+    </include>
+
+
+    <include if="$(eval &quot;'$(var image_number)'>='7'&quot;)" file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/base_camera_container.launch.py">
+        <arg name="mode" value="$(var mode)"/>
+        <arg name="input_image" value="$(var image_6)"/>
+        <arg name="camera_container_name" value="rear_right_camera_container"/>
+        <arg name="input_camera_info" value="$(var camera_info6)"/>
+        <arg name="yolo_type" value="$(var yolo_type)"/>
+        <arg name="label_file" value="$(var label_file)"/>
+        <arg name="gpu_id" value="$(var gpu_id_image_6)"/>
+        <arg name="use_intra_process" value="$(var use_intra_process)"/>
+        <arg name="use_multithread" value="$(var use_multithread)"/>
+        <arg name="output_topic" value="rear_right_camera/rois6"/>
+    </include>
+
+
+    <include if="$(eval &quot;'$(var image_number)'>='8'&quot;)" file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/base_camera_container.launch.py">
+        <arg name="mode" value="$(var mode)"/>
+        <arg name="input_image" value="$(var image_7)"/>
+        <arg name="camera_container_name" value="rear_camera_container"/>
+        <arg name="input_camera_info" value="$(var camera_info7)"/>
+        <arg name="yolo_type" value="$(var yolo_type)"/>
+        <arg name="label_file" value="$(var label_file)"/>
+        <arg name="gpu_id" value="$(var gpu_id_image_7)"/>
+        <arg name="use_intra_process" value="$(var use_intra_process)"/>
+        <arg name="use_multithread" value="$(var use_multithread)"/>
+        <arg name="output_topic" value="rear_camera/rois7"/>
+    </include>
+
+</launch>
+
+

launch/tier4_perception_launch/launch/object_recognition/detection/base_camera_container.launch.py

@@ -0,0 +1,176 @@
+# # Copyright 2020 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 SetLaunchConfiguration
+from launch.conditions import IfCondition
+from launch.conditions import UnlessCondition
+from launch.substitutions.launch_configuration import LaunchConfiguration
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+from launch_ros.substitutions import FindPackageShare
+from launch.actions import OpaqueFunction
+import yaml
+
+def launch_setup(context, *args, **kwargs):
+
+    output_topic= LaunchConfiguration("output_topic").perform(context)
+
+    input_camera_info= LaunchConfiguration("input_camera_info").perform(context)
+    image_name=LaunchConfiguration("input_image").perform(context)
+    camera_container_name = LaunchConfiguration("camera_container_name").perform(context)
+    camera_ns = "/lucid_vision/" + image_name
+
+    # tensorrt params
+    gpu_id = int(LaunchConfiguration("gpu_id").perform(context))
+    mode= LaunchConfiguration("mode").perform(context)
+    calib_image_directory= FindPackageShare("tensorrt_yolo").perform(context) + "/calib_image/"
+    tensorrt_config_path = FindPackageShare('tensorrt_yolo').perform(context)+ "/config/" + LaunchConfiguration("yolo_type").perform(context) + ".param.yaml"
+
+    with open(tensorrt_config_path, "r") as f:
+            tensorrt_yaml_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+
+    camera_param_path=FindPackageShare("arena_camera").perform(context)+"/param/"+image_name+".param.yaml"
+    with open(camera_param_path, "r") as f:
+        camera_yaml_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+
+
+    container = ComposableNodeContainer(
+        name=camera_container_name,
+        namespace="/perception/object_detection",
+        package="rclcpp_components",
+        executable=LaunchConfiguration("container_executable"),
+        output="screen",
+        composable_node_descriptions=[
+            ComposableNode(
+                package="arena_camera",
+                plugin="ArenaCameraNode",
+                name="arena_camera_node",
+                parameters=[{
+                    "camera_name": camera_yaml_param['camera_name'],
+                    "frame_id": camera_yaml_param['frame_id'],
+                    "pixel_format": camera_yaml_param['pixel_format'],
+                    "serial_no": camera_yaml_param['serial_no'],
+                    "camera_info_url": camera_yaml_param['camera_info_url'],
+                    "fps": camera_yaml_param['fps'],
+                    "horizontal_binning": camera_yaml_param['horizontal_binning'],
+                    "vertical_binning": camera_yaml_param['vertical_binning'],
+                    "use_default_device_settings": camera_yaml_param['use_default_device_settings'],
+                    "exposure_auto": camera_yaml_param['exposure_auto'],
+                    "exposure_target": camera_yaml_param['exposure_target'],
+                    "gain_auto": camera_yaml_param['gain_auto'],
+                    "gain_target": camera_yaml_param['gain_target'],
+                    "gamma_target": camera_yaml_param['gamma_target'],
+                    "enable_compressing": camera_yaml_param['enable_compressing'],
+                    "enable_rectifying": camera_yaml_param['enable_rectifying'],
+                }],
+                remappings=[
+                ],
+                extra_arguments=[
+                    {"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
+                ],
+            ),
+            ComposableNode(
+                namespace=camera_ns,
+                package='image_proc',
+                plugin='image_proc::RectifyNode',
+                name='rectify_camera_image_node',
+                # Remap subscribers and publishers
+                remappings=[
+                    ('image', camera_ns+"/image"),
+                    ('camera_info', input_camera_info),
+                    ('image_rect', 'image_rect')
+                ],
+                extra_arguments=[
+                    {"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
+                ],
+            ),
+
+            ComposableNode(
+                package="tensorrt_yolo",
+                plugin="object_recognition::TensorrtYoloNodelet",
+                name="tensorrt_yolo",
+                parameters=[
+                    {
+                    "mode": mode,
+                    "gpu_id": gpu_id,
+                    "onnx_file": FindPackageShare("tensorrt_yolo").perform(context) +  "/data/" + LaunchConfiguration("yolo_type").perform(context) + ".onnx",
+                    "label_file": FindPackageShare("tensorrt_yolo").perform(context) + "/data/" + LaunchConfiguration("label_file").perform(context),
+                    "engine_file": FindPackageShare("tensorrt_yolo").perform(context) + "/data/"+ LaunchConfiguration("yolo_type").perform(context) + ".engine",
+                    "calib_image_directory": calib_image_directory,
+                    "calib_cache_file": FindPackageShare("tensorrt_yolo").perform(context) + "/data/" + LaunchConfiguration("yolo_type").perform(context) + ".cache",
+                    "num_anchors": tensorrt_yaml_param['num_anchors'],
+                    "anchors": tensorrt_yaml_param['anchors'],
+                    "scale_x_y": tensorrt_yaml_param['scale_x_y'],
+                    "score_threshold": tensorrt_yaml_param['score_threshold'],
+                    "iou_thresh": tensorrt_yaml_param['iou_thresh'],
+                    "detections_per_im": tensorrt_yaml_param['detections_per_im'],
+                    "use_darknet_layer": tensorrt_yaml_param['use_darknet_layer'],
+                    "ignore_thresh": tensorrt_yaml_param['ignore_thresh'],
+                }
+                ],
+                remappings=[
+                    ("in/image", camera_ns+"/image_rect"),
+                    ("out/objects", output_topic),
+                    ("out/image", output_topic + "/debug/image"),
+                ],
+                extra_arguments=[
+                    {"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
+                ],
+            ),
+        ],
+
+    )
+    return [container]
+
+
+def generate_launch_description():
+    launch_arguments = []
+
+    def add_launch_arg(name: str, default_value=None, description=None):
+    # a default_value of None is equivalent to not passing that kwarg at all
+        launch_arguments.append(
+            DeclareLaunchArgument(name, default_value=default_value, description=description)
+    )
+    add_launch_arg("mode","")
+    add_launch_arg("input_image","", description="input camera topic")
+    add_launch_arg("camera_container_name","")
+    add_launch_arg("input_camera_info", "", description="input camera info topic")
+    add_launch_arg("yolo_type","", description="yolo model type")
+    add_launch_arg("label_file","" ,description="tensorrt node label file")
+    add_launch_arg("gpu_id","", description="gpu setting")
+    add_launch_arg("use_intra_process", "", "use intra process")
+    add_launch_arg("use_multithread", "", "use multithread")
+
+
+
+    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 launch.LaunchDescription(
+        launch_arguments
+        + [set_container_executable, set_container_mt_executable]
+        + [OpaqueFunction(function=launch_setup)]
+    )
+