search

jsk-ros-pkg / jsk_pr2eus

PR2 euslisp packages
https://github.com/jsk-ros-pkg/jsk_pr2eus
4 stars 41 forks source link

Fix wide_stereo camera model parameters #403

Closed YutoUchimi closed 4 years ago

YutoUchimi commented 5 years ago

cx and cy field of euslisp camera model should be about half of width and height, but parameters of wide_stereo are far from those for now. This can cause wrong output in ...

(send (send *pr2* :wide_stereo-left) :screen-point (send button-coords :worldpos))

so I fixed.

Parameters fixed in this PR are collected from PR1040's /wide_stereo/left/camera_info and /wide_stereo/right/camera_info.

k-okada commented 5 years ago

since pr.l is autogenerated file, so please fix a generator https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/master/pr2eus/make-pr2-model-file.l#L100

◉ Kei Okada

2019年10月18日(金) 16:07 Yuto Uchimi notifications@github.com:

cx and cy field of euslisp camera model should be about half of width and height, but parameters of wide_stereo are far from those for now. This can cause wrong output in ...

(send (send pr2 :wide_stereo-left) :screen-point (send button-coords :worldpos))

so I fixed.

Parameters fixed in this PR are collected from PR1040's /wide_stereo/left/camera_info and /wide_stereo/right/camera_info.

You can view, comment on, or merge this pull request online at:

https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/403 Commit Summary

  • Fix wide_stereo camera model parameters

File Changes

Patch Links:

— You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/403?email_source=notifications&email_token=AADYNXA6YDK3K2F5YWYIK5LQPFODLA5CNFSM4JCDNSC2YY3PNVWWK3TUL52HS4DFUVEXG43VMWVGG33NNVSW45C7NFSM4HSVEFBA, or unsubscribe https://github.com/notifications/unsubscribe-auth/AADYNXGURRFR56XOIAFQ72DQPFODLANCNFSM4JCDNSCQ .

YutoUchimi commented 5 years ago

OK, but could you please tell me how to run make-pr2-model-file.l? Should I install latest euscollada from source? My environment (Ubuntu16.04 + kinetic):

yutouchimi@libra:~/Projects/pr2_subway/src/jsk-ros-pkg/jsk_pr2eus/pr2eus [wide_stereo_camera_info]
$ apt show ros-kinetic-collada-urdf
Package: ros-kinetic-collada-urdf
Version: 1.12.12-0xenial-20190913-215040+0000
Priority: extra
Section: misc
Maintainer: Chris Lalancette <clalancette@osrfoundation.org>
Installed-Size: 586 kB
Depends: libassimp3v5, libboost-filesystem1.58.0, libboost-program-options1.58.0, libboost-system1.58.0, libc6 (>= 2.14), libcollada-dom2.4-dp0, libgcc1 (>= 1:3.0), libstdc++6 (>= 5.2), liburdfdom-model0.4, liburdfdom-world0.4, libassimp-dev, libcollada-dom2.4-dp-dev, liburdfdom-dev, liburdfdom-headers-dev, ros-kinetic-collada-parser, ros-kinetic-geometric-shapes, ros-kinetic-resource-retriever, ros-kinetic-rosconsole, ros-kinetic-urdf
Homepage: http://ros.org/wiki/collada_urdf
Download-Size: 159 kB
APT-Manual-Installed: no
APT-Sources: http://packages.ros.org/ros/ubuntu xenial/main amd64 Packages
Description: This package contains a tool to convert Unified Robot Description Format (URDF) documents into COLLAborative Design Activity (COLLADA) documents.
 Implements robot-specific COLLADA extensions as defined by http://openrave.programmingvision.com/index.php/Started:COLLADA

