[pr2eus] change pr2 camera frame namespace from /openni to /kinect_head

[furushchev]

ref: #149

[k-okada]

did you check rostest on your local environment?

;; #<rotational-joint #X64794e0 head_tilt_joint> :joint-angle(-22.428) violate min-angle(-21.2682)
;; #<rotational-joint #X69288f8 r_wrist_flex_joint> :joint-angle(-4.82714) violate max-angle(-5.72958)
;; #<rotational-joint #X681ec98 l_wrist_flex_joint> :joint-angle(-4.87927) violate max-angle(-5.72958)
[ERROR] test (eps= (norm (send p :difference-position viewing)) 0.0 3.0) failed ... (check head_mount_kinect_ir_optical_frame : #<cascaded-coords #X73a28c0 /base_footprint  -219.547 97.036 1397.645 / -2.013 -1.868e-12 -1.179> #<perspective-viewing #X72546d0 :kinect_head/depth 69.6deg -215.59 95.164 1402.607 / -2.013 -1.777e-12 -1.2>, diff 6.61719 m;; #&lt;rotational-joint #X681ec98 l_wrist_flex_joint&gt; :joint-angle(-4.87927) violate max-angle(-5.72958)
l_frame
).
[ERROR] test (eps= (norm (send p :difference-position viewing)) 0.0 3.0) failed ... (check head_mount_kinect_rgb_optical_frame : #<cascaded-coords #X7669ba0 /base_footprint  -240.699 72.503 1370.406 / -2.027 -0.006 -1.131> #<perspective-viewing #X5b4f6e8 :kinect_head/rgb 74.6deg -237.239 70.866 1375.549 / -2.027 -0.006 -1.151>, diff 6.41083 #X681ec98 l_wrist_flex_joint&amp;gt; :joint-angle(-4.87927) violate max-angle(-5.72958)
l_frame
).
[0mm;; #&amp;lt;rotational-joint ).
testpr2_ik_test ... ok
testpr2_read_state_test ... ok

[furushchev]

@k-okada In my local machine, it fails only on rostest pr2eus pr2eus-test.launch. (at pr2-camera-coords-test ) Does it mean that I should re-record another rosbag, or there is some problem with scripts to generate pr2.l?
https://gist.github.com/d18a8800a8eeafd0705e

[k-okada]

please look into the error message and source code to understand wha is the
side effects of your changes on camera name space

◉ Kei Okada

On Mon, Jul 27, 2015 at 6:37 PM, Furushchev [email protected]
wrote:

@k-okada https://github.com/k-okada In my local machine, it fails only
on rostest pr2eus pr2eus-test.launch. Does it mean that I should
re-record another rosbag, or there is some problem with scripts to generate
pr2.l?

—
Reply to this email directly or view it on GitHub
#153 (comment)
.

[furushchev]

MEMO

(ros::roseus "hoge")

(load "pr2.l")
(load "pr2-interface.l")
(pr2)
(setq *ri* (instance pr2-interface :init))
(setq *tfl* (instance ros::transform-listener :init))
(ros::spin-once)
(send *ri* :spin-once)
(send *pr2* :angle-vector (send *ri* :state :potentio-vector))

(setq frame "head_mount_kinect_ir_optical_frame")

(setq lk (send *pr2* (intern (string-upcase (format nil "~A_lk" frame)) *keyword-package*)))
(send *tfl* :wait-for-transform "/base_footprint" (format nil "/~A" frame) (ros::time 0) 10)
(setq c (send *tfl* :lookup-transform "/base_footprint" (format nil "/~A" frame) (ros::time 0)))
(while lk
  (warn "~A diff-pos: ~A, diff-rot: ~A~%"
        frame (norm (send lk :difference-position c)) (norm (send lk :difference-rotation c)))
  (setq lk (send lk :parent))
  (setq frame (send lk :name))
  (send *tfl* :wait-for-transform "/base_footprint" (format nil "/~A" frame) (ros::time 0) 10)
  (setq c (send *tfl* :lookup-transform "/base_footprint" (format nil "/~A" frame) (ros::time 0)))
  )

head_mount_kinect_ir_optical_frame diff-pos: 6.78835, diff-rot: 0.020766
head_mount_kinect_ir_link diff-pos: 6.78835, diff-rot: 0.020766
head_mount_link diff-pos: 4.0137, diff-rot: 0.020766
head_plate_frame diff-pos: 1.42339, diff-rot: 0.020766
head_tilt_link diff-pos: 2.343718e-13, diff-rot: 0.020766
head_pan_link diff-pos: 2.343819e-13, diff-rot: 3.469447e-17
torso_lift_link diff-pos: 2.291431e-13, diff-rot: 0.0
base_link diff-pos: 2.842171e-14, diff-rot: 0.0
base_footprint diff-pos: 0.0, diff-rot: 0.0

[furushchev]

After roslaunch pr2_bringup pr2_recalibrate.launch and robot start, diff became very small, yay!

$ roseus test-show-diff.l
configuring by "/home/furushchev/ros/hydro_parent/devel/share/euslisp/jskeus/eus//lib/eusrt.l"
;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;; string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;; eusdebug ;; eusforeign ;; coordinates ;; tty ;; history ;; toplevel ;; trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack ;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;; viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;; xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;; Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin
;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtpointcloud irtx eusjpeg euspng png irtimage irtglrgb
;; extending gcstack 0x4833420[16374] --> 0x4c68b00[32748] top=37b8
irtgl irtviewer
EusLisp 9.12(fada0ef 1.0.4) for Linux64 created on pr1012(Thu Jun 18 07:20:07 PDT 2015)
roseus ;; loading roseus("ff5d0b3") on euslisp((9.12 pr1012 Thu Jun 18 07:20:07 PDT 2015 fada0ef 1.0.4))
eustf roseus_c_util head_mount_kinect_ir_optical_frame diff-pos: 9.124390e-13, diff-rot: 1.195665e-15
head_mount_kinect_ir_link diff-pos: 9.096213e-13, diff-rot: 9.714453e-17
head_mount_link diff-pos: 2.283707e-13, diff-rot: 9.714453e-17
head_plate_frame diff-pos: 4.549693e-13, diff-rot: 9.714453e-17
head_tilt_link diff-pos: 4.549693e-13, diff-rot: 9.714453e-17
head_pan_link diff-pos: 4.582863e-13, diff-rot: 1.084202e-19
torso_lift_link diff-pos: 1.171857e-13, diff-rot: 0.0
base_link diff-pos: 2.842171e-14, diff-rot: 0.0
base_footprint diff-pos: 0.0, diff-rot: 0.0

[furushchev]

travis passed. if there is no problem, please merge

[k-okada]

@furushchev @h-kamada @chiwunau this changed pr2.l so it will change demo behavior, please chekck with your demo code using latest pr2.l