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