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
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'
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