grasp_segment.py

Go to the documentation of this file.

#!/usr/bin/env python
#This node listens to pointclouds (from handle-extractor), requesting grasp plans for each
import roslib; #roslib.load_manifest('handle')
import sys
roslib.load_manifest('pr2_gripper_grasp_planner_cluster')
import rospy
from object_manipulation_msgs.srv import GraspPlanning, GraspPlanningResponse
from object_manipulation_msgs.msg import GraspableObject
from sensor_msgs.msg import PointCloud

a = 9

def service_caller(pc):
    rospy.wait_for_service('plan_point_cluster_grasp')
    try:
        myproxy = rospy.ServiceProxy('plan_point_cluster_grasp', GraspPlanning)
        go = GraspableObject()
        go.cluster = pc
        go.reference_frame_id = "/base_link"
        rospy.loginfo(rospy.get_name()+"I used a pointcloud with %d points as cluster",len(go.cluster.points))

        myresponse = myproxy(target=go)
        rospy.loginfo(rospy.get_name()+"I got a %d grasps",len(myresponse.grasps))
        print myresponse
        #for i, g in enumerate(myresponse.grasps):
        #    rospy.loginfo(rospy.get_name()+"grasp %d has success probability %f",i,g.success_probability)
        #    print

        return myresponse
    except rospy.ServiceException, e:
        print "Service call failed: %s"%e

def callback(pc):
    global a
    rospy.loginfo(rospy.get_name()+"Received a pointcloud with %d points",len(pc.points))
    if len(pc.points) > 10:
      a = service_caller(pc)
    rospy.signal_shutdown("done")


if __name__ == '__main__':
    rospy.init_node('grasper')
    rospy.loginfo(rospy.get_name()+" subscribing to pointclouds, requesting grasp plans for each one received")
    rospy.Subscriber("/handle_segment", PointCloud, callback)
    rospy.loginfo(rospy.get_name()+" ready")
    rospy.spin()