yutouchimi@libra:~/Projects/pr2_subway/src/jsk-ros-pkg/jsk_pr2eus/pr2eus [wide_stereo_camera_info]
$ apt show ros-kinetic-euscollada
Package: ros-kinetic-euscollada
Version: 0.4.2-0xenial-20190913-215712+0000
Priority: extra
Section: misc
Maintainer: Yohei Kakiuchi <youhei@jsk.t.u-tokyo.ac.jp>
Installed-Size: 596 kB
Depends: libassimp3v5, libboost-filesystem1.58.0, libboost-program-options1.58.0, libboost-system1.58.0, libc6 (>= 2.14), libcollada-dom2.4-dp0, libgcc1 (>= 1:3.0), libqhull7, libstdc++6 (>= 5.2), liburdfdom-model0.4, libyaml-cpp0.5v5, libcollada-dom2.4-dp-dev, libqhull-dev, liburdfdom-dev, libyaml-cpp-dev, ros-kinetic-assimp-devel, ros-kinetic-collada-parser, ros-kinetic-collada-urdf, ros-kinetic-resource-retriever, ros-kinetic-roscpp, ros-kinetic-rospack, ros-kinetic-rostest, ros-kinetic-tf, ros-kinetic-urdf
Homepage: http://ros.org/wiki/euscollada
Download-Size: 161 kB
APT-Manual-Installed: yes
APT-Sources: http://packages.ros.org/ros/ubuntu xenial/main amd64 Packages
Description: euscollada

I tried:

[http://pr1040:11311][133.11.216.166] yutouchimi@libra:~/Projects/pr2_subway/src/jsk-ros-pkg/jsk_pr2eus/pr2eus [wide_stereo_camera_info]
$ rossetmaster pr1040
set ROS_MASTER_URI to http://pr1040:11311
[http://pr1040:11311][133.11.216.166] yutouchimi@libra:~/Projects/pr2_subway/src/jsk-ros-pkg/jsk_pr2eus/pr2eus [wide_stereo_camera_info]
$ roseus make-pr2-model-file.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 0x5531300[16374] --> 0x59b11a0[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$ (ros::roseus "make_pr2_model_file")
t
2.irteusgl$ (make-pr2-model-file)

Document successfully written to /tmp/pr2_31533.dae
;; Input file is: /tmp/pr2_31533.dae
[ INFO] [1571388362.858544356] [${node}]: joint base_footprint_joint is passive, but adding to hierarchy

[ INFO] [1571388362.858691191] [${node}]: joint base_bellow_joint is passive, but adding to hierarchy

[ INFO] [1571388362.858893852] [${node}]: joint base_laser_joint is passive, but adding to hierarchy

[ INFO] [1571388362.892299279] [${node}]: joint head_chain_cb is passive, but adding to hierarchy

[ INFO] [1571388362.892408032] [${node}]: joint head_plate_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.892520218] [${node}]: joint head_mount_joint is passive, but adding to hierarchy

[ INFO] [1571388362.893477205] [${node}]: joint head_mount_kinect_ir_joint is passive, but adding to hierarchy

[ INFO] [1571388362.893561336] [${node}]: joint head_mount_kinect_ir_optical_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.893630839] [${node}]: joint head_mount_kinect_rgb_joint is passive, but adding to hierarchy

[ INFO] [1571388362.893726804] [${node}]: joint head_mount_kinect_rgb_optical_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.894333946] [${node}]: joint head_mount_prosilica_joint is passive, but adding to hierarchy

[ INFO] [1571388362.894420042] [${node}]: joint head_mount_prosilica_optical_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.944749814] [${node}]: joint projector_wg6802418_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.944872636] [${node}]: joint projector_wg6802418_child_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.945010350] [${node}]: joint sensor_mount_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.945107131] [${node}]: joint double_stereo_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.945210209] [${node}]: joint narrow_stereo_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.945298241] [${node}]: joint narrow_stereo_l_stereo_camera_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.945382999] [${node}]: joint narrow_stereo_l_stereo_camera_optical_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.945465868] [${node}]: joint narrow_stereo_r_stereo_camera_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.945533777] [${node}]: joint narrow_stereo_r_stereo_camera_optical_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.945605237] [${node}]: joint narrow_stereo_optical_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.945696502] [${node}]: joint wide_stereo_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.945783724] [${node}]: joint wide_stereo_l_stereo_camera_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.945866959] [${node}]: joint wide_stereo_l_stereo_camera_optical_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.945952946] [${node}]: joint wide_stereo_r_stereo_camera_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.946038079] [${node}]: joint wide_stereo_r_stereo_camera_optical_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.946124916] [${node}]: joint wide_stereo_optical_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.946273755] [${node}]: joint high_def_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.946326590] [${node}]: joint high_def_optical_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.952084248] [${node}]: joint imu_joint is passive, but adding to hierarchy

