def create_alvar_marker_from_info(annotation_info, world, frame_id): ann = Annotation() ann.timestamp = rospy.Time.now() ann.id = unique_id.toMsg(unique_id.fromRandom()) ann.world = world ann.name = "marker " + str(annotation_info['name']) ann.type = 'ar_track_alvar_msgs/AlvarMarker' ann.keywords.append(str(world)) ann.shape = 0 # Cylinder 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 = frame_id ann.pose.header.stamp = rospy.Time.now() ann.pose.pose.pose.position.x = annotation_info['x'] ann.pose.pose.pose.position.y = annotation_info['y'] ann.pose.pose.pose.position.z = annotation_info['height'] (ann.pose.pose.pose.orientation.x, ann.pose.pose.pose.orientation.y, ann.pose.pose.pose.orientation.z, ann.pose.pose.pose.orientation.w ) = tf.transformations.quaternion_from_euler( radians(annotation_info['roll']), radians(annotation_info['pitch']), radians(annotation_info['yaw'])) obj = ar_msgs.AlvarMarker() obj.id = int(annotation_info['name']) obj.confidence = 80 obj.pose.header = ann.pose.header obj.pose.pose = ann.pose.pose.pose return ann, obj
def read(filename): yaml_data = None with open(filename) as f: yaml_data = yaml.load(f) anns_list = [] data_list = [] for t in yaml_data: ann = Annotation() ann.timestamp = rospy.Time.now() ann.world_id = unique_id.toMsg(uuid.UUID('urn:uuid:' + world_id)) ann.data_id = unique_id.toMsg(unique_id.fromRandom()) ann.id = unique_id.toMsg(unique_id.fromRandom()) ann.name = t['name'] ann.type = 'yocs_msgs/Wall' 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 = 0.8 ann.color.g = 0.2 ann.color.b = 0.2 ann.color.a = 0.4 ann.size.x = float(t['width']) ann.size.y = float(t['length']) ann.size.z = float(t['height']) 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']) # walls are assumed to lay on the floor, so z coordinate is zero; # but WCF assumes that the annotation pose is the center of the object ann.pose.pose.pose.position.z += ann.size.z/2.0 anns_list.append(ann) object = Wall() object.name = t['name'] object.length = float(t['length']) object.width = float(t['width']) object.height = float(t['height']) object.pose.header.frame_id = t['frame_id'] object.pose.header.stamp = rospy.Time.now() object.pose.pose.pose = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Pose',t['pose']) data = AnnotationData() data.id = ann.data_id data.data = pickle.dumps(object) data_list.append(data) print ann, object, 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 = 'yocs_msgs/Table' 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 = 3 #CYLINDER ann.color.r = 0.2 ann.color.g = 0.2 ann.color.b = 0.8 ann.color.a = 0.5 ann.size.x = float(t['radius']) * 2 ann.size.y = float(t['radius']) * 2 ann.size.z = float(t['height']) 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']) # tables are assumed to lay on the floor, so z coordinate is zero; # but WCF assumes that the annotation pose is the center of the object ann.pose.pose.pose.position.z += ann.size.z / 2.0 anns_list.append(ann) object = Table() object.name = t['name'] object.radius = float(t['radius']) object.height = float(t['height']) object.pose.header.frame_id = t['frame_id'] object.pose.header.stamp = rospy.Time.now() object.pose.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 = 'yocs_msgs/Table' 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 = 3 #CYLINDER ann.color.r = 0.2 ann.color.g = 0.2 ann.color.b = 0.8 ann.color.a = 0.5 ann.size.x = float(t['radius'])*2 ann.size.y = float(t['radius'])*2 ann.size.z = float(t['height']) 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']) # tables are assumed to lay on the floor, so z coordinate is zero; # but WCF assumes that the annotation pose is the center of the object ann.pose.pose.pose.position.z += ann.size.z/2.0 anns_list.append(ann) object = Table() object.name = t['name'] object.radius = float(t['radius']) object.height = float(t['height']) object.pose.header.frame_id = t['frame_id'] object.pose.header.stamp = rospy.Time.now() object.pose.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 read(filename): yaml_data = None with open(filename) as f: yaml_data = yaml.load(f) anns_list = [] data_list = [] for t in yaml_data: ann = Annotation() ann.timestamp = rospy.Time.now() ann.world_id = unique_id.toMsg(uuid.UUID('urn:uuid:' + world_id)) ann.data_id = unique_id.toMsg(unique_id.fromRandom()) ann.id = unique_id.toMsg(unique_id.fromRandom()) ann.name = t['name'] ann.type = 'ar_track_alvar/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.data = pickle.dumps(object) data_list.append(data) print ann, object, data return anns_list, data_list
def create_waypoint_from_info(annotation_info, world, frame_id): ann = Annotation() ann.timestamp = rospy.Time.now() ann.id = unique_id.toMsg(unique_id.fromRandom()) ann.world = world ann.name = annotation_info['name'] ann.type = 'yocs_msgs/Waypoint' ann.keywords.append(str(world)) ann.shape = 3 # Arrow ann.color.r = 0.2 ann.color.g = 0.2 ann.color.b = 0.8 ann.color.a = 0.5 ann.size.x = float(annotation_info['radius']) * 2 ann.size.y = float(annotation_info['radius']) * 2 ann.size.z = float(annotation_info['height']) ann.pose.header.frame_id = frame_id ann.pose.header.stamp = rospy.Time.now() ann.pose.pose.pose.position.x = annotation_info['x'] ann.pose.pose.pose.position.y = annotation_info['y'] ann.pose.pose.pose.position.z = 0.0 #annotation_info['height'] (ann.pose.pose.pose.orientation.x, ann.pose.pose.pose.orientation.y, ann.pose.pose.pose.orientation.z, ann.pose.pose.pose.orientation.w ) = tf.transformations.quaternion_from_euler( radians(annotation_info['roll']), radians(annotation_info['pitch']), radians(annotation_info['yaw'])) obj = yocs_msgs.Waypoint() obj.name = annotation_info['name'] obj.header.frame_id = frame_id obj.header.stamp = rospy.Time.now() obj.pose.position.x = annotation_info['x'] obj.pose.position.y = annotation_info['y'] obj.pose.position.z = 0.0 (obj.pose.orientation.x, obj.pose.orientation.y, obj.pose.orientation.z, obj.pose.orientation.w) = tf.transformations.quaternion_from_euler( radians(annotation_info['roll']), radians(annotation_info['pitch']), radians(annotation_info['yaw'])) # waypoints are assumed to lay on the floor, so z coordinate is zero; # but WCF assumes that the annotation pose is the center of the object ann.pose.pose.pose.position.z += ann.size.z / 2.0 return ann, obj
def read(file): ''' Parse a yaml file containing a single map message @param file Target file path ''' yaml_data = None with open(file) as f: yaml_data = yaml.load(f) 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 = map_name ann.type = 'nav_msgs/OccupancyGrid' ann.keywords.append(map_name) ann.shape = 1 # CUBE ann.color.r = 0.2 ann.color.g = 0.2 ann.color.b = 0.2 ann.color.a = 0.01 ann.size.x = yaml_data['info']['width'] * yaml_data['info']['resolution'] ann.size.y = yaml_data['info']['height'] * yaml_data['info']['resolution'] ann.size.z = 0.000001 ann.pose.header.frame_id = '/map' ann.pose.header.stamp = rospy.Time.now() ann.pose.pose.pose = message_converter.convert_dictionary_to_ros_message( 'geometry_msgs/Pose', yaml_data['info']['origin']) object = OccupancyGrid() genpy.message.fill_message_args(object, yaml_data) map = AnnotationData() map.id = ann.data_id map.type = ann.type map.data = serialize_msg(object) return [ann], [ map ] # return as lists, as is what expects save_annotations_data service
def read(filename): ''' Parse a yaml file containing a single map message @param filename Target file path ''' yaml_data = None with open(filename) as f: yaml_data = yaml.load(f) ann = Annotation() ann.timestamp = rospy.Time.now() ann.world_id = unique_id.toMsg(uuid.UUID('urn:uuid:' + world_id)) ann.data_id = unique_id.toMsg(unique_id.fromRandom()) ann.id = unique_id.toMsg(unique_id.fromRandom()) ann.name = map_name ann.type = 'nav_msgs/OccupancyGrid' ann.keywords.append(map_name) ann.shape = 1 # CUBE ann.color.r = 0.2 ann.color.g = 0.2 ann.color.b = 0.2 ann.color.a = 1.0 ann.size.x = yaml_data['info']['width'] * yaml_data['info']['resolution'] ann.size.y = yaml_data['info']['height'] * yaml_data['info']['resolution'] ann.size.z = 0.000001 ann.pose.header.frame_id = '/map' ann.pose.header.stamp = rospy.Time.now() ann.pose.pose.pose = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Pose', yaml_data['info']['origin']) object = OccupancyGrid() genpy.message.fill_message_args(object, yaml_data) map = AnnotationData() map.id = ann.data_id map.data = pickle.dumps(object) print ann return [ann], [map] # return as lists, as is what expects save_annotations_data service