Merge pull request #66 from snozawa/add_display_checkerboard

Add display checkerboard

dxl_armed_turtlebot/euslisp/display-checkerboard.l

@@ -0,0 +1,61 @@
+#!/usr/bin/env roseus
+
+;; robotの初期化
+(load "package://dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot-interface.l")
+(dxl-armed-turtlebot-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* *dxl-armed-turtlebot*))
+
+(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* (;; カメラ相対の座標系は、geometry_msgs/Poseという型で得られるので、Euslispのcoordsに変換する
+                       (cam->obj-coords (ros::tf-pose->coords (send obj-pose :pose)))
+                       ;; *dxl-armed-turtlebot*モデルがカメラの座標系をもってるので、取得する
+                       (cam-coords (send (send *dxl-armed-turtlebot* :camera_rgb_optical_frame_lk) :copy-worldcoords)))
+                  ;; 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 "/camera/rgb/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)
+  )
+

dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot.l

@@ -26,7 +26,10 @@
           ;; (make-coords :pos (float-vector 85.725 9.525 410) ;; 9.525 = (/ 38.1 4.0), 38.1 = 2 pitch width, 85.725 = (+ (/ 152.4 2.0) (/ 38.1 4.0))
           ;;              :rpy (list 0 0 0))
           ;; lower arm version
-          (make-coords :pos (float-vector 85.725 9.525 402) ;; 9.525 = (/ 38.1 4.0), 38.1 = 2 pitch width, 85.725 = (+ (/ 152.4 2.0) (/ 38.1 4.0))
+          ;; (make-coords :pos (float-vector 85.725 9.525 402) ;; 9.525 = (/ 38.1 4.0), 38.1 = 2 pitch width, 85.725 = (+ (/ 152.4 2.0) (/ 38.1 4.0))
+          ;;              :rpy (list 0 0 pi))
+          ;; lower arm version + 長いアーム台座
+          (make-coords :pos (float-vector (+ 58.0 85.725) 9.525 402) ;; 9.525 = (/ 38.1 4.0), 38.1 = 2 pitch width, 85.725 = (+ (/ 152.4 2.0) (/ 38.1 4.0))
                        :rpy (list 0 0 pi))
           )
          ;; turtlebot CAD information is here:

dynamixel_7dof_arm/euslisp/dxl-7dof-arm-robot.l

@@ -371,6 +371,10 @@
    ()
    "Folding arm pose2."
    (send self :angle-vector (float-vector 90.0 90.0 5.0 0.0 -90.0 90.0 0.0)))
+  (:tuckarm-pose3
+   ()
+   "Folding arm pose3."
+   (send self :angle-vector (float-vector 0.0 90.0 0.0 -20.0 90.0 0.0 0.0)))
   ;; inverse-kinematics
   (:inverse-kinematics
    (target-coords &rest args &key (link-list)
@@ -522,4 +526,4 @@
 ;; old version functions
 
 ;; version 1
-|#
+|#