[ INFO] [1571388362.952540294] [${node}]: joint l_upper_arm_joint is passive, but adding to hierarchy

[ INFO] [1571388362.952870564] [${node}]: joint l_forearm_cam_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.952942733] [${node}]: joint l_forearm_cam_optical_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.953053357] [${node}]: joint l_forearm_joint is passive, but adding to hierarchy

[ INFO] [1571388362.953421544] [${node}]: joint l_gripper_palm_joint is passive, but adding to hierarchy

[ INFO] [1571388362.962059801] [${node}]: joint l_gripper_led_joint is passive, but adding to hierarchy

[ INFO] [1571388362.962161721] [${node}]: joint l_gripper_motor_accelerometer_joint is passive, but adding to hierarchy

[ INFO] [1571388362.970883305] [${node}]: joint l_gripper_tool_joint is passive, but adding to hierarchy

[ INFO] [1571388362.973651670] [${node}]: joint left_arm_chain_cb is passive, but adding to hierarchy

[ INFO] [1571388363.002012903] [${node}]: joint l_torso_lift_side_plate_joint is passive, but adding to hierarchy

[ INFO] [1571388363.002255651] [${node}]: joint laser_tilt_joint is passive, but adding to hierarchy

[ INFO] [1571388363.005437126] [${node}]: joint r_upper_arm_joint is passive, but adding to hierarchy

[ INFO] [1571388363.005842699] [${node}]: joint r_forearm_cam_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388363.005937324] [${node}]: joint r_forearm_cam_optical_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388363.006048225] [${node}]: joint r_forearm_joint is passive, but adding to hierarchy

[ INFO] [1571388363.006478557] [${node}]: joint r_gripper_palm_joint is passive, but adding to hierarchy

[ INFO] [1571388363.014939571] [${node}]: joint r_gripper_led_joint is passive, but adding to hierarchy

[ INFO] [1571388363.015044962] [${node}]: joint r_gripper_motor_accelerometer_joint is passive, but adding to hierarchy

[ INFO] [1571388363.023737542] [${node}]: joint r_gripper_tool_joint is passive, but adding to hierarchy

[ INFO] [1571388363.026599531] [${node}]: joint right_arm_chain_cb is passive, but adding to hierarchy

[ INFO] [1571388363.054407921] [${node}]: joint r_torso_lift_side_plate_joint is passive, but adding to hierarchy

