Support panorama detection

jsk_spot_robot/jsk_spot_driver.rosinstall

@@ -86,3 +86,7 @@
 - git:
     local-name: ublox
     uri: https://github.com/KumarRobotics/ublox.git
+# Panoram image recognition need coral_usb, which is not release as apt package.
+- git:
+    local-name: jsk-ros-pkg/coral_usb_ros
+    uri: https://github.com/jsk-ros-pkg/coral_usb_ros.git

jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch

@@ -26,6 +26,13 @@
     <remap from="tf" to="/tf"/>
   </node>
 
+  <node
+    pkg="tf"
+    type="static_transform_publisher"
+    name="insta360_link_publisher"
+    args="-0.3 0 0.4 0 0 0 base_link insta360_link 100"
+  />
+
   <node name="cmd_vel_smoother" pkg="cmd_vel_smoother" type="cmd_vel_smoother">
     <remap from="input" to="spot/cmd_vel_unsmoothed" />
     <remap from="output" to="spot/cmd_vel" />

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>

jsk_spot_robot/jsk_spot_startup/launch/include/peripheral.launch

@@ -1,2 +1,8 @@
 <launch>
+    <include file="$(find jsk_perception)/sample/sample_insta360_air.launch">
+        <arg name="width" value="2176"/>
+        <arg name="height" value="1088"/>
+        <arg name="panorama_resolution_mode" value="low"/>
+        <arg name="gui" value="false"/>
+    </include>
 </launch>

jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch

@@ -1,8 +1,9 @@
 <launch>
   <arg name="credential_config" default="/var/lib/robot/credentials/spot_credential.yaml" />
   <arg name="launch_robot_state_publisher" default="true" />
-  <arg name="launch_peripheral" default="false" />
+  <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" />
@@ -31,4 +32,8 @@
       <arg name="token" value="/var/lib/robot/credentials/switchbot_token.txt"/>
   </include>
 
+  <!-- panorama object detection -->
+  <include file="$(find jsk_spot_startup)/launch/include/panorama_detection.launch"
+           if="$(arg launch_panorama_detection)" />
+
 </launch>

jsk_spot_robot/jsk_spot_startup/package.xml

@@ -21,6 +21,7 @@
   <exec_depend>audio_capture</exec_depend>
   <exec_depend>cmd_vel_smoother</exec_depend>
   <exec_depend>compressed_image_transport</exec_depend>
+  <exec_depend>coral_usb</exec_depend>
   <exec_depend>image_transport</exec_depend>
   <exec_depend>interactive_marker_twist_server</exec_depend>
   <exec_depend>jsk_perception</exec_depend>