From 9070d468d0eec7ca2fa9d40f600b6982f91eb84f Mon Sep 17 00:00:00 2001
From: Manato Hirabayashi <3022416+manato@users.noreply.github.com>
Date: Fri, 29 Sep 2023 15:31:13 +0900
Subject: [PATCH] feat: add `live_sensor` and `perception` arguments (#8)

Signed-off-by: Manato HIRABAYASHI <manato.hirabayashi@tier4.jp>
---
 .../launch/edge_auto_jetson.launch.xml        |  6 ++++
 .../image_transport_decompressor.launch.xml   | 34 +++++++++++++++++++
 .../launch/perception_jetson0.launch.xml      | 26 +++++++++++---
 edge_auto_jetson_launch/package.xml           |  3 +-
 4 files changed, 64 insertions(+), 5 deletions(-)
 create mode 100644 edge_auto_jetson_launch/launch/image_transport_decompressor.launch.xml

diff --git a/edge_auto_jetson_launch/launch/edge_auto_jetson.launch.xml b/edge_auto_jetson_launch/launch/edge_auto_jetson.launch.xml
index 8fb18ae..8b5fd6b 100644
--- a/edge_auto_jetson_launch/launch/edge_auto_jetson.launch.xml
+++ b/edge_auto_jetson_launch/launch/edge_auto_jetson.launch.xml
@@ -1,8 +1,14 @@
 <launch>
   <arg name="vehicle_id" default="$(env VEHICLE_ID default)" />
   <arg name="jetson_id" default="$(env JETSON_ID 0)" />
+  <arg name="live_sensor" default="True"
+       description="If true, camera drivers will be booted. Otherwise, image decompressor will be."/>
+  <arg name="perception" default="True"
+       description="If true, series of perception stack will be booted."/>
 
   <include file="$(find-pkg-share edge_auto_jetson_launch)/launch/perception_jetson$(var jetson_id).launch.xml">
     <arg name="vehicle_id" value="$(var vehicle_id)" />
+    <arg name="live_sensor" value="$(var live_sensor)" />
+    <arg name="perception" value="$(var perception)" />
   </include>
 </launch>
diff --git a/edge_auto_jetson_launch/launch/image_transport_decompressor.launch.xml b/edge_auto_jetson_launch/launch/image_transport_decompressor.launch.xml
new file mode 100644
index 0000000..ae76a17
--- /dev/null
+++ b/edge_auto_jetson_launch/launch/image_transport_decompressor.launch.xml
@@ -0,0 +1,34 @@
+<launch>
+  <!-- image topic name to be subscribed -->
+  <arg name="input/compressed_image" default="image_raw/compressed" />
+  <!-- image topic name to be published -->
+  <arg name="output/raw_image" default="image_raw" />
+
+  <!-- container naem that this ROS node to be loaded -->
+  <arg name="container_name" default="" />
+  <!-- flag to use ROS2 intra process -->
+  <arg name="use_intra_process" default="True" />
+
+
+  <let name="empty_container_is_specified" value="$(eval 'not &quot;$(var container_name)&quot;')" />
+  <!-- If container name is not specified,
+       execute function as an individual node  -->
+  <group if="$(var empty_container_is_specified)">
+    <node pkg="image_transport_decompressor" exec="image_transport_decompressor_node" name="image_transport_decompressor">
+      <remap from="~/input/compressed_image" to="$(var input/compressed_image)" />
+      <remap from="~/output/raw_image" to="$(var output/raw_image)" />
+    </node>
+  </group>
+
+  <!-- If container name is specified,
+       execute function as a composable node and load it into the container  -->
+  <group unless="$(var empty_container_is_specified)">
+    <load_composable_node target="$(var container_name)">
+      <composable_node pkg="image_transport_decompressor" plugin="image_preprocessor::ImageTransportDecompressor" name="image_transport_decompressor">
+        <remap from="~/input/compressed_image" to="$(var input/compressed_image)" />
+        <remap from="~/output/raw_image" to="$(var output/raw_image)" />
+        <extra_arg name="use_intra_process_comms" value="$(var use_intra_process)" />
+      </composable_node>
+    </load_composable_node>
+  </group>
+</launch>
diff --git a/edge_auto_jetson_launch/launch/perception_jetson0.launch.xml b/edge_auto_jetson_launch/launch/perception_jetson0.launch.xml
index 20d4bea..beb904f 100644
--- a/edge_auto_jetson_launch/launch/perception_jetson0.launch.xml
+++ b/edge_auto_jetson_launch/launch/perception_jetson0.launch.xml
@@ -1,5 +1,7 @@
 <launch>
   <arg name="vehicle_id" />
+  <arg name="live_sensor" default="True"/>
+  <arg name="perception" default="True"/>
 
   <!-- camera0 -->
   <group>
@@ -11,7 +13,7 @@
     <node pkg="rclcpp_components" exec="component_container_mt" name="$(var container_name)" namespace="/"/>
 
     <!-- sensing -->
-    <group>
+    <group if="$(var live_sensor)">
       <push-ros-namespace namespace="sensing/camera/$(var camera_name)" />
       <include file="$(find-pkg-share edge_auto_jetson_launch)/launch/v4l2_camera.launch.xml">
         <arg name="container_name" value="$(var container_name)" />
@@ -26,9 +28,17 @@
         <param from="$(find-pkg-share individual_params)/config/$(var vehicle_id)/$(var camera_name)/trigger.param.yaml"/>
       </node>
     </group>
+    <group unless="$(var live_sensor)">
+      <push-ros-namespace namespace="sensing/camera/$(var camera_name)" />
+      <include file="$(find-pkg-share edge_auto_jetson_launch)/launch/image_transport_decompressor.launch.xml">
+        <arg name="container_name" value="$(var container_name)" />
+        <arg name="input/compressed_image" value="image_raw/compressed" />
+        <arg name="output/raw_image" value="image_raw" />
+      </include>
+    </group>
 
     <!-- perception -->
-    <group>
+    <group if="$(var perception)">
       <push-ros-namespace namespace="perception/object_recognition/detection" />
       <include file="$(find-pkg-share edge_auto_jetson_launch)/launch/tensorrt_yolox.launch.xml">
         <arg name="container_name" value="$(var container_name)" />
@@ -56,7 +66,7 @@
     <node pkg="rclcpp_components" exec="component_container_mt" name="$(var container_name)" namespace="/"/>
 
     <!-- sensing -->
-    <group>
+    <group if="$(var live_sensor)">
       <push-ros-namespace namespace="sensing/camera/$(var camera_name)" />
       <include file="$(find-pkg-share edge_auto_jetson_launch)/launch/v4l2_camera.launch.xml">
         <arg name="container_name" value="$(var container_name)" />
@@ -71,9 +81,17 @@
         <param from="$(find-pkg-share individual_params)/config/$(var vehicle_id)/$(var camera_name)/trigger.param.yaml"/>
       </node>
     </group>
+    <group unless="$(var live_sensor)">
+      <push-ros-namespace namespace="sensing/camera/$(var camera_name)" />
+      <include file="$(find-pkg-share edge_auto_jetson_launch)/launch/image_transport_decompressor.launch.xml">
+        <arg name="container_name" value="$(var container_name)" />
+        <arg name="input/compressed_image" value="image_raw/compressed" />
+        <arg name="output/raw_image" value="image_raw" />
+      </include>
+    </group>
 
     <!-- perception -->
-    <group>
+    <group if="$(var perception)">
       <push-ros-namespace namespace="perception/object_recognition/detection" />
       <include file="$(find-pkg-share edge_auto_jetson_launch)/launch/tensorrt_yolox.launch.xml">
         <arg name="container_name" value="$(var container_name)" />
diff --git a/edge_auto_jetson_launch/package.xml b/edge_auto_jetson_launch/package.xml
index 704f308..134b498 100644
--- a/edge_auto_jetson_launch/package.xml
+++ b/edge_auto_jetson_launch/package.xml
@@ -18,7 +18,8 @@
   <exec_depend>tensorrt_yolox</exec_depend>
   <exec_depend>bytetrack</exec_depend>
   <exec_depend>intrinsic_camera_calibrator</exec_depend>
-  
+  <exec_depend>image_transport_decompressor</exec_depend>
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>