コード例 #1
0
def detect_objects():
    '''
    TODO: Need to figure out the target frame in which the cluster point co-ordinates are returned.
    '''
    service_name = '/object_detection'
    rospy.wait_for_service(service_name)
    try:
        table_object_detector = rospy.ServiceProxy(service_name, TabletopDetection)
        
        req = TabletopDetectionRequest()
        req.return_clusters = False
        req.return_models = True
        req.num_models = 1
        
        resp = TabletopDetectionResponse()
        resp = table_object_detector(req)
        if resp.detection.result == resp.detection.SUCCESS:
            points = resp.detection.clusters[0].points
            x_points = [point.x for point in points]
            y_points = [point.y for point in points]
            z_points = [point.z for point in points]
            x = (min(x_points) + max(x_points)) / 2.0
            y = (min(y_points) + max(y_points)) / 2.0
            z = (min(z_points) + max(z_points)) / 2.0
            return x, y, z
        else:
            print resp.detection.result
    except rospy.ServiceException, e:
        print "Service call failed: %s" % e
コード例 #2
0
def detect_objects():
    '''
    TODO: Need to figure out the target frame in which the cluster point co-ordinates are returned.
    '''
    service_name = '/object_detection'
    rospy.wait_for_service(service_name)
    try:
        table_object_detector = rospy.ServiceProxy(service_name,
                                                   TabletopDetection)

        req = TabletopDetectionRequest()
        req.return_clusters = False
        req.return_models = True
        req.num_models = 1

        resp = TabletopDetectionResponse()
        resp = table_object_detector(req)
        if resp.detection.result == resp.detection.SUCCESS:
            points = resp.detection.clusters[0].points
            x_points = [point.x for point in points]
            y_points = [point.y for point in points]
            z_points = [point.z for point in points]
            x = (min(x_points) + max(x_points)) / 2.0
            y = (min(y_points) + max(y_points)) / 2.0
            z = (min(z_points) + max(z_points)) / 2.0
            return x, y, z
        else:
            print resp.detection.result
    except rospy.ServiceException, e:
        print "Service call failed: %s" % e
コード例 #3
0
    def detect_objects(self):

        rospy.loginfo("calling tabletop detection")

        det_req = TabletopDetectionRequest()
        det_req.return_clusters = 1
        det_req.return_models = 1
        #det_req.num_models = num_models

        #call tabletop detection, get a detection result (attempt 3 times)
        for try_num in range(3):
            try:
                det_res = self.grasper_detect_srv(det_req)
            except rospy.ServiceException, e:
                rospy.logerr("error when calling %s: %s" %
                             (self.grasper_detect_name, e))
                self.throw_exception()
                return ([], None)
            if det_res.detection.result == det_res.detection.SUCCESS:
                rospy.loginfo("tabletop detection reports success")
                break
            else:
                rospy.logerr(
                    "tabletop detection failed with error %s, trying again" %
                    det_res.detection.result)
