From 0a4af00dea315b9e26921296e6a383d12a98426d Mon Sep 17 00:00:00 2001
From: Shingo Kitagawa <shingogo@hotmail.co.jp>
Date: Thu, 17 Sep 2020 16:12:16 +0900
Subject: [PATCH] re-enable object detection video with throttled rgb images

---
 .../apps/go_to_kitchen/go_to_kitchen.app      | 22 +++++++++----------
 .../apps/go_to_kitchen/go_to_kitchen.xml      | 12 ++++++++++
 2 files changed, 23 insertions(+), 11 deletions(-)

diff --git a/jsk_fetch_robot/jsk_fetch_startup/apps/go_to_kitchen/go_to_kitchen.app b/jsk_fetch_robot/jsk_fetch_startup/apps/go_to_kitchen/go_to_kitchen.app
index 34a628c97c..820177e3b1 100644
--- a/jsk_fetch_robot/jsk_fetch_startup/apps/go_to_kitchen/go_to_kitchen.app
+++ b/jsk_fetch_robot/jsk_fetch_startup/apps/go_to_kitchen/go_to_kitchen.app
@@ -12,13 +12,13 @@ plugins:
       video_topic_name: /head_camera/rgb/throttled/image_rect_color
       video_fps: 5.0
       video_codec: XVID
-#   - name: object_detection_video_recorder_plugin
-#     type: app_recorder/video_recorder_plugin
-#     launch_args:
-#       video_path: /tmp
-#       video_title: go_to_kitchen_object_detection.avi
-#       video_topic_name: /edgetpu_object_detector/output/image
-#       video_fps: 15.0
+  - name: object_detection_video_recorder_plugin
+    type: app_recorder/video_recorder_plugin
+    launch_args:
+      video_path: /tmp
+      video_title: go_to_kitchen_object_detection.avi
+      video_topic_name: /edgetpu_object_detector_visualization/output
+      video_fps: 5.0
   - name: rosbag_recorder_plugin
     type: app_recorder/rosbag_recorder_plugin
     launch_args:
@@ -55,11 +55,11 @@ plugins:
     plugin_args:
       upload_file_paths:
         - /tmp/go_to_kitchen_head_camera.avi
-        # - /tmp/go_to_kitchen_object_detection.avi
+        - /tmp/go_to_kitchen_object_detection.avi
         - /tmp/go_to_kitchen_rosbag.bag
       upload_file_titles:
         - go_to_kitchen_head_camera.avi
-        # - go_to_kitchen_object_detection.avi
+        - go_to_kitchen_object_detection.avi
         - go_to_kitchen_rosbag.bag
       upload_parents_path: fetch_go_to_kitchen
       upload_server_name: /gdrive_server
@@ -76,14 +76,14 @@ plugins:
 plugin_order:
   start_plugin_order:
     - head_camera_video_recorder_plugin
-    # - object_detection_video_recorder_plugin
+    - object_detection_video_recorder_plugin
     - rosbag_recorder_plugin
     - gdrive_uploader_plugin
     - speech_notifier_plugin
     - mail_notifier_plugin
   stop_plugin_order:
     - head_camera_video_recorder_plugin
-    # - object_detection_video_recorder_plugin
+    - object_detection_video_recorder_plugin
     - rosbag_recorder_plugin
     - gdrive_uploader_plugin
     - speech_notifier_plugin
diff --git a/jsk_fetch_robot/jsk_fetch_startup/apps/go_to_kitchen/go_to_kitchen.xml b/jsk_fetch_robot/jsk_fetch_startup/apps/go_to_kitchen/go_to_kitchen.xml
index bb418e8855..74272166f5 100644
--- a/jsk_fetch_robot/jsk_fetch_startup/apps/go_to_kitchen/go_to_kitchen.xml
+++ b/jsk_fetch_robot/jsk_fetch_startup/apps/go_to_kitchen/go_to_kitchen.xml
@@ -1,4 +1,16 @@
 <launch>
   <node name="go_to_kitchen" pkg="roseus" type="roseus" required="true" output="screen"
         args="$(find jsk_fetch_startup)/euslisp/go-to-kitchen.l" />
+
+  <node name="edgetpu_object_detector_visualization" pkg="jsk_perception"
+        type="draw_rects" output="screen" respawn="true">
+    <remap from="~input" to="/head_camera/rgb/throttled/image_rect_color" />
+    <remap from="~input/rects" to="/edgetpu_object_detector/output/rects" />
+    <remap from="~input/class" to="/edgetpu_object_detector/output/class" />
+    <rosparam>
+      approximate_sync: true
+      queue_size: 100
+      use_classification_result: true
+    </rosparam>
+  </node>
 </launch>