//Written by Eric M.
//Modified by Dalton C.
public bool Load()
{
if (!NameSpace.Equals(string.Empty))
{
if (!RobotDescriptionParam.StartsWith("/"))
{
RobotDescriptionParam = "/" + RobotDescriptionParam;
}
}
else
{
if (RobotDescriptionParam.StartsWith("/"))
{
RobotDescriptionParam.TrimStart('/');
}
}
if (Param.get(NameSpace + RobotDescriptionParam, ref robotdescription))
{
TfTreeManager.Instance.AddListener(vis =>
{
tfviz = vis;
Parse();
});
isLoaded = true;
return(true);
}
isLoaded = false;
return(false);
}
public SensorTFInterface()
{
TfTreeManager.Instance.AddListener(vis =>
{
Debug.LogWarning("SensorTFInterface has a tfvisualizer now!");
tfvisualizer = vis;
});
}
public void SetTFVisualizer(TfVisualizer tfv)
{
lock (beforeReady)
{
Debug.LogWarning("TfVisualizer is ready with " + beforeReady.Count + " things waiting for it!");
tfTree = tfv;
while (beforeReady.Count > 0)
{
beforeReady.Dequeue()(tfTree);
}
}
}
// Use this for initialization
void Start()
{
print("Starting...");
TfTreeManager.Instance.AddListener(vis =>
{
//Debug.LogWarning("LaserMeshView has a tfvisualizer now!");
tfvisualizer = vis;
});
if (is_static)
{
theCloud = new mesh_pcloud[1];
GameObject theCloudObject = new GameObject("pointCloudContainer");
theCloudObject.transform.parent = gameObject.transform;
theCloudObject.AddComponent <mesh_pcloud>();
theCloud[0] = theCloudObject.GetComponent <mesh_pcloud>();
registerMeshPclouds();
print("Starting to load static cloud from " + Mesh_File.name);
string[] pointLocations = Mesh_File.text.Split('\n');
string Version = pointLocations[1].Split(' ')[1];
string[] Fields = pointLocations[2].Split(' ');
string[] Sizes = pointLocations[3].Split(' ');
string[] Type = pointLocations[4].Split(' ');
string[] Count = pointLocations[5].Split(' ');
int Width = Int32.Parse(pointLocations[6].Split(' ')[1]);
int Height = Int32.Parse(pointLocations[7].Split(' ')[1]);
string[] Viewpoint = pointLocations[8].Split(' ');
int Points = Int32.Parse(pointLocations[9].Split(' ')[1]);
string Data = pointLocations[10].Split(' ')[1];
if (Data != "ascii")
{
print("Only ascii pcd format is supported...");
print(Data);
return;
}
//theCloud.vertexBuffers[theCloud.whichBuffer] = new Vector3[10 * vertexCount[pointShape]];//[(pointLocations.Length / 6) * vertexCount[pointShape]];
//theCloud.colorBuffers[theCloud.whichBuffer] = new Color[10 * vertexCount[pointShape]];//[(pointLocations.Length / 6) * vertexCount[pointShape]];
//theCloud.recalculateTriangles(10, indices[pointShape], vertexCount[pointShape]);
theCloud[0].vertexBuffers[theCloud[0].whichBuffer] = new Vector3[Points * vertexCount[pointShape]];
theCloud[0].colorBuffers[theCloud[0].whichBuffer] = new Color[Points * vertexCount[pointShape]];
theCloud[0].recalculateTriangles(Points, indices[pointShape], vertexCount[pointShape]);
for (int i = 11; i < Points + 11; i++)
{
try
{
string[] thisPoint = pointLocations[i].Split(' ');
float x = float.Parse(thisPoint[0]);
float y = float.Parse(thisPoint[1]);
float z = float.Parse(thisPoint[2]);
Color c;
if (thisPoint[3][0] != 'n')
{
byte[] color = System.BitConverter.GetBytes(float.Parse(thisPoint[3]));
c = new Color(color[2] * 0.00392156862F, color[1] * 0.00392156862F, color[0] * 0.00392156862F, 1);
}
else
{
c = new Color(0, 0, 0, 1);
}
addPoint(x, y, z, c);
}
catch (Exception e)
{
print(e);
print(pointLocations[i]);
}
}
print("number of points loaded: " + pointNum);
theCloud[0].whichBuffer = (theCloud[0].whichBuffer + 1) % 2;
theCloud[0].updateVertexData = true;
theCloud[0].updateColorData = true;
theCloud[0].updateTriangleData = true;
}
else
{
ROS.ROS_HOSTNAME = "asgard";
ROS.ROS_MASTER_URI = "http://thor:11311";
ROS.Init(new String[0], "mesh_pcloud_renderer");
nh = new NodeHandle();
//nh= rosmaster.getNodeHandle();
print("Subscribing");
theCloud = new mesh_pcloud[accumulate_data && max_acc_frames > 0 ? max_acc_frames : 1];
for (int i = 0; i < theCloud.Length; i++)
{
GameObject theCloudObject = new GameObject("pointCloudContainer" + i);
theCloudObject.transform.parent = gameObject.transform;
theCloudObject.AddComponent <mesh_pcloud>();
theCloud[i] = theCloudObject.GetComponent <mesh_pcloud>();
}
sub = nh.subscribe <Messages.sensor_msgs.PointCloud2>(topic_name, 2, subCallback);
unity_tf_msg = new Messages.geometry_msgs.PolygonStamped();
unity_tf_msg.header = new Messages.std_msgs.Header();
unity_tf_msg.polygon = new Messages.geometry_msgs.Polygon();
unity_tf_msg.header.frame_id = (is_static ? Mesh_File.name : topic_name);
unity_tf_msg.polygon.points = new Messages.geometry_msgs.Point32[3];
unity_tf_pub = nh.advertise <Messages.geometry_msgs.PolygonStamped>("/unity_tf", 10);
ThreadStart pub_thread_start = new ThreadStart(UnityTFPublisher);
pub_thread = new Thread(pub_thread_start);
pub_thread.Start();
}
print("Started");
//theCloud.Initialize();
}