コード例 #4
0
    def __init__(self, recognize_objects=False):
        smach.StateMachine.__init__(self, [succeeded, preempted, aborted, 'no_table'],
                                    input_keys=[],
                                    output_keys=['tabletop_info'])

        with self:

            # HeadScan and Snapshot action call
            scan_table = PointHeadGoal()
            StateMachine.add('SCAN_TABLE',
                             SimpleActionState('/head_traj_controller/head_scan_snapshot_action',
                                               PointHeadAction,
                                               goal=scan_table),
                             transitions={succeeded: 'TABLE_DETECTION'})

            # TabletopDetection call
            detect_table = TabletopDetectionRequest()
            # Return the pointcloud (cluster) for each detected object
            # if this is false, you have no data to pass to tabletop_collision_map_processing node
            detect_table.return_clusters = recognize_objects
            # Return matching object models
            # Each cluster will have 1 or more models, even if the confidence rating is too low to display a marker in Rviz
            # If the household objects database is not connected, the model list will be empty
            detect_table.return_models = recognize_objects
            # Number of models to return for each object cluster
            # If there is more than 1 model with high confidence, they will all be displayed in Rviz
            detect_table.num_models = 1

            @smach.cb_interface(input_keys=[], output_keys=['tabletop_info'])
            def table_response_cb(userdata, response):
                if response.detection.result == response.detection.SUCCESS:
                    if recognize_objects:
                        userdata.tabletop_info = [response.detection]  # Return table and detected objects
                    else:
                        userdata.tabletop_info = [response.detection.table]  # Return only the table
                    return succeeded
                userdata.tabletop_info = [response.detection]
                return aborted

            StateMachine.add('TABLE_DETECTION', ServiceState('/object_detection', TabletopDetection,
                                                             request=detect_table, response_cb=table_response_cb,
                                                             output_keys=['tabletop_info']),
                             remapping={'tabletop_info': 'tabletop_info'},
                             transitions={succeeded: succeeded, aborted: 'CHECK_RESPONSE'})

            # Check why it failed
            @smach.cb_interface(input_keys=['tabletop_info'], output_keys=[], outcomes=[succeeded, 'no_table'])
            def check_tabletop_data(userdata):
                if userdata.tabletop_info[0].result == userdata.tabletop_info.NO_TABLE:
                   #or userdata.tabletop_info.result == userdata.tabletop_info.NO_CLOUD_RECEIVED:
                    return 'no_table'
                return aborted

            StateMachine.add('CHECK_RESPONSE', CBState(check_tabletop_data,
                                                       input_keys=['in_map', 'in_room_name'],
                                                       output_keys=['out_route'], outcomes=[aborted, 'no_table']),
                             remapping={'tabletop_info': 'tabletop_info'},
                             transitions={'no_table': 'no_table', aborted: aborted})
コード例 #5
0
 def __detect(self, detector):
     req = TabletopDetectionRequest()
     req.return_clusters = True
     req.return_models = True   
     try:
         reply = detector(req)
     except rospy.ServiceException, e:
         rospy.logerr("error when calling object_detection: %s"%e)
         return None
コード例 #6
0
 def __detect(self, detector):
     req = TabletopDetectionRequest()
     req.return_clusters = True
     req.return_models = True
     try:
         reply = detector(req)
     except rospy.ServiceException, e:
         rospy.logerr("error when calling object_detection: %s" % e)
         return None
コード例 #7
0
    def _do_detection(self, point_head_at):
        req = TabletopDetectionRequest()
        req.return_clusters = True
        req.return_models = True
        req.num_models = 1

        if point_head_at:
            point_head_at = tl.transform_point_stamped(self._wi.world_frame, point_head_at)
            zrng = range(0, int(100*self.table_search_max), int(100*self.table_search_resolution))
            sgnrng = [-1.0, 1.0]
            rospy.logdebug('For table finding, trying z offsets for head of:' +str(zrng))
        if not point_head_at or not zrng:
            zrng = [0]
            sgnrng = [1.0]
        first_pass=True

        for z in zrng:
            for sgn in sgnrng:
                if point_head_at:
                    new_point = copy.copy(point_head_at.point)
                    new_point.z += sgn*z/100.0
                    self._head._point_head([new_point.x, new_point.y, new_point.z], point_head_at.header.frame_id)
                    rospy.sleep(0.4) #let the head bobbing settle out
                res = self._detect_srv(req)
                if res.detection.result != res.detection.SUCCESS:
                    rospy.logerr('Unable to find table.  Error was: '+ str(res.detection.result))
                    if first_pass: 
                        exc = DetectionError('Unable to find table', error_code=res.detection.result)
                        first_pass = False
                    continue
                if self.allow_vertical_tables:
                    return res
                table = res.detection.table
                #is the table somewhat horizontal?
                table_pose_bl = tl.transform_pose_stamped(self._wi.robot_frame, table.pose)
                vert = Point()
                vert.z = 1.0
                oripose = Pose()
                oripose.orientation = table_pose_bl.pose.orientation
                table_vertical = gt.transform_point(vert, oripose)
                if table_vertical.z < self.vertical_threshold:
                    rospy.logerr('Table vertical is [%f, %f, %f].  This is not a horizontal table',
                                 table_vertical.x, table_vertical.y, table_vertical.z)
                    res.detection.result = res.detection.OTHER_ERROR
                    if first_pass:
                        exc = DetectionError('Unable to find horizontal table', error_code=res.detection.result)
                        first_pass = False
                    continue
                rospy.loginfo('Found horizontal table. Vertical is [%f, %f, %f]', 
                              table_vertical.x, table_vertical.y, table_vertical.z)
                return res
        raise exc
