From 0c517daff007a7738c604f6ce19c967b0fa09ff6 Mon Sep 17 00:00:00 2001 From: Livox-SDK Date: Mon, 8 Apr 2019 12:20:20 +0800 Subject: [PATCH] feature:add livox_ros_driver package --- .gitignore | 9 + LICENSE.txt | 678 ++++++++++++++++++ README.md | 69 ++ livox_ros_driver/CMakeLists.txt | 262 +++++++ .../config/display_hub_points.rviz | 189 +++++ .../config/display_lidar_points.rviz | 189 +++++ livox_ros_driver/launch/livox_hub.launch | 14 + livox_ros_driver/launch/livox_hub_msg.launch | 14 + livox_ros_driver/launch/livox_hub_rviz.launch | 12 + livox_ros_driver/launch/livox_lidar.launch | 13 + .../launch/livox_lidar_msg.launch | 13 + .../launch/livox_lidar_rviz.launch | 12 + livox_ros_driver/livox_hub.cpp | 661 +++++++++++++++++ livox_ros_driver/livox_lidar.cpp | 616 ++++++++++++++++ livox_ros_driver/msg/CustomMsg.msg | 10 + livox_ros_driver/msg/CustomPoint.msg | 8 + livox_ros_driver/package.xml | 72 ++ 17 files changed, 2841 insertions(+) create mode 100644 .gitignore create mode 100644 LICENSE.txt create mode 100644 README.md create mode 100755 livox_ros_driver/CMakeLists.txt create mode 100644 livox_ros_driver/config/display_hub_points.rviz create mode 100644 livox_ros_driver/config/display_lidar_points.rviz create mode 100644 livox_ros_driver/launch/livox_hub.launch create mode 100644 livox_ros_driver/launch/livox_hub_msg.launch create mode 100644 livox_ros_driver/launch/livox_hub_rviz.launch create mode 100644 livox_ros_driver/launch/livox_lidar.launch create mode 100644 livox_ros_driver/launch/livox_lidar_msg.launch create mode 100644 livox_ros_driver/launch/livox_lidar_rviz.launch create mode 100755 livox_ros_driver/livox_hub.cpp create mode 100755 livox_ros_driver/livox_lidar.cpp create mode 100644 livox_ros_driver/msg/CustomMsg.msg create mode 100644 livox_ros_driver/msg/CustomPoint.msg create mode 100644 livox_ros_driver/package.xml diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..15fc5e1 --- /dev/null +++ b/.gitignore @@ -0,0 +1,9 @@ + +# ignore dir +build/ +devel/ +.catkin_workspace +#whitelist + + + diff --git a/LICENSE.txt b/LICENSE.txt new file mode 100644 index 0000000..0791d5a --- /dev/null +++ b/LICENSE.txt @@ -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 + + 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): + * 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 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. + diff --git a/README.md b/README.md new file mode 100644 index 0000000..308b8b2 --- /dev/null +++ b/README.md @@ -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 +``` diff --git a/livox_ros_driver/CMakeLists.txt b/livox_ros_driver/CMakeLists.txt new file mode 100755 index 0000000..100fb06 --- /dev/null +++ b/livox_ros_driver/CMakeLists.txt @@ -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() + diff --git a/livox_ros_driver/config/display_hub_points.rviz b/livox_ros_driver/config/display_hub_points.rviz new file mode 100644 index 0000000..90d127e --- /dev/null +++ b/livox_ros_driver/config/display_hub_points.rviz @@ -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: + 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: + 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: + 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 diff --git a/livox_ros_driver/config/display_lidar_points.rviz b/livox_ros_driver/config/display_lidar_points.rviz new file mode 100644 index 0000000..bfe3bb7 --- /dev/null +++ b/livox_ros_driver/config/display_lidar_points.rviz @@ -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: + 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: + 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: + 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 diff --git a/livox_ros_driver/launch/livox_hub.launch b/livox_ros_driver/launch/livox_hub.launch new file mode 100644 index 0000000..669142c --- /dev/null +++ b/livox_ros_driver/launch/livox_hub.launch @@ -0,0 +1,14 @@ + + + + + + + + + diff --git a/livox_ros_driver/launch/livox_hub_msg.launch b/livox_ros_driver/launch/livox_hub_msg.launch new file mode 100644 index 0000000..669142c --- /dev/null +++ b/livox_ros_driver/launch/livox_hub_msg.launch @@ -0,0 +1,14 @@ + + + + + + + + + diff --git a/livox_ros_driver/launch/livox_hub_rviz.launch b/livox_ros_driver/launch/livox_hub_rviz.launch new file mode 100644 index 0000000..faa9af0 --- /dev/null +++ b/livox_ros_driver/launch/livox_hub_rviz.launch @@ -0,0 +1,12 @@ + + + + + + + + + diff --git a/livox_ros_driver/launch/livox_lidar.launch b/livox_ros_driver/launch/livox_lidar.launch new file mode 100644 index 0000000..a78e3ef --- /dev/null +++ b/livox_ros_driver/launch/livox_lidar.launch @@ -0,0 +1,13 @@ + + + + + + + + diff --git a/livox_ros_driver/launch/livox_lidar_msg.launch b/livox_ros_driver/launch/livox_lidar_msg.launch new file mode 100644 index 0000000..3858614 --- /dev/null +++ b/livox_ros_driver/launch/livox_lidar_msg.launch @@ -0,0 +1,13 @@ + + + + + + + + diff --git a/livox_ros_driver/launch/livox_lidar_rviz.launch b/livox_ros_driver/launch/livox_lidar_rviz.launch new file mode 100644 index 0000000..ce5cb98 --- /dev/null +++ b/livox_ros_driver/launch/livox_lidar_rviz.launch @@ -0,0 +1,12 @@ + + + + + + + + + diff --git a/livox_ros_driver/livox_hub.cpp b/livox_ros_driver/livox_hub.cpp new file mode 100755 index 0000000..70a1ec6 --- /dev/null +++ b/livox_ros_driver/livox_hub.cpp @@ -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 +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "livox_sdk.h" +#include +#include +#include +#include + +#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 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 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; istorage_packet[i] = new StoragePacket; + } +} + +void ResetQueue(StoragePacketQueue* queue) { + queue->rd_idx = 0; + queue->wr_idx = 0; +} + +void InitStoragePacketPool(void) { + for (int i=0; imask; + 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(eth_packet), \ + GetEthPacketLen(eth_packet, point_num)); + + queue->wr_idx++; + + return 1; +} + +static uint64_t GetStoragePacketTimestamp(StoragePacket* packet) { + + LivoxEthPacket* raw_packet = reinterpret_cast(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(storage_packet.raw_data); + LivoxRawPoint* raw_points = reinterpret_cast(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(storage_packet.raw_data); + LivoxRawPoint* raw_points = reinterpret_cast(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("livox/hub", kMaxStoragePackets); + ROS_INFO("Publish PointCloud2"); + } else { + cloud_pub = livox_node.advertise("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(); +} + + diff --git a/livox_ros_driver/livox_lidar.cpp b/livox_ros_driver/livox_lidar.cpp new file mode 100755 index 0000000..fe95664 --- /dev/null +++ b/livox_ros_driver/livox_lidar.cpp @@ -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 +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "livox_sdk.h" +#include +#include +#include +#include + +#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 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 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; istorage_packet[i] = new StoragePacket; + } +} + +void ResetQueue(StoragePacketQueue* queue) { + queue->rd_idx = 0; + queue->wr_idx = 0; +} + +void InitStoragePacketPool(void) { + for (int i=0; imask; + 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(eth_packet), \ + GetEthPacketLen(eth_packet, point_num)); + + queue->wr_idx++; + + return 1; +} + +static uint64_t GetStoragePacketTimestamp(StoragePacket* packet) { + + LivoxEthPacket* raw_packet = reinterpret_cast(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(storage_packet.raw_data); + LivoxRawPoint* raw_points = reinterpret_cast(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(storage_packet.raw_data); + LivoxRawPoint* raw_points = reinterpret_cast(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("livox/lidar", kMaxStoragePackets); + ROS_INFO("Publish PointCloud2"); + } else { + cloud_pub = livox_node.advertise("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(); +} + + diff --git a/livox_ros_driver/msg/CustomMsg.msg b/livox_ros_driver/msg/CustomMsg.msg new file mode 100644 index 0000000..20006e2 --- /dev/null +++ b/livox_ros_driver/msg/CustomMsg.msg @@ -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 + diff --git a/livox_ros_driver/msg/CustomPoint.msg b/livox_ros_driver/msg/CustomPoint.msg new file mode 100644 index 0000000..8d3ca3a --- /dev/null +++ b/livox_ros_driver/msg/CustomPoint.msg @@ -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 + diff --git a/livox_ros_driver/package.xml b/livox_ros_driver/package.xml new file mode 100644 index 0000000..99e008d --- /dev/null +++ b/livox_ros_driver/package.xml @@ -0,0 +1,72 @@ + + + livox_ros_driver + 1.1.0 + The livox ros driver package + + + + + xxx + + + + + + MIT + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + + roscpp + rospy + std_msgs + message_generation + + roscpp + rospy + std_msgs + + roscpp + rospy + std_msgs + message_runtime + + + + + + +