;; Config file is: /opt/ros/kinetic/share/euscollada/pr2.yaml
;; Output file is: /tmp/pr2_31533.l
;; robot_name is: pr2
torso@5
larm@7
rarm@15
head@23
load model file from parameter server /robot_description to /tmp/pr2_31533.l
received narrow_stereo/left/camera_info #<sensor_msgs::camerainfo #X58253b0> narrow_stereo_optical_frame
received narrow_stereo/right/camera_info #<sensor_msgs::camerainfo #X5929bd0> narrow_stereo_optical_frame
received wide_stereo/left/camera_info #<sensor_msgs::camerainfo #X5b01d28> wide_stereo_optical_frame
received wide_stereo/right/camera_info #<sensor_msgs::camerainfo #X5a67008> wide_stereo_optical_frame
received l_forearm_cam/camera_info #<sensor_msgs::camerainfo #X58cde30> l_forearm_cam_optical_frame
received r_forearm_cam/camera_info #<sensor_msgs::camerainfo #X64aa218> r_forearm_cam_optical_frame
received prosilica/camera_info #<sensor_msgs::camerainfo #X64b40c8> high_def_optical_frame
received kinect_head/rgb/camera_info #<sensor_msgs::camerainfo #X64cdb40> head_mount_kinect_rgb_optical_frame
[ERROR] [1571388372.414675320] [/make_pr2_model_file_1571388345260520413]: could not receive kinect_head/depth/camera_info kinect_head-depth
copy model file from /tmp/pr2_31533.l to /home/yutouchimi/Projects/pr2_subway/src/jsk-ros-pkg/jsk_pr2eus/pr2eus/pr2.l
0
3.irteusgl$ (exit)
[ INFO] [1571388541.216984430] [/make_pr2_model_file_1571388345260520413]: cell* ROSEUS_EXIT(context*, int, cell**)
[ INFO] [1571388541.217055228] [/make_pr2_model_file_1571388345260520413]: exiting roseus 0
[http://pr1040:11311][133.11.216.166] yutouchimi@libra:~/Projects/pr2_subway/src/jsk-ros-pkg/jsk_pr2eus/pr2eus [wide_stereo_camera_info]
$ git status
ブランチ wide_stereo_camera_info
Changes not staged for commit:
  (use "git add <file>..." to update what will be committed)
  (use "git checkout -- <file>..." to discard changes in working directory)

    modified:   pr2.l

no changes added to commit (use "git add" and/or "git commit -a")
[http://pr1040:11311][133.11.216.166] yutouchimi@libra:~/Projects/pr2_subway/src/jsk-ros-pkg/jsk_pr2eus/pr2eus [wide_stereo_camera_info]
$ ls -lh pr2.l 
-rw-rw-r-- 1 yutouchimi yutouchimi 4.7M 10月 18 17:46 pr2.l

Large diff is generated (12.7 MB -> 4.7MB), so I can hardly check if this is correct way.

k-okada commented 5 years ago

https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/master/pr2eus/test/make-pr2-model-file-test.launch tell you how to generate and check the model generator and generated model

◉ Kei Okada

2019年10月18日(金) 18:04 Yuto Uchimi notifications@github.com:

OK, but could you please tell me how to run make-pr2-model-file.l? Should I install latest euscollada from source? My environment (Ubuntu16.04 + kinetic):

yutouchimi@libra:~/Projects/pr2_subway/src/jsk-ros-pkg/jsk_pr2eus/pr2eus [wide_stereo_camera_info]

$ apt show ros-kinetic-collada-urdf

Package: ros-kinetic-collada-urdf

Version: 1.12.12-0xenial-20190913-215040+0000

Priority: extra

Section: misc

Maintainer: Chris Lalancette clalancette@osrfoundation.org

Installed-Size: 586 kB

Depends: libassimp3v5, libboost-filesystem1.58.0, libboost-program-options1.58.0, libboost-system1.58.0, libc6 (>= 2.14), libcollada-dom2.4-dp0, libgcc1 (>= 1:3.0), libstdc++6 (>= 5.2), liburdfdom-model0.4, liburdfdom-world0.4, libassimp-dev, libcollada-dom2.4-dp-dev, liburdfdom-dev, liburdfdom-headers-dev, ros-kinetic-collada-parser, ros-kinetic-geometric-shapes, ros-kinetic-resource-retriever, ros-kinetic-rosconsole, ros-kinetic-urdf

Homepage: http://ros.org/wiki/collada_urdf

Download-Size: 159 kB

APT-Manual-Installed: no

APT-Sources: http://packages.ros.org/ros/ubuntu xenial/main amd64 Packages

Description: This package contains a tool to convert Unified Robot Description Format (URDF) documents into COLLAborative Design Activity (COLLADA) documents.

Implements robot-specific COLLADA extensions as defined by http://openrave.programmingvision.com/index.php/Started:COLLADA

yutouchimi@libra:~/Projects/pr2_subway/src/jsk-ros-pkg/jsk_pr2eus/pr2eus [wide_stereo_camera_info]

$ apt show ros-kinetic-euscollada

Package: ros-kinetic-euscollada

Version: 0.4.2-0xenial-20190913-215712+0000

Priority: extra

Section: misc

Maintainer: Yohei Kakiuchi youhei@jsk.t.u-tokyo.ac.jp

Installed-Size: 596 kB

Depends: libassimp3v5, libboost-filesystem1.58.0, libboost-program-options1.58.0, libboost-system1.58.0, libc6 (>= 2.14), libcollada-dom2.4-dp0, libgcc1 (>= 1:3.0), libqhull7, libstdc++6 (>= 5.2), liburdfdom-model0.4, libyaml-cpp0.5v5, libcollada-dom2.4-dp-dev, libqhull-dev, liburdfdom-dev, libyaml-cpp-dev, ros-kinetic-assimp-devel, ros-kinetic-collada-parser, ros-kinetic-collada-urdf, ros-kinetic-resource-retriever, ros-kinetic-roscpp, ros-kinetic-rospack, ros-kinetic-rostest, ros-kinetic-tf, ros-kinetic-urdf

Homepage: http://ros.org/wiki/euscollada

Download-Size: 161 kB

APT-Manual-Installed: yes

APT-Sources: http://packages.ros.org/ros/ubuntu xenial/main amd64 Packages

Description: euscollada

I tried:

[http://pr1040:11311][133.11.216.166] yutouchimi@libra:~/Projects/pr2_subway/src/jsk-ros-pkg/jsk_pr2eus/pr2eus [wide_stereo_camera_info]

$ rossetmaster pr1040

set ROS_MASTER_URI to http://pr1040:11311

[http://pr1040:11311][133.11.216.166] yutouchimi@libra:~/Projects/pr2_subway/src/jsk-ros-pkg/jsk_pr2eus/pr2eus [wide_stereo_camera_info]

$ roseus make-pr2-model-file.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 0x5531300[16374] --> 0x59b11a0[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$ (ros::roseus "make_pr2_model_file")

t

2.irteusgl$ (make-pr2-model-file)

Document successfully written to /tmp/pr2_31533.dae

;; Input file is: /tmp/pr2_31533.dae

[ INFO] [1571388362.858544356] [${node}]: joint base_footprint_joint is passive, but adding to hierarchy

[ INFO] [1571388362.858691191] [${node}]: joint base_bellow_joint is passive, but adding to hierarchy

[ INFO] [1571388362.858893852] [${node}]: joint base_laser_joint is passive, but adding to hierarchy

[ INFO] [1571388362.892299279] [${node}]: joint head_chain_cb is passive, but adding to hierarchy

[ INFO] [1571388362.892408032] [${node}]: joint head_plate_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.892520218] [${node}]: joint head_mount_joint is passive, but adding to hierarchy

[ INFO] [1571388362.893477205] [${node}]: joint head_mount_kinect_ir_joint is passive, but adding to hierarchy

[ INFO] [1571388362.893561336] [${node}]: joint head_mount_kinect_ir_optical_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.893630839] [${node}]: joint head_mount_kinect_rgb_joint is passive, but adding to hierarchy

[ INFO] [1571388362.893726804] [${node}]: joint head_mount_kinect_rgb_optical_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.894333946] [${node}]: joint head_mount_prosilica_joint is passive, but adding to hierarchy

[ INFO] [1571388362.894420042] [${node}]: joint head_mount_prosilica_optical_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.944749814] [${node}]: joint projector_wg6802418_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.944872636] [${node}]: joint projector_wg6802418_child_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.945010350] [${node}]: joint sensor_mount_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.945107131] [${node}]: joint double_stereo_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.945210209] [${node}]: joint narrow_stereo_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.945298241] [${node}]: joint narrow_stereo_l_stereo_camera_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.945382999] [${node}]: joint narrow_stereo_l_stereo_camera_optical_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.945465868] [${node}]: joint narrow_stereo_r_stereo_camera_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.945533777] [${node}]: joint narrow_stereo_r_stereo_camera_optical_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.945605237] [${node}]: joint narrow_stereo_optical_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.945696502] [${node}]: joint wide_stereo_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.945783724] [${node}]: joint wide_stereo_l_stereo_camera_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.945866959] [${node}]: joint wide_stereo_l_stereo_camera_optical_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.945952946] [${node}]: joint wide_stereo_r_stereo_camera_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.946038079] [${node}]: joint wide_stereo_r_stereo_camera_optical_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.946124916] [${node}]: joint wide_stereo_optical_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.946273755] [${node}]: joint high_def_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.946326590] [${node}]: joint high_def_optical_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.952084248] [${node}]: joint imu_joint is passive, but adding to hierarchy

