public SubmapCloudMsg(JSONNode node)
{
this.submap_version = node["submap_version"].AsInt;
this.resolution = node["resolution"].AsFloat;
this.finished = node["finished"].AsBool;
this.cloud = new PointCloud2Msg(node["cloud"]);
}
private IEnumerator DrawCloud(PointCloud2Msg cloud_msg)
{
PointCloud2Prefab converter = new PointCloud2Prefab(cloud_msg.GetPoints(), cloud_msg.GetColors(), cloud_msg.GetHeader().GetSeq(), matVertex);
GameObject cloud_go = converter.GetPrefab();
cloud_go.transform.position = new Vector3(0.0f, 0.0f, 0.0f);
cloud_go.transform.rotation = new Quaternion(0.0f, 0.0f, 0.0f, 1);
cloud_go.transform.parent = pointCloudContainer.transform;
cloud_go.SetActive(true); // activate Point Cloud in Unity
yield return(null);
}
public new static void CallBack(ROSBridgeMsg msg)
{
if (_i % 5 == 0)
{
PointCloud2Msg cloud_msg = (PointCloud2Msg)msg;
GameObject robot = GameObject.Find("Robot"); // "Robot" -->
ArseaViewer viewer = (ArseaViewer)robot.GetComponent(typeof(ArseaViewer));
viewer.PushCloud(cloud_msg);
}
_i = _i + 1;
}
// 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);
}
// This function should fire on each ros message
public new static void CallBack(ROSBridgeMsg msg)
{
cloud = (PointCloud2Msg)msg;
List <PointXYZRGB> points = (List <PointXYZRGB>)DepthSubscriber.cloud.GetCloud().Points;
PointXYZRGB point = points[0];
Debug.Log(point.R + " | " + point.G + " | " + point.B + " ----- " + point.X + " | " + point.Y + " | " + point.Z);
//Debug.Log("image received");
//Debug.Log("Here is the message received " + msg);
// Update ball position, or whatever
//ball.x = msg.x; // Check msg definition in rosbridgelib
//ball.y = msg.y;
//ball.z = msg.z;
}
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);
}
public void PushCloud(PointCloud2Msg cloud_msg)
{
StartCoroutine("DrawCloud", cloud_msg); // starts a C-O-routine called "drawcloud"
}
