/// <summary>
///
/// </summary>
/// <param name="pcl"></param>
/// <param name="laser"></param>
/// <param name="drawRay">to draw raycasts send this as true</param>
/// <returns>true if there are a complete cloud in pcl otherwise return false</returns>
public bool runCasterSequential(ref pclInterface pcl, laserSensor laser, bool drawRay)
{
if (stackDone)//restart
{
currentAngle = topA;
stackDone = false;
}
if (currentAngle >= bottomA)
{
float currentHoriAngle = leftA;
while (currentHoriAngle < rightA)
{
getPointFromRay(currentHoriAngle, currentAngle, maxDist, ref pcl, laser, drawRay);
currentHoriAngle += horiInc;
}
currentAngle -= horiInc;
}
else
{
//Debug.Log("done " + pcl.cloudHasPoints());
stackDone = true;
if (pcl.cloudHasPoints())
{
return(true);
}
}
return(false);
}
//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);
}
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));
}
}
/// <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 pclSequentialMode(pclInterface pcl)
{
if (stackDone)//restart
{
currentVertAngle = topAngle;
stackDone = false;
}
if (currentVertAngle >= bottomAngle)
{
float currentHoriAngle = leftAngle;
while (currentHoriAngle < rightAngle)
{
getPointFromRay(currentHoriAngle, currentVertAngle, maxDist, pcl);
currentHoriAngle += horiIncrement;
}
currentVertAngle -= horiIncrement;
}
else
{
stackDone = true;
Debug.Log("stackDone");
}
}
/// <summary>
/// when pcl cloud completed return true
/// </summary>
/// <param name="pcl"></param>
/// <param name="laser"></param>
/// <param name="drawRay"></param>
/// <returns></returns>
public bool runCasterSimultaneos(ref pclInterface pcl, laserSensor laser, bool drawRay)
{
float currentVertAngle = topA;
//vertical laser movement
while (currentVertAngle >= bottomA)
{
float currentHoriAngle = leftA;
//horizontal lase movement
while (currentHoriAngle < rightA)
{
getPointFromRay(currentHoriAngle, currentVertAngle, maxDist, ref pcl, laser, drawRay);
currentHoriAngle += horiInc;
}
currentVertAngle -= vertInc;
}
if (pcl.cloudHasPoints())
{
//Debug.Log("Going to be converted");
return(true);
}
return(false);
}
public void runRayCaster(pclInterface pcl)
{
pclSequentialMode(pcl);
}
