 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Using Euslisp(roseus) to control robot behavior through perception
Description: This tutorial shows how to write perception based behavir programTutorial Level: BEGINNER
Contents
Install related software
Euslisp Setup
Install from deb
sudo apt-get install ros-hydro-pr2eus
Gazebo Setup (if use gazebo simulator)
Install from deb
sudo apt-get install ros-hydro-pr2-gazebo
writing the robot node
source code
create the pr2main.l file in your folder https://github.com/jsk-ros-pkg/jsk_pr2eus/tree/master/pr2eus_tutorials
(load "package://pr2eus/pr2-interface.l") (ros::roseus "pr2_main") (defmethod pr2-robot (:initial-pose () (send self :angle-vector (float-vector 12.0 45.0 60.0 0.0 -120.0 0.0 -45.0 0.0 -45.0 60.0 0.0 -120.0 0.0 -45.0 0.0 0.0 35.0)) )) #| ;; make limb's index vector (map integer-vector #'(lambda (j) (position j (send *pr2* :joint-list))) (send *pr2* limb :joint-list)) |# (defmethod robot-interface (:error-coords (limb) (let ((pt (send self :state :potentio-vector)) (ds (send self :state-vector :desired)) (limb-pt (send robot limb :angle-vector)) (pt-cds (send robot limb :end-coords)) limb-ds ds-cds diff-cds) (send robot :angle-vector ds) (setq ds-cds (send robot limb :end-coords) limb-ds (send robot limb :angle-vector)) (setq diff-cds (send pt-cds :transformation ds-cds)) (setf (get diff-cds :diff-angle-vector) (v- limb-pt limb-ds)) (setf (get diff-cds :limb) limb) (send robot :angle-vector pt) diff-cds )) ) ;; initial pose (defun goto-init-pose (&key (wait t) (tm 4000)) (send *pr2* :initial-pose) (send *ri* :angle-vector (send *pr2* :angle-vector) tm) (if wait (send *ri* :wait-interpolation))) (warn " (pr2-init) (goto-init-pose) ")
then create the tabletop-sample.l in the same folder
(ros::roseus-add-msgs "posedetection_msgs") (load "pr2main.l") ;; (load "package://pr2eus_tutorials/arm-navigation-sample.l") (defun detect-callback ( msg ) (setq *msg* msg)) (setq *msg* nil) (ros::roseus "tabletop_object_sample") ;; /ObjectDetection is the topic that tells object_pose (ros::subscribe "/ObjectDetection" posedetection_msgs::ObjectDetection #'detect-callback) ;; initialize PR2 ROS-euslisp interface (pr2-init t) ;; goto initial pose (goto-init-pose) (send *irtviewer* :draw-objects) ;; (unless (boundp '*tfl*) (setq *tfl* (instance ros::transform-listener :init))) ;; get pose in /ObjectDetection (defun detect-one (&optional (timeout 500)) "subscribe just one detection result and return list of worldcoords of object's centroid" (let (lst (cntr 0) trans-base->table) (setq *msg* nil) (ros::rate 10) (while (null *msg*) (ros::spin-once) (ros::sleep) (if (> (incf cntr) timeout) (return-from detect-one nil))) (setq trans-base->table (send *tfl* :lookup-transform "/base_footprint" (send *msg* :header :frame_id) (send *msg* :header :stamp))) (dolist (obj (send *msg* :objects)) (let ((trs (send (send trans-base->table :copy-worldcoords) :transform (ros::tf-pose->coords (send obj :pose))))) (setf (get trs :type) (send obj :type)) (push trs lst) )) lst)) (unix::sleep 5) ;; waiting detection (defun pickup-pick (&key (arm :rarm)) (let ((av-st (send *pr2* :angle-vector)) av-ed av-mid) (setq *res* (detect-one)) ;; sort nearest first// (sort *res* #'(lambda (x y) (<= (norm (send (send (send *pr2* arm :end-coords :worldcoords) :transformation x) :pos)) (norm (send (send (send *pr2* arm :end-coords :worldcoords) :transformation y) :pos))))) (cond ((send *pr2* arm :inverse-kinematics (car *res*) :rotation-axis :z) ;;(setq av-ed (send *pr2* :angle-vector)) ;; move hands to object IK(inverse kinematics) (let ((cds (send *pr2* arm :end-coords :copy-worldcoords))) (send cds :translate (float-vector 25 0 10 )) ;; 25mm offset of centroid (if (send *pr2* arm :inverse-kinematics cds :rotation-axis :z) (setq av-ed (send *pr2* :angle-vector)))) (let ((cds (send *pr2* arm :end-coords :copy-worldcoords))) (send cds :translate (float-vector -75 0 0)) ;; linear reach to object (if (send *pr2* arm :inverse-kinematics cds) (setq av-mid (send *pr2* :angle-vector)))) (send *irtviewer* :draw-objects) (when av-mid ;; open hand (send *ri* :stop-grasp arm) (send *ri* :wait-interpolation) ;; wait for finising action ;; move arm (send *ri* :angle-vector-sequence (list av-mid av-ed) (list 3800 1200)) (send *ri* :wait-interpolation) ;; wait for finising action ;; close hand ;;(> (send *ri* :start-grasp arm) 8) ;; hand width is more than 8mm ;; gripper action not returned while have not reached goal position ??? (send *ri* :move-gripper arm 0.03 :wait nil) (unix::usleep (* 2 1000 1000)) ) ) (t nil)) (send *irtviewer* :draw-objects) )) (defun pickup-up (&key (arm :rarm)) ;; move hand up a little (send *pr2* arm :move-end-pos (float-vector 0 0 55) :world) (send *ri* :angle-vector (send *pr2* :angle-vector) 2500) (send *ri* :wait-interpolation) (send *irtviewer* :draw-objects) (goto-init-pose) (send *irtviewer* :draw-objects) ) (warn ";; ;; (pickup-pick :arm :rarm) ;; (pickup-up :arm :rarm) ;; (pickup-pick :arm :larm) ;; (pickup-up :arm :larm) ;; ")
Examining the codes
launch gazebo world (if use gazebo)
roslaunch pr2_gazebo pr2_empty_world.launch
run your program
roseus tabletop-sample.l
run detection node
you can use any programs that publish /ObjectDetection topic now, we test programs by using 'rostopic pub'
rostopic pub /ObjectDetection posedetection_msgs/ObjectDetection "header: seq: 0 stamp: secs: 0 nsecs: 0 frame_id: '/base_link' objects: - pose: position: x: 0.4 y: 0.1 z: 0.6 orientation: x: 0.0 y: 0.0 z: 0.0 w: 0.0 type: ''" -1
move command
then you can move robot by this command
;; in the terminal you run tabletop-simple node (pickup-pick :arm :rarm)
if the robot doesn't move, you may have to command
(ros::spin-once)
before 'pickup-pick'
