/// <summary> /// Reads paramters of sensorPointCloud2 /// </summary> /// <param name="pc"></param> /// <returns></returns> public string readCloudRosParameters(SensorPointCloud2 pc) { string sout = ""; sout += "height: " + pc.height.ToString() + " "; sout += "width: " + pc.width.ToString() + " "; sout += "is_big-endian: " + pc.is_bigendian.ToString() + " "; sout += "point step: " + pc.point_step.ToString() + " "; sout += "row step: " + pc.row_step.ToString() + " "; sout += "is dense: " + pc.is_dense.ToString() + " "; sout += "Field parameters->"; sout += "names: " + pc.fields[0].name + " " + pc.fields[1].name + " " + pc.fields[2].name + " "; sout += "offsets: " + pc.fields[0].offset + " " + pc.fields[1].offset + " " + pc.fields[2].offset + " "; sout += "datatypes: " + pc.fields[0].datatype + " " + pc.fields[1].datatype + " " + pc.fields[2].datatype + " "; sout += "counts: " + pc.fields[0].count + " " + pc.fields[1].count + " " + pc.fields[2].count + " "; sout += "data size: " + getRosDataSize(cloud) + " "; sout += "data: "; int length = getRosDataSize(cloud); for (int i = 0; i < length; i++) { sout += pc.data[i] + " "; } return(sout); }
void Update() { laser2.defineParameters(maxLeftAngle, maxRightAngle, maxTopAngle, maxBottomAngle, verticalIncrement, horizontalIncrement, maxDistance, 1 << 8, repetitions); if (run) { //this function is resposible for push the correct points to the pcl cloud if (laser2.runCasterSequentialRep(ref pcl, this, drawRay)) { //convert from pcl cloud to pcl cloud2 pcl.convertToCloud2(); //create a new mensage to be send SensorPointCloud2 pc = new SensorPointCloud2(); //from the Pcl cloud2 converted to Ros cloud2 using marshal for better performance pcl.convertToRosCloud(ref pc, frame_id); //publish the ros cloud mensage rosSocket.Publish(publication_id, pc); //restart pcl cloud pc = new SensorPointCloud2(); pcl.createPclCloud(0, 0, true); } } }
public string compareDataCloud2andCloudRos(SensorPointCloud2 pc) { string sout = ""; byte[] data = new byte[0]; oldGetData(ref data); int length = getRosDataSize(cloud); for (int i = 0; i < length; i++) { sout += "(" + pc.data[i] + "," + data[i] + ") "; } return(sout); /* * unsafe * { * byte* ptr = (byte*)getRosData(cloud); * int length = getRosDataSize(cloud); * byte[] rosByteArray = new byte[length]; * string sout = ""; * for (int i = 0; i < length; i++) * { * //rosByteArray[i] = ptr[i]; * sout += "(" + pc.data[i] + "," + pc.data[i] + ") "; * } * Debug.Log("compara(cloud2,Ros)" + sout); * } */ }
/// <summary> /// /// </summary> /// <param name="pc"></param> /// <param name="frame_id"></param> public void convertToRosCloud(ref SensorPointCloud2 pc, string frame_id) { //header should be already set pc.header.frame_id = frame_id; pc.height = getRosHeight(cloud); pc.width = getRosWidth(cloud); pc.is_bigendian = getRosBigendian(cloud); pc.point_step = getRosPointStep(cloud); pc.row_step = getRosRowStep(cloud); pc.is_dense = getRosDense(cloud); int size = getRosFieldSize(cloud); List <string> names = getFieldsStrings(); List <int> offsets = getFieldsIntegers(1); List <int> dataTypes = getFieldsIntegers(2); List <int> counts = getFieldsIntegers(3); //Debug.Log("***************************************"); //Debug.Log("antes"+pc.fields.Length); Array.Resize <SensorPointField>(ref pc.fields, 3); //Debug.LogWarning("depois "+pc.fields.Length); for (int i = 0; i < size; i++) { SensorPointField field = new SensorPointField(); field.datatype = dataTypes[i]; field.name = names[i]; field.offset = offsets[i]; field.count = counts[i]; pc.fields[i] = field; } //copyData cloud2RosCloud(ref pc); }
/// <summary> /// copy entire point cloud to part of the ros cloud msg /// </summary> /// <param name="rosCloud"></param> /// <param name="start"></param> /// <param name="size"></param> private void cloud2PartRosCloud(ref SensorPointCloud2 rosCloud, int start, int size) { unsafe { IntPtr ptr = getRosDataPtr(cloud); Marshal.Copy(ptr, rosCloud.data, start, size); } }
void Start() { rosSocket = GameObject.Find("drone").GetComponent <RosConnector>().RosSocket; droneBody = GameObject.Find("drone").GetComponent <Rigidbody>(); publication_id = rosSocket.Advertize(publishTopic, "sensor_msgs/PointCloud2"); laser = new rayCaster(maxLeftAngle, maxRightAngle, maxTopAngle, maxBottomAngle, verticalIncrement, horizontalIncrement, maxDistance, this, 1 << 8); pcl.createPclCloud(0, 0, true); pc = new SensorPointCloud2(); }
/// <summary> /// copy from cloud2 data to ros cloud data /// </summary> /// <param name="rosCloud"></param> private void cloud2RosCloud(ref SensorPointCloud2 rosCloud) { unsafe { //Array.Resize<byte>(ref rosCloud.data, getRosDataSize(cloud)); rosCloud.data = new byte[getRosDataSize(cloud)]; IntPtr ptr = getRosDataPtr(cloud); Marshal.Copy(ptr, rosCloud.data, 0, getRosDataSize(cloud)); } }
//WORK IN PROGRESS!!!!!!!! public bool casteRay(ref SensorPointCloud2 rosCloud, laserSensor laser, bool drawRay) { if (stackDone)//restart { currentAngle = topA; stackDone = false; interPcl = new pclInterface(); } return(false); }
public PointCloud(SensorPointCloud2 sensorPointCloud2) { int I = sensorPointCloud2.data.Length / sensorPointCloud2.point_step; Points = new Point[I]; byte[] byteSlice = new byte[sensorPointCloud2.point_step]; for (int i = 0; i < I; i++) { Array.Copy(sensorPointCloud2.data, i * sensorPointCloud2.point_step, byteSlice, 0, sensorPointCloud2.point_step); Points[i] = new Point(byteSlice); } }
private void oldCloud2RosCloud(ref SensorPointCloud2 pc) { unsafe { int length = getRosDataSize(cloud); byte *ptr = (byte *)getRosDataFast(cloud); for (int i = 0; i < length; i++) { pc.data[i] = ptr[i]; } } }
// Use this for initialization void Start() { pcl.createPclCloud(0, 0, true); pcl.pushPointToCloud(new Vector3(1, 2, 3)); pcl.convertToCloud2(); SensorPointCloud2 pc = new SensorPointCloud2(); pcl.convertToRosCloud(ref pc, "ok"); Debug.Log("data: " + pc.data[0] + " " + pc.data[1] + " " + pc.data[2] + " " + pc.data[3] + " " + pc.data[4]); //byte[] data= new byte[0]; //pcl.testByteArray222(ref data); //Debug.Log("data: "+data[0]+" "+data[1] + " " + data[2] + " " + data[3] + " " + data[4]); }
/// <summary> /// work in progress /// </summary> /// <param name="pcl"></param> /// <param name="laser"></param> /// <param name="drawRay"></param> /// <param name="rosCloud"></param> /// <returns></returns> public bool runCasterSequentialRep2(ref SensorPointCloud2 rosCloud, laserSensor laser, bool drawRay) { if (stackDone)//restart { currentAngle = topA; stackDone = false; } pclInterface pcl = new pclInterface(); pcl.createPclCloud(0, 0, true); int cicles = 0; while (cicles < repetitions) { if (currentAngle >= bottomA) { float currentHoriAngle = leftA; while (currentHoriAngle < rightA) { getPointFromRay(currentHoriAngle, currentAngle, maxDist, ref pcl, laser, drawRay); currentHoriAngle += horiInc; } currentAngle -= horiInc; } else { stackDone = true; if (rosCloud.data.Length > 0) { return(true); } } cicles++; } return(false); }
private void getPointFromRay(float yAngle, float zAngle, float maxDist, ref pclInterface pcl, laserSensor laserObject, bool drawRay, ref SensorPointCloud2 rosCloud) { Vector3 direction = laserObject.transform.right; direction = Quaternion.AngleAxis(zAngle, laserObject.transform.forward) * direction; direction = Quaternion.AngleAxis(yAngle - 90, laserObject.transform.up) * direction; direction.Normalize(); RaycastHit hit; if (Physics.Raycast(laserObject.transform.position, direction, out hit, maxDist, rayLayer)) { pcl.pushPointToCloud(hit.point); } if (drawRay) { Debug.DrawRay(laserObject.transform.position, direction * maxDist, new Color(254, 254, 254, 0.5f)); } }