mirrors/livox_ros_driver
1
0
Fork 0
mirror of https://github.com/Livox-SDK/livox_ros_driver.git synced 2023-04-06 15:49:55 +08:00
Code Issues Packages Projects Releases Wiki Activity

feature:add livox_ros_driver package

Browse Source
This commit is contained in:
Livox-SDK
2019-04-08 12:20:20 +08:00
commit 0c517daff0
17 changed files with 2841 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
.gitignore vendored Normal file
View File

@@ -0,0 +1,9 @@
# ignore dir
build/
devel/
.catkin_workspace
#whitelist

678
LICENSE.txt Normal file
View File

@@ -0,0 +1,678 @@
The following portions of the LIVOX’s Livox SDK (“Software” referred to in the terms below) are made available to you under the terms of the MIT License provided below.
Livox-SDK
├── CMakeLists.txt
├── sample
│   ├── hub
│   │   ├── CMakeLists.txt
│   │   └── main.c
│   └── lidar
│   ├── CMakeLists.txt
│   └── main.c
└── sdk_core
├── CMakeLists.txt
├── include
│   ├── comm
│   │   ├── comm_port.h
│   │   └── protocol.h
│   ├── livox_def.h
│   └── livox_sdk.h
└── src
├── base
│   ├── command_callback.h
│   ├── io_loop.cpp
│   ├── io_loop.h
│   ├── io_thread.cpp
│   ├── io_thread.h
│   ├── logging.h
│   ├── network_util.cpp
│   ├── network_util.h
│   ├── noncopyable.h
│   ├── thread_base.cpp
│   ├── thread_base.h
│   ├── util.cpp
│   └── util.h
├── comm
│   ├── comm_port.cpp
│   ├── sdk_protocol.cpp
│   └── sdk_protocol.h
├── command_handler
│   ├── command_channel.cpp
│   ├── command_channel.h
│   ├── command_handler.cpp
│   ├── command_handler.h
│   ├── command_impl.cpp
│   ├── command_impl.h
│   ├── hub_command_handler.cpp
│   ├── hub_command_handler.h
│   ├── lidar_command_handler.cpp
│   └── lidar_command_handler.h
├── data_handler
│   ├── data_handler.cpp
│   ├── data_handler.h
│   ├── hub_data_handler.cpp
│   ├── hub_data_handler.h
│   ├── lidar_data_handler.cpp
│   └── lidar_data_handler.h
├── device_discovery.cpp
├── device_discovery.h
├── device_manager.cpp
├── device_manager.h
└── livox_sdk.cpp
-------------------------------------------------------------
The MIT License (MIT)
Copyright (c) 2019 Livox. All rights reserved.
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
===============================================================
LIVOX’s Livox SDK uses unmodified libraries of Apache Portable Runtime Library (APR)(https://github.com/apache/apr), which is licensed under Apache license. A copy of the Apache license is provided below and is also available at https://raw.githubusercontent.com/apache/apr/trunk/LICENSE.
-------------------------------------------------------------
Apache License
Version 2.0, January 2004
http://www.apache.org/licenses/
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
1. Definitions.
"License" shall mean the terms and conditions for use, reproduction,
and distribution as defined by Sections 1 through 9 of this document.
"Licensor" shall mean the copyright owner or entity authorized by
the copyright owner that is granting the License.
"Legal Entity" shall mean the union of the acting entity and all
other entities that control, are controlled by, or are under common
control with that entity. For the purposes of this definition,
"control" means (i) the power, direct or indirect, to cause the
direction or management of such entity, whether by contract or
otherwise, or (ii) ownership of fifty percent (50%) or more of the
outstanding shares, or (iii) beneficial ownership of such entity.
"You" (or "Your") shall mean an individual or Legal Entity
exercising permissions granted by this License.
"Source" form shall mean the preferred form for making modifications,
including but not limited to software source code, documentation
source, and configuration files.
"Object" form shall mean any form resulting from mechanical
transformation or translation of a Source form, including but
not limited to compiled object code, generated documentation,
and conversions to other media types.
"Work" shall mean the work of authorship, whether in Source or
Object form, made available under the License, as indicated by a
copyright notice that is included in or attached to the work
(an example is provided in the Appendix below).
"Derivative Works" shall mean any work, whether in Source or Object
form, that is based on (or derived from) the Work and for which the
editorial revisions, annotations, elaborations, or other modifications
represent, as a whole, an original work of authorship. For the purposes
of this License, Derivative Works shall not include works that remain
separable from, or merely link (or bind by name) to the interfaces of,
the Work and Derivative Works thereof.
"Contribution" shall mean any work of authorship, including
the original version of the Work and any modifications or additions
to that Work or Derivative Works thereof, that is intentionally
submitted to Licensor for inclusion in the Work by the copyright owner
or by an individual or Legal Entity authorized to submit on behalf of
the copyright owner. For the purposes of this definition, "submitted"
means any form of electronic, verbal, or written communication sent
to the Licensor or its representatives, including but not limited to
communication on electronic mailing lists, source code control systems,
and issue tracking systems that are managed by, or on behalf of, the
Licensor for the purpose of discussing and improving the Work, but
excluding communication that is conspicuously marked or otherwise
designated in writing by the copyright owner as "Not a Contribution."
"Contributor" shall mean Licensor and any individual or Legal Entity
on behalf of whom a Contribution has been received by Licensor and
subsequently incorporated within the Work.
2. Grant of Copyright License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
copyright license to reproduce, prepare Derivative Works of,
publicly display, publicly perform, sublicense, and distribute the
Work and such Derivative Works in Source or Object form.
3. Grant of Patent License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
(except as stated in this section) patent license to make, have made,
use, offer to sell, sell, import, and otherwise transfer the Work,
where such license applies only to those patent claims licensable
by such Contributor that are necessarily infringed by their
Contribution(s) alone or by combination of their Contribution(s)
with the Work to which such Contribution(s) was submitted. If You
institute patent litigation against any entity (including a
cross-claim or counterclaim in a lawsuit) alleging that the Work
or a Contribution incorporated within the Work constitutes direct
or contributory patent infringement, then any patent licenses
granted to You under this License for that Work shall terminate
as of the date such litigation is filed.
4. Redistribution. You may reproduce and distribute copies of the
Work or Derivative Works thereof in any medium, with or without
modifications, and in Source or Object form, provided that You
meet the following conditions:
(a) You must give any other recipients of the Work or
Derivative Works a copy of this License; and
(b) You must cause any modified files to carry prominent notices
stating that You changed the files; and
(c) You must retain, in the Source form of any Derivative Works
that You distribute, all copyright, patent, trademark, and
attribution notices from the Source form of the Work,
excluding those notices that do not pertain to any part of
the Derivative Works; and
(d) If the Work includes a "NOTICE" text file as part of its
distribution, then any Derivative Works that You distribute must
include a readable copy of the attribution notices contained
within such NOTICE file, excluding those notices that do not
pertain to any part of the Derivative Works, in at least one
of the following places: within a NOTICE text file distributed
as part of the Derivative Works; within the Source form or
documentation, if provided along with the Derivative Works; or,
within a display generated by the Derivative Works, if and
wherever such third-party notices normally appear. The contents
of the NOTICE file are for informational purposes only and
do not modify the License. You may add Your own attribution
notices within Derivative Works that You distribute, alongside
or as an addendum to the NOTICE text from the Work, provided
that such additional attribution notices cannot be construed
as modifying the License.
You may add Your own copyright statement to Your modifications and
may provide additional or different license terms and conditions
for use, reproduction, or distribution of Your modifications, or
for any such Derivative Works as a whole, provided Your use,
reproduction, and distribution of the Work otherwise complies with
the conditions stated in this License.
5. Submission of Contributions. Unless You explicitly state otherwise,
any Contribution intentionally submitted for inclusion in the Work
by You to the Licensor shall be under the terms and conditions of
this License, without any additional terms or conditions.
Notwithstanding the above, nothing herein shall supersede or modify
the terms of any separate license agreement you may have executed
with Licensor regarding such Contributions.
6. Trademarks. This License does not grant permission to use the trade
names, trademarks, service marks, or product names of the Licensor,
except as required for reasonable and customary use in describing the
origin of the Work and reproducing the content of the NOTICE file.
7. Disclaimer of Warranty. Unless required by applicable law or
agreed to in writing, Licensor provides the Work (and each
Contributor provides its Contributions) on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
implied, including, without limitation, any warranties or conditions
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
PARTICULAR PURPOSE. You are solely responsible for determining the
appropriateness of using or redistributing the Work and assume any
risks associated with Your exercise of permissions under this License.
8. Limitation of Liability. In no event and under no legal theory,
whether in tort (including negligence), contract, or otherwise,
unless required by applicable law (such as deliberate and grossly
negligent acts) or agreed to in writing, shall any Contributor be
liable to You for damages, including any direct, indirect, special,
incidental, or consequential damages of any character arising as a
result of this License or out of the use or inability to use the
Work (including but not limited to damages for loss of goodwill,
work stoppage, computer failure or malfunction, or any and all
other commercial damages or losses), even if such Contributor
has been advised of the possibility of such damages.
9. Accepting Warranty or Additional Liability. While redistributing
the Work or Derivative Works thereof, You may choose to offer,
and charge a fee for, acceptance of support, warranty, indemnity,
or other liability obligations and/or rights consistent with this
License. However, in accepting such obligations, You may act only
on Your own behalf and on Your sole responsibility, not on behalf
of any other Contributor, and only if You agree to indemnify,
defend, and hold each Contributor harmless for any liability
incurred by, or claims asserted against, such Contributor by reason
of your accepting any such warranty or additional liability.
END OF TERMS AND CONDITIONS
APPENDIX: How to apply the Apache License to your work.
To apply the Apache License to your work, attach the following
boilerplate notice, with the fields enclosed by brackets "[]"
replaced with your own identifying information. (Don't include
the brackets!) The text should be enclosed in the appropriate
comment syntax for the file format. We also recommend that a
file or class name and description of purpose be included on the
same "printed page" as the copyright notice for easier
identification within third-party archives.
Copyright [yyyy] [name of copyright owner]
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
APACHE PORTABLE RUNTIME SUBCOMPONENTS:
The Apache Portable Runtime includes a number of subcomponents with
separate copyright notices and license terms. Your use of the source
code for these subcomponents is subject to the terms and conditions
of the following licenses.
From strings/apr_fnmatch.c, include/apr_fnmatch.h, misc/unix/getopt.c,
file_io/unix/mktemp.c, strings/apr_strings.c:
/*
* Copyright (c) 1987, 1993, 1994
* The Regents of the University of California. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* 3. All advertising materials mentioning features or use of this software
* must display the following acknowledgement:
* This product includes software developed by the University of
* California, Berkeley and its contributors.
* 4. Neither the name of the University nor the names of its contributors
* may be used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
* OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
* SUCH DAMAGE.
From network_io/unix/inet_ntop.c, network_io/unix/inet_pton.c:
/* Copyright (c) 1996 by Internet Software Consortium.
*
* Permission to use, copy, modify, and distribute this software for any
* purpose with or without fee is hereby granted, provided that the above
* copyright notice and this permission notice appear in all copies.
*
* THE SOFTWARE IS PROVIDED "AS IS" AND INTERNET SOFTWARE CONSORTIUM DISCLAIMS
* ALL WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES
* OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL INTERNET SOFTWARE
* CONSORTIUM BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL
* DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR
* PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS
* ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS
* SOFTWARE.
From dso/aix/dso.c:
* Based on libdl (dlfcn.c/dlfcn.h) which is
* Copyright (c) 1992,1993,1995,1996,1997,1988
* Jens-Uwe Mager, Helios Software GmbH, Hannover, Germany.
*
* Not derived from licensed software.
*
* Permission is granted to freely use, copy, modify, and redistribute
* this software, provided that the author is not construed to be liable
* for any results of using the software, alterations are clearly marked
* as such, and this notice is not modified.
From strings/apr_strnatcmp.c, include/apr_strings.h:
strnatcmp.c -- Perform 'natural order' comparisons of strings in C.
Copyright (C) 2000 by Martin Pool <mbp@humbug.org.au>
This software is provided 'as-is', without any express or implied
warranty. In no event will the authors be held liable for any damages
arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it
freely, subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not
claim that you wrote the original software. If you use this software
in a product, an acknowledgment in the product documentation would be
appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be
misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
From strings/apr_snprintf.c:
*
* cvt - IEEE floating point formatting routines.
* Derived from UNIX V7, Copyright(C) Caldera International Inc.
*
Copyright(C) Caldera International Inc. 2001-2002. All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are
met:
Redistributions of source code and documentation must retain the above
copyright notice, this list of conditions and the following disclaimer.
Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
All advertising materials mentioning features or use of this software
must display the following acknowledgement:
This product includes software developed or owned by Caldera
International, Inc.
Neither the name of Caldera International, Inc. nor the names of other
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
USE OF THE SOFTWARE PROVIDED FOR UNDER THIS LICENSE BY CALDERA
INTERNATIONAL, INC. AND CONTRIBUTORS ``AS IS'' AND ANY EXPRESS OR IMPLIED
WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN
NO EVENT SHALL CALDERA INTERNATIONAL, INC. BE LIABLE FOR ANY DIRECT,
INDIRECT INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.
For the include\apr_md5.h component:
* This is work is derived from material Copyright RSA Data Security, Inc.
*
* The RSA copyright statement and Licence for that original material is
* included below. This is followed by the Apache copyright statement and
* licence for the modifications made to that material.
Copyright (C) 1991-2, RSA Data Security, Inc. Created 1991. All
rights reserved.
License to copy and use this software is granted provided that it
is identified as the "RSA Data Security, Inc. MD5 Message-Digest
Algorithm" in all material mentioning or referencing this software
or this function.
License is also granted to make and use derivative works provided
that such works are identified as "derived from the RSA Data
Security, Inc. MD5 Message-Digest Algorithm" in all material
mentioning or referencing the derived work.
RSA Data Security, Inc. makes no representations concerning either
the merchantability of this software or the suitability of this
software for any particular purpose. It is provided "as is"
without express or implied warranty of any kind.
These notices must be retained in any copies of any part of this
documentation and/or software.
For the passwd\apr_md5.c component:
* This is work is derived from material Copyright RSA Data Security, Inc.
*
* The RSA copyright statement and Licence for that original material is
* included below. This is followed by the Apache copyright statement and
* licence for the modifications made to that material.
Copyright (C) 1991-2, RSA Data Security, Inc. Created 1991. All
rights reserved.
License to copy and use this software is granted provided that it
is identified as the "RSA Data Security, Inc. MD5 Message-Digest
Algorithm" in all material mentioning or referencing this software
or this function.
License is also granted to make and use derivative works provided
that such works are identified as "derived from the RSA Data
Security, Inc. MD5 Message-Digest Algorithm" in all material
mentioning or referencing the derived work.
RSA Data Security, Inc. makes no representations concerning either
the merchantability of this software or the suitability of this
software for any particular purpose. It is provided "as is"
without express or implied warranty of any kind.
These notices must be retained in any copies of any part of this
documentation and/or software.
* The apr_md5_encode() routine uses much code obtained from the FreeBSD 3.0
* MD5 crypt() function, which is licenced as follows:
* ----------------------------------------------------------------------------
* "THE BEER-WARE LICENSE" (Revision 42):
* <phk@login.dknet.dk> wrote this file. As long as you retain this notice you
* can do whatever you want with this stuff. If we meet some day, and you think
* this stuff is worth it, you can buy me a beer in return. Poul-Henning Kamp
* ----------------------------------------------------------------------------
For the crypto\apr_md4.c component:
* Copyright (C) 1991-2, RSA Data Security, Inc. Created 1991. All
* rights reserved.
*
* License to copy and use this software is granted provided that it
* is identified as the "RSA Data Security, Inc. MD4 Message-Digest
* Algorithm" in all material mentioning or referencing this software
* or this function.
*
* License is also granted to make and use derivative works provided
* that such works are identified as "derived from the RSA Data
* Security, Inc. MD4 Message-Digest Algorithm" in all material
* mentioning or referencing the derived work.
*
* RSA Data Security, Inc. makes no representations concerning either
* the merchantability of this software or the suitability of this
* software for any particular purpose. It is provided "as is"
* without express or implied warranty of any kind.
*
* These notices must be retained in any copies of any part of this
* documentation and/or software.
For the crypto\crypt_blowfish.c(.h) component:
* Written by Solar Designer <solar at openwall.com> in 1998-2011.
* No copyright is claimed, and the software is hereby placed in the public
* domain. In case this attempt to disclaim copyright and place the software
* in the public domain is deemed null and void, then the software is
* Copyright (c) 1998-2011 Solar Designer and it is hereby released to the
* general public under the following terms:
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted.
*
* There's ABSOLUTELY NO WARRANTY, express or implied.
See crypto/crypt_blowfish.c for more information.
For the include\apr_md4.h component:
* This is derived from material copyright RSA Data Security, Inc.
* Their notice is reproduced below in its entirety.
*
* Copyright (C) 1991-2, RSA Data Security, Inc. Created 1991. All
* rights reserved.
*
* License to copy and use this software is granted provided that it
* is identified as the "RSA Data Security, Inc. MD4 Message-Digest
* Algorithm" in all material mentioning or referencing this software
* or this function.
*
* License is also granted to make and use derivative works provided
* that such works are identified as "derived from the RSA Data
* Security, Inc. MD4 Message-Digest Algorithm" in all material
* mentioning or referencing the derived work.
*
* RSA Data Security, Inc. makes no representations concerning either
* the merchantability of this software or the suitability of this
* software for any particular purpose. It is provided "as is"
* without express or implied warranty of any kind.
*
* These notices must be retained in any copies of any part of this
* documentation and/or software.
For the test\testmd4.c component:
* This is derived from material copyright RSA Data Security, Inc.
* Their notice is reproduced below in its entirety.
*
* Copyright (C) 1990-2, RSA Data Security, Inc. Created 1990. All
* rights reserved.
*
* RSA Data Security, Inc. makes no representations concerning either
* the merchantability of this software or the suitability of this
* software for any particular purpose. It is provided "as is"
* without express or implied warranty of any kind.
*
* These notices must be retained in any copies of any part of this
* documentation and/or software.
===============================================================
LIVOX’s Livox SDK uses unmodified libraries of Boost (https://www.boost.org), which is licensed under the Boost Software license. A copy of the Boost Software license is provided below and is also available at https://www.boost.org/LICENSE_1_0.txt.
-------------------------------------------------------------
Boost Software License - Version 1.0 - August 17th, 2003
Permission is hereby granted, free of charge, to any person or organization
obtaining a copy of the software and accompanying documentation covered by
this license (the "Software") to use, reproduce, display, distribute,
execute, and transmit the Software, and to prepare derivative works of the
Software, and to permit third-parties to whom the Software is furnished to
do so, all subject to the following:
The copyright notices in the Software and this entire statement, including
the above license grant, this restriction and the following disclaimer,
must be included in all copies of the Software, in whole or in part, and
all derivative works of the Software, unless such copies or derivative
works are solely in the form of machine-executable object code generated by
a source language processor.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
DEALINGS IN THE SOFTWARE.
===============================================================
FastCRC
└── sdk_core
├── include
│   └── third_party
│   └── FastCRC
│   └── FastCRC.h
└── src
└── third_party
└── FastCRC
├── FastCRCsw.cpp
├── FastCRC_tables.hpp
├── LICENSE.md
└── README.md
LIVOX’s Livox SDK uses modified libraries of FastCRC (https://github.com/FrankBoesing/FastCRC), which is licensed under MIT license. A copy of the MIT license is provided below and is also available at https://raw.githubusercontent.com/FrankBoesing/FastCRC/master/LICENCE.md.
The MIT License (MIT)
Copyright (c) 2016 Frank
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
===============================================================
LIVOX’s Livox SDK uses unmodified libraries of Ros (https://github.com/ros/ros), which is licensed under BSD license. A copy of the BSD license is provided below and is also available at https://opensource.org/licenses/BSD-3-Clause.
-------------------------------------------------------------
Copyright (c) 2001 - 2009, The Board of Trustees of the University of Illinois.
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are
met:
* Redistributions of source code must retain the above
copyright notice, this list of conditions and the
following disclaimer.
* Redistributions in binary form must reproduce the
above copyright notice, this list of conditions
and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of the University of Illinois
nor the names of its contributors may be used to
endorse or promote products derived from this
software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

69
README.md Normal file
View File

@@ -0,0 +1,69 @@
# livox ros driver
### Run livox ros driver
livox_ros_driver is a new ros package under the Livox-SDK/Livox-SDK-ROS/src directory, which is designed to gradually become the standard driver package for livox devices in the ros environment. The driver offers users a wealth of options:
1. Publish pointcloud2 format point cloud and automatically load rviz；
for example：
```
roslaunch livox_ros_driver livox_lidar_rviz.launch bd_list:="broadcast_code1&broadcast_code2&broadcast_code3"
```
or
```
roslaunch livox_ros_driver livox_hub_rviz.launch bd_list:="hub_broadcast_code"
```
2. Publish pointcloud2 format point cloud only；
for example：
```
roslaunch livox_ros_driver livox_lidar.launch bd_list:="broadcast_code1&broadcast_code2&broadcast_code3"
```
or
```
roslaunch livox_ros_driver livox_hub.launch bd_list:="hub_broadcast_code"
```
3. Publish livox custom format point cloud；
for example：
```
roslaunch livox_ros_driver livox_lidar_msg.launch bd_list:="broadcast_code1&broadcast_code2&broadcast_code3"
```
or
```
roslaunch livox_ros_driver livox_hub_msg.launch bd_list:="hub_broadcast_code"
```
livox custom msg format：
```
Header header # ROS standard message header
uint64 timebase # The time of first point
uint32 timestep # Time interval between adjacent point clouds
uint32 point_num # Total number of pointclouds
uint8 lidar_id # Lidar device id number
uint8[3] rsvd # Reserved use
CustomPoint[] points # Pointcloud data
```
pointcloud format:
```
float32 x # X axis, unit:m
float32 y # Y axis, unit:m
float32 z # Z axis, unit:m
uint8 reflectivity # reflectivity, 0~255
uint8 line # laser number in lidar
```

262
livox_ros_driver/CMakeLists.txt Executable file
View File

@@ -0,0 +1,262 @@
cmake_minimum_required(VERSION 2.8.3)
project(livox_ros_driver)
set(PROJECT_LIDAR livox_lidar)
set(PROJECT_HUB livox_hub)
set(PROJECT_LIDAR_SRC ./livox_lidar.cpp)
set(PROJECT_HUB_SRC ./livox_hub.cpp)
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
add_definitions(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
message_generation
)
find_package(Boost REQUIRED)
find_package(Boost REQUIRED COMPONENTS thread)
## get pointcloud package
find_package(PCL REQUIRED)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a run_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
add_message_files(
FILES
CustomPoint.msg
CustomMsg.msg
# Message2.msg
)
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
std_msgs
)
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES lidar_sdk
CATKIN_DEPENDS roscpp rospy std_msgs message_runtime
DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
${catkin_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
)
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/lidar_sdk.cpp
# )
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
##SET(SRC_DIR
## ./
##)
# traverse all source directories and add include dir
##FOREACH(dir ${SRC_DIR})
## MESSAGE(STATUS"src_dir: "${dir})
## include_directories(./include/${dir})
## include_directories(./${dir})
## AUX_SOURCE_DIRECTORY(${dir} source_list)
##ENDFOREACH()
find_package(PkgConfig)
pkg_check_modules(APR apr-1)
if (APR_FOUND)
message(${APR_INCLUDE_DIRS})
message(${APR_LIBRARIES})
endif (APR_FOUND)
include_directories(
./
${APR_INCLUDE_DIRS}
)
## PCL library
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
add_executable(${PROJECT_LIDAR}_node
${PROJECT_LIDAR_SRC})
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
target_link_libraries(${PROJECT_LIDAR}_node
livox_sdk_static.a
${APR_LIBRARIES}
${PCL_LIBRARIES}
${catkin_LIBRARIES}
${Boost_LIBRARIES}
-lrt
)
## add hub project here
add_executable(${PROJECT_HUB}_node
${PROJECT_HUB_SRC})
target_link_libraries(${PROJECT_HUB}_node
livox_sdk_static.a
${APR_LIBRARIES}
${PCL_LIBRARIES}
${catkin_LIBRARIES}
${Boost_LIBRARIES}
-lrt
)
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_lidar_sdk.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()

189
livox_ros_driver/config/display_hub_points.rviz Normal file
View File

@@ -0,0 +1,189 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /Grid1
- /PointCloud1/Autocompute Value Bounds1
- /PointCloud21
Splitter Ratio: 0.500695
Tree Height: 747
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: PointCloud2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.03
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 0.856
Min Value: -0.735
Value: true
Axis: Z
Channel Name: x
Class: rviz/PointCloud
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 1
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: -0.088
Min Color: 0; 0; 0
Min Intensity: -1.951
Name: PointCloud
Position Transformer: XYZ
Queue Size: 1000
Selectable: true
Size (Pixels): 2
Size (m): 0.005
Style: Flat Squares
Topic: /livox/lidar
Unreliable: true
Use Fixed Frame: true
Use rainbow: true
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 0.816
Min Value: -0.674
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 1
Enabled: true
Invert Rainbow: true
Max Color: 255; 255; 255
Max Intensity: 151
Min Color: 0; 0; 0
Min Intensity: 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 1000
Selectable: true
Size (Pixels): 3
Size (m): 0.005
Style: Flat Squares
Topic: /livox/hub
Unreliable: true
Use Fixed Frame: true
Use rainbow: true
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: livox_frame
Frame Rate: 20
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 1.61081
Enable Stereo Rendering:
Stereo Eye Separation: 0.06
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.267255
Y: 0.0618536
Z: 0.150874
Name: Current View
Near Clip Distance: 0.01
Pitch: 0.3048
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 3.01742
Saved:
- Class: rviz/Orbit
Distance: 10
Enable Stereo Rendering:
Stereo Eye Separation: 0.06
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0
Y: 0
Z: 0
Name: Orbit
Near Clip Distance: 0.01
Pitch: 1.1104
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 0.570397
Window Geometry:
Displays:
collapsed: false
Height: 1028
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd0000000400000000000001830000037afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000037a000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005110000003efc0100000002fb0000000800540069006d0065010000000000000511000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003880000037a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 1297
X: 209
Y: 14

189
livox_ros_driver/config/display_lidar_points.rviz Normal file
View File

@@ -0,0 +1,189 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /Grid1
- /PointCloud1/Autocompute Value Bounds1
- /PointCloud21
Splitter Ratio: 0.500695
Tree Height: 747
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: PointCloud2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.03
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 0.856
Min Value: -0.735
Value: true
Axis: Z
Channel Name: x
Class: rviz/PointCloud
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 1
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: -0.088
Min Color: 0; 0; 0
Min Intensity: -1.951
Name: PointCloud
Position Transformer: XYZ
Queue Size: 1000
Selectable: true
Size (Pixels): 2
Size (m): 0.005
Style: Flat Squares
Topic: /livox/lidar
Unreliable: true
Use Fixed Frame: true
Use rainbow: true
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 0.653
Min Value: -0.918
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: AxisColor
Decay Time: 1
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 151
Min Color: 0; 0; 0
Min Intensity: 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 100
Selectable: true
Size (Pixels): 3
Size (m): 0.005
Style: Flat Squares
Topic: /livox/lidar
Unreliable: true
Use Fixed Frame: true
Use rainbow: true
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: livox_frame
Frame Rate: 20
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 0.591995
Enable Stereo Rendering:
Stereo Eye Separation: 0.06
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.267255
Y: 0.0618536
Z: 0.150874
Name: Current View
Near Clip Distance: 0.01
Pitch: 0.229799
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 2.99561
Saved:
- Class: rviz/Orbit
Distance: 10
Enable Stereo Rendering:
Stereo Eye Separation: 0.06
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0
Y: 0
Z: 0
Name: Orbit
Near Clip Distance: 0.01
Pitch: 1.1104
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 0.570397
Window Geometry:
Displays:
collapsed: false
Height: 1028
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd0000000400000000000001830000037afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000037a000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005110000003efc0100000002fb0000000800540069006d0065010000000000000511000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003880000037a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 1297
X: 407
Y: 14

14
livox_ros_driver/launch/livox_hub.launch Normal file
View File

@@ -0,0 +1,14 @@
<launch>
<arg name="bd_list" default="100000000000000"/>
<param name="livox_msg_type" value="0"/>
<node name="livox_hub_publisher" pkg="livox_ros_driver"
type="livox_hub_node" required="true"
output="screen" args="$(arg bd_list)"/>
<!--
<node name="rviz" pkg="rviz" type="rviz" respawn="true"
args="-d $(find livox_ros_driver)/config/display_hub_points.rviz"/>
-->
</launch>

14
livox_ros_driver/launch/livox_hub_msg.launch Normal file
View File

@@ -0,0 +1,14 @@
<launch>
<arg name="bd_list" default="100000000000000"/>
<param name="livox_msg_type" value="0"/>
<node name="livox_hub_publisher" pkg="livox_ros_driver"
type="livox_hub_node" required="true"
output="screen" args="$(arg bd_list)"/>
<!--
<node name="rviz" pkg="rviz" type="rviz" respawn="true"
args="-d $(find livox_ros_driver)/config/display_hub_points.rviz"/>
-->
</launch>

12
livox_ros_driver/launch/livox_hub_rviz.launch Normal file
View File

@@ -0,0 +1,12 @@
<launch>
<arg name="bd_list" default="100000000000000"/>
<param name="livox_msg_type" value="0"/>
<node name="livox_hub_publisher" pkg="livox_ros_driver"
type="livox_hub_node" required="true"
output="screen" args="$(arg bd_list)"/>
<node name="rviz" pkg="rviz" type="rviz" respawn="true"
args="-d $(find livox_ros_driver)/config/display_hub_points.rviz"/>
</launch>

13
livox_ros_driver/launch/livox_lidar.launch Normal file
View File

@@ -0,0 +1,13 @@
<launch>
<arg name="bd_list" default="100000000000000"/>
<param name="livox_msg_type" value="0"/>
<node name="livox_lidar_publisher" pkg="livox_ros_driver"
type="livox_lidar_node" required="true"
output="screen" args="$(arg bd_list)"/>
<!--
<node name="rviz" pkg="rviz" type="rviz" respawn="true"
args="-d $(find livox_ros_driver)/config/display_lidar_points.rviz"/>
-->
</launch>

13
livox_ros_driver/launch/livox_lidar_msg.launch Normal file
View File

@@ -0,0 +1,13 @@
<launch>
<arg name="bd_list" default="100000000000000"/>
<param name="livox_msg_type" value="1"/>
<node name="livox_lidar_publisher" pkg="livox_ros_driver"
type="livox_lidar_node" required="true"
output="screen" args="$(arg bd_list)"/>
<!--
<node name="rviz" pkg="rviz" type="rviz" respawn="true"
args="-d $(find livox_ros_driver)/config/display_lidar_points.rviz"/>
-->
</launch>

12
livox_ros_driver/launch/livox_lidar_rviz.launch Normal file
View File

@@ -0,0 +1,12 @@
<launch>
<arg name="bd_list" default="100000000000000"/>
<param name="livox_msg_type" value="0"/>
<node name="livox_lidar_publisher" pkg="livox_ros_driver"
type="livox_lidar_node" required="true"
output="screen" args="$(arg bd_list)"/>
<node name="rviz" pkg="rviz" type="rviz" respawn="true"
args="-d $(find livox_ros_driver)/config/display_lidar_points.rviz"/>
</launch>

661
livox_ros_driver/livox_hub.cpp Executable file
View File

@@ -0,0 +1,661 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <stdint.h>
#include <math.h>
#include <time.h>
#include <vector>
#include <chrono>
#include "livox_sdk.h"
#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <livox_ros_driver/CustomPoint.h>
#include <livox_ros_driver/CustomMsg.h>
#define kMaxPointPerEthPacket (100)
#define kMaxStoragePackets (128) // must be 2^n
#define KEthPacketHeaderLength (18) //(sizeof(LivoxEthPacket) - 1)
#define KEthPacketMaxLength (1500)
#define KCartesianPointSize (13)
#define KSphericalPointSzie (9)
const uint64_t kPacketTimeGap = 1000000; // 1ms = 1000000ns
const uint64_t kMaxPacketTimeGap = 1700000; // the threshold of packet continuous
const uint64_t kNsPerSecond = 1000000000; // 1s = 1000000000ns
const uint32_t kPublishIntervalMs = 50; // unit:ms
#define BD_ARGC_NUM (4)
#define BD_ARGV_POS (1)
#define COMMANDLINE_BD_SIZE (15)
typedef pcl::PointCloud<pcl::PointXYZI> PointCloud;
#pragma pack(1)
typedef struct {
uint64_t time_rcv; // used for pps sync only mode
uint32_t point_num;
uint8_t raw_data[KEthPacketMaxLength];
} StoragePacket;
typedef struct {
uint8_t lidar_id;
uint8_t rsvd[3];
uint32_t point_num;
uint64_t timestamp; // ns
LivoxPoint *point;
} PublishPacket;
#pragma pack()
typedef struct {
StoragePacket *storage_packet[kMaxStoragePackets]; // size must be 2^n
volatile uint32_t rd_idx;
volatile uint32_t wr_idx;
uint32_t mask;
uint32_t size; // must be 2^n
} StoragePacketQueue;
typedef struct {
uint32_t receive_packet_count;
uint32_t loss_packet_count;
uint64_t last_timestamp;
uint64_t timebase; // unit:nanosecond
uint32_t timebase_state;
} LidarPacketStatistic;
typedef enum {
kCoordinateCartesian = 0,
kCoordinateSpherical
} CoordinateType;
typedef enum {
kPointCloud2Msg = 0,
kLivoxCustomMsg
} LivoxMsgType;
StoragePacketQueue storage_packet_pool[kMaxLidarCount];
/* for global publisher use */
ros::Publisher cloud_pub;
/* for device connect use ----------------------------------------------------------------------- */
typedef enum {
kDeviceStateDisconnect = 0,
kDeviceStateConnect = 1,
kDeviceStateSampling = 2,
} DeviceState;
typedef struct {
uint8_t handle;
DeviceState device_state;
DeviceInfo info;
LidarPacketStatistic statistic_info;
} DeviceItem;
DeviceItem lidars[kMaxLidarCount];
DeviceItem hub;
/* user add hub broadcast code here */
const char* broadcast_code_list[] = {
"000000000000001",
};
#define BROADCAST_CODE_LIST_SIZE (sizeof(broadcast_code_list) / sizeof(intptr_t))
/* total broadcast code, include broadcast_code_list and commandline input */
std::vector<std::string > total_broadcast_code;
/* for pointcloud queue process */
void InitQueue(StoragePacketQueue* queue) {
queue->rd_idx = 0;
queue->wr_idx = 0;
queue->size = kMaxStoragePackets;
queue->mask = kMaxStoragePackets - 1;
for (int i=0; i<kMaxStoragePackets; i++) {
queue->storage_packet[i] = new StoragePacket;
}
}
void ResetQueue(StoragePacketQueue* queue) {
queue->rd_idx = 0;
queue->wr_idx = 0;
}
void InitStoragePacketPool(void) {
for (int i=0; i<kMaxLidarCount; i++) {
InitQueue(&storage_packet_pool[i]);
}
}
uint32_t QueuePop(StoragePacketQueue* queue, StoragePacket* storage_packet) {
uint32_t mask = queue->mask;
uint32_t rd_idx = queue->rd_idx & mask;
memcpy(storage_packet, queue->storage_packet[rd_idx], sizeof(StoragePacket));
queue->rd_idx++;
return 1;
}
uint32_t QueueUsedSize(StoragePacketQueue *queue) {
return (queue->wr_idx - queue->rd_idx) & queue->mask;
}
uint32_t QueueIsFull(StoragePacketQueue *queue) {
return ((queue->wr_idx + 1) == queue->rd_idx);
}
uint32_t QueueIsEmpty(StoragePacketQueue *queue) {
return (queue->rd_idx == queue->wr_idx);
}
static uint32_t GetEthPacketLen(LivoxEthPacket* eth_packet, uint32_t point_num) {
if (kCoordinateCartesian == eth_packet->data_type) {
return (KEthPacketHeaderLength + point_num * KCartesianPointSize);
} else {
return (KEthPacketHeaderLength + point_num * KSphericalPointSzie);
}
}
uint32_t PushEthPacketToStorageQueue(StoragePacketQueue* queue, LivoxEthPacket* eth_packet, \
uint32_t point_num, uint64_t timebase) {
uint32_t mask = queue->mask;
uint32_t wr_idx = queue->wr_idx & mask;
queue->storage_packet[wr_idx]->time_rcv = timebase;
queue->storage_packet[wr_idx]->point_num = point_num;
memcpy(queue->storage_packet[wr_idx]->raw_data, \
reinterpret_cast<char *>(eth_packet), \
GetEthPacketLen(eth_packet, point_num));
queue->wr_idx++;
return 1;
}
static uint64_t GetStoragePacketTimestamp(StoragePacket* packet) {
LivoxEthPacket* raw_packet = reinterpret_cast<LivoxEthPacket *>(packet->raw_data);
uint64_t timestamp = *((uint64_t *)(raw_packet->timestamp));
if (raw_packet->timestamp_type == kTimestampTypePps) {
return (timestamp + packet->time_rcv);
} else if (raw_packet->timestamp_type == kTimestampTypeNoSync) {
return timestamp;
} else if (raw_packet->timestamp_type == kTimestampTypePtp) {
return timestamp;
} else if (raw_packet->timestamp_type == kTimestampTypePpsGps) {
struct tm time_utc;
time_utc.tm_isdst = 0;
time_utc.tm_year = raw_packet->timestamp[0] + 100; // map 2000 to 1990
time_utc.tm_mon = raw_packet->timestamp[1];
time_utc.tm_mday = raw_packet->timestamp[2];
time_utc.tm_hour = raw_packet->timestamp[3];
time_utc.tm_min = 0;
time_utc.tm_sec = 0;
//ROS_INFO("UTC:%d %d %d %d", time_utc.tm_year, time_utc.tm_mon,\
// time_utc.tm_mday, time_utc.tm_hour);
uint64_t time_epoch = mktime(&time_utc);
time_epoch = time_epoch * 1000000 + *((uint32_t *)(&(raw_packet->timestamp[4]))); // to us
time_epoch = time_epoch * 1000; // to ns
return time_epoch;
} else {
ROS_INFO("timestamp type invalid");
return 0;
}
}
/* for pointcloud convert process */
static uint32_t PublishPointcloud2(StoragePacketQueue* queue, uint32_t packet_num, \
uint8_t handle) {
uint64_t timestamp = 0;
uint64_t last_timestamp = 0;
uint32_t published_packet = 0;
/* init point cloud data struct */
PointCloud::Ptr cloud (new PointCloud);
cloud->header.frame_id = "livox_frame";
cloud->height = 1;
cloud->width = 0;
StoragePacket storage_packet;
while (published_packet < packet_num) {
QueuePop(queue, &storage_packet);
LivoxEthPacket* raw_packet = reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
LivoxRawPoint* raw_points = reinterpret_cast<LivoxRawPoint *>(raw_packet->data);
timestamp = GetStoragePacketTimestamp(&storage_packet);
if (published_packet && \
((timestamp - last_timestamp) > kMaxPacketTimeGap)) {
ROS_INFO("packet loss : %ld", timestamp);
break;
}
if (!cloud->width) {
cloud->header.stamp = timestamp / 1000; // to us
ROS_DEBUG("[%d]:%ld us", handle, cloud->header.stamp);
}
cloud->width += storage_packet.point_num;
for (uint32_t i = 0; i < storage_packet.point_num; i++) {
pcl::PointXYZI point;
point.x = raw_points->x/1000.0f;
point.y = raw_points->y/1000.0f;
point.z = raw_points->z/1000.0f;
point.intensity = (float) raw_points->reflectivity;
++raw_points;
cloud->points.push_back(point);
}
last_timestamp = timestamp;
++published_packet;
}
//ROS_INFO("%d", cloud->width);
cloud_pub.publish(cloud);
return published_packet;
}
/* for pointcloud convert process */
static uint32_t PublishCustomPointcloud(StoragePacketQueue* queue, uint32_t packet_num,\
uint8_t handle) {
static uint32_t msg_seq = 0;
uint64_t timestamp = 0;
uint64_t last_timestamp = 0;
uint32_t published_packet = 0;
/* init livox custom msg */
livox_ros_driver::CustomMsg livox_msg;
livox_msg.header.frame_id = "livox_frame";
livox_msg.header.seq = msg_seq;
++msg_seq;
livox_msg.header.stamp = ros::Time::now();
livox_msg.timebase = 0;
livox_msg.timestep = 10000; // 10us = 10^4ns;
livox_msg.point_num = 0;
livox_msg.lidar_id = handle;
StoragePacket storage_packet;
while (published_packet < packet_num) {
QueuePop(queue, &storage_packet);
LivoxEthPacket* raw_packet = reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
LivoxRawPoint* raw_points = reinterpret_cast<LivoxRawPoint *>(raw_packet->data);
timestamp = GetStoragePacketTimestamp(&storage_packet);
if (published_packet && \
((timestamp - last_timestamp) > kMaxPacketTimeGap)) {
ROS_INFO("packet loss : %ld", timestamp);
break;
}
if (!livox_msg.timebase) {
livox_msg.timebase = timestamp; // to us
ROS_DEBUG("[%d]:%ld", handle, livox_msg.timebase);
}
livox_msg.point_num += storage_packet.point_num;
for (uint32_t i = 0; i < storage_packet.point_num; i++) {
livox_ros_driver::CustomPoint point;
point.x = raw_points->x/1000.0f;
point.y = raw_points->y/1000.0f;
point.z = raw_points->z/1000.0f;
point.reflectivity = raw_points->reflectivity;
point.line = 0;
++raw_points;
livox_msg.points.push_back(point);
}
last_timestamp = timestamp;
++published_packet;
}
//ROS_INFO("%d", livox_msg.point_num);
cloud_pub.publish(livox_msg);
return published_packet;
}
static void PointCloudConvert(LivoxPoint *p_dpoint, LivoxRawPoint *p_raw_point) {
p_dpoint->x = p_raw_point->x/1000.0f;
p_dpoint->y = p_raw_point->y/1000.0f;
p_dpoint->z = p_raw_point->z/1000.0f;
p_dpoint->reflectivity = p_raw_point->reflectivity;
}
void GetLidarData(uint8_t hub_handle, LivoxEthPacket *data, uint32_t data_num) {
using namespace std;
LivoxEthPacket *lidar_pack = data;
if (!data || !data_num) {
return;
}
/* caculate which lidar this eth packet data belong to */
uint8_t handle = HubGetLidarHandle(lidar_pack->slot, lidar_pack->id);
if (handle >= kMaxLidarCount) {
return;
}
LidarPacketStatistic *packet_statistic = &lidars[handle].statistic_info;
uint64_t cur_timestamp = *((uint64_t *)(lidar_pack->timestamp));
if (lidar_pack->timestamp_type == kTimestampTypePps) {
if ((cur_timestamp < packet_statistic->last_timestamp) && \
(cur_timestamp < kPacketTimeGap)) { // sync point
auto cur_time = chrono::high_resolution_clock::now();
int64_t sync_time = cur_time.time_since_epoch().count();
packet_statistic->timebase = sync_time;
//ROS_DEBUG("sysnc time : %lu %lu %lu", packet_statistic->timebase, cur_timestamp, \
// packet_statistic->last_timestamp);
}
}
packet_statistic->last_timestamp = cur_timestamp;
StoragePacketQueue *p_queue = &storage_packet_pool[handle];
if (!QueueIsFull(p_queue)) {
if (data_num <= kMaxPointPerEthPacket) {
PushEthPacketToStorageQueue(p_queue, lidar_pack, data_num, packet_statistic->timebase);
}
}
}
void PollPointcloudData(int msg_type) {
for (int i = 0; i < kMaxLidarCount; i++) {
StoragePacketQueue *p_queue = &storage_packet_pool[i];
if (kDeviceStateSampling != lidars[i].device_state) {
continue;
}
while (!QueueIsEmpty(p_queue)) {
//ROS_DEBUG("%d %d %d %d\r\n", i, p_queue->rd_idx, p_queue->wr_idx, QueueUsedSize(p_queue));
uint32_t used_size = QueueUsedSize(p_queue);
if (kPointCloud2Msg == msg_type) {
if (used_size == PublishPointcloud2(p_queue, used_size, i)) {
break;
}
} else {
if (used_size == PublishCustomPointcloud(p_queue, QueueUsedSize(p_queue), i)) {
break;
}
}
}
}
}
/** add bd to total_broadcast_code */
void AddBroadcastCode(const char* bd_str) {
total_broadcast_code.push_back(bd_str);
}
/** add bd in broadcast_code_list to total_broadcast_code */
void AddLocalBroadcastCode(void) {
for (int i = 0; i < BROADCAST_CODE_LIST_SIZE; ++i) {
std::string invalid_bd = "000000000";
ROS_INFO("broadcast code list :%s", broadcast_code_list[i]);
if ((COMMANDLINE_BD_SIZE == strlen(broadcast_code_list[i])) && \
(NULL == strstr(broadcast_code_list[i], invalid_bd.c_str()))) {
AddBroadcastCode(broadcast_code_list[i]);
} else {
ROS_INFO("Invalid bd:%s", broadcast_code_list[i]);
}
}
}
/** add commandline bd to total_broadcast_code */
void AddCommandlineBroadcastCode(const char* cammandline_str) {
char* strs = new char[strlen(cammandline_str) + 1];
strcpy(strs, cammandline_str);
std::string pattern = "&";
char* bd_str = strtok(strs, pattern.c_str());
std::string invalid_bd = "000000000";
while (bd_str != NULL) {
ROS_INFO("commandline input bd:%s", bd_str);
if ((COMMANDLINE_BD_SIZE == strlen(bd_str)) && \
(NULL == strstr(bd_str, invalid_bd.c_str()))) {
AddBroadcastCode(bd_str);
} else {
ROS_INFO("Invalid bd:%s", bd_str);
}
bd_str = strtok(NULL, pattern.c_str());
}
delete [] strs;
}
/* control hub ---------------------------------------------------------------------------------- */
void OnSampleCallback(uint8_t status, uint8_t hub_handle, uint8_t response, void *data) {
ROS_INFO("OnSampleCallback statue %d handle %d response %d \n", status, hub_handle, response);
if (status == kStatusSuccess) {
if (response != 0) {
hub.device_state = kDeviceStateConnect;
}
} else if (status == kStatusTimeout) {
hub.device_state = kDeviceStateConnect;
}
}
/** Callback function of stopping sampling. */
void OnStopSampleCallback(uint8_t status, uint8_t hub_handle, uint8_t response, void *data) {
}
/** Query the firmware version of Livox Hub. */
void OnDeviceInformation(uint8_t status, uint8_t handle, DeviceInformationResponse *ack, void *data) {
if (status != kStatusSuccess) {
ROS_INFO("Device Query Informations Failed %d", status);
}
if (ack) {
ROS_INFO("firm ver: %d.%d.%d.%d",
ack->firmware_version[0],
ack->firmware_version[1],
ack->firmware_version[2],
ack->firmware_version[3]);
}
}
void OnHubLidarInfo(uint8_t status, uint8_t handle, HubQueryLidarInformationResponse *response, void *client_data) {
if (status != kStatusSuccess) {
printf("Device Query Informations Failed %d\n", status);
}
if (response) {
int i = 0;
for (i = 0; i < response->count; ++i) {
printf("Hub Lidar Info broadcast code %s id %d slot %d \n ",
response->device_info_list[i].broadcast_code,
response->device_info_list[i].id,
response->device_info_list[i].slot);
}
}
}
void OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
if (info == NULL) {
return;
}
ROS_INFO("OnDeviceChange broadcast code %s update type %d", info->broadcast_code, type);
uint8_t hub_handle = info->handle;
if (hub_handle >= kMaxLidarCount) {
return;
}
if (type == kEventConnect) {
DeviceInfo *_lidars = (DeviceInfo *) malloc(sizeof(DeviceInfo) * kMaxLidarCount);
uint8_t count = kMaxLidarCount;
uint8_t status = GetConnectedDevices(_lidars, &count);
if (status == kStatusSuccess) {
int i = 0;
for (i = 0; i < count; ++i) {
uint8_t handle = _lidars[i].handle;
if (handle < kMaxLidarCount) {
lidars[handle].handle = handle;
lidars[handle].info = _lidars[i];
lidars[handle].device_state = kDeviceStateSampling;
ROS_INFO("lidar %d : %s\r\n", _lidars[i].handle, _lidars[i].broadcast_code);
}
}
}
if (_lidars) {
free(_lidars);
}
if (info->type == kDeviceTypeHub) {
HubQueryLidarInformation(OnHubLidarInfo, NULL);
}
if (hub.device_state == kDeviceStateDisconnect) {
hub.device_state = kDeviceStateConnect;
hub.info = *info;
}
} else if (type == kEventDisconnect) {
hub.device_state = kDeviceStateDisconnect;
} else if (type == kEventStateChange) {
hub.info = *info;
}
if (hub.device_state == kDeviceStateConnect) {
ROS_INFO("Device State status_code %d", hub.info.status.status_code);
ROS_INFO("Device State working state %d", hub.info.state);
ROS_INFO("Device feature %d", hub.info.feature);
if (hub.info.state == kLidarStateNormal) {
HubStartSampling(OnSampleCallback, NULL);
hub.device_state = kDeviceStateSampling;
}
}
}
void OnDeviceBroadcast(const BroadcastDeviceInfo *info) {
if (info == NULL) {
return;
}
ROS_INFO("Receive Broadcast Code %s, please add it to broacast_code_list if want to connect!\n",\
info->broadcast_code);
if (total_broadcast_code.size() > 0) {
bool found = false;
for (int i = 0; i < total_broadcast_code.size(); ++i) {
if (strncmp(info->broadcast_code, total_broadcast_code[i].c_str(), kBroadcastCodeSize) == 0) {
found = true;
break;
}
}
if (!found) {
ROS_INFO("Not in the broacast_code_list, please add it to if want to connect!");
return;
}
} else {
ROS_INFO("In automatic connection mode, will connect %s", info->broadcast_code);
}
bool result = false;
uint8_t hub_handle = 0;
result = AddHubToConnect(info->broadcast_code, &hub_handle);
if (result == kStatusSuccess && hub_handle < kMaxLidarCount) {
ROS_INFO("set callback");
SetDataCallback(hub_handle, GetLidarData);
hub.handle = hub_handle;
hub.device_state = kDeviceStateDisconnect;
}
}
int main(int argc, char **argv) {
if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info) ) {
ros::console::notifyLoggerLevelsChanged();
}
ROS_INFO("Livox-SDK ros demo");
InitStoragePacketPool();
if (!Init()) {
ROS_FATAL("Livox-SDK init fail!");
return -1;
}
AddLocalBroadcastCode();
if (argc >= BD_ARGC_NUM) {
ROS_INFO("Commandline input %s", argv[BD_ARGV_POS]);
AddCommandlineBroadcastCode(argv[BD_ARGV_POS]);
}
if (total_broadcast_code.size() > 0) {
ROS_INFO("list all valid bd:");
for (int i = 0; i < total_broadcast_code.size(); ++i) {
ROS_INFO("%s", total_broadcast_code[i].c_str());
}
} else {
ROS_INFO("No valid bd input, switch to automatic connection mode!");
}
memset(lidars, 0, sizeof(lidars));
memset(&hub, 0, sizeof(hub));
SetBroadcastCallback(OnDeviceBroadcast);
SetDeviceStateUpdateCallback(OnDeviceChange);
if (!Start()) {
Uninit();
return -1;
}
/* ros related */
ros::init(argc, argv, "livox_hub_publisher");
ros::NodeHandle livox_node;
int msg_type;
livox_node.getParam("livox_msg_type", msg_type);
if (kPointCloud2Msg == msg_type) {
cloud_pub = livox_node.advertise<PointCloud>("livox/hub", kMaxStoragePackets);
ROS_INFO("Publish PointCloud2");
} else {
cloud_pub = livox_node.advertise<livox_ros_driver::CustomMsg>("livox/hub", \
kMaxStoragePackets);
ROS_INFO("Publish Livox Custom Msg");
}
ros::Time::init();
ros::Rate r(1000.0 / kPublishIntervalMs); // 1000.0 / x = hz
while (ros::ok()) {
PollPointcloudData(msg_type);
r.sleep();
}
Uninit();
}

