[fetch] Update go-to-kitchen demo

jsk_fetch_robot/jsk_fetch.rosinstall.indigo

@@ -22,11 +22,11 @@
     uri: https://github.com/jsk-ros-pkg/jsk_robot.git
     version: master
 # to pass build of jsk_robot
-# remove after 2.2.10 is released
+# remove after 2.2.11 is released
 - git:
     local-name: jsk-ros-pkg/jsk_common
     uri: https://github.com/jsk-ros-pkg/jsk_common.git
-    version: 2.2.10
+    version: 799fd309c1519801fcb3a37c9094814004d78594
 # to avoid volume 0 problem
 # remove after 0.3.14 (https://github.com/jsk-ros-pkg/jsk_pr2eus/commit/41183fe3401d742bbec0edd13b67cb909a6968bd) is released
 - git:
@@ -39,7 +39,7 @@
 - git:
     local-name: jsk-ros-pkg/jsk_demos
     uri: https://github.com/jsk-ros-pkg/jsk_demos.git
-    version: 810acc7
+    version: 7c429715d0adf12c5dd34459ceb4b0a5b11dec6c
 # jsk_topic_tools requires nodelet gte 1.9.11
 # remove after 1.9.11 is released by apt
 - git:
@@ -64,7 +64,7 @@
 - git:
     local-name: jsk-ros-pkg/jsk_3rdparty
     uri: https://github.com/jsk-ros-pkg/jsk_3rdparty.git
-    version: 82e897dcbdcd6aa0cbd126fa122d4dbdc9df67c9
+    version: f2b2bd8ac30a8539a6e099dfe7f1e2830ca12430
 # Use joy/joy_remap.py
 - git:
     local-name: ros-drivers/joystick_drivers
@@ -103,3 +103,13 @@
     local-name: fetchrobotics/fetch_open_auto_dock
     uri: https://github.com/fetchrobotics/fetch_open_auto_dock.git
     version: 0.1.2
+# indigo is already EOL and visualization_msgs is never released
+- tar:
+    local-name: ros/common_msgs/visualization_msgs
+    uri: https://github.com/ros-gbp/common_msgs-release/archive/release/kinetic/visualization_msgs/1.12.7-0.tar.gz
+    version: common_msgs-release-release-kinetic-visualization_msgs-1.12.7-0
+# catkin_virtualenv is required in jsk_3rdparty
+- git:
+    local-name: locusrobotics/catkin_virtualenv
+    uri: https://github.com/locusrobotics/catkin_virtualenv.git
+    version: 0.5.0

jsk_fetch_robot/jsk_fetch_startup/apps/go_to_kitchen/go_to_kitchen.app

@@ -3,6 +3,7 @@ platform: fetch
 launch: jsk_fetch_startup/go_to_kitchen.xml
 interface: jsk_fetch_startup/go_to_kitchen.interface
 icon: jsk_fetch_startup/go_to_kitchen.png
+timeout: 1200
 plugins:
   - name: service_notification_saver_plugin
     type: app_notification_saver/service_notification_saver
@@ -24,19 +25,34 @@ plugins:
       video_framerate: 30
       video_encoding: RGB
   - name: object_detection_video_recorder_plugin
-    type: app_recorder/video_recorder_plugin
+    type: app_recorder/audio_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
+      audio_topic_name: /audio
+      audio_channels: 1
+      audio_sample_rate: 16000
+      audio_format: wave
+      audio_sample_format: S16LE
+      video_topic_name: /edgetpu_object_detector/output/image
+      video_height: 480
+      video_width: 640
+      video_framerate: 10
+      video_encoding: RGB
   - name: panorama_video_recorder_plugin
     type: app_recorder/video_recorder_plugin
     launch_args:
       video_path: /tmp
       video_title: go_to_kitchen_panorama.avi
       video_topic_name: /dual_fisheye_to_panorama/output
       video_fps: 1.0
+  - name: rviz_video_recorder_plugin
+    type: app_recorder/video_recorder_plugin
+    launch_args:
+      video_path: /tmp
+      video_title: go_to_kitchen_rviz.avi
+      video_topic_name: /rviz/image
+      video_fps: 30.0
   - name: respeaker_audio_recorder_plugin
     type: app_recorder/audio_recorder_plugin
     launch_args:
@@ -74,6 +90,9 @@ plugins:
         - /head_camera/depth_registered/throttled/camera_info
         - /head_camera/rgb/throttled/image_rect_color/compressed
         - /head_camera/depth_registered/throttled/image_rect/compressedDepth
+        - /server_name/smach/container_init
+        - /server_name/smach/container_status
+        - /server_name/smach/container_structure
         - /audio
         - /rviz/throttled/image/compressed
   - name: result_recorder_plugin