def call_object_detector():

    req = TabletopDetectionRequest()
    req.return_clusters = 1
    req.return_models = 0
    service_name = "/object_detection"
    rospy.loginfo("waiting for object_detection service")
    rospy.wait_for_service(service_name)
    serv = rospy.ServiceProxy(service_name, TabletopDetection)
    try:
        res = serv(req)
    except rospy.ServiceException, e:
        rospy.logerr("error when calling object_detection: %s"%e)  
        return 0
def call_object_detector():

    req = TabletopDetectionRequest()
    req.return_clusters = 1
    req.return_models = 0
    service_name = "/object_detection"
    rospy.loginfo("waiting for object_detection service")
    rospy.wait_for_service(service_name)
    serv = rospy.ServiceProxy(service_name, TabletopDetection)
    try:
        res = serv(req)
    except rospy.ServiceException, e:
        rospy.logerr("error when calling object_detection: %s" % e)
        return 0
コード例 #10
0
    def find_table(self): 
        rospy.loginfo("calling tabletop detection")

        det_req = TabletopDetectionRequest()
        det_req.return_clusters = 0
        det_req.return_models = 0
        det_req.num_models = 0
        self.detected_table = None
        #call tabletop detection, get a detection result
        try:
            det_res = self.grasper_detect_srv(det_req)
        except rospy.ServiceException, e:
            rospy.logerr("error when calling %s: %s"%(self.grasper_detect_name, e))
            rospy.signal_shutdown("no table top detection service")
            return None
コード例 #11
0
    def call_tabletop_object_detection(self):
        rospy.loginfo("object detection service call.")

        det_req = TabletopDetectionRequest()
        det_req.return_clusters = 1
        det_req.return_models = 1
        det_req.num_models = 0

        # call tabletop detection, get a detection result
        for try_num in range(3):
            try:
                det_res = self.detect_srv(det_req)
            except rospy.ServiceException, e:
                rospy.logerr("error when calling %s: %s" % ("/object_detection", e))
                self.throw_exception()
                return ([], None)
            if det_res.detection.result == det_res.detection.SUCCESS:
                rospy.loginfo("tabletop detection reports success")
                break
            else:
                rospy.logerr("tabletop detection failed with error. Trying again")
コード例 #12
0
    def detect_and_pickup(self):
        rospy.loginfo("calling tabletop detection")

        det_req = TabletopDetectionRequest()
        det_req.return_clusters = 1
        det_req.return_models = 1 
        #det_req.num_models = num_models

        #call tabletop detection, get a detection result (attempt 3 times)
        for try_num in range(3):
            try:
                det_res = self.grasper_detect_srv(det_req)
            except rospy.ServiceException, e:
                rospy.logerr("error when calling %s: %s"%(self.grasper_detect_name, e))
                self.throw_exception()
                return ([], None)
            if det_res.detection.result == det_res.detection.SUCCESS:
                rospy.loginfo("tabletop detection reports success")
                break
            else:
                rospy.logerr("tabletop detection failed with error %s, trying again"%det_res.detection.result)
コード例 #13
0
ファイル: youbot_routines4.py プロジェクト: rmeertens/ROSMAV
    def call_tabletop_object_detection(self):
        rospy.loginfo("object detection service call.")

        det_req = TabletopDetectionRequest()
        det_req.return_clusters = 1
        det_req.return_models = 1
        det_req.num_models = 0

        #call tabletop detection, get a detection result
        for try_num in range(3):
            try:
                det_res = self.detect_srv(det_req)
            except rospy.ServiceException, e:
                rospy.logerr("error when calling %s: %s" %
                             ("/object_detection", e))
                self.throw_exception()
                return ([], None)
            if det_res.detection.result == det_res.detection.SUCCESS:
                rospy.loginfo("tabletop detection reports success")
                break
            else:
                rospy.logerr(
                    "tabletop detection failed with error. Trying again")
