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