def read(file): yaml_data = None with open(file) as f: yaml_data = yaml.load(f) anns_list = [] data_list = [] for t in yaml_data: ann = Annotation() ann.timestamp = rospy.Time.now() ann.data_id = unique_id.toMsg(unique_id.fromRandom()) ann.id = unique_id.toMsg(unique_id.fromRandom()) ann.world = world ann.name = t['name'] ann.type = 'ar_track_alvar_msgs/AlvarMarker' for i in range(0, random.randint(0, 11)): ann.keywords.append('kw' + str(random.randint(1, 11))) # if 'prev_id' in vars(): # ann.relationships.append(prev_id) # prev_id = ann.id ann.shape = 1 # CUBE ann.color.r = 1.0 ann.color.g = 1.0 ann.color.b = 1.0 ann.color.a = 1.0 ann.size.x = 0.18 ann.size.y = 0.18 ann.size.z = 0.01 ann.pose.header.frame_id = t['frame_id'] ann.pose.header.stamp = rospy.Time.now() ann.pose.pose.pose = message_converter.convert_dictionary_to_ros_message( 'geometry_msgs/Pose', t['pose']) anns_list.append(ann) object = AlvarMarker() object.id = t['id'] object.confidence = t['confidence'] object.pose.header.frame_id = t['frame_id'] object.pose.header.stamp = rospy.Time.now() object.pose.pose = message_converter.convert_dictionary_to_ros_message( 'geometry_msgs/Pose', t['pose']) data = AnnotationData() data.id = ann.data_id data.type = ann.type data.data = serialize_msg(object) data_list.append(data) return anns_list, data_list
def read(file): yaml_data = None with open(file) as f: yaml_data = yaml.load(f) anns_list = [] data_list = [] for t in yaml_data: ann = Annotation() ann.timestamp = rospy.Time.now() ann.data_id = unique_id.toMsg(unique_id.fromRandom()) ann.id = unique_id.toMsg(unique_id.fromRandom()) ann.world = world ann.name = t['name'] ann.type = 'ar_track_alvar_msgs/AlvarMarker' for i in range(0, random.randint(0,11)): ann.keywords.append('kw'+str(random.randint(1,11))) # if 'prev_id' in vars(): # ann.relationships.append(prev_id) # prev_id = ann.id ann.shape = 1 # CUBE ann.color.r = 1.0 ann.color.g = 1.0 ann.color.b = 1.0 ann.color.a = 1.0 ann.size.x = 0.18 ann.size.y = 0.18 ann.size.z = 0.01 ann.pose.header.frame_id = t['frame_id'] ann.pose.header.stamp = rospy.Time.now() ann.pose.pose.pose = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Pose',t['pose']) anns_list.append(ann) object = AlvarMarker() object.id = t['id'] object.confidence = t['confidence'] object.pose.header.frame_id = t['frame_id'] object.pose.header.stamp = rospy.Time.now() object.pose.pose = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Pose',t['pose']) data = AnnotationData() data.id = ann.data_id data.type = ann.type data.data = serialize_msg(object) data_list.append(data) return anns_list, data_list
def publish(markers): ar_list = AlvarMarkers() for m in markers: ar = AlvarMarker() ar.id = m['id'] ar.confidence = m['confidence'] ar.pose.header.frame_id = m['frame_id'] ar.pose.header.stamp = rospy.Time.now() ar.pose.pose = message_converter.convert_dictionary_to_ros_message( 'geometry_msgs/Pose', m['pose']) ar_list.markers.append(ar) ar_pub.publish(ar_list) return
def ar_pose_grabbing(self, data): num_markers = 0 marker = AlvarMarker() marker.confidence = 0 for i in data.markers: marker = i num_markers = num_markers + 1 # rospy.loginfo('confidence is: %f', i.confidence) # if i.confidence > self.confidence_th: # num_markers = num_markers + 1 # if i.confidence > marker.conficence: # marker = i # if num_markers > 0: if num_markers == 1: if len(self.ar_pose_x_history) < self.ar_pose_history_len: # Consider to compare the marker id to determine whether to append a marker self.ar_pose_history_append(marker.pose.pose) else: self.ar_pose_history_pop() self.ar_pose_history_append(marker.pose.pose) self.update_data_hist(marker.pose.pose) else: rospy.loginfo('No marker is reliablly detected!!!')