Merge pull request #1357 from knorth55/update-fridge-demo

[detect_cans_in_fridge_201202] update fridge demo

detect_cans_in_fridge_201202/CMakeLists.txt

@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(detect_cans_in_fridge_201202)
 
-find_package(catkin REQUIRED roseus)
+find_package(catkin REQUIRED)
 
 catkin_package(
     DEPENDS

detect_cans_in_fridge_201202/apps/pick_object/pick_object.app

@@ -4,3 +4,119 @@ icon: detect_cans_in_fridge_201202/pick_object.png
 interface: detect_cans_in_fridge_201202/pick_object.interface
 launch: detect_cans_in_fridge_201202/pick_object.xml
 platform: pr2
+plugins:
+  - name: kinect_head_video_recorder_plugin
+    type: app_recorder/audio_video_recorder_plugin
+    launch_args:
+      video_path: /tmp
+      video_title: pick_object_kinect_head.avi
+      audio_topic_name: /audio
+      audio_channels: 1
+      audio_sample_rate: 16000
+      audio_format: wave
+      audio_sample_format: S16LE
+      # use raw topic to suppress data transmission between c1 and c2
+      # video_topic_name: /kinect_head/rgb/image_rect_color
+      video_topic_name: /kinect_head/rgb/image_rect_color
+      use_compressed: true
+      video_decompressed_topic_name: /kinect_head/pick_object/rgb/image_rect_color
+      video_height: 480
+      video_width: 640
+      video_framerate: 30
+      video_encoding: BGR
+  - name: human_pose_estimator_video_recorder_plugin
+    type: app_recorder/audio_video_recorder_plugin
+    launch_args:
+      video_path: /tmp
+      video_title: pick_object_kinect_head_human_pose_estimator.avi
+      audio_topic_name: /audio
+      audio_channels: 1
+      audio_sample_rate: 16000
+      audio_format: wave
+      audio_sample_format: S16LE
+      video_topic_name: /edgetpu_human_pose_estimator/output/image
+      use_compressed: true
+      video_decompressed_topic_name: /edgetpu_human_pose_estimator/pick_object/output/image
+      video_height: 480
+      video_width: 640
+      video_framerate: 10
+      video_encoding: BGR
+  - name: rosbag_recorder_plugin
+    type: app_recorder/rosbag_recorder_plugin
+    launch_args:
+      rosbag_path: /tmp
+      rosbag_title: pick_object_rosbag.bag
+      compress: true
+      rosbag_topic_names:
+        - /rosout
+        - /tf
+        - /tf_static
+        - /joint_states
+        - /map
+        - /base_odometry/odom
+        - /robot_pose_ekf/odom_combined
+        - /base_controller/command
+        - /navigation/cmd_vel
+        - /move_base_node/NavFnROS/plan
+        - /move_base_node/DWAPlannerROS/global_plan
+        - /move_base_node/DWAPlannerROS/local_plan
+        - /move_base_node/local_costmap/costmap
+        - /move_base_node/global_costmap/costmap
+        - /move_base_node/global_costmap/footprint
+        - /safe_teleop_base/local_costmap/costmap
+        - /spots_marker_array
+        - /particlecloud
+        - /base_scan_throttled
+        - /tilt_scan_throttled
+        - /kinect_head/rgb/throttled/camera_info
+        - /kinect_head/depth_registered/throttled/camera_info
+        - /kinect_head/rgb/throttled/image_rect_color/compressed
+        - /kinect_head/depth_registered/throttled/image_rect/compressedDepth
+        - /audio
+  - name: result_recorder_plugin
+    type: app_recorder/result_recorder_plugin
+    plugin_args:
+      result_path: /tmp
+      result_title: pick_object_result.yaml
+  - name: gdrive_uploader_plugin
+    type: app_uploader/gdrive_uploader_plugin
+    plugin_args:
+      upload_file_paths:
+        - /tmp/pick_object_kinect_head.avi
+        - /tmp/pick_object_kinect_head_human_pose_estimator.avi
+        - /tmp/pick_object_rosbag.bag
+        - /tmp/pick_object_result.yaml
+      upload_file_titles:
+        - pick_object_kinect_head.avi
+        - pick_object_kinect_head_human_pose_estimator.avi
+        - pick_object_rosbag.bag
+        - pick_object_result.yaml
+      upload_parents_path: pr2_fridge_pick_object
+      upload_server_name: /gdrive_server
+  - name: speech_notifier_plugin
+    type: app_notifier/speech_notifier_plugin
+    plugin_args:
+      client_name: /robotsound
+  - name: mail_notifier_plugin
+    type: app_notifier/mail_notifier_plugin
+    plugin_args:
+      mail_title: PR2 fridge pick object demo
+      use_timestamp_title: true
+    plugin_arg_yaml: /var/lib/robot/pr2_mail_notifier_plugin.yaml
+plugin_order:
+  start_plugin_order:
+    - kinect_head_video_recorder_plugin
+    - human_pose_estimator_video_recorder_plugin
+    - rosbag_recorder_plugin
+    - result_recorder_plugin
+    - gdrive_uploader_plugin
+    - speech_notifier_plugin
+    - mail_notifier_plugin
+  stop_plugin_order:
+    - kinect_head_video_recorder_plugin
+    - human_pose_estimator_video_recorder_plugin
+    - rosbag_recorder_plugin
+    - result_recorder_plugin
+    - gdrive_uploader_plugin
+    - speech_notifier_plugin
+    - mail_notifier_plugin

detect_cans_in_fridge_201202/apps/pick_object/pick_object.l

@@ -7,11 +7,14 @@
 
 (defun main ()
   (let ((object (string-downcase (get-arg :object))))
+    ;; set default object: georgia
+    (if (null object) (setq object "georgia"))
     (cond
       ((find object (list "wonda" "georgia" "boss" "iemon") :test #'string=)
        (send *ri* :clear-costmap)
-       (demo :type object))
-      (t (speak-jp "ごめんなさい。いまちょっと手が離せないです。")))
+       (demo :type object :use-base nil))
+      (t (speak-jp "ごめんなさい。いまちょっと手が離せないです。")
+       nil))
 ))
 
 (run-exit (main))