[ INFO] [1571388362.952540294] [${node}]: joint l_upper_arm_joint is passive, but adding to hierarchy

[ INFO] [1571388362.952870564] [${node}]: joint l_forearm_cam_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.952942733] [${node}]: joint l_forearm_cam_optical_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.953053357] [${node}]: joint l_forearm_joint is passive, but adding to hierarchy

[ INFO] [1571388362.953421544] [${node}]: joint l_gripper_palm_joint is passive, but adding to hierarchy

[ INFO] [1571388362.962059801] [${node}]: joint l_gripper_led_joint is passive, but adding to hierarchy

[ INFO] [1571388362.962161721] [${node}]: joint l_gripper_motor_accelerometer_joint is passive, but adding to hierarchy

[ INFO] [1571388362.970883305] [${node}]: joint l_gripper_tool_joint is passive, but adding to hierarchy

[ INFO] [1571388362.973651670] [${node}]: joint left_arm_chain_cb is passive, but adding to hierarchy

[ INFO] [1571388363.002012903] [${node}]: joint l_torso_lift_side_plate_joint is passive, but adding to hierarchy

[ INFO] [1571388363.002255651] [${node}]: joint laser_tilt_joint is passive, but adding to hierarchy

[ INFO] [1571388363.005437126] [${node}]: joint r_upper_arm_joint is passive, but adding to hierarchy