@@ -89,17 +108,26 @@ plugins:
         - /tmp/go_to_kitchen_head_camera.avi
         - /tmp/go_to_kitchen_object_detection.avi
         - /tmp/go_to_kitchen_panorama.avi
+        - /tmp/go_to_kitchen_rviz.avi
         - /tmp/go_to_kitchen_audio.wav
         - /tmp/go_to_kitchen_rosbag.bag
       upload_file_titles:
         - go_to_kitchen_result.yaml
         - go_to_kitchen_head_camera.avi
         - go_to_kitchen_object_detection.avi
         - go_to_kitchen_panorama.avi
+        - go_to_kitchen_rviz.avi
         - go_to_kitchen_audio.wav
         - go_to_kitchen_rosbag.bag
       upload_parents_path: fetch_go_to_kitchen
       upload_server_name: /gdrive_server
+  - name: tweet_notifier_plugin
+    type: app_notifier/tweet_notifier_plugin
+    plugin_args:
+      client_name: /tweet_image_server/tweet
+      image: true
+      image_topic_name: /edgetpu_object_detector/output/image
+      warning: false
   - name: speech_notifier_plugin
     type: app_notifier/speech_notifier_plugin
     plugin_args:
@@ -135,10 +163,12 @@ plugin_order:
     - head_camera_video_recorder_plugin
     - object_detection_video_recorder_plugin
     - panorama_video_recorder_plugin
+    - rviz_video_recorder_plugin
     - respeaker_audio_recorder_plugin
     - rosbag_recorder_plugin
     - result_recorder_plugin
     - gdrive_uploader_plugin
+    - tweet_notifier_plugin
     - speech_notifier_plugin
     - mail_notifier_plugin
     - shutdown_plugin
@@ -149,10 +179,12 @@ plugin_order:
     - head_camera_video_recorder_plugin
     - object_detection_video_recorder_plugin
     - panorama_video_recorder_plugin
+    - rviz_video_recorder_plugin
     - respeaker_audio_recorder_plugin
     - rosbag_recorder_plugin
     - result_recorder_plugin
     - gdrive_uploader_plugin
+    - tweet_notifier_plugin
     - speech_notifier_plugin
     - mail_notifier_plugin
     - shutdown_plugin

jsk_fetch_robot/jsk_fetch_startup/apps/go_to_kitchen/go_to_kitchen.xml

@@ -1,8 +1,19 @@
 <launch>
-  <node name="go_to_kitchen" pkg="roseus" type="roseus"
-        args="$(find jsk_fetch_startup)/euslisp/go-to-kitchen.l"/>
+  <node name="go_to_kitchen" pkg="roseus" type="roseus" required="true" output="screen"
+        args="$(find jsk_fetch_startup)/euslisp/go-to-kitchen.l" />
 
   <!-- launch rviz with throttle -->
   <include file="$(find jsk_fetch_startup)/launch/rviz_record.launch" />
 
+  <node name="go_to_kitchen_object_detector_score_thresh" pkg="dynamic_reconfigure" type="dynparam"
+        args="set_from_parameters edgetpu_object_detector">
+    <param name="score_thresh" type="double" value="0.60" /> <!-- default: 0.6 -->
+  </node>
+
+  <node name="$(anon rviz)" pkg="rviz" type="rviz"
+        args="-d $(find jsk_fetch_startup)/config/jsk_startup_record.rviz">
+    <env name="DISPLAY" value=":0" />
+    <env name="HOME" value="/home/fetch" />
+  </node>
+
 </launch>

jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l

@@ -115,7 +115,7 @@
   (simple-dock))
 
 
