Subway demo

[YutoUchimi]

Update on 2019/12/30

Requirements

• Add dependency: estimate human pose by Coral Edge TPU (https://github.com/knorth55/coral_usb_ros)
• Recognize mirror in elevator -> Add FCN8sDepthPredictionConcatFirst model to fcn_depth_prediction.py jsk_recognition#2481
• Recover from move_base stuck when moving obstacle is observed -> Enable recovery_behavior jsk_robot#1192
• Fix camera_info to check if pushed button is lit or not -> Fix wide_stereo camera model parameters jsk_pr2eus#426

Recommendations

• See whole panel of elevator inside -> Parameterize frames, transformation and interpolation in virtual_camera_mono jsk_recognition#2470
• Return from :move-to as soon as possible after PR2 calls elevator -> [pr2eus] Add correction arg in :move-to-wait jsk_pr2eus#418

[knorth55]

@YutoUchimi is this still wip?

[knorth55]

I tried this PR today, and I got the following error when PR2 was going back from 7F elevator hall to 73B2.

[ INFO] [1607505304.689815109] [/elevator_move_base:ros.roseus]: Checking if elevator is needed...
[ INFO] [1607505304.767940219] [/elevator_move_base:ros.roseus]: cur-floor: /eng2/7f, target-floor: nil, target-coords: #<cascaded-coords #X170180d8 /eng2  511.799 5801.819 50000.0 / 3.034 0.0 0.0>
Call Stack (max depth: 20):
  0: at (send p1 :worldpos)
  1: at (elt (send p1 :worldpos) 2)
  2: at (< (elt (send p1 :worldpos) 2) (elt (send p2 :worldpos) 2))
  3: at (let ((p1 (find name1 objs :test #'equal :key #'(lambda (x) (send x :name)))) (p2 (find name2 objs :test #'equal :key #'(lambda (x) (send x :name))))) (< (elt (send p1 :worldpos) 2) (elt (send p2 :worldpos) 2)))
  4: at (send *scene* :floor< target-floor cur-floor)
  5: at (cond ((send *scene* :floor< target-floor cur-floor) "down") ((send *scene* :floor< cur-floor target-floor) "up") (t nil))
  6: at (setq up/down (cond ((send *scene* :floor< target-floor cur-floor) "down") ((send *scene* :floor< cur-floor target-floor) "up") (t nil)))
  7: at (let* ((goal (cdr (assoc :goal userdata))) (goal-pose (ros::tf-pose-stamped->coords (send goal :target_pose))) (cur-floor (check-current-floor *scene* *tfl*)) (target-floor (check-current-floor *scene* *tfl* goal-pose)) target-floor-button (target-coords (transform-pose-to-target-frame goal-pose (
send *scene* :name) *tfl*)) up/down) (ros::ros-info (format nil "cur-floor: ~A, target-floor: ~A, target-coords: ~A" cur-floor target-floor target-coords)) (setq up/down (cond ((send *scene* :floor< target-floor cur-floor) "down") ((send *scene* :floor< cur-floor target-floor) "up") (t nil))) (ros::ros-in
fo (format nil "up/down: ~A" up/down)) (setq target-floor-button (elt (assoc target-floor *floors* :test #'string= :key #'(lambda (x) (format nil "~A/~A" (send *scene* :name) (string-downcase (elt x 0))))) 0)) (set-alist 'inside-panel-name "/elevator_inside_panel" userdata) (set-alist 'outside-panel-name 
"/elevator_call_panel" userdata) (set-alist 'elevator-front-coords (let ((coords (send *scene* :transformation (car (send *scene* :find-object (format nil "~A/elevator-outside" cur-floor)))))) (send coords :name (send *scene* :name)) (send coords :rotate pi :z) (send coords :translate #f(0.0 -200.0 0.0) :
world) coords) userdata) (set-alist 'front-coords (let ((coords (send *scene* :transformation (car (send *scene* :find-object (format nil "~A/elevator_call_panel-front" cur-floor)))))) (send coords :name (send *scene* :name)) coords) userdata) (set-alist 'inside-coords (let ((coords (send *scene* :transfo
rmation (car (send *scene* :find-object (format nil "~A/elevator_inside_panel-front" cur-floor)))))) (send coords :name (send *scene* :name)) coords) userdata) (set-alist 'outside-coords (let ((coords (send *scene* :transformation (car (send *scene* :find-object (format nil "~A/elevator-outside" target-fl
oor)))))) (send coords :name (send *scene* :name)) coords) userdata) (set-alist 'target-floor target-floor-button userdata) (set-alist 'outside-button up/down userdata) (set-alist 'target-coords target-coords userdata) (not (null up/down)))
  8: at (funcall action userdata)
  9: at (funcall action userdata)
  10: at (cond ((send self :submachine) (send action :execute userdata :step step)) ((functionp action) (funcall action userdata)) (t (send self :name)))
  11: at (setq res (cond ((send self :submachine) (send action :execute userdata :step step)) ((functionp action) (funcall action userdata)) (t (send self :name))))
  12: at (let (res) (send self :remap userdata :invert nil) (setq res (cond ((send self :submachine) (send action :execute userdata :step step)) ((functionp action) (funcall action userdata)) (t (send self :name)))) (send self :remap userdata :invert t) res)
  13: at (send last-state :execute userdata)
  14: at (let* ((last-state astate) (trans (send self :next-arc-list astate)) (exec-result (send last-state :execute userdata))) (ros::ros-debug "trans: ~A" trans) (setq trans (remove-if-not #'(lambda (tr) (send tr :check exec-result)) trans)) (case (length trans) (0 (error "undefined transition ~A from ~
A~%" exec-result last-state)) (1 t) (t (case (length active-state) (1 (warn "multiple transitions ~A from ~A~%" exec-result last-state)) (t t)))) (when (not (eq astate last-state)) (error "active state has not changed ~A -> ~A~%" last-state astate) (return-from :execute-impl (send astate :name))) (setq ne
xt-active-state (append next-active-state (send-all trans :to))) (if (send astate :submachine) (send (send astate :submachine) :reset-state)) (push exec-result ret))
  15: at (while #:dolist10148331 (setq astate (pop #:dolist10148331)) (let* ((last-state astate) (trans (send self :next-arc-list astate)) (exec-result (send last-state :execute userdata))) (ros::ros-debug "trans: ~A" trans) (setq trans (remove-if-not #'(lambda (tr) (send tr :check exec-result)) trans)) (
case (length trans) (0 (error "undefined transition ~A from ~A~%" exec-result last-state)) (1 t) (t (case (length active-state) (1 (warn "multiple transitions ~A from ~A~%" exec-result last-state)) (t t)))) (when (not (eq astate last-state)) (error "active state has not changed ~A -> ~A~%" last-state asta
te) (return-from :execute-impl (send astate :name))) (setq next-active-state (append next-active-state (send-all trans :to))) (if (send astate :submachine) (send (send astate :submachine) :reset-state)) (push exec-result ret)))
  16: at (let ((astate nil) (#:dolist10148331 active-state)) nil (while #:dolist10148331 (setq astate (pop #:dolist10148331)) (let* ((last-state astate) (trans (send self :next-arc-list astate)) (exec-result (send last-state :execute userdata))) (ros::ros-debug "trans: ~A" trans) (setq trans (remove-if-no
t #'(lambda (tr) (send tr :check exec-result)) trans)) (case (length trans) (0 (error "undefined transition ~A from ~A~%" exec-result last-state)) (1 t) (t (case (length active-state) (1 (warn "multiple transitions ~A from ~A~%" exec-result last-state)) (t t)))) (when (not (eq astate last-state)) (error "
active state has not changed ~A -> ~A~%" last-state astate) (return-from :execute-impl (send astate :name))) (setq next-active-state (append next-active-state (send-all trans :to))) (if (send astate :submachine) (send (send astate :submachine) :reset-state)) (push exec-result ret))) nil)
  17: at (dolist (astate active-state) (let* ((last-state astate) (trans (send self :next-arc-list astate)) (exec-result (send last-state :execute userdata))) (ros::ros-debug "trans: ~A" trans) (setq trans (remove-if-not #'(lambda (tr) (send tr :check exec-result)) trans)) (case (length trans) (0 (error "
undefined transition ~A from ~A~%" exec-result last-state)) (1 t) (t (case (length active-state) (1 (warn "multiple transitions ~A from ~A~%" exec-result last-state)) (t t)))) (when (not (eq astate last-state)) (error "active state has not changed ~A -> ~A~%" last-state astate) (return-from :execute-impl 
(send astate :name))) (setq next-active-state (append next-active-state (send-all trans :to))) (if (send astate :submachine) (send (send astate :submachine) :reset-state)) (push exec-result ret)))
  18: at (let (ret next-active-state) (dolist (astate active-state) (let* ((last-state astate) (trans (send self :next-arc-list astate)) (exec-result (send last-state :execute userdata))) (ros::ros-debug "trans: ~A" trans) (setq trans (remove-if-not #'(lambda (tr) (send tr :check exec-result)) trans)) (ca
se (length trans) (0 (error "undefined transition ~A from ~A~%" exec-result last-state)) (1 t) (t (case (length active-state) (1 (warn "multiple transitions ~A from ~A~%" exec-result last-state)) (t t)))) (when (not (eq astate last-state)) (error "active state has not changed ~A -> ~A~%" last-state astate
) (return-from :execute-impl (send astate :name))) (setq next-active-state (append next-active-state (send-all trans :to))) (if (send astate :submachine) (send (send astate :submachine) :reset-state)) (push exec-result ret))) (setq active-state (unique next-active-state)) ret)
  19: at (send self :execute-impl args :step step)
  And more...
/opt/ros/indigo/share/euslisp/jskeus/eus/Linux64/bin/irteusgl roseus-error: cannot find method :worldpos in (send p1 :worldpos), exitting...
[ INFO] [1607505304.769530701] [/elevator_move_base:ros.roseus]: cell* ROSEUS_EXIT(context*, int, cell**)
[ INFO] [1607505304.769569842] [/elevator_move_base:ros.roseus]: exiting roseus 1
[elevator_move_base-1] process has died [pid 28575, exit code 1, cmd /home/applications/ros/indigo/src/jsk-ros-pkg/jsk_roseus/roseus/bin/roseus /home/applications/ros/indigo/src/jsk-ros-pkg/jsk_demos/elevator_move_base_pr2/src/elevator-move-base.l __name:=elevator_move_base __log:=/var/log/ros/b769458c-39
fa-11eb-835f-d05099c29feb/elevator_move_base-1.log].
log file: /var/log/ros/b769458c-39fa-11eb-835f-d05099c29feb/elevator_move_base-1*.log

cc. @mqcmd196