[ INFO] [1571388363.005842699] [${node}]: joint r_forearm_cam_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388363.005937324] [${node}]: joint r_forearm_cam_optical_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388363.006048225] [${node}]: joint r_forearm_joint is passive, but adding to hierarchy

[ INFO] [1571388363.006478557] [${node}]: joint r_gripper_palm_joint is passive, but adding to hierarchy

[ INFO] [1571388363.014939571] [${node}]: joint r_gripper_led_joint is passive, but adding to hierarchy

[ INFO] [1571388363.015044962] [${node}]: joint r_gripper_motor_accelerometer_joint is passive, but adding to hierarchy

[ INFO] [1571388363.023737542] [${node}]: joint r_gripper_tool_joint is passive, but adding to hierarchy

[ INFO] [1571388363.026599531] [${node}]: joint right_arm_chain_cb is passive, but adding to hierarchy

[ INFO] [1571388363.054407921] [${node}]: joint r_torso_lift_side_plate_joint is passive, but adding to hierarchy

;; Config file is: /opt/ros/kinetic/share/euscollada/pr2.yaml

;; Output file is: /tmp/pr2_31533.l

;; robot_name is: pr2

torso@5

larm@7

rarm@15

head@23

load model file from parameter server /robot_description to /tmp/pr2_31533.l

received narrow_stereo/left/camera_info #<sensor_msgs::camerainfo #X58253b0> narrow_stereo_optical_frame

received narrow_stereo/right/camera_info #<sensor_msgs::camerainfo #X5929bd0> narrow_stereo_optical_frame

received wide_stereo/left/camera_info #<sensor_msgs::camerainfo #X5b01d28> wide_stereo_optical_frame

received wide_stereo/right/camera_info #<sensor_msgs::camerainfo #X5a67008> wide_stereo_optical_frame

received l_forearm_cam/camera_info #<sensor_msgs::camerainfo #X58cde30> l_forearm_cam_optical_frame

received r_forearm_cam/camera_info #<sensor_msgs::camerainfo #X64aa218> r_forearm_cam_optical_frame

received prosilica/camera_info #<sensor_msgs::camerainfo #X64b40c8> high_def_optical_frame

received kinect_head/rgb/camera_info #<sensor_msgs::camerainfo #X64cdb40> head_mount_kinect_rgb_optical_frame

[ERROR] [1571388372.414675320] [/make_pr2_model_file_1571388345260520413]: could not receive kinect_head/depth/camera_info kinect_head-depth

copy model file from /tmp/pr2_31533.l to /home/yutouchimi/Projects/pr2_subway/src/jsk-ros-pkg/jsk_pr2eus/pr2eus/pr2.l

0

3.irteusgl$ (exit)

[ INFO] [1571388541.216984430] [/make_pr2_model_file_1571388345260520413]: cell ROSEUS_EXIT(context, int, cell**)

[ INFO] [1571388541.217055228] [/make_pr2_model_file_1571388345260520413]: exiting roseus 0