detect_cans_in_fridge_201202/apps/pr2_fridge_demo/pr2_fridge_demo.app

@@ -4,3 +4,87 @@ platform: pr2
 launch: detect_cans_in_fridge_201202/pr2_fridge_demo.launch
 interface: detect_cans_in_fridge_201202/pr2_fridge_demo.interface
 icon: detect_cans_in_fridge_201202/pr2_fridge_demo.png
+plugins:
+  - name: kinect_head_video_recorder_plugin
+    type: app_recorder/video_recorder_plugin
+    launch_args:
+      video_path: /tmp
+      video_title: pr2_fridge_demo_kinect_head.avi
+      video_topic_name: /kinect_head/rgb/throttled/image_rect_color
+      video_fps: 5.0
+  - name: human_pose_estimator_video_recorder_plugin
+    type: app_recorder/video_recorder_plugin
+    launch_args:
+      video_path: /tmp
+      video_title: pr2_fridge_demo_kinect_head_human_pose_estimator.avi
+      video_topic_name: /edgetpu_human_pose_estimator/output/image
+      video_fps: 10.0
+  - name: rosbag_recorder_plugin
+    type: app_recorder/rosbag_recorder_plugin
+    launch_args:
+      rosbag_path: /tmp
+      rosbag_title: pr2_fridge_demo_rosbag.bag
+      compress: true
+      rosbag_topic_names:
+        - /rosout
+        - /tf
+        - /tf_static
+        - /joint_states
+        - /map
+        - /base_odometry/odom
+        - /robot_pose_ekf/odom_combined
+        - /base_controller/command
+        - /navigation/cmd_vel
+        - /move_base_node/NavFnROS/plan
+        - /move_base_node/DWAPlannerROS/global_plan
+        - /move_base_node/DWAPlannerROS/local_plan
+        - /move_base_node/local_costmap/costmap
+        - /move_base_node/global_costmap/costmap
+        - /move_base_node/global_costmap/footprint
+        - /safe_teleop_base/local_costmap/costmap
+        - /spots_marker_array
+        - /particlecloud
+        - /base_scan_throttled
+        - /tilt_scan_throttled
+        - /kinect_head/rgb/throttled/camera_info
+        - /kinect_head/depth_registered/throttled/camera_info
+        - /kinect_head/rgb/throttled/image_rect_color/compressed
+        - /kinect_head/depth_registered/throttled/image_rect/compressedDepth
+  - name: gdrive_uploader_plugin
+    type: app_uploader/gdrive_uploader_plugin
+    plugin_args:
+      upload_file_paths:
+        - /tmp/pr2_fridge_demo_kinect_head.avi
+        - /tmp/pr2_fridge_demo_kinect_head_human_pose_estimator.avi
+        - /tmp/pr2_fridge_demo_rosbag.bag
+      upload_file_titles:
+        - pr2_fridge_demo_kinect_head.avi
+        - pr2_fridge_demo_kinect_head_human_pose_estimator.avi
+        - pr2_fridge_demo_rosbag.bag
+      upload_parents_path: pr2_fridge_demo
+      upload_server_name: /gdrive_server
+  - name: speech_notifier_plugin
+    type: app_notifier/speech_notifier_plugin
+    plugin_args:
+      client_name: /robotsound
+  - name: mail_notifier_plugin
+    type: app_notifier/mail_notifier_plugin
+    plugin_args:
+      mail_title: PR2 fridge demo
+      use_timestamp_title: true
+    plugin_arg_yaml: /var/lib/robot/pr2_mail_notifier_plugin.yaml
+plugin_order:
+  start_plugin_order:
+    - kinect_head_video_recorder_plugin
+    - human_pose_estimator_video_recorder_plugin
+    - rosbag_recorder_plugin
+    - gdrive_uploader_plugin
+    - speech_notifier_plugin
+    - mail_notifier_plugin
+  stop_plugin_order:
+    - kinect_head_video_recorder_plugin
+    - human_pose_estimator_video_recorder_plugin
+    - rosbag_recorder_plugin
+    - gdrive_uploader_plugin
+    - speech_notifier_plugin
+    - mail_notifier_plugin

