public static void SetScale(Transform transform, Link.Geometry geometry, GeometryTypes geometryType)
{
switch (geometryType)
{
case GeometryTypes.Box:
transform.localScale =
new Vector3((float)geometry.box.size[1], (float)geometry.box.size[2], (float)geometry.box.size[0]);
break;
case GeometryTypes.Cylinder:
transform.localScale = new Vector3(
(float)geometry.cylinder.radius * 2,
(float)geometry.cylinder.length / 2,
(float)geometry.cylinder.radius * 2);
break;
case GeometryTypes.Sphere:
transform.localScale = new Vector3(
(float)geometry.sphere.radius * 2,
(float)geometry.sphere.radius * 2,
(float)geometry.sphere.radius * 2);
break;
case GeometryTypes.Mesh:
if (geometry?.mesh?.scale != null)
{
Vector3 scale = geometry.mesh.scale.ToVector3().Ros2UnityScale();
transform.localScale = Vector3.Scale(transform.localScale, scale);
transform.localPosition = Vector3.Scale(transform.localPosition, scale);
}
break;
}
}
public static Link.Geometry ExportGeometryData(GeometryTypes geometryType, Transform transform, bool isCollisionGeometry = false)
{
Link.Geometry geometry = null;
switch (geometryType)
{
case GeometryTypes.Box:
geometry = new Link.Geometry(new Link.Geometry.Box(ExportUrdfSize(transform)));
break;
case GeometryTypes.Cylinder:
geometry = new Link.Geometry(
null,
new Link.Geometry.Cylinder(ExportUrdfRadius(transform), ExportCylinderHeight(transform)));
break;
case GeometryTypes.Sphere:
geometry = new Link.Geometry(null, null, new Link.Geometry.Sphere(ExportUrdfRadius(transform)));
break;
case GeometryTypes.Mesh:
geometry = ExportGeometryMeshData(transform.GetChild(0).gameObject, ExportUrdfSize(transform), isCollisionGeometry);
break;
}
return(geometry);
}
public static Link.Collision ExportCollisionData(this UrdfCollision urdfCollision)
{
UrdfGeometry.CheckForUrdfCompatibility(urdfCollision.transform, urdfCollision.geometryType);
Link.Geometry geometry = UrdfGeometry.ExportGeometryData(urdfCollision.geometryType, urdfCollision.transform, true);
string collisionName = urdfCollision.name == "unnamed" ? null : urdfCollision.name;
return(new Link.Collision(geometry, collisionName, UrdfOrigin.ExportOriginData(urdfCollision.transform)));
}
public static Link.Visual ExportVisualData(this UrdfVisual urdfVisual)
{
UrdfGeometry.CheckForUrdfCompatibility(urdfVisual.transform, urdfVisual.geometryType);
Link.Geometry geometry = UrdfGeometry.ExportGeometryData(urdfVisual.geometryType, urdfVisual.transform);
Link.Visual.Material material = null;
if ((geometry.mesh != null))
{
material = UrdfMaterial.ExportMaterialData(urdfVisual.GetComponentInChildren <MeshRenderer>().sharedMaterial);
}
string visualName = urdfVisual.name == "unnamed" ? null : urdfVisual.name;
return(new Link.Visual(geometry, visualName, UrdfOrigin.ExportOriginData(urdfVisual.transform), material));
}
public static Link.Visual ExportVisualData(this UrdfVisual urdfVisual)
{
UrdfGeometry.CheckForUrdfCompatibility(urdfVisual.transform, urdfVisual.GeometryType);
Link.Geometry geometry = UrdfGeometry.ExportGeometryData(urdfVisual.GeometryType, urdfVisual.transform);
Link.Visual.Material material = null;
if (!(geometry.mesh != null && geometry.mesh.filename.ToLower().EndsWith(".dae"))) //Collada files contain their own materials
{
material = UrdfMaterial.ExportMaterialData(urdfVisual.GetComponentInChildren <MeshRenderer>().sharedMaterial);
}
string visualName = urdfVisual.name == "unnamed" ? null : urdfVisual.name;
return(new Link.Visual(geometry, visualName, UrdfOrigin.ExportOriginData(urdfVisual.transform), material));
}
public void _Reads_Link_Visual_Geometry()
{
robot.ConstructFromString(urdfString);
Link sensorLink = robot.links.Find(link => link.name == "sensor_link");
Link.Geometry geometry = sensorLink.visuals[0].geometry;
Link.Geometry.Sphere sphere = geometry.sphere;
Assert.That(sphere.radius, Is.EqualTo(0.1d));
Link baseLink = robot.links.Find(link => link.name == "base_link");
Link.Geometry.Mesh mesh = baseLink.visuals[0].geometry.mesh;
Assert.That(mesh.filename, Is.EqualTo("filename.stl"));
Assert.That(mesh.scale, Is.EqualTo(new[] { 0.001d, 0.001d, 0.001d }));
}
public static GeometryTypes GetGeometryType(Link.Geometry geometry)
{
if (geometry.box != null)
{
return(GeometryTypes.Box);
}
if (geometry.cylinder != null)
{
return(GeometryTypes.Cylinder);
}
if (geometry.sphere != null)
{
return(GeometryTypes.Sphere);
}
return(GeometryTypes.Mesh);
}
public static void Create(Transform parent, GeometryTypes geometryType, Link.Geometry geometry = null)
{
GameObject geometryGameObject = null;
switch (geometryType)
{
case GeometryTypes.Box:
geometryGameObject = new GameObject(geometryType.ToString());
geometryGameObject.AddComponent <BoxCollider>();
break;
case GeometryTypes.Cylinder:
geometryGameObject = CreateCylinderCollider();
break;
case GeometryTypes.Sphere:
geometryGameObject = new GameObject(geometryType.ToString());
geometryGameObject.AddComponent <SphereCollider>();
break;
case GeometryTypes.Mesh:
if (geometry != null)
{
geometryGameObject = CreateMeshCollider(geometry.mesh);
}
else
{
geometryGameObject = new GameObject(geometryType.ToString());
geometryGameObject.AddComponent <MeshCollider>();
}
//var collider = geometryGameObject.GetComponent<MeshCollider>();
//collider.convex = true;
break;
}
if (geometryGameObject != null)
{
geometryGameObject.transform.SetParentAndAlign(parent);
if (geometry != null)
{
SetScale(parent, geometry, geometryType);
}
}
}
public static void Create(Transform parent, GeometryTypes geometryType, Link.Geometry geometry = null)
{
GameObject geometryGameObject = null;
switch (geometryType)
{
case GeometryTypes.Box:
geometryGameObject = GameObject.CreatePrimitive(PrimitiveType.Cube);
geometryGameObject.transform.DestroyImmediateIfExists <BoxCollider>();
break;
case GeometryTypes.Cylinder:
geometryGameObject = GameObject.CreatePrimitive(PrimitiveType.Cylinder);
geometryGameObject.transform.DestroyImmediateIfExists <CapsuleCollider>();
break;
case GeometryTypes.Sphere:
geometryGameObject = GameObject.CreatePrimitive(PrimitiveType.Sphere);
geometryGameObject.transform.DestroyImmediateIfExists <SphereCollider>();
break;
case GeometryTypes.Mesh:
if (geometry != null)
{
geometryGameObject = CreateMeshVisual(geometry.mesh);
}
//else, let user add their own mesh gameObject
break;
}
if (geometryGameObject != null)
{
geometryGameObject.transform.SetParentAndAlign(parent);
if (geometry != null)
{
SetScale(parent, geometry, geometryType);
}
}
}
/// <summary>
/// Compares geometry information of two visuals
/// </summary>
/// <param name="source">First visual's geometry information to be compared</param>
/// <param name="exported">Second visuals's geometry information to be compared</param>
/// <param name="indent">Indent level in the log file</param>
/// <returns></returns>
private bool CompareGeometry(Link.Geometry source, Link.Geometry exported, int indent)
{
if (source.box != null && exported != null)
{
bool boxEqual = source.box.size.DoubleDeltaCompare(exported.box.size, .0001);
linkLog.AppendLine(String.Format("{0}Geometry", Indent(indent)));
linkLog.AppendLine(String.Format("{0}Type:{1,5}", Indent(indent), "Box"));
linkLog.AppendLine(String.Format("{0}Dimensions Equal: {1,6} ", Indent(indent), boxEqual));
linkLog.AppendLine(String.Format("{0}Dimensions: Source: {1,5:F3} {2,5:F3} {3,5:F3} Exported: {4,5:F3} {5,5:F3} {6,5:F3}", Indent(indent), source.box.size[0], source.box.size[1], source.box.size[2], exported.box.size[0], exported.box.size[1], exported.box.size[2]));
if (boxEqual)
{
return(true);
}
else
{
return(false);
}
}
if (source.cylinder != null && exported.cylinder != null)
{
bool cylinderEqual = (source.cylinder.radius == exported.cylinder.radius && source.cylinder.length == exported.cylinder.length);
linkLog.AppendLine(String.Format("{0}Geometry:", Indent(indent)));
linkLog.AppendLine(String.Format("{0}Type:{1,5}", Indent(indent), "Cylinder"));
linkLog.AppendLine(String.Format("{0}Dimensions Equal: {1,6}", Indent(indent), cylinderEqual));
linkLog.AppendLine(String.Format("{0}Source: Radius: {1,5:F3} Length: {2,5:F3} Exported: Radius: {3,5:F3} Length: {4,5:F3}", Indent(indent), source.cylinder.radius, source.cylinder.length, exported.cylinder.radius, exported.cylinder.length));
if (cylinderEqual)
{
return(true);
}
else
{
return(false);
}
}
if (source.sphere != null && exported.sphere != null)
{
bool sphereEqual = source.sphere.radius.EqualsDelta(exported.sphere.radius, .0001);
linkLog.AppendLine(String.Format("{0}Geometry:", Indent(indent)));
linkLog.AppendLine(String.Format("{0}Type:{1,5}", Indent(indent), "Sphere"));
linkLog.AppendLine(String.Format("{0}Dimensions Equal: {1,6}", Indent(indent), sphereEqual));
linkLog.AppendLine(String.Format("{0}Source: Radius: {1,5:F3} Exported: Radius: {1,5:F3}", Indent(indent), source.sphere.radius, exported.sphere.radius));
if (sphereEqual)
{
return(true);
}
else
{
return(false);
}
}
if (source.mesh != null && exported.mesh != null)
{
bool meshNameEqual = (Path.GetFileName(source.mesh.filename) == Path.GetFileName(exported.mesh.filename));
linkLog.AppendLine(String.Format("{0}Geometry:", Indent(indent)));
linkLog.AppendLine(String.Format("{0}Type:{1,5}", Indent(indent), "Mesh"));
linkLog.AppendLine(String.Format("{0}Name Equal: {1,6}", Indent(indent), meshNameEqual));
linkLog.AppendLine(String.Format("{0}Name: Source: {1,12} Exported: {2,12}", Indent(indent), source.mesh.filename, exported.mesh.filename));
if (!meshNameEqual)
{
return(false);
}
if (source.mesh.scale != null && exported.mesh.scale != null)
{
for (int i = 0; i < source.mesh.scale.Length; i++)
{
if (source.mesh.scale[i] != exported.mesh.scale[i])
{
linkLog.AppendLine(String.Format("{0}Scale Equal: {1,6}", Indent(indent), "False"));
linkLog.AppendLine(String.Format("{0}Scale: Source: {1,5:F3} {2,5:F3} {3,5:F3} Exported: {4,5:F3} {5,5:F3} {6,5:F3}", Indent(indent), source.mesh.scale[0], source.mesh.scale[1], source.mesh.scale[2], exported.mesh.scale[0], exported.mesh.scale[1], exported.mesh.scale[2]));
return(false);
}
}
linkLog.AppendLine(String.Format("{0}Scale Equal: {1,6}", Indent(indent), "True"));
linkLog.AppendLine(String.Format("{0}Scale:{1,5:F3} {2,5:F3} {3,5:F3}", Indent(indent), source.mesh.scale[0], source.mesh.scale[1], source.mesh.scale[2]));
}
else if (source.mesh.scale == null && exported.mesh.scale == null)
{
linkLog.AppendLine(String.Format("{0}Scales Equal : (1,1,1)", Indent(indent)));
}
else if ((exported.mesh.scale == null && source.mesh.scale.DoubleDeltaCompare(new double[] { 1, 1, 1 }, 0)) || (source.mesh.scale == null && exported.mesh.scale.DoubleDeltaCompare(new double[] { 1, 1, 1 }, 0)))
{
linkLog.AppendLine(String.Format("{0}Scales Equal : (1,1,1)", Indent(indent)));
}
else
{
linkLog.AppendLine(String.Format("{0}Scale Equal: {1,6} ", Indent(indent), "False"));
return(false);
}
return(true);
}
linkLog.AppendLine(String.Format("{0}No compatible texture shapes found", Indent(indent)));
return(false);
}
