// ROS Topic Subscriber methods
public ROSBridgeMsg OnReceiveMessage(string topic, JSONNode raw_msg, ROSBridgeMsg parsed = null)
{
Debug.Log(" LAMP Recieved message");
ROSBridgeMsg result = null;
// Writing all code in here for now. May need to move out to separate handler functions when it gets too unwieldy.
switch (topic)
{
case "/voxblox_node/surface_pointcloud":
Debug.Log(" LAMP Recieved surface point cloud message");
pointCloudLevel = PointCloudLevel.WHITE;
break;
case "/colorized_points_0":
pointCloudLevel = PointCloudLevel.RED;
break;
case "/colorized_points_1":
pointCloudLevel = PointCloudLevel.ORANGE;
break;
case "/colorized_points_2":
pointCloudLevel = PointCloudLevel.YELLOW;
break;
case "/colorized_points_3":
pointCloudLevel = PointCloudLevel.GREEN;
break;
case "/colorized_points_4":
pointCloudLevel = PointCloudLevel.BLUE;
break;
case "/colorized_points_5":
pointCloudLevel = PointCloudLevel.LIGHT_BLUE;
break;
default:
Debug.LogError("Topic not implemented: " + topic);
return(result);
}
PointCloud2Msg pointCloudMsg = new PointCloud2Msg(raw_msg);
this.pcVisualizers[topic].PointCloudLevel = pointCloudLevel;
this.pcVisualizers[topic].SetPointCloud(pointCloudMsg.GetCloud());
Debug.Log("Updated Point Cloud");
return(result);
}
public new static void CallBack(ROSBridgeMsg msg)
{
PointCloud2Msg pointCloudMsg = (PointCloud2Msg)msg;
if (verbose)
{
StringBuilder sb = new StringBuilder();
sb.Append(pointCloudMsg.GetHeader().GetSeq());
sb.Append(":\n");
sb.AppendFormat("Size: {0} X {1} = {2}\n", pointCloudMsg.GetWidth(), pointCloudMsg.GetHeight(), pointCloudMsg.GetWidth() * pointCloudMsg.GetHeight());
sb.Append(pointCloudMsg.GetFieldString());
Debug.Log(sb.ToString());
}
LidarVisualizer visualizer = GameObject.Find(rendererObjectName).GetComponent <LidarVisualizer>();
visualizer.SetPointCloud(pointCloudMsg.GetCloud());
}
// ROS Topic Subscriber methods
public ROSBridgeMsg OnReceiveMessage(string topic, JSONNode raw_msg, ROSBridgeMsg parsed = null)
{
ROSBridgeMsg result = null;
// Writing all code in here for now. May need to move out to separate handler functions when it gets too unwieldy.
switch (topic)
{
case "/voxblox_node/surface_pointcloud":
PointCloud2Msg pointCloudMsg = new PointCloud2Msg(raw_msg);
this.pcVisualizers[topic].SetPointCloud(pointCloudMsg.GetCloud());
Debug.Log("Updated Point Cloud");
break;
default:
Debug.LogError("Topic not implemented: " + topic);
break;
}
return(result);
}
public static void CallBack(ROSBridgeMsg msg) //after receiving the ros message
{
PointCloud2Msg cloudm = (PointCloud2Msg)msg; //point cloud message
PointCloud <PointXYZRGB> points = cloudm.GetCloud(); // get data, the data has point location and point color information
PointXYZRGB point; //variable to access information of single point in the list of points
//voxel mesh with triangles
numPoints = points.Size; //number of points received
mpoints = new Vector3[numPoints];
colors = new Color[numPoints];
//store points and colors of the point cloud separately in arrays
for (int i = 0; i < numPoints; i++)
{
point = points.At(i); //point at index i in the list
mpoints[i] = new Vector3(point.X, point.Z, point.Y);
colors[i] = new Color(point.R / 255.0f, point.G / 255.0f, point.B / 255.0f);
}
// generate child meshes
total_meshes = Mathf.CeilToInt(numPoints * 1.0f / Points_limit * 1.0f); //calculate the number of child meshes required
main_mesh = GameObject.Find(Mesh_name); //find the mesh in unity environment
if (main_mesh == null)
{
main_mesh = new GameObject(Mesh_name); //if no mesh found, create one
}
else
{
//do nothing
}
//initialize the mesh
for (int i = 0; i < total_meshes - 1; i++)
{
Mesh_initialization(i, Points_limit);
}
Mesh_initialization(total_meshes - 1, numPoints - (total_meshes - 1) * Points_limit);
}