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
def create_object(name):
obj = Object()
obj.name = name
return obj