616
livox_ros_driver/livox_lidar.cpp Executable file
View File

@@ -0,0 +1,616 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <stdint.h>
#include <math.h>
#include <time.h>
#include <vector>
#include <chrono>
#include "livox_sdk.h"
#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <livox_ros_driver/CustomPoint.h>
#include <livox_ros_driver/CustomMsg.h>
#define kMaxPointPerEthPacket (100)
#define kMaxStoragePackets (128) // must be 2^n
#define KEthPacketHeaderLength (18) //(sizeof(LivoxEthPacket) - 1)
#define KEthPacketMaxLength (1500)
#define KCartesianPointSize (13)
#define KSphericalPointSzie (9)
const uint64_t kPacketTimeGap = 1000000; // 1ms = 1000000ns
const uint64_t kMaxPacketTimeGap = 1700000; // the threshold of packet continuous
const uint64_t kNsPerSecond = 1000000000; // 1s = 1000000000ns
const uint32_t kPublishIntervalMs = 50; // unit:ms
#define BD_ARGC_NUM (4)
#define BD_ARGV_POS (1)
#define COMMANDLINE_BD_SIZE (15)
typedef pcl::PointCloud<pcl::PointXYZI> PointCloud;
#pragma pack(1)
typedef struct {
uint64_t time_rcv; // used for pps sync only mode
uint32_t point_num;
uint8_t raw_data[KEthPacketMaxLength];
} StoragePacket;
typedef struct {
uint8_t lidar_id;
uint8_t rsvd[3];
uint32_t point_num;
uint64_t timestamp; // ns
LivoxPoint *point;
} PublishPacket;
#pragma pack()
typedef struct {
StoragePacket *storage_packet[kMaxStoragePackets]; // size must be 2^n
volatile uint32_t rd_idx;
volatile uint32_t wr_idx;
uint32_t mask;
uint32_t size; // must be 2^n
} StoragePacketQueue;
typedef struct {
uint32_t receive_packet_count;
uint32_t loss_packet_count;
uint64_t last_timestamp;
uint64_t timebase; // unit:nanosecond
uint32_t timebase_state;
} LidarPacketStatistic;
typedef enum {
kCoordinateCartesian = 0,
kCoordinateSpherical
} CoordinateType;
typedef enum {
kPointCloud2Msg = 0,
kLivoxCustomMsg
} LivoxMsgType;
StoragePacketQueue storage_packet_pool[kMaxLidarCount];
/* for global publisher use */
ros::Publisher cloud_pub;
/* for device connect use ----------------------------------------------------------------------- */
typedef enum {
kDeviceStateDisconnect = 0,
kDeviceStateConnect = 1,
kDeviceStateSampling = 2,
} DeviceState;
typedef struct {
uint8_t handle;
DeviceState device_state;
DeviceInfo info;
LidarPacketStatistic statistic_info;
} DeviceItem;
DeviceItem lidars[kMaxLidarCount];
/* user add broadcast code here */
const char* broadcast_code_list[] = {
"000000000000001",
};
#define BROADCAST_CODE_LIST_SIZE (sizeof(broadcast_code_list) / sizeof(intptr_t))
/* total broadcast code, include broadcast_code_list and commandline input */
std::vector<std::string > total_broadcast_code;
/* for pointcloud queue process */
void InitQueue(StoragePacketQueue* queue) {
queue->rd_idx = 0;
queue->wr_idx = 0;
queue->size = kMaxStoragePackets;
queue->mask = kMaxStoragePackets - 1;
for (int i=0; i<kMaxStoragePackets; i++) {
queue->storage_packet[i] = new StoragePacket;
}
}
void ResetQueue(StoragePacketQueue* queue) {
queue->rd_idx = 0;
queue->wr_idx = 0;
}
void InitStoragePacketPool(void) {
for (int i=0; i<kMaxLidarCount; i++) {
InitQueue(&storage_packet_pool[i]);
}
}
uint32_t QueuePop(StoragePacketQueue* queue, StoragePacket* storage_packet) {
uint32_t mask = queue->mask;
uint32_t rd_idx = queue->rd_idx & mask;
memcpy(storage_packet, queue->storage_packet[rd_idx], sizeof(StoragePacket));
queue->rd_idx++;
return 1;
}
uint32_t QueueUsedSize(StoragePacketQueue *queue) {
return (queue->wr_idx - queue->rd_idx) & queue->mask;
}
uint32_t QueueIsFull(StoragePacketQueue *queue) {
return ((queue->wr_idx + 1) == queue->rd_idx);
}
uint32_t QueueIsEmpty(StoragePacketQueue *queue) {
return (queue->rd_idx == queue->wr_idx);
}
static uint32_t GetEthPacketLen(LivoxEthPacket* eth_packet, uint32_t point_num) {
if (kCoordinateCartesian == eth_packet->data_type) {
return (KEthPacketHeaderLength + point_num * KCartesianPointSize);
} else {
return (KEthPacketHeaderLength + point_num * KSphericalPointSzie);
}
}
uint32_t PushEthPacketToStorageQueue(StoragePacketQueue* queue, LivoxEthPacket* eth_packet, \
uint32_t point_num, uint64_t timebase) {
uint32_t mask = queue->mask;
uint32_t wr_idx = queue->wr_idx & mask;
queue->storage_packet[wr_idx]->time_rcv = timebase;
queue->storage_packet[wr_idx]->point_num = point_num;
memcpy(queue->storage_packet[wr_idx]->raw_data, \
reinterpret_cast<char *>(eth_packet), \
GetEthPacketLen(eth_packet, point_num));
queue->wr_idx++;
return 1;
}
static uint64_t GetStoragePacketTimestamp(StoragePacket* packet) {
LivoxEthPacket* raw_packet = reinterpret_cast<LivoxEthPacket *>(packet->raw_data);
uint64_t timestamp = *((uint64_t *)(raw_packet->timestamp));
if (raw_packet->timestamp_type == kTimestampTypePps) {
return (timestamp + packet->time_rcv);
} else if (raw_packet->timestamp_type == kTimestampTypeNoSync) {
return timestamp;
} else if (raw_packet->timestamp_type == kTimestampTypePtp) {
return timestamp;
} else if (raw_packet->timestamp_type == kTimestampTypePpsGps) {
struct tm time_utc;
time_utc.tm_isdst = 0;
time_utc.tm_year = raw_packet->timestamp[0] + 100; // map 2000 to 1990
time_utc.tm_mon = raw_packet->timestamp[1];
time_utc.tm_mday = raw_packet->timestamp[2];
time_utc.tm_hour = raw_packet->timestamp[3];
time_utc.tm_min = 0;
time_utc.tm_sec = 0;
uint64_t time_epoch = mktime(&time_utc);
time_epoch = time_epoch * 1000000 + *((uint32_t *)(&(raw_packet->timestamp[4]))); // to us
time_epoch = time_epoch * 1000; // to ns
return time_epoch;
} else {
ROS_INFO("timestamp type invalid");
return 0;
}
}
/* for pointcloud convert process */
static uint32_t PublishPointcloud2(StoragePacketQueue* queue, uint32_t packet_num, \
uint8_t handle) {
uint64_t timestamp = 0;
uint64_t last_timestamp = 0;
uint32_t published_packet = 0;
/* init point cloud data struct */
PointCloud::Ptr cloud (new PointCloud);
cloud->header.frame_id = "livox_frame";
cloud->height = 1;
cloud->width = 0;
StoragePacket storage_packet;
while (published_packet < packet_num) {
QueuePop(queue, &storage_packet);
LivoxEthPacket* raw_packet = reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
LivoxRawPoint* raw_points = reinterpret_cast<LivoxRawPoint *>(raw_packet->data);
timestamp = GetStoragePacketTimestamp(&storage_packet);
if (published_packet && \
((timestamp - last_timestamp) > kMaxPacketTimeGap)) {
ROS_INFO("packet loss : %ld", timestamp);
break;
}
if (!cloud->width) {
cloud->header.stamp = timestamp / 1000; // to us
ROS_DEBUG("[%d]:%ld us", handle, cloud->header.stamp);
}
cloud->width += storage_packet.point_num;
for (uint32_t i = 0; i < storage_packet.point_num; i++) {
pcl::PointXYZI point;
point.x = raw_points->x/1000.0f;
point.y = raw_points->y/1000.0f;
point.z = raw_points->z/1000.0f;
point.intensity = (float) raw_points->reflectivity;
++raw_points;
cloud->points.push_back(point);
}
last_timestamp = timestamp;
++published_packet;
}
//ROS_INFO("%d", cloud->width);
cloud_pub.publish(cloud);
return published_packet;
}
/* for pointcloud convert process */
static uint32_t PublishCustomPointcloud(StoragePacketQueue* queue, uint32_t packet_num,\
uint8_t handle) {
static uint32_t msg_seq = 0;
uint64_t timestamp = 0;
uint64_t last_timestamp = 0;
uint32_t published_packet = 0;
/* init livox custom msg */
livox_ros_driver::CustomMsg livox_msg;
livox_msg.header.frame_id = "livox_frame";
livox_msg.header.seq = msg_seq;
++msg_seq;
livox_msg.header.stamp = ros::Time::now();
livox_msg.timebase = 0;
livox_msg.timestep = 10000; // 10us = 10^4ns;
livox_msg.point_num = 0;
livox_msg.lidar_id = handle;
StoragePacket storage_packet;
while (published_packet < packet_num) {
QueuePop(queue, &storage_packet);
LivoxEthPacket* raw_packet = reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
LivoxRawPoint* raw_points = reinterpret_cast<LivoxRawPoint *>(raw_packet->data);
timestamp = GetStoragePacketTimestamp(&storage_packet);
if (published_packet && \
((timestamp - last_timestamp) > kMaxPacketTimeGap)) {
ROS_INFO("packet loss : %ld", timestamp);
break;
}
if (!livox_msg.timebase) {
livox_msg.timebase = timestamp; // to us
ROS_DEBUG("[%d]:%ld", handle, livox_msg.timebase);
}
livox_msg.point_num += storage_packet.point_num;
for (uint32_t i = 0; i < storage_packet.point_num; i++) {
livox_ros_driver::CustomPoint point;
point.x = raw_points->x/1000.0f;
point.y = raw_points->y/1000.0f;
point.z = raw_points->z/1000.0f;
point.reflectivity = raw_points->reflectivity;
point.line = 0;
++raw_points;
livox_msg.points.push_back(point);
}
last_timestamp = timestamp;
++published_packet;
}
//ROS_INFO("%d", livox_msg.point_num);
cloud_pub.publish(livox_msg);
return published_packet;
}
static void PointCloudConvert(LivoxPoint *p_dpoint, LivoxRawPoint *p_raw_point) {
p_dpoint->x = p_raw_point->x/1000.0f;
p_dpoint->y = p_raw_point->y/1000.0f;
p_dpoint->z = p_raw_point->z/1000.0f;
p_dpoint->reflectivity = p_raw_point->reflectivity;
}
void GetLidarData(uint8_t handle, LivoxEthPacket *data, uint32_t data_num) {
using namespace std;
LivoxEthPacket* lidar_pack = data;
if (!data || !data_num || (handle >= kMaxLidarCount)) {
return;
}
LidarPacketStatistic *packet_statistic = &lidars[handle].statistic_info;
uint64_t cur_timestamp = *((uint64_t *)(lidar_pack->timestamp));
if (lidar_pack->timestamp_type == kTimestampTypePps) {
if ((cur_timestamp < packet_statistic->last_timestamp) && \
(cur_timestamp < kPacketTimeGap)) { // sync point
auto cur_time = chrono::high_resolution_clock::now();
int64_t sync_time = cur_time.time_since_epoch().count();
packet_statistic->timebase = sync_time;
//ROS_DEBUG("sysnc time : %lu %lu %lu", packet_statistic->timebase, cur_timestamp, \
// packet_statistic->last_timestamp);
}
}
packet_statistic->last_timestamp = cur_timestamp;
StoragePacketQueue *p_queue = &storage_packet_pool[handle];
if (!QueueIsFull(p_queue)) {
if (data_num <= kMaxPointPerEthPacket) {
PushEthPacketToStorageQueue(p_queue, lidar_pack, data_num, packet_statistic->timebase);
}
}
}
void PollPointcloudData(int msg_type) {
for (int i = 0; i < kMaxLidarCount; i++) {
StoragePacketQueue *p_queue = &storage_packet_pool[i];
if (kDeviceStateSampling != lidars[i].device_state) {
continue;
}
while (!QueueIsEmpty(p_queue)) {
//ROS_DEBUG("%d %d %d %d\r\n", i, p_queue->rd_idx, p_queue->wr_idx, QueueUsedSize(p_queue));
uint32_t used_size = QueueUsedSize(p_queue);
if (kPointCloud2Msg == msg_type) {
if (used_size == PublishPointcloud2(p_queue, used_size, i)) {
break;
}
} else {
if (used_size == PublishCustomPointcloud(p_queue, QueueUsedSize(p_queue), i)) {
break;
}
}
}
}
}
/** add bd to total_broadcast_code */
void AddBroadcastCode(const char* bd_str) {
total_broadcast_code.push_back(bd_str);
}
/** add bd in broadcast_code_list to total_broadcast_code */
void AddLocalBroadcastCode(void) {
for (int i = 0; i < BROADCAST_CODE_LIST_SIZE; ++i) {
std::string invalid_bd = "000000000";
ROS_INFO("broadcast code list :%s", broadcast_code_list[i]);
if ((COMMANDLINE_BD_SIZE == strlen(broadcast_code_list[i])) && \
(NULL == strstr(broadcast_code_list[i], invalid_bd.c_str()))) {
AddBroadcastCode(broadcast_code_list[i]);
} else {
ROS_INFO("Invalid bd:%s", broadcast_code_list[i]);
}
}
}
/** add commandline bd to total_broadcast_code */
void AddCommandlineBroadcastCode(const char* cammandline_str) {
char* strs = new char[strlen(cammandline_str) + 1];
strcpy(strs, cammandline_str);
std::string pattern = "&";
char* bd_str = strtok(strs, pattern.c_str());
std::string invalid_bd = "000000000";
while (bd_str != NULL) {
ROS_INFO("commandline input bd:%s", bd_str);
if ((COMMANDLINE_BD_SIZE == strlen(bd_str)) && \
(NULL == strstr(bd_str, invalid_bd.c_str()))) {
AddBroadcastCode(bd_str);
} else {
ROS_INFO("Invalid bd:%s", bd_str);
}
bd_str = strtok(NULL, pattern.c_str());
}
delete [] strs;
}
/** Callback function of starting sampling. */
void OnSampleCallback(uint8_t status, uint8_t handle, uint8_t response, void *data) {
ROS_INFO("OnSampleCallback statue %d handle %d response %d", status, handle, response);
if (status == kStatusSuccess) {
if (response != 0) {
lidars[handle].device_state = kDeviceStateConnect;
}
} else if (status == kStatusTimeout) {
lidars[handle].device_state = kDeviceStateConnect;
}
}
/** Callback function of stopping sampling. */
void OnStopSampleCallback(uint8_t status, uint8_t handle, uint8_t response, void *data) {
}
/** Query the firmware version of Livox LiDAR. */
void OnDeviceInformation(uint8_t status, uint8_t handle, DeviceInformationResponse *ack, void *data) {
if (status != kStatusSuccess) {
ROS_INFO("Device Query Informations Failed %d", status);
}
if (ack) {
ROS_INFO("firm ver: %d.%d.%d.%d",
ack->firmware_version[0],
ack->firmware_version[1],
ack->firmware_version[2],
ack->firmware_version[3]);
}
}
/** Callback function of changing of device state. */
void OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
if (info == NULL) {
return;
}
ROS_INFO("OnDeviceChange broadcast code %s update type %d", info->broadcast_code, type);
uint8_t handle = info->handle;
if (handle >= kMaxLidarCount) {
return;
}
if (type == kEventConnect) {
QueryDeviceInformation(handle, OnDeviceInformation, NULL);
if (lidars[handle].device_state == kDeviceStateDisconnect) {
lidars[handle].device_state = kDeviceStateConnect;
lidars[handle].info = *info;
}
} else if (type == kEventDisconnect) {
lidars[handle].device_state = kDeviceStateDisconnect;
} else if (type == kEventStateChange) {
lidars[handle].info = *info;
}
if (lidars[handle].device_state == kDeviceStateConnect) {
ROS_INFO("Device State status_code %d", lidars[handle].info.status.status_code);
ROS_INFO("Device State working state %d", lidars[handle].info.state);
ROS_INFO("Device feature %d", lidars[handle].info.feature);
if (lidars[handle].info.state == kLidarStateNormal) {
if (lidars[handle].info.type == kDeviceTypeHub) {
HubStartSampling(OnSampleCallback, NULL);
} else {
LidarStartSampling(handle, OnSampleCallback, NULL);
}
lidars[handle].device_state = kDeviceStateSampling;
}
}
}
void OnDeviceBroadcast(const BroadcastDeviceInfo *info) {
if (info == NULL) {
return;
}
ROS_INFO("Receive Broadcast Code %s, please add it to broacast_code_list if want to connect!\n",\
info->broadcast_code);
if (total_broadcast_code.size() > 0) {
bool found = false;
for (int i = 0; i < total_broadcast_code.size(); ++i) {
if (strncmp(info->broadcast_code, total_broadcast_code[i].c_str(), kBroadcastCodeSize) == 0) {
found = true;
break;
}
}
if (!found) {
ROS_INFO("Not in the broacast_code_list, please add it to if want to connect!");
return;
}
} else {
ROS_INFO("In automatic connection mode, will connect %s", info->broadcast_code);
}
bool result = false;
uint8_t handle = 0;
result = AddLidarToConnect(info->broadcast_code, &handle);
if (result == kStatusSuccess && handle < kMaxLidarCount) {
SetDataCallback(handle, GetLidarData);
lidars[handle].handle = handle;
lidars[handle].device_state = kDeviceStateDisconnect;
}
}
int main(int argc, char **argv) {
if(ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info)) {
ros::console::notifyLoggerLevelsChanged();
}
ROS_INFO("Livox-SDK ros demo");
InitStoragePacketPool();
if (!Init()) {
ROS_FATAL("Livox-SDK init fail!");
return -1;
}
AddLocalBroadcastCode();
if (argc >= BD_ARGC_NUM) {
ROS_INFO("Commandline input %s", argv[BD_ARGV_POS]);
AddCommandlineBroadcastCode(argv[BD_ARGV_POS]);
}
if (total_broadcast_code.size() > 0) {
ROS_INFO("list all valid bd:");
for (int i = 0; i < total_broadcast_code.size(); ++i) {
ROS_INFO("%s", total_broadcast_code[i].c_str());
}
} else {
ROS_INFO("No valid bd input, switch to automatic connection mode!");
}
memset(lidars, 0, sizeof(lidars));
SetBroadcastCallback(OnDeviceBroadcast);
SetDeviceStateUpdateCallback(OnDeviceChange);
if (!Start()) {
Uninit();
return -1;
}
/* ros related */
ros::init(argc, argv, "livox_lidar_publisher");
ros::NodeHandle livox_node;
int msg_type;
livox_node.getParam("livox_msg_type", msg_type);
if (kPointCloud2Msg == msg_type) {
cloud_pub = livox_node.advertise<PointCloud>("livox/lidar", kMaxStoragePackets);
ROS_INFO("Publish PointCloud2");
} else {
cloud_pub = livox_node.advertise<livox_ros_driver::CustomMsg>("livox/lidar", \
kMaxStoragePackets);
ROS_INFO("Publish Livox Custom Msg");
}
ros::Time::init();
ros::Rate r(1000.0 / kPublishIntervalMs); // 1000.0 / x = hz
while (ros::ok()) {
PollPointcloudData(msg_type);
r.sleep();
}
Uninit();
}

10
livox_ros_driver/msg/CustomMsg.msg Normal file
View File

@@ -0,0 +1,10 @@
# Livox publish pointcloud msg format.
Header header # ROS standard message header
uint64 timebase # The time of first point
uint32 timestep # Time interval between adjacent point clouds
uint32 point_num # Total number of pointclouds
uint8 lidar_id # Lidar device id number
uint8[3] rsvd # Reserved use
CustomPoint[] points # Pointcloud data

8
livox_ros_driver/msg/CustomPoint.msg Normal file
View File

@@ -0,0 +1,8 @@
# Livox costom pointcloud format.
float32 x # X axis, unit:m
float32 y # Y axis, unit:m
float32 z # Z axis, unit:m
uint8 reflectivity # reflectivity, 0~255
uint8 line # laser number in lidar

72
livox_ros_driver/package.xml Normal file
View File

@@ -0,0 +1,72 @@
<?xml version="1.0"?>
<package format="2">
<name>livox_ros_driver</name>
<version>1.1.0</version>
<description>The livox ros driver package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="dev@livoxtech.com">xxx</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>MIT</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/lidar_sdk</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>message_generation</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>message_runtime</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>