void SendPointCloud(List <VelodynePointCloudVertex> pointCloud) { if (Bridge == null || Bridge.Status != Ros.Status.Connected) { return; } var pointCount = pointCloud.Count; byte[] byteData = new byte[32 * pointCount]; for (int i = 0; i < pointCount; i++) { var local = sensorLocalspaceTransform.InverseTransformPoint(pointCloud[i].position); if (targetEnv == ROSTargetEnvironment.AUTOWARE) { local.Set(local.z, -local.x, local.y); } else { local.Set(local.x, local.z, local.y); } var scaledPos = local * exportScaleFactor; var x = System.BitConverter.GetBytes(scaledPos.x); var y = System.BitConverter.GetBytes(scaledPos.y); var z = System.BitConverter.GetBytes(scaledPos.z); //var intensity = System.BitConverter.GetBytes(pointCloud[i].color.maxColorComponent); //var intensity = System.BitConverter.GetBytes((float)(((int)pointCloud[i].color.r) << 16 | ((int)pointCloud[i].color.g) << 8 | ((int)pointCloud[i].color.b))); //var intensity = System.BitConverter.GetBytes((byte)pointCloud[i].distance); var intensity = System.BitConverter.GetBytes((byte)(pointCloud[i].color.grayscale * 255)); var ring = System.BitConverter.GetBytes(pointCloud[i].ringNumber); var ts = System.BitConverter.GetBytes((double)0.0); System.Buffer.BlockCopy(x, 0, byteData, i * 32 + 0, 4); System.Buffer.BlockCopy(y, 0, byteData, i * 32 + 4, 4); System.Buffer.BlockCopy(z, 0, byteData, i * 32 + 8, 4); System.Buffer.BlockCopy(intensity, 0, byteData, i * 32 + 16, 1); System.Buffer.BlockCopy(ts, 0, byteData, i * 32 + 24, 8); } var msg = new Ros.PointCloud2() { header = new Ros.Header() { stamp = Ros.Time.Now(), seq = seqId++, frame_id = "velodyne", // needed for Autoware }, height = 1, width = (uint)pointCount, fields = new Ros.PointField[] { new Ros.PointField() { name = "x", offset = 0, datatype = 7, count = 1, }, new Ros.PointField() { name = "y", offset = 4, datatype = 7, count = 1, }, new Ros.PointField() { name = "z", offset = 8, datatype = 7, count = 1, }, new Ros.PointField() { name = "intensity", offset = 16, datatype = 2, count = 1, }, new Ros.PointField() { name = "timestamp", offset = 24, datatype = 8, count = 1, }, }, is_bigendian = false, point_step = 32, row_step = (uint)pointCount * 32, data = byteData, is_dense = true, }; if (targetEnv == ROSTargetEnvironment.AUTOWARE) { Bridge.Publish(topicName, msg); } if (targetEnv == ROSTargetEnvironment.APOLLO) { Bridge.Publish(ApolloTopicName, msg); } }
void SendMessage() { if (Bridge == null || Bridge.Status != Ros.Status.Connected) { return; } #if UNITY_EDITOR UnityEngine.Profiling.Profiler.BeginSample("SendMessage"); #endif var worldToLocal = Matrix4x4.TRS(transform.position, transform.rotation, Vector3.one).inverse; if (TargetEnvironment == ROSTargetEnvironment.APOLLO) { // local.Set(local.x, local.z, local.y); worldToLocal = new Matrix4x4(new Vector4(1, 0, 0, 0), new Vector4(0, 0, 1, 0), new Vector4(0, 1, 0, 0), Vector4.zero) * worldToLocal; } else if (TargetEnvironment == ROSTargetEnvironment.AUTOWARE) { // local.Set(local.z, -local.x, local.y); worldToLocal = new Matrix4x4(new Vector4(0, -1, 0, 0), new Vector4(0, 0, 1, 0), new Vector4(1, 0, 0, 0), Vector4.zero) * worldToLocal; } int count = 0; unsafe { fixed(byte *ptr = RosPointCloud) { int offset = 0; for (int i = 0; i < PointCloud.Length; i++) { var point = PointCloud[i]; if (point == Vector4.zero) { continue; } var worldPos = new Vector3(point.x, point.y, point.z); float intensity = point.w; *(Vector3 *)(ptr + offset) = worldToLocal.MultiplyPoint3x4(worldPos); *(ptr + offset + 16) = (byte)(intensity * 255); offset += 32; count++; } } } var msg = new Ros.PointCloud2() { header = new Ros.Header() { stamp = Ros.Time.Now(), seq = Sequence++, frame_id = "velodyne", // needed for Autoware }, height = 1, width = (uint)count, fields = new Ros.PointField[] { new Ros.PointField() { name = "x", offset = 0, datatype = 7, count = 1, }, new Ros.PointField() { name = "y", offset = 4, datatype = 7, count = 1, }, new Ros.PointField() { name = "z", offset = 8, datatype = 7, count = 1, }, new Ros.PointField() { name = "intensity", offset = 16, datatype = 2, count = 1, }, new Ros.PointField() { name = "timestamp", offset = 24, datatype = 8, count = 1, }, }, is_bigendian = false, point_step = 32, row_step = (uint)count * 32, data = new Ros.PartialByteArray() { Base64 = System.Convert.ToBase64String(RosPointCloud, 0, count * 32), }, is_dense = true, }; #if UNITY_EDITOR UnityEngine.Profiling.Profiler.BeginSample("Publish"); #endif if (TargetEnvironment == ROSTargetEnvironment.APOLLO) { Bridge.Publish(ApolloTopicName, msg); } else if (TargetEnvironment == ROSTargetEnvironment.AUTOWARE) { Bridge.Publish(AutowareTopicName, msg); } else { Bridge.Publish(TopicName, msg); } #if UNITY_EDITOR UnityEngine.Profiling.Profiler.EndSample(); #endif #if UNITY_EDITOR UnityEngine.Profiling.Profiler.EndSample(); #endif }
void SendMessage(int currentEndIndex, float currentEndAngle) { if (Bridge == null || Bridge.Status != Comm.BridgeStatus.Connected) { return; } #if UNITY_EDITOR UnityEngine.Profiling.Profiler.BeginSample("SendMessage"); #endif // Lidar x is forward, y is left, z is up var worldToLocal = new Matrix4x4(new Vector4(0, -1, 0, 0), new Vector4(0, 0, 1, 0), new Vector4(1, 0, 0, 0), Vector4.zero); if (Compensated) { worldToLocal = worldToLocal * transform.worldToLocalMatrix; } if (CurrentRayCount != 1) { System.DateTime Unixepoch = new System.DateTime(1970, 1, 1, 0, 0, 0, System.DateTimeKind.Utc); double measurement_time = (double)(System.DateTime.UtcNow - Unixepoch).TotalSeconds; if (TargetEnvironment == ROSTargetEnvironment.APOLLO35) { var msg = new Apollo.Drivers.PointCloud() { Header = new Apollo.Common.Header() { // TimestampSec = measurement_time, SequenceNum = Sequence++, FrameId = FrameName, }, FrameId = "lidar128", IsDense = false, MeasurementTime = measurement_time, Height = 1, Width = (uint)PointCloud.Length, // TODO is this right? }; for (int i = 0; i < PointCloud.Length; i++) { var point = PointCloud[i]; if (point == Vector4.zero) { continue; } var pos = new Vector3(point.x, point.y, point.z); float intensity = point.w; pos = worldToLocal.MultiplyPoint3x4(pos); msg.Point.Add(new Apollo.Drivers.PointXYZIT() { X = pos.x, Y = pos.y, Z = pos.z, Intensity = (byte)(intensity * 255), Timestamp = (ulong)measurement_time }); } ; Apollo35WriterPointCloud2.Publish(msg); } else { int count = 0; unsafe { fixed(byte *ptr = RosPointCloud) { int offset = 0; for (int i = 0; i < PointCloud.Length; i++) { var point = PointCloud[i]; if (point == Vector4.zero) { continue; } var pos = new Vector3(point.x, point.y, point.z); float intensity = point.w; *(Vector3 *)(ptr + offset) = worldToLocal.MultiplyPoint3x4(pos); *(ptr + offset + 16) = (byte)(intensity * 255); offset += 32; count++; } } } var msg = new Ros.PointCloud2() { header = new Ros.Header() { stamp = Ros.Time.Now(), seq = Sequence++, frame_id = FrameName, }, height = 1, width = (uint)count, fields = new Ros.PointField[] { new Ros.PointField() { name = "x", offset = 0, datatype = 7, count = 1, }, new Ros.PointField() { name = "y", offset = 4, datatype = 7, count = 1, }, new Ros.PointField() { name = "z", offset = 8, datatype = 7, count = 1, }, new Ros.PointField() { name = "intensity", offset = 16, datatype = 2, count = 1, }, new Ros.PointField() { name = "timestamp", offset = 24, datatype = 8, count = 1, }, }, is_bigendian = false, point_step = 32, row_step = (uint)count * 32, data = new Ros.PartialByteArray() { Base64 = System.Convert.ToBase64String(RosPointCloud, 0, count * 32), }, is_dense = true, }; if (TargetEnvironment == ROSTargetEnvironment.APOLLO) { ApolloWriterPointCloud2.Publish(msg); } else if (TargetEnvironment == ROSTargetEnvironment.AUTOWARE) { AutowareWriterPointCloud2.Publish(msg); } else { WriterPointCloud2.Publish(msg); } } } else // for planar lidar only (single ray lidar) publish LaserScan instead of PoinCloud2 { float fixup = currentEndAngle; FixupAngle = currentEndAngle; float LaserScanMinAngle = -Mathf.PI; float LaserScanMaxAngle = Mathf.PI; float LaserScanMinRange = 0.0f; float LaserScanMaxRange = 40.0f; float[] range_array = new float[CurrentMeasurementsPerRotation]; float[] intensity_array = new float[CurrentMeasurementsPerRotation]; int offset = (int)(currentEndIndex - currentEndAngle / 360.0f * CurrentMeasurementsPerRotation); for (int i = 0; i < PointCloud.Length; i++) { var point = PointCloud[i]; if (point == Vector4.zero) { continue; } range_array[(i + offset + CurrentMeasurementsPerRotation) % CurrentMeasurementsPerRotation] = Vector3.Distance(new Vector3(point.x, point.y, point.z), transform.position); intensity_array[(i + offset + CurrentMeasurementsPerRotation) % CurrentMeasurementsPerRotation] = point.w; } // Populate LaserScan message var msg = new Ros.LaserScan() { header = new Ros.Header() { stamp = Ros.Time.Now(), seq = Sequence++, frame_id = FrameName, }, angle_min = LaserScanMinAngle, angle_max = LaserScanMaxAngle, angle_increment = Mathf.Abs(LaserScanMaxAngle - LaserScanMinAngle) / CurrentMeasurementsPerRotation, time_increment = 1.0f / (CurrentMeasurementsPerRotation * RotationFrequency), scan_time = 1.0f / RotationFrequency, range_min = LaserScanMinRange, range_max = LaserScanMaxRange, ranges = range_array, // TODO: Should make sure only sending range between min and max intensities = intensity_array, }; WriterLaserScan.Publish(msg); } #if UNITY_EDITOR UnityEngine.Profiling.Profiler.EndSample(); #endif }
void SendMessage(int currentEndIndex, float currentEndAngle) { if (Bridge == null || Bridge.Status != Comm.BridgeStatus.Connected) { return; } // Lidar x is forward, y is left, z is up var worldToLocal = new Matrix4x4(new Vector4(0, -1, 0, 0), new Vector4(0, 0, 1, 0), new Vector4(1, 0, 0, 0), Vector4.zero); worldToLocal = worldToLocal * transform.worldToLocalMatrix; int count = 0; unsafe { fixed(byte *ptr = RosPointCloud) { int offset = 0; for (int i = 0; i < PointCloud.Length; i++) { var point = PointCloud[i]; if (point == Vector4.zero) { continue; } var pos = new Vector3(point.x, point.y, point.z); float intensity = point.w; *(Vector3 *)(ptr + offset) = worldToLocal.MultiplyPoint3x4(pos); *(ptr + offset + 16) = (byte)(intensity * 255); offset += 32; count++; //if (i == 0) { Debug.Log("<color=blue>:</color>"); } } } } var msg = new Ros.PointCloud2() { header = new Ros.Header() { stamp = Ros.Time.Now(), seq = Sequence++, frame_id = FrameName, }, height = 1, width = (uint)count, fields = new Ros.PointField[] { new Ros.PointField() { name = "x", offset = 0, datatype = 7, count = 1, }, new Ros.PointField() { name = "y", offset = 4, datatype = 7, count = 1, }, new Ros.PointField() { name = "z", offset = 8, datatype = 7, count = 1, }, new Ros.PointField() { name = "intensity", offset = 16, datatype = 2, count = 1, }, new Ros.PointField() { name = "timestamp", offset = 24, datatype = 8, count = 1, }, }, is_bigendian = false, point_step = 32, row_step = (uint)count * 32, data = new Ros.PartialByteArray() { Base64 = System.Convert.ToBase64String(RosPointCloud, 0, count * 32), }, is_dense = true, }; WriterPointCloud2.Publish(msg); }
void SendMessage(int currentEndIndex, float currentEndAngle) { if (Bridge == null || Bridge.Status != Ros.Status.Connected) { return; } #if UNITY_EDITOR UnityEngine.Profiling.Profiler.BeginSample("SendMessage"); #endif Matrix4x4 worldToLocal; if (TargetEnvironment == ROSTargetEnvironment.APOLLO) { // local.Set(local.x, local.z, local.y); worldToLocal = new Matrix4x4(new Vector4(1, 0, 0, 0), new Vector4(0, 0, 1, 0), new Vector4(0, 1, 0, 0), Vector4.zero); } else if (TargetEnvironment == ROSTargetEnvironment.AUTOWARE) { // local.Set(local.z, -local.x, local.y); worldToLocal = new Matrix4x4(new Vector4(0, -1, 0, 0), new Vector4(0, 0, 1, 0), new Vector4(1, 0, 0, 0), Vector4.zero); } else { worldToLocal = Matrix4x4.identity; } if (Compensated) { worldToLocal = worldToLocal * transform.worldToLocalMatrix; } if (CurrentRayCount != 1) { int count = 0; unsafe { fixed(byte *ptr = RosPointCloud) { int offset = 0; for (int i = 0; i < PointCloud.Length; i++) { var point = PointCloud[i]; if (point == Vector4.zero) { continue; } var pos = new Vector3(point.x, point.y, point.z); float intensity = point.w; *(Vector3 *)(ptr + offset) = worldToLocal.MultiplyPoint3x4(pos); *(ptr + offset + 16) = (byte)(intensity * 255); offset += 32; count++; } } } var msg = new Ros.PointCloud2() { header = new Ros.Header() { stamp = Ros.Time.Now(), seq = Sequence++, frame_id = FrameName, }, height = 1, width = (uint)count, fields = new Ros.PointField[] { new Ros.PointField() { name = "x", offset = 0, datatype = 7, count = 1, }, new Ros.PointField() { name = "y", offset = 4, datatype = 7, count = 1, }, new Ros.PointField() { name = "z", offset = 8, datatype = 7, count = 1, }, new Ros.PointField() { name = "intensity", offset = 16, datatype = 2, count = 1, }, new Ros.PointField() { name = "timestamp", offset = 24, datatype = 8, count = 1, }, }, is_bigendian = false, point_step = 32, row_step = (uint)count * 32, data = new Ros.PartialByteArray() { Base64 = System.Convert.ToBase64String(RosPointCloud, 0, count * 32), }, is_dense = true, }; #if UNITY_EDITOR UnityEngine.Profiling.Profiler.BeginSample("Publish"); #endif if (TargetEnvironment == ROSTargetEnvironment.APOLLO) { Bridge.Publish(ApolloTopicName, msg); } else if (TargetEnvironment == ROSTargetEnvironment.AUTOWARE) { Bridge.Publish(AutowareTopicName, msg); } else { Bridge.Publish(TopicName, msg); } #if UNITY_EDITOR UnityEngine.Profiling.Profiler.EndSample(); #endif } else // for planar lidar only (single ray lidar) publish LaserScan instead of PoinCloud2 { float fixup = currentEndAngle; FixupAngle = currentEndAngle; float LaserScanMinAngle = -Mathf.PI; float LaserScanMaxAngle = Mathf.PI; float LaserScanMinRange = 0.0f; float LaserScanMaxRange = 40.0f; float[] range_array = new float[CurrentMeasurementsPerRotation]; float[] intensity_array = new float[CurrentMeasurementsPerRotation]; int offset = (int)(currentEndIndex - currentEndAngle / 360.0f * CurrentMeasurementsPerRotation); for (int i = 0; i < PointCloud.Length; i++) { var point = PointCloud[i]; if (point == Vector4.zero) { continue; } range_array[(i + offset + CurrentMeasurementsPerRotation) % CurrentMeasurementsPerRotation] = Vector3.Distance(new Vector3(point.x, point.y, point.z), transform.position); intensity_array[(i + offset + CurrentMeasurementsPerRotation) % CurrentMeasurementsPerRotation] = point.w; } // Populate LaserScan message var msg = new Ros.LaserScan() { header = new Ros.Header() { stamp = Ros.Time.Now(), seq = Sequence++, frame_id = FrameName, }, angle_min = LaserScanMinAngle, angle_max = LaserScanMaxAngle, angle_increment = Mathf.Abs(LaserScanMaxAngle - LaserScanMinAngle) / CurrentMeasurementsPerRotation, time_increment = 1.0f / (CurrentMeasurementsPerRotation * RotationFrequency), scan_time = 1.0f / RotationFrequency, range_min = LaserScanMinRange, range_max = LaserScanMaxRange, ranges = range_array, // TODO: Should make sure only sending range between min and max intensities = intensity_array, }; Bridge.Publish(TopicName, msg); } #if UNITY_EDITOR UnityEngine.Profiling.Profiler.EndSample(); #endif }