コード例 #14
0
ファイル: test_grasping.py プロジェクト: ktiwari9/PR2-Feeder
        rospy.signal_shutdown("")
        sys.exit()   
    collision_processing_srv = rospy.ServiceProxy(COLLISION_PROCESSING_SERVICE_NAME, TabletopCollisionMapProcessing)
    
    #pickup client
    pickup_client = actionlib.SimpleActionClient(PICKUP_ACTION_NAME, PickupAction)
    while not pickup_client.wait_for_server(rospy.Duration(2.0)):
        rospy.loginfo("Witing for action client %s", PICKUP_ACTION_NAME)
    
    #place client
    place_client = actionlib.SimpleActionClient(PLACE_ACTION_NAME, PlaceAction)
    while not place_client.wait_for_server(rospy.Duration(2.0)):
        rospy.loginfo("Witing for action client %s", PICKUP_ACTION_NAME)
        
    rospy.loginfo("Calling tabletop detector")
    detection_call = TabletopDetectionRequest()
    detection_call.return_clusters = True
    detection_call.return_models = True
    
    detection_reply = object_detection_srv.call(detection_call)    
    if len(detection_reply.detection.clusters) == 0:
        rospy.logerr("No objects found!")
        rospy.signal_shutdown("")
        sys.exit()   
    
        rospy.loginfo("%d objects detected" % len(detection_reply.detection.clusters))
#    detection_reply.detection.cluster_model_indices = tuple(xrange(len(detection_reply.detection.clusters)))

    rospy.loginfo("Calling collision map processing")
    processing_call = TabletopCollisionMapProcessingRequest()    
    processing_call.detection_result = detection_reply.detection
