def publish_marker(ean):
global marker_pub, barcode_to_object
if marker_pub is None:
marker_pub = rospy.Publisher('shelf_objects', Marker, queue_size=100)
m = Marker()
text = Marker()
text.header.frame_id = ean
m.header.frame_id = ean
m.pose.orientation.w = 1
m.action = Marker.ADD
text.action = Marker.ADD
text.type = Marker.TEXT_VIEW_FACING
text.text = ean
text.scale = Vector3(0,0,.05)
text.color = ColorRGBA(1,1,1,1)
text.pose.position.z = 0.05
if ean in barcode_to_object and barcode_to_object[ean].split('.')[-1]=='dae':
m.pose.orientation = Quaternion(*quaternion_from_euler(0,0,np.pi/2))
m.type = Marker.MESH_RESOURCE
m.mesh_resource = refills_models_path + barcode_to_object[ean]
m.scale = Vector3(1,1,1)
m.color = ColorRGBA(0,0,0,0)
m.mesh_use_embedded_materials = True
else:
m.type = Marker.CUBE
m.scale = Vector3(.05,.05,.05)
m.color = ColorRGBA(.5,.5,.5,1.0)
m.text = ean
m.ns = ean
text.ns = ean
m.id = 2
marker_pub.publish(m)
marker_pub.publish(text)
def update_information(self, object_id, marker_ns, visualize, transform, static_transforms, mesh, color,
depth, width, height):
"""
:type marker_ns: str
:type visualize: bool
:type transform: TransformStamped
:param static_transforms: [TransformStamped]
:type static_transforms: list
:param color: [r, g, b, a]
:type color: list
:type mesh: str
:type depth: float
:type width: float
:type height: float
"""
self.__marker_ns = marker_ns
self.__has_visual = visualize
self.update_transform(transform)
for static_transform in static_transforms:
self.set_static_transform(static_transform)
if visualize:
self.update_color(*color)
self.__mesh_path = mesh
self.update_dimensions(depth, width, height)
else:
self.__color = ColorRGBA(0, 0, 0, 1)
self.update_dimensions(1, 1, 1)
self.__initialized = True
self.__object_name = object_id
def __init__(self):
self.transform = None
self.mesh_path = ''
self.object_name = ''
self.object_type = ''
self.ref_frame = ''
self.initialized = False
self.visualize = False
self.color = ColorRGBA(0, 0, 0, 1)
self.scale = Vector3(0.05, 0.05, 0.05)
def update_color(self, r, g, b, a):
self.__color = ColorRGBA()
self.__color.r = r
self.__color.g = g
self.__color.b = b
self.__color.a = a
def update_color(self, r, g, b, a):
self.color = ColorRGBA()
self.color.r = float(r)
self.color.g = float(g)
self.color.b = float(b)
self.color.a = float(a)
def __cell_to_color(self, c):
color = ColorRGBA()
if c.is_marked():
color.r = 1
color.g = 0.5
color.b = 0
elif c.is_free():
color.r = 1
color.g = 1
color.b = 1
elif c.is_obstacle() or c.is_object():
if c.is_blue():
color.r = 0
color.g = 0
color.b = 1
elif c.is_green():
color.r = 0
color.g = 1
color.b = 0
elif c.is_cyan():
color.r = 0
color.g = 1
color.b = 1
elif c.is_red():
color.r = 1
color.g = 0
color.b = 0
elif c.is_magenta():
color.r = 1
color.g = 0
color.b = 1
elif c.is_yellow():
color.r = 1
color.g = 1
color.b = 0
elif c.is_undef():
color.r = 0.5
color.g = 0.5
color.b = 0.5
else:
color.r = 0
color.g = 0
color.b = 0
color.a = 1
return color
def __init__(self):
self.transform = None
self.mesh_path = ''
self.color = ColorRGBA()
def __init__(self):
self.transform = None
self.mesh_path = ''
self.color = ColorRGBA()
self.initialized = False
def __cell_to_color(self, c):
color = ColorRGBA()
if c.is_marked():
color.r = 1
color.g = 0.5
color.b = 0
elif c.is_free():
color.r = 1
color.g = 1
color.b = 1
elif c.is_obstacle() or c.is_object():
if c.is_blue():
color.r = 0
color.g = 0
color.b = 1
elif c.is_green():
color.r = 0
color.g = 1
color.b = 0
elif c.is_cyan():
color.r = 0
color.g = 1
color.b = 1
elif c.is_red():
color.r = 1
color.g = 0
color.b = 0
elif c.is_magenta():
color.r = 1
color.g = 0
color.b = 1
elif c.is_yellow():
color.r = 1
color.g = 1
color.b = 0
elif c.is_undef():
color.r = 0.5
color.g = 0.5
color.b = 0.5
else:
color.r = 0
color.g = 0
color.b = 0
color.a = 1
return color