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 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 = '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 = '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(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 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 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(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
def create_map_annotation(world, map_name, map_msg):
ann = Annotation()
ann.timestamp = rospy.Time.now()
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 = map_msg.info.width * map_msg.info.resolution
ann.size.y = map_msg.info.height * map_msg.info.resolution
ann.size.z = 0.000001
ann.pose.header.frame_id = '/map'
ann.pose.header.stamp = rospy.Time.now()
ann.pose.pose.pose = map_msg.info.origin
return ann