public void SetScan(float time, LaserScan msg, GameObject _goParent, Transform _TF)
{
goParent = _goParent;
TF = _TF;
recycleCount++;
angMin = msg.angle_min;
angInc = msg.angle_increment;
maxRange = msg.range_max;
minRange = msg.range_min;
lastUpdate = time;
if (distBuffer == null || distBuffer.Length != msg.ranges.Length)
{
distBuffer = new float[msg.ranges.Length];
}
Array.Copy(msg.ranges, distBuffer, distBuffer.Length);
changed = true;
//deactivate old points
if (pointBuffer != null)
{
foreach (GameObject point in pointBuffer)
{
point.SetActive(false);
}
}
//turn GO on
gameObject.SetActive(true);
}
public void Render(LaserScan lidarData, Transform origin)
{
if (_meshHolder == null)
{
Init(origin);
}
if (lidarData.Ranges.Count != _logicalVertsCount)
{
Debug.LogWarning("Renderer is configured to handle different resolution of lidar data than it is being passed.");
}
for (int vInd = 0; vInd < _verts.Length; vInd += 2)
{
// vInd = index for column in the ladder;
// vInd+1 = second ring/top of column which
// should vary from vInd only by y displacement
float rad = (((float)(vInd / 2) / (float)lidarData.Ranges.Count) * (lidarData.Angle_min + vInd * lidarData.Angle_increment)) - (Mathf.PI / 2);
Vector3 offset = new Vector3(Mathf.Cos(rad), 0f, Mathf.Sin(rad)) * lidarData.Ranges[vInd / 2];
_verts[vInd] = offset;
_verts[vInd + 1] = offset + Vector3.up * _owner.ringHeight;
}
_mesh.RecalculateBounds();
_mesh.vertices = _verts;
}
public override void Render(LaserScan scan, Transform origin)
{
// Move the balls
base.Render(scan, origin);
if (_material != null)
{
for (int i = 0; i < _ballCache.Length; i++)
{
LineRenderer line = null;
if (_ballCache[i].TryGetComponent <LineRenderer>(out line) == false)
{
line = _ballCache[i].AddComponent <LineRenderer>() as LineRenderer;
line.material = _material;
line.endColor = new Color(0f, 0f, 0f, 0f);
line.startColor = new Color(.5f, 0f, 0f, .5f);
line.startWidth = 0.01f;
line.endWidth = 0.01f;
}
if (line != null)
{
// wake up/activate the object if it wasn't used last frame
line.enabled = _ballCache[i].activeSelf;
line.SetPosition(0, _ballCache[i].transform.position);
line.SetPosition(1, origin.position);
}
}
}
}
public ROS2LidarSubscription()
{
_r2l = ROS2Listener.instance;
_sub = _r2l.node.CreateSubscription <LaserScan>(
"scan", msg => {
_curScan = msg;
}, ROS2.Utils.QosProfile.Profile.SensorData);
}
/// <summary>
/// Queries the lidar and renders the updated information
/// </summary>
public void RegenerateSite()
{
LaserScan scan = _provider.Query();
if (scan != null)
{
_renderer.Render(scan, transform);
}
}
public void Config(LidarVisualizer viz)
{
_owner = viz;
_r2l = ROS2Listener.instance;
_sub = _r2l.node.CreateSubscription <LaserScan>(
_owner.topic, msg => {
_curScan = msg;
}, ROS2.Utils.QosProfile.Profile.SensorData);
}
static void Main(string[] args)
{
ydlidar.os_init();
CYdLidar lidarClass = new CYdLidar();
string port = "COM7";
int optname = (int)LidarProperty.LidarPropSerialPort;
lidarClass.setlidaropt(optname, port);
optname = (int)LidarProperty.LidarPropSerialBaudrate;
lidarClass.setlidaropt(optname, 512000);
optname = (int)LidarProperty.LidarPropLidarType;
int lidarType = (int)LidarTypeID.TYPE_TOF;
lidarClass.setlidaropt(optname, lidarType);
bool ret = lidarClass.initialize();
if (ret)
{
ret = lidarClass.turnOn();
}
else
{
Console.WriteLine("error:" + lidarClass.DescribeError());
}
LaserScan scan = new LaserScan();
while (ret && ydlidar.os_isOk())
{
if (lidarClass.doProcessSimple(scan))
{
Console.WriteLine("stamp: " + scan.stamp + ", size: " + scan.points.Count);
}
}
lidarClass.turnOff();
lidarClass.disconnecting();
lidarClass.Dispose();
}
public virtual void Render(LaserScan lidarData, Transform origin)
{
if (_ballPrefab == null)
{
return;
}
if (_ballCache == null)
{
_ballCacheSize = lidarData.Ranges.Count;
_ballCache = new GameObject[_ballCacheSize];
}
ResizeCache(lidarData.Ranges.Count);
for (int i = 0; i < _ballCacheSize; i++)
{
if (_ballCache[i] == null)
{
GameObject ball = GameObject.Instantiate(_ballPrefab, origin);
_ballCache[i] = ball;
}
if ((lidarData.Ranges[i] > lidarData.Range_max) || (lidarData.Ranges[i] < lidarData.Range_min))
{
// Don't show data out of range
_ballCache[i].SetActive(false);
}
else
{
float rad = lidarData.Angle_min + i * lidarData.Angle_increment;
Vector3 offset = new Vector3(Mathf.Cos(rad), 0f, Mathf.Sin(rad)) * lidarData.Ranges[i];
// wake up/activate the object if it wasn't used last frame
_ballCache[i].SetActive(true);
_ballCache[i].transform.localPosition = offset;
}
}
}
public LaserScan Query()
{
LaserScan scan = new LaserScan();
scan.Angle_min = -Mathf.PI;
scan.Angle_max = Mathf.PI; // TODO: Make this configurable
scan.Angle_increment = (float)(scan.Angle_max - scan.Angle_min) / _owner.lidarResolution;
scan.Scan_time = _owner.renderCallsPerSecond;
scan.Time_increment = _owner.renderCallsPerSecond;
scan.Range_min = _owner.randomRange.x;
scan.Range_max = _owner.randomRange.y;
scan.Ranges = _reserved;
if (!_owner.spiral)
{
// Replace with random data;
for (int i = 0; i < _owner.lidarResolution; i++)
{
scan.Ranges[i] = Random.Range(_owner.randomRange.x, _owner.randomRange.y);
}
}
return(scan);
}