コード例 #15
0
ファイル: view_objects.py プロジェクト: arii/pr2_awesomeness
    def pick_up(self, whicharm="l", NUM_TRIES=5, near_pose=None):
        self.pickup_client.cancel_goal()

        for i in range(NUM_TRIES):
          rospy.loginfo("Attempt %d  to grasp " % i )
          # call the tabletop detection
          rospy.loginfo("Calling tabletop detector")

          detection_call = TabletopDetectionRequest()

          # we want recognized database objects returned
          # set this to false if you are using the pipeline without the database
          detection_call.return_clusters = True

          #we want the individual object point clouds returned as well
          detection_call.return_models = True
          detection_call.num_models = 1


          detection_call_response = self.object_detection_srv(detection_call).detection
          if not detection_call_response:
              rospy.loginfo("Tabletop detection service failed")
              success=False; break

          if detection_call_response.result != detection_call_response.SUCCESS:
              rospy.loginfo("Tabletop detection returned error code %d", 
                        detection_call_response.result)
              success=False; break
            
          if len(detection_call_response.clusters)==0\
            and len(detection_call_response.models)==0 :
              rospy.loginfo("The tabletop detector detected the table, "
                        "but found no objects")
              success=False; break



          # call collision map processing
          rospy.loginfo("Calling collision map processing")

          processing_call = TabletopCollisionMapProcessingRequest()

          # pass the result of the tabletop detection 
          processing_call.detection_result = detection_call_response
          print detection_call_response.models
          import pdb;pdb.set_trace()

          # ask for the existing map and collision models to be reset
          processing_call.reset_collision_models = True
          processing_call.reset_attached_models = True

          #ask for the results to be returned in base link frame
          processing_call.desired_frame = "base_link"

          processing_call_response = self.collision_processing_srv(processing_call)
          if not processing_call_response:
              rospy.loginfo("Collision map processing service failed")
              success=False; break

          #the collision map processor returns instances of graspable objects
          if len(processing_call_response.graspable_objects) == 0:
              rospy.loginfo("Collision map processing returned no graspable objects")
              success=False; break
          graspable_objects = processing_call_response.graspable_objects

          # sort graspable objects
          import pdb;
          # sort graspable objects
          objects_with_offsets = [None]*len(graspable_objects)
          #near_point = np.array(list(near_pose[:3]) + [1.0])
          for i, obj in enumerate(graspable_objects):
            pdb.set_trace()
            points  = point_cloud_to_mat(obj.cluster)
            #centroid =np.reshape(np.mean(points, axis=1), 4) - near_point
            #offset = np.linalg.norm(centroid - near_point)
            #objects_with_offsets[i] = (offset,obj)
          #ordering = sorted(objects_with_offsets, key=lambda item:float((item[0])))
          #graspable_objects = [items[1] for items in ordering]

          #print [items[0] for items in ordering]
          for i, grasp_object in enumerate(graspable_objects):
              
              # call object pickup
              rospy.loginfo("Calling the pickup action");
              pickup_goal = PickupGoal()

              # pass one of the graspable objects returned 
              # by the collision map processor
              #pickup_goal.target = processing_call_response.graspable_objects[0]
              pickup_goal.target = grasp_object

              # pass the name that the object has in the collision environment
              # this name was also returned by the collision map processor
              pickup_goal.collision_object_name = \
                  processing_call_response.collision_object_names[i]

              # pass the collision name of the table, also returned by the collision 
              # map processor
              pickup_goal.collision_support_surface_name = \
                  processing_call_response.collision_support_surface_name

              # pick up the object with the left arm
              if whicharm=="l":
                  pickup_goal.arm_name = "left_arm"
              else:
                  pickup_goal.arm_name = "right_arm"

              # we will be lifting the object along the "vertical" direction
              # which is along the z axis in the base_link frame

              direction = Vector3Stamped()
              direction.header.stamp = rospy.Time.now()
              direction.header.frame_id = "base_link"
              direction.vector.x = 0
              direction.vector.y = 0
              direction.vector.z = 1
              pickup_goal.lift.direction = direction
              #request a vertical lift of 10cm after grasping the object

              pickup_goal.lift.desired_distance = 0.1;
              pickup_goal.lift.min_distance = 0.05;
              #do not use tactile-based grasping or tactile-based lift

              pickup_goal.use_reactive_lift = False;
              pickup_goal.use_reactive_execution = False;
              
              #pickup_goal.allow_gripper_support_collision = True
              #pickup_goal.ignore_collisions = True
              #send the goal
              rospy.loginfo("Waiting for the pickup action...")
              self.pickup_client.send_goal_and_wait(pickup_goal,rospy.Duration(15))
              
              pickup_result =self.pickup_client.get_result()
              assert isinstance(pickup_result, PickupResult)
              success = pickup_result.manipulation_result.value ==\
                pickup_result.manipulation_result.SUCCESS
               
              if success:
                  rospy.loginfo("Success! Grasped Object.")
                  return success
        if not success:
            rospy.loginfo("failed to grasp object")
        return success
