[dxl_armed_turtlebot / euslisp] display-checkerboard.l の/image_markerの結果の確認の仕方について

[kochigami]

roslaunch turtlebot_bringup minimal.launch
roslaunch turtlebot_bringup 3dsensor.launch 
roslaunch roseus_tutorials checkerboard_detector.launch rect0_size_x:=0.025 rect0_size_y:=0.025 grid0_size_x:=5 grid0_size_y:=4 translation0:="0 0 0" image:=image_rect_mono group:=/camera/rgb frame_id:=camera_rgb_frame
roseus display-checkerboard.l

を行い，表示されたCheckerBoard Detector という名前のカメラ画像・euslisp実行端末の表示文字列から，チェッカーボード検出の確認をしました．

dxl_armed_turtlebot/euslisp/display-checkerboard.l の， チェッカーボードを見つけた後のobjectdetection-cb の処理の以下の部分，

 ;; image_markerを出力                                                       
    (dolist (obj-pose (send msg :objects))
      (setq type (send obj-pose :type))
      (unless (eq (char type 0) #\/) (setq type (concatenate string "/" type)))
      (setq ret (ros::tf-pose->coords (send obj-pose :pose)))
      (send mrk :type image_view2::ImageMarker2::*FRAMES*)
      (send mrk :frames (list type))
      (send *tfb* :send-transform ret frame-id type)
      (ros::ros-info "~A ~A ~A" ret frame-id type)
      (ros::publish "image_marker" mrk))

で新たに立ち上げたimage_view2 のビューワに何かマーカが出るのかと思うのですが， rosrun image_view2 image_view2 image:=/camera/rgb/image_raw camera_info:=/camera/rgb/camera_info　すると，

[ERROR] [1458459132.769960178]: [image_view2] TF exception:
Lookup would require extrapolation into the future.  Requested time 1458459131.754825312 but the latest data is at time 1458459129.735534750, when looking up transform from frame [test_object] to frame [camera_rgb_optical_frame]

とチェッカーボードとカメラの座標変換がうまくいっていないとエラーが出つづけ，マーカは出ません．

rostopic echo /image_marker
header: 
  seq: 0
  stamp: 
    secs: 0
    nsecs: 0
  frame_id: ''
ns: ''
id: 0
type: 5
action: 0
position: 
  x: 0.0
  y: 0.0
  z: 0.0
position3D: 
  header: 
    seq: 0
    stamp: 
      secs: 0
      nsecs: 0
    frame_id: ''
  point: 
    x: 0.0
    y: 0.0
    z: 0.0
pose: 
  header: 
    seq: 0
    stamp: 
      secs: 0
      nsecs: 0
    frame_id: ''
  pose: 
    position: 
      x: 0.0
      y: 0.0
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.0
      w: 0.0
scale: 0.0
width: 0.0
outline_color: 
  r: 0.0
  g: 0.0
  b: 0.0
  a: 0.0
filled: 0
fill_color: 
  r: 0.0
  g: 0.0
  b: 0.0
  a: 0.0
lifetime: 
  secs: 0
  nsecs: 0
arc: 0
angle: 0.0
points: []
points3D: 
  header: 
    seq: 0
    stamp: 
      secs: 0
      nsecs: 0
    frame_id: ''
  points: []
outline_colors: []
frames: ['/test_object']
text: ''
left_up_origin: False
ratio_scale: False

---

となっており，座標がうまく渡されていません． しかしこの変換は，display-checkerboard.lの中の，

  (send *tfb* :send-transform ret frame-id type) ;; ret: checkerboardのカメラ相対の座標 frame-id: /camera_rgb_frame type: /test_object (checkerboard) 

で行われているのではないかと思います． 勉強不足ですので，分かったらissueを閉じます．

[k-okada]

マシンを２台以上使っていると時間があっていない時にこのエラーがでますが， そんなにマシンつかっている？

あと，どのマシンでどのノードが走っているかは

rosnode list | xargs -n1 rosnode info 2&>1 rosnode.txt

をみればいいかな．

とりあえずTFでこまったら rosrun tf view_frames してでてくるpdf を貼り付けてみて下さい．

http://wiki.ros.org/tf/Tutorials/Debugging%20tf%20problems によるとtf_monitorというのも有効のようです．

．

◉ Kei Okada

2016-03-20 16:59 GMT+09:00 Kanae Kochigami notifications@github.com:

roslaunch turtlebot_bringup minimal.launch roslaunch turtlebot_bringup 3dsensor.launch roslaunch roseus_tutorials checkerboard_detector.launch rect0_size_x:=0.025 rect0_size_y:=0.025 grid0_size_x:=5 grid0_size_y:=4 translation0:="0 0 0" image:=image_rect_mono group:=/camera/rgb frame_id:=camera_rgb_frame roseus display-checkerboard.l

を行い，表示されたCheckerBoard Detector という名前のカメラ画像・euslisp実行端末の表示文字列から，チェッカーボード検出の確認をしました．

dxl_armed_turtlebot/euslisp/display-checkerboard.l の， チェッカーボードを見つけた後のobjectdetection-cb の処理の以下の部分，

;; image_markerを出力 (dolist (obj-pose (send msg :objects)) (setq type (send obj-pose :type)) (unless (eq (char type 0) #\/) (setq type (concatenate string "/" type))) (setq ret (ros::tf-pose->coords (send obj-pose :pose))) (send mrk :type image_view2::ImageMarker2::FRAMES) (send mrk :frames (list type)) (send tfb :send-transform ret frame-id type) (ros::ros-info "~A ~A ~A" ret frame-id type) (ros::publish "image_marker" mrk))

で新たに立ち上げたimage_view2 のビューワに何かマーカが出るのかと思うのですが， rosrun image_view2 image_view2 image:=/camera/rgb/image_raw camera_info:=/camera/rgb/camera_info すると，

[ERROR] [1458459132.769960178]: [image_view2] TF exception: Lookup would require extrapolation into the future. Requested time 1458459131.754825312 but the latest data is at time 1458459129.735534750, when looking up transform from frame [test_object] to frame [camera_rgb_optical_frame]

とチェッカーボードとカメラの座標変換がうまくいっていないとエラーが出つづけ，マーカは出ません．

rostopic echo /image_marker header: seq: 0 stamp: secs: 0 nsecs: 0 frame_id: '' ns: '' id: 0 type: 5 action: 0 position: x: 0.0 y: 0.0 z: 0.0 position3D: header: seq: 0 stamp: secs: 0 nsecs: 0 frame_id: '' point: x: 0.0 y: 0.0 z: 0.0 pose: header: seq: 0 stamp: secs: 0 nsecs: 0 frame_id: '' pose: position: x: 0.0 y: 0.0 z: 0.0 orientation: x: 0.0 y: 0.0 z: 0.0 w: 0.0 scale: 0.0 width: 0.0 outline_color: r: 0.0 g: 0.0 b: 0.0 a: 0.0 filled: 0 fill_color: r: 0.0 g: 0.0 b: 0.0 a: 0.0 lifetime: secs: 0 nsecs: 0 arc: 0 angle: 0.0 points: [] points3D: header: seq: 0 stamp: secs: 0 nsecs: 0 frame_id: '' points: [] outline_colors: [] frames: ['/test_object'] text: '' left_up_origin: False

ratio_scale: False

となっており，座標がうまく渡されていません． しかしこの変換は，display-checkerboard.lの中の，

(send tfb :send-transform ret frame-id type) ;; ret: checkerboardのカメラ相対の座標 frame-id: /camera_rgb_frame type: /test_object (checkerboard)

で行われているのではないかと思います． 勉強不足ですので，分かったらissueを閉じます．

— You are receiving this because you are subscribed to this thread. Reply to this email directly or view it on GitHub https://github.com/jsk-enshu/robot-programming/issues/74

[kochigami]

ご教示ありがとうございました． タートルボットではなく，ペッパーで確認しました．

おそらく，issueを立てた時は，フレームの名前が適切ではなかったのかなと思います．（rosrun tf view_frames => frames.pdf の中にない名前を書いていた？） 今は，それは改善し，publishされる座標変換のトピックの周期が遅くてエラーが出るようになったと思います．

チェッカーボードとカメラフレームの座標変換が遅れる時に，どうすればエラーが出なくなるかもう少し考えます．

実行コマンド

roslaunch jsk_pepper_startup jsk_pepper_startup.launch 
roslaunch roseus_tutorials checkerboard-detector.launch rect0_size_x:=0.025 rect0_size_y:=0.025 grid0_size_x:=5 grid0_size_y:=4 translation0:="0 0 0" image:=image_raw group:=/pepper_robot/naoqi_driver_node/camera/front frame_id:=CameraTop_optical_frame
roseus display-checkerboard.l (pepper version)
rosrun image_view2 image_view2 image:=/pepper_robot/naoqi_driver_node/camera/front/image_raw camera_info:=/pepper_robot/naoqi_driver_node/camera/front/camera_info

image_view2実行画面で発生したエラー：画面が黒くなって動かなくなります．

rosrun image_view2 image_view2 image:=/pepper_robot/naoqi_driver_node/camera/front/image_raw camera_info:=/pepper_robot/naoqi_driver_node/camera/front/camera_info
[ERROR] [1460277536.649552728]: [image_view2] TF exception:
Lookup would require extrapolation into the future.  Requested time 1460277535.137155081 but the latest data is at time 1460277534.252620203, when looking up transform from frame [test_object] to frame [CameraTop_optical_frame]

その後 rosrun tf view_frames を実行して，frames.pdfで /test_object と/CameraTop_optical_frame　がつながっていることを確認しました．

pepper版のdisplay-checkerboard.l

#!/usr/bin/env roseus

;; robotの初期化                                                                
(load "package://peppereus/pepper-interface.l")
(pepper-init) 

;; ObjectDetection型トピックを使うため                                          
(ros::roseus-add-msgs "std_msgs")
(ros::roseus-add-msgs "roseus")
(ros::roseus-add-msgs "geometry_msgs")
(ros::roseus-add-msgs "image_view2")
(ros::roseus-add-msgs "posedetection_msgs")

;;; 表示モデルなど                                                              
(load "models/chessboard-30-7x5-object.l")
(if (not (boundp '*irtviewer*)) (make-irtviewer))
(setq *target-object* (chessboard-30-7x5 :name "/test_object"))
(objects (list *target-object* *pepper*))

(defvar *root-frame-id* "camera_rgb_frame")
(setq *tfb* (instance ros::transform-broadcaster :init))

(ros::roseus "objectdetection_client")

;; ObjectDetection用コールバック関数定義                                        
(defun objectdetection-cb (msg)
  (let ((mrk (instance image_view2::ImageMarker2 :init)) frame-id type ret)
    ;; 物体モデルを配置                                                         
    (setq frame-id (concatenate string "/" (send msg :header :frame_id)))
    (mapcar #'(lambda (obj-pose)
                (let* (;; (1) カメラ相対の座標系は、geometry_msgs/Poseという型で得られるので、Euslispのcoordsに変換する                                       
                       (cam->obj-coords (ros::tf-pose->coords (send obj-pose :pose)))
                        ;; (2) *pepper*モデルがカメラの座標系をもってるので、取得する                                                                          
                       (cam-coords (send (send *pepper* :cameraTop_optical_frame_lk) :copy-worldcoords)))
                  ;; (3) Euslisp内部でのworld座標系の値にして、そこにcheckerboardモデルを配置する                                                              
                  (send *target-object* :newcoords (send cam-coords :transform cam->obj-coords))
                  ))
            (send msg :objects))
    ;; image_markerを出力                                                       
    (dolist (obj-pose (send msg :objects))
      (setq type (send obj-pose :type))
      (unless (eq (char type 0) #\/) (setq type (concatenate string "/" type)))
      (setq ret (ros::tf-pose->coords (send obj-pose :pose)))
      (send mrk :type image_view2::ImageMarker2::*FRAMES*)
      (send mrk :frames (list type))
      (send *tfb* :send-transform ret frame-id type)
      (ros::ros-info "~A ~A ~A" ret frame-id type)
      (ros::publish "image_marker" mrk))
    ))

(ros::advertise "image_marker" image_view2::ImageMarker2 1)
(ros::subscribe "/pepper_robot/naoqi_driver_node/camera/front/ObjectDetection"  posedetection_msgs::ObjectDetection #'objectdetection-cb)

(ros::rate 10)
(do-until-key
  (ros::spin-once)
  (send *irtviewer* :draw-objects)
  (x::window-main-one)
  (ros::sleep)
  )

表示結果