def _publish_rviz_sim_cameras(self):
marker_array=MarkerArray()
for p in self.payloads:
payload=self.payloads[p]
msg=payload.payload_msg
id = 0
for i in xrange(len(msg.visual_geometry.mesh_resources)):
marker=Marker()
marker.ns="payload_" + msg.name
marker.id=id
marker.type=marker.MESH_RESOURCE
marker.action=marker.ADD
marker.mesh_resource=msg.visual_geometry.mesh_resources[i]
marker.mesh_use_embedded_materials=True
marker.pose = rox_msg.transform2pose_msg(rox_msg.msg2transform(msg.pose) * rox_msg.msg2transform(msg.visual_geometry.mesh_poses[i]))
marker.header.frame_id = payload.attached_link
marker.scale=msg.visual_geometry.mesh_scales[i]
if i < len(msg.visual_geometry.mesh_colors):
marker.color=msg.visual_geometry.mesh_colors[i]
else:
marker.color.a=1
marker.color.r=0.5
marker.color.g=0.5
marker.color.b=0.5
marker.header.stamp=rospy.Time.now()
marker.frame_locked=True
marker._check_types()
#marker.colors.append(ColorRGBA(0.5,0.5,0.5,1))
marker_array.markers.append(marker)
id += 1
for i in xrange(len(msg.visual_geometry.primitives)):
marker=Marker()
marker.ns="payload_" + msg.name
marker.id=id
primitive = msg.visual_geometry.primitives[i]
primitive_type = primitive.type
if primitive_type == SolidPrimitive.BOX:
marker.type=Marker.CUBE
assert len(primitive.dimensions) == 3
marker.scale = Vector3(*primitive.dimensions)
elif primitive_type == SolidPrimitive.CYLINDER:
marker.type=Marker.CYLINDER
assert len(primitive.dimensions) == 2
marker.scale = Vector3(primitive.dimensions[1]*2, primitive.dimensions[1]*2, primitive.dimensions[0])
elif primitive_type == SolidPrimitive.SPHERE:
marker.type=Marker.SPHERE
assert len(primitive.dimensions) == 1
marker.scale = Vector3(*([primitive.dimensions[0]*2]*3))
else:
assert False, "Invalid geometry specified for SolidPrimitive"
marker.action=marker.ADD
marker.pose = rox_msg.transform2pose_msg(rox_msg.msg2transform(msg.pose) * rox_msg.msg2transform(msg.visual_geometry.primitive_poses[i]))
marker.header.frame_id = payload.attached_link
if i < len(msg.visual_geometry.primitive_colors):
marker.color=msg.visual_geometry.primitive_colors[i]
else:
marker.color.a=1
marker.color.r=0.5
marker.color.g=0.5
marker.color.b=0.5
marker.header.stamp=rospy.Time.now()
marker.frame_locked=True
marker._check_types()
marker_array.markers.append(marker)
id += 1
marker_array._check_types
self.rviz_cam_publisher.publish(marker_array)
