[pr2eus_moveit] fix collision-object-sample.l and :add-object in collision-object-publisher.l

[knorth55]

• fix :frame_id -> :frame-id
• evaluate faces of collision objects

[k-okada]

this looks good, but could not confirm it fixes the problem. Could you provide sample code or update the following pr2 example?

https://gist.github.com/28aa1d47c0216dd888e432f7d5edfc5b

[knorth55]

this typo does not matter with pr2 and base_footprint because of default parameters but if you want to add collision object from other frame id, it matters

[knorth55]

Oh, I confirm the bug. Something is wrong with meshes which are translated.

[knorth55]

原因がわかりました。 原因はEuslispでのmeshのverticesの評価が(send obj :worldcoords)されていないことです。 https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/master/pr2eus_moveit/euslisp/collision-object-publisher.l#L106-L107 この上記の行で:reset-coordsして評価しているつもりですが、そこではverticesの値は評価されていないです。

以下のコードで試してみました。 :translateに関してもverticesの位置は描画されるまで評価されていません。

(load "models/foldable-desk-object.l")

(print "start")
(setq *table* (instance foldable-desk-object :init))
(print "init")
(print (send (car (send (body-to-faces *table*) :faces)) :vertices))

(send *table* :translate #f(1000 800 500))
(print "translate")
(print (send (car (send (body-to-faces *table*) :faces)) :vertices))

(send *table* :worldcoords)
(print "worldcoords")
(print (send (car (send (body-to-faces *table*) :faces)) :vertices))

(objects (list *table*))
(print "visualize")
(print (send (car (send (body-to-faces *table*) :faces)) :vertices))

"start"
"init"
(#f(240.0 370.0 700.0) #f(-240.0 -370.0 700.0) #f(240.0 -370.0 700.0) #f(240.0 370.0 700.0))
"translate"
(#f(240.0 370.0 700.0) #f(-240.0 -370.0 700.0) #f(240.0 -370.0 700.0) #f(240.0 370.0 700.0))
"worldcoords"
(#f(240.0 370.0 700.0) #f(-240.0 -370.0 700.0) #f(240.0 -370.0 700.0) #f(240.0 370.0 700.0))
;; (make-irtviewer) executed
"visualize"
(#f(1240.0 1170.0 1200.0) #f(760.0 430.0 1200.0) #f(1240.0 430.0 1200.0) #f(1240.0 1170.0 1200.0))

[knorth55]

あと関係ないと思いますが、色々試してみたときに、以下のコードで返り値がかなり違うのが疑問です。

16. $ (send (car (send *table* :faces)) :vertices)
(#f(1240.0 430.0 1200.0) #f(1240.0 1170.0 1200.0) #f(760.0 1170.0 1200.0) #f(760.0 430.0 1200.0) #f(1240.0 430.0 1200.0))
17. $ (send (car (send (body-to-faces *table*) :faces)) :vertices)
(#f(1240.0 1170.0 1200.0) #f(760.0 430.0 1200.0) #f(1240.0 430.0 1200.0) #f(1240.0 1170.0 1200.0))

[knorth55]

色々ためしてみた結果 (send obj :bodies)することで評価されることをみつけました。

71.E2-irteusgl$ (send *table* :reset-coords)
#<foldable-desk-object #X65f7cc8 foldable-desk  0.0 0.0 0.0 / 0.0 0.0 0.0>
72.E2-irteusgl$ (send (car (send (body-to-faces *table*) :faces)) :vertices)
(#f(1240.0 370.0 700.0) #f(760.0 -370.0 700.0) #f(1240.0 -370.0 700.0) #f(1240.0 370.0 700.0))
73.E2-irteusgl$ (send *table* :bodies)
(#<faceset #X65dd330  0.0 0.0 0.0 / 0.0 0.0 0.0> #<faceset #X6e23070  0.0 0.0 0.0 / 0.0 0.0 0.0> #<faceset #X6e2a820  0.0 0.0 0.0 / 0.0 0.0 0.0> #<faceset #X6e2f050  0.0 0.0 0.0 / 0.0 0.0 0.0> #<faceset #X6e3e2e0  0.0 0.0 0.0 / 0.0 0.0 0.0> #<faceset #X6e53858  0.0 0.0 0.0 / 0.0 0.0 0.0> #<faceset #X6e6bc18  0.0 0.0 0.0 / 0.0 0.0 0.0> #<faceset #X6e56100  0.0 0.0 0.0 / 0.0 0.0 0.0> #<faceset #X6e77d30  0.0 0.0 0.0 / 0.0 0.0 0.0> #<faceset #X6e8af50  0.0 0.0 0.0 / 0.0 0.0 0.0> #<faceset #X6e8e4b8  0.0 0.0 0.0 / 0.0 0.0 0.0> #<faceset #X6eb96c8  0.0 0.0 0.0 / 0.0 0.0 0.0> #<faceset #X6ea39b8  0.0 0.0 0.0 / 0.0 0.0 0.0> #<faceset #X6ecd200  0.0 0.0 0.0 / 0.0 0.0 0.0> #<faceset #X6ee2448  0.0 0.0 0.0 / 0.0 0.0 0.0> #<faceset #X6edc418  0.0 0.0 0.0 / 0.0 0.0 0.0>)
74.E2-irteusgl$ (send (car (send (body-to-faces *table*) :faces)) :vertices)
(#f(240.0 370.0 700.0) #f(-240.0 -370.0 700.0) #f(240.0 -370.0 700.0) #f(240.0 370.0 700.0))

[knorth55]

I fixed the problem. Lazy evaluation of euslisp was the key.

[knorth55]

TODO: add sample program for pr2 not for sample robot.

[knorth55]

~@k-okada Travis test passed. Could you merge this PR?~ ~We need this code for the seminar for bachelor students.~

Oh, I will add sample code for PR2.