Fix wide_stereo camera model parameters

[k-okada]

rewrited version #403, because it just replace pr1012 settings with pr1040

--

update pr10*_urdf.yaml for pretty format, for make it easy to compare between robot and versions

use python command to convert from robot_description string format to xml format, use lxml because of pretty function https://www.ronrothman.com/public/leftbraned/xml-dom-minidom-toprettyxml-and-silly-whitespace/, instead of minidom or ElementTree

python -c "import yaml; import datetime; from lxml import etree; comment=etree.Comment(datetime.datetime.today().strftime(' generated from pr1040 on %Y/%m/%d ')); urdf=etree.XML(yaml.load(open('pr1040_urdf.yaml').read())); print(etree.tostring(comment, pretty_print=True, xml_declaration=True)); print(etree.tostring(urdf, pretty_print=True, xml_declaration=False));"

[k-okada]

@YutoUchimi please look at #426, which at least support camera parameter for both pr1012 and pr1040,

• It should have camera parameter for pr1014, if you use (pr2 :pr1040), but it did not include link offset, I am not sure if this is require, so please check this with both test code and real robot
• I think something like (defun pr2 (&optional (name (or (ros::get-param "robot/name") ":pr1012)) is useful for real robot users, please fix this pseudo code and update make-pr2-model-file.l.

[YutoUchimi]

  (:init (n &rest args)
   (send-super* :init args)
   (setq name n)
   ;; kinect_head frame definition, this data is taken from jsk_pr2_startup kinect_head_launch 
#|
 get frame coordinates data from pr1012:/etc/ros/distro/urdf/robot.xml
|#
   (case name
     ;; define cameras for pr1012
     (:pr1012
      ;; sensor_msgs::camerainfo #f(575.816 0.0 315.0 0.0 0.0 575.816 237.0 0.0 0.0 0.0 1.0 0.0)
      (setq kinect_head-depth (make-camera-from-ros-camera-info-aux 640 480 #f(575.816 0.0 315.0 0.0 0.0 575.816 237.0 0.0 0.0 0.0 1.0 0.0) head_mount_kinect_ir_optical_frame_lk :name :kinect_head/depth))

      ;; sensor_msgs::camerainfo #f(525.0 0.0 319.5 0.0 0.0 525.0 239.5 0.0 0.0 0.0 1.0 0.0)
      (setq kinect_head-rgb (make-camera-from-ros-camera-info-aux 640 480 #f(525.0 0.0 319.5 0.0 0.0 525.0 239.5 0.0 0.0 0.0 1.0 0.0) head_mount_kinect_rgb_optical_frame_lk :name :kinect_head/rgb))

      ;; sensor_msgs::camerainfo #f(341.275 0.0 326.117 0.0 0.0 382.992 244.101 0.0 0.0 0.0 1.0 0.0)
      (setq r_forearm_cam (make-camera-from-ros-camera-info-aux 640 480 #f(341.275 0.0 326.117 0.0 0.0 382.992 244.101 0.0 0.0 0.0 1.0 0.0) r_forearm_cam_optical_frame_lk :name :r_forearm_cam))

      ;; sensor_msgs::camerainfo #f(338.589 0.0 290.813 0.0 0.0 378.466 241.389 0.0 0.0 0.0 1.0 0.0)
      (setq l_forearm_cam (make-camera-from-ros-camera-info-aux 640 480 #f(338.589 0.0 290.813 0.0 0.0 378.466 241.389 0.0 0.0 0.0 1.0 0.0) l_forearm_cam_optical_frame_lk :name :l_forearm_cam))

      ;; sensor_msgs::camerainfo #f(445.634 0.0 240.474 -39.9524 0.0 445.634 193.274 0.0 0.0 0.0 1.0 0.0)
      (setq wide_stereo-right (make-camera-from-ros-camera-info-aux 640 480 #f(445.634 0.0 240.474 -39.9524 0.0 445.634 193.274 0.0 0.0 0.0 1.0 0.0) wide_stereo_optical_frame_lk :name :wide_stereo/right))

      ;; sensor_msgs::camerainfo #f(445.634 0.0 240.474 0.0 0.0 445.634 193.274 0.0 0.0 0.0 1.0 0.0)
      (setq wide_stereo-left (make-camera-from-ros-camera-info-aux 640 480 #f(445.634 0.0 240.474 0.0 0.0 445.634 193.274 0.0 0.0 0.0 1.0 0.0) wide_stereo_optical_frame_lk :name :wide_stereo/left))

      ;; sensor_msgs::camerainfo #f(910.712 0.0 320.414 -81.6247 0.0 910.712 263.573 0.0 0.0 0.0 1.0 0.0)
      (setq narrow_stereo-right (make-camera-from-ros-camera-info-aux 640 480 #f(910.712 0.0 320.414 -81.6247 0.0 910.712 263.573 0.0 0.0 0.0 1.0 0.0) narrow_stereo_optical_frame_lk :name :narrow_stereo/right))

      ;; sensor_msgs::camerainfo #f(910.712 0.0 320.414 0.0 0.0 910.712 263.573 0.0 0.0 0.0 1.0 0.0)
      (setq narrow_stereo-left (make-camera-from-ros-camera-info-aux 640 480 #f(910.712 0.0 320.414 0.0 0.0 910.712 263.573 0.0 0.0 0.0 1.0 0.0) narrow_stereo_optical_frame_lk :name :narrow_stereo/left))

      (setq cameras (list  (send self :kinect_head-depth) (send self :kinect_head-rgb) (send self :r_forearm_cam) (send self :l_forearm_cam) (send self :wide_stereo-right) (send self :wide_stereo-left) (send self :narrow_stereo-right) (send self :narrow_stereo-left)))
      ) ;; case pr1012
     ;; define cameras for pr1040
     (:pr1040
      ;; sensor_msgs::camerainfo #f(575.816 0.0 315.0 0.0 0.0 575.816 237.0 0.0 0.0 0.0 1.0 0.0)
      (setq kinect_head-depth (make-camera-from-ros-camera-info-aux 640 480 #f(575.816 0.0 315.0 0.0 0.0 575.816 237.0 0.0 0.0 0.0 1.0 0.0) head_mount_kinect_ir_optical_frame_lk :name :kinect_head/depth))

      ;; sensor_msgs::camerainfo #f(525.0 0.0 319.5 0.0 0.0 525.0 239.5 0.0 0.0 0.0 1.0 0.0)
      (setq kinect_head-rgb (make-camera-from-ros-camera-info-aux 640 480 #f(525.0 0.0 319.5 0.0 0.0 525.0 239.5 0.0 0.0 0.0 1.0 0.0) head_mount_kinect_rgb_optical_frame_lk :name :kinect_head/rgb))

      ;; sensor_msgs::camerainfo #f(2014.54 0.0 1197.49 0.0 0.0 2099.84 980.052 0.0 0.0 0.0 1.0 0.0)
      (setq prosilica (make-camera-from-ros-camera-info-aux 2448 2050 #f(2014.54 0.0 1197.49 0.0 0.0 2099.84 980.052 0.0 0.0 0.0 1.0 0.0) high_def_optical_frame_lk :name :prosilica))

      ;; sensor_msgs::camerainfo #f(341.275 0.0 326.117 0.0 0.0 382.992 244.101 0.0 0.0 0.0 1.0 0.0)
      (setq r_forearm_cam (make-camera-from-ros-camera-info-aux 640 480 #f(341.275 0.0 326.117 0.0 0.0 382.992 244.101 0.0 0.0 0.0 1.0 0.0) r_forearm_cam_optical_frame_lk :name :r_forearm_cam))

      ;; sensor_msgs::camerainfo #f(338.589 0.0 290.813 0.0 0.0 378.466 241.389 0.0 0.0 0.0 1.0 0.0)
      (setq l_forearm_cam (make-camera-from-ros-camera-info-aux 640 480 #f(338.589 0.0 290.813 0.0 0.0 378.466 241.389 0.0 0.0 0.0 1.0 0.0) l_forearm_cam_optical_frame_lk :name :l_forearm_cam))

      ;; sensor_msgs::camerainfo #f(445.634 0.0 240.474 -39.9524 0.0 445.634 193.274 0.0 0.0 0.0 1.0 0.0)
      (setq wide_stereo-right (make-camera-from-ros-camera-info-aux 640 480 #f(445.634 0.0 240.474 -39.9524 0.0 445.634 193.274 0.0 0.0 0.0 1.0 0.0) wide_stereo_optical_frame_lk :name :wide_stereo/right))

      ;; sensor_msgs::camerainfo #f(445.634 0.0 240.474 0.0 0.0 445.634 193.274 0.0 0.0 0.0 1.0 0.0)
      (setq wide_stereo-left (make-camera-from-ros-camera-info-aux 640 480 #f(445.634 0.0 240.474 0.0 0.0 445.634 193.274 0.0 0.0 0.0 1.0 0.0) wide_stereo_optical_frame_lk :name :wide_stereo/left))

      ;; sensor_msgs::camerainfo #f(910.712 0.0 320.414 -81.6247 0.0 910.712 263.573 0.0 0.0 0.0 1.0 0.0)
      (setq narrow_stereo-right (make-camera-from-ros-camera-info-aux 640 480 #f(910.712 0.0 320.414 -81.6247 0.0 910.712 263.573 0.0 0.0 0.0 1.0 0.0) narrow_stereo_optical_frame_lk :name :narrow_stereo/right))

      ;; sensor_msgs::camerainfo #f(910.712 0.0 320.414 0.0 0.0 910.712 263.573 0.0 0.0 0.0 1.0 0.0)
      (setq narrow_stereo-left (make-camera-from-ros-camera-info-aux 640 480 #f(910.712 0.0 320.414 0.0 0.0 910.712 263.573 0.0 0.0 0.0 1.0 0.0) narrow_stereo_optical_frame_lk :name :narrow_stereo/left))

      (setq cameras (list  (send self :kinect_head-depth) (send self :kinect_head-rgb) (send self :prosilica) (send self :r_forearm_cam) (send self :l_forearm_cam) (send self :wide_stereo-right) (send self :wide_stereo-left) (send self :narrow_stereo-right) (send self :narrow_stereo-left)))
      ) ;; case pr1040
   )
   self)

Seeing pr2.l you committed, (:pr1012 ...) and (:pr1040 ...) does exist. But in my understanding, we must have both /pr1012/robot_description and /pr1040/robot_description parameters to do this, but the real robot does not have such parameters. How can we reproduce your generating process?

---

Also, camera parameters for PR1040 are not fixed. I haven't checked if link offset is required, but at least the camera parameter is irrelevant to link offset.

yutouchimi@libra:~/Projects/pr2_subway/src/jsk-ros-pkg/jsk_pr2eus/pr2eus [(HEAD detached at k-okada/wide_stereo_camera_info)]
$ roseus pr2.l
configuring by "/opt/ros/kinetic/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 
connected to Xserver DISPLAY=:0
X events are being asynchronously monitored.
;; 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 ___time ___pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtpointcloud irtx eusjpeg euspng png irtimage irtglrgb 
;; extending gcstack 0x53d0300[16374] --> 0x58501b0[32748] top=3d61
irtgl irtglc irtviewer 
EusLisp 9.26( 1.2.1) for Linux64 created on ip-172-30-1-168(Wed Jun 12 16:16:30 PST 2019)
roseus ;; loading roseus("1.7.4-17-gea59620") on euslisp((9.26 ip-172-30-1-168 Wed Jun 12 16:16:30 PST 2019  1.2.1))
eustf roseus_c_util 1.irteusgl$ (pr2 :pr1040)
#<pr2-sensor-robot #X644a568 pr2  0.0 0.0 0.0 / 0.0 0.0 0.0>
2.irteusgl$ (send *pr2* :wide_stereo-left :cx)
240.474
3.irteusgl$ (send *pr2* :wide_stereo-left :cy)
193.274
4.irteusgl$ 

[k-okada]

@YutoUchimi added make-pr2-model-file.launch that generates pr2.l, also fixed generator and output different camera param for different robot, please check again.