public static void Draw <C>(PoseMsg message, Drawing3d drawing, float size = 0.1f, bool drawUnityAxes = false)
where C : ICoordinateSpace, new()
{
VisualizationUtils.DrawAxisVectors <C>(
drawing,
new Vector3Msg(message.position.x, message.position.y, message.position.z),
message.orientation,
size,
drawUnityAxes
);
}
public override void Draw(Drawing3d drawing, JointStateMsg message, MessageMetadata meta)
{
#if URDF_IMPORTER
drawing.SetTFTrackingSettings(m_TFTrackingSettings, message.header);
var color = SelectColor(m_Color, meta);
m_RobotData.DrawGhost(drawing, message, color);
if (m_ShowEffort)
{
m_RobotData.DrawEffort(drawing, message, color);
}
#endif
}
public override void Draw(Drawing3d drawing, PointMsg msg, MessageMetadata meta)
{
// If the user doesn't specify a color, SelectColor helpfully picks one
// based on the message topic.
Color finalColor = VisualizationUtils.SelectColor(m_Color, meta);
// Most of the default visualizers offer static drawing functions
// so that your own visualizers can easily send work to them.
PointDefaultVisualizer.Draw <FLU>(msg, drawing, finalColor, m_Size);
// You can also directly use the drawing functions provided by the Drawing class
drawing.DrawLabel(m_Label, msg.From <FLU>(), finalColor, m_Size);
}
public static void Draw <C>(PlaneMsg message, Drawing3d drawing, Color color, Vector3 origin, float size = 10.0f) where C : ICoordinateSpace, new()
{
Vector3 normal = new Vector3 <C>((float)message.coef[0], (float)message.coef[1], (float)message.coef[2]).toUnity;
float d = (float)message.coef[3];
float normalScale = (Vector3.Dot(normal, origin) + d) / normal.sqrMagnitude;
Vector3 center = origin - normal * normalScale;
Vector3 forward = (Mathf.Abs(normal.x) > Mathf.Abs(normal.y)) ? Vector3.Cross(normal, Vector3.up).normalized : Vector3.Cross(normal, Vector3.right).normalized;
Vector3 side = Vector3.Cross(normal, forward).normalized;
Vector3 diagonalA = (forward + side) * size;
Vector3 diagonalB = (forward - side) * size;
drawing.DrawQuad(center - diagonalA, center + diagonalB, center + diagonalA, center - diagonalB, color, true);
}
public static void Draw <C>(MeshMsg message, Drawing3d drawing, Color color, GameObject origin = null) where C : ICoordinateSpace, new()
{
Mesh mesh = new Mesh();
mesh.vertices = message.vertices.Select(v => v.From <C>()).ToArray();
mesh.triangles = message.triangles.SelectMany(tri => tri.vertex_indices.Select(i => (int)i)).ToArray();
if (origin != null)
{
drawing.DrawMesh(mesh, origin.transform, color);
}
else
{
drawing.DrawMesh(mesh, Vector3.zero, Quaternion.identity, Vector3.one, color);
}
}
public void Redraw()
{
if (m_Mesh == null)
{
m_Mesh = new Mesh();
m_Mesh.vertices = new[]
{ Vector3.zero, new Vector3(0, 0, 1), new Vector3(1, 0, 1), new Vector3(1, 0, 0) };
m_Mesh.uv = new[] { Vector2.zero, Vector2.up, Vector2.one, Vector2.right };
m_Mesh.triangles = new[] { 0, 1, 2, 2, 3, 0 };
}
if (m_Material == null)
{
m_Material = (m_Settings.m_Material != null) ? new Material(m_Settings.m_Material) : new Material(Shader.Find("Unlit/OccupancyGrid"));
}
m_Material.mainTexture = GetTexture();
m_Material.SetColor(k_Color0, m_Settings.m_Unoccupied);
m_Material.SetColor(k_Color100, m_Settings.m_Occupied);
m_Material.SetColor(k_ColorUnknown, m_Settings.m_Unknown);
var origin = m_Message.info.origin.position.From <FLU>();
var rotation = m_Message.info.origin.orientation.From <FLU>();
rotation.eulerAngles += new Vector3(0, -90, 0); // TODO: Account for differing texture origin
var scale = m_Message.info.resolution;
if (m_Drawing == null)
{
m_Drawing = Drawing3dManager.CreateDrawing();
}
else
{
m_Drawing.Clear();
}
m_Drawing.SetTFTrackingSettings(m_Settings.m_TFTrackingSettings, m_Message.header);
// offset the mesh by half a grid square, because the message's position defines the CENTER of grid square 0,0
Vector3 drawOrigin = origin - rotation * new Vector3(scale * 0.5f, 0, scale * 0.5f) + m_Settings.m_Offset;
m_Drawing.DrawMesh(m_Mesh, drawOrigin, rotation,
new Vector3(m_Message.info.width * scale, 1, m_Message.info.height * scale), m_Material);
}
public override void Draw(Drawing3d drawing, ImuMsg message, MessageMetadata meta)
{
drawing.SetTFTrackingSettings(m_TFTrackingSettings, message.header);
Draw <FLU>(message, drawing, SelectColor(m_Color, meta), m_LengthScale, m_SphereRadius, m_Thickness);
}
public static void Draw <C>(ImuMsg message, Drawing3d drawing, Color color, float lengthScale = 1, float sphereRadius = 1, float thickness = 0.01f) where C : ICoordinateSpace, new()
{
QuaternionDefaultVisualizer.Draw <C>(message.orientation, drawing);
drawing.DrawArrow(Vector3.zero, message.linear_acceleration.From <C>() * lengthScale, color, thickness);
VisualizationUtils.DrawAngularVelocityArrow(drawing, message.angular_velocity.From <C>(), Vector3.zero, color, sphereRadius, thickness);
}
public override void Draw(Drawing3d drawing, PoseArrayMsg message, MessageMetadata meta)
{
drawing.SetTFTrackingSettings(m_TFTrackingSettings, message.header);
Draw <FLU>(message, drawing, m_Size, m_DrawUnityAxes);
}
public override void Draw(Drawing3d drawing, PoseStampedMsg message, MessageMetadata meta)
{
drawing.SetTFTrackingSettings(m_TFTrackingSettings, message.header);
PoseDefaultVisualizer.Draw <FLU>(message.pose, drawing, m_Size, m_DrawUnityAxes);
}
public override void Draw(Drawing3d drawing, GridCellsMsg message, MessageMetadata meta)
{
drawing.SetTFTrackingSettings(m_TFTrackingSettings, message.header);
Draw <FLU>(message, drawing, SelectColor(m_Color, meta), m_Radius);
}
public static void Draw <C>(MultiEchoLaserScanMsg message, Drawing3d drawing, MultiEchoLaserScanVisualizerSettings settings)
where C : ICoordinateSpace, new()
{
Draw <C>(message, drawing.AddPointCloud(message.ranges.Length), settings);
}
public override void Draw(Drawing3d drawing, PlaneMsg message, MessageMetadata meta)
{
Draw <FLU>(message, drawing, SelectColor(m_Color, meta));
}
public override void Draw(Drawing3d drawing, SolidPrimitiveMsg message, MessageMetadata meta)
{
Draw <FLU>(message, drawing, SelectColor(m_Color, meta), m_Origin);
}
public override void Draw(Drawing3d drawing, PoseMsg message, MessageMetadata meta)
{
Draw <FLU>(message, drawing, m_Size, m_DrawUnityAxes);
}
public static void Draw <C>(PlaneMsg message, Drawing3d drawing, Color color, GameObject center = null, float size = 10.0f)
where C : ICoordinateSpace, new()
{
Draw <C>(message, drawing, color, (center != null) ? center.transform.position : Vector3.zero, size);
}
public override void Draw(Drawing3d drawing, InertiaStampedMsg message, MessageMetadata meta)
{
drawing.SetTFTrackingSettings(m_TFTrackingType, message.header);
Vector3DefaultVisualizer.Draw <FLU>(message.inertia.com, drawing, m_Origin, SelectColor(m_Color, meta), SelectLabel(m_Label, meta), m_Radius);
}
public override void Draw(Drawing3d drawing, InertiaMsg message, MessageMetadata meta)
{
Vector3DefaultVisualizer.Draw <FLU>(message.com, drawing, m_Origin, SelectColor(m_Color, meta), "Center of mass", m_Radius);
}
public static void Draw <C>(OdometryMsg message, Drawing3d drawing, Color color, float lengthScale = 1, float sphereRadius = 1, float thickness = 0.01f) where C : ICoordinateSpace, new()
{
PoseDefaultVisualizer.Draw <C>(message.pose.pose, drawing);
TwistDefaultVisualizer.Draw <C>(message.twist.twist, drawing, color, message.pose.pose.position.From <C>(), lengthScale, sphereRadius, thickness);
}
public override void Draw(Drawing3d drawing, AccelWithCovarianceMsg message, MessageMetadata meta)
{
Draw <FLU>(message, drawing, SelectColor(m_Color, meta), m_Origin, m_LengthScale, m_SphereRadius, m_Thickness);
}
public override void Draw(Drawing3d drawing, MagneticFieldMsg message, MessageMetadata meta)
{
drawing.SetTFTrackingSettings(m_TFTrackingSettings, message.header);
Draw <FLU>(message, drawing, SelectColor(m_Color, meta));
}
public static void Draw <C>(AccelWithCovarianceMsg message, Drawing3d drawing, Color color, GameObject origin, float lengthScale = 1, float sphereRadius = 1, float thickness = 0.01f) where C : ICoordinateSpace, new()
{
AccelDefaultVisualizer.Draw <FLU>(message.accel, drawing, color, origin, lengthScale, sphereRadius, thickness);
}
public static void Draw <C>(PathMsg message, Drawing3d drawing, Color color, float thickness = 0.1f, Vector3 offset = default(Vector3))
where C : ICoordinateSpace, new()
{
drawing.DrawPath(message.poses.Select(pose => pose.pose.position.From <C>() + offset), color, thickness);
}
public override void Draw(Drawing3d drawing, AccelWithCovarianceStampedMsg message, MessageMetadata meta)
{
drawing.SetTFTrackingSettings(m_TFTrackingSettings, message.header);
AccelWithCovarianceDefaultVisualizer.Draw <FLU>(message.accel, drawing, SelectColor(m_Color, meta), m_Origin, m_LengthScale, m_SphereRadius, m_Thickness);
}
public static void Draw <C>(MagneticFieldMsg message, Drawing3d drawing, Color color, float lengthScale = 1) where C : ICoordinateSpace, new()
{
drawing.DrawArrow(Vector3.zero, message.magnetic_field.From <C>() * lengthScale, color);
}
public static void Draw <C>(GridCellsMsg message, Drawing3d drawing, Color color, float radius = 0.01f)
where C : ICoordinateSpace, new()
{
VisualizationUtils.DrawPointCloud <C>(message.cells, drawing, color, radius);
}
public override void Draw(Drawing3d drawing, MultiEchoLaserScanMsg message, MessageMetadata meta)
{
Draw <FLU>(message, drawing, Settings);
}
public override void Draw(Drawing3d drawing, PathMsg message, MessageMetadata meta)
{
drawing.SetTFTrackingSettings(m_TFTrackingSettings, message.header);
Draw <FLU>(message, drawing, SelectColor(m_Color, meta), m_Thickness, m_Offset);
}
public override void Draw(Drawing3d drawing, PoseWithCovarianceMsg message, MessageMetadata meta)
{
PoseDefaultVisualizer.Draw <FLU>(message.pose, drawing, m_Size, m_DrawUnityAxes);
}