コード例 #16
0
    def __init__(self, distance_treshold=0.5, recognize_objects=False):
        smach.StateMachine.__init__(self, [succeeded, preempted, aborted, 'no_table'],
                                    input_keys=[],
                                    output_keys=['tabletop_info'])

        with self:
            @smach.cb_interface(input_keys=[], output_keys=['out_tabletop_info', 'out_n_looks'],
                                outcomes=[succeeded])
            def init_udata(userdata):
                userdata.out_n_looks = 0  # Number of looks done
                userdata.out_tabletop_info = []  # To be filled in the GATHER_TTOP_DATA state
                return succeeded

            StateMachine.add('INIT_USERDATA', CBState(init_udata, output_keys=['out_tabletop_info', 'out_n_looks'],
                                                      outcomes=[succeeded]),
                             remapping={'out_tabletop_info': 'tabletop_info',
                                        'out_n_looks': 'n_looks'},
                             transitions={succeeded: 'CONTROL_STATE'})

            @smach.cb_interface(input_keys=['in_n_looks', 'in_tabletop_info'],
                                output_keys=['look_goal', 'out_n_looks'], outcomes=[succeeded, 'next', 'no_table'])
            def control_cb(userdata):
                if userdata.in_n_looks == 0:  # first look
                    userdata.look_goal = look_right().trajectory
                elif userdata.in_n_looks == 1:  # Second look
                    userdata.look_goal = look_straight().trajectory
                elif userdata.in_n_looks == 2:  # Third look
                    userdata.look_goal = look_left().trajectory
                else:  # Finished all the looks
                    if not userdata.in_tabletop_info:
                        return 'no_table'
                    return succeeded
                userdata.out_n_looks = userdata.in_n_looks + 1
                return 'next'

            StateMachine.add('CONTROL_STATE', CBState(control_cb,
                                                      input_keys=['n_looks'],
                                                      output_keys=['look_goal'],
                                                      outcomes=[succeeded, 'next', 'no_table']),
                             transitions={'next': 'LOOK_STATE',
                                          succeeded: succeeded,
                                          'no_table': 'no_table'},
                             remapping={'in_n_looks': 'n_looks', 'out_n_looks': 'n_looks',
                                        'look_goal': 'look_goal',
                                        'in_tabletop_info': 'tabletop_info'})

            # Move the head before detecting table
            StateMachine.add('LOOK_STATE',
                             SimpleActionState(HEAD_ACTION_NAME,
                                               HEAD_ACTION_TYPE,
                                               goal_slots=['trajectory']),
                             transitions={succeeded: 'TAKE_SNAPSHOT'},
                             remapping={'trajectory': 'look_goal'})

            StateMachine.add('TAKE_SNAPSHOT', TakeXtionSnapshot(WAIT_BEFORE_SNAPSHOT),
                             transitions={succeeded: 'TABLE_DETECTION'})

            # TabletopDetection call
            detect_table = TabletopDetectionRequest()
            # Return the pointcloud (cluster) for each detected object
            # if this is false, you have no data to pass to tabletop_collision_map_processing node
            detect_table.return_clusters = recognize_objects
            # Return matching object models
            # Each cluster will have 1 or more models, even if the confidence rating is too low to display a marker in Rviz
            # If the household objects database is not connected, the model list will be empty
            detect_table.return_models = recognize_objects
            # Number of models to return for each object cluster
            # If there is more than 1 model with high confidence, they will all be displayed in Rviz
            detect_table.num_models = 1

            @smach.cb_interface(input_keys=[], output_keys=['tabletop_response'])
            def table_response_cb(userdata, response):
                if response.detection.result == response.detection.SUCCESS:
                    if recognize_objects:
                        userdata.tabletop_response = response.detection  # Return table and detected objects
                    else:
                        userdata.tabletop_response = response.detection.table  # Return only the table
                    return succeeded
                userdata.tabletop_response = response.detection
                return aborted

            StateMachine.add('TABLE_DETECTION', ServiceState('/object_detection', TabletopDetection,
                                                             request=detect_table, response_cb=table_response_cb,
                                                             output_keys=['tabletop_response']),
                             remapping={'tabletop_response': 'tabletop_response'},
                             transitions={succeeded: 'GATHER_TTOP_DATA', aborted: 'CHECK_FAILURE'})

            # Check why it failed
            @smach.cb_interface(input_keys=['tabletop_response'], output_keys=[], outcomes=[succeeded, 'no_table'])
            def check_if_no_table(userdata):
                if userdata.tabletop_response.result == userdata.tabletop_response.NO_TABLE:
                   #or userdata.tabletop_response.result == userdata.tabletop_response.NO_CLOUD_RECEIVED:
                    return 'no_table'
                print 'Tabletop detection aborted with status: %s' % userdata.tabletop_response.result
                return aborted

            StateMachine.add('CHECK_FAILURE', CBState(check_if_no_table,
                                                      input_keys=[],
                                                      output_keys=['out_route'], outcomes=[aborted, 'no_table']),
                             remapping={'tabletop_response': 'tabletop_response'},
                             transitions={'no_table': 'CONTROL_STATE', aborted: aborted})

            @smach.cb_interface(input_keys=['in_tabletop_info', 'in_tabletop_response'], output_keys=['out_tabletop_info'],
                                outcomes=[succeeded])
            def check_gather_data(userdata):
                table_data = userdata.in_tabletop_response

                table_orient = table_data.pose.pose.orientation
                if table_orient.w < 0.95 or table_orient.x > 0.05 or table_orient.y > 0.05 or table_orient.z > 0.05:
                    # The surface is not horizontal, so we don't care about it.
                    print 'It was not a table...'
                    return succeeded

                table_pose = ((table_data.x_min+table_data.x_max)/2, (table_data.y_min+table_data.y_max)/2)

                for ti in userdata.in_tabletop_info:
                    tp = ((ti.x_min+ti.x_max)/2, (ti.y_min+ti.y_max)/2)
                    if ofb_utils.euclidean_distance(tp, table_pose) <= distance_treshold:
                        break
                else:  # The loop ended without breaking
                    userdata.in_tabletop_info.append(userdata.in_tabletop_response)
                    userdata.out_tabletop_info = userdata.in_tabletop_info
                return succeeded
            StateMachine.add('GATHER_TTOP_DATA', CBState(check_gather_data,
                                                         input_keys=['in_tabletop_info', 'tabletop_response'],
                                                         output_keys=['out_tabletop_info'],
                                                         outcomes=[succeeded]),
                             remapping={'in_tabletop_response': 'tabletop_response',
                                        'in_tabletop_info': 'tabletop_info',
                                        'out_tabletop_info': 'tabletop_info'},
                             transitions={succeeded: 'CONTROL_STATE'})
