Fix wide_stereo camera model parameters #426

k-okada · 2019-12-15T09:25:50Z

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 · 2019-12-17T12:16:16Z

@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 · 2019-12-19T05:38:32Z

  (: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$

pr2eus/make-pr2-model-file.l

k-okada · 2019-12-19T10:46:37Z

@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.

YutoUchimi · 2019-12-20T04:14:41Z

pr2eus/make-pr2-model-file.l

+
+(defun get-pr2-names ()
+  ;; get list of rosparam
+  (reverse (mapcan #'(lambda (x) (if (substringp "robot_description" x) (list (subseq x 1 (- (length x) (length "/robot_description"))))))


Thanks for updating, but error occurs while executing make-pr2-model-file-test.launch: https://travis-ci.org/jsk-ros-pkg/jsk_pr2eus/jobs/627167295#L5569-L5580

So we have to change that test launch file like

diff --git a/pr2eus/test/make-pr2-model-file-test.launch b/pr2eus/test/make-pr2-model-file-test.launch index a43362d..0fa2292 100644 --- a/pr2eus/test/make-pr2-model-file-test.launch +++ b/pr2eus/test/make-pr2-model-file-test.launch @@ -1,9 +1,11 @@ <launch> - <arg name="PR2_NO" default="1012" /> - <rosparam command="load" file="$(find pr2eus)/test/pr$(arg PR2_NO)_urdf.yaml" param="robot_description"/> - <node name="pr2_camera_info" pkg="rosbag" type="play" - args="-l $(find pr2eus)/test/pr1012_camera_info.bag" /> + <rosparam command="load" file="$(find pr2eus)/test/pr1012_urdf.yaml" param="/pr1012/robot_description"/> + <rosparam command="load" file="$(find pr2eus)/test/pr1040_urdf.yaml" param="/pr1040/robot_description"/> + <node name="pr1012_camera_info" pkg="rosbag" type="play" + args="--prefix=/pr1012 -l $(find pr2eus)/test/pr1012_camera_info.bag" /> + <node name="pr1040_camera_info" pkg="rosbag" type="play" + args="--prefix=/pr1040 -l $(find pr2eus)/test/pr1040_camera_info.bag" /> <test test-name="make_pr2_model_file_test" pkg="roseus" type="roseus" - args="$(find pr2eus)/test/make-pr2-model-file-test.l" + args="$(find pr2eus)/test/make-pr2-model-file-test.l" time-limit="1800" /> -</launch> \ No newline at end of file +</launch> (END)

So we have to change that test launch file like

So we have to fix this error, to keep backward compatibility for example b891ba4

and you fix did not test pr1040 case, so can you update the test code?

YutoUchimi · 2019-12-20T04:17:04Z

pr2eus/make-pr2-model-file.l

+                "narrow_stereo/right"
+                "wide_stereo/left"
+                "wide_stereo/right"
+                 "l_forearm_cam"


Please align indent.

… 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));" ```

…tion/gazebo_ros_pkgs#704 for more info

…ed in test/pr2-ri-test.launch

… before (send *ri* :angle-vector-duration (send *ri* :state :potentio-vector) (send *pr2* :angle-vector) 5 1.0)

k-okada changed the title ~~[pr2eus_tutorials] add pr2 tabletop demo test and sample~~ Fix wide_stereo camera model parameters Dec 15, 2019

pazeshun requested changes Dec 19, 2019

View reviewed changes

pr2eus/make-pr2-model-file.l Outdated Show resolved Hide resolved

k-okada force-pushed the wide_stereo_camera_info branch from 1be1bfe to c70059f Compare December 19, 2019 10:44

YutoUchimi requested changes Dec 20, 2019

View reviewed changes

YutoUchimi mentioned this pull request Dec 20, 2019

Fix wide_stereo camera model parameters #403

Closed

k-okada force-pushed the wide_stereo_camera_info branch from 6b7d974 to b891ba4 Compare December 20, 2019 05:18

knorth55 mentioned this pull request Dec 20, 2019

[WIP] Merge PR1040 branch #407

Closed

3 tasks

This was referenced Dec 21, 2019

Test both pr1012 and pr1040 k-okada/jsk_pr2eus#18

Open

Subway demo jsk-ros-pkg/jsk_demos#1276

Open

knorth55 mentioned this pull request Nov 11, 2021

pr2.l をロードしてからfetch-init するとエラーが出る jsk-ros-pkg/jsk_robot#1407

Open

k-okada and others added 9 commits November 12, 2021 14:57

generate with euscollada 0.4.3 (with jsk-ros-pkg/jsk_model_tools#227 )

5e33509

Update pr2 model file from pr1040 urdf and camera_info

1451fc9

fix make-pr2-model-file.l to support multiple robot

b3e2738

update pr2.l for both pr1012/pr1040

81034ea

add launch file to generate pr2.l

7db436b

use subseq, according to @pazeshun 's comment

cd4c858

fix for backword compatibility (robot_description without prefix)

b84c443

[pr2eus/make-pr2-model-file.l] fix indent on (setq camera-paths

c1b65f4

k-okada force-pushed the wide_stereo_camera_info branch from 17fbe8c to c1b65f4 Compare November 12, 2021 06:19

k-okada added 3 commits November 12, 2021 18:54

need tose gazebo/enable_ros_network to publish /clock, see ros-simula…

f05e592

…tion/gazebo_ros_pkgs#704 for more info

fix test name for test/pr2-ri-test-simple.launch, which is already us…

266b27d

…ed in test/pr2-ri-test.launch

pr2eus/test/pr2-ri-test-simple.l: add (send *ri* :wait-interpolation)…

37e44f7

… before (send *ri* :angle-vector-duration (send *ri* :state :potentio-vector) (send *pr2* :angle-vector) 5 1.0)

k-okada merged commit f26979e into jsk-ros-pkg:master Nov 17, 2021

knorth55 mentioned this pull request Nov 17, 2021

Reproduce of PR2 Subway Fetch Demo. jsk-ros-pkg/jsk_demos#1321

Open

6 tasks

k-okada mentioned this pull request Nov 17, 2021

add test code for (pr2) and (instance pr2-robot :init) (instance r2-sensor-robot :init) #466

Merged

tkmtnt7000 mentioned this pull request Dec 7, 2023

Add Subway demo jsk-ros-pkg/jsk_demos#1397

Open

2 tasks

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Fix wide_stereo camera model parameters #426

Fix wide_stereo camera model parameters #426

k-okada commented Dec 15, 2019 •

edited

k-okada commented Dec 17, 2019

YutoUchimi commented Dec 19, 2019

k-okada commented Dec 19, 2019

YutoUchimi Dec 20, 2019

k-okada Dec 20, 2019

YutoUchimi Dec 20, 2019

Fix wide_stereo camera model parameters #426

Fix wide_stereo camera model parameters #426

Conversation

k-okada commented Dec 15, 2019 • edited

k-okada commented Dec 17, 2019

YutoUchimi commented Dec 19, 2019

k-okada commented Dec 19, 2019

YutoUchimi Dec 20, 2019

Choose a reason for hiding this comment

k-okada Dec 20, 2019

Choose a reason for hiding this comment

YutoUchimi Dec 20, 2019

Choose a reason for hiding this comment

k-okada commented Dec 15, 2019 •

edited