Reproduce of PR2 Subway Fetch Demo. #1321

knorth55 · 2020-12-09T09:37:24Z

PR2 subway demo branch.
this branch is rebased version of #1276

TODO

Remove GPU door button recognition
- Because there is no door button now.
Elevator panel template change
- Because there is a switchbot on the elevator call panel.
- Or use switchbot from PR2.
- Or remove the switchbot for the experiments.
Check if this PR works now.
Try mirror human recognition
- Reproduce of @YutoUchimi 's master thesis
Connect to sanshiro in forum

Upstream depends

Fix wide_stereo camera model parameters Fix wide_stereo camera model parameters jsk_pr2eus#426

Trials

2020/12/09
- Failed
- Reproduce of PR2 Subway Fetch Demo. #1321 (comment)

knorth55 · 2020-12-09T09:38:03Z

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

knorth55 · 2021-11-17T07:52:42Z

elevator_move_base_pr2/sample/fetch-sandwich.l

+
+
+(defun init ()
+  (setq *pr2* (pr2))


we need to set pr2 robot name

Suggested change

(setq *pr2* (pr2))

(setq *pr2* (pr2 :pr1040))

jsk-ros-pkg/jsk_pr2eus#426

cc. @Kanazawanaoaki

@tkmtnt7000 @mqcmd196
I think we need this change to #1397 .

Thank you!!
We defenitly want this!!

We need this when we use :screen-point.

Nice comment and thank you for your information @pazeshun @tkmtnt7000

fixed 2338f3a

Hmm, but I think the better solution is setting with args or something

Something like https://github.com/jsk-ros-pkg/jsk_robot/blob/23037dbfe66757052074a8109ab61875a73c3a4c/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l#L19-L26 ??

…ve_base action

…is in the way of traffic

knorth55 · 2022-10-05T16:30:30Z

Now I fixed this PR and all test in elevator_move_base_pr2 are passed.

$ catkin_test_results elevator_move_base_pr2 --verbose --all
test_results/elevator_move_base_pr2/rostest-test_test-button-light.xml: 1 tests
test_results/elevator_move_base_pr2/rostest-test_test-color-point-detector.xml: 2 tests
test_results/elevator_move_base_pr2/rostest-test_test-modules-callpanel.xml: 1 tests
test_results/elevator_move_base_pr2/rostest-test_test-modules-insidepanel.xml: 2 tests
test_results/elevator_move_base_pr2/rostest-test_test-panel-pose-detection.xml: 1 tests
test_results/elevator_move_base_pr2/rosunit-test_button_light_recognition.xml: 1 tests
test_results/elevator_move_base_pr2/rosunit-test_color_point_detector.xml: 1 tests
test_results/elevator_move_base_pr2/rosunit-test_hztest.xml: 1 tests
test_results/elevator_move_base_pr2/rosunit-test_number_recognition.xml: 1 tests
test_results/elevator_move_base_pr2/rosunit-test_panel_pose_detection.xml: 1 tests
Summary: 12 tests, 0 errors, 0 failures, 0 skipped

knorth55 · 2022-10-05T16:31:37Z

TODO:

Check if this PR works now.
Remove GPU section because there is no door button now.

knorth55 mentioned this pull request Dec 15, 2020

[elevator_move_base_fetch?] エレベータの階表示パネルをよみたい #1322

Open

knorth55 commented Nov 17, 2021

View reviewed changes

YutoUchimi added 27 commits October 5, 2022 20:43

Call wait-interpolation after torso down

5010502

Fix sleep

8eb5afe

Reset pose before looking at panel

d7ebf40

Unsafe go-pos during ready-to-outside

b822ae3

Set line_buffer_length

bc58d41

Do not reset head pose in each look-button loop

665d192

Loosen tolerance before turning left in elevator

76b9d77

Draw rectangle not with matplotlib but with cv2 in match_template.py

287f456

Do not show probability in debug image of match_template.py

404a29e

Move speak code to move-inside-elevator.l for check-elevator-open func

cad80ff

Look ahead as much as possible when moving base

a91b882

Change push depth: 30mm -> 20mm

ee3e03d

Reset *change-pc* at the beginning of each calling check-elevator-open

2740986

Look front before checking if elevator open

99df74c

Reduce unnecessary multiple angle-vector and subscribing

fe4be62

Wait for interpolation of look-forward-in-nav.l

10bc332

Wait at most 6 min when receiving sandwich at subway

45be6b8

Subscribe more to get stable point cloud

846bb19

Show quarter and throttled point cloud in rviz

8975043

Fix typo: row-info -> ros-info

d995963

Set after-stamp to (ros::time-now) while checking if forum door is open

ab612fb

Push forum door button only 10mm

87e978b

Fix resetting pose before pushing forum door button

4335782

Update text visualization

5c8fef4

Fix state name

b37b46e

Fix unbound variable timeout while checking if forum door open

1c61c9d

Fix too fast angle-vector

d071b23

YutoUchimi and others added 25 commits October 5, 2022 20:43

Update ordering sandwich

4513463

Update rviz config

7710155

Add speaking 'leave it to the clerk' at the end of ordering

6b95d4a

Refactor wait-for-hand-over

841888e

Decrease timeout of speak-to-human-behind

3c18620

Update rviz config

a616554

Disable correction at the end of :go-to since this is a wrapper of mo…

28857e3

…ve_base action

Disable look_at_human while taking elevator

07fbeaf

Refactor find_human_in_mirror.py

e05365f

Look at human until order is finished

13199e2

Disable tilt_scan observation before moving elevator front

319300e

Update pick-bag-pose

f04f1d7

Launch demo from app_manager

d21f871

Prepare for executing demo from dialogflow

94f18d9

Update ordering saidwich for applying reduced comsumption tax

9f8e7e1

Lookup price from sandwich name

7de3e30

Fix for dialogflow

8c995f0

Add mid-point while coming back to initial pose since previous point …

038b656

…is in the way of traffic

Avoid buying 'nil'

c953368

Add .rosinstall for elevator_move_base_pr2

dc25a9c

move look-at-human.l to node_scripts

67cad81

add launch_find_elevator_button arg

118e975

fix test-button-light test

b37012f

fix test-modules-callpanel test

bc0dbe3

enable all tests

16741dc

knorth55 force-pushed the forum_button branch from 94b3135 to 16741dc Compare October 5, 2022 16:29

knorth55 changed the title ~~Subway demo 2020/12/09~~ Reproduce of PR2 Subway Fetch Demo. Oct 5, 2022

mqcmd196 mentioned this pull request Nov 28, 2023

Add Subway demo #1397

Open

2 tasks

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Reproduce of PR2 Subway Fetch Demo. #1321

Reproduce of PR2 Subway Fetch Demo. #1321

knorth55 commented Dec 9, 2020 •

edited

Loading

knorth55 commented Dec 9, 2020

knorth55 Nov 17, 2021 •

edited

Loading

pazeshun Dec 7, 2023

tkmtnt7000 Dec 7, 2023

tkmtnt7000 Dec 7, 2023

mqcmd196 Dec 7, 2023

mqcmd196 Dec 7, 2023

mqcmd196 Dec 7, 2023

tkmtnt7000 Dec 7, 2023

knorth55 commented Oct 5, 2022

knorth55 commented Oct 5, 2022

Reproduce of PR2 Subway Fetch Demo. #1321

Are you sure you want to change the base?

Reproduce of PR2 Subway Fetch Demo. #1321

Conversation

knorth55 commented Dec 9, 2020 • edited Loading

TODO

Upstream depends

Trials

knorth55 commented Dec 9, 2020

knorth55 Nov 17, 2021 • edited Loading

Choose a reason for hiding this comment

pazeshun Dec 7, 2023

Choose a reason for hiding this comment

tkmtnt7000 Dec 7, 2023

Choose a reason for hiding this comment

tkmtnt7000 Dec 7, 2023

Choose a reason for hiding this comment

mqcmd196 Dec 7, 2023

Choose a reason for hiding this comment

mqcmd196 Dec 7, 2023

Choose a reason for hiding this comment

mqcmd196 Dec 7, 2023

Choose a reason for hiding this comment

tkmtnt7000 Dec 7, 2023

Choose a reason for hiding this comment

knorth55 commented Oct 5, 2022

knorth55 commented Oct 5, 2022

knorth55 commented Dec 9, 2020 •

edited

Loading

knorth55 Nov 17, 2021 •

edited

Loading