コード例 #17
0
    rospy.loginfo("waiting for object_detection service")
    rospy.wait_for_service(grasper_detect_name)
    rospy.loginfo("waiting for collision_map_processing service")
    rospy.wait_for_service(collision_map_processing_name)

    # create service proxies
    grasper_detect_srv = rospy.ServiceProxy(grasper_detect_name,
                                            TabletopDetection)
    collision_map_processing_srv = rospy.ServiceProxy(
        collision_map_processing_name, TabletopCollisionMapProcessing)
    '''
    object detection
    '''
    rospy.loginfo("detecting objects...")
    # set up detection request
    det_req = TabletopDetectionRequest()
    # we want recognized database objects returned, set this to 0 if using the pipeline without the database
    det_req.return_models = 1
    # we want the individual object point clouds returned as well
    det_req.return_clusters = 1

    # call tabletop detection, get a detection result
    try:
        det_res = grasper_detect_srv(det_req)
    except rospy.ServiceException, e:
        rospy.logerr("error when calling %s: %s" % (grasper_detect_name, e))
        sys.exit()
    if det_res.detection.result == det_res.detection.SUCCESS:
        rospy.loginfo("tabletop detection reports success")
    else:
        rospy.logerr("tabletop detection failed with error code %d" %
コード例 #18
0
ファイル: test_grasping.py プロジェクト: ktiwari9/PR2-Feeder
    collision_processing_srv = rospy.ServiceProxy(
        COLLISION_PROCESSING_SERVICE_NAME, TabletopCollisionMapProcessing)

    #pickup client
    pickup_client = actionlib.SimpleActionClient(PICKUP_ACTION_NAME,
                                                 PickupAction)
    while not pickup_client.wait_for_server(rospy.Duration(2.0)):
        rospy.loginfo("Witing for action client %s", PICKUP_ACTION_NAME)

    #place client
    place_client = actionlib.SimpleActionClient(PLACE_ACTION_NAME, PlaceAction)
    while not place_client.wait_for_server(rospy.Duration(2.0)):
        rospy.loginfo("Witing for action client %s", PICKUP_ACTION_NAME)

    rospy.loginfo("Calling tabletop detector")
    detection_call = TabletopDetectionRequest()
    detection_call.return_clusters = True
    detection_call.return_models = True

    detection_reply = object_detection_srv.call(detection_call)
    if len(detection_reply.detection.clusters) == 0:
        rospy.logerr("No objects found!")
        rospy.signal_shutdown("")
        sys.exit()

        rospy.loginfo("%d objects detected" %
                      len(detection_reply.detection.clusters))
#    detection_reply.detection.cluster_model_indices = tuple(xrange(len(detection_reply.detection.clusters)))

    rospy.loginfo("Calling collision map processing")
    processing_call = TabletopCollisionMapProcessingRequest()