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" }