diff --git a/pr2eus_moveit/euslisp/collision-object-publisher.l b/pr2eus_moveit/euslisp/collision-object-publisher.l index a9f9dba36..52342e82d 100644 --- a/pr2eus_moveit/euslisp/collision-object-publisher.l +++ b/pr2eus_moveit/euslisp/collision-object-publisher.l @@ -105,7 +105,8 @@ (let ((org-cds (send obj :copy-worldcoords))) (send obj :reset-coords) (send obj :worldcoords) - (mapcar #'(lambda (body) (send body :worldcoords)) (send obj :bodies)) + (when (find-method obj :bodies) + (mapcar #'(lambda (body) (send body :worldcoords)) (send obj :bodies))) (let ((fs (body-to-faces obj)) (geom (instance shape_msgs::mesh :init)) idx-lst vertices (cntr 0)) diff --git a/pr2eus_tutorials/README.md b/pr2eus_tutorials/README.md index 196c8c70d..002fd2388 100644 --- a/pr2eus_tutorials/README.md +++ b/pr2eus_tutorials/README.md @@ -127,6 +127,18 @@ You can click any object that you want the robot to pick up. ![pr2_tabletop_sim](https://gist.githubusercontent.com/furushchev/b3f3bb08953407966f80f4b0ac70c7dd/raw/pr2_tabletop_screen.png) + +##### Click object on image_view2 + +You can click any pixels that you want the robot to pick up. + +```bash +rosrun image_view2 image_view2 image:=/kinect_head/rgb/throttled/image_rect_color camera_info:=/head_mount_kinect/rgb/camera_info +``` + +![pr2_interactive](https://user-images.githubusercontent.com/19769486/81817136-3913f200-9567-11ea-800a-f95bd4057cb5.png) + + ### Step-by-step Description of Demo program In the bottom of the demo program `pr2-tabletop-object-grasp.l`, you can see a main function `demo`. diff --git a/pr2eus_tutorials/config/pr2_tabletop.rviz b/pr2eus_tutorials/config/pr2_tabletop.rviz index 61ea6acbf..63acb0bfe 100644 --- a/pr2eus_tutorials/config/pr2_tabletop.rviz +++ b/pr2eus_tutorials/config/pr2_tabletop.rviz @@ -8,7 +8,7 @@ Panels: - /Status1 - /TF1/Frames1 Splitter Ratio: 0.5 - Tree Height: 745 + Tree Height: 698 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -17,7 +17,7 @@ Panels: - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679016 + Splitter Ratio: 0.5886790156364441 - Class: rviz/Views Expanded: - /Current View1 @@ -28,6 +28,8 @@ Panels: Name: Time SyncMode: 0 SyncSource: Kinect PointCloud +Preferences: + PromptSaveOnExit: true Toolbars: toolButtonStyle: 2 Visualization Manager: @@ -39,7 +41,7 @@ Visualization Manager: Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 @@ -540,7 +542,7 @@ Visualization Manager: Queue Size: 10 Selectable: true Size (Pixels): 3 - Size (m): 0.00999999978 + Size (m): 0.009999999776482582 Style: Flat Squares Topic: /kinect_head/depth_registered/throttled/points Unreliable: false @@ -553,10 +555,10 @@ Visualization Manager: Topic: /segmentation_decomposer/boxes Unreliable: false Value: true - alpha: 0.800000012 + alpha: 0.800000011920929 color: 25; 255; 0 coloring: Auto - line width: 0.00499999989 + line width: 0.004999999888241291 only edge: false show coords: true - Alpha: 1 @@ -569,7 +571,7 @@ Visualization Manager: Value: true coloring: Auto enable lighting: true - normal length: 0.100000001 + normal length: 0.10000000149011612 only border: true show normal: true - Class: rviz/InteractiveMarkers @@ -582,11 +584,11 @@ Visualization Manager: Update Topic: /bounding_box_interactive_marker/update Value: true - Align Bottom: false - Background Alpha: 0.800000012 + Background Alpha: 0.800000011920929 Background Color: 0; 0; 0 Class: jsk_rviz_plugin/OverlayText Enabled: true - Foreground Alpha: 0.800000012 + Foreground Alpha: 0.800000011920929 Foreground Color: 25; 255; 240 Name: Grasp Status Overtake Color Properties: false @@ -600,6 +602,15 @@ Visualization Manager: text size: 12 top: 0 width: 128 + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: ray_marker_array + Name: RayMarkerArray + Namespaces: + test_line: true + test_sphere: true + Queue Size: 100 + Value: true Enabled: true Global Options: Background Color: 0; 0; 0 @@ -615,7 +626,10 @@ Visualization Manager: - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint @@ -625,33 +639,33 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 3.52001023 + Distance: 3.520010232925415 Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -0.0445861518 - Y: 0.339318067 - Z: 0.386238486 + X: -0.04458615183830261 + Y: 0.33931806683540344 + Z: 0.3862384855747223 Focal Shape Fixed Size: true - Focal Shape Size: 0.0500000007 + Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View - Near Clip Distance: 0.00999999978 - Pitch: 0.629800022 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.629800021648407 Target Frame: Value: Orbit (rviz) - Yaw: 5.77176809 + Yaw: 5.771768093109131 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1056 + Height: 1025 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001fb00000378fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000378000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000064afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000064a000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000005cfc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000053e0000037800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001fb00000345fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000345000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000064afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000064a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000005cfc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000053c0000034500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -660,6 +674,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1855 - X: 65 - Y: 24 + Width: 1853 + X: 67 + Y: 27 diff --git a/pr2eus_tutorials/euslisp/pr2-tabletop-object-grasp.l b/pr2eus_tutorials/euslisp/pr2-tabletop-object-grasp.l index 534e42442..1e58257a1 100755 --- a/pr2eus_tutorials/euslisp/pr2-tabletop-object-grasp.l +++ b/pr2eus_tutorials/euslisp/pr2-tabletop-object-grasp.l @@ -5,7 +5,14 @@ (defvar *grasp-target-bounding-box-topic* "/bounding_box_marker/selected_box") (defvar *grasp-status-topic* "/tabletop_object_grasp_status") (defvar *base-frame-id* "/base_footprint") - +(defvar *camera-frame-id* "/head_mount_kinect_rgb_optical_frame") +(defvar *click-point-topic* "/kinect_head/rgb/throttled/image_rect_color/screenpoint") +(defvar *ray-marker-topic* "/ray_marker_array") +(defvar *image-marker-topic* "/image_marker") +(defvar *screenpoint-output-topic* "/pointcloud_screenpoint_nodelet/output_point") + +(ros::load-ros-manifest "geometry_msgs") +(ros::load-ros-manifest "image_view2") (ros::load-ros-manifest "jsk_recognition_msgs") (ros::load-ros-manifest "jsk_rviz_plugins") @@ -74,92 +81,153 @@ (send (send base->parent :copy-worldcoords) :transform cds)))) +;; Mainly copied from https://github.com/jsk-ros-pkg/jsk_recognition/blob/master/jsk_pcl_ros/euslisp/pointcloud_screenpoint.l +(defun show-marker (frame p1) + "publish markers +@param frame frame id +@param p1 position of clicked point" + (let* ((header (instance std_msgs::header :init + :stamp (ros::time-now) :frame_id frame)) + (p0 (float-vector 0 0 0)) + (sp (make-sphere 30 :pos p1)) + ;; Marker::SPHERE + (sp-msg (sphere->marker-msg sp header + :color (float-vector 1 0 1) :alpha 1.0)) + ;; Marker::LINE + (li-msg (line->marker-msg (list p0 p1) header :scale 15 + :color (float-vector 0 1 1) :alpha 0.5)) + ;; MarkerArray msg + (msg (instance visualization_msgs::MarkerArray :init)) + ) + (send sp-msg :ns "test_sphere") + (send li-msg :ns "test_line") + (send sp-msg :lifetime (ros::time 5)) + (send li-msg :lifetime (ros::time 5)) + (send msg :markers (list sp-msg li-msg)) + (ros::publish *ray-marker-topic* msg) + )) + (defun grasp-target-callback (msg) - (let ((target-obj (bounding-box->cube msg))) - (if *grasping-object-p* - (progn - (publish-warn "cancel grasping...") - (send *ri* :stop-motion) - (send *ri* :stop-grasp *arm* :wait t) - (setq *grasping-object-p* nil)) - (setq *grasping-object-p* t)) - (unless + (let ((camera-to-base (send *tfl* :lookup-transform *camera-frame-id* *base-frame-id* (ros::time 0))) + (coords (make-coords :pos (send (bounding-box->cube msg) :worldpos)))) + (send coords :transform camera-to-base :world) + (show-marker *camera-frame-id* (send coords :pos)) + (grasp (send (bounding-box->cube msg) :worldpos)))) + +(defun grasp-target-2d-callback (msg) + (let ((base-to-camera (send *tfl* :lookup-transform *base-frame-id* *camera-frame-id* (ros::time 0))) + (coords (make-coords :pos (ros::tf-point->pos (send msg :point))))) + (show-marker *camera-frame-id* (ros::tf-point->pos (send msg :point))) + (send coords :transform base-to-camera :world) + (grasp (send coords :pos)))) + +(defun click-point-callback (msg) + (let ((mrk (instance image_view2::ImageMarker2 :init)) + (x (send (send msg :point) :x)) + (y (send (send msg :point) :y))) + (send mrk :type image_view2::ImageMarker2::*CIRCLE*) + (send mrk :id 0) + (send mrk :position (instance geometry_msgs::Point :init :x x :y y)) + (send mrk :outline_color (instance std_msgs::ColorRGBA :init :r 0.0 :g 1.0 :b 0.0 :a 1.0)) + (send mrk :lifetime (ros::Time 5)) + (send mrk :scale 20.0) + (ros::publish *image-marker-topic* mrk))) + +(defun grasp (pos) + (if *grasping-object-p* + (progn + (publish-warn "cancel grasping...") + (send *ri* :stop-motion) + (send *ri* :stop-grasp *arm* :wait t) + (setq *grasping-object-p* nil)) + (setq *grasping-object-p* t)) + (unless (send *pr2* *arm* :inverse-kinematics - (make-coords :pos (v+ (send target-obj :worldpos) + (make-coords :pos (v+ pos (float-vector -120 0 0)) :rpy #f(0 0 0)) :rotation-axis t) - (publish-error "out of reach...") - (return-from grasp-target-callback nil)) - (publish-info "openning gripper...") - - (setq *grasp-state* "reaching") - (publish-grasp-state) - (publish-info "pre grasp pose...") - (send *ri* :stop-grasp *arm* :wait t) - (send *pr2* :head :look-at-hand *arm*) - (send *ri* :angle-vector (send *pr2* :angle-vector) 6000) - (send *ri* :wait-interpolation) - - (setq *grasp-state* "grasp-approaching") - (publish-grasp-state) - (publish-info "reaching...") - (send *pr2* *arm* :move-end-pos #f(160 0 0) :world) - (send *pr2* :head :look-at-hand *arm*) - (send *ri* :angle-vector (send *pr2* :angle-vector) 3000) - (send *ri* :wait-interpolation) - - (setq *grasp-state* "grasp-closing") - (publish-grasp-state) - (publish-info "grasping...") - (let ((grasp-result (send *ri* :start-grasp *arm*))) - (send *ri* :wait-interpolation) - (unless (< 5.0 grasp-result) - (publish-error "failed to grasp ~A" grasp-result) - (send *ri* :stop-grasp *arm* :wait t) - (publish-info "back to pre grasp...") - (pr2-pregrasp-pose) - (return-from grasp-target-callback nil))) - - (setq *grasp-state* "picking") - (publish-grasp-state) - (publish-info "picking up...") - (send *pr2* *arm* :move-end-pos #f(0 0 150) :world) - (send *pr2* :head :look-at-hand *arm*) - (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) - (send *ri* :wait-interpolation) - (publish-info "grasp succeeded!") - - (setq *grasp-state* "placing") - (publish-grasp-state) - (publish-info "placing ...") - (send *pr2* *arm* :move-end-pos #f(0 0 -150) :world) - (send *ri* :angle-vector (send *pr2* :angle-vector) 3000) - (send *ri* :wait-interpolation) - - (setq *grasp-state* "grasp-opening") - (publish-grasp-state) - (send *ri* :stop-grasp *arm* :wait t) - (publish-info "return object...") - - (setq *grasp-state* "leaving") - (publish-grasp-state) - (send *pr2* *arm* :move-end-pos #f(-160 0 0) :world) - (send *ri* :angle-vector (send *pr2* :angle-vector) 3000) + (publish-error "out of reach...") + (return-from grasp nil)) + (publish-info "openning gripper...") + + (setq *grasp-state* "reaching") + (publish-grasp-state) + (publish-info "pre grasp pose...") + (send *ri* :stop-grasp *arm* :wait t) + (send *pr2* :head :look-at-hand *arm*) + (send *ri* :angle-vector (send *pr2* :angle-vector) 6000) + (send *ri* :wait-interpolation) + + (setq *grasp-state* "grasp-approaching") + (publish-grasp-state) + (publish-info "reaching...") + (send *pr2* *arm* :move-end-pos #f(160 0 0) :world) + (send *pr2* :head :look-at-hand *arm*) + (send *ri* :angle-vector (send *pr2* :angle-vector) 3000) + (send *ri* :wait-interpolation) + + (setq *grasp-state* "grasp-closing") + (publish-grasp-state) + (publish-info "grasping...") + (let ((grasp-result (send *ri* :start-grasp *arm*))) (send *ri* :wait-interpolation) - (publish-info "pre grasp pose") - - (setq *grasp-state* "returning") - (publish-grasp-state) - (pr2-pregrasp-pose) - (publish-info "click tabletop object!") - )) + (unless (< 5.0 grasp-result) + (publish-error "failed to grasp ~A" grasp-result) + (send *ri* :stop-grasp *arm* :wait t) + (publish-info "back to pre grasp...") + (pr2-pregrasp-pose) + (return-from grasp nil))) + + (setq *grasp-state* "picking") + (publish-grasp-state) + (publish-info "picking up...") + (send *pr2* *arm* :move-end-pos #f(0 0 150) :world) + (send *pr2* :head :look-at-hand *arm*) + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (publish-info "grasp succeeded!") + + (setq *grasp-state* "placing") + (publish-grasp-state) + (publish-info "placing ...") + (send *pr2* *arm* :move-end-pos #f(0 0 -150) :world) + (send *ri* :angle-vector (send *pr2* :angle-vector) 3000) + (send *ri* :wait-interpolation) + + (setq *grasp-state* "grasp-opening") + (publish-grasp-state) + (send *ri* :stop-grasp *arm* :wait t) + (publish-info "return object...") + + (setq *grasp-state* "leaving") + (publish-grasp-state) + (send *pr2* *arm* :move-end-pos #f(-160 0 0) :world) + (send *ri* :angle-vector (send *pr2* :angle-vector) 3000) + (send *ri* :wait-interpolation) + (publish-info "pre grasp pose") + + (setq *grasp-state* "returning") + (publish-grasp-state) + (pr2-pregrasp-pose) + (publish-info "click tabletop object in Rviz!") + (publish-info "click tabletop object in image_view2!") + ) (defun wait-for-grasp-target () (ros::subscribe *grasp-target-bounding-box-topic* jsk_recognition_msgs::BoundingBox #'grasp-target-callback) - (publish-info "click tabletop object!")) + (publish-info "click tabletop object in Rviz!")) + +(defun wait-for-grasp-target-2d () + (ros::advertise *image-marker-topic* image_view2::ImageMarker2 1) + (ros::advertise *ray-marker-topic* visualization_msgs::MarkerArray 10) + (ros::subscribe *screenpoint-output-topic* + geometry_msgs::PointStamped #'grasp-target-2d-callback) + (ros::subscribe *click-point-topic* + geometry_msgs::PointStamped #'click-point-callback) + (publish-info "click tabletop object in image_view2!")) (defun publish-grasp-state () (ros::publish "/grasp_state" (instance std_msgs::String :init :data *grasp-state*))) @@ -174,5 +242,6 @@ (pr2-init) (pr2-pregrasp-pose) (wait-for-grasp-target) + (wait-for-grasp-target-2d) (ros::spin)) (demo) diff --git a/pr2eus_tutorials/launch/gazebo/pr2_tabletop_scene.launch b/pr2eus_tutorials/launch/gazebo/pr2_tabletop_scene.launch index 8db0fd225..b7dbc9f86 100644 --- a/pr2eus_tutorials/launch/gazebo/pr2_tabletop_scene.launch +++ b/pr2eus_tutorials/launch/gazebo/pr2_tabletop_scene.launch @@ -19,5 +19,15 @@ lazy: true + + + + + + + lazy: true + + diff --git a/pr2eus_tutorials/launch/pr2_tabletop.launch b/pr2eus_tutorials/launch/pr2_tabletop.launch index 93fa6166c..7a0277782 100644 --- a/pr2eus_tutorials/launch/pr2_tabletop.launch +++ b/pr2eus_tutorials/launch/pr2_tabletop.launch @@ -19,7 +19,26 @@ - + + + + + queue_size: 10 + crop_size: 10 + search_size: 16 + + + + + + + + + + + diff --git a/pr2eus_tutorials/package.xml b/pr2eus_tutorials/package.xml index b4270c991..2cdd68868 100644 --- a/pr2eus_tutorials/package.xml +++ b/pr2eus_tutorials/package.xml @@ -24,6 +24,7 @@ pr2_gazebo pr2eus roseus_tutorials + image_view2 jsk_pcl_ros jsk_interactive_marker jsk_recognition_msgs