From 60929bc644d13b80ffdf72732574e5ae3ec44d60 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Mon, 16 Nov 2015 18:15:50 +0900 Subject: [PATCH 1/3] Add example to display checkerboard pose with turtlebot --- .../euslisp/display-checkerboard.l | 61 +++++++++++++++++++ 1 file changed, 61 insertions(+) create mode 100644 dxl_armed_turtlebot/euslisp/display-checkerboard.l diff --git a/dxl_armed_turtlebot/euslisp/display-checkerboard.l b/dxl_armed_turtlebot/euslisp/display-checkerboard.l new file mode 100644 index 00000000..1e057619 --- /dev/null +++ b/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) + ) + From 33ebb5cf9b16f311a3735547a150951a8caf4111 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Mon, 16 Nov 2015 18:16:28 +0900 Subject: [PATCH 2/3] Update arm base --- dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot.l | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot.l b/dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot.l index 5924e923..8fc85228 100644 --- a/dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot.l +++ b/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: From 1158cc24183780fd8e1887c3ac8d5d29ae9dfd16 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Mon, 16 Nov 2015 18:16:55 +0900 Subject: [PATCH 3/3] Add arm pose to get wide view field --- dynamixel_7dof_arm/euslisp/dxl-7dof-arm-robot.l | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/dynamixel_7dof_arm/euslisp/dxl-7dof-arm-robot.l b/dynamixel_7dof_arm/euslisp/dxl-7dof-arm-robot.l index 9646ccf9..37529684 100644 --- a/dynamixel_7dof_arm/euslisp/dxl-7dof-arm-robot.l +++ b/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 -|# \ No newline at end of file +|#