// 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]);
}
private void getPointFromRay(float yAngle, float zAngle, float maxDist, pclInterface pcl)
{
Vector3 direction = laserObject.transform.right;
direction = Quaternion.AngleAxis(zAngle, laserObject.transform.forward) * direction;
direction = Quaternion.AngleAxis(yAngle - 90, laserObject.transform.up) * direction;
direction.Normalize();
//Debug.Log("zangle: " + zAngle.ToString()+" yangle: "+yAngle.ToString());
RaycastHit hit;
if (Physics.Raycast(laserObject.transform.position, direction, out hit, maxDist, rayLayer))
{
pcl.pushPointToCloud(hit.point);
}
// Debug.DrawRay(laserObject.transform.position, direction * maxDist, new Color(254, 254, 254, 0.5f));
}
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));
}
}