[http://pr1040:11311][133.11.216.166] yutouchimi@libra:~/Projects/pr2_subway/src/jsk-ros-pkg/jsk_pr2eus/pr2eus [wide_stereo_camera_info]

$ git status

ブランチ wide_stereo_camera_info

Changes not staged for commit:

(use "git add ..." to update what will be committed)

(use "git checkout -- ..." to discard changes in working directory)

modified: pr2.l

no changes added to commit (use "git add" and/or "git commit -a")

[http://pr1040:11311][133.11.216.166] yutouchimi@libra:~/Projects/pr2_subway/src/jsk-ros-pkg/jsk_pr2eus/pr2eus [wide_stereo_camera_info]

$ ls -lh pr2.l

-rw-rw-r-- 1 yutouchimi yutouchimi 4.7M 10月 18 17:46 pr2.l

Large diff is generated (12.7 MB -> 4.7MB), so I can hardly check if this is correct way.

— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/403?email_source=notifications&email_token=AADYNXBGPATEZUI4NM2V32LQPF3YDA5CNFSM4JCDNSC2YY3PNVWWK3TUL52HS4DFVREXG43VMVBW63LNMVXHJKTDN5WW2ZLOORPWSZGOEBTOV6Q#issuecomment-543615738, or unsubscribe https://github.com/notifications/unsubscribe-auth/AADYNXGNKZAC7L4J7TZAI6LQPF3YDANCNFSM4JCDNSCQ .

YutoUchimi commented 5 years ago

With newly generated PR2 model, I confirmed that the center of wide_stereo is correctly set.

2.irteusgl$ (send (send *pr2* :wide_stereo-left) :cx)
321.406
3.irteusgl$ (send (send *pr2* :wide_stereo-left) :cy)
241.11
YutoUchimi commented 5 years ago

Travis passed!

k-okada commented 4 years ago

I am not sure why this fix your problem, because make-camera-from-ros-camera-info-aux and camera parameter seems almost same for me.

-(defun make-camera-from-ros-camera-info-aux (pwidth pheight p frame-coords &rest args)
-  (let* ((fx (elt p 0))(fy (elt p 5))
-         (cx (elt p 2))(cy (elt p 6))
-         (fx*tx (elt p 3))(fy*ty (elt p 7))
-         (tx (* 1000 (/ fx*tx fx)))
-         (ty (* 1000 (/ fy*ty fy)))
-         )
-    (apply #'make-camera-from-param :pwidth pwidth :pheight pheight
-          :fx fx :fy fy :cx cx :cy cy
-          :tx tx :ty ty :parent-coords frame-coords args)))
+(defun make-camera-from-ros-camera-info-aux (pwidth pheight p frame-coords &rest args) (let* ((fx (elt p 0)) (fy (elt p 5)) (cx (elt p 2)) (cy (elt p 6)) (fx*tx (elt p 3)) (fy*ty (elt p 7)) (tx (* 1000 (/ fx*tx fx))) (ty (* 1000 (/ fy*ty fy)))) (apply #'make-camera-from-param :pwidth pwidth :pheight pheight :fx fx :fy fy :cx cx :cy cy :tx tx :ty ty :parent-coords frame-coords args)))

-   ;; #<sensor_msgs::camerainfo #X63fd3f0> #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 #X62f1578> #f(386.179 0.0 321.406 0.0 0.0 386.179 241.11 0.0 0.0 0.0 1.0 0.0)
+   (setq wide_stereo-left (make-camera-from-ros-camera-info-aux 640 480 #f(386.179 0.0 321.406 0.0 0.0 386.179 241.11 0.0 0.0 0.0 1.0 0.0) wide_stereo_optical_frame_lk :name :wide_stereo/left))
YutoUchimi commented 4 years ago

make-camera-from-ros-camera-info-aux has no functional change in this PR.

But the camera matrix was definitely changed! #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) -> #f(386.179 0.0 321.406 0.0 0.0 386.179 241.11 0.0 0.0 0.0 1.0 0.0) Especially (cx, cy): (240.474, 193.274) -> (321.406, 241.11) is a dramatical improvement. I came up with the idea that previous parameter was the result of too rough camera calibration. This is why I added a rosbag file for PR1040, from which camera parameter above is generated.

YutoUchimi commented 4 years ago

Move to #426