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