├── LICENSE ├── LICENSE.meta ├── README.md ├── README.md.meta ├── Scripts.meta ├── Scripts ├── Runtime.meta └── Runtime │ ├── Clock.meta │ ├── Clock │ ├── ClockPublisher.cs │ └── ClockPublisher.cs.meta │ ├── DCamera.meta │ ├── DCamera │ ├── DCameraPublisher.cs │ └── DCameraPublisher.cs.meta │ ├── GPS.meta │ ├── GPS │ ├── GPSPublisher.cs │ └── GPSPublisher.cs.meta │ ├── IMU.meta │ ├── IMU │ ├── IMUPublisher.cs │ └── IMUPublisher.cs.meta │ ├── LaserScan.meta │ ├── LaserScan │ ├── LaserScanPublisher.cs │ └── LaserScanPublisher.cs.meta │ ├── Livox.meta │ ├── Livox │ ├── LivoxPublisher.cs │ └── LivoxPublisher.cs.meta │ ├── Messages.meta │ ├── Messages │ ├── LivoxRosDriver.meta │ ├── LivoxRosDriver │ │ ├── msg.meta │ │ └── msg │ │ │ ├── CustomMsgMsg.cs │ │ │ ├── CustomMsgMsg.cs.meta │ │ │ ├── CustomPointMsg.cs │ │ │ └── CustomPointMsg.cs.meta │ ├── Nmea.meta │ ├── Nmea │ │ ├── msg.meta │ │ └── msg │ │ │ ├── SentenceMsg.cs │ │ │ └── SentenceMsg.cs.meta │ ├── Rosgraph.meta │ ├── Velodyne.meta │ └── Velodyne │ │ ├── msg.meta │ │ └── msg │ │ ├── VelodynePacketMsg.cs │ │ ├── VelodynePacketMsg.cs.meta │ │ ├── VelodyneScanMsg.cs │ │ └── VelodyneScanMsg.cs.meta │ ├── RGBCamera.meta │ ├── RGBCamera │ ├── RGBCameraPublisher.cs │ └── RGBCameraPublisher.cs.meta │ ├── Velodyne.meta │ └── Velodyne │ ├── VLP16Publisher.cs │ └── VLP16Publisher.cs.meta ├── package.json └── package.json.meta /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /LICENSE.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 3dab90dd8fc551bed81e5be5d3f3f86b 3 | DefaultImporter: 4 | externalObjects: {} 5 | userData: 6 | assetBundleName: 7 | assetBundleVariant: 8 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Unity Sensors ROS 2 | Sensor tools for Robotics connecting with ROS and ROS2 environment. 3 | 4 | # Environment 5 | ### Unity Version 6 | 2020.2 or later 7 | ### Depend Packages 8 | - Burst : >=1.4.8 9 | - Mathematics : >=1.2.1 10 | - ROS TCP Connector : >=v0.5.0 11 | - UnitySensors : >=0.1.0 12 | 13 | # Installation 14 | ## Install dependencies 15 | 1. Open the package manager from `Window` -> `Package Manager` and select "Add package from git URL..." 16 | 2. Enter the following each URLs, respectively. 17 | - com.unity.burst 18 | - com.unity.mathematics 19 | - https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.ros-tcp-connector 20 | - https://github.com/Field-Robotics-Japan/UnitySensors.git 21 | 3. Click `Install` buton on the right bottom corner for each depend packages, respectively. 22 | 23 | ## Install UnitySensors package 24 | 1. Open the package manager from `Window` -> `Package Manager` and select "Add package from git URL..." 25 | 2. Enter the following URL. If you don't want to use the latest version, substitute your desired version tag where we've put `v0.1.0` in this example: 26 | `https://github.com/Field-Robotics-Japan/UnitySensorsROS.git#v0.1.0` 27 | 3. Click `Add`. 28 | -------------------------------------------------------------------------------- /README.md.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 335d2ce7a2e93f2da8e7973d4321462b 3 | TextScriptImporter: 4 | externalObjects: {} 5 | userData: 6 | assetBundleName: 7 | assetBundleVariant: 8 | -------------------------------------------------------------------------------- /Scripts.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 602276adea5fb249f844ebc8e9219de1 3 | folderAsset: yes 4 | DefaultImporter: 5 | externalObjects: {} 6 | userData: 7 | assetBundleName: 8 | assetBundleVariant: 9 | -------------------------------------------------------------------------------- /Scripts/Runtime.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 3139e515c7f485e6bbeb8210f03b7fbe 3 | folderAsset: yes 4 | DefaultImporter: 5 | externalObjects: {} 6 | userData: 7 | assetBundleName: 8 | assetBundleVariant: 9 | -------------------------------------------------------------------------------- /Scripts/Runtime/Clock.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 16cf61dcd3b284ea8bb55529f426bb5e 3 | folderAsset: yes 4 | DefaultImporter: 5 | externalObjects: {} 6 | userData: 7 | assetBundleName: 8 | assetBundleVariant: 9 | -------------------------------------------------------------------------------- /Scripts/Runtime/Clock/ClockPublisher.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Collections; 3 | using System.Collections.Generic; 4 | using UnityEngine; 5 | 6 | using Unity.Robotics.ROSTCPConnector; 7 | using RosMessageTypes.Rosgraph; 8 | 9 | public class ClockPublisher : MonoBehaviour 10 | { 11 | 12 | [SerializeField] private string _topicName = "clock"; 13 | 14 | private float _timeStamp = 0f; 15 | 16 | private ROSConnection _ros; 17 | private ClockMsg _message; 18 | 19 | void Start() 20 | { 21 | // setup ROS 22 | this._ros = ROSConnection.instance; 23 | this._ros.RegisterPublisher(this._topicName); 24 | 25 | // setup ROS Message 26 | this._message = new ClockMsg(); 27 | this._message.clock.sec = 0; 28 | this._message.clock.nanosec = 0; 29 | } 30 | 31 | void Update() 32 | { 33 | this._timeStamp = Time.time; 34 | # if ROS2 35 | int sec = (int)Math.Truncate(this._timeStamp); 36 | # else 37 | uint sec = (uint)Math.Truncate(this._timeStamp); 38 | # endif 39 | uint nanosec = (uint)( (this._timeStamp - sec)*1e+9 ); 40 | this._message.clock.sec = sec; 41 | this._message.clock.nanosec = nanosec; 42 | this._ros.Send(this._topicName, this._message); 43 | } 44 | } 45 | -------------------------------------------------------------------------------- /Scripts/Runtime/Clock/ClockPublisher.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: d6f743f957ebac07f83aa7845191de67 3 | MonoImporter: 4 | externalObjects: {} 5 | serializedVersion: 2 6 | defaultReferences: [] 7 | executionOrder: 0 8 | icon: {instanceID: 0} 9 | userData: 10 | assetBundleName: 11 | assetBundleVariant: 12 | -------------------------------------------------------------------------------- /Scripts/Runtime/DCamera.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: c7ac7a641979727fb845c96cac3bbc1c 3 | folderAsset: yes 4 | DefaultImporter: 5 | externalObjects: {} 6 | userData: 7 | assetBundleName: 8 | assetBundleVariant: 9 | -------------------------------------------------------------------------------- /Scripts/Runtime/DCamera/DCameraPublisher.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Collections; 3 | using System.Collections.Generic; 4 | using UnityEngine; 5 | 6 | using Unity.Robotics.ROSTCPConnector; 7 | using RosMessageTypes.Sensor; 8 | 9 | namespace FRJ.Sensor 10 | { 11 | [RequireComponent(typeof(DCamera))] 12 | public class DCameraPublisher : MonoBehaviour 13 | { 14 | [SerializeField] 15 | private string _topic; 16 | [SerializeField] 17 | private string _frameId; 18 | 19 | private DCamera _dcam; 20 | 21 | private ROSConnection _ros; 22 | private PointCloud2Msg _message; 23 | 24 | private float _timeElapsed = 0f; 25 | private float _timeStamp = 0f; 26 | 27 | private void Awake() 28 | { 29 | _dcam = GetComponent(); 30 | } 31 | 32 | private void Start() 33 | { 34 | _ros = ROSConnection.GetOrCreateInstance(); 35 | _ros.RegisterPublisher(_topic); 36 | 37 | _message = new PointCloud2Msg(); 38 | _message.header.frame_id = _frameId; 39 | _message.height = 1; 40 | _message.width = (uint)(_dcam.resolution.x*_dcam.resolution.y); 41 | _message.fields = new PointFieldMsg[4]; 42 | for(int i = 0; i < 4; i++) _message.fields[i] = new PointFieldMsg(); 43 | _message.fields[0].name = "x"; 44 | _message.fields[0].offset = 0; 45 | _message.fields[0].datatype = 7; 46 | _message.fields[0].count = 1; 47 | _message.fields[1].name = "y"; 48 | _message.fields[1].offset = 4; 49 | _message.fields[1].datatype = 7; 50 | _message.fields[1].count = 1; 51 | _message.fields[2].name = "z"; 52 | _message.fields[2].offset = 8; 53 | _message.fields[2].datatype = 7; 54 | _message.fields[2].count = 1; 55 | _message.fields[3].name = "rgba"; 56 | _message.fields[3].offset = 12; 57 | _message.fields[3].datatype = 6; 58 | _message.is_bigendian = false; 59 | _message.point_step = 16; 60 | _message.row_step = (uint)(_dcam.resolution.x*_dcam.resolution.y*16); 61 | _message.data = new byte[(uint)(_dcam.resolution.x*_dcam.resolution.y*16)]; 62 | _message.is_dense = true; 63 | } 64 | 65 | private void Update() 66 | { 67 | _timeElapsed += Time.deltaTime; 68 | if (_timeElapsed > (1f / _dcam.scanRate)) 69 | { 70 | _timeElapsed = 0; 71 | _timeStamp = Time.time; 72 | 73 | uint sec = (uint)Math.Truncate(this._timeStamp); 74 | uint nanosec = (uint)((this._timeStamp - sec) * 1e+9); 75 | _message.header.stamp.sec = sec; 76 | _message.header.stamp.nanosec = nanosec; 77 | _message.data = _dcam.data; 78 | _ros.Publish(_topic, _message); 79 | } 80 | } 81 | } 82 | } 83 | -------------------------------------------------------------------------------- /Scripts/Runtime/DCamera/DCameraPublisher.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 8e8563449ef87c0c4ae9e39623952f49 3 | MonoImporter: 4 | externalObjects: {} 5 | serializedVersion: 2 6 | defaultReferences: [] 7 | executionOrder: 0 8 | icon: {instanceID: 0} 9 | userData: 10 | assetBundleName: 11 | assetBundleVariant: 12 | -------------------------------------------------------------------------------- /Scripts/Runtime/GPS.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: d95706ba1f059bcd3812ad7e3216e39b 3 | folderAsset: yes 4 | DefaultImporter: 5 | externalObjects: {} 6 | userData: 7 | assetBundleName: 8 | assetBundleVariant: 9 | -------------------------------------------------------------------------------- /Scripts/Runtime/GPS/GPSPublisher.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Collections; 3 | using System.Collections.Generic; 4 | using UnityEngine; 5 | 6 | using Unity.Robotics.ROSTCPConnector; 7 | using RosMessageTypes.Nmea; 8 | 9 | [RequireComponent(typeof(FRJ.Sensor.GPS))] 10 | public class GPSPublisher : MonoBehaviour 11 | { 12 | private enum FORMAT_TYPE 13 | { 14 | GPRMC, 15 | GPGGA, 16 | GPGSA, 17 | GPVTG 18 | } 19 | 20 | [SerializeField] private string _topicName = "nmea/sentence"; 21 | [SerializeField] private string _frameId = "nmea_link"; 22 | [SerializeField] private FORMAT_TYPE _formatType; 23 | 24 | private float _timeElapsed = 0f; 25 | private float _timeStamp = 0f; 26 | 27 | private ROSConnection _ros; 28 | private SentenceMsg _message; 29 | 30 | private FRJ.Sensor.GPS _gps; 31 | 32 | void Start() 33 | { 34 | // Setup GPS 35 | this._gps = GetComponent(); 36 | this._gps.Init(); 37 | 38 | // setup ROS 39 | this._ros = ROSConnection.instance; 40 | this._ros.RegisterPublisher(this._topicName); 41 | 42 | // setup ROS Message 43 | this._message = new SentenceMsg(); 44 | this._message.header.frame_id = this._frameId; 45 | } 46 | 47 | void Update() 48 | { 49 | this._timeElapsed += Time.deltaTime; 50 | 51 | if(this._timeElapsed > (1f/this._gps.updateRate)) 52 | { 53 | // Update time 54 | this._timeElapsed = 0; 55 | this._timeStamp = Time.time; 56 | 57 | // Update GPS 58 | this._gps.updateGPS(); 59 | 60 | // Update ROS Message 61 | # if ROS2 62 | int sec = (int)Math.Truncate(this._timeStamp); 63 | # else 64 | uint sec = (uint)Math.Truncate(this._timeStamp); 65 | # endif 66 | uint nanosec = (uint)( (this._timeStamp - sec)*1e+9 ); 67 | this._message.header.stamp.sec = sec; 68 | this._message.header.stamp.nanosec = nanosec; 69 | 70 | switch (_formatType) 71 | { 72 | case FORMAT_TYPE.GPRMC: 73 | this._message.sentence = this._gps.gprmc; 74 | break; 75 | case FORMAT_TYPE.GPGGA: 76 | this._message.sentence = this._gps.gpgga; 77 | break; 78 | case FORMAT_TYPE.GPGSA: 79 | this._message.sentence = this._gps.gpgsa; 80 | break; 81 | case FORMAT_TYPE.GPVTG: 82 | this._message.sentence = this._gps.gpvtg; 83 | break; 84 | default: 85 | this._message.sentence = ""; 86 | break; 87 | } 88 | 89 | this._ros.Send(this._topicName, this._message); 90 | } 91 | } 92 | } 93 | -------------------------------------------------------------------------------- /Scripts/Runtime/GPS/GPSPublisher.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 4072a511603c277029a24913af269eb7 3 | MonoImporter: 4 | externalObjects: {} 5 | serializedVersion: 2 6 | defaultReferences: [] 7 | executionOrder: 0 8 | icon: {instanceID: 0} 9 | userData: 10 | assetBundleName: 11 | assetBundleVariant: 12 | -------------------------------------------------------------------------------- /Scripts/Runtime/IMU.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: d06f48d87ad0cd0b0889e760a96e3f37 3 | folderAsset: yes 4 | DefaultImporter: 5 | externalObjects: {} 6 | userData: 7 | assetBundleName: 8 | assetBundleVariant: 9 | -------------------------------------------------------------------------------- /Scripts/Runtime/IMU/IMUPublisher.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Collections; 3 | using System.Collections.Generic; 4 | using UnityEngine; 5 | 6 | using Unity.Robotics.ROSTCPConnector; 7 | using Unity.Robotics.ROSTCPConnector.ROSGeometry; 8 | using RosMessageTypes.Sensor; 9 | using RosMessageTypes.Geometry; 10 | 11 | [RequireComponent(typeof(FRJ.Sensor.IMU))] 12 | public class IMUPublisher : MonoBehaviour 13 | { 14 | 15 | [SerializeField] private string _topicName = "imu/raw_data"; 16 | [SerializeField] private string _frameId = "imu_link"; 17 | 18 | private float _timeElapsed = 0f; 19 | private float _timeStamp = 0f; 20 | 21 | private ROSConnection _ros; 22 | public ImuMsg _message; 23 | 24 | private FRJ.Sensor.IMU _imu; 25 | 26 | void Start() 27 | { 28 | // Get Rotate Lidar 29 | this._imu = GetComponent(); 30 | 31 | // setup ROS 32 | this._ros = ROSConnection.instance; 33 | this._ros.RegisterPublisher(this._topicName); 34 | 35 | // setup ROS Message 36 | this._message = new ImuMsg(); 37 | this._message.header.frame_id = this._frameId; 38 | } 39 | 40 | void Update() 41 | { 42 | this._timeElapsed += Time.deltaTime; 43 | 44 | if (this._timeElapsed > (1f / this._imu.scanRate)) 45 | { 46 | // Update time 47 | this._timeElapsed = 0; 48 | this._timeStamp = Time.time; 49 | 50 | // Update IMU data 51 | this._imu.UpdateIMU(); 52 | 53 | // Update ROS Message 54 | # if ROS2 55 | int sec = (int)Math.Truncate(this._timeStamp); 56 | # else 57 | uint sec = (uint)Math.Truncate(this._timeStamp); 58 | # endif 59 | uint nanosec = (uint)((this._timeStamp - sec) * 1e+9); 60 | this._message.header.stamp.sec = sec; 61 | this._message.header.stamp.nanosec = nanosec; 62 | Quaternion orientation_ros = new Quaternion(this._imu.GeometryQuaternion.x, 63 | this._imu.GeometryQuaternion.y, 64 | this._imu.GeometryQuaternion.z, 65 | this._imu.GeometryQuaternion.w).To(); 66 | QuaternionMsg orientation = 67 | new QuaternionMsg(orientation_ros.x, 68 | orientation_ros.y, 69 | orientation_ros.z, 70 | orientation_ros.w); 71 | this._message.orientation = orientation; 72 | Vector3 angular_velocity_ros = new Vector3(this._imu.AngularVelocity).To(); 73 | Vector3Msg angular_velocity = 74 | new Vector3Msg(angular_velocity_ros.x, 75 | angular_velocity_ros.y, 76 | angular_velocity_ros.z); 77 | this._message.angular_velocity = angular_velocity; 78 | Vector3 linear_acceleration_ros = new Vector3(this._imu.LinearAcceleration).To(); 79 | Vector3Msg linear_acceleration = 80 | new Vector3Msg(linear_acceleration_ros.x, 81 | linear_acceleration_ros.y, 82 | linear_acceleration_ros.z); 83 | this._message.linear_acceleration = linear_acceleration; 84 | this._ros.Send(this._topicName, this._message); 85 | } 86 | } 87 | } 88 | -------------------------------------------------------------------------------- /Scripts/Runtime/IMU/IMUPublisher.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 7724d69d8cddc8981aee4cb75301fe79 3 | MonoImporter: 4 | externalObjects: {} 5 | serializedVersion: 2 6 | defaultReferences: [] 7 | executionOrder: 0 8 | icon: {instanceID: 0} 9 | userData: 10 | assetBundleName: 11 | assetBundleVariant: 12 | -------------------------------------------------------------------------------- /Scripts/Runtime/LaserScan.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: de389e98e4eb5f68e80fa0fea3381ba1 3 | folderAsset: yes 4 | DefaultImporter: 5 | externalObjects: {} 6 | userData: 7 | assetBundleName: 8 | assetBundleVariant: 9 | -------------------------------------------------------------------------------- /Scripts/Runtime/LaserScan/LaserScanPublisher.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Collections; 3 | using System.Collections.Generic; 4 | using Unity.Burst; 5 | using UnityEngine.Jobs; 6 | using Unity.Jobs; 7 | using UnityEngine; 8 | 9 | using Unity.Robotics.ROSTCPConnector; 10 | using RosMessageTypes.Sensor; 11 | 12 | [RequireComponent(typeof(FRJ.Sensor.RotateLidar))] 13 | public class LaserScanPublisher : MonoBehaviour 14 | { 15 | 16 | [SerializeField] private string _topicName = "scan"; 17 | [SerializeField] private string _frameId = "scan_link"; 18 | 19 | private JobHandle _handle; 20 | private float _timeElapsed = 0f; 21 | private float _timeStamp = 0f; 22 | 23 | private ROSConnection _ros; 24 | private LaserScanMsg _message; 25 | 26 | private FRJ.Sensor.RotateLidar _lidar; 27 | 28 | float Deg2Rad(float deg) 29 | { 30 | return deg * Mathf.PI / 180f; 31 | } 32 | 33 | void Start() 34 | { 35 | // Get Rotate Lidar 36 | this._lidar = GetComponent(); 37 | this._lidar.Init(); 38 | 39 | // setup ROS 40 | this._ros = ROSConnection.instance; 41 | this._ros.RegisterPublisher(this._topicName); 42 | 43 | // setup ROS Message 44 | this._message = new LaserScanMsg(); 45 | this._message.header.frame_id = this._frameId; 46 | this._message.angle_min = Deg2Rad(this._lidar.minAzimuthAngle); 47 | this._message.angle_max = Deg2Rad(this._lidar.maxAzimuthAngle); 48 | var deg_increment = (this._lidar.maxAzimuthAngle - this._lidar.minAzimuthAngle) / (float)this._lidar.numOfIncrements; 49 | this._message.angle_increment = Deg2Rad(deg_increment); 50 | this._message.time_increment = 1f/this._lidar.scanRate/(float)this._lidar.numOfIncrements; 51 | this._message.scan_time = 1f/this._lidar.scanRate; 52 | this._message.range_min = this._lidar.minRange; 53 | this._message.range_max = this._lidar.maxRange; 54 | } 55 | 56 | void OnDisable() 57 | { 58 | this._handle.Complete(); 59 | this._lidar.Dispose(); 60 | } 61 | 62 | void Update() 63 | { 64 | this._handle.Complete(); 65 | this._timeElapsed += Time.deltaTime; 66 | 67 | if(this._timeElapsed > (1f/this._lidar.scanRate)) { 68 | // Update ROS Message 69 | # if ROS2 70 | int sec = (int)Math.Truncate(this._timeStamp); 71 | # else 72 | uint sec = (uint)Math.Truncate(this._timeStamp); 73 | # endif 74 | uint nanosec = (uint)( (this._timeStamp - sec)*1e+9 ); 75 | this._message.header.stamp.sec = sec; 76 | this._message.header.stamp.nanosec = nanosec; 77 | this._message.ranges = this._lidar.distances.ToArray(); 78 | this._message.intensities = this._lidar.intensities.ToArray(); 79 | _ros.Send(this._topicName, this._message); 80 | 81 | // Update time 82 | this._timeElapsed = 0; 83 | this._timeStamp = Time.time; 84 | 85 | // Update Raycast Command 86 | for (int incr = 0; incr < this._lidar.numOfIncrements; incr++) { 87 | for (int layer = 0; layer < this._lidar.numOfLayers; layer++) { 88 | int index = layer + incr * this._lidar.numOfLayers; 89 | this._lidar.commands[index] = 90 | new RaycastCommand(this.transform.position, 91 | this.transform.rotation * this._lidar.commandDirVecs[index], 92 | this._lidar.maxRange); 93 | } 94 | } 95 | 96 | // Update Parallel Jobs 97 | var raycastJobHandle = RaycastCommand.ScheduleBatch(this._lidar.commands, this._lidar.results, 360); 98 | // Update Distance data 99 | if(this._lidar.randomSeed++ == 0) 100 | this._lidar.randomSeed = 1; 101 | this._lidar.job.random.InitState(this._lidar.randomSeed); 102 | this._handle = this._lidar.job.Schedule(this._lidar.results.Length, 360, raycastJobHandle); 103 | JobHandle.ScheduleBatchedJobs(); 104 | } 105 | } 106 | } 107 | -------------------------------------------------------------------------------- /Scripts/Runtime/LaserScan/LaserScanPublisher.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: a6d928fda1f4ea4f18f589c4b7da6aad 3 | MonoImporter: 4 | externalObjects: {} 5 | serializedVersion: 2 6 | defaultReferences: [] 7 | executionOrder: 0 8 | icon: {instanceID: 0} 9 | userData: 10 | assetBundleName: 11 | assetBundleVariant: 12 | -------------------------------------------------------------------------------- /Scripts/Runtime/Livox.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: f420c4c5e2de8594886a944d8173db71 3 | folderAsset: yes 4 | DefaultImporter: 5 | externalObjects: {} 6 | userData: 7 | assetBundleName: 8 | assetBundleVariant: 9 | -------------------------------------------------------------------------------- /Scripts/Runtime/Livox/LivoxPublisher.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Collections; 3 | using System.Collections.Generic; 4 | using Unity.Burst; 5 | using Unity.Collections; 6 | using UnityEngine.Jobs; 7 | using Unity.Jobs; 8 | using UnityEngine; 9 | 10 | using Unity.Robotics.ROSTCPConnector; 11 | using RosMessageTypes.LivoxRosDriver; 12 | using RosMessageTypes.Std; 13 | using RosMessageTypes.Sensor; 14 | 15 | [RequireComponent(typeof(FRJ.Sensor.CSVLidar))] 16 | public class LivoxPublisher : MonoBehaviour 17 | { 18 | enum Model 19 | { 20 | Avia, 21 | Horizon, 22 | Mid40, 23 | Mid70, 24 | Tele 25 | }; 26 | 27 | enum PublishType 28 | { 29 | PointCloud2, 30 | LivoxCustomMsg 31 | } 32 | 33 | [Header("Parameters")] 34 | [SerializeField] private Model _model; 35 | [SerializeField] private PublishType _publishType; 36 | [SerializeField] private string _topicName = "/livox/lidar"; 37 | [SerializeField] private string _frameId = "/livox"; 38 | [SerializeField] private byte lidar_id; 39 | [SerializeField] private byte[] rsvd; 40 | 41 | [Header("Informations(No need to input)")] 42 | 43 | [SerializeField] private string _avia_filePath = "avia"; 44 | [SerializeField] private string _horizon_filePath = "horizon"; 45 | [SerializeField] private string _mid40_filePath = "mid40"; 46 | [SerializeField] private string _mid70_filePath = "mid70"; 47 | [SerializeField] private string _tele_filePath = "tele"; 48 | 49 | private JobHandle _handle; 50 | public ulong timebase; 51 | private float _timeElapsed = 0f; 52 | private float _timeStamp = 0f; 53 | 54 | private ROSConnection _ros; 55 | private CustomMsgMsg _message_cmm; 56 | private PointCloud2Msg _message_pc2; 57 | 58 | private FRJ.Sensor.CSVLidar _lidar; 59 | private FRJ.Sensor.LivoxSerializer _serializer; 60 | 61 | void Start() 62 | { 63 | // Get CSVLidar 64 | this._lidar = GetComponent(); 65 | 66 | string filePath = ""; 67 | switch (_model) 68 | { 69 | case Model.Avia: 70 | filePath = _avia_filePath; 71 | break; 72 | case Model.Horizon: 73 | filePath = _horizon_filePath; 74 | break; 75 | case Model.Mid40: 76 | filePath = _mid40_filePath; 77 | break; 78 | case Model.Mid70: 79 | filePath = _mid70_filePath; 80 | break; 81 | case Model.Tele: 82 | filePath = _tele_filePath; 83 | break; 84 | } 85 | this._lidar.csvFilePath = filePath; 86 | this._lidar.Init(); 87 | 88 | this._serializer = new FRJ.Sensor.LivoxSerializer(this._lidar.numOfLasersPerScan); 89 | this._serializer.job.timebase = this.timebase = (UInt64)(Time.time * 1000); 90 | this._serializer.job.point = this._lidar.point; 91 | this._serializer.job.intensities = this._lidar.intensities; 92 | 93 | // setup ROS 94 | this._ros = ROSConnection.instance; 95 | switch (_publishType) 96 | { 97 | case PublishType.PointCloud2: 98 | this._ros.RegisterPublisher(this._topicName); 99 | break; 100 | case PublishType.LivoxCustomMsg: 101 | this._ros.RegisterPublisher(this._topicName); 102 | break; 103 | } 104 | 105 | // setup ROS Message 106 | 107 | switch (_publishType) 108 | { 109 | case PublishType.PointCloud2: 110 | this._message_pc2 = new PointCloud2Msg(); 111 | this._message_pc2.header.frame_id = this._frameId; 112 | this._message_pc2.height = 1; 113 | this._message_pc2.width = (uint)(_lidar.numOfLasersPerScan); 114 | this._message_pc2.fields = new PointFieldMsg[3]; 115 | for(int i = 0; i < 3; i++) 116 | { 117 | this._message_pc2.fields[i] = new PointFieldMsg(); 118 | } 119 | this._message_pc2.fields[0].name = "x"; 120 | this._message_pc2.fields[0].offset = 0; 121 | this._message_pc2.fields[0].datatype = 7; 122 | this._message_pc2.fields[0].count = 1; 123 | this._message_pc2.fields[1].name = "y"; 124 | this._message_pc2.fields[1].offset = 4; 125 | this._message_pc2.fields[1].datatype = 7; 126 | this._message_pc2.fields[1].count = 1; 127 | this._message_pc2.fields[2].name = "z"; 128 | this._message_pc2.fields[2].offset = 8; 129 | this._message_pc2.fields[2].datatype = 7; 130 | this._message_pc2.fields[2].count = 1; 131 | this._message_pc2.is_bigendian = false; 132 | this._message_pc2.point_step = 12; 133 | this._message_pc2.row_step = (uint)(_lidar.numOfLasersPerScan * 12); 134 | this._message_pc2.data = new byte[_lidar.numOfLasersPerScan*12]; 135 | this._message_pc2.is_dense = true; 136 | break; 137 | case PublishType.LivoxCustomMsg: 138 | this._message_cmm = new CustomMsgMsg(); 139 | this._message_cmm.header.frame_id = this._frameId; 140 | this._message_cmm.points = new CustomPointMsg[this._lidar.numOfLasersPerScan]; 141 | for (int i = 0; i < this._lidar.numOfLasersPerScan; i++) 142 | { 143 | this._message_cmm.points[i] = new CustomPointMsg(); 144 | } 145 | break; 146 | } 147 | } 148 | 149 | void OnDisable() 150 | { 151 | this._handle.Complete(); 152 | this._lidar.Dispose(); 153 | this._serializer.Dispose(); 154 | } 155 | 156 | void Update() 157 | { 158 | if (!this._lidar.isInitialized) return; 159 | this._handle.Complete(); 160 | this._timeElapsed += Time.deltaTime; 161 | 162 | if (this._timeElapsed > (1f / this._lidar.scanRate)) 163 | { 164 | // Update time 165 | this._timeElapsed = 0; 166 | this._timeStamp = Time.time; 167 | 168 | // Update ROS Message 169 | uint sec = (uint)Math.Truncate(this._timeStamp); 170 | uint nanosec = (uint)((this._timeStamp - sec) * 1e+9); 171 | this._serializer.job.time = (UInt64)(this._timeStamp * 1000 - this.timebase); 172 | 173 | switch (_publishType) 174 | { 175 | case PublishType.PointCloud2: 176 | this._message_pc2.header.stamp.sec = sec; 177 | this._message_pc2.header.stamp.nanosec = nanosec; 178 | for (int i = 0; i < this._lidar.numOfLasersPerScan*12; i++) 179 | { 180 | this._message_pc2.data[i] = this._serializer.data[i]; 181 | } 182 | Debug.Log(this._message_pc2.data[0]); 183 | _ros.Send(this._topicName, this._message_pc2); 184 | break; 185 | case PublishType.LivoxCustomMsg: 186 | this._message_cmm.header.stamp.sec = sec; 187 | this._message_cmm.header.stamp.nanosec = nanosec; 188 | this._message_cmm.timebase = this.timebase; 189 | this._message_cmm.point_num = (uint)this._lidar.numOfLasersPerScan; 190 | this._message_cmm.lidar_id = this.lidar_id; 191 | this._message_cmm.rsvd = this.rsvd; 192 | for (int i = 0; i < this._lidar.numOfLasersPerScan; i++) 193 | { 194 | this._message_cmm.points[i].x = this._serializer.x[i]; 195 | this._message_cmm.points[i].y = this._serializer.y[i]; 196 | this._message_cmm.points[i].z = this._serializer.z[i]; 197 | this._message_cmm.points[i].reflectivity = this._serializer.reflectivity[i]; 198 | this._message_cmm.points[i].tag = this._serializer.tag[i]; 199 | this._message_cmm.points[i].line = this._serializer.line[i]; 200 | } 201 | _ros.Send(this._topicName, this._message_cmm); 202 | break; 203 | } 204 | 205 | this._lidar.UpdateCommandDirVecs(); 206 | 207 | // Update Raycast Command and Origin Transform 208 | for (int i = 0; i < this._lidar.numOfLasersPerScan; i++) 209 | { 210 | this._lidar.origin_pos[i] = this.transform.position; 211 | this._lidar.origin_rot[i] = this.transform.rotation; 212 | this._lidar.commands[i] = 213 | new RaycastCommand(this.transform.position, 214 | this.transform.rotation * this._lidar.commandDirVecs[i], 215 | this._lidar.maxRange); 216 | } 217 | 218 | // Update Parallel Jobs 219 | var raycastJobHandle = RaycastCommand.ScheduleBatch(this._lidar.commands, this._lidar.results, 360); 220 | // Update Distance data 221 | if (this._lidar.randomSeed++ == 0) 222 | this._lidar.randomSeed = 1; 223 | this._lidar.job.random.InitState(this._lidar.randomSeed); 224 | var pointJobHandle = this._lidar.job.Schedule(this._lidar.results.Length, 360, raycastJobHandle); 225 | this._handle = this._serializer.job.Schedule(pointJobHandle); 226 | JobHandle.ScheduleBatchedJobs(); 227 | } 228 | } 229 | } 230 | -------------------------------------------------------------------------------- /Scripts/Runtime/Livox/LivoxPublisher.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 21d196921d84b6947a59b9dcbb30a65d 3 | MonoImporter: 4 | externalObjects: {} 5 | serializedVersion: 2 6 | defaultReferences: [] 7 | executionOrder: 0 8 | icon: {instanceID: 0} 9 | userData: 10 | assetBundleName: 11 | assetBundleVariant: 12 | -------------------------------------------------------------------------------- /Scripts/Runtime/Messages.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 578d68fb5b2bed5aa9272744d8492712 3 | folderAsset: yes 4 | DefaultImporter: 5 | externalObjects: {} 6 | userData: 7 | assetBundleName: 8 | assetBundleVariant: 9 | -------------------------------------------------------------------------------- /Scripts/Runtime/Messages/LivoxRosDriver.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 690c3dd83371acb4696586c2b6df2b38 3 | folderAsset: yes 4 | DefaultImporter: 5 | externalObjects: {} 6 | userData: 7 | assetBundleName: 8 | assetBundleVariant: 9 | -------------------------------------------------------------------------------- /Scripts/Runtime/Messages/LivoxRosDriver/msg.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: d2b83a78952d98949927164801ff826f 3 | folderAsset: yes 4 | DefaultImporter: 5 | externalObjects: {} 6 | userData: 7 | assetBundleName: 8 | assetBundleVariant: 9 | -------------------------------------------------------------------------------- /Scripts/Runtime/Messages/LivoxRosDriver/msg/CustomMsgMsg.cs: -------------------------------------------------------------------------------- 1 | //Do not edit! This file was generated by Unity-ROS MessageGeneration. 2 | using System; 3 | using System.Linq; 4 | using System.Collections.Generic; 5 | using System.Text; 6 | using Unity.Robotics.ROSTCPConnector.MessageGeneration; 7 | using RosMessageTypes.Std; 8 | 9 | namespace RosMessageTypes.LivoxRosDriver 10 | { 11 | [Serializable] 12 | public class CustomMsgMsg : Message 13 | { 14 | public const string k_RosMessageName = "livox_ros_driver/CustomMsg"; 15 | public override string RosMessageName => k_RosMessageName; 16 | 17 | // Livox publish pointcloud msg format. 18 | public HeaderMsg header; 19 | // ROS standard message header 20 | public ulong timebase; 21 | // The time of first point 22 | public uint point_num; 23 | // Total number of pointclouds 24 | public byte lidar_id; 25 | // Lidar device id number 26 | public byte[] rsvd; 27 | // Reserved use 28 | public CustomPointMsg[] points; 29 | // Pointcloud data 30 | 31 | public CustomMsgMsg() 32 | { 33 | this.header = new HeaderMsg(); 34 | this.timebase = 0; 35 | this.point_num = 0; 36 | this.lidar_id = 0; 37 | this.rsvd = new byte[3]; 38 | this.points = new CustomPointMsg[0]; 39 | } 40 | 41 | public CustomMsgMsg(HeaderMsg header, ulong timebase, uint point_num, byte lidar_id, byte[] rsvd, CustomPointMsg[] points) 42 | { 43 | this.header = header; 44 | this.timebase = timebase; 45 | this.point_num = point_num; 46 | this.lidar_id = lidar_id; 47 | this.rsvd = rsvd; 48 | this.points = points; 49 | } 50 | 51 | public static CustomMsgMsg Deserialize(MessageDeserializer deserializer) => new CustomMsgMsg(deserializer); 52 | 53 | private CustomMsgMsg(MessageDeserializer deserializer) 54 | { 55 | this.header = HeaderMsg.Deserialize(deserializer); 56 | deserializer.Read(out this.timebase); 57 | deserializer.Read(out this.point_num); 58 | deserializer.Read(out this.lidar_id); 59 | deserializer.Read(out this.rsvd, sizeof(byte), 3); 60 | deserializer.Read(out this.points, CustomPointMsg.Deserialize, deserializer.ReadLength()); 61 | } 62 | 63 | public override void SerializeTo(MessageSerializer serializer) 64 | { 65 | serializer.Write(this.header); 66 | serializer.Write(this.timebase); 67 | serializer.Write(this.point_num); 68 | serializer.Write(this.lidar_id); 69 | serializer.Write(this.rsvd); 70 | serializer.WriteLength(this.points); 71 | serializer.Write(this.points); 72 | } 73 | 74 | public override string ToString() 75 | { 76 | return "CustomMsgMsg: " + 77 | "\nheader: " + header.ToString() + 78 | "\ntimebase: " + timebase.ToString() + 79 | "\npoint_num: " + point_num.ToString() + 80 | "\nlidar_id: " + lidar_id.ToString() + 81 | "\nrsvd: " + System.String.Join(", ", rsvd.ToList()) + 82 | "\npoints: " + System.String.Join(", ", points.ToList()); 83 | } 84 | 85 | #if UNITY_EDITOR 86 | [UnityEditor.InitializeOnLoadMethod] 87 | #else 88 | [UnityEngine.RuntimeInitializeOnLoadMethod] 89 | #endif 90 | public static void Register() 91 | { 92 | MessageRegistry.Register(k_RosMessageName, Deserialize); 93 | } 94 | } 95 | } 96 | -------------------------------------------------------------------------------- /Scripts/Runtime/Messages/LivoxRosDriver/msg/CustomMsgMsg.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 5f50b8d3df53e074585b7a14eab706c9 3 | MonoImporter: 4 | externalObjects: {} 5 | serializedVersion: 2 6 | defaultReferences: [] 7 | executionOrder: 0 8 | icon: {instanceID: 0} 9 | userData: 10 | assetBundleName: 11 | assetBundleVariant: 12 | -------------------------------------------------------------------------------- /Scripts/Runtime/Messages/LivoxRosDriver/msg/CustomPointMsg.cs: -------------------------------------------------------------------------------- 1 | //Do not edit! This file was generated by Unity-ROS MessageGeneration. 2 | using System; 3 | using System.Linq; 4 | using System.Collections.Generic; 5 | using System.Text; 6 | using Unity.Robotics.ROSTCPConnector.MessageGeneration; 7 | 8 | namespace RosMessageTypes.LivoxRosDriver 9 | { 10 | [Serializable] 11 | public class CustomPointMsg : Message 12 | { 13 | public const string k_RosMessageName = "livox_ros_driver/CustomPoint"; 14 | public override string RosMessageName => k_RosMessageName; 15 | 16 | // Livox costom pointcloud format. 17 | public uint offset_time; 18 | // offset time relative to the base time 19 | public float x; 20 | // X axis, unit:m 21 | public float y; 22 | // Y axis, unit:m 23 | public float z; 24 | // Z axis, unit:m 25 | public byte reflectivity; 26 | // reflectivity, 0~255 27 | public byte tag; 28 | // livox tag 29 | public byte line; 30 | // laser number in lidar 31 | 32 | public CustomPointMsg() 33 | { 34 | this.offset_time = 0; 35 | this.x = 0.0f; 36 | this.y = 0.0f; 37 | this.z = 0.0f; 38 | this.reflectivity = 0; 39 | this.tag = 0; 40 | this.line = 0; 41 | } 42 | 43 | public CustomPointMsg(uint offset_time, float x, float y, float z, byte reflectivity, byte tag, byte line) 44 | { 45 | this.offset_time = offset_time; 46 | this.x = x; 47 | this.y = y; 48 | this.z = z; 49 | this.reflectivity = reflectivity; 50 | this.tag = tag; 51 | this.line = line; 52 | } 53 | 54 | public static CustomPointMsg Deserialize(MessageDeserializer deserializer) => new CustomPointMsg(deserializer); 55 | 56 | private CustomPointMsg(MessageDeserializer deserializer) 57 | { 58 | deserializer.Read(out this.offset_time); 59 | deserializer.Read(out this.x); 60 | deserializer.Read(out this.y); 61 | deserializer.Read(out this.z); 62 | deserializer.Read(out this.reflectivity); 63 | deserializer.Read(out this.tag); 64 | deserializer.Read(out this.line); 65 | } 66 | 67 | public override void SerializeTo(MessageSerializer serializer) 68 | { 69 | serializer.Write(this.offset_time); 70 | serializer.Write(this.x); 71 | serializer.Write(this.y); 72 | serializer.Write(this.z); 73 | serializer.Write(this.reflectivity); 74 | serializer.Write(this.tag); 75 | serializer.Write(this.line); 76 | } 77 | 78 | public override string ToString() 79 | { 80 | return "CustomPointMsg: " + 81 | "\noffset_time: " + offset_time.ToString() + 82 | "\nx: " + x.ToString() + 83 | "\ny: " + y.ToString() + 84 | "\nz: " + z.ToString() + 85 | "\nreflectivity: " + reflectivity.ToString() + 86 | "\ntag: " + tag.ToString() + 87 | "\nline: " + line.ToString(); 88 | } 89 | 90 | #if UNITY_EDITOR 91 | [UnityEditor.InitializeOnLoadMethod] 92 | #else 93 | [UnityEngine.RuntimeInitializeOnLoadMethod] 94 | #endif 95 | public static void Register() 96 | { 97 | MessageRegistry.Register(k_RosMessageName, Deserialize); 98 | } 99 | } 100 | } 101 | -------------------------------------------------------------------------------- /Scripts/Runtime/Messages/LivoxRosDriver/msg/CustomPointMsg.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: e3ef58ad272e3584ca914627acdcc992 3 | MonoImporter: 4 | externalObjects: {} 5 | serializedVersion: 2 6 | defaultReferences: [] 7 | executionOrder: 0 8 | icon: {instanceID: 0} 9 | userData: 10 | assetBundleName: 11 | assetBundleVariant: 12 | -------------------------------------------------------------------------------- /Scripts/Runtime/Messages/Nmea.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: e5298326eb5eacbccb76c02a6cb5e498 3 | folderAsset: yes 4 | DefaultImporter: 5 | externalObjects: {} 6 | userData: 7 | assetBundleName: 8 | assetBundleVariant: 9 | -------------------------------------------------------------------------------- /Scripts/Runtime/Messages/Nmea/msg.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 3d2824f20957933bc83dfc278f624a98 3 | folderAsset: yes 4 | DefaultImporter: 5 | externalObjects: {} 6 | userData: 7 | assetBundleName: 8 | assetBundleVariant: 9 | -------------------------------------------------------------------------------- /Scripts/Runtime/Messages/Nmea/msg/SentenceMsg.cs: -------------------------------------------------------------------------------- 1 | //Do not edit! This file was generated by Unity-ROS MessageGeneration. 2 | using System; 3 | using System.Linq; 4 | using System.Collections.Generic; 5 | using System.Text; 6 | using Unity.Robotics.ROSTCPConnector.MessageGeneration; 7 | using RosMessageTypes.Std; 8 | 9 | namespace RosMessageTypes.Nmea 10 | { 11 | [Serializable] 12 | public class SentenceMsg : Message 13 | { 14 | public const string k_RosMessageName = "nmea_msgs/Sentence"; 15 | public override string RosMessageName => k_RosMessageName; 16 | 17 | // A message representing a single NMEA0183 sentence. 18 | // header.stamp is the ROS Time when the sentence was read. 19 | // header.frame_id is the frame of reference reported by the satellite 20 | // receiver, usually the location of the antenna. This is a 21 | // Euclidean frame relative to the vehicle, not a reference 22 | // ellipsoid. 23 | public HeaderMsg header; 24 | // This should only contain ASCII characters in order to be a valid NMEA0183 sentence. 25 | public string sentence; 26 | 27 | public SentenceMsg() 28 | { 29 | this.header = new HeaderMsg(); 30 | this.sentence = ""; 31 | } 32 | 33 | public SentenceMsg(HeaderMsg header, string sentence) 34 | { 35 | this.header = header; 36 | this.sentence = sentence; 37 | } 38 | 39 | public static SentenceMsg Deserialize(MessageDeserializer deserializer) => new SentenceMsg(deserializer); 40 | 41 | private SentenceMsg(MessageDeserializer deserializer) 42 | { 43 | this.header = HeaderMsg.Deserialize(deserializer); 44 | deserializer.Read(out this.sentence); 45 | } 46 | 47 | public override void SerializeTo(MessageSerializer serializer) 48 | { 49 | serializer.Write(this.header); 50 | serializer.Write(this.sentence); 51 | } 52 | 53 | public override string ToString() 54 | { 55 | return "SentenceMsg: " + 56 | "\nheader: " + header.ToString() + 57 | "\nsentence: " + sentence.ToString(); 58 | } 59 | 60 | #if UNITY_EDITOR 61 | [UnityEditor.InitializeOnLoadMethod] 62 | #else 63 | [UnityEngine.RuntimeInitializeOnLoadMethod] 64 | #endif 65 | public static void Register() 66 | { 67 | MessageRegistry.Register(k_RosMessageName, Deserialize); 68 | } 69 | } 70 | } 71 | -------------------------------------------------------------------------------- /Scripts/Runtime/Messages/Nmea/msg/SentenceMsg.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 5097bc2b93f062ac282fd76c31f964e6 3 | MonoImporter: 4 | externalObjects: {} 5 | serializedVersion: 2 6 | defaultReferences: [] 7 | executionOrder: 0 8 | icon: {instanceID: 0} 9 | userData: 10 | assetBundleName: 11 | assetBundleVariant: 12 | -------------------------------------------------------------------------------- /Scripts/Runtime/Messages/Rosgraph.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: c5e0eb194310f9af7bd4ccdca5acb93a 3 | folderAsset: yes 4 | DefaultImporter: 5 | externalObjects: {} 6 | userData: 7 | assetBundleName: 8 | assetBundleVariant: 9 | -------------------------------------------------------------------------------- /Scripts/Runtime/Messages/Velodyne.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 1817d76f9ad2315bc96c67a039b868c6 3 | folderAsset: yes 4 | DefaultImporter: 5 | externalObjects: {} 6 | userData: 7 | assetBundleName: 8 | assetBundleVariant: 9 | -------------------------------------------------------------------------------- /Scripts/Runtime/Messages/Velodyne/msg.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: bfe0a617a1b183d578072c1ef35a049b 3 | folderAsset: yes 4 | DefaultImporter: 5 | externalObjects: {} 6 | userData: 7 | assetBundleName: 8 | assetBundleVariant: 9 | -------------------------------------------------------------------------------- /Scripts/Runtime/Messages/Velodyne/msg/VelodynePacketMsg.cs: -------------------------------------------------------------------------------- 1 | //Do not edit! This file was generated by Unity-ROS MessageGeneration. 2 | using System; 3 | using System.Linq; 4 | using System.Collections.Generic; 5 | using System.Text; 6 | using Unity.Robotics.ROSTCPConnector.MessageGeneration; 7 | using RosMessageTypes.BuiltinInterfaces; 8 | 9 | namespace RosMessageTypes.Velodyne 10 | { 11 | [Serializable] 12 | public class VelodynePacketMsg : Message 13 | { 14 | public const string k_RosMessageName = "velodyne_msgs/VelodynePacket"; 15 | public override string RosMessageName => k_RosMessageName; 16 | 17 | // Raw Velodyne LIDAR packet. 18 | public TimeMsg stamp; 19 | // packet timestamp 20 | public byte[] data; 21 | // packet contents 22 | 23 | public VelodynePacketMsg() 24 | { 25 | this.stamp = new TimeMsg(); 26 | this.data = new byte[1206]; 27 | } 28 | 29 | public VelodynePacketMsg(TimeMsg stamp, byte[] data) 30 | { 31 | this.stamp = stamp; 32 | this.data = data; 33 | } 34 | 35 | public static VelodynePacketMsg Deserialize(MessageDeserializer deserializer) => new VelodynePacketMsg(deserializer); 36 | 37 | private VelodynePacketMsg(MessageDeserializer deserializer) 38 | { 39 | this.stamp = TimeMsg.Deserialize(deserializer); 40 | deserializer.Read(out this.data, sizeof(byte), 1206); 41 | } 42 | 43 | public override void SerializeTo(MessageSerializer serializer) 44 | { 45 | serializer.Write(this.stamp); 46 | serializer.Write(this.data); 47 | } 48 | 49 | public override string ToString() 50 | { 51 | return "VelodynePacketMsg: " + 52 | "\nstamp: " + stamp.ToString() + 53 | "\ndata: " + System.String.Join(", ", data.ToList()); 54 | } 55 | 56 | #if UNITY_EDITOR 57 | [UnityEditor.InitializeOnLoadMethod] 58 | #else 59 | [UnityEngine.RuntimeInitializeOnLoadMethod] 60 | #endif 61 | public static void Register() 62 | { 63 | MessageRegistry.Register(k_RosMessageName, Deserialize); 64 | } 65 | } 66 | } 67 | -------------------------------------------------------------------------------- /Scripts/Runtime/Messages/Velodyne/msg/VelodynePacketMsg.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: a397c7894e141050b8f45b7ff15606b4 3 | MonoImporter: 4 | externalObjects: {} 5 | serializedVersion: 2 6 | defaultReferences: [] 7 | executionOrder: 0 8 | icon: {instanceID: 0} 9 | userData: 10 | assetBundleName: 11 | assetBundleVariant: 12 | -------------------------------------------------------------------------------- /Scripts/Runtime/Messages/Velodyne/msg/VelodyneScanMsg.cs: -------------------------------------------------------------------------------- 1 | //Do not edit! This file was generated by Unity-ROS MessageGeneration. 2 | using System; 3 | using System.Linq; 4 | using System.Collections.Generic; 5 | using System.Text; 6 | using Unity.Robotics.ROSTCPConnector.MessageGeneration; 7 | using RosMessageTypes.Std; 8 | 9 | namespace RosMessageTypes.Velodyne 10 | { 11 | [Serializable] 12 | public class VelodyneScanMsg : Message 13 | { 14 | public const string k_RosMessageName = "velodyne_msgs/VelodyneScan"; 15 | public override string RosMessageName => k_RosMessageName; 16 | 17 | // Velodyne LIDAR scan packets. 18 | public HeaderMsg header; 19 | // standard ROS message header 20 | public VelodynePacketMsg[] packets; 21 | // vector of raw packets 22 | 23 | public VelodyneScanMsg() 24 | { 25 | this.header = new HeaderMsg(); 26 | this.packets = new VelodynePacketMsg[0]; 27 | } 28 | 29 | public VelodyneScanMsg(HeaderMsg header, VelodynePacketMsg[] packets) 30 | { 31 | this.header = header; 32 | this.packets = packets; 33 | } 34 | 35 | public static VelodyneScanMsg Deserialize(MessageDeserializer deserializer) => new VelodyneScanMsg(deserializer); 36 | 37 | private VelodyneScanMsg(MessageDeserializer deserializer) 38 | { 39 | this.header = HeaderMsg.Deserialize(deserializer); 40 | deserializer.Read(out this.packets, VelodynePacketMsg.Deserialize, deserializer.ReadLength()); 41 | } 42 | 43 | public override void SerializeTo(MessageSerializer serializer) 44 | { 45 | serializer.Write(this.header); 46 | serializer.WriteLength(this.packets); 47 | serializer.Write(this.packets); 48 | } 49 | 50 | public override string ToString() 51 | { 52 | return "VelodyneScanMsg: " + 53 | "\nheader: " + header.ToString() + 54 | "\npackets: " + System.String.Join(", ", packets.ToList()); 55 | } 56 | 57 | #if UNITY_EDITOR 58 | [UnityEditor.InitializeOnLoadMethod] 59 | #else 60 | [UnityEngine.RuntimeInitializeOnLoadMethod] 61 | #endif 62 | public static void Register() 63 | { 64 | MessageRegistry.Register(k_RosMessageName, Deserialize); 65 | } 66 | } 67 | } 68 | -------------------------------------------------------------------------------- /Scripts/Runtime/Messages/Velodyne/msg/VelodyneScanMsg.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 0e07ea030791faf3990cb326cd91b3a2 3 | MonoImporter: 4 | externalObjects: {} 5 | serializedVersion: 2 6 | defaultReferences: [] 7 | executionOrder: 0 8 | icon: {instanceID: 0} 9 | userData: 10 | assetBundleName: 11 | assetBundleVariant: 12 | -------------------------------------------------------------------------------- /Scripts/Runtime/RGBCamera.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 588f84065ba6a53f4bd9d6f7d7c48a71 3 | folderAsset: yes 4 | DefaultImporter: 5 | externalObjects: {} 6 | userData: 7 | assetBundleName: 8 | assetBundleVariant: 9 | -------------------------------------------------------------------------------- /Scripts/Runtime/RGBCamera/RGBCameraPublisher.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Collections; 3 | using System.Collections.Generic; 4 | using UnityEngine; 5 | 6 | using Unity.Robotics.ROSTCPConnector; 7 | using RosMessageTypes.Sensor; 8 | 9 | [RequireComponent(typeof(Camera))] 10 | [RequireComponent(typeof(FRJ.Sensor.RGBCamera))] 11 | public class RGBCameraPublisher : MonoBehaviour 12 | { 13 | 14 | [SerializeField] private string _topicName = "image"; 15 | [SerializeField] private string _frameId = "camera"; 16 | 17 | private float _timeElapsed = 0f; 18 | private float _timeStamp = 0f; 19 | 20 | private ROSConnection _ros; 21 | private CompressedImageMsg _message; 22 | 23 | private FRJ.Sensor.RGBCamera _camera; 24 | 25 | void Start() 26 | { 27 | // Get Rotate Lidar 28 | this._camera = GetComponent(); 29 | this._camera.Init(); 30 | 31 | // setup ROS 32 | this._ros = ROSConnection.instance; 33 | this._topicName += "/compressed"; 34 | this._ros.RegisterPublisher(this._topicName); 35 | 36 | // setup ROS Message 37 | this._message = new CompressedImageMsg(); 38 | this._message.header.frame_id = this._frameId; 39 | this._message.format = "jpeg"; 40 | } 41 | 42 | void Update() 43 | { 44 | this._timeElapsed += Time.deltaTime; 45 | 46 | if(this._timeElapsed > (1f/this._camera.scanRate)) 47 | { 48 | // Update ROS Message 49 | # if ROS2 50 | int sec = (int)Math.Truncate(this._timeStamp); 51 | # else 52 | uint sec = (uint)Math.Truncate(this._timeStamp); 53 | # endif 54 | uint nanosec = (uint)( (this._timeStamp - sec)*1e+9 ); 55 | this._message.header.stamp.sec = sec; 56 | this._message.header.stamp.nanosec = nanosec; 57 | this._message.data = this._camera.data; 58 | this._ros.Send(this._topicName, this._message); 59 | 60 | // Update time 61 | this._timeElapsed = 0; 62 | this._timeStamp = Time.time; 63 | } 64 | } 65 | } 66 | -------------------------------------------------------------------------------- /Scripts/Runtime/RGBCamera/RGBCameraPublisher.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: dd213003aa9dc2016a26bb3080d05265 3 | MonoImporter: 4 | externalObjects: {} 5 | serializedVersion: 2 6 | defaultReferences: [] 7 | executionOrder: 0 8 | icon: {instanceID: 0} 9 | userData: 10 | assetBundleName: 11 | assetBundleVariant: 12 | -------------------------------------------------------------------------------- /Scripts/Runtime/Velodyne.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 1547d4df3d8600630a8c4e840ce6f7e9 3 | folderAsset: yes 4 | DefaultImporter: 5 | externalObjects: {} 6 | userData: 7 | assetBundleName: 8 | assetBundleVariant: 9 | -------------------------------------------------------------------------------- /Scripts/Runtime/Velodyne/VLP16Publisher.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Collections; 3 | using System.Collections.Generic; 4 | using Unity.Burst; 5 | using Unity.Collections; 6 | using UnityEngine.Jobs; 7 | using Unity.Jobs; 8 | using UnityEngine; 9 | 10 | using Unity.Robotics.ROSTCPConnector; 11 | using RosMessageTypes.Velodyne; 12 | using RosMessageTypes.Std; 13 | 14 | [RequireComponent(typeof(FRJ.Sensor.RotateLidar))] 15 | public class VLP16Publisher : MonoBehaviour 16 | { 17 | 18 | [SerializeField] private string _topicName = "velodyne_packets"; 19 | [SerializeField] private string _frameId = "velodyne"; 20 | 21 | private JobHandle _handle; 22 | private float _timeElapsed = 0f; 23 | private float _timeStamp = 0f; 24 | 25 | private ROSConnection _ros; 26 | private VelodyneScanMsg _message; 27 | 28 | private FRJ.Sensor.RotateLidar _lidar; 29 | private FRJ.Sensor.VLP16Serializer _serializer; 30 | 31 | 32 | void Start() 33 | { 34 | // Get Rotate Lidar 35 | this._lidar = GetComponent(); 36 | this._lidar.Init(); 37 | 38 | // Setup serializer 39 | this._serializer = 40 | new FRJ.Sensor.VLP16Serializer(this._lidar.numOfLayers, 41 | this._lidar.numOfIncrements, 42 | this._lidar.minAzimuthAngle, 43 | this._lidar.maxAzimuthAngle); 44 | this._serializer.job.distances = this._lidar.distances; 45 | this._serializer.job.intensities = this._lidar.intensities; 46 | 47 | // setup ROS 48 | this._ros = ROSConnection.instance; 49 | this._ros.RegisterPublisher(this._topicName); 50 | 51 | // setup ROS Message 52 | this._message = new VelodyneScanMsg(); 53 | this._message.header.frame_id = this._frameId; 54 | this._message.packets = new VelodynePacketMsg[this._lidar.numOfIncrements/12]; 55 | for(int i=0; i (1f/this._lidar.scanRate)) { 74 | // Update ROS Message 75 | # if ROS2 76 | int sec = (int)Math.Truncate(this._timeStamp); 77 | # else 78 | uint sec = (uint)Math.Truncate(this._timeStamp); 79 | # endif 80 | uint nanosec = (uint)( (this._timeStamp - sec)*1e+9 ); 81 | this._serializer.job.timeStamp = this._timeStamp; 82 | this._message.header.stamp.sec = sec; 83 | this._message.header.stamp.nanosec = nanosec; 84 | for(int i=0; i