// 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); }