-(defun undock ()
+(defun undock (&key (rotate-in-place nil))
   (unless *undock-action*
     (setq *undock-action*
           (instance ros::simple-action-client :init
@@ -124,7 +124,8 @@
     (ros::ros-error "/undock action server is not started")
     (return-from undock nil))
   (send *undock-action* :send-goal
-        (instance fetch_auto_dock_msgs::UndockActionGoal :init))
+        (instance fetch_auto_dock_msgs::UndockActionGoal :init
+                  :goal (instance fetch_auto_dock_msgs::UndockGoal :rotate_in_place rotate-in-place)))
   (unless (send *undock-action* :wait-for-result :timeout 60)
     (ros::ros-error "No result returned from /undock action server")
     (return-from undock nil))
@@ -167,10 +168,7 @@ Args:
             (return-from go-to-spot-undock t))
           (if (equal battery-charging-state :charging)
             (progn
-              (setq undock-success (auto-undock :n-trial 3))
-              ;; rotate after undock
-              (if (and undock-success undock-rotate)
-                (send *ri* :go-pos-unsafe 0 0 180)))
+              (setq undock-success (auto-undock :n-trial 3 :rotate-in-place undock-rotate)))
             (return-from go-to-spot-undock t))
           (if (not undock-success)
             (progn
@@ -200,14 +198,14 @@ Args:
     success))
 
 
-(defun auto-undock (&key (n-trial 1))
+(defun auto-undock (&key (n-trial 1) (rotate-in-place nil))
   (let ((success nil))
     (unless (boundp '*ri*)
       (require :fetch-interface "package://fetcheus/fetch-interface.l")
       (fetch-init))
     (dotimes (i n-trial)
       (ros::ros-info "trying to do undock.")
-      (setq success (undock))
+      (setq success (undock :rotate-in-place rotate-in-place))
       (when success (return-from auto-undock success)))
     (if (not success)
       (let ((enable-request (instance power_msgs::BreakerCommandRequest :init :enable t))

jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch

@@ -97,6 +97,11 @@
   <!-- twitter -->
   <include file="$(find jsk_fetch_startup)/launch/fetch_tweet.launch" />
 
+  <!-- switchbot -->
+  <include file="$(find switchbot_ros)/launch/switchbot.launch">
+    <arg name="token" value="/var/lib/robot/switchbot_token.txt" />
+  </include>
+
   <!-- app manager -->
   <include file="$(find jsk_robot_startup)/lifelog/app_manager.launch">
     <arg name="use_applist" value="false" /> <!-- use plugin -->

jsk_fetch_robot/jsk_fetch_startup/launch/fetch_insta360_melodic.launch

@@ -8,6 +8,7 @@
     <!-- $ v4l2-ctl -d /dev/insta360 -\-list-formats-ext -->
     <arg name="height" value="736" />
     <arg name="width" value="1472" />
+    <arg name="throttle" value="true" />
     <!-- for melodic -->
     <arg name="use_usb_cam" value="false" />
   </include>

jsk_fetch_robot/jsk_fetch_startup/launch/fetch_tweet.launch

@@ -1,11 +1,10 @@
 <launch>
-
   <include file="$(find jsk_fetch_startup)/jsk_fetch.machine" />
 
   <include file="$(find jsk_robot_startup)/lifelog/tweet.launch">
     <arg name="robot_name" value="fetch15"/>
 
-    <arg name="image_topics" default="/head_camera/rgb/image_rect_color"/>
+    <arg name="image_topics" default="/head_camera/rgb/image_rect_color /edgetpu_object_detector/output/image /edgetpu_human_pose_estimator/output/image"/>
     <arg name="machine" value="localhost"/>
     <arg name="output" value="screen"/>
     <arg name="motor_subscribe" value="true"/>

jsk_fetch_robot/jsk_fetch_startup/launch/navigation/fetch1075/fetch_move_base_common_params.yaml

@@ -0,0 +1,36 @@
+move_base:
+  recovery_behavior_enabled: true
+  recovery_behaviors:
+    - name: "conservative_reset"
+      type: "clear_costmap_recovery/ClearCostmapRecovery"
+    - name: "rotate_recovery0"
+      type: "rotate_recovery/RotateRecovery"
+    - name: "speak_and_wait0"
+      type: "speak_and_wait_recovery/SpeakAndWaitRecovery"
+    - name: "aggressive_reset"
+      type: "clear_costmap_recovery/ClearCostmapRecovery"
+    - name: "rotate_recovery1"
+      type: "rotate_recovery/RotateRecovery"
+    - name: "speak_and_wait1"
+      type: "speak_and_wait_recovery/SpeakAndWaitRecovery"
+    - name: "all_reset"
+      type: "clear_costmap_recovery/ClearCostmapRecovery"
+    - name: "rotate_recovery2"
+      type: "rotate_recovery/RotateRecovery"
+    - name: "move_slow_and_clear"
+      type: "move_slow_and_clear/MoveSlowAndClear"
+  speak_and_wait0:
+    speak_text: "とおれません、みちをあけてください"
+    duration_wait: 5.0
+    duration_timeout: 1.0
+    sound_action: /robotsound_jp
+  speak_and_wait1:
+    speak_text: "とおれません、みちをあけてください"
+    duration_wait: 5.0
+    duration_timeout: 1.0
+    sound_action: /robotsound_jp
+  move_slow_and_clear:
+    planner_namespace: TrajectoryPlannerROS
+    max_trans_param_name: max_vel_x
+    max_rot_param_name: max_vel_theta
+  max_planning_retries: 2

jsk_fetch_robot/jsk_fetch_startup/launch/navigation/fetch15/fetch_move_base_common_params.yaml

@@ -22,6 +22,9 @@ move_base:
   # base_local_planner: base_local_planner/TrajectoryPlannerROS
   GlobalPlanner:
     allow_unknown: true
+    neural_cost: 50
+    lethal_cost: 253
+    cost_factor: 0.8
     default_tolerance: 0.0
     orientation_mode: 1
     visualize_potential: false
@@ -86,7 +89,7 @@ move_base:
     weight_acc_lim_x: 1
     weight_acc_lim_theta: 1
     weight_kinematics_nh: 1000
-    weight_kinematics_forward_drive: 1
+    weight_kinematics_forward_drive: 1000
     weight_kinematics_turning_radius: 100
     weight_optimaltime: 1
     weight_obstacle: 50
@@ -110,10 +113,14 @@ move_base:
       type: "clear_costmap_recovery/ClearCostmapRecovery"
     - name: "rotate_recovery0"
       type: "rotate_recovery/RotateRecovery"
+    - name: "speak_and_wait0"
+      type: "speak_and_wait_recovery/SpeakAndWaitRecovery"
     - name: "aggressive_reset"
       type: "clear_costmap_recovery/ClearCostmapRecovery"
     - name: "rotate_recovery1"
       type: "rotate_recovery/RotateRecovery"
+    - name: "speak_and_wait1"
+      type: "speak_and_wait_recovery/SpeakAndWaitRecovery"
     - name: "all_reset"
       type: "clear_costmap_recovery/ClearCostmapRecovery"
     - name: "rotate_recovery2"
@@ -125,11 +132,21 @@ move_base:
   rotate_recovery0:
     frequency: 20.0
     sim_granularity: 0.017
+  speak_and_wait0:
+    speak_text: "とおれません、みちをあけてください"
+    duration_wait: 5.0
+    duration_timeout: 1.0
+    sound_action: /robotsound_jp
   aggressive_reset:
     reset_distance: 1.0 # 0.5
   rotate_recovery1:
     frequency: 20.0
     sim_granularity: 0.017
+  speak_and_wait1:
+    speak_text: "とおれません、みちをあけてください"
+    duration_wait: 5.0
+    duration_timeout: 1.0
+    sound_action: /robotsound_jp
   all_reset:
     reset_distance: 0.0
   rotate_recovery2:
@@ -140,7 +157,7 @@ move_base:
     limited_trans_speed: 0.3
     limited_rot_speed: 0.8
     limited_distance: 0.3
-    planner_namespace: TrajectoryPlannerROS
+    planner_namespace: TebLocalPlannerROS
     max_trans_param_name: max_vel_x
     max_rot_param_name: max_vel_theta
   max_planning_retries: 2

jsk_fetch_robot/jsk_fetch_startup/launch/rosbag_record.launch

@@ -35,6 +35,9 @@
           /head_camera/depth_registered/throttled/camera_info
           /head_camera/rgb/throttled/image_rect_color/compressed
           /head_camera/depth_registered/throttled/image_rect/compressedDepth
+          /server_name/smach/container_init
+          /server_name/smach/container_status
+          /server_name/smach/container_structure
           /audio
           $(arg other_topics)
           $(arg regex_flag)

jsk_fetch_robot/jsk_fetch_startup/package.xml

@@ -25,6 +25,7 @@
   <run_depend>pr2_computer_monitor</run_depend>
   <run_depend>robot_pose_publisher</run_depend>
   <run_depend>rviz</run_depend>
+  <run_depend>switchbot_ros</run_depend>
   <run_depend>touchegg</run_depend>
   <run_depend>tf2_ros</run_depend>
   <run_depend>voice_text</run_depend>
@@ -41,6 +42,7 @@
   <run_depend>librealsense2</run_depend>
   <run_depend>realsense2_camera</run_depend>
   <run_depend>realsense2_description</run_depend>
+  <run_depend>teb_local_planner</run_depend>
 
   <!-- for fetch teleop -->
   <run_depend>app_manager</run_depend>

jsk_fetch_robot/jsk_fetch_startup/scripts/correct_position.py

@@ -21,7 +21,8 @@ def __init__(self):
         if robot_name == 'fetch15':
             dock_name = '/eng2/7f/room73B2-fetch-dock-front'
         elif robot_name == 'fetch1075':
-            dock_name = '/eng2/7f/room73B2-fetch-dock-entrance'
+            dock_name = '/eng2/7f/room73B2-fetch-dock2-front'
+            # dock_name = '/eng2/7f/room73B2-fetch-dock-entrance'
         for spot in spots.markers:
             if spot.text == dock_name:
                 self.dock_pose = spot.pose