def detect_markers_cb(request):
    rospy.loginfo('Received [%s] request.' % SERVICE)
    r = random.random()
    response = GetObjectsResponse()
    response.stamp = rospy.Time.now()
    if r > 0.8:  # no markers found
        return response
    if r > 0.6:  # two markers found
        response.objects.append(create_object('B1'))
    if r > 0.3:  # one marker found
        response.objects.append(create_object('B2'))
    else:
        response.objects.append(create_object('B3'))
    return response
예제 #2
0
 def execute(self, ud):
     ud.response = GetObjectsResponse()
     for c, b, n in zip(ud.clusters, ud.bounding_boxes, ud.names):
         obj = Object()
         q = self.get_orientation(b.vertices)
         obj.dimensions.vector = b.dimensions
         obj.pose.header = c.header
         obj.pose.pose.position = b.center
         obj.pose.pose.orientation.w = q[0]
         obj.pose.pose.orientation.x = q[1]
         obj.pose.pose.orientation.y = q[2]
         obj.pose.pose.orientation.z = q[3]
         obj.name = n
         ud.response.objects.append(obj)
     return 'done'
예제 #3
0
def detect_markers_cb(request):
    rospy.loginfo('Received [%s] request.' % SERVICE)
    markers = defaultdict(list)
    frame_id = '/openni_rgb_optical_frame'

    def markers_cb(msg):
        for m in msg.markers:
            markers[m.id].append(m.pose.pose)

    marker_sub = rospy.Subscriber('ar_pose_markers', ARMarkers, markers_cb)
    accumulate_time = rospy.get_param('~accumulate_time', 2)
    rospy.sleep(accumulate_time)
    marker_sub.unregister()

    response = GetObjectsResponse()
    response.stamp = rospy.Time.now()
    for i, l in markers.iteritems():
        obj = Object()
        obj.name = str(i)
        obj.pose.header.frame_id = frame_id
        obj.pose.pose = average_pose(l)
        response.objects.append(obj)
    return response