detect_cans_in_fridge_201202/apps/pr2_fridge_demo/pr2_fridge_demo.l

@@ -1,8 +1,8 @@
 #!/usr/bin/env roseus
 
+(require :app-utils "package://jsk_demo_common/euslisp/app-utils.l")
+
 (load "package://detect_cans_in_fridge_201202/euslisp/main.l")
 
-(init)
 (send *ri* :clear-costmap)
-(demo)
-(exit)
+(run-exit (demo :type "georgia"))

detect_cans_in_fridge_201202/euslisp/main.l

@@ -90,7 +90,7 @@
            (speak-jp (format nil "Please take ~a." *type*)))
           (t (speak-jp (format nil "~aをどうぞ。" *type*))))
        (speak-jp (format nil "~a を どうぞ" *type*)))
-     (hand-over use-arm :wait-shock t))
+     (hand-over use-arm :wait-shock t :tuckarm :rarm))
     )
   t
   )
@@ -130,6 +130,7 @@
                   (finish-type :pass-to-human) ;; :pass-to-human, :put-on-turtlebot, ...
                   ;;(use-arm :rarm)
                   (use-arm :larm)
+                  (use-base nil)
                   )
   (disable-behavior-server)
   (case app-manager
@@ -152,7 +153,7 @@
      ;; do nothing
      ))
 
-  (unless (open-fridge-door :use-arm use-arm)
+  (unless (open-fridge-door :use-arm use-arm :use-base use-base)
     (return-from demo))
 
   ;; move
@@ -177,7 +178,7 @@
        (return-from demo)))
     )
   (enable-behavior-server)
-  (if (eq app-manager :true) (ros::exit))
+  (if (eq app-manager :true) (ros::exit) t)
   )
 
 (defun wait-android-query (&optional (demo-type :map))