From 80d27105936c141ddc57fe8afb64a5f058fca6bc Mon Sep 17 00:00:00 2001
From: Koki Shinjo <sktometometo@gmail.com>
Date: Wed, 29 Nov 2023 15:24:05 +0900
Subject: [PATCH] [jsk_spot_startup] add coral usb panorama detection

---
 .../launch/include/panorama_detection.launch  | 58 +++++++++++++++++++
 .../launch/jsk_spot_bringup.launch            |  5 ++
 2 files changed, 63 insertions(+)
 create mode 100644 jsk_spot_robot/jsk_spot_startup/launch/include/panorama_detection.launch

diff --git a/jsk_spot_robot/jsk_spot_startup/launch/include/panorama_detection.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/panorama_detection.launch
new file mode 100644
index 0000000000..0def9ef903
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/launch/include/panorama_detection.launch
@@ -0,0 +1,58 @@
+<launch>
+  <arg name="INPUT_PANORAMA_IMAGE" default="/dual_fisheye_to_panorama/output" />
+  <arg name="INPUT_PANORAMA_INFO" default="/dual_fisheye_to_panorama/panorama_info" />
+
+  <!-- edge detection parameter -->
+  <arg name="model_file" default="$(find coral_usb)/models/mobilenet_ssd_v2_coco_quant_postprocess_edgetpu.tflite"/>
+  <arg name="label_file" default="$(find coral_usb)/models/coco_labels.txt"/>
+  <arg name="device_id" default="0" />
+  <arg name="n_split" default="3" />
+  <arg name="overlap" default="true" />
+
+  <arg name="TOPIC_OBJ_CLASS" default="/spot_recognition/class" />
+  <arg name="TOPIC_OBJ_RECTS" default="/spot_recognition/rects" />
+
+  <!-- EdgeTPU object detection -->
+  <node
+      pkg="coral_usb"
+      type="edgetpu_panorama_object_detector.py"
+      name="edgetpu_panorama_object_detector"
+      output="screen"
+      respawn="true"
+      >
+    <remap from="~input" to="$(arg INPUT_PANORAMA_IMAGE)" />
+    <remap from="~output/class" to="$(arg TOPIC_OBJ_CLASS)" />
+    <remap from="~output/rects" to="$(arg TOPIC_OBJ_RECTS)" />
+    <rosparam subst_value="true" >
+      score_thresh: 0.6
+      model_file: $(arg model_file)
+      label_file: $(arg label_file)
+      image_transport: raw
+      device_id: $(arg device_id)
+      n_split: $(arg n_split)
+      overlap: $(arg overlap)
+      enable_visualization: false
+    </rosparam>
+  </node>
+
+  <!-- rects 2 bounding box array -->
+  <node
+      pkg="jsk_perception"
+      type="rect_array_in_panorama_to_bounding_box_array.py"
+      name="rect_array_in_panorama_to_bounding_box_array"
+      output="screen"
+      >
+    <remap from="~panorama_image" to="/dual_fisheye_to_panorama/output" />
+    <remap from="~panorama_info" to="/dual_fisheye_to_panorama/panorama_info" />
+    <remap from="~input_class" to="$(arg TOPIC_OBJ_CLASS)" />
+    <remap from="~input_rects" to="$(arg TOPIC_OBJ_RECTS)" />
+    <remap from="~bbox_array" to="/spot_recognition/bbox_array" />
+    <rosparam subst_value="true">
+        frame_fixed: "odom"
+        dimensions_labels:
+            person: [0.5, 0.5, 1.5]
+            car: [4.0, 4.0, 2.0]
+            truck: [4.0, 4.0, 3.0]
+    </rosparam>
+  </node>
+</launch>
diff --git a/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch b/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch
index 448c4468a2..c1dd3ada07 100644
--- a/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch
+++ b/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch
@@ -3,6 +3,7 @@
   <arg name="launch_robot_state_publisher" default="true" />
   <arg name="launch_peripheral" default="true" />
   <arg name="launch_interaction" default="false" />
+  <arg name="launch_panorama_detection" default="true" />
 
   <arg name="use_driver" default="true" />
   <arg name="use_app_manager" default="true" />
@@ -27,4 +28,8 @@
   <include file="$(find jsk_spot_startup)/launch/include/interaction.launch"
 	   if="$(arg launch_interaction)" />
 
+  <!-- panorama object detection -->
+  <include file="$(find jsk_spot_startup)/launch/include/panorama_detection.launch"
+           if="$(arg launch_panorama_detection)" />
+
 </launch>