Initial commit
This commit is contained in:
@@ -0,0 +1,21 @@
|
||||
[](https://github.com/ros-drivers/velodyne/actions)
|
||||
|
||||
Overview
|
||||
========
|
||||
|
||||
Velodyne<sup>1</sup> is a collection of ROS<sup>2</sup> packages supporting `Velodyne high
|
||||
definition 3D LIDARs`<sup>3</sup>.
|
||||
|
||||
**Warning**:
|
||||
|
||||
The master branch normally contains code being tested for the next
|
||||
ROS release. It will not always work with every previous release.
|
||||
To check out the source for the most recent release, check out the
|
||||
tag `<version>` with the highest version number.
|
||||
|
||||
The current ``master`` branch works with ROS Kinetic and Melodic.
|
||||
CI builds are currently run for Kinetic and Melodic.
|
||||
|
||||
- <sup>1</sup>Velodyne: http://www.ros.org/wiki/velodyne
|
||||
- <sup>2</sup>ROS: http://www.ros.org
|
||||
- <sup>3</sup>`Velodyne high definition 3D LIDARs`: http://www.velodynelidar.com/lidar/lidar.aspx
|
||||
@@ -0,0 +1,89 @@
|
||||
Change history
|
||||
==============
|
||||
|
||||
1.5.2 (2019-01-28)
|
||||
------------------
|
||||
|
||||
1.5.1 (2018-12-10)
|
||||
------------------
|
||||
|
||||
1.5.0 (2018-10-19)
|
||||
------------------
|
||||
|
||||
1.4.0 (2018-09-19)
|
||||
------------------
|
||||
* Merge pull request `#160 <https://github.com/ros-drivers/velodyne/issues/160>`_ from ros-drivers/maint/updating_package_xml_to_v2
|
||||
* Updated all package.xmls to ver 2. Cleaned up catkin_lint errors.
|
||||
All package.xml files are now compatible with version 2 of the
|
||||
package.xml specification in REP 140. Removed some unnecessary
|
||||
execute permissions on a few files. Fixed a missing test_depend.
|
||||
* Contributors: Andre Volk, Joshua Whitley
|
||||
|
||||
1.3.0 (2017-11-10)
|
||||
------------------
|
||||
* Merge pull request `#110 <https://github.com/ros-drivers/velodyne/issues/110>`_ from kmhallen/master
|
||||
Added velodyne_laserscan package
|
||||
* Added velodyne_laserscan package and inserted into existing launch files
|
||||
* Contributors: Jack O'Quin, Joshua Whitley, Kevin Hallenbeck
|
||||
|
||||
1.2.0 (2014-08-06)
|
||||
------------------
|
||||
|
||||
1.1.2 (2013-11-05)
|
||||
-------------------
|
||||
|
||||
1.1.1 (2013-07-30)
|
||||
------------------
|
||||
|
||||
1.1.0 (2013-07-16)
|
||||
------------------
|
||||
|
||||
1.0.1 (2013-06-15)
|
||||
------------------
|
||||
|
||||
1.0.0 (2013-06-14)
|
||||
------------------
|
||||
|
||||
* Convert to catkin (`#1`_).
|
||||
* Release to Hydro.
|
||||
|
||||
0.9.2 (2013-07-08)
|
||||
------------------
|
||||
|
||||
0.9.1 (2012-06-05)
|
||||
------------------
|
||||
|
||||
0.9.0 (2012-04-03)
|
||||
------------------
|
||||
|
||||
* Completely revised API, anticipating a 1.0.0 release.
|
||||
* New velodyne_driver and velodyne_pointcloud packages.
|
||||
* Old velodyne_common and velodyne_pcl packages no longer included.
|
||||
* Released to Electric, Fuerte and Groovy.
|
||||
|
||||
0.2.6 (2011-02-23)
|
||||
------------------
|
||||
|
||||
0.2.5 (2010-11-19)
|
||||
------------------
|
||||
|
||||
* Initial implementation of new 0.3 interfaces.
|
||||
|
||||
0.2.0 (2010-08-17)
|
||||
------------------
|
||||
|
||||
* Initial release to ROS C-turtle.
|
||||
|
||||
.. _`#1`: https://github.com/ros-drivers/velodyne/issues/1
|
||||
.. _`#4`: https://github.com/ros-drivers/velodyne/issues/4
|
||||
.. _`#7`: https://github.com/ros-drivers/velodyne/issues/7
|
||||
.. _`#8`: https://github.com/ros-drivers/velodyne/pull/8
|
||||
.. _`#9`: https://github.com/ros-drivers/velodyne/issues/9
|
||||
.. _`#10`: https://github.com/ros-drivers/velodyne/issues/10
|
||||
.. _`#11`: https://github.com/ros-drivers/velodyne/issues/11
|
||||
.. _`#12`: https://github.com/ros-drivers/velodyne/pull/12
|
||||
.. _`#13`: https://github.com/ros-drivers/velodyne/issues/13
|
||||
.. _`#14`: https://github.com/ros-drivers/velodyne/pull/14
|
||||
.. _`#17`: https://github.com/ros-drivers/velodyne/issues/17
|
||||
.. _`#18`: https://github.com/ros-drivers/velodyne/issues/18
|
||||
.. _`#20`: https://github.com/ros-drivers/velodyne/issues/20
|
||||
@@ -0,0 +1,4 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(velodyne)
|
||||
find_package(catkin REQUIRED)
|
||||
catkin_metapackage()
|
||||
@@ -0,0 +1,25 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>velodyne</name>
|
||||
<version>1.5.2</version>
|
||||
<description>
|
||||
Basic ROS support for the Velodyne 3D LIDARs.
|
||||
</description>
|
||||
<maintainer email="josh.whitley@autoware.org">Josh Whitley</maintainer>
|
||||
<author>Jack O'Quin</author>
|
||||
<license>BSD</license>
|
||||
<url type="website">http://www.ros.org/wiki/velodyne</url>
|
||||
<url type="repository">https://github.com/ros-drivers/velodyne</url>
|
||||
<url type="bugtracker">https://github.com/ros-drivers/velodyne/issues</url>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<exec_depend>velodyne_driver</exec_depend>
|
||||
<exec_depend>velodyne_laserscan</exec_depend>
|
||||
<exec_depend>velodyne_msgs</exec_depend>
|
||||
<exec_depend>velodyne_pointcloud</exec_depend>
|
||||
|
||||
<export>
|
||||
<metapackage/>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,187 @@
|
||||
Change history
|
||||
==============
|
||||
|
||||
1.5.2 (2019-01-28)
|
||||
------------------
|
||||
* Merge pull request `#212 <https://github.com/ros-drivers/velodyne/issues/212>`_ from ros-drivers/maint/vdump_as_root
|
||||
Modifying vdump script for use as root.
|
||||
Tested by @andersfischernielsen.
|
||||
* Merge pull request `#205 <https://github.com/ros-drivers/velodyne/issues/205>`_ from xiesc/master
|
||||
support for 64E-S3
|
||||
* Contributors: Joshua Whitley, Shichao XIE, xiesc
|
||||
|
||||
1.5.1 (2018-12-10)
|
||||
------------------
|
||||
|
||||
1.5.0 (2018-10-19)
|
||||
------------------
|
||||
* Merge pull request `#187 <https://github.com/ros-drivers/velodyne/issues/187>`_ from moooeeeep/master
|
||||
Fixed sign error in return value of InputSocket::getPacket()
|
||||
* bugfix: getPacket() function is expected to return negative value on error
|
||||
* Contributors: Fabian Maas, Joshua Whitley
|
||||
|
||||
1.4.0 (2018-09-19)
|
||||
------------------
|
||||
* Merge pull request `#178 <https://github.com/ros-drivers/velodyne/issues/178>`_ from sts-thm/bugfix_issue\_`#174 <https://github.com/ros-drivers/velodyne/issues/174>`_
|
||||
Bugfix issue `#174 <https://github.com/ros-drivers/velodyne/issues/174>`_
|
||||
* Removed debug outputs
|
||||
* Changes fixing deadlock for specific cut_angle values.
|
||||
* Merge pull request `#135 <https://github.com/ros-drivers/velodyne/issues/135>`_ from cfneuhaus/bugfix
|
||||
Bugfix: when no device ip is set, we still want to filter by udp port.
|
||||
* Merge pull request `#170 <https://github.com/ros-drivers/velodyne/issues/170>`_ from ros-drivers/maint/move_header_files
|
||||
Moving header files to traditional location inside include folders.
|
||||
* Merge pull request `#160 <https://github.com/ros-drivers/velodyne/issues/160>`_ from ros-drivers/maint/updating_package_xml_to_v2
|
||||
* Updated all package.xmls to ver 2. Cleaned up catkin_lint errors.
|
||||
All package.xml files are now compatible with version 2 of the
|
||||
package.xml specification in REP 140. Removed some unnecessary
|
||||
execute permissions on a few files. Fixed a missing test_depend.
|
||||
* Merge pull request `#151 <https://github.com/ros-drivers/velodyne/issues/151>`_ from Axel13fr/feature/No_Communication_Diag_Update
|
||||
* Fix packet rate for the Velodyne 32C
|
||||
* Conventions: adding name for unused method parameter.
|
||||
* Added a periodic update of the diagnostics so that when no data is received at all from the Velodyne, a diagnostic information will be published. The previous implementation would publish diagnostics only on packet reception.
|
||||
* Merge pull request `#139 <https://github.com/ros-drivers/velodyne/issues/139>`_ from ASDeveloper00/vlp32
|
||||
Adding support for VLP-32C.
|
||||
* Merge pull request `#138 <https://github.com/ros-drivers/velodyne/issues/138>`_ from volkandre/cut_at_specified_angle_feature
|
||||
* cut_angle parameter is now in rad according to REP 103
|
||||
* Fixed timestamp related bug found by @cfneuhaus, which was described here: https://github.com/ros-drivers/velodyne/pull/126#discussion_r154137793
|
||||
* bugfix: when no device ip is set, we still want to filter by udp port.
|
||||
* Contributors: Andre Volk, CNR, Denis Dillenberger, Frank Neuhaus, Jack O'Quin, Joshua Whitley, Sammy Pfeiffer, Tobias Athmer, axd, kennouni
|
||||
|
||||
1.3.0 (2017-11-10)
|
||||
------------------
|
||||
* Merge pull request `#129 <https://github.com/ros-drivers/velodyne/issues/129>`_ from kmhallen/pluginlib_macro
|
||||
Modern pluginlib macro
|
||||
* Update to use non deprecated pluginlib macro
|
||||
* add launch args to support multiple devices (`#108 <https://github.com/ros-drivers/velodyne/issues/108>`_)
|
||||
* Merge pull request `#101 <https://github.com/ros-drivers/velodyne/issues/101>`_ from teosnare/master
|
||||
velodyne_driver/src/lib/input.cc : fix for device_ip filter
|
||||
* Merge pull request `#104 <https://github.com/ros-drivers/velodyne/issues/104>`_ from altrouge/launch_options
|
||||
Add more options in launch files.
|
||||
* Rearranged alphabetically.
|
||||
* Add more options in launch files.
|
||||
- rpm, device_ip, port, read_once, read_fast, repeat_delay
|
||||
* velodyne_driver/src/lib/input.cc : fix for device_ip filter
|
||||
Fix for device_ip filter in InputSocket: initialization of devip\_ for correct ip filtering in InputSocket::getPacket.
|
||||
* velodyne_driver: credit @priyankadey for VLP-16 bug fix (`#96 <https://github.com/ros-drivers/velodyne/issues/96>`_)
|
||||
* Merge pull request `#96 <https://github.com/ros-drivers/velodyne/issues/96>`_ from priyankadey/master
|
||||
updated VLP-16 packet rate from user manual.
|
||||
* updated VLP-16 packet rate from user manual.
|
||||
Also verified with sensor. It reduced overlap in the pointcloud
|
||||
* update change history
|
||||
* Merge pull request `#94 <https://github.com/ros-drivers/velodyne/issues/94>`_ from ros-drivers/pcap_port
|
||||
velodyne_driver: use port number for PCAP data (`#46 <https://github.com/ros-drivers/velodyne/issues/46>`_, `#66 <https://github.com/ros-drivers/velodyne/issues/66>`_)
|
||||
* fix g++ 5.3.1 compile errors (`#94 <https://github.com/ros-drivers/velodyne/issues/94>`_)
|
||||
* merge current master (`#94 <https://github.com/ros-drivers/velodyne/issues/94>`_)
|
||||
* Merge pull request `#91 <https://github.com/ros-drivers/velodyne/issues/91>`_ from chukcha2/master
|
||||
update velodyne_driver package description to include all models
|
||||
* update velodyne_driver package description to include all models
|
||||
* Merge pull request `#89 <https://github.com/ros-drivers/velodyne/issues/89>`_ from Tones29/feat_dynrec_driver
|
||||
Add dynamic latency configuration to velodyne_driver
|
||||
* velodyne_driver: Add dynamic_reconfigure and time_offset correction
|
||||
The value of time_offset is added to the calculated time stamp in live mode for each packet.
|
||||
* velodyne_driver: Make input destructors virtual
|
||||
* prepare change history for coming Indigo release (`#59 <https://github.com/ros-drivers/velodyne/issues/59>`_)
|
||||
* velodyne_driver: use port number for PCAP data (`#66 <https://github.com/ros-drivers/velodyne/issues/66>`_)
|
||||
* Merge pull request `#39 <https://github.com/ros-drivers/velodyne/issues/39>`_ from zooxco/multivelodyne
|
||||
support for multiple velodynes
|
||||
* Merge pull request `#44 <https://github.com/ros-drivers/velodyne/issues/44>`_ from SISegwayRmp/master
|
||||
adding driver and pointcloud support for the VLP16
|
||||
* adding the VLP16 test scripts and updating the CMakeLists to include the test file from http://download.ros.org/data/velodyne/vlp16.pcap
|
||||
* adding support for the VLP16
|
||||
* parameters to set the udp port
|
||||
* fixed missing header
|
||||
* cleanup debug line
|
||||
* parameter and code added for working with multiple velodynes
|
||||
* Contributors: Andreas Wachaja, Brice Rebsamen, Daniel Jartoux, Denis Dillenberger, Gabor Meszaros, Ilya, Jack O'Quin, Joshua Whitley, Kevin Hallenbeck, Matteo Murtas, Micho Radovnikovich, Priyanka Dey, William Woodall, jack.oquin, junior, phussey
|
||||
|
||||
1.2.0 (2014-08-06)
|
||||
------------------
|
||||
* Fixed bug in diagnostic rate for driver (`#16
|
||||
<https://github.com/ros-drivers/velodyne/issues/16>`_)
|
||||
* Contributors: Brice Rebsamen, Jack O'Quin
|
||||
|
||||
1.1.2 (2013-11-05)
|
||||
-------------------
|
||||
|
||||
* Move unit test data to download.ros.org (`#18`_).
|
||||
* Install missing vdump script (`#17`_).
|
||||
|
||||
1.1.1 (2013-07-30)
|
||||
------------------
|
||||
|
||||
* Add support for HDL-64E S2 and S2.1 models, which were not working before (`#11`_), thanks to Gabor Meszaros (`#12`_).
|
||||
* Add additional parameters to launch files (`#14`_).
|
||||
|
||||
1.1.0 (2013-07-16)
|
||||
------------------
|
||||
|
||||
* Fix build problems due to PCL 1.7 API incompatibilities (`#8`_),
|
||||
thanks to William Woodall. This version also works with Groovy, as
|
||||
long as the correct ``pcl_conversions`` is installed.
|
||||
* Fix errors with Mac OSX compiler (`#8`_).
|
||||
* Install ``pluginlib`` XML files (`#9`_).
|
||||
* Install some launch and parameter files.
|
||||
* Enable unit tests when ``CATKIN_ENABLE_TESTING`` is set (`#10`_).
|
||||
|
||||
1.0.1 (2013-06-15)
|
||||
------------------
|
||||
|
||||
* Declare explicit ``pluginlib`` dependency (`#4`_).
|
||||
|
||||
1.0.0 (2013-06-14)
|
||||
------------------
|
||||
|
||||
* Convert to catkin (`#1`_).
|
||||
* Release to Hydro.
|
||||
|
||||
0.9.2 (2013-07-08)
|
||||
------------------
|
||||
|
||||
* Fix Groovy build problem (`#7`_).
|
||||
|
||||
0.9.1 (2012-06-05)
|
||||
------------------
|
||||
|
||||
* Driver socket read path improvements.
|
||||
* Add unit tests with 32E data.
|
||||
* Released to Electric, Fuerte and Groovy.
|
||||
|
||||
0.9.0 (2012-04-03)
|
||||
------------------
|
||||
|
||||
* Completely revised API, anticipating a 1.0.0 release.
|
||||
* HDL-32E device support.
|
||||
* New velodyne_driver and velodyne_pointcloud packages.
|
||||
* Old velodyne_common and velodyne_pcl packages no longer included.
|
||||
* Released to Electric, Fuerte and Groovy.
|
||||
|
||||
0.2.6 (2011-02-23)
|
||||
------------------
|
||||
|
||||
* Label all timing-dependent tests "realtime" so they do not run by
|
||||
default on the build farm machines.
|
||||
|
||||
0.2.5 (2010-11-19)
|
||||
------------------
|
||||
|
||||
* Initial implementation of new 0.3 interfaces.
|
||||
* Support for ROS 1.3 `std_msgs::Header` changes.
|
||||
|
||||
0.2.0 (2010-08-17)
|
||||
------------------
|
||||
|
||||
* Initial release to ROS C-turtle.
|
||||
|
||||
.. _`#1`: https://github.com/ros-drivers/velodyne/issues/1
|
||||
.. _`#4`: https://github.com/ros-drivers/velodyne/issues/4
|
||||
.. _`#7`: https://github.com/ros-drivers/velodyne/issues/7
|
||||
.. _`#8`: https://github.com/ros-drivers/velodyne/pull/8
|
||||
.. _`#9`: https://github.com/ros-drivers/velodyne/issues/9
|
||||
.. _`#10`: https://github.com/ros-drivers/velodyne/issues/10
|
||||
.. _`#11`: https://github.com/ros-drivers/velodyne/issues/11
|
||||
.. _`#12`: https://github.com/ros-drivers/velodyne/pull/12
|
||||
.. _`#13`: https://github.com/ros-drivers/velodyne/issues/13
|
||||
.. _`#14`: https://github.com/ros-drivers/velodyne/pull/14
|
||||
.. _`#17`: https://github.com/ros-drivers/velodyne/issues/17
|
||||
.. _`#18`: https://github.com/ros-drivers/velodyne/issues/18
|
||||
.. _`#20`: https://github.com/ros-drivers/velodyne/issues/20
|
||||
@@ -0,0 +1,94 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(velodyne_driver)
|
||||
|
||||
# Set minimum C++ standard to C++11
|
||||
if (NOT "${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}")
|
||||
message(STATUS "Changing CXX_STANDARD from C++98 to C++11")
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
|
||||
elseif ("${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}" STREQUAL "98")
|
||||
message(STATUS "Changing CXX_STANDARD from C++98 to C++11")
|
||||
set(CMAKE_CXX_STANDARD 11)
|
||||
endif()
|
||||
|
||||
set(${PROJECT_NAME}_CATKIN_DEPS
|
||||
diagnostic_updater
|
||||
dynamic_reconfigure
|
||||
nodelet
|
||||
roscpp
|
||||
tf
|
||||
velodyne_msgs)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS ${${PROJECT_NAME}_CATKIN_DEPS} roslint)
|
||||
|
||||
# This driver uses Boost threads
|
||||
find_package(Boost REQUIRED COMPONENTS thread)
|
||||
|
||||
# libpcap provides no pkg-config or find_package module:
|
||||
set(libpcap_LIBRARIES -lpcap)
|
||||
|
||||
include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS})
|
||||
|
||||
# Generate dynamic_reconfigure server
|
||||
generate_dynamic_reconfigure_options(cfg/VelodyneNode.cfg)
|
||||
|
||||
# objects needed by other ROS packages that depend on this one
|
||||
catkin_package(CATKIN_DEPENDS ${${PROJECT_NAME}_CATKIN_DEPS}
|
||||
INCLUDE_DIRS include
|
||||
LIBRARIES velodyne_input)
|
||||
|
||||
# compile the driver and input library
|
||||
add_subdirectory(src/lib)
|
||||
add_subdirectory(src/driver)
|
||||
|
||||
install(DIRECTORY include/${PROJECT_NAME}/
|
||||
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
|
||||
install(FILES nodelet_velodyne.xml
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
|
||||
install(DIRECTORY launch/
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch)
|
||||
install(PROGRAMS src/vdump
|
||||
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
|
||||
|
||||
roslint_cpp()
|
||||
|
||||
if (CATKIN_ENABLE_TESTING)
|
||||
|
||||
# these dependencies are only needed for unit testing
|
||||
find_package(roslaunch REQUIRED)
|
||||
find_package(rostest REQUIRED)
|
||||
|
||||
# Download packet capture (PCAP) files containing test data.
|
||||
# Store them in devel-space, so rostest can easily find them.
|
||||
catkin_download_test_data(
|
||||
${PROJECT_NAME}_tests_class.pcap
|
||||
http://download.ros.org/data/velodyne/class.pcap
|
||||
DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/tests
|
||||
MD5 65808d25772101358a3719b451b3d015)
|
||||
catkin_download_test_data(
|
||||
${PROJECT_NAME}_tests_32e.pcap
|
||||
http://download.ros.org/data/velodyne/32e.pcap
|
||||
DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/tests
|
||||
MD5 e41d02aac34f0967c03a5597e1d554a9)
|
||||
catkin_download_test_data(
|
||||
${PROJECT_NAME}_tests_vlp16.pcap
|
||||
http://download.ros.org/data/velodyne/vlp16.pcap
|
||||
DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/tests
|
||||
MD5 f45c2bb1d7ee358274e423ea3b66fd73)
|
||||
|
||||
# unit tests
|
||||
add_rostest(tests/pcap_node_hertz.test)
|
||||
add_rostest(tests/pcap_nodelet_hertz.test)
|
||||
add_rostest(tests/pcap_32e_node_hertz.test)
|
||||
add_rostest(tests/pcap_32e_nodelet_hertz.test)
|
||||
add_rostest(tests/pcap_vlp16_node_hertz.test)
|
||||
add_rostest(tests/pcap_vlp16_nodelet_hertz.test)
|
||||
|
||||
# parse check all the launch/*.launch files
|
||||
roslaunch_add_file_check(launch)
|
||||
|
||||
# unit test
|
||||
catkin_add_gtest(time_test tests/timeconversiontest.cpp)
|
||||
target_link_libraries(time_test
|
||||
${catkin_LIBRARIES}
|
||||
${Boost_LIBRARIES})
|
||||
endif (CATKIN_ENABLE_TESTING)
|
||||
@@ -0,0 +1,15 @@
|
||||
#!/usr/bin/env python
|
||||
PACKAGE = "velodyne_driver"
|
||||
NODE_NAME = "velodyne_node"
|
||||
PARAMS_NAME = "VelodyneNode"
|
||||
|
||||
from math import pi
|
||||
from dynamic_reconfigure.parameter_generator_catkin import *
|
||||
|
||||
gen = ParameterGenerator()
|
||||
|
||||
gen.add("time_offset", double_t, 1, "A manually calibrated offset (in seconds) to add to the timestamp before publication of a message.",
|
||||
0.0, -1.0, 1.0)
|
||||
gen.add("enabled", bool_t, 2, "Switch to enable and disable lidar packet consumption", True);
|
||||
|
||||
exit(gen.generate(PACKAGE, NODE_NAME, PARAMS_NAME))
|
||||
@@ -0,0 +1,97 @@
|
||||
// Copyright (C) 2012 Austin Robot Technology, Jack O'Quin
|
||||
// All rights reserved.
|
||||
//
|
||||
// Software License Agreement (BSD License 2.0)
|
||||
//
|
||||
// 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 {copyright_holder} 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.
|
||||
|
||||
#ifndef VELODYNE_DRIVER_DRIVER_H
|
||||
#define VELODYNE_DRIVER_DRIVER_H
|
||||
|
||||
#include <string>
|
||||
#include <ros/ros.h>
|
||||
#include <diagnostic_updater/diagnostic_updater.h>
|
||||
#include <diagnostic_updater/publisher.h>
|
||||
#include <dynamic_reconfigure/server.h>
|
||||
|
||||
#include <velodyne_driver/input.h>
|
||||
#include <velodyne_driver/VelodyneNodeConfig.h>
|
||||
|
||||
namespace velodyne_driver
|
||||
{
|
||||
|
||||
class VelodyneDriver
|
||||
{
|
||||
public:
|
||||
VelodyneDriver(ros::NodeHandle node,
|
||||
ros::NodeHandle private_nh,
|
||||
std::string const & node_name = ros::this_node::getName());
|
||||
~VelodyneDriver() {}
|
||||
|
||||
bool poll(void);
|
||||
|
||||
private:
|
||||
// Callback for dynamic reconfigure
|
||||
void callback(velodyne_driver::VelodyneNodeConfig &config,
|
||||
uint32_t level);
|
||||
// Callback for diagnostics update for lost communication with vlp
|
||||
void diagTimerCallback(const ros::TimerEvent&event);
|
||||
|
||||
// Pointer to dynamic reconfigure service srv_
|
||||
boost::shared_ptr<dynamic_reconfigure::Server<velodyne_driver::
|
||||
VelodyneNodeConfig> > srv_;
|
||||
|
||||
// configuration parameters
|
||||
struct
|
||||
{
|
||||
std::string frame_id; // tf frame ID
|
||||
std::string model; // device model name
|
||||
int npackets; // number of packets to collect
|
||||
double rpm; // device rotation rate (RPMs)
|
||||
int cut_angle; // cutting angle in 1/100°
|
||||
double time_offset; // time in seconds added to each velodyne time stamp
|
||||
bool enabled; // polling is enabled
|
||||
bool timestamp_first_packet;
|
||||
}
|
||||
config_;
|
||||
|
||||
boost::shared_ptr<Input> input_;
|
||||
ros::Publisher output_;
|
||||
int last_azimuth_;
|
||||
|
||||
/* diagnostics updater */
|
||||
ros::Timer diag_timer_;
|
||||
diagnostic_updater::Updater diagnostics_;
|
||||
double diag_min_freq_;
|
||||
double diag_max_freq_;
|
||||
boost::shared_ptr<diagnostic_updater::TopicDiagnostic> diag_topic_;
|
||||
};
|
||||
|
||||
} // namespace velodyne_driver
|
||||
|
||||
#endif // VELODYNE_DRIVER_DRIVER_H
|
||||
@@ -0,0 +1,149 @@
|
||||
// Copyright (C) 2007, 2009, 2010, 2012, 2015Yaxin Liu, Patrick Beeson, Austin Robot Technology, Jack O'Quin
|
||||
// All rights reserved.
|
||||
//
|
||||
// Software License Agreement (BSD License 2.0)
|
||||
//
|
||||
// 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 {copyright_holder} 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.
|
||||
|
||||
/** @file
|
||||
*
|
||||
* Velodyne 3D LIDAR data input classes
|
||||
*
|
||||
* These classes provide raw Velodyne LIDAR input packets from
|
||||
* either a live socket interface or a previously-saved PCAP dump
|
||||
* file.
|
||||
*
|
||||
* Classes:
|
||||
*
|
||||
* velodyne::Input -- base class for accessing the data
|
||||
* independently of its source
|
||||
*
|
||||
* velodyne::InputSocket -- derived class reads live data from the
|
||||
* device via a UDP socket
|
||||
*
|
||||
* velodyne::InputPCAP -- derived class provides a similar interface
|
||||
* from a PCAP dump file
|
||||
*/
|
||||
|
||||
#ifndef VELODYNE_DRIVER_INPUT_H
|
||||
#define VELODYNE_DRIVER_INPUT_H
|
||||
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <pcap.h>
|
||||
#include <netinet/in.h>
|
||||
#include <string>
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <velodyne_msgs/VelodynePacket.h>
|
||||
|
||||
namespace velodyne_driver
|
||||
{
|
||||
|
||||
static uint16_t DATA_PORT_NUMBER = 2368; // default data port
|
||||
static uint16_t POSITION_PORT_NUMBER = 8308; // default position port
|
||||
|
||||
/** @brief Velodyne input base class */
|
||||
class Input
|
||||
{
|
||||
public:
|
||||
Input(ros::NodeHandle private_nh, uint16_t port);
|
||||
virtual ~Input() {}
|
||||
|
||||
/** @brief Read one Velodyne packet.
|
||||
*
|
||||
* @param pkt points to VelodynePacket message
|
||||
*
|
||||
* @returns 0 if successful,
|
||||
* -1 if end of file
|
||||
* > 0 if incomplete packet (is this possible?)
|
||||
*/
|
||||
virtual int getPacket(velodyne_msgs::VelodynePacket *pkt,
|
||||
const double time_offset) = 0;
|
||||
|
||||
protected:
|
||||
ros::NodeHandle private_nh_;
|
||||
uint16_t port_;
|
||||
std::string devip_str_;
|
||||
bool gps_time_;
|
||||
};
|
||||
|
||||
/** @brief Live Velodyne input from socket. */
|
||||
class InputSocket: public Input
|
||||
{
|
||||
public:
|
||||
InputSocket(ros::NodeHandle private_nh,
|
||||
uint16_t port = DATA_PORT_NUMBER);
|
||||
virtual ~InputSocket();
|
||||
|
||||
virtual int getPacket(velodyne_msgs::VelodynePacket *pkt,
|
||||
const double time_offset);
|
||||
void setDeviceIP(const std::string& ip);
|
||||
|
||||
private:
|
||||
int sockfd_;
|
||||
in_addr devip_;
|
||||
};
|
||||
|
||||
|
||||
/** @brief Velodyne input from PCAP dump file.
|
||||
*
|
||||
* Dump files can be grabbed by libpcap, Velodyne's DSR software,
|
||||
* ethereal, wireshark, tcpdump, or the \ref vdump_command.
|
||||
*/
|
||||
class InputPCAP: public Input
|
||||
{
|
||||
public:
|
||||
InputPCAP(ros::NodeHandle private_nh,
|
||||
uint16_t port = DATA_PORT_NUMBER,
|
||||
double packet_rate = 0.0,
|
||||
std::string filename = "",
|
||||
bool read_once = false,
|
||||
bool read_fast = false,
|
||||
double repeat_delay = 0.0);
|
||||
virtual ~InputPCAP();
|
||||
|
||||
virtual int getPacket(velodyne_msgs::VelodynePacket *pkt,
|
||||
const double time_offset);
|
||||
void setDeviceIP(const std::string& ip);
|
||||
|
||||
private:
|
||||
ros::Rate packet_rate_;
|
||||
std::string filename_;
|
||||
pcap_t *pcap_;
|
||||
bpf_program pcap_packet_filter_;
|
||||
char errbuf_[PCAP_ERRBUF_SIZE];
|
||||
bool empty_;
|
||||
bool read_once_;
|
||||
bool read_fast_;
|
||||
double repeat_delay_;
|
||||
};
|
||||
|
||||
} // namespace velodyne_driver
|
||||
|
||||
#endif // VELODYNE_DRIVER_INPUT_H
|
||||
@@ -0,0 +1,77 @@
|
||||
// Copyright (C) 2010, 2019 Austin Robot Technology, Jack O'Quin, Joshua Whitley
|
||||
// All rights reserved.
|
||||
//
|
||||
// Software License Agreement (BSD License 2.0)
|
||||
//
|
||||
// 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 {copyright_holder} 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.
|
||||
|
||||
/** \file
|
||||
*
|
||||
* Velodyne HDL-64E 3D LIDAR laser ring sequence.
|
||||
*
|
||||
* \author Jack O'Quin
|
||||
*/
|
||||
|
||||
#ifndef VELODYNE_DRIVER_RING_SEQUENCE_H
|
||||
#define VELODYNE_DRIVER_RING_SEQUENCE_H
|
||||
|
||||
namespace velodyne
|
||||
{
|
||||
|
||||
// number of lasers
|
||||
const int N_LASERS = 64;
|
||||
|
||||
// ring sequence for device laser numbers
|
||||
const int LASER_SEQUENCE[N_LASERS] =
|
||||
{
|
||||
6, 7, 10, 11, 0, 1, 4, 5,
|
||||
8, 9, 14, 15, 18, 19, 22, 23,
|
||||
12, 13, 16, 17, 20, 21, 26, 27,
|
||||
30, 31, 2, 3, 24, 25, 28, 29,
|
||||
38, 39, 42, 43, 32, 33, 36, 37,
|
||||
40, 41, 46, 47, 50, 51, 54, 55,
|
||||
44, 45, 48, 49, 52, 53, 58, 59,
|
||||
62, 63, 34, 35, 56, 57, 60, 61
|
||||
};
|
||||
|
||||
// convert laser number to ring sequence (inverse of LASER_SEQUENCE)
|
||||
const int LASER_RING[N_LASERS] =
|
||||
{
|
||||
4, 5, 26, 27, 6, 7, 0, 1,
|
||||
8, 9, 2, 3, 16, 17, 10, 11,
|
||||
18, 19, 12, 13, 20, 21, 14, 15,
|
||||
28, 29, 22, 23, 30, 31, 24, 25,
|
||||
36, 37, 58, 59, 38, 39, 32, 33,
|
||||
40, 41, 34, 35, 48, 49, 42, 43,
|
||||
50, 51, 44, 45, 52, 53, 46, 47,
|
||||
60, 61, 54, 55, 62, 63, 56, 57
|
||||
};
|
||||
|
||||
} // namespace velodyne
|
||||
|
||||
#endif // VELODYNE_DRIVER_RING_SEQUENCE_H
|
||||
@@ -0,0 +1,78 @@
|
||||
// Copyright (C) 2019 Matthew Pitropov, Joshua Whitley
|
||||
// All rights reserved.
|
||||
//
|
||||
// Software License Agreement (BSD License 2.0)
|
||||
//
|
||||
// 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 {copyright_holder} 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.
|
||||
|
||||
#ifndef VELODYNE_DRIVER_TIME_CONVERSION_HPP
|
||||
#define VELODYNE_DRIVER_TIME_CONVERSION_HPP
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <ros/time.h>
|
||||
|
||||
/** @brief Function used to check that hour assigned to timestamp in conversion is
|
||||
* correct. Velodyne only returns time since the top of the hour, so if the computer clock
|
||||
* and the velodyne clock (gps-synchronized) are a little off, there is a chance the wrong
|
||||
* hour may be associated with the timestamp
|
||||
*
|
||||
* @param stamp timestamp recovered from velodyne
|
||||
* @param nominal_stamp time coming from computer's clock
|
||||
* @return timestamp from velodyne, possibly shifted by 1 hour if the function arguments
|
||||
* disagree by more than a half-hour.
|
||||
*/
|
||||
ros::Time resolveHourAmbiguity(const ros::Time &stamp, const ros::Time &nominal_stamp) {
|
||||
const int HALFHOUR_TO_SEC = 1800;
|
||||
ros::Time retval = stamp;
|
||||
if (nominal_stamp.sec > stamp.sec) {
|
||||
if (nominal_stamp.sec - stamp.sec > HALFHOUR_TO_SEC) {
|
||||
retval.sec = retval.sec + 2*HALFHOUR_TO_SEC;
|
||||
}
|
||||
} else if (stamp.sec - nominal_stamp.sec > HALFHOUR_TO_SEC) {
|
||||
retval.sec = retval.sec - 2*HALFHOUR_TO_SEC;
|
||||
}
|
||||
return retval;
|
||||
}
|
||||
|
||||
ros::Time rosTimeFromGpsTimestamp(const uint8_t * const data) {
|
||||
const int HOUR_TO_SEC = 3600;
|
||||
// time for each packet is a 4 byte uint
|
||||
// It is the number of microseconds from the top of the hour
|
||||
uint32_t usecs = (uint32_t) ( ((uint32_t) data[3]) << 24 |
|
||||
((uint32_t) data[2] ) << 16 |
|
||||
((uint32_t) data[1] ) << 8 |
|
||||
((uint32_t) data[0] ));
|
||||
ros::Time time_nom = ros::Time::now(); // use this to recover the hour
|
||||
uint32_t cur_hour = time_nom.sec / HOUR_TO_SEC;
|
||||
ros::Time stamp = ros::Time((cur_hour * HOUR_TO_SEC) + (usecs / 1000000),
|
||||
(usecs % 1000000) * 1000);
|
||||
stamp = resolveHourAmbiguity(stamp, time_nom);
|
||||
return stamp;
|
||||
}
|
||||
|
||||
#endif //VELODYNE_DRIVER_TIME_CONVERSION_HPP
|
||||
@@ -0,0 +1,40 @@
|
||||
<!-- -*- mode: XML -*- -->
|
||||
<!-- start velodyne_driver/DriverNodelet in a nodelet manager -->
|
||||
|
||||
<launch>
|
||||
|
||||
<arg name="device_ip" default="" />
|
||||
<arg name="frame_id" default="velodyne" />
|
||||
<arg name="manager" default="$(arg frame_id)_nodelet_manager" />
|
||||
<arg name="model" default="64E" />
|
||||
<arg name="pcap" default="" />
|
||||
<arg name="port" default="2368" />
|
||||
<arg name="read_fast" default="false" />
|
||||
<arg name="read_once" default="false" />
|
||||
<arg name="repeat_delay" default="0.0" />
|
||||
<arg name="rpm" default="600.0" />
|
||||
<arg name="gps_time" default="false" />
|
||||
<arg name="cut_angle" default="-0.01" />
|
||||
<arg name="timestamp_first_packet" default="false" />
|
||||
|
||||
<!-- start nodelet manager -->
|
||||
<node pkg="nodelet" type="nodelet" name="$(arg manager)" args="manager" output="screen"/>
|
||||
|
||||
<!-- load driver nodelet into it -->
|
||||
<node pkg="nodelet" type="nodelet" name="$(arg manager)_driver"
|
||||
args="load velodyne_driver/DriverNodelet $(arg manager)" >
|
||||
<param name="device_ip" value="$(arg device_ip)" />
|
||||
<param name="frame_id" value="$(arg frame_id)"/>
|
||||
<param name="model" value="$(arg model)"/>
|
||||
<param name="pcap" value="$(arg pcap)"/>
|
||||
<param name="port" value="$(arg port)" />
|
||||
<param name="read_fast" value="$(arg read_fast)"/>
|
||||
<param name="read_once" value="$(arg read_once)"/>
|
||||
<param name="repeat_delay" value="$(arg repeat_delay)"/>
|
||||
<param name="rpm" value="$(arg rpm)"/>
|
||||
<param name="gps_time" value="$(arg gps_time)"/>
|
||||
<param name="cut_angle" value="$(arg cut_angle)"/>
|
||||
<param name="timestamp_first_packet" value="$(arg timestamp_first_packet)"/>
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
@@ -0,0 +1,77 @@
|
||||
/**
|
||||
\mainpage
|
||||
|
||||
\htmlinclude manifest.html
|
||||
|
||||
ROS device driver for Velodyne 3D LIDARs.
|
||||
|
||||
\section read Velodyne device driver
|
||||
|
||||
ROS device driver node that captures Velodyne 3D LIDAR data and
|
||||
publishes it to the \b velodyne_msgs/VelodyneScan topic.
|
||||
|
||||
\subsection read_examples Examples
|
||||
|
||||
Read the Velodyne input socket as fast as possible. Publish each
|
||||
complete revolution to \b velodyne/rawscan.
|
||||
|
||||
\verbatim
|
||||
$ rosrun velodyne_driver velodyne_node
|
||||
\endverbatim
|
||||
|
||||
Read previously captured Velodyne packets from dump.pcap file.
|
||||
Publish messages to \b velodyne/rawscan at approximately 10 Hz rate.
|
||||
|
||||
Dump files can be grabbed by libpcap, Velodyne's DSR software,
|
||||
ethereal, wireshark, tcpdump, or the velodyne_driver vdump command.
|
||||
|
||||
\verbatim
|
||||
$ rosrun velodyne_driver velodyne_node _pcap:=dump.pcap
|
||||
\endverbatim
|
||||
|
||||
\subsection read_names ROS names
|
||||
|
||||
Node name: \b velodyne_node
|
||||
|
||||
Publishes: \b velodyne_packets raw Velodyne data packets for one
|
||||
entire revolution of the device.
|
||||
|
||||
Parameters:
|
||||
|
||||
- \b ~pcap (string): PCAP dump input file name (default: use real device)
|
||||
- \b ~input/read_once (bool): if true, read input file only once
|
||||
(default false).
|
||||
- \b ~input/read_fast (bool): if true, read input file as fast as
|
||||
possible (default false).
|
||||
- \b ~input/repeat_delay (double): number of seconds to delay before
|
||||
repeating input file (default: 0.0).
|
||||
|
||||
\section vdump_command Vdump Command
|
||||
|
||||
The vdump command dumps raw data from the Velodyne LIDAR in PCAP
|
||||
format. It is a shell script wrapper with some obscure options for
|
||||
the powerful tcpdump command.
|
||||
|
||||
Other methods of acquiring PCAP data include using tcpdump directly,
|
||||
wireshark, Velodyne's DSR software, and programming with libpcap.
|
||||
|
||||
\subsection vdump_usage Usage
|
||||
|
||||
\verbatim
|
||||
rosrun velodyne_driver vdump <file_prefix> [ <interface> ]
|
||||
|
||||
<file_prefix> file name to dump (with 3-digit number suffix)
|
||||
<interface> interface to read from (default: "eth1")
|
||||
\endverbatim
|
||||
|
||||
\subsection vdump_examples Examples
|
||||
|
||||
Dump Velodyne packets to a series of files named "pcap-000",
|
||||
"pcap-001", etc. Each file will be about 100MB, holding a little more
|
||||
than 30 seconds of Velodyne packets. Type ^C when finished.
|
||||
|
||||
\verbatim
|
||||
$ rosrun velodyne_driver vdump pcap- eth0
|
||||
\endverbatim
|
||||
|
||||
*/
|
||||
@@ -0,0 +1,9 @@
|
||||
<library path="lib/libdriver_nodelet">
|
||||
<class name="velodyne_driver/DriverNodelet"
|
||||
type="velodyne_driver::DriverNodelet"
|
||||
base_class_type="nodelet::Nodelet">
|
||||
<description>
|
||||
Publish raw Velodyne data packets.
|
||||
</description>
|
||||
</class>
|
||||
</library>
|
||||
@@ -0,0 +1,38 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>velodyne_driver</name>
|
||||
<version>1.5.2</version>
|
||||
<description>
|
||||
ROS device driver for Velodyne 3D LIDARs.
|
||||
</description>
|
||||
<maintainer email="josh.whitley@autoware.org">Josh Whitley</maintainer>
|
||||
<maintainer email="brice.rebsamen@gmail.com">Brice Rebsamen</maintainer>
|
||||
<author>Jack O'Quin</author>
|
||||
<author>Patrick Beeson</author>
|
||||
<author>Michael Quinlan</author>
|
||||
<author>Yaxin Liu </author>
|
||||
<license>BSD</license>
|
||||
|
||||
<url type="website">http://www.ros.org/wiki/velodyne_driver</url>
|
||||
<url type="repository">https://github.com/ros-drivers/velodyne</url>
|
||||
<url type="bugtracker">https://github.com/ros-drivers/velodyne/issues</url>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<build_depend>roslint</build_depend>
|
||||
|
||||
<depend>diagnostic_updater</depend>
|
||||
<depend>dynamic_reconfigure</depend>
|
||||
<depend>libpcap</depend>
|
||||
<depend>nodelet</depend>
|
||||
<depend>roscpp</depend>
|
||||
<depend>tf</depend>
|
||||
<depend>velodyne_msgs</depend>
|
||||
|
||||
<test_depend>roslaunch</test_depend>
|
||||
<test_depend>rostest</test_depend>
|
||||
|
||||
<export>
|
||||
<nodelet plugin="${prefix}/nodelet_velodyne.xml"/>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,26 @@
|
||||
# build the driver node
|
||||
add_executable(velodyne_node velodyne_node.cc driver.cc)
|
||||
add_dependencies(velodyne_node velodyne_driver_gencfg)
|
||||
target_link_libraries(velodyne_node
|
||||
velodyne_input
|
||||
${catkin_LIBRARIES}
|
||||
${libpcap_LIBRARIES}
|
||||
)
|
||||
|
||||
# build the nodelet version
|
||||
add_library(driver_nodelet nodelet.cc driver.cc)
|
||||
add_dependencies(driver_nodelet velodyne_driver_gencfg)
|
||||
target_link_libraries(driver_nodelet
|
||||
velodyne_input
|
||||
${catkin_LIBRARIES}
|
||||
${libpcap_LIBRARIES}
|
||||
)
|
||||
|
||||
# install runtime files
|
||||
install(TARGETS velodyne_node
|
||||
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
COMPONENT main
|
||||
)
|
||||
install(TARGETS driver_nodelet
|
||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
)
|
||||
@@ -0,0 +1,297 @@
|
||||
// Copyright (C) 2007, 2009-2012 Austin Robot Technology, Patrick Beeson, Jack O'Quin
|
||||
// All rights reserved.
|
||||
//
|
||||
// Software License Agreement (BSD License 2.0)
|
||||
//
|
||||
// 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 {copyright_holder} 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.
|
||||
|
||||
/** \file
|
||||
*
|
||||
* ROS driver implementation for the Velodyne 3D LIDARs
|
||||
*/
|
||||
|
||||
#include <string>
|
||||
#include <cmath>
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <tf/transform_listener.h>
|
||||
#include <velodyne_msgs/VelodyneScan.h>
|
||||
|
||||
#include "velodyne_driver/driver.h"
|
||||
|
||||
namespace velodyne_driver
|
||||
{
|
||||
|
||||
VelodyneDriver::VelodyneDriver(ros::NodeHandle node,
|
||||
ros::NodeHandle private_nh,
|
||||
std::string const & node_name)
|
||||
: diagnostics_(node, private_nh, node_name)
|
||||
{
|
||||
// use private node handle to get parameters
|
||||
private_nh.param("frame_id", config_.frame_id, std::string("velodyne"));
|
||||
std::string tf_prefix = tf::getPrefixParam(private_nh);
|
||||
ROS_DEBUG_STREAM("tf_prefix: " << tf_prefix);
|
||||
config_.frame_id = tf::resolve(tf_prefix, config_.frame_id);
|
||||
|
||||
// get model name, validate string, determine packet rate
|
||||
private_nh.param("model", config_.model, std::string("64E"));
|
||||
double packet_rate; // packet frequency (Hz)
|
||||
std::string model_full_name;
|
||||
if ((config_.model == "64E_S2") ||
|
||||
(config_.model == "64E_S2.1")) // generates 1333312 points per second
|
||||
{ // 1 packet holds 384 points
|
||||
packet_rate = 3472.17; // 1333312 / 384
|
||||
model_full_name = std::string("HDL-") + config_.model;
|
||||
}
|
||||
else if (config_.model == "64E")
|
||||
{
|
||||
packet_rate = 2600.0;
|
||||
model_full_name = std::string("HDL-") + config_.model;
|
||||
}
|
||||
else if (config_.model == "64E_S3") // generates 2222220 points per second (half for strongest and half for lastest)
|
||||
{ // 1 packet holds 384 points
|
||||
packet_rate = 5787.03; // 2222220 / 384
|
||||
model_full_name = std::string("HDL-") + config_.model;
|
||||
}
|
||||
else if (config_.model == "32E")
|
||||
{
|
||||
packet_rate = 1808.0;
|
||||
model_full_name = std::string("HDL-") + config_.model;
|
||||
}
|
||||
else if (config_.model == "32C")
|
||||
{
|
||||
packet_rate = 1507.0;
|
||||
model_full_name = std::string("VLP-") + config_.model;
|
||||
}
|
||||
else if (config_.model == "VLP16")
|
||||
{
|
||||
packet_rate = 754; // 754 Packets/Second for Last or Strongest mode 1508 for dual (VLP-16 User Manual)
|
||||
model_full_name = "VLP-16";
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR_STREAM("unknown Velodyne LIDAR model: " << config_.model);
|
||||
packet_rate = 2600.0;
|
||||
}
|
||||
std::string deviceName(std::string("Velodyne ") + model_full_name);
|
||||
|
||||
private_nh.param("rpm", config_.rpm, 600.0);
|
||||
ROS_INFO_STREAM(deviceName << " rotating at " << config_.rpm << " RPM");
|
||||
double frequency = (config_.rpm / 60.0); // expected Hz rate
|
||||
|
||||
// default number of packets for each scan is a single revolution
|
||||
// (fractions rounded up)
|
||||
config_.npackets = (int) ceil(packet_rate / frequency);
|
||||
private_nh.getParam("npackets", config_.npackets);
|
||||
ROS_INFO_STREAM("publishing " << config_.npackets << " packets per scan");
|
||||
|
||||
// if we are timestamping based on the first or last packet in the scan
|
||||
private_nh.param("timestamp_first_packet", config_.timestamp_first_packet, false);
|
||||
if (config_.timestamp_first_packet)
|
||||
ROS_INFO("Setting velodyne scan start time to timestamp of first packet");
|
||||
|
||||
std::string dump_file;
|
||||
private_nh.param("pcap", dump_file, std::string(""));
|
||||
|
||||
double cut_angle;
|
||||
private_nh.param("cut_angle", cut_angle, -0.01);
|
||||
if (cut_angle < 0.0)
|
||||
{
|
||||
ROS_INFO_STREAM("Cut at specific angle feature deactivated.");
|
||||
}
|
||||
else if (cut_angle < (2*M_PI))
|
||||
{
|
||||
ROS_INFO_STREAM("Cut at specific angle feature activated. "
|
||||
"Cutting velodyne points always at " << cut_angle << " rad.");
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR_STREAM("cut_angle parameter is out of range. Allowed range is "
|
||||
<< "between 0.0 and 2*PI or negative values to deactivate this feature.");
|
||||
cut_angle = -0.01;
|
||||
}
|
||||
|
||||
// Convert cut_angle from radian to one-hundredth degree,
|
||||
// which is used in velodyne packets
|
||||
config_.cut_angle = int((cut_angle*360/(2*M_PI))*100);
|
||||
|
||||
int udp_port;
|
||||
private_nh.param("port", udp_port, (int) DATA_PORT_NUMBER);
|
||||
|
||||
// Initialize dynamic reconfigure
|
||||
srv_ = boost::make_shared <dynamic_reconfigure::Server<velodyne_driver::
|
||||
VelodyneNodeConfig> > (private_nh);
|
||||
dynamic_reconfigure::Server<velodyne_driver::VelodyneNodeConfig>::
|
||||
CallbackType f;
|
||||
f = boost::bind (&VelodyneDriver::callback, this, _1, _2);
|
||||
srv_->setCallback (f); // Set callback function und call initially
|
||||
|
||||
// initialize diagnostics
|
||||
diagnostics_.setHardwareID(deviceName);
|
||||
const double diag_freq = packet_rate/config_.npackets;
|
||||
diag_max_freq_ = diag_freq;
|
||||
diag_min_freq_ = diag_freq;
|
||||
ROS_INFO("expected frequency: %.3f (Hz)", diag_freq);
|
||||
|
||||
using namespace diagnostic_updater;
|
||||
diag_topic_.reset(new TopicDiagnostic("velodyne_packets", diagnostics_,
|
||||
FrequencyStatusParam(&diag_min_freq_,
|
||||
&diag_max_freq_,
|
||||
0.1, 10),
|
||||
TimeStampStatusParam()));
|
||||
diag_timer_ = private_nh.createTimer(ros::Duration(0.2), &VelodyneDriver::diagTimerCallback,this);
|
||||
|
||||
config_.enabled = true;
|
||||
|
||||
// open Velodyne input device or file
|
||||
if (dump_file != "") // have PCAP file?
|
||||
{
|
||||
// read data from packet capture file
|
||||
input_.reset(new velodyne_driver::InputPCAP(private_nh, udp_port,
|
||||
packet_rate, dump_file));
|
||||
}
|
||||
else
|
||||
{
|
||||
// read data from live socket
|
||||
input_.reset(new velodyne_driver::InputSocket(private_nh, udp_port));
|
||||
}
|
||||
|
||||
// raw packet output topic
|
||||
output_ =
|
||||
node.advertise<velodyne_msgs::VelodyneScan>("velodyne_packets", 10);
|
||||
|
||||
last_azimuth_ = -1;
|
||||
}
|
||||
|
||||
/** poll the device
|
||||
*
|
||||
* @returns true unless end of file reached
|
||||
*/
|
||||
bool VelodyneDriver::poll(void)
|
||||
{
|
||||
if (!config_.enabled) {
|
||||
// If we are not enabled exit once a second to let the caller handle
|
||||
// anything it might need to, such as if it needs to exit.
|
||||
ros::Duration(1).sleep();
|
||||
return true;
|
||||
}
|
||||
|
||||
// Allocate a new shared pointer for zero-copy sharing with other nodelets.
|
||||
velodyne_msgs::VelodyneScanPtr scan(new velodyne_msgs::VelodyneScan);
|
||||
|
||||
if( config_.cut_angle >= 0) //Cut at specific angle feature enabled
|
||||
{
|
||||
scan->packets.reserve(config_.npackets);
|
||||
velodyne_msgs::VelodynePacket tmp_packet;
|
||||
while(true)
|
||||
{
|
||||
while(true)
|
||||
{
|
||||
int rc = input_->getPacket(&tmp_packet, config_.time_offset);
|
||||
if (rc == 0) break; // got a full packet?
|
||||
if (rc < 0) return false; // end of file reached?
|
||||
}
|
||||
scan->packets.push_back(tmp_packet);
|
||||
|
||||
// Extract base rotation of first block in packet
|
||||
std::size_t azimuth_data_pos = 100*0+2;
|
||||
int azimuth = *( (u_int16_t*) (&tmp_packet.data[azimuth_data_pos]));
|
||||
|
||||
//if first packet in scan, there is no "valid" last_azimuth_
|
||||
if (last_azimuth_ == -1) {
|
||||
last_azimuth_ = azimuth;
|
||||
continue;
|
||||
}
|
||||
if((last_azimuth_ < config_.cut_angle && config_.cut_angle <= azimuth)
|
||||
|| ( config_.cut_angle <= azimuth && azimuth < last_azimuth_)
|
||||
|| (azimuth < last_azimuth_ && last_azimuth_ < config_.cut_angle))
|
||||
{
|
||||
last_azimuth_ = azimuth;
|
||||
break; // Cut angle passed, one full revolution collected
|
||||
}
|
||||
last_azimuth_ = azimuth;
|
||||
}
|
||||
}
|
||||
else // standard behaviour
|
||||
{
|
||||
// Since the velodyne delivers data at a very high rate, keep
|
||||
// reading and publishing scans as fast as possible.
|
||||
scan->packets.resize(config_.npackets);
|
||||
for (int i = 0; i < config_.npackets; ++i)
|
||||
{
|
||||
while (true)
|
||||
{
|
||||
// keep reading until full packet received
|
||||
int rc = input_->getPacket(&scan->packets[i], config_.time_offset);
|
||||
if (rc == 0) break; // got a full packet?
|
||||
if (rc < 0) return false; // end of file reached?
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// publish message using time of last packet read
|
||||
ROS_DEBUG("Publishing a full Velodyne scan.");
|
||||
if (config_.timestamp_first_packet){
|
||||
scan->header.stamp = scan->packets.front().stamp;
|
||||
}
|
||||
else{
|
||||
scan->header.stamp = scan->packets.back().stamp;
|
||||
}
|
||||
scan->header.frame_id = config_.frame_id;
|
||||
output_.publish(scan);
|
||||
|
||||
// notify diagnostics that a message has been published, updating
|
||||
// its status
|
||||
diag_topic_->tick(scan->header.stamp);
|
||||
diagnostics_.update();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void VelodyneDriver::callback(velodyne_driver::VelodyneNodeConfig &config,
|
||||
uint32_t level)
|
||||
{
|
||||
ROS_INFO("Reconfigure Request");
|
||||
if (level & 1)
|
||||
{
|
||||
config_.time_offset = config.time_offset;
|
||||
}
|
||||
if (level & 2)
|
||||
{
|
||||
config_.enabled = config.enabled;
|
||||
}
|
||||
}
|
||||
|
||||
void VelodyneDriver::diagTimerCallback(const ros::TimerEvent &event)
|
||||
{
|
||||
(void)event;
|
||||
// Call necessary to provide an error when no velodyne packets are received
|
||||
diagnostics_.update();
|
||||
}
|
||||
|
||||
} // namespace velodyne_driver
|
||||
@@ -0,0 +1,109 @@
|
||||
// Copyright (C) 2012 Austin Robot Technology, Jack O'Quin
|
||||
// All rights reserved.
|
||||
//
|
||||
// Software License Agreement (BSD License 2.0)
|
||||
//
|
||||
// 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 {copyright_holder} 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.
|
||||
|
||||
/** \file
|
||||
*
|
||||
* ROS driver nodelet for the Velodyne 3D LIDARs
|
||||
*/
|
||||
|
||||
#include <string>
|
||||
#include <boost/thread.hpp>
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include <nodelet/nodelet.h>
|
||||
|
||||
#include "velodyne_driver/driver.h"
|
||||
|
||||
namespace velodyne_driver
|
||||
{
|
||||
|
||||
class DriverNodelet: public nodelet::Nodelet
|
||||
{
|
||||
public:
|
||||
|
||||
DriverNodelet():
|
||||
running_(false)
|
||||
{}
|
||||
|
||||
~DriverNodelet()
|
||||
{
|
||||
if (running_)
|
||||
{
|
||||
NODELET_INFO("shutting down driver thread");
|
||||
running_ = false;
|
||||
deviceThread_->join();
|
||||
NODELET_INFO("driver thread stopped");
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
virtual void onInit(void);
|
||||
virtual void devicePoll(void);
|
||||
|
||||
volatile bool running_; ///< device thread is running
|
||||
boost::shared_ptr<boost::thread> deviceThread_;
|
||||
|
||||
boost::shared_ptr<VelodyneDriver> dvr_; ///< driver implementation class
|
||||
};
|
||||
|
||||
void DriverNodelet::onInit()
|
||||
{
|
||||
// start the driver
|
||||
dvr_.reset(new VelodyneDriver(getNodeHandle(), getPrivateNodeHandle(), getName()));
|
||||
|
||||
// spawn device poll thread
|
||||
running_ = true;
|
||||
deviceThread_ = boost::shared_ptr< boost::thread >
|
||||
(new boost::thread(boost::bind(&DriverNodelet::devicePoll, this)));
|
||||
}
|
||||
|
||||
/** @brief Device poll thread main loop. */
|
||||
void DriverNodelet::devicePoll()
|
||||
{
|
||||
while(ros::ok())
|
||||
{
|
||||
// poll device until end of file
|
||||
running_ = dvr_->poll();
|
||||
if (!running_)
|
||||
ROS_ERROR_THROTTLE(1.0, "DriverNodelet::devicePoll - Failed to poll device.");
|
||||
}
|
||||
running_ = false;
|
||||
}
|
||||
|
||||
} // namespace velodyne_driver
|
||||
|
||||
// Register this plugin with pluginlib. Names must match nodelet_velodyne.xml.
|
||||
//
|
||||
// parameters are: class type, base class type
|
||||
PLUGINLIB_EXPORT_CLASS(velodyne_driver::DriverNodelet, nodelet::Nodelet)
|
||||
@@ -0,0 +1,62 @@
|
||||
// Copyright (C) 2012 Austin Robot Technology, Jack O'Quin
|
||||
// All rights reserved.
|
||||
//
|
||||
// Software License Agreement (BSD License 2.0)
|
||||
//
|
||||
// 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 {copyright_holder} 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.
|
||||
|
||||
/** \file
|
||||
*
|
||||
* ROS driver node for the Velodyne 3D LIDARs.
|
||||
*/
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include "velodyne_driver/driver.h"
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
ros::init(argc, argv, "velodyne_node");
|
||||
ros::NodeHandle node;
|
||||
ros::NodeHandle private_nh("~");
|
||||
|
||||
// start the driver
|
||||
velodyne_driver::VelodyneDriver dvr(node, private_nh);
|
||||
|
||||
// loop until shut down or end of file
|
||||
while(ros::ok())
|
||||
{
|
||||
// poll device until end of file
|
||||
bool polled_ = dvr.poll();
|
||||
if (!polled_)
|
||||
ROS_ERROR_THROTTLE(1.0, "Velodyne - Failed to poll device.");
|
||||
|
||||
ros::spinOnce();
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,12 @@
|
||||
add_library(velodyne_input input.cc)
|
||||
target_link_libraries(velodyne_input
|
||||
${catkin_LIBRARIES}
|
||||
${libpcap_LIBRARIES}
|
||||
)
|
||||
if(catkin_EXPORTED_TARGETS)
|
||||
add_dependencies(velodyne_input ${catkin_EXPORTED_TARGETS})
|
||||
endif()
|
||||
|
||||
install(TARGETS velodyne_input
|
||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
)
|
||||
@@ -0,0 +1,356 @@
|
||||
// Copyright (C) 2007, 2009, 2010, 2015 Austin Robot Technology, Patrick Beeson, Jack O'Quin
|
||||
// All rights reserved.
|
||||
//
|
||||
// Software License Agreement (BSD License 2.0)
|
||||
//
|
||||
// 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 {copyright_holder} 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.
|
||||
|
||||
/** \file
|
||||
*
|
||||
* Input classes for the Velodyne HDL-64E 3D LIDAR:
|
||||
*
|
||||
* Input -- base class used to access the data independently of
|
||||
* its source
|
||||
*
|
||||
* InputSocket -- derived class reads live data from the device
|
||||
* via a UDP socket
|
||||
*
|
||||
* InputPCAP -- derived class provides a similar interface from a
|
||||
* PCAP dump
|
||||
*/
|
||||
|
||||
#include <unistd.h>
|
||||
#include <string>
|
||||
#include <sstream>
|
||||
#include <sys/socket.h>
|
||||
#include <arpa/inet.h>
|
||||
#include <poll.h>
|
||||
#include <errno.h>
|
||||
#include <fcntl.h>
|
||||
#include <sys/file.h>
|
||||
#include <velodyne_driver/input.h>
|
||||
#include <velodyne_driver/time_conversion.hpp>
|
||||
|
||||
namespace velodyne_driver
|
||||
{
|
||||
static const size_t packet_size =
|
||||
sizeof(velodyne_msgs::VelodynePacket().data);
|
||||
|
||||
////////////////////////////////////////////////////////////////////////
|
||||
// Input base class implementation
|
||||
////////////////////////////////////////////////////////////////////////
|
||||
|
||||
/** @brief constructor
|
||||
*
|
||||
* @param private_nh ROS private handle for calling node.
|
||||
* @param port UDP port number.
|
||||
*/
|
||||
Input::Input(ros::NodeHandle private_nh, uint16_t port):
|
||||
private_nh_(private_nh),
|
||||
port_(port)
|
||||
{
|
||||
private_nh.param("device_ip", devip_str_, std::string(""));
|
||||
private_nh.param("gps_time", gps_time_, false);
|
||||
if (!devip_str_.empty())
|
||||
ROS_INFO_STREAM("Only accepting packets from IP address: "
|
||||
<< devip_str_);
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////
|
||||
// InputSocket class implementation
|
||||
////////////////////////////////////////////////////////////////////////
|
||||
|
||||
/** @brief constructor
|
||||
*
|
||||
* @param private_nh ROS private handle for calling node.
|
||||
* @param port UDP port number
|
||||
*/
|
||||
InputSocket::InputSocket(ros::NodeHandle private_nh, uint16_t port):
|
||||
Input(private_nh, port)
|
||||
{
|
||||
sockfd_ = -1;
|
||||
|
||||
if (!devip_str_.empty()) {
|
||||
inet_aton(devip_str_.c_str(),&devip_);
|
||||
}
|
||||
|
||||
// connect to Velodyne UDP port
|
||||
ROS_INFO_STREAM("Opening UDP socket: port " << port);
|
||||
sockfd_ = socket(PF_INET, SOCK_DGRAM, 0);
|
||||
if (sockfd_ == -1)
|
||||
{
|
||||
perror("socket"); // TODO: ROS_ERROR errno
|
||||
return;
|
||||
}
|
||||
|
||||
sockaddr_in my_addr; // my address information
|
||||
memset(&my_addr, 0, sizeof(my_addr)); // initialize to zeros
|
||||
my_addr.sin_family = AF_INET; // host byte order
|
||||
my_addr.sin_port = htons(port); // port in network byte order
|
||||
my_addr.sin_addr.s_addr = INADDR_ANY; // automatically fill in my IP
|
||||
|
||||
if (bind(sockfd_, (sockaddr *)&my_addr, sizeof(sockaddr)) == -1)
|
||||
{
|
||||
perror("bind"); // TODO: ROS_ERROR errno
|
||||
return;
|
||||
}
|
||||
|
||||
if (fcntl(sockfd_,F_SETFL, O_NONBLOCK|FASYNC) < 0)
|
||||
{
|
||||
perror("non-block");
|
||||
return;
|
||||
}
|
||||
|
||||
ROS_DEBUG("Velodyne socket fd is %d\n", sockfd_);
|
||||
}
|
||||
|
||||
/** @brief destructor */
|
||||
InputSocket::~InputSocket(void)
|
||||
{
|
||||
(void) close(sockfd_);
|
||||
}
|
||||
|
||||
/** @brief Get one velodyne packet. */
|
||||
int InputSocket::getPacket(velodyne_msgs::VelodynePacket *pkt, const double time_offset)
|
||||
{
|
||||
double time1 = ros::Time::now().toSec();
|
||||
|
||||
struct pollfd fds[1];
|
||||
fds[0].fd = sockfd_;
|
||||
fds[0].events = POLLIN;
|
||||
static const int POLL_TIMEOUT = 1000; // one second (in msec)
|
||||
|
||||
sockaddr_in sender_address;
|
||||
socklen_t sender_address_len = sizeof(sender_address);
|
||||
|
||||
while (true)
|
||||
{
|
||||
// Unfortunately, the Linux kernel recvfrom() implementation
|
||||
// uses a non-interruptible sleep() when waiting for data,
|
||||
// which would cause this method to hang if the device is not
|
||||
// providing data. We poll() the device first to make sure
|
||||
// the recvfrom() will not block.
|
||||
//
|
||||
// Note, however, that there is a known Linux kernel bug:
|
||||
//
|
||||
// Under Linux, select() may report a socket file descriptor
|
||||
// as "ready for reading", while nevertheless a subsequent
|
||||
// read blocks. This could for example happen when data has
|
||||
// arrived but upon examination has wrong checksum and is
|
||||
// discarded. There may be other circumstances in which a
|
||||
// file descriptor is spuriously reported as ready. Thus it
|
||||
// may be safer to use O_NONBLOCK on sockets that should not
|
||||
// block.
|
||||
|
||||
// poll() until input available
|
||||
do
|
||||
{
|
||||
int retval = poll(fds, 1, POLL_TIMEOUT);
|
||||
if (retval < 0) // poll() error?
|
||||
{
|
||||
if (errno != EINTR)
|
||||
ROS_ERROR("poll() error: %s", strerror(errno));
|
||||
return -1;
|
||||
}
|
||||
if (retval == 0) // poll() timeout?
|
||||
{
|
||||
ROS_WARN("Velodyne poll() timeout");
|
||||
return -1;
|
||||
}
|
||||
if ((fds[0].revents & POLLERR)
|
||||
|| (fds[0].revents & POLLHUP)
|
||||
|| (fds[0].revents & POLLNVAL)) // device error?
|
||||
{
|
||||
ROS_ERROR("poll() reports Velodyne error");
|
||||
return -1;
|
||||
}
|
||||
} while ((fds[0].revents & POLLIN) == 0);
|
||||
|
||||
// Receive packets that should now be available from the
|
||||
// socket using a blocking read.
|
||||
ssize_t nbytes = recvfrom(sockfd_, &pkt->data[0],
|
||||
packet_size, 0,
|
||||
(sockaddr*) &sender_address,
|
||||
&sender_address_len);
|
||||
|
||||
if (nbytes < 0)
|
||||
{
|
||||
if (errno != EWOULDBLOCK)
|
||||
{
|
||||
perror("recvfail");
|
||||
ROS_INFO("recvfail");
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
else if ((size_t) nbytes == packet_size)
|
||||
{
|
||||
// read successful,
|
||||
// if packet is not from the lidar scanner we selected by IP,
|
||||
// continue otherwise we are done
|
||||
if(devip_str_ != ""
|
||||
&& sender_address.sin_addr.s_addr != devip_.s_addr)
|
||||
continue;
|
||||
else
|
||||
break; //done
|
||||
}
|
||||
|
||||
ROS_DEBUG_STREAM("incomplete Velodyne packet read: "
|
||||
<< nbytes << " bytes");
|
||||
}
|
||||
|
||||
if (!gps_time_) {
|
||||
// Average the times at which we begin and end reading. Use that to
|
||||
// estimate when the scan occurred. Add the time offset.
|
||||
double time2 = ros::Time::now().toSec();
|
||||
pkt->stamp = ros::Time((time2 + time1) / 2.0 + time_offset);
|
||||
} else {
|
||||
// time for each packet is a 4 byte uint located starting at offset 1200 in
|
||||
// the data packet
|
||||
pkt->stamp = rosTimeFromGpsTimestamp(&(pkt->data[1200]));
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////
|
||||
// InputPCAP class implementation
|
||||
////////////////////////////////////////////////////////////////////////
|
||||
|
||||
/** @brief constructor
|
||||
*
|
||||
* @param private_nh ROS private handle for calling node.
|
||||
* @param port UDP port number
|
||||
* @param packet_rate expected device packet frequency (Hz)
|
||||
* @param filename PCAP dump file name
|
||||
*/
|
||||
InputPCAP::InputPCAP(ros::NodeHandle private_nh, uint16_t port,
|
||||
double packet_rate, std::string filename,
|
||||
bool read_once, bool read_fast, double repeat_delay):
|
||||
Input(private_nh, port),
|
||||
packet_rate_(packet_rate),
|
||||
filename_(filename)
|
||||
{
|
||||
pcap_ = NULL;
|
||||
empty_ = true;
|
||||
|
||||
// get parameters using private node handle
|
||||
private_nh.param("read_once", read_once_, false);
|
||||
private_nh.param("read_fast", read_fast_, false);
|
||||
private_nh.param("repeat_delay", repeat_delay_, 0.0);
|
||||
|
||||
if (read_once_)
|
||||
ROS_INFO("Read input file only once.");
|
||||
if (read_fast_)
|
||||
ROS_INFO("Read input file as quickly as possible.");
|
||||
if (repeat_delay_ > 0.0)
|
||||
ROS_INFO("Delay %.3f seconds before repeating input file.",
|
||||
repeat_delay_);
|
||||
|
||||
// Open the PCAP dump file
|
||||
ROS_INFO("Opening PCAP file \"%s\"", filename_.c_str());
|
||||
if ((pcap_ = pcap_open_offline(filename_.c_str(), errbuf_) ) == NULL)
|
||||
{
|
||||
ROS_FATAL("Error opening Velodyne socket dump file.");
|
||||
return;
|
||||
}
|
||||
|
||||
std::stringstream filter;
|
||||
if( devip_str_ != "" ) // using specific IP?
|
||||
{
|
||||
filter << "src host " << devip_str_ << " && ";
|
||||
}
|
||||
filter << "udp dst port " << port;
|
||||
pcap_compile(pcap_, &pcap_packet_filter_,
|
||||
filter.str().c_str(), 1, PCAP_NETMASK_UNKNOWN);
|
||||
}
|
||||
|
||||
/** destructor */
|
||||
InputPCAP::~InputPCAP(void)
|
||||
{
|
||||
pcap_close(pcap_);
|
||||
}
|
||||
|
||||
/** @brief Get one velodyne packet. */
|
||||
int InputPCAP::getPacket(velodyne_msgs::VelodynePacket *pkt, const double time_offset)
|
||||
{
|
||||
struct pcap_pkthdr *header;
|
||||
const u_char *pkt_data;
|
||||
|
||||
while (true)
|
||||
{
|
||||
int res;
|
||||
if ((res = pcap_next_ex(pcap_, &header, &pkt_data)) >= 0)
|
||||
{
|
||||
// Skip packets not for the correct port and from the
|
||||
// selected IP address.
|
||||
if (0 == pcap_offline_filter(&pcap_packet_filter_,
|
||||
header, pkt_data))
|
||||
continue;
|
||||
|
||||
// Keep the reader from blowing through the file.
|
||||
if (read_fast_ == false)
|
||||
packet_rate_.sleep();
|
||||
|
||||
memcpy(&pkt->data[0], pkt_data+42, packet_size);
|
||||
pkt->stamp = ros::Time::now(); // time_offset not considered here, as no synchronization required
|
||||
empty_ = false;
|
||||
return 0; // success
|
||||
}
|
||||
|
||||
if (empty_) // no data in file?
|
||||
{
|
||||
ROS_WARN("Error %d reading Velodyne packet: %s",
|
||||
res, pcap_geterr(pcap_));
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (read_once_)
|
||||
{
|
||||
ROS_INFO("end of file reached -- done reading.");
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (repeat_delay_ > 0.0)
|
||||
{
|
||||
ROS_INFO("end of file reached -- delaying %.3f seconds.",
|
||||
repeat_delay_);
|
||||
usleep(rint(repeat_delay_ * 1000000.0));
|
||||
}
|
||||
|
||||
ROS_DEBUG("replaying Velodyne dump file");
|
||||
|
||||
// I can't figure out how to rewind the file, because it
|
||||
// starts with some kind of header. So, close the file
|
||||
// and reopen it with pcap.
|
||||
pcap_close(pcap_);
|
||||
pcap_ = pcap_open_offline(filename_.c_str(), errbuf_);
|
||||
empty_ = true; // maybe the file disappeared?
|
||||
} // loop back and try again
|
||||
}
|
||||
|
||||
} // velodyne namespace
|
||||
@@ -0,0 +1,23 @@
|
||||
#!/bin/bash
|
||||
|
||||
# dump velodyne packets
|
||||
# $Id: vdump 8892 2009-10-24 15:13:57Z joq $
|
||||
|
||||
if [ x$1 = x ]
|
||||
then echo -e "usage:\t`basename $0` file-prefix [ interface ]"
|
||||
echo -e "\n\tfile-prefix is completed with a three-digit number"
|
||||
echo -e "\tinterface default is 'eth0'\n"
|
||||
exit 9
|
||||
fi
|
||||
|
||||
IF=${2:-eth0}
|
||||
UN=`id -un`
|
||||
ID=`id -u`
|
||||
|
||||
echo "acquiring packets on $IF for user $UN; press ^C when done"
|
||||
|
||||
if [ $ID = 0 ]; then
|
||||
/usr/sbin/tcpdump -i $IF -s 0 -C 100 -W 999 -w $1
|
||||
else
|
||||
sudo /usr/sbin/tcpdump -i $IF -Z $UN -s 0 -C 100 -W 999 -w $1
|
||||
fi
|
||||
@@ -0,0 +1,17 @@
|
||||
## Diagnostic aggregator parameters for testing Velodyne diagnostics.
|
||||
#
|
||||
# $ rosparam load $(rospack find velodyne_driver)/tests/diagnostic_agg.yaml
|
||||
# $ rosrun diagnostic_aggregator aggregator_node
|
||||
#
|
||||
diagnostic_aggregator:
|
||||
analyzers:
|
||||
sensors:
|
||||
type: diagnostic_aggregator/AnalyzerGroup
|
||||
path: Sensors
|
||||
analyzers:
|
||||
velodyne:
|
||||
type: diagnostic_aggregator/GenericAnalyzer
|
||||
path: Velodyne HDL
|
||||
timeout: 5.0
|
||||
find_and_remove_prefix: velodyne_nodelet_manager
|
||||
num_items: 1
|
||||
@@ -0,0 +1,30 @@
|
||||
<!-- -*- mode: XML -*- -->
|
||||
<!-- rostest of reading Velodyne 32E PCAP data -->
|
||||
|
||||
<launch>
|
||||
|
||||
<!-- start read with example PCAP file -->
|
||||
<node pkg="velodyne_driver" type="velodyne_node" name="velodyne_node">
|
||||
<param name="model" value="32E"/>
|
||||
<param name="pcap" value="$(find velodyne_driver)/tests/32e.pcap"/>
|
||||
</node>
|
||||
|
||||
<test test-name="pcap_32e_node_hertz_test" pkg="rostest"
|
||||
type="hztest" name="hztest_packets_node_32e" >
|
||||
<param name="hz" value="10.0" />
|
||||
<param name="hzerror" value="3.0" />
|
||||
<param name="test_duration" value="5.0" />
|
||||
<param name="topic" value="velodyne_packets" />
|
||||
<param name="wait_time" value="2.0" />
|
||||
</test>
|
||||
|
||||
<test test-name="node_diagnostics_test"
|
||||
pkg="rostest" type="hztest" name="hztest_diag_node_32e">
|
||||
<param name="hz" value="1.0" />
|
||||
<param name="hzerror" value="0.5" />
|
||||
<param name="test_duration" value="5.0" />
|
||||
<param name="topic" value="diagnostics" />
|
||||
<param name="wait_time" value="2.0" />
|
||||
</test>
|
||||
|
||||
</launch>
|
||||
@@ -0,0 +1,31 @@
|
||||
<!-- -*- mode: XML -*- -->
|
||||
<!-- rostest of publishing a PointCloud from 32E PCAP data. -->
|
||||
|
||||
<launch>
|
||||
|
||||
<!-- start nodelet manager, driver and pointcloud nodelets -->
|
||||
<include file="$(find velodyne_driver)/launch/nodelet_manager.launch">
|
||||
<arg name="model" value="32E"/>
|
||||
<arg name="pcap" value="$(find velodyne_driver)/tests/32e.pcap"/>
|
||||
</include>
|
||||
|
||||
<!-- verify PointCloud publication rate -->
|
||||
<test test-name="pcap_32e_nodelet_hertz_test" pkg="rostest"
|
||||
type="hztest" name="hztest_packets_nodelet_32e" >
|
||||
<param name="hz" value="10.0" />
|
||||
<param name="hzerror" value="3.0" />
|
||||
<param name="test_duration" value="5.0" />
|
||||
<param name="topic" value="velodyne_packets" />
|
||||
<param name="wait_time" value="2.0" />
|
||||
</test>
|
||||
|
||||
<test test-name="nodelet_diagnostics_test"
|
||||
pkg="rostest" type="hztest" name="hztest_diag_nodelet_32e" >
|
||||
<param name="hz" value="1.0" />
|
||||
<param name="hzerror" value="0.5" />
|
||||
<param name="test_duration" value="5.0" />
|
||||
<param name="topic" value="diagnostics" />
|
||||
<param name="wait_time" value="2.0" />
|
||||
</test>
|
||||
|
||||
</launch>
|
||||
@@ -0,0 +1,29 @@
|
||||
<!-- -*- mode: XML -*- -->
|
||||
<!-- rostest of reading Velodyne 64E PCAP files -->
|
||||
|
||||
<launch>
|
||||
|
||||
<!-- start read with example PCAP file -->
|
||||
<node pkg="velodyne_driver" type="velodyne_node" name="velodyne_node">
|
||||
<param name="pcap" value="$(find velodyne_driver)/tests/class.pcap"/>
|
||||
</node>
|
||||
|
||||
<test test-name="pcap_node_hertz_test" pkg="rostest"
|
||||
type="hztest" name="hztest_packets_node_64e" >
|
||||
<param name="hz" value="10.0" />
|
||||
<param name="hzerror" value="3.0" />
|
||||
<param name="test_duration" value="5.0" />
|
||||
<param name="topic" value="velodyne_packets" />
|
||||
<param name="wait_time" value="2.0" />
|
||||
</test>
|
||||
|
||||
<test test-name="node_diagnostics_test"
|
||||
pkg="rostest" type="hztest" name="hztest_diag_node_64e">
|
||||
<param name="hz" value="1.0" />
|
||||
<param name="hzerror" value="0.5" />
|
||||
<param name="test_duration" value="5.0" />
|
||||
<param name="topic" value="diagnostics" />
|
||||
<param name="wait_time" value="2.0" />
|
||||
</test>
|
||||
|
||||
</launch>
|
||||
@@ -0,0 +1,30 @@
|
||||
<!-- -*- mode: XML -*- -->
|
||||
<!-- rostest of publishing a PointCloud from 64E PCAP data. -->
|
||||
|
||||
<launch>
|
||||
|
||||
<!-- start nodelet manager, driver and pointcloud nodelets -->
|
||||
<include file="$(find velodyne_driver)/launch/nodelet_manager.launch">
|
||||
<arg name="pcap" value="$(find velodyne_driver)/tests/class.pcap"/>
|
||||
</include>
|
||||
|
||||
<!-- verify PointCloud publication rate -->
|
||||
<test test-name="pcap_nodelet_hertz_test" pkg="rostest"
|
||||
type="hztest" name="hztest_packets_nodelet_64e" >
|
||||
<param name="hz" value="10.0" />
|
||||
<param name="hzerror" value="3.0" />
|
||||
<param name="test_duration" value="5.0" />
|
||||
<param name="topic" value="velodyne_packets" />
|
||||
<param name="wait_time" value="2.0" />
|
||||
</test>
|
||||
|
||||
<test test-name="nodelet_diagnostics_test"
|
||||
pkg="rostest" type="hztest" name="hztest_diag_nodelet_64e" >
|
||||
<param name="hz" value="1.0" />
|
||||
<param name="hzerror" value="0.5" />
|
||||
<param name="test_duration" value="5.0" />
|
||||
<param name="topic" value="diagnostics" />
|
||||
<param name="wait_time" value="2.0" />
|
||||
</test>
|
||||
|
||||
</launch>
|
||||
@@ -0,0 +1,30 @@
|
||||
<!-- -*- mode: XML -*- -->
|
||||
<!-- rostest of reading Velodyne VLP16 PCAP data -->
|
||||
|
||||
<launch>
|
||||
|
||||
<!-- start read with example PCAP file -->
|
||||
<node pkg="velodyne_driver" type="velodyne_node" name="velodyne_node">
|
||||
<param name="model" value="VLP16"/>
|
||||
<param name="pcap" value="$(find velodyne_driver)/tests/vlp16.pcap"/>
|
||||
</node>
|
||||
|
||||
<test test-name="pcap_vlp16_node_hertz_test" pkg="rostest"
|
||||
type="hztest" name="hztest_packets_node_vlp16" >
|
||||
<param name="hz" value="10.0" />
|
||||
<param name="hzerror" value="3.0" />
|
||||
<param name="test_duration" value="5.0" />
|
||||
<param name="topic" value="velodyne_packets" />
|
||||
<param name="wait_time" value="2.0" />
|
||||
</test>
|
||||
|
||||
<test test-name="node_diagnostics_test"
|
||||
pkg="rostest" type="hztest" name="hztest_diag_node_vlp16">
|
||||
<param name="hz" value="1.0" />
|
||||
<param name="hzerror" value="0.5" />
|
||||
<param name="test_duration" value="5.0" />
|
||||
<param name="topic" value="diagnostics" />
|
||||
<param name="wait_time" value="2.0" />
|
||||
</test>
|
||||
|
||||
</launch>
|
||||
@@ -0,0 +1,31 @@
|
||||
<!-- -*- mode: XML -*- -->
|
||||
<!-- rostest of publishing a PointCloud from VLP16 PCAP data. -->
|
||||
|
||||
<launch>
|
||||
|
||||
<!-- start nodelet manager, driver and pointcloud nodelets -->
|
||||
<include file="$(find velodyne_driver)/launch/nodelet_manager.launch">
|
||||
<arg name="model" value="VLP16"/>
|
||||
<arg name="pcap" value="$(find velodyne_driver)/tests/vlp16.pcap"/>
|
||||
</include>
|
||||
|
||||
<!-- verify PointCloud publication rate -->
|
||||
<test test-name="pcap_vlp16_nodelet_hertz_test" pkg="rostest"
|
||||
type="hztest" name="hztest_packets_nodelet_vlp16" >
|
||||
<param name="hz" value="10.0" />
|
||||
<param name="hzerror" value="3.0" />
|
||||
<param name="test_duration" value="5.0" />
|
||||
<param name="topic" value="velodyne_packets" />
|
||||
<param name="wait_time" value="2.0" />
|
||||
</test>
|
||||
|
||||
<test test-name="nodelet_diagnostics_test"
|
||||
pkg="rostest" type="hztest" name="hztest_diag_nodelet_vlp16" >
|
||||
<param name="hz" value="1.0" />
|
||||
<param name="hzerror" value="0.5" />
|
||||
<param name="test_duration" value="5.0" />
|
||||
<param name="topic" value="diagnostics" />
|
||||
<param name="wait_time" value="2.0" />
|
||||
</test>
|
||||
|
||||
</launch>
|
||||
@@ -0,0 +1,63 @@
|
||||
// Copyright (C) 2019 Matthew Pitropov, Joshua Whitley
|
||||
// All rights reserved.
|
||||
//
|
||||
// Software License Agreement (BSD License 2.0)
|
||||
//
|
||||
// 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 {copyright_holder} 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.
|
||||
|
||||
#include "velodyne_driver/time_conversion.hpp"
|
||||
#include <ros/time.h>
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
TEST(TimeConversion, BytesToTimestamp)
|
||||
{
|
||||
ros::Time::init();
|
||||
ros::Time ros_stamp = ros::Time::now();
|
||||
|
||||
// get the seconds past the hour and multiply by 1million to convert to microseconds
|
||||
// divide nanoseconds by 1000 to convert to microseconds
|
||||
uint32_t since_the_hour = ((ros_stamp.sec % 3600) * 1000000) + ros_stamp.nsec / 1000;
|
||||
|
||||
uint8_t native_format[4];
|
||||
native_format[0] = 0xFF & since_the_hour;
|
||||
native_format[1] = 0xFF & (((uint32_t)since_the_hour) >> 8);
|
||||
native_format[2] = 0xFF & (((uint32_t)since_the_hour) >> 16);
|
||||
native_format[3] = 0xFF & (((uint32_t)since_the_hour) >> 24);
|
||||
|
||||
ros::Time ros_stamp_converted = rosTimeFromGpsTimestamp(native_format);
|
||||
|
||||
ASSERT_EQ(ros_stamp_converted.sec, ros_stamp.sec);
|
||||
ASSERT_NEAR(ros_stamp_converted.nsec, ros_stamp.nsec, 1000);
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
::testing::InitGoogleTest(&argc, argv);
|
||||
int ret = RUN_ALL_TESTS();
|
||||
return ret;
|
||||
}
|
||||
@@ -0,0 +1,42 @@
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
Changelog for package velodyne_laserscan
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
1.5.2 (2019-01-28)
|
||||
------------------
|
||||
|
||||
1.5.1 (2018-12-10)
|
||||
------------------
|
||||
|
||||
1.5.0 (2018-10-19)
|
||||
------------------
|
||||
|
||||
1.4.0 (2018-09-19)
|
||||
------------------
|
||||
* Merge pull request `#170 <https://github.com/ros-drivers/velodyne/issues/170>`_ from ros-drivers/maint/move_header_files
|
||||
* Moving header files to traditional location inside include folders.
|
||||
* Merge pull request `#160 <https://github.com/ros-drivers/velodyne/issues/160>`_ from ros-drivers/maint/updating_package_xml_to_v2
|
||||
* Updated all package.xmls to ver 2. Cleaned up catkin_lint errors.
|
||||
All package.xml files are now compatible with version 2 of the
|
||||
package.xml specification in REP 140. Removed some unnecessary
|
||||
execute permissions on a few files. Fixed a missing test_depend.
|
||||
* Merge pull request `#146 <https://github.com/ros-drivers/velodyne/issues/146>`_ from stsundermann/patch-2
|
||||
Use std::abs instead of fabsf
|
||||
* Merge pull request `#150 <https://github.com/ros-drivers/velodyne/issues/150>`_ from ros-drivers/mikaelarguedas-patch-1
|
||||
* update to use non deprecated pluginlib macro
|
||||
* Use std::abs instead of fabsf
|
||||
cfg\_.resolution is double but fabsf takes a float which may cause truncation of value.
|
||||
* Contributors: Andre Volk, CNR, Joshua Whitley, Mikael Arguedas, Stephan Sundermann
|
||||
|
||||
1.3.0 (2017-11-10)
|
||||
------------------
|
||||
* Merge pull request `#110 <https://github.com/ros-drivers/velodyne/issues/110>`_ from kmhallen/master
|
||||
Added velodyne_laserscan package
|
||||
* Added tests for velodyne_laserscan
|
||||
* Fixed validating PointCloud2 field types
|
||||
* Package.xml format version 2
|
||||
* Merge pull request `#1 <https://github.com/ros-drivers/velodyne/issues/1>`_ from volkandre/master
|
||||
Fixed bug. Laserscans now cover full 360 degrees.
|
||||
* Fixed bug. Laserscans now cover full 360 degrees.
|
||||
* Added velodyne_laserscan package and inserted into existing launch files
|
||||
* Contributors: Joshua Whitley, Kevin Hallenbeck, kmhallen, volkandre
|
||||
@@ -0,0 +1,66 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(velodyne_laserscan)
|
||||
|
||||
# Set minimum C++ standard to C++11
|
||||
if (NOT "${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}")
|
||||
message(STATUS "Changing CXX_STANDARD from C++98 to C++11")
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
|
||||
elseif ("${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}" STREQUAL "98")
|
||||
message(STATUS "Changing CXX_STANDARD from C++98 to C++11")
|
||||
set(CMAKE_CXX_STANDARD 11)
|
||||
endif()
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
nodelet
|
||||
sensor_msgs
|
||||
dynamic_reconfigure
|
||||
roslint
|
||||
)
|
||||
|
||||
generate_dynamic_reconfigure_options(
|
||||
cfg/VelodyneLaserScan.cfg
|
||||
)
|
||||
|
||||
catkin_package(CATKIN_DEPENDS
|
||||
sensor_msgs
|
||||
)
|
||||
|
||||
include_directories(
|
||||
include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
add_library(${PROJECT_NAME}
|
||||
src/velodyne_laserscan.cpp
|
||||
src/nodelet.cpp
|
||||
)
|
||||
add_dependencies(${PROJECT_NAME}
|
||||
${PROJECT_NAME}_gencfg
|
||||
)
|
||||
target_link_libraries(${PROJECT_NAME}
|
||||
${catkin_LIBRARIES}
|
||||
)
|
||||
|
||||
add_executable(${PROJECT_NAME}_node
|
||||
src/node.cpp
|
||||
)
|
||||
target_link_libraries(${PROJECT_NAME}_node
|
||||
${catkin_LIBRARIES}
|
||||
${PROJECT_NAME}
|
||||
)
|
||||
|
||||
install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
|
||||
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
)
|
||||
install(FILES nodelets.xml
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
)
|
||||
|
||||
roslint_cpp()
|
||||
|
||||
if (CATKIN_ENABLE_TESTING)
|
||||
add_subdirectory(tests)
|
||||
endif()
|
||||
|
||||
@@ -0,0 +1,13 @@
|
||||
#! /usr/bin/env python
|
||||
|
||||
PACKAGE='velodyne_laserscan'
|
||||
|
||||
from dynamic_reconfigure.parameter_generator_catkin import *
|
||||
|
||||
gen = ParameterGenerator()
|
||||
|
||||
# Name Type Lvl Description Default Min Max
|
||||
gen.add("ring", int_t, 0, "Ring to extract as laser scan (-1 default)", -1, -1, 31)
|
||||
gen.add("resolution", double_t, 0, "Laser scan angular resolution (rad)", 0.007, 0.001, 0.05)
|
||||
|
||||
exit(gen.generate(PACKAGE, PACKAGE, "VelodyneLaserScan"))
|
||||
@@ -0,0 +1,72 @@
|
||||
// Copyright (C) 2018, 2019 Kevin Hallenbeck, Joshua Whitley
|
||||
// All rights reserved.
|
||||
//
|
||||
// Software License Agreement (BSD License 2.0)
|
||||
//
|
||||
// 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 {copyright_holder} 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.
|
||||
|
||||
#ifndef VELODYNE_LASERSCAN_VELODYNE_LASERSCAN_H
|
||||
#define VELODYNE_LASERSCAN_VELODYNE_LASERSCAN_H
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <sensor_msgs/LaserScan.h>
|
||||
|
||||
#include <boost/thread/mutex.hpp>
|
||||
#include <boost/thread/lock_guard.hpp>
|
||||
|
||||
#include <dynamic_reconfigure/server.h>
|
||||
#include <velodyne_laserscan/VelodyneLaserScanConfig.h>
|
||||
|
||||
namespace velodyne_laserscan
|
||||
{
|
||||
|
||||
class VelodyneLaserScan
|
||||
{
|
||||
public:
|
||||
VelodyneLaserScan(ros::NodeHandle &nh, ros::NodeHandle &nh_priv);
|
||||
|
||||
private:
|
||||
boost::mutex connect_mutex_;
|
||||
void connectCb();
|
||||
void recvCallback(const sensor_msgs::PointCloud2ConstPtr& msg);
|
||||
|
||||
ros::NodeHandle nh_;
|
||||
ros::Subscriber sub_;
|
||||
ros::Publisher pub_;
|
||||
|
||||
VelodyneLaserScanConfig cfg_;
|
||||
dynamic_reconfigure::Server<VelodyneLaserScanConfig> srv_;
|
||||
void reconfig(VelodyneLaserScanConfig& config, uint32_t level);
|
||||
|
||||
unsigned int ring_count_;
|
||||
};
|
||||
|
||||
} // namespace velodyne_laserscan
|
||||
|
||||
#endif // VELODYNE_LASERSCAN_VELODYNE_LASERSCAN_H
|
||||
@@ -0,0 +1,10 @@
|
||||
<library path="libvelodyne_laserscan">
|
||||
<class name="velodyne_laserscan/LaserScanNodelet"
|
||||
type="velodyne_laserscan::LaserScanNodelet"
|
||||
base_class_type="nodelet::Nodelet">
|
||||
<description>
|
||||
Extract a single ring from a Velodyne PointCloud2 and publish
|
||||
as a LaserScan.
|
||||
</description>
|
||||
</class>
|
||||
</library>
|
||||
@@ -0,0 +1,32 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>velodyne_laserscan</name>
|
||||
<version>1.5.2</version>
|
||||
<description>
|
||||
Extract a single ring of a Velodyne PointCloud2 and publish it as a LaserScan message
|
||||
</description>
|
||||
<maintainer email="josh.whitley@autoware.org">Josh Whitley</maintainer>
|
||||
<author>Micho Radovnikovich</author>
|
||||
<author>Kevin Hallenbeck</author>
|
||||
<license>BSD</license>
|
||||
|
||||
<url type="website">http://ros.org/wiki/velodyne_laserscan</url>
|
||||
<url type="repository">https://github.com/ros-drivers/velodyne</url>
|
||||
<url type="bugtracker">https://github.com/ros-drivers/velodyne/issues</url>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<build_depend>roslint</build_depend>
|
||||
|
||||
<depend>roscpp</depend>
|
||||
<depend>nodelet</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>dynamic_reconfigure</depend>
|
||||
|
||||
<test_depend>roslaunch</test_depend>
|
||||
<test_depend>rostest</test_depend>
|
||||
|
||||
<export>
|
||||
<nodelet plugin="${prefix}/nodelets.xml" />
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,49 @@
|
||||
// Copyright (C) 2018, 2019 Kevin Hallenbeck, Joshua Whitley
|
||||
// All rights reserved.
|
||||
//
|
||||
// Software License Agreement (BSD License 2.0)
|
||||
//
|
||||
// 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 {copyright_holder} 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.
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include "velodyne_laserscan/velodyne_laserscan.h"
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
ros::init(argc, argv, "velodyne_laserscan_node");
|
||||
ros::NodeHandle nh;
|
||||
ros::NodeHandle nh_priv("~");
|
||||
|
||||
// create VelodyneLaserScan class
|
||||
velodyne_laserscan::VelodyneLaserScan n(nh, nh_priv);
|
||||
|
||||
// handle callbacks until shut down
|
||||
ros::spin();
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,58 @@
|
||||
// Copyright (C) 2018, 2019 Kevin Hallenbeck, Joshua Whitley
|
||||
// All rights reserved.
|
||||
//
|
||||
// Software License Agreement (BSD License 2.0)
|
||||
//
|
||||
// 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 {copyright_holder} 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.
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include <nodelet/nodelet.h>
|
||||
#include "velodyne_laserscan/velodyne_laserscan.h"
|
||||
|
||||
namespace velodyne_laserscan
|
||||
{
|
||||
|
||||
class LaserScanNodelet: public nodelet::Nodelet
|
||||
{
|
||||
public:
|
||||
LaserScanNodelet() {}
|
||||
~LaserScanNodelet() {}
|
||||
|
||||
private:
|
||||
virtual void onInit()
|
||||
{
|
||||
node_.reset(new VelodyneLaserScan(getNodeHandle(), getPrivateNodeHandle()));
|
||||
}
|
||||
|
||||
boost::shared_ptr<VelodyneLaserScan> node_;
|
||||
};
|
||||
|
||||
} // namespace velodyne_laserscan
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(velodyne_laserscan::LaserScanNodelet, nodelet::Nodelet);
|
||||
@@ -0,0 +1,280 @@
|
||||
// Copyright (C) 2018, 2019 Kevin Hallenbeck, Joshua Whitley
|
||||
// All rights reserved.
|
||||
//
|
||||
// Software License Agreement (BSD License 2.0)
|
||||
//
|
||||
// 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 {copyright_holder} 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.
|
||||
|
||||
#include "velodyne_laserscan/velodyne_laserscan.h"
|
||||
#include <sensor_msgs/point_cloud2_iterator.h>
|
||||
|
||||
namespace velodyne_laserscan
|
||||
{
|
||||
|
||||
VelodyneLaserScan::VelodyneLaserScan(ros::NodeHandle &nh, ros::NodeHandle &nh_priv) :
|
||||
nh_(nh), srv_(nh_priv), ring_count_(0)
|
||||
{
|
||||
ros::SubscriberStatusCallback connect_cb = boost::bind(&VelodyneLaserScan::connectCb, this);
|
||||
pub_ = nh.advertise<sensor_msgs::LaserScan>("scan", 10, connect_cb, connect_cb);
|
||||
|
||||
srv_.setCallback(boost::bind(&VelodyneLaserScan::reconfig, this, _1, _2));
|
||||
}
|
||||
|
||||
void VelodyneLaserScan::connectCb()
|
||||
{
|
||||
boost::lock_guard<boost::mutex> lock(connect_mutex_);
|
||||
if (!pub_.getNumSubscribers())
|
||||
{
|
||||
sub_.shutdown();
|
||||
}
|
||||
else if (!sub_)
|
||||
{
|
||||
sub_ = nh_.subscribe("velodyne_points", 10, &VelodyneLaserScan::recvCallback, this);
|
||||
}
|
||||
}
|
||||
|
||||
void VelodyneLaserScan::recvCallback(const sensor_msgs::PointCloud2ConstPtr& msg)
|
||||
{
|
||||
// Latch ring count
|
||||
if (!ring_count_)
|
||||
{
|
||||
// Check for PointCloud2 field 'ring'
|
||||
bool found = false;
|
||||
for (size_t i = 0; i < msg->fields.size(); i++)
|
||||
{
|
||||
if (msg->fields[i].datatype == sensor_msgs::PointField::UINT16)
|
||||
{
|
||||
if (msg->fields[i].name == "ring")
|
||||
{
|
||||
found = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (!found)
|
||||
{
|
||||
ROS_ERROR("VelodyneLaserScan: Field 'ring' of type 'UINT16' not present in PointCloud2");
|
||||
return;
|
||||
}
|
||||
for (sensor_msgs::PointCloud2ConstIterator<uint16_t> it(*msg, "ring"); it != it.end(); ++it)
|
||||
{
|
||||
const uint16_t ring = *it;
|
||||
|
||||
if (ring + 1 > ring_count_)
|
||||
{
|
||||
ring_count_ = ring + 1;
|
||||
}
|
||||
}
|
||||
if (ring_count_)
|
||||
{
|
||||
ROS_INFO("VelodyneLaserScan: Latched ring count of %u", ring_count_);
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR("VelodyneLaserScan: Field 'ring' of type 'UINT16' not present in PointCloud2");
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// Select ring to use
|
||||
uint16_t ring;
|
||||
|
||||
if ((cfg_.ring < 0) || (cfg_.ring >= ring_count_))
|
||||
{
|
||||
// Default to ring closest to being level for each known sensor
|
||||
if (ring_count_ > 32)
|
||||
{
|
||||
ring = 57; // HDL-64E
|
||||
}
|
||||
else if (ring_count_ > 16)
|
||||
{
|
||||
ring = 23; // HDL-32E
|
||||
}
|
||||
else
|
||||
{
|
||||
ring = 8; // VLP-16
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ring = cfg_.ring;
|
||||
}
|
||||
|
||||
ROS_INFO_ONCE("VelodyneLaserScan: Extracting ring %u", ring);
|
||||
|
||||
// Load structure of PointCloud2
|
||||
int offset_x = -1;
|
||||
int offset_y = -1;
|
||||
int offset_z = -1;
|
||||
int offset_i = -1;
|
||||
int offset_r = -1;
|
||||
|
||||
for (size_t i = 0; i < msg->fields.size(); i++)
|
||||
{
|
||||
if (msg->fields[i].datatype == sensor_msgs::PointField::FLOAT32)
|
||||
{
|
||||
if (msg->fields[i].name == "x")
|
||||
{
|
||||
offset_x = msg->fields[i].offset;
|
||||
}
|
||||
else if (msg->fields[i].name == "y")
|
||||
{
|
||||
offset_y = msg->fields[i].offset;
|
||||
}
|
||||
else if (msg->fields[i].name == "z")
|
||||
{
|
||||
offset_z = msg->fields[i].offset;
|
||||
}
|
||||
else if (msg->fields[i].name == "intensity")
|
||||
{
|
||||
offset_i = msg->fields[i].offset;
|
||||
}
|
||||
}
|
||||
else if (msg->fields[i].datatype == sensor_msgs::PointField::UINT16)
|
||||
{
|
||||
if (msg->fields[i].name == "ring")
|
||||
{
|
||||
offset_r = msg->fields[i].offset;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Construct LaserScan message
|
||||
if ((offset_x >= 0) && (offset_y >= 0) && (offset_r >= 0))
|
||||
{
|
||||
const float RESOLUTION = std::abs(cfg_.resolution);
|
||||
const size_t SIZE = 2.0 * M_PI / RESOLUTION;
|
||||
sensor_msgs::LaserScanPtr scan(new sensor_msgs::LaserScan());
|
||||
scan->header = msg->header;
|
||||
scan->angle_increment = RESOLUTION;
|
||||
scan->angle_min = -M_PI;
|
||||
scan->angle_max = M_PI;
|
||||
scan->range_min = 0.0;
|
||||
scan->range_max = 200.0;
|
||||
scan->time_increment = 0.0;
|
||||
scan->ranges.resize(SIZE, INFINITY);
|
||||
|
||||
if ((offset_x == 0) &&
|
||||
(offset_y == 4) &&
|
||||
(offset_i % 4 == 0) &&
|
||||
(offset_r % 4 == 0))
|
||||
{
|
||||
scan->intensities.resize(SIZE);
|
||||
|
||||
const size_t X = 0;
|
||||
const size_t Y = 1;
|
||||
const size_t I = offset_i / 4;
|
||||
const size_t R = offset_r / 4;
|
||||
for (sensor_msgs::PointCloud2ConstIterator<float> it(*msg, "x"); it != it.end(); ++it)
|
||||
{
|
||||
const uint16_t r = *((const uint16_t*)(&it[R])); // ring
|
||||
|
||||
if (r == ring)
|
||||
{
|
||||
const float x = it[X]; // x
|
||||
const float y = it[Y]; // y
|
||||
const float i = it[I]; // intensity
|
||||
const int bin = (atan2f(y, x) + static_cast<float>(M_PI)) / RESOLUTION;
|
||||
|
||||
if ((bin >= 0) && (bin < static_cast<int>(SIZE)))
|
||||
{
|
||||
scan->ranges[bin] = sqrtf(x * x + y * y);
|
||||
scan->intensities[bin] = i;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_WARN_ONCE("VelodyneLaserScan: PointCloud2 fields in unexpected order. Using slower generic method.");
|
||||
|
||||
if (offset_i >= 0)
|
||||
{
|
||||
scan->intensities.resize(SIZE);
|
||||
sensor_msgs::PointCloud2ConstIterator<uint16_t> iter_r(*msg, "ring");
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_x(*msg, "x");
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_y(*msg, "y");
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_i(*msg, "intensity");
|
||||
for ( ; iter_r != iter_r.end(); ++iter_x, ++iter_y, ++iter_r, ++iter_i)
|
||||
{
|
||||
const uint16_t r = *iter_r; // ring
|
||||
|
||||
if (r == ring)
|
||||
{
|
||||
const float x = *iter_x; // x
|
||||
const float y = *iter_y; // y
|
||||
const float i = *iter_i; // intensity
|
||||
const int bin = (atan2f(y, x) + static_cast<float>(M_PI)) / RESOLUTION;
|
||||
|
||||
if ((bin >= 0) && (bin < static_cast<int>(SIZE)))
|
||||
{
|
||||
scan->ranges[bin] = sqrtf(x * x + y * y);
|
||||
scan->intensities[bin] = i;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
sensor_msgs::PointCloud2ConstIterator<uint16_t> iter_r(*msg, "ring");
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_x(*msg, "x");
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_y(*msg, "y");
|
||||
|
||||
for (; iter_r != iter_r.end(); ++iter_x, ++iter_y, ++iter_r)
|
||||
{
|
||||
const uint16_t r = *iter_r; // ring
|
||||
|
||||
if (r == ring)
|
||||
{
|
||||
const float x = *iter_x; // x
|
||||
const float y = *iter_y; // y
|
||||
const int bin = (atan2f(y, x) + static_cast<float>(M_PI)) / RESOLUTION;
|
||||
|
||||
if ((bin >= 0) && (bin < static_cast<int>(SIZE)))
|
||||
{
|
||||
scan->ranges[bin] = sqrtf(x * x + y * y);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub_.publish(scan);
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR("VelodyneLaserScan: PointCloud2 missing one or more required fields! (x,y,ring)");
|
||||
}
|
||||
}
|
||||
|
||||
void VelodyneLaserScan::reconfig(VelodyneLaserScanConfig& config, uint32_t level)
|
||||
{
|
||||
cfg_ = config;
|
||||
}
|
||||
|
||||
} // namespace velodyne_laserscan
|
||||
@@ -0,0 +1,30 @@
|
||||
### Unit tests
|
||||
#
|
||||
# Only configured when CATKIN_ENABLE_TESTING is true.
|
||||
|
||||
# These dependencies are only needed for unit testing
|
||||
find_package(roslaunch REQUIRED)
|
||||
find_package(rostest REQUIRED)
|
||||
|
||||
# C++ gtests
|
||||
#catkin_add_gtest(test_calibration test_calibration.cpp)
|
||||
#add_dependencies(test_calibration ${catkin_EXPORTED_TARGETS})
|
||||
#target_link_libraries(test_calibration velodyne_rawdata ${catkin_LIBRARIES})
|
||||
|
||||
# ROS rostests
|
||||
add_rostest_gtest(test_lazy_subscriber_node lazy_subscriber_node.test lazy_subscriber.cpp)
|
||||
add_dependencies(test_lazy_subscriber_node ${catkin_EXPORTED_TARGETS})
|
||||
target_link_libraries(test_lazy_subscriber_node ${catkin_LIBRARIES})
|
||||
|
||||
add_rostest_gtest(test_lazy_subscriber_nodelet lazy_subscriber_nodelet.test lazy_subscriber.cpp)
|
||||
add_dependencies(test_lazy_subscriber_nodelet ${catkin_EXPORTED_TARGETS})
|
||||
target_link_libraries(test_lazy_subscriber_nodelet ${catkin_LIBRARIES})
|
||||
|
||||
add_rostest_gtest(test_system_node system_node.test system.cpp)
|
||||
add_dependencies(test_system_node ${catkin_EXPORTED_TARGETS})
|
||||
target_link_libraries(test_system_node ${catkin_LIBRARIES})
|
||||
|
||||
add_rostest_gtest(test_system_nodelet system_nodelet.test system.cpp)
|
||||
add_dependencies(test_system_nodelet ${catkin_EXPORTED_TARGETS})
|
||||
target_link_libraries(test_system_nodelet ${catkin_LIBRARIES})
|
||||
|
||||
@@ -0,0 +1,124 @@
|
||||
// Copyright (C) 2018, 2019 Kevin Hallenbeck, Joshua Whitley
|
||||
// All rights reserved.
|
||||
//
|
||||
// Software License Agreement (BSD License 2.0)
|
||||
//
|
||||
// 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 {copyright_holder} 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.
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <sensor_msgs/LaserScan.h>
|
||||
|
||||
// Subscriber receive callback
|
||||
void recv(const sensor_msgs::LaserScanConstPtr& msg) {}
|
||||
|
||||
// Build and publish a minimal PointCloud2 message
|
||||
void publish(const ros::Publisher &pub)
|
||||
{
|
||||
const uint32_t POINT_STEP = 32;
|
||||
sensor_msgs::PointCloud2 msg;
|
||||
msg.header.frame_id = "";
|
||||
msg.header.stamp = ros::Time::now();
|
||||
msg.fields.resize(5);
|
||||
msg.fields[0].name = "x";
|
||||
msg.fields[0].offset = 0;
|
||||
msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
|
||||
msg.fields[0].count = 1;
|
||||
msg.fields[1].name = "y";
|
||||
msg.fields[1].offset = 4;
|
||||
msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
|
||||
msg.fields[1].count = 1;
|
||||
msg.fields[2].name = "z";
|
||||
msg.fields[2].offset = 8;
|
||||
msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
|
||||
msg.fields[2].count = 1;
|
||||
msg.fields[3].name = "intensity";
|
||||
msg.fields[3].offset = 16;
|
||||
msg.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
|
||||
msg.fields[3].count = 1;
|
||||
msg.fields[4].name = "ring";
|
||||
msg.fields[4].offset = 20;
|
||||
msg.fields[4].datatype = sensor_msgs::PointField::UINT16;
|
||||
msg.fields[4].count = 1;
|
||||
msg.data.resize(1 * POINT_STEP, 0x00);
|
||||
msg.point_step = POINT_STEP;
|
||||
msg.row_step = msg.data.size();
|
||||
msg.height = 1;
|
||||
msg.width = msg.row_step / POINT_STEP;
|
||||
msg.is_bigendian = false;
|
||||
msg.is_dense = true;
|
||||
pub.publish(msg);
|
||||
}
|
||||
|
||||
// Verify correct handling of subscribe and unsubscribe events
|
||||
TEST(Main, subscribe_unsubscribe)
|
||||
{
|
||||
ros::NodeHandle nh;
|
||||
ros::Publisher pub = nh.advertise<sensor_msgs::PointCloud2>("velodyne_points", 2);
|
||||
|
||||
// Wait for node to startup
|
||||
ros::WallDuration(2.0).sleep();
|
||||
ros::spinOnce();
|
||||
EXPECT_EQ(0, pub.getNumSubscribers());
|
||||
|
||||
// Subscribe to 'scan' and expect the node to subscribe to 'velodyne_points'
|
||||
ros::Subscriber sub = nh.subscribe("scan", 2, recv);
|
||||
|
||||
for (size_t i = 10; i > 0; i--)
|
||||
{
|
||||
publish(pub);
|
||||
ros::WallDuration(0.1).sleep();
|
||||
ros::spinOnce();
|
||||
}
|
||||
|
||||
EXPECT_EQ(1, sub.getNumPublishers());
|
||||
EXPECT_EQ(1, pub.getNumSubscribers());
|
||||
|
||||
// Unsubscribe from 'scan' and expect the node to unsubscribe from 'velodyne_points'
|
||||
sub.shutdown();
|
||||
|
||||
for (size_t i = 10; i > 0; i--)
|
||||
{
|
||||
publish(pub);
|
||||
ros::WallDuration(0.1).sleep();
|
||||
ros::spinOnce();
|
||||
}
|
||||
|
||||
EXPECT_EQ(0, sub.getNumPublishers());
|
||||
EXPECT_EQ(0, pub.getNumSubscribers());
|
||||
}
|
||||
|
||||
// Run all the tests that were declared with TEST()
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
ros::init(argc, argv, "test_lazy_subscriber");
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
@@ -0,0 +1,17 @@
|
||||
<!-- -*- mode: XML -*- -->
|
||||
<!-- rostest of the lazy subscriber -->
|
||||
|
||||
<launch>
|
||||
|
||||
<!-- Select log or screen output -->
|
||||
<arg name="output" default="log"/> <!-- screen/log -->
|
||||
|
||||
<!-- Start the laserscan node -->
|
||||
<node pkg="velodyne_laserscan" type="velodyne_laserscan_node" name="laserscan" output="$(arg output)" />
|
||||
|
||||
<!-- Start the rostest -->
|
||||
<test test-name="test_lazy_subscriber_node" pkg="velodyne_laserscan"
|
||||
type="test_lazy_subscriber_node" name="test_lazy_subscriber">
|
||||
</test>
|
||||
|
||||
</launch>
|
||||
@@ -0,0 +1,21 @@
|
||||
<!-- -*- mode: XML -*- -->
|
||||
<!-- rostest of the lazy subscriber -->
|
||||
|
||||
<launch>
|
||||
|
||||
<!-- Select log or screen output -->
|
||||
<arg name="output" default="log"/> <!-- screen/log -->
|
||||
|
||||
<!-- Start the laserscan nodelet -->
|
||||
<arg name="manager" default="nodelet_manager" />
|
||||
<node pkg="nodelet" type="nodelet" name="$(arg manager)" args="manager" output="$(arg output)" />
|
||||
<node pkg="nodelet" type="nodelet" name="velodyne_laserscan" output="$(arg output)"
|
||||
args="load velodyne_laserscan/LaserScanNodelet $(arg manager)">
|
||||
</node>
|
||||
|
||||
<!-- Start the rostest -->
|
||||
<test test-name="test_lazy_subscriber_nodelet" pkg="velodyne_laserscan"
|
||||
type="test_lazy_subscriber_nodelet" name="test_lazy_subscriber">
|
||||
</test>
|
||||
|
||||
</launch>
|
||||
@@ -0,0 +1,711 @@
|
||||
// Copyright (C) 2018, 2019 Kevin Hallenbeck, Joshua Whitley
|
||||
// All rights reserved.
|
||||
//
|
||||
// Software License Agreement (BSD License 2.0)
|
||||
//
|
||||
// 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 {copyright_holder} 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.
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <sensor_msgs/LaserScan.h>
|
||||
|
||||
#include <cstdlib>
|
||||
#include <algorithm>
|
||||
#include <vector>
|
||||
|
||||
// Define our own PointCloud type for easy use
|
||||
typedef struct
|
||||
{
|
||||
float x; // x
|
||||
float y; // y
|
||||
float z; // z
|
||||
float i; // intensity
|
||||
uint16_t r; // ring
|
||||
}
|
||||
Point;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
std_msgs::Header header;
|
||||
std::vector<Point> points;
|
||||
}
|
||||
PointCloud;
|
||||
|
||||
// Global variables
|
||||
ros::Publisher g_pub;
|
||||
ros::Subscriber g_sub;
|
||||
sensor_msgs::LaserScan g_scan;
|
||||
volatile bool g_scan_new = false;
|
||||
|
||||
// Convert WallTime to Time
|
||||
static inline ros::Time rosTime(const ros::WallTime &stamp)
|
||||
{
|
||||
return ros::Time(stamp.sec, stamp.nsec);
|
||||
}
|
||||
|
||||
// Subscriber receive callback
|
||||
void recv(const sensor_msgs::LaserScanConstPtr& msg)
|
||||
{
|
||||
g_scan = *msg;
|
||||
g_scan_new = true;
|
||||
}
|
||||
|
||||
// Wait for incoming LaserScan message
|
||||
bool waitForScan(ros::WallDuration dur)
|
||||
{
|
||||
const ros::WallTime start = ros::WallTime::now();
|
||||
|
||||
while (!g_scan_new)
|
||||
{
|
||||
if ((ros::WallTime::now() - start) > dur)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
ros::WallDuration(0.001).sleep();
|
||||
ros::spinOnce();
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// Build and publish PointCloud2 messages of various structures
|
||||
void publishXYZIR1(const PointCloud &cloud)
|
||||
{
|
||||
g_scan_new = false;
|
||||
const uint32_t POINT_STEP = 32;
|
||||
sensor_msgs::PointCloud2 msg;
|
||||
msg.header.frame_id = cloud.header.frame_id;
|
||||
msg.header.stamp = cloud.header.stamp;
|
||||
msg.fields.resize(5);
|
||||
msg.fields[0].name = "x";
|
||||
msg.fields[0].offset = 0;
|
||||
msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
|
||||
msg.fields[0].count = 1;
|
||||
msg.fields[1].name = "y";
|
||||
msg.fields[1].offset = 4;
|
||||
msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
|
||||
msg.fields[1].count = 1;
|
||||
msg.fields[2].name = "z";
|
||||
msg.fields[2].offset = 8;
|
||||
msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
|
||||
msg.fields[2].count = 1;
|
||||
msg.fields[3].name = "intensity";
|
||||
msg.fields[3].offset = 16;
|
||||
msg.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
|
||||
msg.fields[3].count = 1;
|
||||
msg.fields[4].name = "ring";
|
||||
msg.fields[4].offset = 20;
|
||||
msg.fields[4].datatype = sensor_msgs::PointField::UINT16;
|
||||
msg.fields[4].count = 1;
|
||||
msg.data.resize(std::max((size_t)1, cloud.points.size()) * POINT_STEP, 0x00);
|
||||
msg.point_step = POINT_STEP;
|
||||
msg.row_step = msg.data.size();
|
||||
msg.height = 1;
|
||||
msg.width = msg.row_step / POINT_STEP;
|
||||
msg.is_bigendian = false;
|
||||
msg.is_dense = true;
|
||||
uint8_t *ptr = msg.data.data();
|
||||
|
||||
for (size_t i = 0; i < cloud.points.size(); i++)
|
||||
{
|
||||
*(reinterpret_cast<float*>(ptr + 0)) = cloud.points[i].x;
|
||||
*(reinterpret_cast<float*>(ptr + 4)) = cloud.points[i].y;
|
||||
*(reinterpret_cast<float*>(ptr + 8)) = cloud.points[i].z;
|
||||
*(reinterpret_cast<float*>(ptr + 16)) = cloud.points[i].i;
|
||||
*(reinterpret_cast<uint16_t*>(ptr + 20)) = cloud.points[i].r;
|
||||
ptr += POINT_STEP;
|
||||
}
|
||||
|
||||
g_pub.publish(msg);
|
||||
}
|
||||
|
||||
void publishXYZIR2(const PointCloud &cloud)
|
||||
{
|
||||
g_scan_new = false;
|
||||
const uint32_t POINT_STEP = 19;
|
||||
sensor_msgs::PointCloud2 msg;
|
||||
msg.header.frame_id = cloud.header.frame_id;
|
||||
msg.header.stamp = cloud.header.stamp;
|
||||
msg.fields.resize(5);
|
||||
msg.fields[0].name = "z";
|
||||
msg.fields[0].offset = 4;
|
||||
msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
|
||||
msg.fields[0].count = 1;
|
||||
msg.fields[1].name = "y";
|
||||
msg.fields[1].offset = 8;
|
||||
msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
|
||||
msg.fields[1].count = 1;
|
||||
msg.fields[2].name = "x";
|
||||
msg.fields[2].offset = 12;
|
||||
msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
|
||||
msg.fields[2].count = 1;
|
||||
msg.fields[3].name = "intensity";
|
||||
msg.fields[3].offset = 0;
|
||||
msg.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
|
||||
msg.fields[3].count = 1;
|
||||
msg.fields[4].name = "ring";
|
||||
msg.fields[4].offset = 16;
|
||||
msg.fields[4].datatype = sensor_msgs::PointField::UINT16;
|
||||
msg.fields[4].count = 1;
|
||||
msg.data.resize(std::max((size_t)1, cloud.points.size()) * POINT_STEP, 0x00);
|
||||
msg.point_step = POINT_STEP;
|
||||
msg.row_step = msg.data.size();
|
||||
msg.height = 1;
|
||||
msg.width = msg.row_step / POINT_STEP;
|
||||
msg.is_bigendian = false;
|
||||
msg.is_dense = true;
|
||||
uint8_t *ptr = msg.data.data();
|
||||
|
||||
for (size_t i = 0; i < cloud.points.size(); i++)
|
||||
{
|
||||
*(reinterpret_cast<float*>(ptr + 0)) = cloud.points[i].i;
|
||||
*(reinterpret_cast<float*>(ptr + 4)) = cloud.points[i].z;
|
||||
*(reinterpret_cast<float*>(ptr + 8)) = cloud.points[i].y;
|
||||
*(reinterpret_cast<float*>(ptr + 12)) = cloud.points[i].x;
|
||||
*(reinterpret_cast<uint16_t*>(ptr + 16)) = cloud.points[i].r;
|
||||
ptr += POINT_STEP;
|
||||
}
|
||||
|
||||
g_pub.publish(msg);
|
||||
}
|
||||
|
||||
void publishXYZR(const PointCloud &cloud)
|
||||
{
|
||||
g_scan_new = false;
|
||||
const uint32_t POINT_STEP = 15;
|
||||
sensor_msgs::PointCloud2 msg;
|
||||
msg.header.frame_id = cloud.header.frame_id;
|
||||
msg.header.stamp = cloud.header.stamp;
|
||||
msg.fields.resize(4);
|
||||
msg.fields[0].name = "x";
|
||||
msg.fields[0].offset = 0;
|
||||
msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
|
||||
msg.fields[0].count = 1;
|
||||
msg.fields[1].name = "y";
|
||||
msg.fields[1].offset = 4;
|
||||
msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
|
||||
msg.fields[1].count = 1;
|
||||
msg.fields[2].name = "z";
|
||||
msg.fields[2].offset = 8;
|
||||
msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
|
||||
msg.fields[2].count = 1;
|
||||
msg.fields[3].name = "ring";
|
||||
msg.fields[3].offset = 12;
|
||||
msg.fields[3].datatype = sensor_msgs::PointField::UINT16;
|
||||
msg.fields[3].count = 1;
|
||||
msg.data.resize(std::max((size_t)1, cloud.points.size()) * POINT_STEP, 0x00);
|
||||
msg.point_step = POINT_STEP;
|
||||
msg.row_step = msg.data.size();
|
||||
msg.height = 1;
|
||||
msg.width = msg.row_step / POINT_STEP;
|
||||
msg.is_bigendian = false;
|
||||
msg.is_dense = true;
|
||||
uint8_t *ptr = msg.data.data();
|
||||
|
||||
for (size_t i = 0; i < cloud.points.size(); i++)
|
||||
{
|
||||
*(reinterpret_cast<float*>(ptr + 0)) = cloud.points[i].x;
|
||||
*(reinterpret_cast<float*>(ptr + 4)) = cloud.points[i].y;
|
||||
*(reinterpret_cast<float*>(ptr + 8)) = cloud.points[i].z;
|
||||
*(reinterpret_cast<uint16_t*>(ptr + 12)) = cloud.points[i].r;
|
||||
ptr += POINT_STEP;
|
||||
}
|
||||
|
||||
g_pub.publish(msg);
|
||||
}
|
||||
|
||||
void publishR(const PointCloud &cloud)
|
||||
{
|
||||
g_scan_new = false;
|
||||
const uint32_t POINT_STEP = 2;
|
||||
sensor_msgs::PointCloud2 msg;
|
||||
msg.header.stamp = rosTime(ros::WallTime::now());
|
||||
msg.fields.resize(1);
|
||||
msg.fields[0].name = "ring";
|
||||
msg.fields[0].offset = 0;
|
||||
msg.fields[0].datatype = sensor_msgs::PointField::UINT16;
|
||||
msg.fields[0].count = 1;
|
||||
msg.data.resize(std::max((size_t)1, cloud.points.size()) * POINT_STEP, 0x00);
|
||||
msg.point_step = POINT_STEP;
|
||||
msg.row_step = msg.data.size();
|
||||
msg.height = 1;
|
||||
msg.width = msg.row_step / POINT_STEP;
|
||||
uint8_t *ptr = msg.data.data();
|
||||
|
||||
for (size_t i = 0; i < cloud.points.size(); i++)
|
||||
{
|
||||
*(reinterpret_cast<uint16_t*>(ptr + 0)) = cloud.points[i].r;
|
||||
ptr += POINT_STEP;
|
||||
}
|
||||
|
||||
g_pub.publish(msg);
|
||||
}
|
||||
|
||||
void publishXYZR32(const PointCloud &cloud)
|
||||
{
|
||||
g_scan_new = false;
|
||||
const uint32_t POINT_STEP = 16;
|
||||
sensor_msgs::PointCloud2 msg;
|
||||
msg.header.frame_id = cloud.header.frame_id;
|
||||
msg.header.stamp = cloud.header.stamp;
|
||||
msg.fields.resize(4);
|
||||
msg.fields[0].name = "x";
|
||||
msg.fields[0].offset = 0;
|
||||
msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
|
||||
msg.fields[0].count = 1;
|
||||
msg.fields[1].name = "y";
|
||||
msg.fields[1].offset = 4;
|
||||
msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
|
||||
msg.fields[1].count = 1;
|
||||
msg.fields[2].name = "z";
|
||||
msg.fields[2].offset = 8;
|
||||
msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
|
||||
msg.fields[2].count = 1;
|
||||
msg.fields[3].name = "ring";
|
||||
msg.fields[3].offset = 12;
|
||||
msg.fields[3].datatype = sensor_msgs::PointField::UINT32;
|
||||
msg.fields[3].count = 1;
|
||||
msg.data.resize(std::max((size_t)1, cloud.points.size()) * POINT_STEP, 0x00);
|
||||
msg.point_step = POINT_STEP;
|
||||
msg.row_step = msg.data.size();
|
||||
msg.height = 1;
|
||||
msg.width = msg.row_step / POINT_STEP;
|
||||
msg.is_bigendian = false;
|
||||
msg.is_dense = true;
|
||||
uint8_t *ptr = msg.data.data();
|
||||
|
||||
for (size_t i = 0; i < cloud.points.size(); i++)
|
||||
{
|
||||
*(reinterpret_cast<float*>(ptr + 0)) = cloud.points[i].x;
|
||||
*(reinterpret_cast<float*>(ptr + 4)) = cloud.points[i].y;
|
||||
*(reinterpret_cast<float*>(ptr + 8)) = cloud.points[i].z;
|
||||
*(reinterpret_cast<uint32_t*>(ptr + 12)) = cloud.points[i].r;
|
||||
ptr += POINT_STEP;
|
||||
}
|
||||
|
||||
g_pub.publish(msg);
|
||||
}
|
||||
|
||||
void publishXYZ(const PointCloud &cloud)
|
||||
{
|
||||
g_scan_new = false;
|
||||
const uint32_t POINT_STEP = 12;
|
||||
sensor_msgs::PointCloud2 msg;
|
||||
msg.header.stamp = rosTime(ros::WallTime::now());
|
||||
msg.fields.resize(3);
|
||||
msg.fields[0].name = "x";
|
||||
msg.fields[0].offset = 0;
|
||||
msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
|
||||
msg.fields[0].count = 1;
|
||||
msg.fields[1].name = "y";
|
||||
msg.fields[1].offset = 4;
|
||||
msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
|
||||
msg.fields[1].count = 1;
|
||||
msg.fields[2].name = "z";
|
||||
msg.fields[2].offset = 8;
|
||||
msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
|
||||
msg.fields[2].count = 1;
|
||||
msg.data.resize(std::max((size_t)1, cloud.points.size()) * POINT_STEP, 0x00);
|
||||
msg.point_step = POINT_STEP;
|
||||
msg.row_step = msg.data.size();
|
||||
msg.height = 1;
|
||||
msg.width = msg.row_step / POINT_STEP;
|
||||
uint8_t *ptr = msg.data.data();
|
||||
|
||||
for (size_t i = 0; i < cloud.points.size(); i++)
|
||||
{
|
||||
*(reinterpret_cast<float*>(ptr + 0)) = cloud.points[i].x;
|
||||
*(reinterpret_cast<float*>(ptr + 4)) = cloud.points[i].y;
|
||||
*(reinterpret_cast<float*>(ptr + 8)) = cloud.points[i].z;
|
||||
ptr += POINT_STEP;
|
||||
}
|
||||
|
||||
g_pub.publish(msg);
|
||||
}
|
||||
|
||||
void publishNone()
|
||||
{
|
||||
g_scan_new = false;
|
||||
const uint32_t POINT_STEP = 16;
|
||||
sensor_msgs::PointCloud2 msg;
|
||||
msg.header.stamp = rosTime(ros::WallTime::now());
|
||||
msg.data.resize(1 * POINT_STEP, 0x00);
|
||||
msg.point_step = POINT_STEP;
|
||||
msg.row_step = msg.data.size();
|
||||
msg.height = 1;
|
||||
msg.width = msg.row_step / POINT_STEP;
|
||||
g_pub.publish(msg);
|
||||
}
|
||||
|
||||
// Find the index of the point in the PointCloud with the shortest 2d distance to the point (x,y)
|
||||
static inline float SQUARE(float x)
|
||||
{
|
||||
return x * x;
|
||||
}
|
||||
|
||||
size_t findClosestIndex(const PointCloud &cloud, uint16_t ring, float x, float y)
|
||||
{
|
||||
size_t index = SIZE_MAX;
|
||||
float delta = INFINITY;
|
||||
|
||||
for (size_t i = 0; i < cloud.points.size(); i++)
|
||||
{
|
||||
if (cloud.points[i].r == ring)
|
||||
{
|
||||
float dist = SQUARE(x - cloud.points[i].x) + SQUARE(y - cloud.points[i].y);
|
||||
|
||||
if (dist < delta)
|
||||
{
|
||||
delta = dist;
|
||||
index = i;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return index;
|
||||
}
|
||||
|
||||
// Verify that all LaserScan header values are values are passed through, and other values are default
|
||||
void verifyScanEmpty(const PointCloud &cloud, bool intensity = true)
|
||||
{
|
||||
ASSERT_EQ(cloud.header.stamp, g_scan.header.stamp);
|
||||
EXPECT_EQ(cloud.header.frame_id, g_scan.header.frame_id);
|
||||
|
||||
for (size_t i = 0; i < g_scan.ranges.size(); i++)
|
||||
{
|
||||
EXPECT_EQ(INFINITY, g_scan.ranges[i]);
|
||||
}
|
||||
|
||||
if (!intensity)
|
||||
{
|
||||
EXPECT_EQ(0, g_scan.intensities.size());
|
||||
}
|
||||
else
|
||||
{
|
||||
EXPECT_EQ(g_scan.ranges.size(), g_scan.intensities.size());
|
||||
|
||||
for (size_t i = 0; i < g_scan.intensities.size(); i++)
|
||||
{
|
||||
EXPECT_EQ(0.0, g_scan.intensities[i]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Verify that every PointCloud point made it to the LaserScan and other values are default
|
||||
void verifyScanSparse(const PointCloud &cloud, uint16_t ring, uint16_t ring_count, bool intensity = true)
|
||||
{
|
||||
ASSERT_EQ(cloud.header.stamp, g_scan.header.stamp);
|
||||
EXPECT_EQ(cloud.header.frame_id, g_scan.header.frame_id);
|
||||
EXPECT_EQ(intensity ? g_scan.ranges.size() : 0, g_scan.intensities.size());
|
||||
size_t count = 0;
|
||||
|
||||
for (size_t i = 0; i < g_scan.ranges.size(); i++)
|
||||
{
|
||||
double r = g_scan.ranges[i];
|
||||
|
||||
if (std::isfinite(r))
|
||||
{
|
||||
float a = g_scan.angle_min + i * g_scan.angle_increment;
|
||||
float x = g_scan.ranges[i] * cosf(a);
|
||||
float y = g_scan.ranges[i] * sinf(a);
|
||||
float e = g_scan.ranges[i] * g_scan.angle_increment + static_cast<float>(1e-3); // allowable error
|
||||
size_t index = findClosestIndex(cloud, ring, x, y);
|
||||
|
||||
if (index < cloud.points.size())
|
||||
{
|
||||
count++;
|
||||
EXPECT_NEAR(cloud.points[index].x, x, e);
|
||||
EXPECT_NEAR(cloud.points[index].y, y, e);
|
||||
|
||||
if (i < g_scan.intensities.size())
|
||||
{
|
||||
EXPECT_EQ(cloud.points[index].i, g_scan.intensities[i]);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
EXPECT_TRUE(false); // LaserScan point not found in PointCloud
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
EXPECT_EQ(INFINITY, r);
|
||||
}
|
||||
}
|
||||
|
||||
if (ring_count > 0)
|
||||
{
|
||||
EXPECT_EQ(cloud.points.size() / ring_count, count); // Make sure that all points were converted to ranges
|
||||
}
|
||||
}
|
||||
|
||||
// Verify that every LaserScan point is not default, and every point came from the PointCloud
|
||||
void verifyScanDense(const PointCloud &cloud, uint16_t ring, bool intensity = true)
|
||||
{
|
||||
ASSERT_EQ(cloud.header.stamp, g_scan.header.stamp);
|
||||
EXPECT_EQ(cloud.header.frame_id, g_scan.header.frame_id);
|
||||
EXPECT_EQ(intensity ? g_scan.ranges.size() : 0, g_scan.intensities.size());
|
||||
|
||||
for (size_t i = 0; i < g_scan.ranges.size(); i++)
|
||||
{
|
||||
double r = g_scan.ranges[i];
|
||||
|
||||
if (std::isfinite(r))
|
||||
{
|
||||
float a = g_scan.angle_min + i * g_scan.angle_increment;
|
||||
float x = g_scan.ranges[i] * cosf(a);
|
||||
float y = g_scan.ranges[i] * sinf(a);
|
||||
float e = g_scan.ranges[i] * g_scan.angle_increment + static_cast<float>(1e-3); // allowable error
|
||||
size_t index = findClosestIndex(cloud, ring, x, y);
|
||||
|
||||
if (index < cloud.points.size())
|
||||
{
|
||||
EXPECT_NEAR(cloud.points[index].x, x, e);
|
||||
EXPECT_NEAR(cloud.points[index].y, y, e);
|
||||
// @TODO: Test for matching intensity
|
||||
}
|
||||
else
|
||||
{
|
||||
EXPECT_TRUE(false); // LaserScan point not found in PointCloud
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
EXPECT_TRUE(false); // Dense PointCloud should populate every range in LaserScan
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Verify that no LaserScan is generated when the PointCloud2 message is missing required fields
|
||||
TEST(System, missing_fields)
|
||||
{
|
||||
// Make sure system is connected
|
||||
ASSERT_EQ(1, g_sub.getNumPublishers());
|
||||
ASSERT_EQ(1, g_pub.getNumSubscribers());
|
||||
|
||||
// Create PointCloud with 16 rings
|
||||
PointCloud cloud;
|
||||
cloud.points.resize(1);
|
||||
cloud.points[0].x = 0.0;
|
||||
cloud.points[0].y = 0.0;
|
||||
cloud.points[0].z = 0.0;
|
||||
cloud.points[0].i = 0.0;
|
||||
cloud.points[0].r = 15;
|
||||
|
||||
// Verify no LaserScan when PointCloud2 fields are empty
|
||||
publishNone();
|
||||
EXPECT_FALSE(waitForScan(ros::WallDuration(0.5)));
|
||||
|
||||
// Verify no LaserScan when PointCloud2 fields are missing 'ring'
|
||||
publishXYZ(cloud);
|
||||
EXPECT_FALSE(waitForScan(ros::WallDuration(0.5)));
|
||||
|
||||
// Verify no LaserScan when PointCloud2 field 'ring' is the incorrect type
|
||||
publishXYZR32(cloud);
|
||||
EXPECT_FALSE(waitForScan(ros::WallDuration(0.5)));
|
||||
|
||||
|
||||
// Verify no LaserScan when PointCloud2 fields are missing 'x' and 'y'
|
||||
publishR(cloud);
|
||||
EXPECT_FALSE(waitForScan(ros::WallDuration(0.5)));
|
||||
|
||||
// Verify that the node hasn't crashed by sending normal PointCloud2 fields
|
||||
cloud.header.stamp = rosTime(ros::WallTime::now());
|
||||
publishXYZIR1(cloud);
|
||||
ASSERT_TRUE(waitForScan(ros::WallDuration(1.0)));
|
||||
ASSERT_EQ(cloud.header.stamp, g_scan.header.stamp);
|
||||
}
|
||||
|
||||
// Verify that non-point fields are passed through unmodified
|
||||
TEST(System, empty_data)
|
||||
{
|
||||
// Make sure system is connected
|
||||
ASSERT_EQ(1, g_sub.getNumPublishers());
|
||||
ASSERT_EQ(1, g_pub.getNumSubscribers());
|
||||
|
||||
// Create PointCloud with 16 rings
|
||||
PointCloud cloud;
|
||||
cloud.header.frame_id = "abcdefghijklmnopqrstuvwxyz";
|
||||
cloud.points.resize(1);
|
||||
cloud.points[0].x = 0.0;
|
||||
cloud.points[0].y = 0.0;
|
||||
cloud.points[0].z = 0.0;
|
||||
cloud.points[0].i = 0.0;
|
||||
cloud.points[0].r = 15;
|
||||
|
||||
// Verify that all three PointCloud2 types create proper default values
|
||||
|
||||
// PointXYZIR (expected format)
|
||||
cloud.header.stamp = rosTime(ros::WallTime::now());
|
||||
publishXYZIR1(cloud);
|
||||
ASSERT_TRUE(waitForScan(ros::WallDuration(1.0)));
|
||||
verifyScanEmpty(cloud, true);
|
||||
|
||||
// PointXYZIR (unexpected format with intensity)
|
||||
cloud.header.stamp = rosTime(ros::WallTime::now());
|
||||
publishXYZIR2(cloud);
|
||||
ASSERT_TRUE(waitForScan(ros::WallDuration(1.0)));
|
||||
verifyScanEmpty(cloud, true);
|
||||
|
||||
// PointXYZR (unexpected format without intensity)
|
||||
cloud.header.stamp = rosTime(ros::WallTime::now());
|
||||
publishXYZR(cloud);
|
||||
ASSERT_TRUE(waitForScan(ros::WallDuration(1.0)));
|
||||
verifyScanEmpty(cloud, false);
|
||||
}
|
||||
|
||||
// Verify that every piece of a small amount of random data is passed through
|
||||
TEST(System, random_data_sparse)
|
||||
{
|
||||
// Make sure system is connected
|
||||
ASSERT_EQ(1, g_sub.getNumPublishers());
|
||||
ASSERT_EQ(1, g_pub.getNumSubscribers());
|
||||
|
||||
// Create PointCloud with sparse random data
|
||||
PointCloud cloud;
|
||||
cloud.header.frame_id = "velodyne";
|
||||
const size_t RANGE_COUNT = 100;
|
||||
const size_t RING_COUNT = 16;
|
||||
const double RANGE_MAX = 20.0;
|
||||
const double INTENSITY_MAX = 1.0;
|
||||
|
||||
for (size_t i = 0; i < RANGE_COUNT; i++)
|
||||
{
|
||||
double angle_y = i * 1.99 * M_PI / RANGE_COUNT; // yaw
|
||||
|
||||
for (size_t j = 0; j < RING_COUNT; j++)
|
||||
{
|
||||
double angle_p = j * 0.2 * M_PI / RING_COUNT - 0.1 * M_PI; // pitch
|
||||
double range = std::rand() * (RANGE_MAX / RAND_MAX);
|
||||
Point point;
|
||||
point.x = range * cos(angle_p) * cos(angle_y);
|
||||
point.y = range * cos(angle_p) * sin(angle_y);
|
||||
point.z = range * sin(angle_p);
|
||||
point.i = std::rand() * (INTENSITY_MAX / RAND_MAX);
|
||||
point.r = j;
|
||||
cloud.points.push_back(point);
|
||||
}
|
||||
}
|
||||
|
||||
// Verify that all three PointCloud2 types are handled correctly
|
||||
|
||||
// PointXYZIR (expected format)
|
||||
cloud.header.stamp = rosTime(ros::WallTime::now());
|
||||
publishXYZIR1(cloud);
|
||||
ASSERT_TRUE(waitForScan(ros::WallDuration(1.0)));
|
||||
verifyScanSparse(cloud, 8, RING_COUNT, true);
|
||||
|
||||
// PointXYZIR (unexpected format with intensity)
|
||||
cloud.header.stamp = rosTime(ros::WallTime::now());
|
||||
publishXYZIR2(cloud);
|
||||
ASSERT_TRUE(waitForScan(ros::WallDuration(1.0)));
|
||||
verifyScanSparse(cloud, 8, RING_COUNT, true);
|
||||
|
||||
// PointXYZR (unexpected format without intensity)
|
||||
cloud.header.stamp = rosTime(ros::WallTime::now());
|
||||
publishXYZR(cloud);
|
||||
ASSERT_TRUE(waitForScan(ros::WallDuration(1.0)));
|
||||
verifyScanSparse(cloud, 8, RING_COUNT, false);
|
||||
}
|
||||
|
||||
// Verify that every LaserScan range is valid when given an extra large amount of random data
|
||||
TEST(System, random_data_dense)
|
||||
{
|
||||
// Make sure system is connected
|
||||
ASSERT_EQ(1, g_sub.getNumPublishers());
|
||||
ASSERT_EQ(1, g_pub.getNumSubscribers());
|
||||
|
||||
// Create PointCloud with dense random data
|
||||
PointCloud cloud;
|
||||
cloud.header.frame_id = "velodyne";
|
||||
const size_t RANGE_COUNT = 2500;
|
||||
const size_t RING_COUNT = 16;
|
||||
const double RANGE_MAX = 20.0;
|
||||
const double INTENSITY_MAX = 1.0;
|
||||
|
||||
for (size_t i = 0; i < RANGE_COUNT; i++)
|
||||
{
|
||||
double angle_y = i * 2.0 * M_PI / RANGE_COUNT; // yaw
|
||||
|
||||
for (size_t j = 0; j < RING_COUNT; j++)
|
||||
{
|
||||
double angle_p = j * 0.2 * M_PI / RING_COUNT - 0.1 * M_PI; // pitch
|
||||
double range = std::rand() * (RANGE_MAX / RAND_MAX);
|
||||
Point point;
|
||||
point.x = range * cos(angle_p) * cos(angle_y);
|
||||
point.y = range * cos(angle_p) * sin(angle_y);
|
||||
point.z = range * sin(angle_p);
|
||||
point.i = std::rand() * (INTENSITY_MAX / RAND_MAX);
|
||||
point.r = j;
|
||||
cloud.points.push_back(point);
|
||||
}
|
||||
}
|
||||
|
||||
// Verify that all three PointCloud2 types are handled correctly
|
||||
|
||||
// PointXYZIR (expected format)
|
||||
cloud.header.stamp = rosTime(ros::WallTime::now());
|
||||
publishXYZIR1(cloud);
|
||||
ASSERT_TRUE(waitForScan(ros::WallDuration(1.0)));
|
||||
verifyScanDense(cloud, 8, true);
|
||||
|
||||
// PointXYZIR (unexpected format with intensity)
|
||||
cloud.header.stamp = rosTime(ros::WallTime::now());
|
||||
publishXYZIR2(cloud);
|
||||
ASSERT_TRUE(waitForScan(ros::WallDuration(1.0)));
|
||||
verifyScanDense(cloud, 8, true);
|
||||
|
||||
// PointXYZR (unexpected format without intensity)
|
||||
cloud.header.stamp = rosTime(ros::WallTime::now());
|
||||
publishXYZR(cloud);
|
||||
ASSERT_TRUE(waitForScan(ros::WallDuration(1.0)));
|
||||
verifyScanDense(cloud, 8, false);
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
|
||||
// Initialize ROS
|
||||
ros::init(argc, argv, "test_lazy_subscriber");
|
||||
ros::NodeHandle nh;
|
||||
|
||||
// Setup publisher and subscriber
|
||||
g_pub = nh.advertise<sensor_msgs::PointCloud2>("velodyne_points", 2);
|
||||
g_sub = nh.subscribe("scan", 2, recv);
|
||||
|
||||
// Wait for other nodes to startup
|
||||
ros::WallDuration(1.0).sleep();
|
||||
ros::spinOnce();
|
||||
|
||||
// Run all the tests that were declared with TEST()
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
@@ -0,0 +1,17 @@
|
||||
<!-- -*- mode: XML -*- -->
|
||||
<!-- rostest of the velodyne_laserscan system -->
|
||||
|
||||
<launch>
|
||||
|
||||
<!-- Select log or screen output -->
|
||||
<arg name="output" default="log"/> <!-- screen/log -->
|
||||
|
||||
<!-- Start the laserscan node -->
|
||||
<node pkg="velodyne_laserscan" type="velodyne_laserscan_node" name="laserscan" output="$(arg output)" />
|
||||
|
||||
<!-- Start the rostest -->
|
||||
<test test-name="test_system_node" pkg="velodyne_laserscan"
|
||||
type="test_system_node" name="test_system">
|
||||
</test>
|
||||
|
||||
</launch>
|
||||
@@ -0,0 +1,21 @@
|
||||
<!-- -*- mode: XML -*- -->
|
||||
<!-- rostest of the velodyne_laserscan system -->
|
||||
|
||||
<launch>
|
||||
|
||||
<!-- Select log or screen output -->
|
||||
<arg name="output" default="log"/> <!-- screen/log -->
|
||||
|
||||
<!-- Start the laserscan nodelet -->
|
||||
<arg name="manager" default="nodelet_manager" />
|
||||
<node pkg="nodelet" type="nodelet" name="$(arg manager)" args="manager" output="$(arg output)" />
|
||||
<node pkg="nodelet" type="nodelet" name="velodyne_laserscan" output="$(arg output)"
|
||||
args="load velodyne_laserscan/LaserScanNodelet $(arg manager)">
|
||||
</node>
|
||||
|
||||
<!-- Start the rostest -->
|
||||
<test test-name="test_system_nodelet" pkg="velodyne_laserscan"
|
||||
type="test_system_nodelet" name="test_system">
|
||||
</test>
|
||||
|
||||
</launch>
|
||||
@@ -0,0 +1,86 @@
|
||||
Change history
|
||||
==============
|
||||
|
||||
1.5.2 (2019-01-28)
|
||||
------------------
|
||||
|
||||
1.5.1 (2018-12-10)
|
||||
------------------
|
||||
|
||||
1.5.0 (2018-10-19)
|
||||
------------------
|
||||
|
||||
1.4.0 (2018-09-19)
|
||||
------------------
|
||||
* Updated all package.xmls to ver 2. Cleaned up catkin_lint errors.
|
||||
All package.xml files are now compatible with version 2 of the
|
||||
package.xml specification in REP 140. Removed some unnecessary
|
||||
execute permissions on a few files. Fixed a missing test_depend.
|
||||
* Updated cut_at_specified_angle_feature with latest master version.
|
||||
* Contributors: Andre Volk, Joshua Whitley
|
||||
|
||||
1.3.0 (2017-11-10)
|
||||
------------------
|
||||
|
||||
1.2.0 (2014-08-06)
|
||||
------------------
|
||||
|
||||
1.1.2 (2013-11-05)
|
||||
-------------------
|
||||
|
||||
1.1.1 (2013-07-30)
|
||||
------------------
|
||||
|
||||
1.1.0 (2013-07-16)
|
||||
------------------
|
||||
|
||||
1.0.1 (2013-06-15)
|
||||
------------------
|
||||
|
||||
1.0.0 (2013-06-14)
|
||||
------------------
|
||||
|
||||
* Convert to catkin (`#1`_).
|
||||
* Release to Hydro.
|
||||
|
||||
0.9.2 (2013-07-08)
|
||||
------------------
|
||||
|
||||
0.9.1 (2012-06-05)
|
||||
------------------
|
||||
|
||||
* Released to Electric, Fuerte and Groovy.
|
||||
|
||||
0.9.0 (2012-04-03)
|
||||
------------------
|
||||
|
||||
* Completely revised API, anticipating a 1.0.0 release.
|
||||
* Released to Electric, Fuerte and Groovy.
|
||||
|
||||
0.2.6 (2011-02-23)
|
||||
------------------
|
||||
|
||||
0.2.5 (2010-11-19)
|
||||
------------------
|
||||
|
||||
* Initial implementation of new 0.3 interfaces.
|
||||
* Support for ROS 1.3 `std_msgs::Header` changes.
|
||||
|
||||
0.2.0 (2010-08-17)
|
||||
------------------
|
||||
|
||||
* Initial release to ROS C-turtle.
|
||||
|
||||
.. _`#1`: https://github.com/ros-drivers/velodyne/issues/1
|
||||
.. _`#4`: https://github.com/ros-drivers/velodyne/issues/4
|
||||
.. _`#7`: https://github.com/ros-drivers/velodyne/issues/7
|
||||
.. _`#8`: https://github.com/ros-drivers/velodyne/pull/8
|
||||
.. _`#9`: https://github.com/ros-drivers/velodyne/issues/9
|
||||
.. _`#10`: https://github.com/ros-drivers/velodyne/issues/10
|
||||
.. _`#11`: https://github.com/ros-drivers/velodyne/issues/11
|
||||
.. _`#12`: https://github.com/ros-drivers/velodyne/pull/12
|
||||
.. _`#13`: https://github.com/ros-drivers/velodyne/issues/13
|
||||
.. _`#14`: https://github.com/ros-drivers/velodyne/pull/14
|
||||
.. _`#17`: https://github.com/ros-drivers/velodyne/issues/17
|
||||
.. _`#18`: https://github.com/ros-drivers/velodyne/issues/18
|
||||
.. _`#20`: https://github.com/ros-drivers/velodyne/issues/20
|
||||
@@ -0,0 +1,16 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(velodyne_msgs)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS message_generation std_msgs)
|
||||
|
||||
add_message_files(
|
||||
DIRECTORY msg
|
||||
FILES
|
||||
VelodynePacket.msg
|
||||
VelodyneScan.msg
|
||||
)
|
||||
generate_messages(DEPENDENCIES std_msgs)
|
||||
|
||||
catkin_package(
|
||||
CATKIN_DEPENDS message_runtime std_msgs
|
||||
)
|
||||
@@ -0,0 +1,10 @@
|
||||
/**
|
||||
\mainpage
|
||||
\htmlinclude manifest.html
|
||||
|
||||
The @b velodyne_msgs package collects ROS messages specific to the
|
||||
Velodyne HDL-64E 3D and HDL-64E S2 LIDARs.
|
||||
|
||||
No other programming interfaces or ROS nodes are provided.
|
||||
|
||||
*/
|
||||
@@ -0,0 +1,5 @@
|
||||
# Raw Velodyne LIDAR packet.
|
||||
|
||||
time stamp # packet timestamp
|
||||
uint8[1206] data # packet contents
|
||||
|
||||
@@ -0,0 +1,4 @@
|
||||
# Velodyne LIDAR scan packets.
|
||||
|
||||
Header header # standard ROS message header
|
||||
VelodynePacket[] packets # vector of raw packets
|
||||
@@ -0,0 +1,23 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>velodyne_msgs</name>
|
||||
<version>1.5.2</version>
|
||||
<description>
|
||||
ROS message definitions for Velodyne 3D LIDARs.
|
||||
</description>
|
||||
<maintainer email="josh.whitley@autoware.org">Josh Whitley</maintainer>
|
||||
<author>Jack O'Quin</author>
|
||||
<license>BSD</license>
|
||||
|
||||
<url type="website">http://ros.org/wiki/velodyne_msgs</url>
|
||||
<url type="repository">https://github.com/ros-drivers/velodyne</url>
|
||||
<url type="bugtracker">https://github.com/ros-drivers/velodyne/issues</url>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<build_depend>message_generation</build_depend>
|
||||
|
||||
<depend>std_msgs</depend>
|
||||
|
||||
<exec_depend>message_runtime</exec_depend>
|
||||
</package>
|
||||
@@ -0,0 +1,301 @@
|
||||
Change history
|
||||
==============
|
||||
|
||||
1.5.2 (2019-01-28)
|
||||
------------------
|
||||
* Merge pull request `#205 <https://github.com/ros-drivers/velodyne/issues/205>`_ from xiesc/master
|
||||
support for 64E-S3
|
||||
* add an example yaml file for S3
|
||||
* Contributors: Joshua Whitley, Shichao XIE, xiesc
|
||||
|
||||
1.5.1 (2018-12-10)
|
||||
------------------
|
||||
* Merge pull request `#194 <https://github.com/ros-drivers/velodyne/issues/194>`_ from ros-drivers/avoid_unnecessary_computation
|
||||
Avoid unnecessary computation - causes approximately 20% performance increase on VLP-32C - should be similar for other sensors
|
||||
* std::vector<>::reserve is your friend
|
||||
* add static to avoid frequence memory allocation
|
||||
* avoid unecesary calculations in unpack()
|
||||
* Contributors: Davide Faconti, Joshua Whitley
|
||||
|
||||
1.5.0 (2018-10-19)
|
||||
------------------
|
||||
* Merge pull request `#164 <https://github.com/ros-drivers/velodyne/issues/164>`_ from ros-drivers/maint/vlp_32c_support
|
||||
Adding VLP-32C support.
|
||||
This was tested by AutonomouStuff and several external users. Though it does not include new information that I've learned (it appears that the distance resolution is different <50m vs >=50m), it is a good start.
|
||||
* Merge pull request `#189 <https://github.com/ros-drivers/velodyne/issues/189>`_ from kveretennicov/patch-1
|
||||
* Fix malformed plugin description XML
|
||||
ROS pluginlib only recognizes multiple <library> elements if they are under
|
||||
<class_libraries> XML root. It silently ignores malformed XMLs with multiple
|
||||
<library> "root"s and just reads the first one, due to relaxed way tinyxml2 does
|
||||
parsing. Though if you do `rosrun nodelet declared_nodelets`, the issue is
|
||||
reported properly.
|
||||
See also similar issue in https://github.com/ros-perception/perception_pcl/issues/131
|
||||
* Adding distance_resolution to test yaml files.
|
||||
* Adding VLP-32C support.
|
||||
Based on work done by @rockcdr. Adds distance_resolution calibration
|
||||
value to support 0.004m distance resolution for VLP-32C.
|
||||
* Contributors: Joshua Whitley, Konstantin Veretennicov
|
||||
|
||||
1.4.0 (2018-09-19)
|
||||
------------------
|
||||
* Merge pull request `#178 <https://github.com/ros-drivers/velodyne/issues/178>`_ from sts-thm/bugfix_issue\_`#174 <https://github.com/ros-drivers/velodyne/issues/174>`_
|
||||
Bugfix issue `#174 <https://github.com/ros-drivers/velodyne/issues/174>`_
|
||||
* Merge pull request `#177 <https://github.com/ros-drivers/velodyne/issues/177>`_ from C-NR/feature/WrapPointcloudData
|
||||
Feature/wrap pointcloud data
|
||||
* Changes fixing deadlock for specific cut_angle values.
|
||||
* moved definition of VPoint and VPointCloud back to namespace rawdata in rawdata.h
|
||||
* put a wrapper around pointcloud data including a generic setter method to enable the use of arbitrary data structures (pcl pointcloud, depth image, octomaps and so on) to be filled by just using RawData::unpack method with the wrapper object as parameter
|
||||
* Merge pull request `#170 <https://github.com/ros-drivers/velodyne/issues/170>`_ from ros-drivers/maint/move_header_files
|
||||
Moving header files to traditional location inside include folders.
|
||||
* Merge pull request `#160 <https://github.com/ros-drivers/velodyne/issues/160>`_ from ros-drivers/maint/updating_package_xml_to_v2
|
||||
* Updated all package.xmls to ver 2. Cleaned up catkin_lint errors.
|
||||
All package.xml files are now compatible with version 2 of the
|
||||
package.xml specification in REP 140. Removed some unnecessary
|
||||
execute permissions on a few files. Fixed a missing test_depend.
|
||||
* Merge pull request `#136 <https://github.com/ros-drivers/velodyne/issues/136>`_ from stsundermann/patch-1
|
||||
Use std::abs instead of abs
|
||||
* Adding missing 32C configuration file.
|
||||
* Merge pull request `#139 <https://github.com/ros-drivers/velodyne/issues/139>`_ from ASDeveloper00/vlp32
|
||||
Adding support for VLP-32C.
|
||||
* Merge pull request `#138 <https://github.com/ros-drivers/velodyne/issues/138>`_ from volkandre/cut_at_specified_angle_feature
|
||||
Cut at specified angle feature
|
||||
* Updated default cut_angle parameters in launch files after switching from deg to rad.
|
||||
* Use std::abs instead of abs
|
||||
abs is the c version which returns an integer. This is probably not intended here, so use the templated std::abs function.
|
||||
* Contributors: Andre Volk, Autonomoustuff Developer, CNR, Joshua Whitley, Kyle Rector, Stephan Sundermann, Tobias Athmer, kennouni
|
||||
|
||||
1.3.0 (2017-11-10)
|
||||
-------------------
|
||||
* Merge pull request `#110 <https://github.com/ros-drivers/velodyne/issues/110>`_ from kmhallen/master
|
||||
Added velodyne_laserscan package
|
||||
* Merge remote-tracking branch ros-drivers/master
|
||||
* Merge pull request `#129 <https://github.com/ros-drivers/velodyne/issues/129>`_ from kmhallen/pluginlib_macro
|
||||
Modern pluginlib macro
|
||||
* Update to use non deprecated pluginlib macro
|
||||
* Merge pull request `#127 <https://github.com/ros-drivers/velodyne/issues/127>`_ from swri-robotics/add_vlp16_hires_support
|
||||
Add VLP16 Puck Hi-Res config file
|
||||
* Add VLP16 Puck Hi-Res support
|
||||
* velodyne_pointcloud: remove incorrect catkin_package() DEPENDS option (`#93 <https://github.com/ros-drivers/velodyne/issues/93>`_)
|
||||
This eliminates a CMake warning when building on Xenial.
|
||||
* Merge pull request `#111 <https://github.com/ros-drivers/velodyne/issues/111>`_ from OrebroUniversity/master
|
||||
Added an interface to set up raw data processing offline
|
||||
* Added an interface to set up raw data processing from a locally defined calibration file. This method is useful when processing data offline from a bag file, without starting any ros master
|
||||
* Added velodyne_laserscan package and inserted into existing launch files
|
||||
* test multiple nodelet manager support (`#108 <https://github.com/ros-drivers/velodyne/issues/108>`_)
|
||||
* add launch args to support multiple devices (`#108 <https://github.com/ros-drivers/velodyne/issues/108>`_)
|
||||
* Merge pull request `#105 <https://github.com/ros-drivers/velodyne/issues/105>`_ from fudger/patch-1
|
||||
Remove unused constants.
|
||||
* Merge pull request `#104 <https://github.com/ros-drivers/velodyne/issues/104>`_ from altrouge/launch_options
|
||||
Add more options in launch files.
|
||||
* Rearranged alphabetically.
|
||||
* Remove unused constants.
|
||||
DISTANCE_MAX and DISTANCE_MAX_UNITS are not used anywhere in the code.
|
||||
Furthermore, using them would lead to errors as both VLP-64 manuals state that returns above 120 m should not be used. The VLP-32 manual allows 70 m as the maximum valid sensor range.
|
||||
* Merge pull request `#103 <https://github.com/ros-drivers/velodyne/issues/103>`_ from fudger/patch-1
|
||||
Fix misleading typecasts.
|
||||
* Add more options in launch files.
|
||||
- rpm, device_ip, port, read_once, read_fast, repeat_delay
|
||||
* Fix misleading typecasts.
|
||||
intensity and VPoint::intensity are both of type float.
|
||||
* update change history
|
||||
* merge current master (`#94 <https://github.com/ros-drivers/velodyne/issues/94>`_)
|
||||
* Merge pull request `#92 <https://github.com/ros-drivers/velodyne/issues/92>`_ from adasta/master
|
||||
GCC Build Warnings
|
||||
* Modified velodyne_point_cloud/src/lib/rawdata.cc to address warning
|
||||
that last_azimuth_diff variable may be used uninitialized. Variable
|
||||
is now initialized to 0 at creation.
|
||||
velodyne/velodyne_pointcloud/src/lib/rawdata.cc:328:57: error: ‘last_azimuth_diff’ may be used uninitialized in this function [-Werror=maybe-uninitialized]
|
||||
azimuth_corrected_f = azimuth + (azimuth_diff * ((dsr*VLP16_DSR_TOFFSET) + (firing*VLP16_FIRING_TOFFSET)) / VLP16_BLOCK_TDURATION);
|
||||
* Modified velodyne_pointcloud/src/conversion/colors.cc to remove
|
||||
address build warning for strict-aliasing.
|
||||
velodyne/velodyne_pointcloud/src/conversions/colors.cc:84:58:
|
||||
* Merge pull request `#89 <https://github.com/ros-drivers/velodyne/issues/89>`_ from Tones29/feat_dynrec_driver
|
||||
Add dynamic latency configuration to velodyne_driver
|
||||
* velodyne_pointcloud: Fix compile warning "Wrong initialization order"
|
||||
* velodyne_pointcloud: add dynamic reconfig update to change log (`#78 <https://github.com/ros-drivers/velodyne/issues/78>`_)
|
||||
* Merge branch fudger-reconfigure_transform_node
|
||||
* velodyne_pointcloud: use recommended add_dependencies() CMake variable `#78 <https://github.com/ros-drivers/velodyne/issues/78>`_
|
||||
* velodyne_pointcloud: fix transform unit tests
|
||||
Use tf2_ros static_transform_publisher for more consistent timing (`#2 <https://github.com/ros-drivers/velodyne/issues/2>`_)
|
||||
* Merge branch reconfigure_transform_node of https://github.com/fudger/velodyne
|
||||
* prepare change history for coming Indigo release (`#59 <https://github.com/ros-drivers/velodyne/issues/59>`_)
|
||||
* calibration: unit test case improvements (`#84 <https://github.com/ros-drivers/velodyne/issues/84>`_)
|
||||
* calibration: read all intensities as float, then convert (`#84 <https://github.com/ros-drivers/velodyne/issues/84>`_)
|
||||
* calibration: add gtest for `#84 <https://github.com/ros-drivers/velodyne/issues/84>`_
|
||||
This currently fails on 64e_s2.1-sztaki.yaml and on issue_84_float_intensities.yaml.
|
||||
* calibration: make max_intensity and min_intensity optional (`#84 <https://github.com/ros-drivers/velodyne/issues/84>`_)
|
||||
This fixes a regression in the 32e and VLP-16 calibrations which do not contain
|
||||
intensity values. There is still a problem with the 64e_s2.1 calibration.
|
||||
* Merge pull request `#76 <https://github.com/ros-drivers/velodyne/issues/76>`_ from pomerlef/master
|
||||
Sign inversion in some equations
|
||||
* Merge pull request `#82 <https://github.com/ros-drivers/velodyne/issues/82>`_ from ros-drivers/fix_pr_80
|
||||
Fix pr 80; adding travis CI tests.
|
||||
* fix the yaml-cpp 0.5 code paths
|
||||
* Merge pull request `#80 <https://github.com/ros-drivers/velodyne/issues/80>`_ from ros-drivers/fix_yaml_import
|
||||
allow floats in min/max_intensity and make horiz_offset_correction optional
|
||||
* allow horiz_offset_correction to be optional with 0 as default
|
||||
* allow floats instead of ints in min/max_intensity
|
||||
* Resolve frame ID name using tf prefix.
|
||||
* Improve coding style.
|
||||
* Set up dynamic reconfiguration for transform_node.
|
||||
Previously, transform_node has neither read parameters other than frame_id from the command line nor has it exposed these parameters via dynamic reconfigure. As parameters like max_range and view_width have been initialized to zero, the inconfigurable transform_node has returned an empty point cloud.
|
||||
Now, transform_node launches an reconfigure server just as cloud_node. In contrast to cloud_node, transform node exposes another parameter for dynamic reconfiguration: frame_id, i.e. the frame of reference the incoming Velodyne points are transformed to.
|
||||
* Merge pull request `#77 <https://github.com/ros-drivers/velodyne/issues/77>`_ from fudger/pretty_print
|
||||
Fix output of calibration data onto console
|
||||
* Add a missing space.
|
||||
* Fix line that always indicates use of model VLP-16.
|
||||
* Align console output of calibration data.
|
||||
* Merge branch master of https://github.com/ros-drivers/velodyne
|
||||
* resolve sign error
|
||||
* Merge pull request `#73 <https://github.com/ros-drivers/velodyne/issues/73>`_ from fudger/master
|
||||
Correct important data type error for VLP-16
|
||||
* Fix data type error that distorts the point cloud.
|
||||
* Fix and add a few comments.
|
||||
* Merge pull request `#68 <https://github.com/ros-drivers/velodyne/issues/68>`_ from jlblancoc/patch-1
|
||||
Remove unused variable
|
||||
* Remove unused variable
|
||||
I think that `dsr` was unused. See line 317:
|
||||
for (int dsr=0; ...
|
||||
* VLP-16: skip badly formatted data packets (`#62 <https://github.com/ros-drivers/velodyne/issues/62>`_, `#63 <https://github.com/ros-drivers/velodyne/issues/63>`_)
|
||||
* restore VLP-16 min_range setting to 0.4 (`#60 <https://github.com/ros-drivers/velodyne/issues/60>`_)
|
||||
NOTE: There is still some other problem keeping that from working.
|
||||
* permit min_range settings below 0.9 meters (`#60 <https://github.com/ros-drivers/velodyne/issues/60>`_)
|
||||
No known models are currently known to return closer measurements.
|
||||
* Merge pull request `#55 <https://github.com/ros-drivers/velodyne/issues/55>`_ from lemiant/azimuth_bug_VLP16
|
||||
Fixed azimuth overflow bug.
|
||||
* Fixed azimuth overflow bug.
|
||||
For interpolated azimuth values between 35999.5 and 36000.0 the nested round(fmod())
|
||||
yields a value of 36000 which is invalid and overflows the pre-computed sin/cos arrays,
|
||||
since they only go form 0..35999
|
||||
* Merge pull request `#51 <https://github.com/ros-drivers/velodyne/issues/51>`_ from kunlileo/master
|
||||
Added vertical sin angle correction
|
||||
* Added vertical sin angle correction
|
||||
* Merge pull request `#47 <https://github.com/ros-drivers/velodyne/issues/47>`_ from prclibo/master
|
||||
fixed rounding bug in intensity calculation found by songshiyu
|
||||
* fixed rounding bug in intensity calculation found by songshiyu
|
||||
* fix some overly long C++ source lines
|
||||
* Merge pull request `#44 <https://github.com/ros-drivers/velodyne/issues/44>`_ from SISegwayRmp/master
|
||||
adding driver and pointcloud support for the VLP16
|
||||
* missed the space in the file name which caused the build to fail, removed space before extension
|
||||
* adding the VLP16 test scripts and updating the CMakeLists to include the test file from http://download.ros.org/data/velodyne/vlp16.pcap
|
||||
* adding support for the VLP16
|
||||
* Merge pull request `#43 <https://github.com/ros-drivers/velodyne/issues/43>`_ from prclibo/fix_rawdata
|
||||
fixed point computation according to the 64e_s2(.1) velodyne manual
|
||||
* fixed point computation according to the 64e_s2(.1) velodyne manual, with luopei"s help
|
||||
* Merge pull request `#41 <https://github.com/ros-drivers/velodyne/issues/41>`_ from prclibo/master
|
||||
fixed a calibration file parsing bug
|
||||
* Merge pull request `#42 <https://github.com/ros-drivers/velodyne/issues/42>`_ from prclibo/fix_gen_calibration
|
||||
fixed gen_calibration min/max intensity type
|
||||
* fixed gen_calibration min/max intensity type
|
||||
* fixed a calibration file parsing bug
|
||||
* Contributors: Adam Stambler, Alex Rodrigues, Alexander Schaefer, Andreas Wachaja, Bo Li, Daniel Jartoux, Gabor Meszaros, Jack OQuin, Jose Luis Blanco-Claraco, Joshua Whitley, Kevin Hallenbeck, Kris Kozak, Kun Li, Micho Radovnikovich, Scott K Logan, Thomas Solatges, Todor Stoyanov, William Woodall, jack.oquin, libo24, phussey, piyushk, pomerlef
|
||||
|
||||
1.2.0 (2014-08-06)
|
||||
------------------
|
||||
|
||||
* velodyne_pointcloud: remove model-dependent "constants" from
|
||||
rawdata.h (`#28
|
||||
<https://github.com/ros-drivers/velodyne/issues/28>`_)
|
||||
* velodyne_pointcloud: change default min_range to 0.9 meters (`#25
|
||||
<https://github.com/ros-drivers/velodyne/issues/25>`_)
|
||||
* Added support for YAML-CPP 0.5+ (`#23
|
||||
<https://github.com/ros-drivers/velodyne/pull/23>`_).
|
||||
* Add dynamic_reconfigure feature.
|
||||
* Add angular limits to the output point cloud, useful for omitting
|
||||
part of it. (`#22 <https://github.com/ros-drivers/velodyne/pull/22>`_).
|
||||
* Contributors: Jack OQuin, Scott K Logan, Thomas Solatges
|
||||
|
||||
1.1.2 (2013-11-05)
|
||||
------------------
|
||||
|
||||
* Move unit test data to download.ros.org (`#18`_).
|
||||
* Install missing gen_calibration.py script (`#20`_).
|
||||
|
||||
1.1.1 (2013-07-30)
|
||||
------------------
|
||||
|
||||
* Fix lost frame_id transform problem caused by PCL 1.7 fix (`#13`_).
|
||||
* Add support for HDL-64E S2 and S2.1 models, which were not working
|
||||
before (`#11`_), thanks to Gabor Meszaros (`#12`_).
|
||||
* Add additional parameters to launch files (`#14`_).
|
||||
* Contributors: Gabor Meszaros, Jack OQuin
|
||||
|
||||
1.1.0 (2013-07-16)
|
||||
------------------
|
||||
|
||||
* Fix build problems due to PCL 1.7 API incompatibilities (`#8`_),
|
||||
thanks to William Woodall. This version also works with Groovy, as
|
||||
long as the correct ``pcl_conversions`` is installed.
|
||||
* Fix errors with Mac OSX compiler (`#8`_).
|
||||
* Install ``pluginlib`` XML files (`#9`_).
|
||||
* Install some launch and parameter files.
|
||||
* Enable unit tests when ``CATKIN_ENABLE_TESTING`` is set (`#10`_).
|
||||
|
||||
1.0.1 (2013-06-15)
|
||||
------------------
|
||||
|
||||
* Declare explicit ``pluginlib`` dependency (`#4`_).
|
||||
|
||||
1.0.0 (2013-06-14)
|
||||
------------------
|
||||
|
||||
* Convert to catkin (`#1`_).
|
||||
* Release to Hydro.
|
||||
|
||||
0.9.2 (2013-07-08)
|
||||
------------------
|
||||
|
||||
* Fix Groovy build problem (`#7`_).
|
||||
|
||||
0.9.1 (2012-06-05)
|
||||
------------------
|
||||
|
||||
* Only include "enabled" lasers in YAML calibration file.
|
||||
* New param subdirectory for parameter files.
|
||||
* Add launch file for the HDL-32E.
|
||||
* Add rviz_points.vcg file for viewing Velodyne point clouds with rviz.
|
||||
* Fix bug when reading configuration with default minIntensity.
|
||||
* Add unit tests with 32E data.
|
||||
* Released to Electric, Fuerte and Groovy.
|
||||
|
||||
0.9.0 (2012-04-03)
|
||||
------------------
|
||||
|
||||
* Completely revised API, anticipating a 1.0.0 release.
|
||||
* HDL-32E device support.
|
||||
* New YAML configuration file format.
|
||||
* New velodyne_driver and velodyne_pointcloud packages.
|
||||
* Old velodyne_common and velodyne_pcl packages no longer included.
|
||||
* Released to Electric, Fuerte and Groovy.
|
||||
|
||||
0.2.6 (2011-02-23)
|
||||
------------------
|
||||
|
||||
* Label all timing-dependent tests "realtime" so they do not run by
|
||||
default on the build farm machines.
|
||||
|
||||
0.2.5 (2010-11-19)
|
||||
------------------
|
||||
|
||||
* Initial implementation of new 0.3 interfaces.
|
||||
* Support for ROS 1.3 `std_msgs::Header` changes.
|
||||
|
||||
0.2.0 (2010-08-17)
|
||||
------------------
|
||||
|
||||
* Initial release to ROS C-turtle.
|
||||
|
||||
.. _`#1`: https://github.com/ros-drivers/velodyne/issues/1
|
||||
.. _`#4`: https://github.com/ros-drivers/velodyne/issues/4
|
||||
.. _`#7`: https://github.com/ros-drivers/velodyne/issues/7
|
||||
.. _`#8`: https://github.com/ros-drivers/velodyne/pull/8
|
||||
.. _`#9`: https://github.com/ros-drivers/velodyne/issues/9
|
||||
.. _`#10`: https://github.com/ros-drivers/velodyne/issues/10
|
||||
.. _`#11`: https://github.com/ros-drivers/velodyne/issues/11
|
||||
.. _`#12`: https://github.com/ros-drivers/velodyne/pull/12
|
||||
.. _`#13`: https://github.com/ros-drivers/velodyne/issues/13
|
||||
.. _`#14`: https://github.com/ros-drivers/velodyne/pull/14
|
||||
.. _`#17`: https://github.com/ros-drivers/velodyne/issues/17
|
||||
.. _`#18`: https://github.com/ros-drivers/velodyne/issues/18
|
||||
.. _`#20`: https://github.com/ros-drivers/velodyne/issues/20
|
||||
.. _`#50`: https://github.com/ros-drivers/velodyne/issue/50
|
||||
@@ -0,0 +1,86 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(velodyne_pointcloud)
|
||||
|
||||
# Set minimum C++ standard to C++11
|
||||
if (NOT "${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}")
|
||||
message(STATUS "Changing CXX_STANDARD from C++98 to C++11")
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
|
||||
elseif ("${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}" STREQUAL "98")
|
||||
message(STATUS "Changing CXX_STANDARD from C++98 to C++11")
|
||||
set(CMAKE_CXX_STANDARD 11)
|
||||
endif()
|
||||
|
||||
set(${PROJECT_NAME}_CATKIN_DEPS
|
||||
angles
|
||||
nodelet
|
||||
roscpp
|
||||
roslib
|
||||
sensor_msgs
|
||||
tf
|
||||
velodyne_driver
|
||||
velodyne_msgs
|
||||
dynamic_reconfigure
|
||||
diagnostic_updater
|
||||
)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
${${PROJECT_NAME}_CATKIN_DEPS}
|
||||
roslint)
|
||||
|
||||
find_package(Boost COMPONENTS signals)
|
||||
find_package(Eigen3 REQUIRED)
|
||||
# Resolve system dependency on yaml-cpp, which apparently does not
|
||||
# provide a CMake find_package() module.
|
||||
find_package(PkgConfig REQUIRED)
|
||||
pkg_check_modules(YAML_CPP REQUIRED yaml-cpp)
|
||||
find_path(YAML_CPP_INCLUDE_DIR
|
||||
NAMES yaml_cpp.h
|
||||
PATHS ${YAML_CPP_INCLUDE_DIRS})
|
||||
find_library(YAML_CPP_LIBRARY
|
||||
NAMES YAML_CPP
|
||||
PATHS ${YAML_CPP_LIBRARY_DIRS})
|
||||
|
||||
link_directories(${YAML_CPP_LIBRARY_DIRS})
|
||||
|
||||
generate_dynamic_reconfigure_options(
|
||||
cfg/CloudNode.cfg cfg/TransformNode.cfg
|
||||
)
|
||||
|
||||
if(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5")
|
||||
add_definitions(-DHAVE_NEW_YAMLCPP)
|
||||
endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5")
|
||||
|
||||
include_directories(include ${catkin_INCLUDE_DIRS}
|
||||
${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake
|
||||
${EIGEN3_INCLUDE_DIR}
|
||||
)
|
||||
|
||||
catkin_package(
|
||||
CATKIN_DEPENDS ${${PROJECT_NAME}_CATKIN_DEPS}
|
||||
INCLUDE_DIRS include
|
||||
LIBRARIES velodyne_rawdata)
|
||||
|
||||
#add_executable(dynamic_reconfigure_node src/dynamic_reconfigure_node.cpp)
|
||||
#target_link_libraries(dynamic_reconfigure_node
|
||||
# ${catkin_LIBRARIES}
|
||||
# )
|
||||
|
||||
add_subdirectory(src/lib)
|
||||
add_subdirectory(src/conversions)
|
||||
|
||||
install(DIRECTORY include/${PROJECT_NAME}/
|
||||
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
|
||||
install(FILES nodelets.xml
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
|
||||
install(DIRECTORY launch/
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch)
|
||||
install(DIRECTORY params/
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/params)
|
||||
install(PROGRAMS scripts/gen_calibration.py
|
||||
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
|
||||
|
||||
roslint_cpp()
|
||||
|
||||
if (CATKIN_ENABLE_TESTING)
|
||||
add_subdirectory(tests)
|
||||
endif()
|
||||
@@ -0,0 +1,19 @@
|
||||
#!/usr/bin/env python
|
||||
PACKAGE = "velodyne_pointcloud"
|
||||
|
||||
from math import pi
|
||||
from dynamic_reconfigure.parameter_generator_catkin import *
|
||||
|
||||
gen = ParameterGenerator()
|
||||
|
||||
gen.add("fixed_frame", str_t, 0, "The desired input frame", "velodyne")
|
||||
gen.add("target_frame", str_t, 0, "The desired output frame", "velodyne")
|
||||
gen.add("min_range", double_t, 0, "min range to publish", 0.9, 0.1, 10.0)
|
||||
gen.add("max_range", double_t, 0, "max range to publish", 130, 0.1, 200)
|
||||
gen.add("view_direction", double_t, 0, "angle defining the center of view",
|
||||
0.0, -pi, pi)
|
||||
gen.add("view_width", double_t, 0, "angle defining the view width",
|
||||
2*pi, 0.0, 2*pi)
|
||||
gen.add("organize_cloud", bool_t, 0, "organized cloud", False)
|
||||
|
||||
exit(gen.generate(PACKAGE, "cloud_node", "CloudNode"))
|
||||
@@ -0,0 +1,45 @@
|
||||
#!/usr/bin/env python
|
||||
PACKAGE = "velodyne_pointcloud"
|
||||
|
||||
from math import pi
|
||||
import dynamic_reconfigure.parameter_generator_catkin as pgc
|
||||
|
||||
gen = pgc.ParameterGenerator()
|
||||
|
||||
gen.add("min_range",
|
||||
pgc.double_t,
|
||||
0,
|
||||
"min range to publish",
|
||||
0.9, 0.1, 10.0)
|
||||
|
||||
gen.add("max_range",
|
||||
pgc.double_t,
|
||||
0,
|
||||
"max range to publish",
|
||||
130, 0.1, 200)
|
||||
|
||||
gen.add("view_direction",
|
||||
pgc.double_t,
|
||||
0,
|
||||
"angle defining the center of view",
|
||||
0.0, -pi, pi)
|
||||
|
||||
gen.add("view_width",
|
||||
pgc.double_t,
|
||||
0,
|
||||
"angle defining the view width",
|
||||
2*pi, 0.0, 2*pi)
|
||||
|
||||
gen.add("frame_id",
|
||||
pgc.str_t,
|
||||
0,
|
||||
"fixed frame of reference for point clouds",
|
||||
"map")
|
||||
|
||||
gen.add("organize_cloud",
|
||||
pgc.bool_t,
|
||||
0,
|
||||
"organize cloud",
|
||||
False)
|
||||
|
||||
exit(gen.generate(PACKAGE, "transform_node", "TransformNode"))
|
||||
@@ -0,0 +1,109 @@
|
||||
// Copyright (C) 2012, 2019 Austin Robot Technology, Piyush Khandelwal, Joshua Whitley
|
||||
// All rights reserved.
|
||||
//
|
||||
// Software License Agreement (BSD License 2.0)
|
||||
//
|
||||
// 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 {copyright_holder} 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.
|
||||
|
||||
#ifndef VELODYNE_POINTCLOUD_CALIBRATION_H
|
||||
#define VELODYNE_POINTCLOUD_CALIBRATION_H
|
||||
|
||||
#include <map>
|
||||
#include <vector>
|
||||
#include <string>
|
||||
|
||||
namespace velodyne_pointcloud
|
||||
{
|
||||
|
||||
/** \brief correction values for a single laser
|
||||
*
|
||||
* Correction values for a single laser (as provided by db.xml from
|
||||
* Velodyne). Includes parameters for Velodyne HDL-64E S2.1.
|
||||
*
|
||||
* http://velodynelidar.com/lidar/products/manual/63-HDL64E%20S2%20Manual_Rev%20D_2011_web.pdf
|
||||
*/
|
||||
|
||||
/** \brief Correction information for a single laser. */
|
||||
struct LaserCorrection
|
||||
{
|
||||
/** parameters in db.xml */
|
||||
float rot_correction;
|
||||
float vert_correction;
|
||||
float dist_correction;
|
||||
bool two_pt_correction_available;
|
||||
float dist_correction_x;
|
||||
float dist_correction_y;
|
||||
float vert_offset_correction;
|
||||
float horiz_offset_correction;
|
||||
int max_intensity;
|
||||
int min_intensity;
|
||||
float focal_distance;
|
||||
float focal_slope;
|
||||
|
||||
/** cached values calculated when the calibration file is read */
|
||||
float cos_rot_correction; ///< cosine of rot_correction
|
||||
float sin_rot_correction; ///< sine of rot_correction
|
||||
float cos_vert_correction; ///< cosine of vert_correction
|
||||
float sin_vert_correction; ///< sine of vert_correction
|
||||
|
||||
int laser_ring; ///< ring number for this laser
|
||||
};
|
||||
|
||||
/** \brief Calibration information for the entire device. */
|
||||
class Calibration
|
||||
{
|
||||
public:
|
||||
float distance_resolution_m;
|
||||
std::map<int, LaserCorrection> laser_corrections_map;
|
||||
std::vector<LaserCorrection> laser_corrections;
|
||||
int num_lasers;
|
||||
bool initialized;
|
||||
bool ros_info;
|
||||
|
||||
public:
|
||||
explicit Calibration(bool info = true)
|
||||
: distance_resolution_m(0.002f),
|
||||
num_lasers(0),
|
||||
initialized(false),
|
||||
ros_info(info) {}
|
||||
explicit Calibration(
|
||||
const std::string& calibration_file,
|
||||
bool info = true)
|
||||
: distance_resolution_m(0.002f),
|
||||
ros_info(info)
|
||||
{
|
||||
read(calibration_file);
|
||||
}
|
||||
|
||||
void read(const std::string& calibration_file);
|
||||
void write(const std::string& calibration_file);
|
||||
};
|
||||
|
||||
} // namespace velodyne_pointcloud
|
||||
|
||||
#endif // VELODYNE_POINTCLOUD_CALIBRATION_H
|
||||
@@ -0,0 +1,102 @@
|
||||
// Copyright (C) 2009, 2010, 2011, 2012, 2019 Austin Robot Technology, Jack O'Quin, Jesse Vera, Joshua Whitley
|
||||
// All rights reserved.
|
||||
//
|
||||
// Software License Agreement (BSD License 2.0)
|
||||
//
|
||||
// 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 {copyright_holder} 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.
|
||||
|
||||
/** @file
|
||||
|
||||
This class converts raw Velodyne 3D LIDAR packets to PointCloud2.
|
||||
|
||||
*/
|
||||
|
||||
#ifndef VELODYNE_POINTCLOUD_CONVERT_H
|
||||
#define VELODYNE_POINTCLOUD_CONVERT_H
|
||||
|
||||
#include <string>
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <diagnostic_updater/diagnostic_updater.h>
|
||||
#include <diagnostic_updater/publisher.h>
|
||||
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <velodyne_pointcloud/rawdata.h>
|
||||
|
||||
#include <dynamic_reconfigure/server.h>
|
||||
#include <velodyne_pointcloud/CloudNodeConfig.h>
|
||||
|
||||
namespace velodyne_pointcloud
|
||||
{
|
||||
class Convert
|
||||
{
|
||||
public:
|
||||
Convert(
|
||||
ros::NodeHandle node,
|
||||
ros::NodeHandle private_nh,
|
||||
std::string const & node_name = ros::this_node::getName());
|
||||
~Convert() {}
|
||||
|
||||
private:
|
||||
void callback(velodyne_pointcloud::CloudNodeConfig &config, uint32_t level);
|
||||
void processScan(const velodyne_msgs::VelodyneScan::ConstPtr &scanMsg);
|
||||
|
||||
boost::shared_ptr<dynamic_reconfigure::Server<velodyne_pointcloud::CloudNodeConfig> > srv_;
|
||||
|
||||
boost::shared_ptr<velodyne_rawdata::RawData> data_;
|
||||
ros::Subscriber velodyne_scan_;
|
||||
ros::Publisher output_;
|
||||
|
||||
boost::shared_ptr<velodyne_rawdata::DataContainerBase> container_ptr_;
|
||||
|
||||
boost::mutex reconfigure_mtx_;
|
||||
|
||||
/// configuration parameters
|
||||
typedef struct
|
||||
{
|
||||
std::string target_frame; ///< target frame
|
||||
std::string fixed_frame; ///< fixed frame
|
||||
bool organize_cloud; ///< enable/disable organized cloud structure
|
||||
double max_range; ///< maximum range to publish
|
||||
double min_range; ///< minimum range to publish
|
||||
uint16_t num_lasers; ///< number of lasers
|
||||
int npackets; ///< number of packets to combine
|
||||
}
|
||||
Config;
|
||||
Config config_;
|
||||
bool first_rcfg_call;
|
||||
|
||||
// diagnostics updater
|
||||
diagnostic_updater::Updater diagnostics_;
|
||||
double diag_min_freq_;
|
||||
double diag_max_freq_;
|
||||
boost::shared_ptr<diagnostic_updater::TopicDiagnostic> diag_topic_;
|
||||
};
|
||||
} // namespace velodyne_pointcloud
|
||||
|
||||
#endif // VELODYNE_POINTCLOUD_CONVERT_H
|
||||
@@ -0,0 +1,208 @@
|
||||
#ifndef VELODYNE_POINTCLOUD_DATACONTAINERBASE_H
|
||||
#define VELODYNE_POINTCLOUD_DATACONTAINERBASE_H
|
||||
// Copyright (C) 2012, 2019 Austin Robot Technology, Jack O'Quin, Joshua Whitley, Sebastian Pütz
|
||||
// All rights reserved.
|
||||
//
|
||||
// Software License Agreement (BSD License 2.0)
|
||||
//
|
||||
// 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 {copyright_holder} 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.
|
||||
|
||||
#include <tf/transform_listener.h>
|
||||
#include <velodyne_msgs/VelodyneScan.h>
|
||||
#include <sensor_msgs/point_cloud2_iterator.h>
|
||||
#include <Eigen/Dense>
|
||||
#include <string>
|
||||
#include <algorithm>
|
||||
#include <cstdarg>
|
||||
|
||||
namespace velodyne_rawdata
|
||||
{
|
||||
class DataContainerBase
|
||||
{
|
||||
public:
|
||||
DataContainerBase(const double max_range, const double min_range, const std::string& target_frame,
|
||||
const std::string& fixed_frame, const unsigned int init_width, const unsigned int init_height,
|
||||
const bool is_dense, const unsigned int scans_per_packet,
|
||||
boost::shared_ptr<tf::TransformListener>& tf_ptr, int fields, ...)
|
||||
: config_(max_range, min_range, target_frame, fixed_frame, init_width, init_height, is_dense, scans_per_packet)
|
||||
, tf_ptr(tf_ptr)
|
||||
{
|
||||
va_list vl;
|
||||
cloud.fields.clear();
|
||||
cloud.fields.reserve(fields);
|
||||
va_start(vl, fields);
|
||||
int offset = 0;
|
||||
for (int i = 0; i < fields; ++i)
|
||||
{
|
||||
// Create the corresponding PointField
|
||||
std::string name(va_arg(vl, char*));
|
||||
int count(va_arg(vl, int));
|
||||
int datatype(va_arg(vl, int));
|
||||
offset = addPointField(cloud, name, count, datatype, offset);
|
||||
}
|
||||
va_end(vl);
|
||||
cloud.point_step = offset;
|
||||
cloud.row_step = init_width * cloud.point_step;
|
||||
if (config_.transform && !tf_ptr)
|
||||
{
|
||||
tf_ptr = boost::shared_ptr<tf::TransformListener>(new tf::TransformListener);
|
||||
}
|
||||
}
|
||||
|
||||
struct Config
|
||||
{
|
||||
double max_range; ///< maximum range to publish
|
||||
double min_range; ///< minimum range to publish
|
||||
std::string target_frame; ///< target frame to transform a point
|
||||
std::string fixed_frame; ///< fixed frame used for transform
|
||||
unsigned int init_width;
|
||||
unsigned int init_height;
|
||||
bool is_dense;
|
||||
unsigned int scans_per_packet;
|
||||
bool transform; ///< enable / disable transform points
|
||||
|
||||
Config(double max_range, double min_range, std::string target_frame, std::string fixed_frame,
|
||||
unsigned int init_width, unsigned int init_height, bool is_dense, unsigned int scans_per_packet)
|
||||
: max_range(max_range)
|
||||
, min_range(min_range)
|
||||
, target_frame(target_frame)
|
||||
, fixed_frame(fixed_frame)
|
||||
, transform(fixed_frame != target_frame)
|
||||
, init_width(init_width)
|
||||
, init_height(init_height)
|
||||
, is_dense(is_dense)
|
||||
, scans_per_packet(scans_per_packet)
|
||||
{
|
||||
ROS_INFO_STREAM("Initialized container with "
|
||||
<< "min_range: " << min_range << ", max_range: " << max_range
|
||||
<< ", target_frame: " << target_frame << ", fixed_frame: " << fixed_frame
|
||||
<< ", init_width: " << init_width << ", init_height: " << init_height
|
||||
<< ", is_dense: " << is_dense << ", scans_per_packet: " << scans_per_packet);
|
||||
}
|
||||
};
|
||||
|
||||
virtual void setup(const velodyne_msgs::VelodyneScan::ConstPtr& scan_msg)
|
||||
{
|
||||
cloud.header = scan_msg->header;
|
||||
cloud.data.resize(scan_msg->packets.size() * config_.scans_per_packet * cloud.point_step);
|
||||
cloud.width = config_.init_width;
|
||||
cloud.height = config_.init_height;
|
||||
cloud.is_dense = static_cast<uint8_t>(config_.is_dense);
|
||||
}
|
||||
|
||||
virtual void addPoint(float x, float y, float z, const uint16_t ring, const uint16_t azimuth, const float distance,
|
||||
const float intensity, const float time) = 0;
|
||||
virtual void newLine() = 0;
|
||||
|
||||
const sensor_msgs::PointCloud2& finishCloud()
|
||||
{
|
||||
cloud.data.resize(cloud.point_step * cloud.width * cloud.height);
|
||||
cloud.row_step = cloud.point_step * cloud.width;
|
||||
|
||||
if (!config_.target_frame.empty())
|
||||
{
|
||||
cloud.header.frame_id = config_.target_frame;
|
||||
}
|
||||
|
||||
ROS_DEBUG_STREAM("Prepared cloud width" << cloud.height * cloud.width
|
||||
<< " Velodyne points, time: " << cloud.header.stamp);
|
||||
return cloud;
|
||||
}
|
||||
|
||||
void configure(const double max_range, const double min_range, const std::string fixed_frame,
|
||||
const std::string target_frame)
|
||||
{
|
||||
config_.max_range = max_range;
|
||||
config_.min_range = min_range;
|
||||
config_.fixed_frame = fixed_frame;
|
||||
config_.target_frame = target_frame;
|
||||
|
||||
config_.transform = fixed_frame.compare(target_frame) != 0;
|
||||
if (config_.transform && !tf_ptr)
|
||||
{
|
||||
tf_ptr = boost::shared_ptr<tf::TransformListener>(new tf::TransformListener);
|
||||
}
|
||||
}
|
||||
|
||||
sensor_msgs::PointCloud2 cloud;
|
||||
|
||||
inline void vectorTfToEigen(tf::Vector3& tf_vec, Eigen::Vector3f& eigen_vec)
|
||||
{
|
||||
eigen_vec(0) = tf_vec[0];
|
||||
eigen_vec(1) = tf_vec[1];
|
||||
eigen_vec(2) = tf_vec[2];
|
||||
}
|
||||
|
||||
inline bool computeTransformation(const ros::Time& time)
|
||||
{
|
||||
tf::StampedTransform transform;
|
||||
try
|
||||
{
|
||||
tf_ptr->lookupTransform(config_.target_frame, cloud.header.frame_id, time, transform);
|
||||
}
|
||||
catch (tf::LookupException& e)
|
||||
{
|
||||
ROS_ERROR("%s", e.what());
|
||||
return false;
|
||||
}
|
||||
catch (tf::ExtrapolationException& e)
|
||||
{
|
||||
ROS_ERROR("%s", e.what());
|
||||
return false;
|
||||
}
|
||||
|
||||
tf::Quaternion quaternion = transform.getRotation();
|
||||
Eigen::Quaternionf rotation(quaternion.w(), quaternion.x(), quaternion.y(), quaternion.z());
|
||||
|
||||
Eigen::Vector3f eigen_origin;
|
||||
vectorTfToEigen(transform.getOrigin(), eigen_origin);
|
||||
Eigen::Translation3f translation(eigen_origin);
|
||||
transformation = translation * rotation;
|
||||
return true;
|
||||
}
|
||||
|
||||
inline void transformPoint(float& x, float& y, float& z)
|
||||
{
|
||||
Eigen::Vector3f p = transformation * Eigen::Vector3f(x, y, z);
|
||||
x = p.x();
|
||||
y = p.y();
|
||||
z = p.z();
|
||||
}
|
||||
|
||||
inline bool pointInRange(float range)
|
||||
{
|
||||
return (range >= config_.min_range && range <= config_.max_range);
|
||||
}
|
||||
|
||||
protected:
|
||||
Config config_;
|
||||
boost::shared_ptr<tf::TransformListener> tf_ptr; ///< transform listener
|
||||
Eigen::Affine3f transformation;
|
||||
};
|
||||
} /* namespace velodyne_rawdata */
|
||||
#endif // VELODYNE_POINTCLOUD_DATACONTAINERBASE_H
|
||||
+61
@@ -0,0 +1,61 @@
|
||||
// Copyright (C) 2012, 2019 Austin Robot Technology, Jack O'Quin, Joshua Whitley, Sebastian Pütz
|
||||
// All rights reserved.
|
||||
//
|
||||
// Software License Agreement (BSD License 2.0)
|
||||
//
|
||||
// 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 {copyright_holder} 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.
|
||||
|
||||
#ifndef VELODYNE_POINTCLOUD_ORGANIZED_CLOUDXYZIR_H
|
||||
#define VELODYNE_POINTCLOUD_ORGANIZED_CLOUDXYZIR_H
|
||||
|
||||
#include <velodyne_pointcloud/datacontainerbase.h>
|
||||
#include <sensor_msgs/point_cloud2_iterator.h>
|
||||
#include <string>
|
||||
|
||||
namespace velodyne_pointcloud
|
||||
{
|
||||
class OrganizedCloudXYZIR : public velodyne_rawdata::DataContainerBase
|
||||
{
|
||||
public:
|
||||
OrganizedCloudXYZIR(const double max_range, const double min_range, const std::string& target_frame,
|
||||
const std::string& fixed_frame, const unsigned int num_lasers, const unsigned int scans_per_block,
|
||||
boost::shared_ptr<tf::TransformListener> tf_ptr = boost::shared_ptr<tf::TransformListener>());
|
||||
|
||||
virtual void newLine();
|
||||
|
||||
virtual void setup(const velodyne_msgs::VelodyneScan::ConstPtr& scan_msg);
|
||||
|
||||
virtual void addPoint(float x, float y, float z, const uint16_t ring, const uint16_t azimuth, const float distance,
|
||||
const float intensity, const float time);
|
||||
|
||||
private:
|
||||
sensor_msgs::PointCloud2Iterator<float> iter_x, iter_y, iter_z, iter_intensity, iter_time;
|
||||
sensor_msgs::PointCloud2Iterator<uint16_t> iter_ring;
|
||||
};
|
||||
} /* namespace velodyne_pointcloud */
|
||||
#endif // VELODYNE_POINTCLOUD_ORGANIZED_CLOUDXYZIR_H
|
||||
@@ -0,0 +1,60 @@
|
||||
// Copyright (C) 2012, 2019 Austin Robot Technology, Jack O'Quin, Joshua Whitley, Sebastian Pütz
|
||||
// All rights reserved.
|
||||
//
|
||||
// Software License Agreement (BSD License 2.0)
|
||||
//
|
||||
// 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 {copyright_holder} 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.
|
||||
|
||||
#ifndef VELODYNE_POINTCLOUD_POINTCLOUDXYZIR_H
|
||||
#define VELODYNE_POINTCLOUD_POINTCLOUDXYZIR_H
|
||||
|
||||
#include <velodyne_pointcloud/datacontainerbase.h>
|
||||
#include <string>
|
||||
|
||||
namespace velodyne_pointcloud
|
||||
{
|
||||
class PointcloudXYZIR : public velodyne_rawdata::DataContainerBase
|
||||
{
|
||||
public:
|
||||
PointcloudXYZIR(const double max_range, const double min_range, const std::string& target_frame,
|
||||
const std::string& fixed_frame, const unsigned int scans_per_block,
|
||||
boost::shared_ptr<tf::TransformListener> tf_ptr = boost::shared_ptr<tf::TransformListener>());
|
||||
|
||||
virtual void newLine();
|
||||
|
||||
virtual void setup(const velodyne_msgs::VelodyneScan::ConstPtr& scan_msg);
|
||||
|
||||
virtual void addPoint(float x, float y, float z, const uint16_t ring, const uint16_t azimuth,
|
||||
const float distance, const float intensity, const float time);
|
||||
|
||||
sensor_msgs::PointCloud2Iterator<float> iter_x, iter_y, iter_z, iter_intensity, iter_time;
|
||||
sensor_msgs::PointCloud2Iterator<uint16_t> iter_ring;
|
||||
};
|
||||
} // namespace velodyne_pointcloud
|
||||
|
||||
#endif // VELODYNE_POINTCLOUD_POINTCLOUDXYZIR_H
|
||||
@@ -0,0 +1,216 @@
|
||||
// Copyright (C) 2007, 2009, 2010, 2012, 2019 Yaxin Liu, Patrick Beeson, Jack O'Quin, Joshua Whitley
|
||||
// All rights reserved.
|
||||
//
|
||||
// Software License Agreement (BSD License 2.0)
|
||||
//
|
||||
// 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 {copyright_holder} 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.
|
||||
|
||||
/** @file
|
||||
*
|
||||
* @brief Interfaces for interpreting raw packets from the Velodyne 3D LIDAR.
|
||||
*
|
||||
* @author Yaxin Liu
|
||||
* @author Patrick Beeson
|
||||
* @author Jack O'Quin
|
||||
*/
|
||||
|
||||
#ifndef VELODYNE_POINTCLOUD_RAWDATA_H
|
||||
#define VELODYNE_POINTCLOUD_RAWDATA_H
|
||||
|
||||
#include <errno.h>
|
||||
#include <stdint.h>
|
||||
#include <string>
|
||||
#include <boost/format.hpp>
|
||||
#include <math.h>
|
||||
#include <vector>
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <velodyne_msgs/VelodyneScan.h>
|
||||
#include <velodyne_pointcloud/calibration.h>
|
||||
#include <velodyne_pointcloud/datacontainerbase.h>
|
||||
|
||||
namespace velodyne_rawdata
|
||||
{
|
||||
/**
|
||||
* Raw Velodyne packet constants and structures.
|
||||
*/
|
||||
static const int SIZE_BLOCK = 100;
|
||||
static const int RAW_SCAN_SIZE = 3;
|
||||
static const int SCANS_PER_BLOCK = 32;
|
||||
static const int BLOCK_DATA_SIZE = (SCANS_PER_BLOCK * RAW_SCAN_SIZE);
|
||||
|
||||
static const float ROTATION_RESOLUTION = 0.01f; // [deg]
|
||||
static const uint16_t ROTATION_MAX_UNITS = 36000u; // [deg/100]
|
||||
|
||||
/** @todo make this work for both big and little-endian machines */
|
||||
static const uint16_t UPPER_BANK = 0xeeff;
|
||||
static const uint16_t LOWER_BANK = 0xddff;
|
||||
|
||||
/** Special Defines for VLP16 support **/
|
||||
static const int VLP16_FIRINGS_PER_BLOCK = 2;
|
||||
static const int VLP16_SCANS_PER_FIRING = 16;
|
||||
static const float VLP16_BLOCK_TDURATION = 110.592f; // [µs]
|
||||
static const float VLP16_DSR_TOFFSET = 2.304f; // [µs]
|
||||
static const float VLP16_FIRING_TOFFSET = 55.296f; // [µs]
|
||||
|
||||
/** \brief Raw Velodyne data block.
|
||||
*
|
||||
* Each block contains data from either the upper or lower laser
|
||||
* bank. The device returns three times as many upper bank blocks.
|
||||
*
|
||||
* use stdint.h types, so things work with both 64 and 32-bit machines
|
||||
*/
|
||||
typedef struct raw_block
|
||||
{
|
||||
uint16_t header; ///< UPPER_BANK or LOWER_BANK
|
||||
uint16_t rotation; ///< 0-35999, divide by 100 to get degrees
|
||||
uint8_t data[BLOCK_DATA_SIZE];
|
||||
}
|
||||
raw_block_t;
|
||||
|
||||
/** used for unpacking the first two data bytes in a block
|
||||
*
|
||||
* They are packed into the actual data stream misaligned. I doubt
|
||||
* this works on big endian machines.
|
||||
*/
|
||||
union two_bytes
|
||||
{
|
||||
uint16_t uint;
|
||||
uint8_t bytes[2];
|
||||
};
|
||||
|
||||
static const int PACKET_SIZE = 1206;
|
||||
static const int BLOCKS_PER_PACKET = 12;
|
||||
static const int PACKET_STATUS_SIZE = 4;
|
||||
static const int SCANS_PER_PACKET = (SCANS_PER_BLOCK * BLOCKS_PER_PACKET);
|
||||
|
||||
/** \brief Raw Velodyne packet.
|
||||
*
|
||||
* revolution is described in the device manual as incrementing
|
||||
* (mod 65536) for each physical turn of the device. Our device
|
||||
* seems to alternate between two different values every third
|
||||
* packet. One value increases, the other decreases.
|
||||
*
|
||||
* \todo figure out if revolution is only present for one of the
|
||||
* two types of status fields
|
||||
*
|
||||
* status has either a temperature encoding or the microcode level
|
||||
*/
|
||||
typedef struct raw_packet
|
||||
{
|
||||
raw_block_t blocks[BLOCKS_PER_PACKET];
|
||||
uint16_t revolution;
|
||||
uint8_t status[PACKET_STATUS_SIZE];
|
||||
}
|
||||
raw_packet_t;
|
||||
|
||||
/** \brief Velodyne data conversion class */
|
||||
class RawData
|
||||
{
|
||||
public:
|
||||
RawData();
|
||||
~RawData()
|
||||
{
|
||||
}
|
||||
|
||||
/** \brief Set up for data processing.
|
||||
*
|
||||
* Perform initializations needed before data processing can
|
||||
* begin:
|
||||
*
|
||||
* - read device-specific angles calibration
|
||||
*
|
||||
* @param private_nh private node handle for ROS parameters
|
||||
* @returns an optional calibration
|
||||
*/
|
||||
boost::optional<velodyne_pointcloud::Calibration> setup(ros::NodeHandle private_nh);
|
||||
|
||||
|
||||
/** \brief Set up for data processing offline.
|
||||
* Performs the same initialization as in setup, in the abscence of a ros::NodeHandle.
|
||||
* this method is useful if unpacking data directly from bag files, without passing
|
||||
* through a communication overhead.
|
||||
*
|
||||
* @param calibration_file path to the calibration file
|
||||
* @param max_range_ cutoff for maximum range
|
||||
* @param min_range_ cutoff for minimum range
|
||||
* @returns 0 if successful;
|
||||
* errno value for failure
|
||||
*/
|
||||
int setupOffline(std::string calibration_file, double max_range_, double min_range_);
|
||||
|
||||
void unpack(const velodyne_msgs::VelodynePacket& pkt, DataContainerBase& data,
|
||||
const ros::Time& scan_start_time);
|
||||
|
||||
void setParameters(double min_range, double max_range, double view_direction, double view_width);
|
||||
|
||||
int scansPerPacket() const;
|
||||
|
||||
private:
|
||||
/** configuration parameters */
|
||||
typedef struct
|
||||
{
|
||||
std::string model;
|
||||
std::string calibrationFile; ///< calibration file name
|
||||
double max_range; ///< maximum range to publish
|
||||
double min_range; ///< minimum range to publish
|
||||
int min_angle; ///< minimum angle to publish
|
||||
int max_angle; ///< maximum angle to publish
|
||||
|
||||
double tmp_min_angle;
|
||||
double tmp_max_angle;
|
||||
}
|
||||
Config;
|
||||
Config config_;
|
||||
|
||||
/**
|
||||
* Calibration file
|
||||
*/
|
||||
velodyne_pointcloud::Calibration calibration_;
|
||||
float sin_rot_table_[ROTATION_MAX_UNITS];
|
||||
float cos_rot_table_[ROTATION_MAX_UNITS];
|
||||
|
||||
// timing offset lookup table
|
||||
std::vector< std::vector<float> > timing_offsets;
|
||||
|
||||
/** \brief setup per-point timing offsets
|
||||
*
|
||||
* Runs during initialization and determines the firing time for each point in the scan
|
||||
*
|
||||
* NOTE: Does not support all sensors yet (vlp16, vlp32, and hdl32 are currently supported)
|
||||
*/
|
||||
bool buildTimings();
|
||||
|
||||
/** add private function to handle the VLP16 **/
|
||||
void unpack_vlp16(const velodyne_msgs::VelodynePacket& pkt, DataContainerBase& data,
|
||||
const ros::Time& scan_start_time);
|
||||
};
|
||||
|
||||
} // namespace velodyne_rawdata
|
||||
|
||||
#endif // VELODYNE_POINTCLOUD_RAWDATA_H
|
||||
@@ -0,0 +1,112 @@
|
||||
// Copyright (C) 2009, 2010, 2011, 2012, 2019 Austin Robot Technology, Jack O'Quin, Jesse Vera, Joshua Whitley,
|
||||
// Sebastian Pütz All rights reserved.
|
||||
//
|
||||
// Software License Agreement (BSD License 2.0)
|
||||
//
|
||||
// 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 {copyright_holder} 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.
|
||||
|
||||
/** @file
|
||||
|
||||
This class transforms raw Velodyne 3D LIDAR packets to PointCloud2
|
||||
in the /map frame of reference.
|
||||
|
||||
*/
|
||||
|
||||
#ifndef VELODYNE_POINTCLOUD_TRANSFORM_H
|
||||
#define VELODYNE_POINTCLOUD_TRANSFORM_H
|
||||
|
||||
#include <string>
|
||||
#include <ros/ros.h>
|
||||
#include "tf/message_filter.h"
|
||||
#include "message_filters/subscriber.h"
|
||||
#include <diagnostic_updater/diagnostic_updater.h>
|
||||
#include <diagnostic_updater/publisher.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
|
||||
#include <velodyne_pointcloud/rawdata.h>
|
||||
#include <velodyne_pointcloud/pointcloudXYZIR.h>
|
||||
|
||||
#include <dynamic_reconfigure/server.h>
|
||||
#include <velodyne_pointcloud/TransformNodeConfig.h>
|
||||
|
||||
namespace velodyne_pointcloud
|
||||
{
|
||||
using TransformNodeCfg = velodyne_pointcloud::TransformNodeConfig;
|
||||
|
||||
class Transform
|
||||
{
|
||||
public:
|
||||
Transform(
|
||||
ros::NodeHandle node,
|
||||
ros::NodeHandle private_nh,
|
||||
std::string const & node_name = ros::this_node::getName());
|
||||
~Transform()
|
||||
{
|
||||
}
|
||||
|
||||
private:
|
||||
void processScan(const velodyne_msgs::VelodyneScan::ConstPtr& scanMsg);
|
||||
|
||||
// Pointer to dynamic reconfigure service srv_
|
||||
boost::shared_ptr<dynamic_reconfigure::Server<velodyne_pointcloud::TransformNodeConfig>> srv_;
|
||||
void reconfigure_callback(velodyne_pointcloud::TransformNodeConfig& config, uint32_t level);
|
||||
|
||||
const std::string tf_prefix_;
|
||||
boost::shared_ptr<velodyne_rawdata::RawData> data_;
|
||||
message_filters::Subscriber<velodyne_msgs::VelodyneScan> velodyne_scan_;
|
||||
ros::Publisher output_;
|
||||
boost::shared_ptr<tf::MessageFilter<velodyne_msgs::VelodyneScan>> tf_filter_ptr_;
|
||||
boost::shared_ptr<tf::TransformListener> tf_ptr_;
|
||||
|
||||
/// configuration parameters
|
||||
typedef struct
|
||||
{
|
||||
std::string target_frame; ///< target frame
|
||||
std::string fixed_frame; ///< fixed frame
|
||||
bool organize_cloud; ///< enable/disable organized cloud structure
|
||||
double max_range; ///< maximum range to publish
|
||||
double min_range; ///< minimum range to publish
|
||||
uint16_t num_lasers; ///< number of lasers
|
||||
}
|
||||
Config;
|
||||
Config config_;
|
||||
|
||||
bool first_rcfg_call;
|
||||
|
||||
boost::shared_ptr<velodyne_rawdata::DataContainerBase> container_ptr;
|
||||
|
||||
// diagnostics updater
|
||||
diagnostic_updater::Updater diagnostics_;
|
||||
double diag_min_freq_;
|
||||
double diag_max_freq_;
|
||||
boost::shared_ptr<diagnostic_updater::TopicDiagnostic> diag_topic_;
|
||||
boost::mutex reconfigure_mtx_;
|
||||
};
|
||||
} // namespace velodyne_pointcloud
|
||||
|
||||
#endif // VELODYNE_POINTCLOUD_TRANSFORM_H
|
||||
@@ -0,0 +1,62 @@
|
||||
<!-- -*- mode: XML -*- -->
|
||||
<!-- run velodyne_pointcloud/CloudNodelet in a nodelet manager for an HDL-32E -->
|
||||
|
||||
<launch>
|
||||
|
||||
<!-- declare arguments with default values -->
|
||||
<arg name="calibration" default="$(find velodyne_pointcloud)/params/32db.yaml"/>
|
||||
<arg name="device_ip" default="" />
|
||||
<arg name="frame_id" default="velodyne" />
|
||||
<arg name="manager" default="$(arg frame_id)_nodelet_manager" />
|
||||
<arg name="max_range" default="130.0" />
|
||||
<arg name="min_range" default="0.4" />
|
||||
<arg name="pcap" default="" />
|
||||
<arg name="port" default="2368" />
|
||||
<arg name="read_fast" default="false" />
|
||||
<arg name="read_once" default="false" />
|
||||
<arg name="repeat_delay" default="0.0" />
|
||||
<arg name="rpm" default="600.0" />
|
||||
<arg name="gps_time" default="false" />
|
||||
<arg name="cut_angle" default="-0.01" />
|
||||
<arg name="timestamp_first_packet" default="false" />
|
||||
<arg name="laserscan_ring" default="-1" />
|
||||
<arg name="laserscan_resolution" default="0.007" />
|
||||
<arg name="organize_cloud" default="false" />
|
||||
|
||||
<!-- start nodelet manager and driver nodelets -->
|
||||
<include file="$(find velodyne_driver)/launch/nodelet_manager.launch">
|
||||
<arg name="device_ip" value="$(arg device_ip)"/>
|
||||
<arg name="frame_id" value="$(arg frame_id)"/>
|
||||
<arg name="manager" value="$(arg manager)" />
|
||||
<arg name="model" value="32E"/>
|
||||
<arg name="pcap" value="$(arg pcap)"/>
|
||||
<arg name="port" value="$(arg port)"/>
|
||||
<arg name="read_fast" value="$(arg read_fast)"/>
|
||||
<arg name="read_once" value="$(arg read_once)"/>
|
||||
<arg name="repeat_delay" value="$(arg repeat_delay)"/>
|
||||
<arg name="rpm" value="$(arg rpm)"/>
|
||||
<arg name="gps_time" value="$(arg gps_time)"/>
|
||||
<arg name="cut_angle" value="$(arg cut_angle)"/>
|
||||
<arg name="timestamp_first_packet" value="$(arg timestamp_first_packet)"/>
|
||||
</include>
|
||||
|
||||
<!-- start cloud nodelet -->
|
||||
<include file="$(find velodyne_pointcloud)/launch/cloud_nodelet.launch">
|
||||
<arg name="model" value="32E"/>
|
||||
<arg name="calibration" value="$(arg calibration)"/>
|
||||
<arg name="manager" value="$(arg manager)" />
|
||||
<arg name="fixed_frame" value="$(arg frame_id)" />
|
||||
<arg name="target_frame" value="$(arg frame_id)" />
|
||||
<arg name="max_range" value="$(arg max_range)"/>
|
||||
<arg name="min_range" value="$(arg min_range)"/>
|
||||
<arg name="organize_cloud" value="$(arg organize_cloud)"/>
|
||||
</include>
|
||||
|
||||
<!-- start laserscan nodelet -->
|
||||
<include file="$(find velodyne_pointcloud)/launch/laserscan_nodelet.launch">
|
||||
<arg name="manager" value="$(arg manager)" />
|
||||
<arg name="ring" value="$(arg laserscan_ring)"/>
|
||||
<arg name="resolution" value="$(arg laserscan_resolution)"/>
|
||||
</include>
|
||||
|
||||
</launch>
|
||||
@@ -0,0 +1,61 @@
|
||||
<!-- -*- mode: XML -*- -->
|
||||
<!-- run velodyne_pointcloud/CloudNodelet in a nodelet manager for an HDL-32E -->
|
||||
|
||||
<launch>
|
||||
|
||||
<!-- declare arguments with default values -->
|
||||
<arg name="calibration" default="$(find velodyne_pointcloud)/params/64e_s3-xiesc.yaml"/>
|
||||
<arg name="device_ip" default="" />
|
||||
<arg name="frame_id" default="velodyne" />
|
||||
<arg name="manager" default="$(arg frame_id)_nodelet_manager" />
|
||||
<arg name="max_range" default="130.0" />
|
||||
<arg name="min_range" default="0.4" />
|
||||
<arg name="pcap" default="" />
|
||||
<arg name="port" default="2368" />
|
||||
<arg name="read_fast" default="false" />
|
||||
<arg name="read_once" default="false" />
|
||||
<arg name="repeat_delay" default="0.0" />
|
||||
<arg name="rpm" default="600.0" />
|
||||
<arg name="gps_time" default="false" />
|
||||
<arg name="cut_angle" default="-0.01" />
|
||||
<arg name="timestamp_first_packet" default="false" />
|
||||
<arg name="laserscan_ring" default="-1" />
|
||||
<arg name="laserscan_resolution" default="0.007" />
|
||||
<arg name="model" value="64E_S3"/>
|
||||
|
||||
<!-- start nodelet manager and driver nodelets -->
|
||||
<include file="$(find velodyne_driver)/launch/nodelet_manager.launch">
|
||||
<arg name="device_ip" value="$(arg device_ip)"/>
|
||||
<arg name="frame_id" value="$(arg frame_id)"/>
|
||||
<arg name="manager" value="$(arg manager)" />
|
||||
<arg name="model" value="$(arg model)"/>
|
||||
<arg name="pcap" value="$(arg pcap)"/>
|
||||
<arg name="port" value="$(arg port)"/>
|
||||
<arg name="read_fast" value="$(arg read_fast)"/>
|
||||
<arg name="read_once" value="$(arg read_once)"/>
|
||||
<arg name="repeat_delay" value="$(arg repeat_delay)"/>
|
||||
<arg name="rpm" value="$(arg rpm)"/>
|
||||
<arg name="gps_time" value="$(arg gps_time)"/>
|
||||
<arg name="cut_angle" value="$(arg cut_angle)"/>
|
||||
<arg name="timestamp_first_packet" value="$(arg timestamp_first_packet)"/>
|
||||
</include>
|
||||
|
||||
<!-- start cloud nodelet -->
|
||||
<include file="$(find velodyne_pointcloud)/launch/cloud_nodelet.launch">
|
||||
<arg name="model" value="$(arg model)"/>
|
||||
<arg name="calibration" value="$(arg calibration)"/>
|
||||
<arg name="manager" value="$(arg manager)" />
|
||||
<arg name="fixed_frame" value="$(arg frame_id)" />
|
||||
<arg name="target_frame" value="$(arg frame_id)" />
|
||||
<arg name="max_range" value="$(arg max_range)"/>
|
||||
<arg name="min_range" value="$(arg min_range)"/>
|
||||
</include>
|
||||
|
||||
<!-- start laserscan nodelet -->
|
||||
<include file="$(find velodyne_pointcloud)/launch/laserscan_nodelet.launch">
|
||||
<arg name="manager" value="$(arg manager)" />
|
||||
<arg name="ring" value="$(arg laserscan_ring)"/>
|
||||
<arg name="resolution" value="$(arg laserscan_resolution)"/>
|
||||
</include>
|
||||
|
||||
</launch>
|
||||
@@ -0,0 +1,62 @@
|
||||
<!-- -*- mode: XML -*- -->
|
||||
<!-- run velodyne_pointcloud/CloudNodelet in a nodelet manager for an VLP-32C -->
|
||||
|
||||
<launch>
|
||||
|
||||
<!-- declare arguments with default values -->
|
||||
<arg name="calibration" default="$(find velodyne_pointcloud)/params/VeloView-VLP-32C.yaml"/>
|
||||
<arg name="device_ip" default="" />
|
||||
<arg name="frame_id" default="velodyne" />
|
||||
<arg name="manager" default="$(arg frame_id)_nodelet_manager" />
|
||||
<arg name="max_range" default="200.0" />
|
||||
<arg name="min_range" default="0.4" />
|
||||
<arg name="pcap" default="" />
|
||||
<arg name="port" default="2368" />
|
||||
<arg name="read_fast" default="false" />
|
||||
<arg name="read_once" default="false" />
|
||||
<arg name="repeat_delay" default="0.0" />
|
||||
<arg name="rpm" default="600.0" />
|
||||
<arg name="gps_time" default="false" />
|
||||
<arg name="cut_angle" default="-0.01" />
|
||||
<arg name="timestamp_first_packet" default="false" />
|
||||
<arg name="laserscan_ring" default="-1" />
|
||||
<arg name="laserscan_resolution" default="0.007" />
|
||||
<arg name="organize_cloud" default="false" />
|
||||
|
||||
<!-- start nodelet manager and driver nodelets -->
|
||||
<include file="$(find velodyne_driver)/launch/nodelet_manager.launch">
|
||||
<arg name="device_ip" value="$(arg device_ip)"/>
|
||||
<arg name="frame_id" value="$(arg frame_id)"/>
|
||||
<arg name="manager" value="$(arg manager)" />
|
||||
<arg name="model" value="32C"/>
|
||||
<arg name="pcap" value="$(arg pcap)"/>
|
||||
<arg name="port" value="$(arg port)"/>
|
||||
<arg name="read_fast" value="$(arg read_fast)"/>
|
||||
<arg name="read_once" value="$(arg read_once)"/>
|
||||
<arg name="repeat_delay" value="$(arg repeat_delay)"/>
|
||||
<arg name="rpm" value="$(arg rpm)"/>
|
||||
<arg name="gps_time" value="$(arg gps_time)"/>
|
||||
<arg name="cut_angle" value="$(arg cut_angle)"/>
|
||||
<arg name="timestamp_first_packet" value="$(arg timestamp_first_packet)"/>
|
||||
</include>
|
||||
|
||||
<!-- start cloud nodelet -->
|
||||
<include file="$(find velodyne_pointcloud)/launch/cloud_nodelet.launch">
|
||||
<arg name="model" value="32C"/>
|
||||
<arg name="calibration" value="$(arg calibration)"/>
|
||||
<arg name="manager" value="$(arg manager)" />
|
||||
<arg name="fixed_frame" value="$(arg frame_id)" />
|
||||
<arg name="target_frame" value="$(arg frame_id)" />
|
||||
<arg name="max_range" value="$(arg max_range)"/>
|
||||
<arg name="min_range" value="$(arg min_range)"/>
|
||||
<arg name="organize_cloud" value="$(arg organize_cloud)"/>
|
||||
</include>
|
||||
|
||||
<!-- start laserscan nodelet -->
|
||||
<include file="$(find velodyne_pointcloud)/launch/laserscan_nodelet.launch">
|
||||
<arg name="manager" value="$(arg manager)" />
|
||||
<arg name="ring" value="$(arg laserscan_ring)"/>
|
||||
<arg name="resolution" value="$(arg laserscan_resolution)"/>
|
||||
</include>
|
||||
|
||||
</launch>
|
||||
@@ -0,0 +1,62 @@
|
||||
<!-- -*- mode: XML -*- -->
|
||||
<!-- run velodyne_pointcloud/CloudNodelet in a nodelet manager for a VLP-16 -->
|
||||
|
||||
<launch>
|
||||
|
||||
<!-- declare arguments with default values -->
|
||||
<arg name="calibration" default="$(find velodyne_pointcloud)/params/VLP16db.yaml"/>
|
||||
<arg name="device_ip" default="" />
|
||||
<arg name="frame_id" default="velodyne" />
|
||||
<arg name="manager" default="$(arg frame_id)_nodelet_manager" />
|
||||
<arg name="max_range" default="130.0" />
|
||||
<arg name="min_range" default="0.4" />
|
||||
<arg name="pcap" default="" />
|
||||
<arg name="port" default="2368" />
|
||||
<arg name="read_fast" default="false" />
|
||||
<arg name="read_once" default="false" />
|
||||
<arg name="repeat_delay" default="0.0" />
|
||||
<arg name="rpm" default="600.0" />
|
||||
<arg name="gps_time" default="false" />
|
||||
<arg name="cut_angle" default="-0.01" />
|
||||
<arg name="timestamp_first_packet" default="false" />
|
||||
<arg name="laserscan_ring" default="-1" />
|
||||
<arg name="laserscan_resolution" default="0.007" />
|
||||
<arg name="organize_cloud" default="false" />
|
||||
|
||||
<!-- start nodelet manager and driver nodelets -->
|
||||
<include file="$(find velodyne_driver)/launch/nodelet_manager.launch">
|
||||
<arg name="device_ip" value="$(arg device_ip)"/>
|
||||
<arg name="frame_id" value="$(arg frame_id)"/>
|
||||
<arg name="manager" value="$(arg manager)" />
|
||||
<arg name="model" value="VLP16"/>
|
||||
<arg name="pcap" value="$(arg pcap)"/>
|
||||
<arg name="port" value="$(arg port)"/>
|
||||
<arg name="read_fast" value="$(arg read_fast)"/>
|
||||
<arg name="read_once" value="$(arg read_once)"/>
|
||||
<arg name="repeat_delay" value="$(arg repeat_delay)"/>
|
||||
<arg name="rpm" value="$(arg rpm)"/>
|
||||
<arg name="gps_time" value="$(arg gps_time)"/>
|
||||
<arg name="cut_angle" value="$(arg cut_angle)"/>
|
||||
<arg name="timestamp_first_packet" value="$(arg timestamp_first_packet)"/>
|
||||
</include>
|
||||
|
||||
<!-- start cloud nodelet -->
|
||||
<include file="$(find velodyne_pointcloud)/launch/cloud_nodelet.launch">
|
||||
<arg name="model" value="VLP16"/>
|
||||
<arg name="calibration" value="$(arg calibration)"/>
|
||||
<arg name="manager" value="$(arg manager)" />
|
||||
<arg name="fixed_frame" value="$(arg frame_id)" />
|
||||
<arg name="target_frame" value="$(arg frame_id)" />
|
||||
<arg name="max_range" value="$(arg max_range)"/>
|
||||
<arg name="min_range" value="$(arg min_range)"/>
|
||||
<arg name="organize_cloud" value="$(arg organize_cloud)"/>
|
||||
</include>
|
||||
|
||||
<!-- start laserscan nodelet -->
|
||||
<!-- <include file="$(find velodyne_pointcloud)/launch/laserscan_nodelet.launch">
|
||||
<arg name="manager" value="$(arg manager)" />
|
||||
<arg name="ring" value="$(arg laserscan_ring)"/>
|
||||
<arg name="resolution" value="$(arg laserscan_resolution)"/>
|
||||
</include> -->
|
||||
|
||||
</launch>
|
||||
@@ -0,0 +1,24 @@
|
||||
<!-- -*- mode: XML -*- -->
|
||||
<!-- run velodyne_pointcloud/CloudNodelet in a nodelet manager -->
|
||||
|
||||
<launch>
|
||||
<arg name="model" default="" />
|
||||
<arg name="calibration" default="" />
|
||||
<arg name="manager" default="velodyne_nodelet_manager" />
|
||||
<arg name="fixed_frame" default="velodyne" />
|
||||
<arg name="target_frame" default="velodyne" />
|
||||
<arg name="max_range" default="130.0" />
|
||||
<arg name="min_range" default="0.9" />
|
||||
<arg name="organize_cloud" default="false" />
|
||||
|
||||
<node pkg="nodelet" type="nodelet" name="$(arg manager)_cloud"
|
||||
args="load velodyne_pointcloud/CloudNodelet $(arg manager)">
|
||||
<param name="model" value="$(arg model)"/>
|
||||
<param name="calibration" value="$(arg calibration)"/>
|
||||
<param name="fixed_frame" value="$(arg fixed_frame)"/>
|
||||
<param name="target_frame" value="$(arg target_frame)"/>
|
||||
<param name="max_range" value="$(arg max_range)"/>
|
||||
<param name="min_range" value="$(arg min_range)"/>
|
||||
<param name="organize_cloud" value="$(arg organize_cloud)"/>
|
||||
</node>
|
||||
</launch>
|
||||
@@ -0,0 +1,14 @@
|
||||
<!-- -*- mode: XML -*- -->
|
||||
<!-- run velodyne_laserscan/LaserScanNodelet in a nodelet manager -->
|
||||
|
||||
<launch>
|
||||
<arg name="manager" default="velodyne_nodelet_manager" />
|
||||
<arg name="ring" default="-1" />
|
||||
<arg name="resolution" default="0.007" />
|
||||
|
||||
<node pkg="nodelet" type="nodelet" name="$(arg manager)_laserscan"
|
||||
args="load velodyne_laserscan/LaserScanNodelet $(arg manager)">
|
||||
<param name="ring" value="$(arg ring)"/>
|
||||
<param name="resolution" value="$(arg resolution)"/>
|
||||
</node>
|
||||
</launch>
|
||||
@@ -0,0 +1,21 @@
|
||||
<!-- -*- mode: XML -*- -->
|
||||
<!-- run velodyne_pointcloud/TransformNodelet in a nodelet manager -->
|
||||
|
||||
<launch>
|
||||
<arg name="model" default="" />
|
||||
<arg name="calibration" default="" />
|
||||
<arg name="frame_id" default="map" />
|
||||
<arg name="manager" default="velodyne_nodelet_manager" />
|
||||
<arg name="max_range" default="130.0" />
|
||||
<arg name="min_range" default="0.9" />
|
||||
<arg name="organize_cloud" default="false" />
|
||||
<node pkg="nodelet" type="nodelet" name="$(arg manager)_transform"
|
||||
args="load velodyne_pointcloud/TransformNodelet $(arg manager)" >
|
||||
<param name="model" value="$(arg model)"/>
|
||||
<param name="calibration" value="$(arg calibration)"/>
|
||||
<param name="frame_id" value="$(arg frame_id)"/>
|
||||
<param name="max_range" value="$(arg max_range)"/>
|
||||
<param name="min_range" value="$(arg min_range)"/>
|
||||
<param name="organize_cloud" value="$(arg organize_cloud)"/>
|
||||
</node>
|
||||
</launch>
|
||||
@@ -0,0 +1,8 @@
|
||||
/**
|
||||
\mainpage
|
||||
\htmlinclude manifest.html
|
||||
|
||||
Nodes and nodelets for converting raw Velodyne 3D LIDAR data to point
|
||||
clouds.
|
||||
|
||||
*/
|
||||
@@ -0,0 +1,35 @@
|
||||
<class_libraries>
|
||||
|
||||
<library path="lib/libcloud_nodelet">
|
||||
<class name="velodyne_pointcloud/CloudNodelet"
|
||||
type="velodyne_pointcloud::CloudNodelet"
|
||||
base_class_type="nodelet::Nodelet">
|
||||
<description>
|
||||
Aggregates points from multiple packets, publishing PointCloud2.
|
||||
</description>
|
||||
</class>
|
||||
</library>
|
||||
|
||||
<library path="lib/libringcolors_nodelet">
|
||||
<class name="velodyne_pointcloud/RingColorsNodelet"
|
||||
type="velodyne_pointcloud::RingColorsNodelet"
|
||||
base_class_type="nodelet::Nodelet">
|
||||
<description>
|
||||
Converts a Velodyne PointCloud2 to PointXYZRGB, assigning colors
|
||||
for visualization of the laser rings.
|
||||
</description>
|
||||
</class>
|
||||
</library>
|
||||
|
||||
<library path="lib/libtransform_nodelet">
|
||||
<class name="velodyne_pointcloud/TransformNodelet"
|
||||
type="velodyne_pointcloud::TransformNodelet"
|
||||
base_class_type="nodelet::Nodelet">
|
||||
<description>
|
||||
Transforms packets into /map frame, publishing multiple packets
|
||||
as PointCloud2.
|
||||
</description>
|
||||
</class>
|
||||
</library>
|
||||
|
||||
</class_libraries>
|
||||
@@ -0,0 +1,45 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>velodyne_pointcloud</name>
|
||||
<version>1.5.2</version>
|
||||
<description>
|
||||
Point cloud conversions for Velodyne 3D LIDARs.
|
||||
</description>
|
||||
<maintainer email="josh.whitley@autoware.org">Josh Whitley</maintainer>
|
||||
<author>Jack O'Quin</author>
|
||||
<author>Piyush Khandelwal</author>
|
||||
<author>Jesse Vera</author>
|
||||
<author>Sebastian Pütz</author>
|
||||
<license>BSD</license>
|
||||
<url type="website">http://ros.org/wiki/velodyne_pointcloud</url>
|
||||
<url type="repository">https://github.com/ros-drivers/velodyne</url>
|
||||
<url type="bugtracker">https://github.com/ros-drivers/velodyne/issues</url>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<build_depend>roslint</build_depend>
|
||||
|
||||
<depend>angles</depend>
|
||||
<depend>nodelet</depend>
|
||||
<depend>roscpp</depend>
|
||||
<depend>roslib</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>tf</depend>
|
||||
<depend>velodyne_driver</depend>
|
||||
<depend>velodyne_msgs</depend>
|
||||
<depend>yaml-cpp</depend>
|
||||
<depend>dynamic_reconfigure</depend>
|
||||
<depend>diagnostic_updater</depend>
|
||||
<depend>eigen</depend>
|
||||
|
||||
<exec_depend>velodyne_laserscan</exec_depend>
|
||||
|
||||
<test_depend>rosunit</test_depend>
|
||||
<test_depend>roslaunch</test_depend>
|
||||
<test_depend>rostest</test_depend>
|
||||
<test_depend>tf2_ros</test_depend>
|
||||
|
||||
<export>
|
||||
<nodelet plugin="${prefix}/nodelets.xml"/>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,100 @@
|
||||
# standard Velodyne HDL-32E calibration parameters
|
||||
lasers:
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 0, rot_correction: 0.0,
|
||||
vert_correction: -0.5352924815866609, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 1, rot_correction: 0.0,
|
||||
vert_correction: -0.1628392174657417, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 2, rot_correction: 0.0,
|
||||
vert_correction: -0.5119050696099369, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 3, rot_correction: 0.0,
|
||||
vert_correction: -0.13962634015954636, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 4, rot_correction: 0.0,
|
||||
vert_correction: -0.4886921905584123, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 5, rot_correction: 0.0,
|
||||
vert_correction: -0.11641346285335104, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 6, rot_correction: 0.0,
|
||||
vert_correction: -0.4654793115068877, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 7, rot_correction: 0.0,
|
||||
vert_correction: -0.09302604738596851, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 8, rot_correction: 0.0,
|
||||
vert_correction: -0.44209189953016365, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 9, rot_correction: 0.0,
|
||||
vert_correction: -0.06981317007977318, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 10, rot_correction: 0.0,
|
||||
vert_correction: -0.4188790204786391, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 11, rot_correction: 0.0,
|
||||
vert_correction: -0.046600292773577856, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 12, rot_correction: 0.0,
|
||||
vert_correction: -0.39566614142711454, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 13, rot_correction: 0.0,
|
||||
vert_correction: -0.023212879051524585, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 14, rot_correction: 0.0,
|
||||
vert_correction: -0.3722787294503905, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 15, rot_correction: 0.0,
|
||||
vert_correction: 0.0, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 16, rot_correction: 0.0,
|
||||
vert_correction: -0.3490658503988659, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 17, rot_correction: 0.0,
|
||||
vert_correction: 0.023212879051524585, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 18, rot_correction: 0.0,
|
||||
vert_correction: -0.32585297134734137, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 19, rot_correction: 0.0,
|
||||
vert_correction: 0.046600292773577856, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 20, rot_correction: 0.0,
|
||||
vert_correction: -0.30246555937061725, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 21, rot_correction: 0.0,
|
||||
vert_correction: 0.06981317007977318, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 22, rot_correction: 0.0,
|
||||
vert_correction: -0.2792526803190927, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 23, rot_correction: 0.0,
|
||||
vert_correction: 0.09302604738596851, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 24, rot_correction: 0.0,
|
||||
vert_correction: -0.25603980126756815, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 25, rot_correction: 0.0,
|
||||
vert_correction: 0.11641346285335104, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 26, rot_correction: 0.0,
|
||||
vert_correction: -0.23265238929084414, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 27, rot_correction: 0.0,
|
||||
vert_correction: 0.13962634015954636, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 28, rot_correction: 0.0,
|
||||
vert_correction: -0.20943951023931956, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 29, rot_correction: 0.0,
|
||||
vert_correction: 0.1628392174657417, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 30, rot_correction: 0.0,
|
||||
vert_correction: -0.18622663118779495, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 31, rot_correction: 0.0,
|
||||
vert_correction: 0.18622663118779495, vert_offset_correction: 0.0}
|
||||
num_lasers: 32
|
||||
distance_resolution: 0.002
|
||||
@@ -0,0 +1,246 @@
|
||||
lasers:
|
||||
- {dist_correction: 1.5195264000000002, dist_correction_x: 1.5500304, dist_correction_y: 1.5231381,
|
||||
focal_distance: 12.0, focal_slope: 1.4, horiz_offset_correction: 0.025999999, laser_id: 0,
|
||||
max_intensity: 235.0, min_intensity: 30.0, rot_correction: -0.1248942899601548,
|
||||
vert_correction: -0.15304134919741974, vert_offset_correction: 0.19548199}
|
||||
- {dist_correction: 1.5145139, dist_correction_x: 1.5256960000000002, dist_correction_y: 1.5491043,
|
||||
focal_distance: 5.0, focal_slope: 1.0, horiz_offset_correction: -0.025999999, laser_id: 1,
|
||||
min_intensity: 40.0, rot_correction: -0.06924466398252106, vert_correction: -0.1458455539136526,
|
||||
vert_offset_correction: 0.19601112}
|
||||
- {dist_correction: 1.4963768, dist_correction_x: 1.5571011, dist_correction_y: 1.5456782999999998,
|
||||
focal_distance: 5.0, focal_slope: 0.89999998, horiz_offset_correction: 0.025999999,
|
||||
laser_id: 2, min_intensity: 60.0, rot_correction: 0.0824016906676694, vert_correction: 0.04339494149708345,
|
||||
vert_offset_correction: 0.20969539999999998}
|
||||
- {dist_correction: 1.3771207, dist_correction_x: 1.4103835, dist_correction_y: 1.4343457000000002,
|
||||
focal_distance: 9.5, focal_slope: 1.1, horiz_offset_correction: -0.025999999, laser_id: 3,
|
||||
min_intensity: 20.0, rot_correction: 0.137808349360133, vert_correction: 0.05196082373432465,
|
||||
vert_offset_correction: 0.21031273}
|
||||
- {dist_correction: 1.2947885, dist_correction_x: 1.3720455999999999, dist_correction_y: 1.4025244000000001,
|
||||
focal_distance: 10.0, focal_slope: 1.0, horiz_offset_correction: 0.025999999, laser_id: 4,
|
||||
min_intensity: 50.0, rot_correction: -0.011199603626188263, vert_correction: -0.1358262679404349,
|
||||
vert_offset_correction: 0.19674603999999998}
|
||||
- {dist_correction: 1.4395787000000002, dist_correction_x: 1.4801956, dist_correction_y: 1.5074649,
|
||||
focal_distance: 7.0, focal_slope: 1.0, horiz_offset_correction: -0.025999999, laser_id: 5,
|
||||
min_intensity: 30.0, rot_correction: 0.045795414990398894, vert_correction: -0.12678532627942263,
|
||||
vert_offset_correction: 0.19740747}
|
||||
- {dist_correction: 1.3618773, dist_correction_x: 1.4004077000000001, dist_correction_y: 1.3900876,
|
||||
focal_distance: 5.0, focal_slope: 1.1, horiz_offset_correction: 0.025999999, laser_id: 6,
|
||||
min_intensity: 65.0, rot_correction: -0.031646046452444135, vert_correction: -0.1895564226946273,
|
||||
vert_offset_correction: 0.19277746}
|
||||
- {dist_correction: 1.5325716, dist_correction_x: 1.5337143, dist_correction_y: 1.5452948000000002,
|
||||
focal_distance: 6.0, focal_slope: 1.0, horiz_offset_correction: -0.025999999, laser_id: 7,
|
||||
min_intensity: 10.0, rot_correction: 0.023408326257150665, vert_correction: -0.18086723120040343,
|
||||
vert_offset_correction: 0.19342419}
|
||||
- {dist_correction: 1.3743323, dist_correction_x: 1.4474606, dist_correction_y: 1.4533472,
|
||||
focal_distance: 6.0, focal_slope: 1.0, horiz_offset_correction: 0.025999999, laser_id: 8,
|
||||
min_intensity: 30.0, rot_correction: 0.10065885915180103, vert_correction: -0.11973897655818674,
|
||||
vert_offset_correction: 0.19792192}
|
||||
- {dist_correction: 1.4969112000000002, dist_correction_x: 1.4754176, dist_correction_y: 1.4888799000000001,
|
||||
focal_distance: 5.0, focal_slope: 1.0, horiz_offset_correction: -0.025999999, laser_id: 9,
|
||||
min_intensity: 10.0, rot_correction: 0.15996423607269877, vert_correction: -0.10924820210484752,
|
||||
vert_offset_correction: 0.19868624000000001}
|
||||
- {dist_correction: 1.434263, dist_correction_x: 1.4957901000000002, dist_correction_y: 1.5191892999999999,
|
||||
focal_distance: 5.0, focal_slope: 0.89999998, horiz_offset_correction: 0.025999999,
|
||||
laser_id: 10, min_intensity: 35.0, rot_correction: 0.08313316164783874, vert_correction: -0.17135657256846043,
|
||||
vert_offset_correction: 0.19412970999999998}
|
||||
- {dist_correction: 1.5500841, dist_correction_x: 1.542697, dist_correction_y: 1.5716737,
|
||||
focal_distance: 8.0, focal_slope: 1.2, horiz_offset_correction: -0.025999999, laser_id: 11,
|
||||
min_intensity: 30.0, rot_correction: 0.13862572370075624, vert_correction: -0.1630088948849621,
|
||||
vert_offset_correction: 0.19474705}
|
||||
- {dist_correction: 1.3725992, dist_correction_x: 1.4353555, dist_correction_y: 1.3932405,
|
||||
focal_distance: 11.0, focal_slope: 1.1, horiz_offset_correction: 0.025999999, laser_id: 12,
|
||||
min_intensity: 50.0, rot_correction: -0.1255805045528199, vert_correction: -0.04526314018308893,
|
||||
vert_offset_correction: 0.20331627000000002}
|
||||
- {dist_correction: 1.3111591000000002, dist_correction_x: 1.3470857, dist_correction_y: 1.3652054000000002,
|
||||
focal_distance: 6.0, focal_slope: 0.69999999, horiz_offset_correction: -0.025999999,
|
||||
laser_id: 13, min_intensity: 30.0, rot_correction: -0.06716508498014805, vert_correction: -0.03587557613263058,
|
||||
vert_offset_correction: 0.20399239000000002}
|
||||
- {dist_correction: 1.5360803, dist_correction_x: 1.5528035, dist_correction_y: 1.5666801000000001,
|
||||
focal_distance: 8.0, focal_slope: 1.1, horiz_offset_correction: 0.025999999, laser_id: 14,
|
||||
min_intensity: 20.0, rot_correction: -0.14578889529014513, vert_correction: -0.09893566067472198,
|
||||
vert_offset_correction: 0.19943586}
|
||||
- {dist_correction: 1.4755448999999998, dist_correction_x: 1.5032428, dist_correction_y: 1.5326752,
|
||||
focal_distance: 5.0, focal_slope: 0.89999998, horiz_offset_correction: -0.025999999,
|
||||
laser_id: 15, min_intensity: 30.0, rot_correction: -0.08892898296002069, vert_correction: -0.09062974348423608,
|
||||
vert_offset_correction: 0.20003851}
|
||||
- {dist_correction: 1.4410587000000001, dist_correction_x: 1.4870845, dist_correction_y: 1.4627965,
|
||||
focal_distance: 18.0, focal_slope: 0.5, horiz_offset_correction: 0.025999999, laser_id: 16,
|
||||
min_intensity: 40.0, rot_correction: -0.013006177528832626, vert_correction: -0.02770725895504266,
|
||||
vert_offset_correction: 0.20458033}
|
||||
- {dist_correction: 1.4434521, dist_correction_x: 1.4897508, dist_correction_y: 1.4791382,
|
||||
focal_distance: 11.0, focal_slope: 0.80000001, horiz_offset_correction: -0.025999999,
|
||||
laser_id: 17, min_intensity: 55.0, rot_correction: 0.04596628970548614, vert_correction: -0.02014825541794774,
|
||||
vert_offset_correction: 0.20512417}
|
||||
- {dist_correction: 1.3243448000000002, dist_correction_x: 1.3816666000000002, dist_correction_y: 1.3647719,
|
||||
focal_distance: 12.0, focal_slope: 1.0, horiz_offset_correction: 0.025999999, laser_id: 18,
|
||||
min_intensity: 10.0, rot_correction: -0.03237303019110658, vert_correction: -0.08210824496987311,
|
||||
vert_offset_correction: 0.20065581999999998}
|
||||
- {dist_correction: 1.4565853999999998, dist_correction_x: 1.4340645, dist_correction_y: 1.4550516,
|
||||
focal_distance: 20.0, focal_slope: 0.69999999, horiz_offset_correction: -0.025999999,
|
||||
laser_id: 19, min_intensity: 30.0, rot_correction: 0.02431275238487562, vert_correction: -0.0727614640280461,
|
||||
vert_offset_correction: 0.20133196000000003}
|
||||
- {dist_correction: 1.3313776, dist_correction_x: 1.3819601000000001, dist_correction_y: 1.3847791,
|
||||
focal_distance: 5.0, focal_slope: 0.89999998, horiz_offset_correction: 0.025999999,
|
||||
laser_id: 20, min_intensity: 45.0, rot_correction: 0.10029393174916004, vert_correction: -0.009929893699589059,
|
||||
vert_offset_correction: 0.20585909000000002}
|
||||
- {dist_correction: 1.3787535, dist_correction_x: 1.3978789, dist_correction_y: 1.4257806,
|
||||
focal_distance: 10.0, focal_slope: 1.1, horiz_offset_correction: -0.025999999, laser_id: 21,
|
||||
min_intensity: 35.0, rot_correction: 0.15867894419560363, vert_correction: -0.0037977317325845416,
|
||||
vert_offset_correction: 0.20630005}
|
||||
- {dist_correction: 1.3419412, dist_correction_x: 1.4059189, dist_correction_y: 1.4113303,
|
||||
focal_distance: 11.0, focal_slope: 1.0, horiz_offset_correction: 0.025999999, laser_id: 22,
|
||||
min_intensity: 40.0, rot_correction: 0.08044522544540282, vert_correction: -0.06482699129962814,
|
||||
vert_offset_correction: 0.20190518999999998}
|
||||
- {dist_correction: 1.4338304, dist_correction_x: 1.4212059, dist_correction_y: 1.4900565000000001,
|
||||
focal_distance: 11.0, focal_slope: 1.1, horiz_offset_correction: -0.025999999, laser_id: 23,
|
||||
min_intensity: 55.0, rot_correction: 0.13867708175932542, vert_correction: -0.05566171063737453,
|
||||
vert_offset_correction: 0.20256662}
|
||||
- {dist_correction: 1.4930911, dist_correction_x: 1.5572188, dist_correction_y: 1.5182672,
|
||||
focal_distance: 5.0, focal_slope: 1.0, horiz_offset_correction: 0.025999999, laser_id: 24,
|
||||
min_intensity: 25.0, rot_correction: -0.12505298227706313, vert_correction: 0.06113007152996804,
|
||||
vert_offset_correction: 0.21097416}
|
||||
- {dist_correction: 1.4367653000000002, dist_correction_x: 1.4614236, dist_correction_y: 1.4587589,
|
||||
focal_distance: 3.0, focal_slope: 1.0, horiz_offset_correction: -0.025999999, laser_id: 25,
|
||||
min_intensity: 50.0, rot_correction: -0.06814826559971075, vert_correction: 0.06988221181432357,
|
||||
vert_offset_correction: 0.21160620000000002}
|
||||
- {dist_correction: 1.5279892000000002, dist_correction_x: 1.5686107999999999, dist_correction_y: 1.55737,
|
||||
focal_distance: 7.5, focal_slope: 1.0, horiz_offset_correction: 0.025999999, laser_id: 26,
|
||||
min_intensity: 30.0, rot_correction: -0.14531660046790917, vert_correction: 0.00887576771486082,
|
||||
vert_offset_correction: 0.20721134}
|
||||
- {dist_correction: 1.5077638, dist_correction_x: 1.5453738000000001, dist_correction_y: 1.5483368,
|
||||
focal_distance: 4.0, focal_slope: 0.89999998, horiz_offset_correction: -0.025999999,
|
||||
laser_id: 27, min_intensity: 30.0, rot_correction: -0.08904354811745085, vert_correction: 0.017050959569839413,
|
||||
vert_offset_correction: 0.20779928000000003}
|
||||
- {dist_correction: 1.3627797000000001, dist_correction_x: 1.4232985, dist_correction_y: 1.4116524000000001,
|
||||
focal_distance: 4.0, focal_slope: 1.0, horiz_offset_correction: 0.025999999, laser_id: 28,
|
||||
min_intensity: 50.0, rot_correction: -0.012061568860271928, vert_correction: 0.07842048992411524,
|
||||
vert_offset_correction: 0.21222351}
|
||||
- {dist_correction: 1.3749608, dist_correction_x: 1.4045798, dist_correction_y: 1.4006630999999998,
|
||||
focal_distance: 4.0, focal_slope: 1.1, horiz_offset_correction: -0.025999999, laser_id: 29,
|
||||
min_intensity: 40.0, rot_correction: 0.043892943273872005, vert_correction: 0.08674443287511571,
|
||||
vert_offset_correction: 0.21282616000000001}
|
||||
- {dist_correction: 1.2940709000000001, dist_correction_x: 1.392794, dist_correction_y: 1.3225565000000001,
|
||||
focal_distance: 8.0, focal_slope: 0.89999998, horiz_offset_correction: 0.025999999,
|
||||
laser_id: 30, min_intensity: 90.0, rot_correction: -0.033068739374902546, vert_correction: 0.02522388232225749,
|
||||
vert_offset_correction: 0.20838722}
|
||||
- {dist_correction: 1.3907666, dist_correction_x: 1.4365166, dist_correction_y: 1.4501437000000001,
|
||||
focal_distance: 9.0, focal_slope: 1.0, horiz_offset_correction: -0.025999999, laser_id: 31,
|
||||
min_intensity: 5.0, rot_correction: 0.025535290948715324, vert_correction: 0.034414332377654115,
|
||||
vert_offset_correction: 0.20904865}
|
||||
- {dist_correction: 1.3461819, dist_correction_x: 1.3678523000000002, dist_correction_y: 1.3552880999999999,
|
||||
focal_distance: 11.0, focal_slope: 1.5, horiz_offset_correction: 0.025999999, laser_id: 32,
|
||||
min_intensity: 45.0, rot_correction: -0.13309965698710405, vert_correction: -0.39666389380060213,
|
||||
vert_offset_correction: 0.10812234999999999}
|
||||
- {dist_correction: 1.2487466, dist_correction_x: 1.2929747, dist_correction_y: 1.3438803,
|
||||
focal_distance: 5.0, focal_slope: 0.89999998, horiz_offset_correction: -0.025999999,
|
||||
laser_id: 33, min_intensity: 35.0, rot_correction: -0.07241159183553281, vert_correction: -0.39021216204755743,
|
||||
vert_offset_correction: 0.10859233}
|
||||
- {dist_correction: 1.5016498000000003, dist_correction_x: 1.5384890999999998, dist_correction_y: 1.5506186,
|
||||
focal_distance: 4.5, focal_slope: 1.0, horiz_offset_correction: 0.025999999, laser_id: 34,
|
||||
min_intensity: 20.0, rot_correction: 0.08292710807634829, vert_correction: -0.2010672181773803,
|
||||
vert_offset_correction: 0.12148494}
|
||||
- {dist_correction: 1.2495708, dist_correction_x: 1.2891127, dist_correction_y: 1.2943669,
|
||||
focal_distance: 11.0, focal_slope: 1.3, horiz_offset_correction: -0.025999999, laser_id: 35,
|
||||
min_intensity: 25.0, rot_correction: 0.14006506182829093, vert_correction: -0.19103771853737994,
|
||||
vert_offset_correction: 0.12213274}
|
||||
- {dist_correction: 1.2674536, dist_correction_x: 1.3298663, dist_correction_y: 1.3577469,
|
||||
focal_distance: 6.0, focal_slope: 0.89999998, horiz_offset_correction: 0.025999999,
|
||||
laser_id: 36, min_intensity: 35.0, rot_correction: -0.012004346320883118, vert_correction: -0.3819670695815035,
|
||||
vert_offset_correction: 0.10918932}
|
||||
- {dist_correction: 1.2481956, dist_correction_x: 1.28627, dist_correction_y: 1.29235,
|
||||
focal_distance: 14.5, focal_slope: 1.5, horiz_offset_correction: -0.025999999, laser_id: 37,
|
||||
min_intensity: 30.0, rot_correction: 0.04832190474636683, vert_correction: -0.3718940414299584,
|
||||
vert_offset_correction: 0.10991334}
|
||||
- {dist_correction: 1.4516722, dist_correction_x: 1.4842308, dist_correction_y: 1.4868384000000001,
|
||||
focal_distance: 4.0, focal_slope: 1.1, horiz_offset_correction: 0.025999999, laser_id: 38,
|
||||
min_intensity: 55.0, rot_correction: -0.03583078923869515, vert_correction: -0.4336284663746853,
|
||||
vert_offset_correction: 0.1053787}
|
||||
- {dist_correction: 1.3843003999999999, dist_correction_x: 1.4010727, dist_correction_y: 1.4332015999999999,
|
||||
focal_distance: 6.5, focal_slope: 1.3, horiz_offset_correction: -0.025999999, laser_id: 39,
|
||||
min_intensity: 40.0, rot_correction: 0.026883486237381612, vert_correction: -0.4261966798867682,
|
||||
vert_offset_correction: 0.10593759000000001}
|
||||
- {dist_correction: 1.244738, dist_correction_x: 1.3131859000000001, dist_correction_y: 1.354245,
|
||||
focal_distance: 6.5, focal_slope: 1.0, horiz_offset_correction: 0.025999999, laser_id: 40,
|
||||
min_intensity: 25.0, rot_correction: 0.1076316048478218, vert_correction: -0.3640637439139335,
|
||||
vert_offset_correction: 0.11047223}
|
||||
- {dist_correction: 1.3284543, dist_correction_x: 1.3295518000000002, dist_correction_y: 1.3809489,
|
||||
focal_distance: 12.5, focal_slope: 1.7, horiz_offset_correction: -0.025999999, laser_id: 41,
|
||||
min_intensity: 50.0, rot_correction: 0.1679293847080498, vert_correction: -0.3511493721000192,
|
||||
vert_offset_correction: 0.11138678}
|
||||
- {dist_correction: 1.4055542, dist_correction_x: 1.4426663, dist_correction_y: 1.430847,
|
||||
focal_distance: 1.5, focal_slope: 1.1, horiz_offset_correction: 0.025999999, laser_id: 42,
|
||||
min_intensity: 10.0, rot_correction: 0.0861156692854385, vert_correction: -0.4163231777753111,
|
||||
vert_offset_correction: 0.10667431000000001}
|
||||
- {dist_correction: 1.3529747, dist_correction_x: 1.3834917999999998, dist_correction_y: 1.4029074,
|
||||
focal_distance: 6.0, focal_slope: 1.2, horiz_offset_correction: -0.025999999, laser_id: 43,
|
||||
min_intensity: 35.0, rot_correction: 0.15024020221305323, vert_correction: -0.4046365927302973,
|
||||
vert_offset_correction: 0.10753805}
|
||||
- {dist_correction: 1.2407380000000001, dist_correction_x: 1.3171521000000002, dist_correction_y: 1.2836400000000001,
|
||||
focal_distance: 15.0, focal_slope: 1.6, horiz_offset_correction: 0.025999999, laser_id: 44,
|
||||
max_intensity: 220.0, rot_correction: -0.12998527227122358, vert_correction: -0.28892197890806653,
|
||||
vert_offset_correction: 0.11568009}
|
||||
- {dist_correction: 1.5288051999999999, dist_correction_x: 1.5904514, dist_correction_y: 1.6288455,
|
||||
focal_distance: 10.0, focal_slope: 0.89999998, horiz_offset_correction: -0.025999999,
|
||||
laser_id: 45, rot_correction: -0.07009281894983248, vert_correction: -0.28120381879648226,
|
||||
vert_offset_correction: 0.11620087}
|
||||
- {dist_correction: 1.5423979, dist_correction_x: 1.5919412, dist_correction_y: 1.610177,
|
||||
focal_distance: 13.0, focal_slope: 1.7, horiz_offset_correction: 0.025999999, laser_id: 46,
|
||||
rot_correction: -0.1533711640661691, vert_correction: -0.34156398894148127, vert_offset_correction: 0.11205999}
|
||||
- {dist_correction: 1.3086252999999999, dist_correction_x: 1.3737134, dist_correction_y: 1.4177727,
|
||||
focal_distance: 5.5, focal_slope: 0.89999998, horiz_offset_correction: -0.025999999,
|
||||
laser_id: 47, rot_correction: -0.0927890927600715, vert_correction: -0.3348332488542128,
|
||||
vert_offset_correction: 0.11252997}
|
||||
- {dist_correction: 1.3405330000000002, dist_correction_x: 1.4512436, dist_correction_y: 1.4471467999999998,
|
||||
focal_distance: 13.5, focal_slope: 1.0, horiz_offset_correction: 0.025999999, laser_id: 48,
|
||||
rot_correction: -0.012004348415278218, vert_correction: -0.2738300470529015, vert_offset_correction: 0.11669625}
|
||||
- {dist_correction: 1.2994545, dist_correction_x: 1.3971343999999999, dist_correction_y: 1.36849,
|
||||
focal_distance: 14.0, focal_slope: 1.0, horiz_offset_correction: -0.025999999, laser_id: 49,
|
||||
rot_correction: 0.047471358677980184, vert_correction: -0.2649030020882158, vert_offset_correction: 0.11729325}
|
||||
- {dist_correction: 1.4499762999999999, dist_correction_x: 1.5111346, dist_correction_y: 1.542395,
|
||||
focal_distance: 13.0, focal_slope: 1.2, horiz_offset_correction: 0.025999999, laser_id: 50,
|
||||
rot_correction: -0.03378136079915033, vert_correction: -0.32678797913421975, vert_offset_correction: 0.11308886}
|
||||
- {dist_correction: 1.3516956, dist_correction_x: 1.4231836999999998, dist_correction_y: 1.4301146,
|
||||
focal_distance: 13.0, focal_slope: 1.1, horiz_offset_correction: -0.025999999, laser_id: 51,
|
||||
rot_correction: 0.025334358173250228, vert_correction: -0.3179609589889659, vert_offset_correction: 0.11369856}
|
||||
- {dist_correction: 1.2374236, dist_correction_x: 1.308008, dist_correction_y: 1.3549429000000002,
|
||||
focal_distance: 5.0, focal_slope: 0.89999998, horiz_offset_correction: 0.025999999,
|
||||
laser_id: 52, rot_correction: 0.10481975724981128, vert_correction: -0.25478428121685354,
|
||||
vert_offset_correction: 0.11796646000000001}
|
||||
- {dist_correction: 1.3730562000000002, dist_correction_x: 1.403678, dist_correction_y: 1.4347415000000001,
|
||||
focal_distance: 13.0, focal_slope: 1.4, horiz_offset_correction: -0.025999999, laser_id: 53,
|
||||
rot_correction: 0.16311715242247551, vert_correction: -0.24576616497179882, vert_offset_correction: 0.11856346000000001}
|
||||
- {dist_correction: 1.4445888, dist_correction_x: 1.5025624, dist_correction_y: 1.5189749000000001,
|
||||
focal_distance: 10.0, focal_slope: 1.0, horiz_offset_correction: 0.025999999, laser_id: 54,
|
||||
rot_correction: 0.08384971878954978, vert_correction: -0.31000962288931516, vert_offset_correction: 0.11424474999999999}
|
||||
- {dist_correction: 1.3861449, dist_correction_x: 1.4228815000000001, dist_correction_y: 1.4620186000000002,
|
||||
focal_distance: 15.0, focal_slope: 1.6, horiz_offset_correction: -0.025999999, laser_id: 55,
|
||||
rot_correction: 0.14374613400834294, vert_correction: -0.2986601307360315, vert_offset_correction: 0.11501958000000001}
|
||||
- {dist_correction: 1.4635341, dist_correction_x: 1.5232283000000002, dist_correction_y: 1.5063837000000002,
|
||||
focal_distance: 13.0, focal_slope: 1.3, horiz_offset_correction: 0.025999999, laser_id: 56,
|
||||
rot_correction: -0.12741928396895377, vert_correction: -0.18393445537456576, vert_offset_correction: 0.12259002000000001}
|
||||
- {dist_correction: 1.3715826000000002, dist_correction_x: 1.4186972, dist_correction_y: 1.4328435,
|
||||
focal_distance: 3.5, focal_slope: 1.0, horiz_offset_correction: -0.025999999, laser_id: 57,
|
||||
rot_correction: -0.06853129555735342, vert_correction: -0.1744342722087932, vert_offset_correction: 0.12319972}
|
||||
- {dist_correction: 1.4644831999999999, dist_correction_x: 1.5095984000000002, dist_correction_y: 1.5149265,
|
||||
focal_distance: 11.0, focal_slope: 1.4, horiz_offset_correction: 0.025999999, laser_id: 58,
|
||||
rot_correction: -0.1497316045423758, vert_correction: -0.23400086557751998, vert_offset_correction: 0.11933828}
|
||||
- {dist_correction: 1.3074548, dist_correction_x: 1.3521214000000001, dist_correction_y: 1.3830151000000002,
|
||||
focal_distance: 5.0, focal_slope: 0.89999998, horiz_offset_correction: -0.025999999,
|
||||
laser_id: 59, rot_correction: -0.08973572126418226, vert_correction: -0.22644383426247983,
|
||||
vert_offset_correction: 0.11983367}
|
||||
- {dist_correction: 1.4378391, dist_correction_x: 1.4799687000000001, dist_correction_y: 1.4889211,
|
||||
focal_distance: 5.0, focal_slope: 1.1, horiz_offset_correction: 0.025999999, laser_id: 60,
|
||||
rot_correction: -0.011751946229237658, vert_correction: -0.1678843098347878, vert_offset_correction: 0.12361888}
|
||||
- {dist_correction: 1.3585466, dist_correction_x: 1.4135242, dist_correction_y: 1.4170488,
|
||||
focal_distance: 2.0, focal_slope: 0.89999998, horiz_offset_correction: -0.025999999,
|
||||
laser_id: 61, rot_correction: 0.04489272721060892, vert_correction: -0.158331168143522,
|
||||
vert_offset_correction: 0.12422858}
|
||||
- {dist_correction: 1.4478067, dist_correction_x: 1.5442529, dist_correction_y: 1.5512207,
|
||||
focal_distance: 10.0, focal_slope: 1.0, horiz_offset_correction: 0.025999999, laser_id: 62,
|
||||
rot_correction: -0.0340408982402389, vert_correction: -0.21671681763514258, vert_offset_correction: 0.12046877}
|
||||
- {dist_correction: 1.4329738, dist_correction_x: 1.4817114, dist_correction_y: 1.4954124,
|
||||
focal_distance: 9.0, focal_slope: 0.80000001, horiz_offset_correction: -0.025999999,
|
||||
laser_id: 63, rot_correction: 0.024857907722065305, vert_correction: -0.2106649408137298,
|
||||
vert_offset_correction: 0.12086253}
|
||||
num_lasers: 64
|
||||
distance_resolution: 0.002
|
||||
@@ -0,0 +1,227 @@
|
||||
lasers:
|
||||
- {dist_correction: 1.4139490000000001, dist_correction_x: 1.4198446999999998, dist_correction_y: 1.4058145,
|
||||
focal_distance: 10.5, focal_slope: 1.85, horiz_offset_correction: 0.025999999, laser_id: 0,
|
||||
min_intensity: 5, rot_correction: -0.07648247457737148, vert_correction: -0.1261818455292898,
|
||||
vert_offset_correction: 0.21569468}
|
||||
- {dist_correction: 1.5094685, dist_correction_x: 1.5403023, dist_correction_y: 1.5012577999999999,
|
||||
focal_distance: 15.0, focal_slope: 1.2, horiz_offset_correction: -0.025999999, laser_id: 1,
|
||||
min_intensity: 10, rot_correction: -0.03932070772308096, vert_correction: -0.11953745909742221,
|
||||
vert_offset_correction: 0.21520962000000002}
|
||||
- {dist_correction: 1.4579400999999999, dist_correction_x: 1.4846666, dist_correction_y: 1.4228239,
|
||||
focal_distance: 24.0, focal_slope: 1.05, horiz_offset_correction: 0.025999999, laser_id: 2,
|
||||
min_intensity: 30, rot_correction: 0.06005350008746038, vert_correction: 0.00356118725906175,
|
||||
vert_offset_correction: 0.20631704}
|
||||
- {dist_correction: 1.5046124, dist_correction_x: 1.5335535999999999, dist_correction_y: 1.5401996,
|
||||
focal_distance: 14.5, focal_slope: 1.7, horiz_offset_correction: -0.025999999, laser_id: 3,
|
||||
min_intensity: 10, rot_correction: 0.09919490315516696, vert_correction: 0.010306535403103584,
|
||||
vert_offset_correction: 0.20583200000000001}
|
||||
- {dist_correction: 1.4370993, dist_correction_x: 1.4714845, dist_correction_y: 1.4524646,
|
||||
focal_distance: 15.0, focal_slope: 0.40000001, horiz_offset_correction: 0.025999999,
|
||||
laser_id: 4, min_intensity: 30, rot_correction: -0.00095355016485159, vert_correction: -0.1144967440141401,
|
||||
vert_offset_correction: 0.21484217000000003}
|
||||
- {dist_correction: 1.3536626999999999, dist_correction_x: 1.3909094, dist_correction_y: 1.3700326999999999,
|
||||
focal_distance: 17.0, focal_slope: 1.45, horiz_offset_correction: -0.025999999,
|
||||
laser_id: 5, min_intensity: 30, rot_correction: 0.03824387099052699, vert_correction: -0.10803612329921503,
|
||||
vert_offset_correction: 0.21437181}
|
||||
- {dist_correction: 1.4117873, dist_correction_x: 1.4350795, dist_correction_y: 1.3829624999999999,
|
||||
focal_distance: 10.5, focal_slope: 1.0, horiz_offset_correction: 0.025999999, laser_id: 6,
|
||||
min_intensity: 10, rot_correction: -0.015297255529962315, vert_correction: -0.14944538101702426,
|
||||
vert_offset_correction: 0.21739968999999998}
|
||||
- {dist_correction: 1.4269646, dist_correction_x: 1.451161, dist_correction_y: 1.4328421,
|
||||
focal_distance: 14.0, focal_slope: 1.3, horiz_offset_correction: -0.025999999, laser_id: 7,
|
||||
min_intensity: 10, rot_correction: 0.024255809272700053, vert_correction: -0.14344353231329066,
|
||||
vert_offset_correction: 0.21695873}
|
||||
- {dist_correction: 1.3808284000000002, dist_correction_x: 1.421472, dist_correction_y: 1.3838283000000002,
|
||||
focal_distance: 15.0, focal_slope: 0.94999999, horiz_offset_correction: 0.025999999,
|
||||
laser_id: 8, min_intensity: 30, rot_correction: 0.07354442571180776, vert_correction: -0.10278016723125998,
|
||||
vert_offset_correction: 0.21398966000000003}
|
||||
- {dist_correction: 1.3644336000000001, dist_correction_x: 1.3931616, dist_correction_y: 1.3852122,
|
||||
focal_distance: 12.5, focal_slope: 1.95, horiz_offset_correction: -0.025999999,
|
||||
laser_id: 9, min_intensity: 10, rot_correction: 0.11317857886058363, vert_correction: -0.09569590023202451,
|
||||
vert_offset_correction: 0.21347521}
|
||||
- {dist_correction: 1.4102663000000002, dist_correction_x: 1.4163402, dist_correction_y: 1.3592377,
|
||||
focal_distance: 12.5, focal_slope: 1.95, horiz_offset_correction: 0.025999999, laser_id: 10,
|
||||
min_intensity: 10, rot_correction: 0.05954228616823424, vert_correction: -0.1386345201601863,
|
||||
vert_offset_correction: 0.21660597}
|
||||
- {dist_correction: 1.5019737000000002, dist_correction_x: 1.532475, dist_correction_y: 1.5237663000000001,
|
||||
focal_distance: 13.0, focal_slope: 1.9, horiz_offset_correction: -0.025999999, laser_id: 11,
|
||||
min_intensity: 10, rot_correction: 0.09906092120980835, vert_correction: -0.13181073657049397,
|
||||
vert_offset_correction: 0.21610622}
|
||||
- {dist_correction: 1.4399905000000002, dist_correction_x: 1.4887962, dist_correction_y: 1.4455237,
|
||||
focal_distance: 14.5, focal_slope: 1.7, horiz_offset_correction: 0.025999999, laser_id: 12,
|
||||
max_intensity: 235, rot_correction: -0.07574798243226695, vert_correction: -0.054438932963427306,
|
||||
vert_offset_correction: 0.21049141}
|
||||
- {dist_correction: 1.5207593, dist_correction_x: 1.5401453, dist_correction_y: 1.5190082,
|
||||
focal_distance: 20.5, focal_slope: 1.25, horiz_offset_correction: -0.025999999,
|
||||
laser_id: 13, max_intensity: 240, rot_correction: -0.037047761947550245, vert_correction: -0.048730533448148504,
|
||||
vert_offset_correction: 0.21007986}
|
||||
- {dist_correction: 1.4746239, dist_correction_x: 1.5300989999999999, dist_correction_y: 1.4663734,
|
||||
focal_distance: 12.5, focal_slope: 1.9, horiz_offset_correction: 0.025999999, laser_id: 14,
|
||||
max_intensity: 245, rot_correction: -0.0895955468906376, vert_correction: -0.08961595328025192,
|
||||
vert_offset_correction: 0.21303425}
|
||||
- {dist_correction: 1.4697629, dist_correction_x: 1.5051735, dist_correction_y: 1.4558601,
|
||||
focal_distance: 19.5, focal_slope: 0.94999999, horiz_offset_correction: -0.025999999,
|
||||
laser_id: 15, max_intensity: 245, rot_correction: -0.05072966149865084, vert_correction: -0.0839353306800186,
|
||||
vert_offset_correction: 0.21262267999999998}
|
||||
- {dist_correction: 1.4143376, dist_correction_x: 1.4432597000000003, dist_correction_y: 1.4038483,
|
||||
focal_distance: 24.0, focal_slope: 0.69999999, horiz_offset_correction: 0.025999999,
|
||||
laser_id: 16, max_intensity: 245, rot_correction: -0.001864320564407547, vert_correction: -0.043427018903405855,
|
||||
vert_offset_correction: 0.20969770000000001}
|
||||
- {dist_correction: 1.4815961, dist_correction_x: 1.5173615, dist_correction_y: 1.4871606,
|
||||
focal_distance: 14.5, focal_slope: 1.65, horiz_offset_correction: -0.025999999,
|
||||
laser_id: 17, rot_correction: 0.03747852840556422, vert_correction: -0.03648801216715539,
|
||||
vert_offset_correction: 0.20919794}
|
||||
- {dist_correction: 1.420331, dist_correction_x: 1.4755219, dist_correction_y: 1.3919118000000001,
|
||||
focal_distance: 17.5, focal_slope: 1.4, horiz_offset_correction: 0.025999999, laser_id: 18,
|
||||
rot_correction: -0.015194972429011364, vert_correction: -0.07906187922560139, vert_offset_correction: 0.21226994000000002}
|
||||
- {dist_correction: 1.4900717, dist_correction_x: 1.5143349000000002, dist_correction_y: 1.4829907,
|
||||
focal_distance: 24.0, focal_slope: 1.25, horiz_offset_correction: -0.025999999,
|
||||
laser_id: 19, rot_correction: 0.02391977928648433, vert_correction: -0.07255814015150577,
|
||||
vert_offset_correction: 0.21179958}
|
||||
- {dist_correction: 1.4823865, dist_correction_x: 1.5357741999999999, dist_correction_y: 1.4956403,
|
||||
focal_distance: 24.0, focal_slope: 1.1, horiz_offset_correction: 0.025999999, laser_id: 20,
|
||||
rot_correction: 0.07264374331532833, vert_correction: -0.031587736747464255, vert_offset_correction: 0.20884519999999998}
|
||||
- {dist_correction: 1.5553201, dist_correction_x: 1.5698593, dist_correction_y: 1.5624425,
|
||||
focal_distance: 15.5, focal_slope: 1.55, horiz_offset_correction: -0.025999999,
|
||||
laser_id: 21, rot_correction: 0.11143265968730214, vert_correction: -0.025460288914769372,
|
||||
vert_offset_correction: 0.20840424}
|
||||
- {dist_correction: 1.4437845999999999, dist_correction_x: 1.4909378000000002, dist_correction_y: 1.4347861,
|
||||
focal_distance: 20.5, focal_slope: 1.2, horiz_offset_correction: 0.025999999, laser_id: 22,
|
||||
rot_correction: 0.05942554283989759, vert_correction: -0.06665878416276627, vert_offset_correction: 0.21137333000000003}
|
||||
- {dist_correction: 1.4939513, dist_correction_x: 1.5194868, dist_correction_y: 1.5106346,
|
||||
focal_distance: 14.5, focal_slope: 1.65, horiz_offset_correction: -0.025999999,
|
||||
laser_id: 23, rot_correction: 0.09797042203986979, vert_correction: -0.06055112836378901,
|
||||
vert_offset_correction: 0.21093236999999998}
|
||||
- {dist_correction: 1.4521244999999998, dist_correction_x: 1.4884882, dist_correction_y: 1.4649333,
|
||||
focal_distance: 11.5, focal_slope: 2.0, horiz_offset_correction: 0.025999999, laser_id: 24,
|
||||
rot_correction: -0.07658879305408597, vert_correction: 0.015620435355027301, vert_offset_correction: 0.20544985000000002}
|
||||
- {dist_correction: 1.5351622, dist_correction_x: 1.524973, dist_correction_y: 1.5029565,
|
||||
focal_distance: 18.5, focal_slope: 1.25, horiz_offset_correction: -0.025999999,
|
||||
laser_id: 25, rot_correction: -0.0379098725675606, vert_correction: 0.022567997346205196,
|
||||
vert_offset_correction: 0.20495010000000002}
|
||||
- {dist_correction: 1.5321327, dist_correction_x: 1.5833431999999998, dist_correction_y: 1.5359726,
|
||||
focal_distance: 13.0, focal_slope: 1.9, horiz_offset_correction: 0.025999999, laser_id: 26,
|
||||
rot_correction: -0.0904730670226138, vert_correction: -0.019943931467746017, vert_offset_correction: 0.20800737000000002}
|
||||
- {dist_correction: 1.5166023000000002, dist_correction_x: 1.4874829, dist_correction_y: 1.4625635,
|
||||
focal_distance: 21.5, focal_slope: 1.1, horiz_offset_correction: -0.025999999, laser_id: 27,
|
||||
rot_correction: -0.051308328902808065, vert_correction: -0.013200099491225388, vert_offset_correction: 0.20752234000000003}
|
||||
- {dist_correction: 1.4549194, dist_correction_x: 1.4688664, dist_correction_y: 1.4194371000000001,
|
||||
focal_distance: 13.5, focal_slope: 1.83, horiz_offset_correction: 0.025999999, laser_id: 28,
|
||||
rot_correction: -0.0019143155208309244, vert_correction: 0.027266617424120905, vert_offset_correction: 0.20461203000000003}
|
||||
- {dist_correction: 1.5617532, dist_correction_x: 1.5887217999999999, dist_correction_y: 1.5643922000000001,
|
||||
focal_distance: 18.5, focal_slope: 1.35, horiz_offset_correction: -0.025999999,
|
||||
laser_id: 29, rot_correction: 0.03681404490741569, vert_correction: 0.03421014630846329,
|
||||
vert_offset_correction: 0.20411228}
|
||||
- {dist_correction: 1.5132924, dist_correction_x: 1.5661812000000002, dist_correction_y: 1.5102689,
|
||||
focal_distance: 18.5, focal_slope: 1.35, horiz_offset_correction: 0.025999999, laser_id: 30,
|
||||
rot_correction: -0.015542064486559148, vert_correction: -0.007885903531460533, vert_offset_correction: 0.20714018}
|
||||
- {dist_correction: 1.5638524, dist_correction_x: 1.5621136000000002, dist_correction_y: 1.5383017,
|
||||
focal_distance: 21.5, focal_slope: 1.15, horiz_offset_correction: -0.025999999,
|
||||
laser_id: 31, rot_correction: 0.023284423588222334, vert_correction: -0.0015491891506552067,
|
||||
vert_offset_correction: 0.20668451000000002}
|
||||
- {dist_correction: 1.3077792, dist_correction_x: 1.3433452, dist_correction_y: 1.2840973,
|
||||
focal_distance: 10.5, focal_slope: 2.0, horiz_offset_correction: 0.025999999, laser_id: 32,
|
||||
rot_correction: -0.1278634161583795, vert_correction: -0.39021216204755743, vert_offset_correction: 0.15970787}
|
||||
- {dist_correction: 1.4303885, dist_correction_x: 1.4429931999999999, dist_correction_y: 1.4607234,
|
||||
focal_distance: 0.25, focal_slope: 1.0, horiz_offset_correction: -0.025999999, laser_id: 33,
|
||||
rot_correction: -0.066528586091226, vert_correction: -0.38355018793281753, vert_offset_correction: 0.15922519}
|
||||
- {dist_correction: 1.3746819, dist_correction_x: 1.3873923, dist_correction_y: 1.4089924999999999,
|
||||
focal_distance: 0.25, focal_slope: 1.0, horiz_offset_correction: 0.025999999, laser_id: 34,
|
||||
rot_correction: 0.08854290740283328, vert_correction: -0.197138848550284, vert_offset_correction: 0.14656122}
|
||||
- {dist_correction: 1.3814778, dist_correction_x: 1.3932765, dist_correction_y: 1.4301869,
|
||||
focal_distance: 11.0, focal_slope: 2.0, horiz_offset_correction: -0.025999999, laser_id: 35,
|
||||
rot_correction: 0.14547956884148489, vert_correction: -0.18768579625563228, vert_offset_correction: 0.14595152}
|
||||
- {dist_correction: 1.4313307, dist_correction_x: 1.4734517, dist_correction_y: 1.4390849000000001,
|
||||
focal_distance: 9.5, focal_slope: 1.85, horiz_offset_correction: 0.025999999, laser_id: 36,
|
||||
rot_correction: -0.005071588212420635, vert_correction: -0.3750836655445631, vert_offset_correction: 0.15861549}
|
||||
- {dist_correction: 1.3414835, dist_correction_x: 1.3820609, dist_correction_y: 1.355589,
|
||||
focal_distance: 9.5, focal_slope: 1.4, horiz_offset_correction: -0.025999999, laser_id: 37,
|
||||
rot_correction: 0.05427589303135225, vert_correction: -0.36762882325722723, vert_offset_correction: 0.15808201}
|
||||
- {dist_correction: 1.4895827, dist_correction_x: 1.5283238000000001, dist_correction_y: 1.4803529,
|
||||
focal_distance: 9.0, focal_slope: 1.65, horiz_offset_correction: 0.025999999, laser_id: 38,
|
||||
rot_correction: -0.028143856558087547, vert_correction: -0.4285668719175616, vert_offset_correction: 0.16254044}
|
||||
- {dist_correction: 1.3727733000000002, dist_correction_x: 1.3623596, dist_correction_y: 1.392661,
|
||||
focal_distance: 0.25, focal_slope: 1.15, horiz_offset_correction: -0.025999999,
|
||||
laser_id: 39, rot_correction: 0.03172272051181349, vert_correction: -0.4214410765541042,
|
||||
vert_offset_correction: 0.16200695}
|
||||
- {dist_correction: 1.4348983999999998, dist_correction_x: 1.4775407000000003, dist_correction_y: 1.4712174999999998,
|
||||
focal_distance: 0.25, focal_slope: 0.92000002, horiz_offset_correction: 0.025999999,
|
||||
laser_id: 40, rot_correction: 0.11411698480351568, vert_correction: -0.3565455112681652,
|
||||
vert_offset_correction: 0.15729448}
|
||||
- {dist_correction: 1.335618, dist_correction_x: 1.3519691, dist_correction_y: 1.3315152000000001,
|
||||
focal_distance: 11.200000000000001, focal_slope: 2.0, horiz_offset_correction: -0.025999999,
|
||||
laser_id: 41, rot_correction: 0.1735498527902839, vert_correction: -0.34717860842539194,
|
||||
vert_offset_correction: 0.15663397}
|
||||
- {dist_correction: 1.4905824, dist_correction_x: 1.488313, dist_correction_y: 1.4948479000000001,
|
||||
focal_distance: 0.25, focal_slope: 1.1, horiz_offset_correction: 0.025999999, laser_id: 42,
|
||||
rot_correction: 0.09564756333839054, vert_correction: -0.4110102035460227, vert_offset_correction: 0.16123213}
|
||||
- {dist_correction: 1.3288423, dist_correction_x: 1.3409723, dist_correction_y: 1.3440451,
|
||||
focal_distance: 10.0, focal_slope: 1.95, horiz_offset_correction: -0.025999999,
|
||||
laser_id: 43, rot_correction: 0.15538288292189534, vert_correction: -0.40083030888437793,
|
||||
vert_offset_correction: 0.16048269}
|
||||
- {dist_correction: 1.3771290999999999, dist_correction_x: 1.3773239000000002, dist_correction_y: 1.3497290000000002,
|
||||
focal_distance: 11.5, focal_slope: 2.0, horiz_offset_correction: 0.025999999, laser_id: 44,
|
||||
rot_correction: -0.12413605610123538, vert_correction: -0.2840315837972709, vert_offset_correction: 0.15228986}
|
||||
- {dist_correction: 1.3367807, dist_correction_x: 1.370152, dist_correction_y: 1.370934,
|
||||
focal_distance: 0.25, focal_slope: 0.44999999, horiz_offset_correction: -0.025999999,
|
||||
laser_id: 45, rot_correction: -0.06413447432303407, vert_correction: -0.27761533458791926,
|
||||
vert_offset_correction: 0.15185799}
|
||||
- {dist_correction: 1.4788651, dist_correction_x: 1.5014308, dist_correction_y: 1.4727454,
|
||||
focal_distance: 11.5, focal_slope: 2.0, horiz_offset_correction: 0.025999999, laser_id: 46,
|
||||
rot_correction: -0.14872602785785152, vert_correction: -0.3359268721635124, vert_offset_correction: 0.15584644}
|
||||
- {dist_correction: 1.3220766000000002, dist_correction_x: 1.3654865, dist_correction_y: 1.3739757000000001,
|
||||
focal_distance: 0.25, focal_slope: 0.94999999, horiz_offset_correction: -0.025999999,
|
||||
laser_id: 47, rot_correction: -0.08707280959256145, vert_correction: -0.33026751989087316,
|
||||
vert_offset_correction: 0.15545268}
|
||||
- {dist_correction: 1.4612433999999999, dist_correction_x: 1.5250436, dist_correction_y: 1.4817635999999998,
|
||||
focal_distance: 13.0, focal_slope: 1.0, horiz_offset_correction: 0.025999999, laser_id: 48,
|
||||
rot_correction: -0.006799911150716457, vert_correction: -0.26851710773019805, vert_offset_correction: 0.15124829}
|
||||
- {dist_correction: 1.3407353000000002, dist_correction_x: 1.3478844, dist_correction_y: 1.3154279,
|
||||
focal_distance: 13.5, focal_slope: 1.8, horiz_offset_correction: -0.025999999, laser_id: 49,
|
||||
rot_correction: 0.05242808851765633, vert_correction: -0.26051854302098837, vert_offset_correction: 0.1507148}
|
||||
- {dist_correction: 1.3465115, dist_correction_x: 1.3874431999999999, dist_correction_y: 1.3508052000000002,
|
||||
focal_distance: 12.0, focal_slope: 1.85, horiz_offset_correction: 0.025999999, laser_id: 50,
|
||||
rot_correction: -0.028383715411859876, vert_correction: -0.3216451745070007, vert_offset_correction: 0.15485568000000002}
|
||||
- {dist_correction: 1.3629696999999998, dist_correction_x: 1.4037315000000001, dist_correction_y: 1.3869237,
|
||||
focal_distance: 12.0, focal_slope: 1.1, horiz_offset_correction: -0.025999999, laser_id: 51,
|
||||
rot_correction: 0.031843273893907245, vert_correction: -0.3135280670349956, vert_offset_correction: 0.15429679000000002}
|
||||
- {dist_correction: 1.4472754, dist_correction_x: 1.4817390000000001, dist_correction_y: 1.4782272,
|
||||
focal_distance: 2.0, focal_slope: 1.0, horiz_offset_correction: 0.025999999, laser_id: 52,
|
||||
rot_correction: 0.10955275158734502, vert_correction: -0.24941678290173272, vert_offset_correction: 0.14997807999999999}
|
||||
- {dist_correction: 1.3800671, dist_correction_x: 1.3796414, dist_correction_y: 1.3943782,
|
||||
focal_distance: 11.200000000000001, focal_slope: 2.0, horiz_offset_correction: -0.025999999,
|
||||
laser_id: 53, rot_correction: 0.16876716543828738, vert_correction: -0.2409525119882134,
|
||||
vert_offset_correction: 0.14941919}
|
||||
- {dist_correction: 1.5190169, dist_correction_x: 1.5619051, dist_correction_y: 1.5511705,
|
||||
focal_distance: 2.5, focal_slope: 1.05, horiz_offset_correction: 0.025999999, laser_id: 54,
|
||||
rot_correction: 0.09037283276367176, vert_correction: -0.30313513748485243, vert_offset_correction: 0.15358547}
|
||||
- {dist_correction: 1.3803656000000002, dist_correction_x: 1.3960698, dist_correction_y: 1.386106,
|
||||
focal_distance: 12.5, focal_slope: 1.9, horiz_offset_correction: -0.025999999, laser_id: 55,
|
||||
rot_correction: 0.14871534295217081, vert_correction: -0.29323634555253386, vert_offset_correction: 0.15291226}
|
||||
- {dist_correction: 1.5827696, dist_correction_x: 1.6050609, dist_correction_y: 1.5844797,
|
||||
focal_distance: 10.0, focal_slope: 1.95, horiz_offset_correction: 0.025999999, laser_id: 56,
|
||||
rot_correction: -0.12100867217308608, vert_correction: -0.17760463486977288, vert_offset_correction: 0.14530372}
|
||||
- {dist_correction: 1.3843919, dist_correction_x: 1.3915251000000002, dist_correction_y: 1.4156927,
|
||||
focal_distance: 0.25, focal_slope: 0.94999999, horiz_offset_correction: -0.025999999,
|
||||
laser_id: 57, rot_correction: -0.06431965899265843, vert_correction: -0.17006926832673774,
|
||||
vert_offset_correction: 0.14482104}
|
||||
- {dist_correction: 1.5491273, dist_correction_x: 1.5298964000000002, dist_correction_y: 1.5107637,
|
||||
focal_distance: 11.5, focal_slope: 2.0, horiz_offset_correction: 0.025999999, laser_id: 58,
|
||||
rot_correction: -0.14486168214370612, vert_correction: -0.22974121500510264, vert_offset_correction: 0.14868247}
|
||||
- {dist_correction: 1.3646004, dist_correction_x: 1.3803583000000001, dist_correction_y: 1.4061511,
|
||||
focal_distance: 0.25, focal_slope: 1.0, horiz_offset_correction: -0.025999999, laser_id: 59,
|
||||
rot_correction: -0.0852480451703211, vert_correction: -0.22391891879349718, vert_offset_correction: 0.14830141}
|
||||
- {dist_correction: 1.5239935, dist_correction_x: 1.560786, dist_correction_y: 1.5632524,
|
||||
focal_distance: 5.0, focal_slope: 1.15, horiz_offset_correction: 0.025999999, laser_id: 60,
|
||||
rot_correction: -0.005967226432828876, vert_correction: -0.16231529056824404, vert_offset_correction: 0.14432566}
|
||||
- {dist_correction: 1.4112029000000001, dist_correction_x: 1.4223886, dist_correction_y: 1.4542918,
|
||||
focal_distance: 2.5, focal_slope: 1.05, horiz_offset_correction: -0.025999999, laser_id: 61,
|
||||
rot_correction: 0.050600613599087636, vert_correction: -0.15314424682880032, vert_offset_correction: 0.14374136}
|
||||
- {dist_correction: 1.5013637, dist_correction_x: 1.5208610999999999, dist_correction_y: 1.5006433000000001,
|
||||
focal_distance: 16.5, focal_slope: 1.25, horiz_offset_correction: 0.025999999, laser_id: 62,
|
||||
rot_correction: -0.027436902217920986, vert_correction: -0.21457119711920336, vert_offset_correction: 0.14769171}
|
||||
- {dist_correction: 1.4423058, dist_correction_x: 1.4454633000000001, dist_correction_y: 1.4321198000000002,
|
||||
focal_distance: 9.0, focal_slope: 1.45, horiz_offset_correction: -0.025999999, laser_id: 63,
|
||||
rot_correction: 0.0290479702718764, vert_correction: -0.2079266762969834, vert_offset_correction: 0.14725984}
|
||||
num_lasers: 64
|
||||
distance_resolution: 0.002
|
||||
@@ -0,0 +1,259 @@
|
||||
# University of Texas HDL-64E calibration parameters
|
||||
lasers:
|
||||
- {dist_correction: 0.100000001490116, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 0, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.0698131695389748, two_pt_correction_available: false, vert_correction: -0.124932751059532,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.280000001192093, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 1, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.0392699092626572, two_pt_correction_available: false, vert_correction: -0.118993431329727,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.319999992847443, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 2, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.0698131695389748, two_pt_correction_available: false, vert_correction: 0.0055470340885222,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.230000004172325, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 3, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.104719758033752, two_pt_correction_available: false, vert_correction: 0.0114863449707627,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.0700000002980232, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 4, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.013962633907795, two_pt_correction_available: false, vert_correction: -0.113056324422359,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.0900000035762787, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 5, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.0392699092626572, two_pt_correction_available: false, vert_correction: -0.107121199369431,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.119999997317791, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 6, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0, two_pt_correction_available: false, vert_correction: -0.148716226220131,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.200000002980232, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 7, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.0226892791688442, two_pt_correction_available: false, vert_correction: -0.142765983939171,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.129999995231628, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 8, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.0820304751396179, two_pt_correction_available: false, vert_correction: -0.101187855005264,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.159999996423721, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 9, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.113446399569511, two_pt_correction_available: false, vert_correction: -0.0952560678124428,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.159999996423721, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 10, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.0698131695389748, two_pt_correction_available: false, vert_correction: -0.136818811297417,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.159999996423721, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 11, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.104719758033752, two_pt_correction_available: false, vert_correction: -0.130874469876289,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.129999995231628, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 12, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.068067841231823, two_pt_correction_available: false, vert_correction: -0.05375986546278,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.200000002980232, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 13, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.0349065847694874, two_pt_correction_available: false, vert_correction: -0.0478330813348293,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.170000001788139, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 14, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.0837758108973503, two_pt_correction_available: false, vert_correction: -0.0893256440758705,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.239999994635582, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 15, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.0479965545237064, two_pt_correction_available: false, vert_correction: -0.0833963677287102,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.180000007152557, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 16, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.00872664619237185, two_pt_correction_available: false, vert_correction: -0.0419059917330742,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.0599999986588955, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 17, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.0392699092626572, two_pt_correction_available: false, vert_correction: -0.035978376865387,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.140000000596046, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 18, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0, two_pt_correction_available: false, vert_correction: -0.0774680152535439,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.150000005960464, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 19, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.0305432621389627, two_pt_correction_available: false, vert_correction: -0.0715404152870178,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.219999998807907, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 20, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.0855211317539215, two_pt_correction_available: false, vert_correction: -0.0300500374287367,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.140000000596046, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 21, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.109083078801632, two_pt_correction_available: false, vert_correction: -0.024120757356286,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.0799999982118607, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 22, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.0698131695389748, two_pt_correction_available: false, vert_correction: -0.0656133219599724,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.159999996423721, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 23, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.104719758033752, two_pt_correction_available: false, vert_correction: -0.0596865378320217,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.119999997317791, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 24, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.0610865242779255, two_pt_correction_available: false, vert_correction: 0.0174280721694231,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.219999998807907, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 25, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.0349065847694874, two_pt_correction_available: false, vert_correction: 0.0233724191784859,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.159999996423721, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 26, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.0785398185253143, two_pt_correction_available: false, vert_correction: -0.0181903336197138,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.259999990463257, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 27, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.0471238903701305, two_pt_correction_available: false, vert_correction: -0.0122585473582149,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.140000000596046, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 28, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.0104719763621688, two_pt_correction_available: false, vert_correction: 0.0293195936828852,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.219999998807907, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 29, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.0349065847694874, two_pt_correction_available: false, vert_correction: 0.0352698266506195,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.209999993443489, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 30, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.00436332309618592, two_pt_correction_available: false, vert_correction: -0.00632520206272602,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.140000000596046, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 31, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.0296705979853868, two_pt_correction_available: false, vert_correction: -0.000390077300835401,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.119999997317791, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 32, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.122173048555851, two_pt_correction_available: false, vert_correction: -0.396850973367691,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.0199999995529652, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 33, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.0610865242779255, two_pt_correction_available: false, vert_correction: -0.387918144464493,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.100000001490116, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 34, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.0959931090474129, two_pt_correction_available: false, vert_correction: -0.200955957174301,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.230000004172325, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 35, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.148352980613708, two_pt_correction_available: false, vert_correction: -0.192023113369942,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.170000001788139, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 36, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.00872664619237185, two_pt_correction_available: false, vert_correction: -0.378992766141891,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.150000005960464, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 37, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.0610865242779255, two_pt_correction_available: false, vert_correction: -0.370074152946472,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.0500000007450581, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 38, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.0174532923847437, two_pt_correction_available: false, vert_correction: -0.43128889799118,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.270000010728836, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 39, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.0349065847694874, two_pt_correction_available: false, vert_correction: -0.423701733350754,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.180000007152557, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 40, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.122173048555851, two_pt_correction_available: false, vert_correction: -0.361161530017853,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.189999997615814, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 41, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.174532920122147, two_pt_correction_available: false, vert_correction: -0.352254241704941,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.100000001490116, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 42, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.104719758033752, two_pt_correction_available: false, vert_correction: -0.414742022752762,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.200000002980232, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 43, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.165806278586388, two_pt_correction_available: false, vert_correction: -0.405792057514191,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.219999998807907, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 44, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.113446399569511, two_pt_correction_available: false, vert_correction: -0.28999200463295,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.200000002980232, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 45, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.0567231997847557, two_pt_correction_available: false, vert_correction: -0.281101644039154,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.150000005960464, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 46, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.13962633907795, two_pt_correction_available: false, vert_correction: -0.343351542949677,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.219999998807907, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 47, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.0872664600610733, two_pt_correction_available: false, vert_correction: -0.334452718496323,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.0599999986588955, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 48, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0, two_pt_correction_available: false, vert_correction: -0.272210210561752,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.129999995231628, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 49, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.0558505356311798, two_pt_correction_available: false, vert_correction: -0.26331701874733,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.00999999977648258, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 50, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.0174532923847437, two_pt_correction_available: false, vert_correction: -0.323895305395126,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.150000005960464, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 51, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.0349065847694874, two_pt_correction_available: false, vert_correction: -0.316663861274719,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.200000002980232, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 52, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.122173048555851, two_pt_correction_available: false, vert_correction: -0.254421383142471,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.239999994635582, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 53, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.165806278586388, two_pt_correction_available: false, vert_correction: -0.245522528886795,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.180000007152557, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 54, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.104719758033752, two_pt_correction_available: false, vert_correction: -0.307772427797318,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.219999998807907, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 55, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.157079637050629, two_pt_correction_available: false, vert_correction: -0.298882067203522,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.140000000596046, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 56, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.104719758033752, two_pt_correction_available: false, vert_correction: -0.183082059025764,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.319999992847443, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 57, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.0610865242779255, two_pt_correction_available: false, vert_correction: -0.17413204908371,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.200000002980232, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 58, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.13962633907795, two_pt_correction_available: false, vert_correction: -0.236619830131531,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.25, dist_correction_x: 0, dist_correction_y: 0, focal_distance: 0,
|
||||
focal_slope: 0, laser_id: 59, max_intensity: 255, min_intensity: 0, rot_correction: -0.0785398185253143,
|
||||
two_pt_correction_available: false, vert_correction: -0.22771255671978, vert_offset_correction: 0}
|
||||
- {dist_correction: 0.170000001788139, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 60, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0, two_pt_correction_available: false, vert_correction: -0.16517236828804,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.230000004172325, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 61, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.0436332300305367, two_pt_correction_available: false, vert_correction: -0.156202226877213,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.150000005960464, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 62, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.0174532923847437, two_pt_correction_available: false, vert_correction: -0.218799933791161,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.230000004172325, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 63, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.0314159244298935, two_pt_correction_available: false, vert_correction: -0.209881335496902,
|
||||
vert_offset_correction: 0}
|
||||
num_lasers: 64
|
||||
distance_resolution: 0.002
|
||||
@@ -0,0 +1,51 @@
|
||||
lasers:
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 0, rot_correction: 0.0,
|
||||
vert_correction: -0.17453292519943295, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 1, rot_correction: 0.0,
|
||||
vert_correction: 0.011635528346628864, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 2, rot_correction: 0.0,
|
||||
vert_correction: -0.15126186850617523, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 3, rot_correction: 0.0,
|
||||
vert_correction: 0.03490658503988659, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 4, rot_correction: 0.0,
|
||||
vert_correction: -0.1279908118129175, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 5, rot_correction: 0.0,
|
||||
vert_correction: 0.05817764173314432, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 6, rot_correction: 0.0,
|
||||
vert_correction: -0.10471975511965977, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 7, rot_correction: 0.0,
|
||||
vert_correction: 0.08144869842640205, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 8, rot_correction: 0.0,
|
||||
vert_correction: -0.08144869842640205, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 9, rot_correction: 0.0,
|
||||
vert_correction: 0.10471975511965977, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 10, rot_correction: 0.0,
|
||||
vert_correction: -0.05817764173314432, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 11, rot_correction: 0.0,
|
||||
vert_correction: 0.1279908118129175, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 12, rot_correction: 0.0,
|
||||
vert_correction: -0.03490658503988659, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 13, rot_correction: 0.0,
|
||||
vert_correction: 0.15126186850617523, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 14, rot_correction: 0.0,
|
||||
vert_correction: -0.011635528346628864, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 15, rot_correction: 0.0,
|
||||
vert_correction: 0.17453292519943295, vert_offset_correction: 0.0}
|
||||
num_lasers: 16
|
||||
distance_resolution: 0.002
|
||||
@@ -0,0 +1,51 @@
|
||||
lasers:
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 0, rot_correction: 0.0,
|
||||
vert_correction: -0.2617993877991494, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 1, rot_correction: 0.0,
|
||||
vert_correction: 0.017453292519943295, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 2, rot_correction: 0.0,
|
||||
vert_correction: -0.22689280275926285, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 3, rot_correction: 0.0,
|
||||
vert_correction: 0.05235987755982989, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 4, rot_correction: 0.0,
|
||||
vert_correction: -0.19198621771937624, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 5, rot_correction: 0.0,
|
||||
vert_correction: 0.08726646259971647, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 6, rot_correction: 0.0,
|
||||
vert_correction: -0.15707963267948966, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 7, rot_correction: 0.0,
|
||||
vert_correction: 0.12217304763960307, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 8, rot_correction: 0.0,
|
||||
vert_correction: -0.12217304763960307, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 9, rot_correction: 0.0,
|
||||
vert_correction: 0.15707963267948966, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 10, rot_correction: 0.0,
|
||||
vert_correction: -0.08726646259971647, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 11, rot_correction: 0.0,
|
||||
vert_correction: 0.19198621771937624, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 12, rot_correction: 0.0,
|
||||
vert_correction: -0.05235987755982989, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 13, rot_correction: 0.0,
|
||||
vert_correction: 0.22689280275926285, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 14, rot_correction: 0.0,
|
||||
vert_correction: -0.017453292519943295, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 15, rot_correction: 0.0,
|
||||
vert_correction: 0.2617993877991494, vert_offset_correction: 0.0}
|
||||
num_lasers: 16
|
||||
distance_resolution: 0.002
|
||||
@@ -0,0 +1,99 @@
|
||||
lasers:
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 0, rot_correction: -0.024434609527920613,
|
||||
vert_correction: -0.4363323129985824, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 1, rot_correction: 0.07330382858376185,
|
||||
vert_correction: -0.017453292519943295, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 2, rot_correction: -0.024434609527920613,
|
||||
vert_correction: -0.029094638630745476, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 3, rot_correction: 0.024434609527920613,
|
||||
vert_correction: -0.2729520417193932, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 4, rot_correction: -0.024434609527920613,
|
||||
vert_correction: -0.19739673840055869, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 5, rot_correction: 0.024434609527920613,
|
||||
vert_correction: 0.0, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 6, rot_correction: -0.07330382858376185,
|
||||
vert_correction: -0.011641346110802179, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 7, rot_correction: 0.024434609527920613,
|
||||
vert_correction: -0.15433946575385857, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 8, rot_correction: -0.024434609527920613,
|
||||
vert_correction: -0.12660618393966866, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 9, rot_correction: 0.07330382858376185,
|
||||
vert_correction: 0.005811946409141118, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 10, rot_correction: -0.024434609527920613,
|
||||
vert_correction: -0.005811946409141118, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 11, rot_correction: 0.024434609527920613,
|
||||
vert_correction: -0.10730284241261137, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 12, rot_correction: -0.07330382858376185,
|
||||
vert_correction: -0.0930784090088576, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 13, rot_correction: 0.024434609527920613,
|
||||
vert_correction: 0.02326523892908441, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 14, rot_correction: -0.07330382858376185,
|
||||
vert_correction: 0.011641346110802179, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 15, rot_correction: 0.024434609527920613,
|
||||
vert_correction: -0.06981317007977318, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 16, rot_correction: -0.024434609527920613,
|
||||
vert_correction: -0.08145451619057535, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 17, rot_correction: 0.07330382858376185,
|
||||
vert_correction: 0.029094638630745476, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 18, rot_correction: -0.024434609527920613,
|
||||
vert_correction: 0.017453292519943295, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 19, rot_correction: 0.07330382858376185,
|
||||
vert_correction: -0.06400122367063206, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 20, rot_correction: -0.07330382858376185,
|
||||
vert_correction: -0.058171823968971005, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 21, rot_correction: 0.024434609527920613,
|
||||
vert_correction: 0.058171823968971005, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 22, rot_correction: -0.024434609527920613,
|
||||
vert_correction: 0.04071853144902771, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 23, rot_correction: 0.024434609527920613,
|
||||
vert_correction: -0.04654793115068877, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 24, rot_correction: -0.024434609527920613,
|
||||
vert_correction: -0.05235987755982989, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 25, rot_correction: 0.024434609527920613,
|
||||
vert_correction: 0.12217304763960307, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 26, rot_correction: -0.024434609527920613,
|
||||
vert_correction: 0.08145451619057535, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 27, rot_correction: 0.07330382858376185,
|
||||
vert_correction: -0.04071853144902771, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 28, rot_correction: -0.07330382858376185,
|
||||
vert_correction: -0.03490658503988659, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 29, rot_correction: 0.024434609527920613,
|
||||
vert_correction: 0.2617993877991494, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 30, rot_correction: -0.024434609527920613,
|
||||
vert_correction: 0.18034487160857407, vert_offset_correction: 0.0}
|
||||
- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
|
||||
focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 31, rot_correction: 0.024434609527920613,
|
||||
vert_correction: -0.02326523892908441, vert_offset_correction: 0.0}
|
||||
num_lasers: 32
|
||||
distance_resolution: 0.004
|
||||
@@ -0,0 +1,78 @@
|
||||
Background\ ColorB=0
|
||||
Background\ ColorG=0
|
||||
Background\ ColorR=0
|
||||
Camera\ Config=0.384797 3.10041 41.6639 0 0 0
|
||||
Camera\ Type=rviz::OrbitViewController
|
||||
Fixed\ Frame=/velodyne
|
||||
Grid.Alpha=0.5
|
||||
Grid.Cell\ Size=1
|
||||
Grid.ColorB=0.5
|
||||
Grid.ColorG=0.5
|
||||
Grid.ColorR=0.5
|
||||
Grid.Enabled=1
|
||||
Grid.Line\ Style=0
|
||||
Grid.Line\ Width=0.03
|
||||
Grid.Normal\ Cell\ Count=0
|
||||
Grid.OffsetX=0
|
||||
Grid.OffsetY=0
|
||||
Grid.OffsetZ=0
|
||||
Grid.Plane=0
|
||||
Grid.Plane\ Cell\ Count=50
|
||||
Grid.Reference\ Frame=<Fixed Frame>
|
||||
Property\ Grid\ Splitter=751,78
|
||||
Property\ Grid\ State=expanded=.Global Options,Grid.Enabled,Velodyne Cloud.Enabled;splitterratio=0.5
|
||||
QMainWindow=000000ff00000000fd00000003000000000000011d0000038ffc0200000001fb000000100044006900730070006c006100790073010000001d0000038f000000ee00ffffff00000001000001300000038ffc0200000003fb0000001e0054006f006f006c002000500072006f0070006500720074006900650073010000001d0000011f0000006700fffffffb0000000a00560069006500770073010000014200000145000000bb00fffffffb0000001200530065006c0065006300740069006f006e010000028d0000011f0000006700ffffff00000003000005d10000003efc0100000001fb0000000800540069006d00650100000000000005d1000002bf00ffffff000003780000038f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
TF.All\ Enabled=1
|
||||
TF.Enabled=1
|
||||
TF.Frame\ Timeout=15
|
||||
TF.Marker\ Scale=1
|
||||
TF.Show\ Arrows=1
|
||||
TF.Show\ Axes=1
|
||||
TF.Show\ Names=1
|
||||
TF.Update\ Interval=0
|
||||
Target\ Frame=<Fixed Frame>
|
||||
Tool\ 2D\ Nav\ GoalTopic=move_base_simple/goal
|
||||
Tool\ 2D\ Pose\ EstimateTopic=initialpose
|
||||
Velodyne\ Cloud..AxisColorAutocompute\ Value\ Bounds=1
|
||||
Velodyne\ Cloud..AxisColorAxis=2
|
||||
Velodyne\ Cloud..AxisColorMax\ Value=10
|
||||
Velodyne\ Cloud..AxisColorMin\ Value=-10
|
||||
Velodyne\ Cloud..AxisColorUse\ Fixed\ Frame=1
|
||||
Velodyne\ Cloud..FlatColorColorB=1
|
||||
Velodyne\ Cloud..FlatColorColorG=1
|
||||
Velodyne\ Cloud..FlatColorColorR=1
|
||||
Velodyne\ Cloud..IntensityAutocompute\ Intensity\ Bounds=1
|
||||
Velodyne\ Cloud..IntensityChannel\ Name=intensity
|
||||
Velodyne\ Cloud..IntensityMax\ ColorB=1
|
||||
Velodyne\ Cloud..IntensityMax\ ColorG=0.776471
|
||||
Velodyne\ Cloud..IntensityMax\ ColorR=0.796078
|
||||
Velodyne\ Cloud..IntensityMax\ Intensity=255
|
||||
Velodyne\ Cloud..IntensityMin\ ColorB=1
|
||||
Velodyne\ Cloud..IntensityMin\ ColorG=0.0823529
|
||||
Velodyne\ Cloud..IntensityMin\ ColorR=0.141176
|
||||
Velodyne\ Cloud..IntensityMin\ Intensity=95
|
||||
Velodyne\ Cloud..IntensityUse\ full\ RGB\ spectrum=0
|
||||
Velodyne\ Cloud.Alpha=1
|
||||
Velodyne\ Cloud.Billboard\ Size=0.1
|
||||
Velodyne\ Cloud.Color\ Transformer=Intensity
|
||||
Velodyne\ Cloud.Decay\ Time=0
|
||||
Velodyne\ Cloud.Enabled=1
|
||||
Velodyne\ Cloud.Position\ Transformer=XYZ
|
||||
Velodyne\ Cloud.Queue\ Size=10
|
||||
Velodyne\ Cloud.Selectable=1
|
||||
Velodyne\ Cloud.Style=0
|
||||
Velodyne\ Cloud.Topic=/velodyne_points
|
||||
[Display0]
|
||||
ClassName=rviz::GridDisplay
|
||||
Name=Grid
|
||||
[Display1]
|
||||
ClassName=rviz::TFDisplay
|
||||
Name=TF
|
||||
[Display2]
|
||||
ClassName=rviz::PointCloud2Display
|
||||
Name=Velodyne Cloud
|
||||
[Window]
|
||||
Height=1045
|
||||
Width=1505
|
||||
X=-8
|
||||
Y=-29
|
||||
@@ -0,0 +1,209 @@
|
||||
#!/usr/bin/python
|
||||
# Software License Agreement (BSD License)
|
||||
#
|
||||
# Copyright (C) 2012, Austin Robot Technology
|
||||
# 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 Austin Robot Technology, Inc. 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.
|
||||
#
|
||||
# Revision $Id$
|
||||
|
||||
"""
|
||||
Generate YAML calibration file from Velodyne db.xml.
|
||||
|
||||
The input data provided by the manufacturer are in degrees and
|
||||
centimeters. The YAML file uses radians and meters, following ROS
|
||||
standards [REP-0103].
|
||||
|
||||
"""
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
import math
|
||||
import optparse
|
||||
import os
|
||||
import sys
|
||||
from xml.etree import ElementTree
|
||||
import yaml
|
||||
|
||||
# parse the command line
|
||||
usage = """usage: %prog infile.xml [outfile.yaml]
|
||||
|
||||
Default output file is input file with .yaml suffix."""
|
||||
parser = optparse.OptionParser(usage=usage)
|
||||
options, args = parser.parse_args()
|
||||
|
||||
if len(args) < 1:
|
||||
parser.error('XML file name missing')
|
||||
sys.exit(9)
|
||||
|
||||
xmlFile = args[0]
|
||||
if len(args) >= 2:
|
||||
yamlFile = args[1]
|
||||
else:
|
||||
yamlFile, ext = os.path.splitext(xmlFile)
|
||||
yamlFile += '.yaml'
|
||||
|
||||
print('converting "' + xmlFile + '" to "' + yamlFile + '"')
|
||||
|
||||
calibrationGood = True
|
||||
def xmlError(msg):
|
||||
'handle XML calibration error'
|
||||
global calibrationGood
|
||||
calibrationGood = False
|
||||
print('gen_calibration.py: ' + msg)
|
||||
|
||||
db = None
|
||||
try:
|
||||
db = ElementTree.parse(xmlFile)
|
||||
except IOError:
|
||||
xmlError('unable to read ' + xmlFile)
|
||||
except ElementTree.ParseError:
|
||||
xmlError('XML parse failed for ' + xmlFile)
|
||||
|
||||
if not calibrationGood:
|
||||
sys.exit(2)
|
||||
|
||||
# create a dictionary to hold all relevant calibration values
|
||||
calibration = {'num_lasers': 0, 'lasers': [], 'distance_resolution': 0.2}
|
||||
cm2meters = 0.01 # convert centimeters to meters
|
||||
|
||||
def addLaserCalibration(laser_num, key, val):
|
||||
'Define key and corresponding value for laser_num'
|
||||
global calibration
|
||||
if laser_num < len(calibration['lasers']):
|
||||
calibration['lasers'][laser_num][key] = val
|
||||
else:
|
||||
calibration['lasers'].append({key: val})
|
||||
|
||||
# add enabled flags
|
||||
num_enabled = 0
|
||||
enabled_lasers = []
|
||||
enabled = db.find('DB/enabled_')
|
||||
if enabled == None:
|
||||
print('no enabled tags found: assuming all 64 enabled')
|
||||
num_enabled = 64
|
||||
enabled_lasers = [True for i in xrange(num_enabled)]
|
||||
else:
|
||||
index = 0
|
||||
for el in enabled:
|
||||
if el.tag == 'item':
|
||||
this_enabled = int(el.text) != 0
|
||||
enabled_lasers.append(this_enabled)
|
||||
index += 1
|
||||
if this_enabled:
|
||||
num_enabled += 1
|
||||
|
||||
calibration['num_lasers'] = num_enabled
|
||||
print(str(num_enabled) + ' lasers')
|
||||
|
||||
# add distance resolution (cm)
|
||||
distLSB = db.find('DB/distLSB_')
|
||||
if distLSB != None:
|
||||
calibration['distance_resolution'] = float(distLSB.text) * cm2meters
|
||||
|
||||
# add minimum laser intensities
|
||||
minIntensities = db.find('DB/minIntensity_')
|
||||
if minIntensities != None:
|
||||
index = 0
|
||||
for el in minIntensities:
|
||||
if el.tag == 'item':
|
||||
if enabled_lasers[index]:
|
||||
value = int(el.text)
|
||||
if value != 0:
|
||||
addLaserCalibration(index, 'min_intensity', value)
|
||||
index += 1
|
||||
|
||||
# add maximum laser intensities
|
||||
maxIntensities = db.find('DB/maxIntensity_')
|
||||
if maxIntensities != None:
|
||||
index = 0
|
||||
for el in maxIntensities:
|
||||
if el.tag == 'item':
|
||||
if enabled_lasers[index]:
|
||||
value = int(el.text)
|
||||
if value != 255:
|
||||
addLaserCalibration(index, 'max_intensity', value)
|
||||
index += 1
|
||||
|
||||
# add calibration information for each laser
|
||||
for el in db.find('DB/points_'):
|
||||
if el.tag == 'item':
|
||||
for px in el:
|
||||
for field in px:
|
||||
if field.tag == 'id_':
|
||||
index = int(field.text)
|
||||
if not enabled_lasers[index]:
|
||||
break # skip this laser, it is not enabled
|
||||
addLaserCalibration(index, 'laser_id', index)
|
||||
|
||||
if field.tag == 'rotCorrection_':
|
||||
addLaserCalibration(index, 'rot_correction',
|
||||
math.radians(float(field.text)))
|
||||
elif field.tag == 'vertCorrection_':
|
||||
addLaserCalibration(index, 'vert_correction',
|
||||
math.radians(float(field.text)))
|
||||
elif field.tag == 'distCorrection_':
|
||||
addLaserCalibration(index, 'dist_correction',
|
||||
float(field.text) * cm2meters)
|
||||
elif field.tag == 'distCorrectionX_':
|
||||
addLaserCalibration(index, 'dist_correction_x',
|
||||
float(field.text) * cm2meters)
|
||||
elif field.tag == 'distCorrectionY_':
|
||||
addLaserCalibration(index, 'dist_correction_y',
|
||||
float(field.text) * cm2meters)
|
||||
elif field.tag == 'vertOffsetCorrection_':
|
||||
addLaserCalibration(index, 'vert_offset_correction',
|
||||
float(field.text) * cm2meters)
|
||||
elif field.tag == 'horizOffsetCorrection_':
|
||||
addLaserCalibration(index, 'horiz_offset_correction',
|
||||
float(field.text) * cm2meters)
|
||||
elif field.tag == 'focalDistance_':
|
||||
addLaserCalibration(index, 'focal_distance',
|
||||
float(field.text) * cm2meters)
|
||||
elif field.tag == 'focalSlope_':
|
||||
addLaserCalibration(index, 'focal_slope', float(field.text))
|
||||
|
||||
# validate input data
|
||||
if calibration['num_lasers'] <= 0:
|
||||
xmlError('no lasers defined')
|
||||
elif calibration['num_lasers'] != num_enabled:
|
||||
xmlError('inconsistent number of lasers defined')
|
||||
|
||||
# TODO: make sure all required fields are present.
|
||||
# (Which ones are required?)
|
||||
|
||||
if calibrationGood:
|
||||
|
||||
# write calibration data to YAML file
|
||||
f = open(yamlFile, 'w')
|
||||
try:
|
||||
yaml.dump(calibration, f)
|
||||
finally:
|
||||
f.close()
|
||||
@@ -0,0 +1,31 @@
|
||||
add_executable(cloud_node cloud_node.cc convert.cc pointcloudXYZIR.cc organized_cloudXYZIR.cc)
|
||||
add_dependencies(cloud_node ${${PROJECT_NAME}_EXPORTED_TARGETS})
|
||||
target_link_libraries(cloud_node velodyne_rawdata
|
||||
${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES})
|
||||
install(TARGETS cloud_node
|
||||
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
|
||||
|
||||
add_library(cloud_nodelet cloud_nodelet.cc convert.cc pointcloudXYZIR.cc organized_cloudXYZIR.cc)
|
||||
add_dependencies(cloud_nodelet ${${PROJECT_NAME}_EXPORTED_TARGETS})
|
||||
target_link_libraries(cloud_nodelet velodyne_rawdata
|
||||
${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES})
|
||||
install(TARGETS cloud_nodelet
|
||||
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
|
||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
|
||||
|
||||
add_executable(transform_node transform_node.cc transform.cc pointcloudXYZIR.cc organized_cloudXYZIR.cc)
|
||||
add_dependencies(transform_node ${${PROJECT_NAME}_EXPORTED_TARGETS})
|
||||
target_link_libraries(transform_node velodyne_rawdata
|
||||
${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES})
|
||||
install(TARGETS transform_node
|
||||
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
|
||||
|
||||
add_library(transform_nodelet transform_nodelet.cc transform.cc pointcloudXYZIR.cc organized_cloudXYZIR.cc)
|
||||
add_dependencies(transform_nodelet ${${PROJECT_NAME}_EXPORTED_TARGETS})
|
||||
target_link_libraries(transform_nodelet velodyne_rawdata
|
||||
${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES})
|
||||
install(TARGETS transform_nodelet
|
||||
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
|
||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
|
||||
@@ -0,0 +1,31 @@
|
||||
/*
|
||||
* Copyright (C) 2012 Austin Robot Technology, Jack O'Quin
|
||||
* License: Modified BSD Software License Agreement
|
||||
*
|
||||
* $Id$
|
||||
*/
|
||||
|
||||
/** \file
|
||||
|
||||
This ROS node converts raw Velodyne LIDAR packets to PointCloud2.
|
||||
|
||||
*/
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include "velodyne_pointcloud/convert.h"
|
||||
|
||||
/** Main node entry point. */
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "cloud_node");
|
||||
ros::NodeHandle node;
|
||||
ros::NodeHandle priv_nh("~");
|
||||
|
||||
// create conversion class, which subscribes to raw data
|
||||
velodyne_pointcloud::Convert conv(node, priv_nh);
|
||||
|
||||
// handle callbacks until shut down
|
||||
ros::spin();
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,48 @@
|
||||
/*
|
||||
* Copyright (C) 2012 Austin Robot Technology, Jack O'Quin
|
||||
* License: Modified BSD Software License Agreement
|
||||
*
|
||||
* $Id$
|
||||
*/
|
||||
|
||||
/** @file
|
||||
|
||||
This ROS nodelet converts raw Velodyne 3D LIDAR packets to a
|
||||
PointCloud2.
|
||||
|
||||
*/
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include <nodelet/nodelet.h>
|
||||
|
||||
#include "velodyne_pointcloud/convert.h"
|
||||
|
||||
namespace velodyne_pointcloud
|
||||
{
|
||||
class CloudNodelet: public nodelet::Nodelet
|
||||
{
|
||||
public:
|
||||
|
||||
CloudNodelet() {}
|
||||
~CloudNodelet() {}
|
||||
|
||||
private:
|
||||
|
||||
virtual void onInit();
|
||||
boost::shared_ptr<Convert> conv_;
|
||||
};
|
||||
|
||||
/** @brief Nodelet initialization. */
|
||||
void CloudNodelet::onInit()
|
||||
{
|
||||
conv_.reset(new Convert(getNodeHandle(), getPrivateNodeHandle(), getName()));
|
||||
}
|
||||
|
||||
} // namespace velodyne_pointcloud
|
||||
|
||||
|
||||
// Register this plugin with pluginlib. Names must match nodelets.xml.
|
||||
//
|
||||
// parameters: class type, base class type
|
||||
PLUGINLIB_EXPORT_CLASS(velodyne_pointcloud::CloudNodelet, nodelet::Nodelet)
|
||||
@@ -0,0 +1,150 @@
|
||||
/*
|
||||
* Copyright (C) 2009, 2010 Austin Robot Technology, Jack O'Quin
|
||||
* Copyright (C) 2011 Jesse Vera
|
||||
* Copyright (C) 2012 Austin Robot Technology, Jack O'Quin
|
||||
* License: Modified BSD Software License Agreement
|
||||
*
|
||||
* $Id$
|
||||
*/
|
||||
|
||||
/** @file
|
||||
|
||||
This class converts raw Velodyne 3D LIDAR packets to PointCloud2.
|
||||
|
||||
*/
|
||||
|
||||
#include "velodyne_pointcloud/convert.h"
|
||||
|
||||
#include <velodyne_pointcloud/pointcloudXYZIR.h>
|
||||
#include <velodyne_pointcloud/organized_cloudXYZIR.h>
|
||||
|
||||
namespace velodyne_pointcloud
|
||||
{
|
||||
/** @brief Constructor. */
|
||||
Convert::Convert(ros::NodeHandle node, ros::NodeHandle private_nh, std::string const & node_name):
|
||||
data_(new velodyne_rawdata::RawData()), first_rcfg_call(true),
|
||||
diagnostics_(node, private_nh, node_name)
|
||||
{
|
||||
// Get startup parameters
|
||||
private_nh.param<std::string>("fixed_frame", config_.fixed_frame, "velodyne");
|
||||
private_nh.param<std::string>("target_Frame", config_.target_frame, "velodyne");
|
||||
private_nh.param<double>("min_range", config_.min_range, 10.0);
|
||||
private_nh.param<double>("max_range", config_.max_range, 200.0);
|
||||
private_nh.param<bool>("organize_cloud", config_.organize_cloud, false);
|
||||
|
||||
boost::optional<velodyne_pointcloud::Calibration> calibration = data_->setup(private_nh);
|
||||
if(calibration)
|
||||
{
|
||||
ROS_DEBUG_STREAM("Calibration file loaded.");
|
||||
config_.num_lasers = static_cast<uint16_t>(calibration.get().num_lasers);
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR_STREAM("Could not load calibration file!");
|
||||
}
|
||||
|
||||
if(config_.organize_cloud)
|
||||
{
|
||||
container_ptr_ = boost::shared_ptr<OrganizedCloudXYZIR>(
|
||||
new OrganizedCloudXYZIR(config_.max_range, config_.min_range,
|
||||
config_.target_frame, config_.fixed_frame,
|
||||
config_.num_lasers, data_->scansPerPacket()));
|
||||
}
|
||||
else
|
||||
{
|
||||
container_ptr_ = boost::shared_ptr<PointcloudXYZIR>(
|
||||
new PointcloudXYZIR(config_.max_range, config_.min_range,
|
||||
config_.target_frame, config_.fixed_frame,
|
||||
data_->scansPerPacket()));
|
||||
}
|
||||
|
||||
|
||||
// advertise output point cloud (before subscribing to input data)
|
||||
output_ =
|
||||
node.advertise<sensor_msgs::PointCloud2>("velodyne_points", 10);
|
||||
|
||||
srv_ = boost::make_shared <dynamic_reconfigure::Server<velodyne_pointcloud::
|
||||
CloudNodeConfig> > (private_nh);
|
||||
dynamic_reconfigure::Server<velodyne_pointcloud::CloudNodeConfig>::
|
||||
CallbackType f;
|
||||
f = boost::bind (&Convert::callback, this, _1, _2);
|
||||
srv_->setCallback (f);
|
||||
|
||||
// subscribe to VelodyneScan packets
|
||||
velodyne_scan_ =
|
||||
node.subscribe("velodyne_packets", 10,
|
||||
&Convert::processScan, (Convert *) this,
|
||||
ros::TransportHints().tcpNoDelay(true));
|
||||
|
||||
// Diagnostics
|
||||
diagnostics_.setHardwareID("Velodyne Convert");
|
||||
// Arbitrary frequencies since we don't know which RPM is used, and are only
|
||||
// concerned about monitoring the frequency.
|
||||
diag_min_freq_ = 2.0;
|
||||
diag_max_freq_ = 20.0;
|
||||
using namespace diagnostic_updater;
|
||||
diag_topic_.reset(new TopicDiagnostic("velodyne_points", diagnostics_,
|
||||
FrequencyStatusParam(&diag_min_freq_,
|
||||
&diag_max_freq_,
|
||||
0.1, 10),
|
||||
TimeStampStatusParam()));
|
||||
}
|
||||
|
||||
void Convert::callback(velodyne_pointcloud::CloudNodeConfig &config,
|
||||
uint32_t level)
|
||||
{
|
||||
ROS_INFO("Reconfigure Request");
|
||||
data_->setParameters(config.min_range, config.max_range, config.view_direction,
|
||||
config.view_width);
|
||||
config_.fixed_frame = config.fixed_frame;
|
||||
config_.target_frame = config.target_frame;
|
||||
config_.min_range = config.min_range;
|
||||
config_.max_range = config.max_range;
|
||||
|
||||
if(first_rcfg_call || config.organize_cloud != config_.organize_cloud){
|
||||
first_rcfg_call = false;
|
||||
config_.organize_cloud = config.organize_cloud;
|
||||
if(config_.organize_cloud) // TODO only on change
|
||||
{
|
||||
ROS_INFO_STREAM("Using the organized cloud format...");
|
||||
container_ptr_ = boost::shared_ptr<OrganizedCloudXYZIR>(
|
||||
new OrganizedCloudXYZIR(config_.max_range, config_.min_range,
|
||||
config_.target_frame, config_.fixed_frame,
|
||||
config_.num_lasers, data_->scansPerPacket()));
|
||||
}
|
||||
else
|
||||
{
|
||||
container_ptr_ = boost::shared_ptr<PointcloudXYZIR>(
|
||||
new PointcloudXYZIR(config_.max_range, config_.min_range,
|
||||
config_.target_frame, config_.fixed_frame,
|
||||
data_->scansPerPacket()));
|
||||
}
|
||||
}
|
||||
|
||||
container_ptr_->configure(config_.max_range, config_.min_range, config_.fixed_frame, config_.target_frame);
|
||||
|
||||
}
|
||||
|
||||
/** @brief Callback for raw scan messages. */
|
||||
void Convert::processScan(const velodyne_msgs::VelodyneScan::ConstPtr &scanMsg)
|
||||
{
|
||||
if (output_.getNumSubscribers() == 0) // no one listening?
|
||||
return; // avoid much work
|
||||
|
||||
boost::lock_guard<boost::mutex> guard(reconfigure_mtx_);
|
||||
// allocate a point cloud with same time and frame ID as raw data
|
||||
container_ptr_->setup(scanMsg);
|
||||
|
||||
// process each packet provided by the driver
|
||||
for (size_t i = 0; i < scanMsg->packets.size(); ++i)
|
||||
{
|
||||
data_->unpack(scanMsg->packets[i], *container_ptr_, scanMsg->header.stamp);
|
||||
}
|
||||
|
||||
// publish the accumulated cloud message
|
||||
diag_topic_->tick(scanMsg->header.stamp);
|
||||
diagnostics_.update();
|
||||
output_.publish(container_ptr_->finishCloud());
|
||||
}
|
||||
|
||||
} // namespace velodyne_pointcloud
|
||||
@@ -0,0 +1,80 @@
|
||||
|
||||
#include <velodyne_pointcloud/organized_cloudXYZIR.h>
|
||||
|
||||
namespace velodyne_pointcloud
|
||||
{
|
||||
|
||||
OrganizedCloudXYZIR::OrganizedCloudXYZIR(
|
||||
const double max_range, const double min_range,
|
||||
const std::string& target_frame, const std::string& fixed_frame,
|
||||
const unsigned int num_lasers, const unsigned int scans_per_block,
|
||||
boost::shared_ptr<tf::TransformListener> tf_ptr)
|
||||
: DataContainerBase(
|
||||
max_range, min_range, target_frame, fixed_frame,
|
||||
num_lasers, 0, false, scans_per_block, tf_ptr, 6,
|
||||
"x", 1, sensor_msgs::PointField::FLOAT32,
|
||||
"y", 1, sensor_msgs::PointField::FLOAT32,
|
||||
"z", 1, sensor_msgs::PointField::FLOAT32,
|
||||
"intensity", 1, sensor_msgs::PointField::FLOAT32,
|
||||
"ring", 1, sensor_msgs::PointField::UINT16,
|
||||
"time", 1, sensor_msgs::PointField::FLOAT32),
|
||||
iter_x(cloud, "x"), iter_y(cloud, "y"), iter_z(cloud, "z"),
|
||||
iter_intensity(cloud, "intensity"), iter_ring(cloud, "ring"), iter_time(cloud, "time")
|
||||
{
|
||||
}
|
||||
|
||||
void OrganizedCloudXYZIR::newLine()
|
||||
{
|
||||
iter_x = iter_x + config_.init_width;
|
||||
iter_y = iter_y + config_.init_width;
|
||||
iter_z = iter_z + config_.init_width;
|
||||
iter_ring = iter_ring + config_.init_width;
|
||||
iter_intensity = iter_intensity + config_.init_width;
|
||||
iter_time = iter_time + config_.init_width;
|
||||
++cloud.height;
|
||||
}
|
||||
|
||||
void OrganizedCloudXYZIR::setup(const velodyne_msgs::VelodyneScan::ConstPtr& scan_msg){
|
||||
DataContainerBase::setup(scan_msg);
|
||||
iter_x = sensor_msgs::PointCloud2Iterator<float>(cloud, "x");
|
||||
iter_y = sensor_msgs::PointCloud2Iterator<float>(cloud, "y");
|
||||
iter_z = sensor_msgs::PointCloud2Iterator<float>(cloud, "z");
|
||||
iter_intensity = sensor_msgs::PointCloud2Iterator<float>(cloud, "intensity");
|
||||
iter_ring = sensor_msgs::PointCloud2Iterator<uint16_t >(cloud, "ring");
|
||||
iter_time = sensor_msgs::PointCloud2Iterator<float >(cloud, "time");
|
||||
}
|
||||
|
||||
|
||||
void OrganizedCloudXYZIR::addPoint(float x, float y, float z,
|
||||
const uint16_t ring, const uint16_t /*azimuth*/, const float distance, const float intensity, const float time)
|
||||
{
|
||||
/** The laser values are not ordered, the organized structure
|
||||
* needs ordered neighbour points. The right order is defined
|
||||
* by the laser_ring value.
|
||||
* To keep the right ordering, the filtered values are set to
|
||||
* NaN.
|
||||
*/
|
||||
if (pointInRange(distance))
|
||||
{
|
||||
if(config_.transform)
|
||||
transformPoint(x, y, z);
|
||||
|
||||
*(iter_x+ring) = x;
|
||||
*(iter_y+ring) = y;
|
||||
*(iter_z+ring) = z;
|
||||
*(iter_intensity+ring) = intensity;
|
||||
*(iter_ring+ring) = ring;
|
||||
*(iter_time+time) = time;
|
||||
}
|
||||
else
|
||||
{
|
||||
*(iter_x+ring) = nanf("");
|
||||
*(iter_y+ring) = nanf("");
|
||||
*(iter_z+ring) = nanf("");
|
||||
*(iter_intensity+ring) = nanf("");
|
||||
*(iter_ring+ring) = ring;
|
||||
*(iter_time+time) = time;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,64 @@
|
||||
|
||||
|
||||
|
||||
#include <velodyne_pointcloud/pointcloudXYZIR.h>
|
||||
|
||||
namespace velodyne_pointcloud
|
||||
{
|
||||
|
||||
PointcloudXYZIR::PointcloudXYZIR(
|
||||
const double max_range, const double min_range,
|
||||
const std::string& target_frame, const std::string& fixed_frame,
|
||||
const unsigned int scans_per_block, boost::shared_ptr<tf::TransformListener> tf_ptr)
|
||||
: DataContainerBase(
|
||||
max_range, min_range, target_frame, fixed_frame,
|
||||
0, 1, true, scans_per_block, tf_ptr, 6,
|
||||
"x", 1, sensor_msgs::PointField::FLOAT32,
|
||||
"y", 1, sensor_msgs::PointField::FLOAT32,
|
||||
"z", 1, sensor_msgs::PointField::FLOAT32,
|
||||
"intensity", 1, sensor_msgs::PointField::FLOAT32,
|
||||
"ring", 1, sensor_msgs::PointField::UINT16,
|
||||
"time", 1, sensor_msgs::PointField::FLOAT32),
|
||||
iter_x(cloud, "x"), iter_y(cloud, "y"), iter_z(cloud, "z"),
|
||||
iter_ring(cloud, "ring"), iter_intensity(cloud, "intensity"), iter_time(cloud, "time")
|
||||
{};
|
||||
|
||||
void PointcloudXYZIR::setup(const velodyne_msgs::VelodyneScan::ConstPtr& scan_msg){
|
||||
DataContainerBase::setup(scan_msg);
|
||||
iter_x = sensor_msgs::PointCloud2Iterator<float>(cloud, "x");
|
||||
iter_y = sensor_msgs::PointCloud2Iterator<float>(cloud, "y");
|
||||
iter_z = sensor_msgs::PointCloud2Iterator<float>(cloud, "z");
|
||||
iter_intensity = sensor_msgs::PointCloud2Iterator<float>(cloud, "intensity");
|
||||
iter_ring = sensor_msgs::PointCloud2Iterator<uint16_t >(cloud, "ring");
|
||||
iter_time = sensor_msgs::PointCloud2Iterator<float >(cloud, "time");
|
||||
}
|
||||
|
||||
void PointcloudXYZIR::newLine()
|
||||
{}
|
||||
|
||||
void PointcloudXYZIR::addPoint(float x, float y, float z, uint16_t ring, uint16_t /*azimuth*/, float distance, float intensity, float time)
|
||||
{
|
||||
if(!pointInRange(distance)) return;
|
||||
|
||||
// convert polar coordinates to Euclidean XYZ
|
||||
|
||||
if(config_.transform)
|
||||
transformPoint(x, y, z);
|
||||
|
||||
*iter_x = x;
|
||||
*iter_y = y;
|
||||
*iter_z = z;
|
||||
*iter_ring = ring;
|
||||
*iter_intensity = intensity;
|
||||
*iter_time = time;
|
||||
|
||||
++cloud.width;
|
||||
++iter_x;
|
||||
++iter_y;
|
||||
++iter_z;
|
||||
++iter_ring;
|
||||
++iter_intensity;
|
||||
++iter_time;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,158 @@
|
||||
/*
|
||||
* Copyright (C) 2009, 2010 Austin Robot Technology, Jack O'Quin
|
||||
* Copyright (C) 2011 Jesse Vera
|
||||
* Copyright (C) 2012 Austin Robot Technology, Jack O'Quin
|
||||
* License: Modified BSD Software License Agreement
|
||||
*
|
||||
* $Id$
|
||||
*/
|
||||
|
||||
/** @file
|
||||
|
||||
This class transforms raw Velodyne 3D LIDAR packets to PointCloud2
|
||||
in the /map frame of reference.
|
||||
|
||||
@author Jack O'Quin
|
||||
@author Jesse Vera
|
||||
@author Sebastian Pütz
|
||||
|
||||
*/
|
||||
|
||||
#include "velodyne_pointcloud/transform.h"
|
||||
|
||||
#include <velodyne_pointcloud/pointcloudXYZIR.h>
|
||||
#include <velodyne_pointcloud/organized_cloudXYZIR.h>
|
||||
|
||||
namespace velodyne_pointcloud
|
||||
{
|
||||
/** @brief Constructor. */
|
||||
Transform::Transform(ros::NodeHandle node, ros::NodeHandle private_nh, std::string const & node_name):
|
||||
tf_prefix_(tf::getPrefixParam(private_nh)),
|
||||
data_(new velodyne_rawdata::RawData),
|
||||
first_rcfg_call(true),
|
||||
diagnostics_(node, private_nh, node_name)
|
||||
{
|
||||
boost::optional<velodyne_pointcloud::Calibration> calibration = data_->setup(private_nh);
|
||||
if(calibration)
|
||||
{
|
||||
ROS_DEBUG_STREAM("Calibration file loaded.");
|
||||
config_.num_lasers = static_cast<uint16_t>(calibration.get().num_lasers);
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR_STREAM("Could not load calibration file!");
|
||||
}
|
||||
|
||||
config_.target_frame = config_.fixed_frame = "velodyne";
|
||||
tf_ptr_ = boost::make_shared<tf::TransformListener>();
|
||||
|
||||
if(config_.organize_cloud)
|
||||
{
|
||||
container_ptr = boost::shared_ptr<OrganizedCloudXYZIR>(
|
||||
new OrganizedCloudXYZIR(config_.max_range, config_.min_range, config_.target_frame, config_.fixed_frame,
|
||||
config_.num_lasers, data_->scansPerPacket(), tf_ptr_));
|
||||
}
|
||||
else
|
||||
{
|
||||
container_ptr = boost::shared_ptr<PointcloudXYZIR>(
|
||||
new PointcloudXYZIR(config_.max_range, config_.min_range,
|
||||
config_.target_frame, config_.fixed_frame,
|
||||
data_->scansPerPacket(), tf_ptr_));
|
||||
}
|
||||
|
||||
// advertise output point cloud (before subscribing to input data)
|
||||
output_ =
|
||||
node.advertise<sensor_msgs::PointCloud2>("velodyne_points", 10);
|
||||
|
||||
srv_ = boost::make_shared<dynamic_reconfigure::Server<TransformNodeCfg>> (private_nh);
|
||||
dynamic_reconfigure::Server<TransformNodeCfg>::CallbackType f;
|
||||
f = boost::bind (&Transform::reconfigure_callback, this, _1, _2);
|
||||
srv_->setCallback (f);
|
||||
|
||||
// subscribe to VelodyneScan packets using transform filter
|
||||
velodyne_scan_.subscribe(node, "velodyne_packets", 10);
|
||||
tf_filter_ptr_ = boost::shared_ptr<tf::MessageFilter<velodyne_msgs::VelodyneScan> >(
|
||||
new tf::MessageFilter<velodyne_msgs::VelodyneScan>(velodyne_scan_, *tf_ptr_, config_.target_frame, 10));
|
||||
tf_filter_ptr_->registerCallback(boost::bind(&Transform::processScan, this, _1));
|
||||
private_nh.param<std::string>("fixed_frame", config_.fixed_frame, "odom");
|
||||
|
||||
// Diagnostics
|
||||
diagnostics_.setHardwareID("Velodyne Transform");
|
||||
// Arbitrary frequencies since we don't know which RPM is used, and are only
|
||||
// concerned about monitoring the frequency.
|
||||
diag_min_freq_ = 2.0;
|
||||
diag_max_freq_ = 20.0;
|
||||
using namespace diagnostic_updater;
|
||||
diag_topic_.reset(new TopicDiagnostic("velodyne_points", diagnostics_,
|
||||
FrequencyStatusParam(&diag_min_freq_,
|
||||
&diag_max_freq_,
|
||||
0.1, 10),
|
||||
TimeStampStatusParam()));
|
||||
|
||||
}
|
||||
|
||||
void Transform::reconfigure_callback(
|
||||
velodyne_pointcloud::TransformNodeConfig &config, uint32_t level)
|
||||
{
|
||||
ROS_INFO_STREAM("Reconfigure request.");
|
||||
data_->setParameters(config.min_range, config.max_range,
|
||||
config.view_direction, config.view_width);
|
||||
config_.target_frame = tf::resolve(tf_prefix_, config.frame_id);
|
||||
ROS_INFO_STREAM("Target frame ID now: " << config_.target_frame);
|
||||
config_.min_range = config.min_range;
|
||||
config_.max_range = config.max_range;
|
||||
|
||||
boost::lock_guard<boost::mutex> guard(reconfigure_mtx_);
|
||||
|
||||
if(first_rcfg_call || config.organize_cloud != config_.organize_cloud){
|
||||
first_rcfg_call = false;
|
||||
config_.organize_cloud = config.organize_cloud;
|
||||
if(config_.organize_cloud)
|
||||
{
|
||||
ROS_INFO_STREAM("Using the organized cloud format...");
|
||||
container_ptr = boost::shared_ptr<OrganizedCloudXYZIR>(
|
||||
new OrganizedCloudXYZIR(config_.max_range, config_.min_range,
|
||||
config_.target_frame, config_.fixed_frame,
|
||||
config_.num_lasers, data_->scansPerPacket()));
|
||||
}
|
||||
else
|
||||
{
|
||||
container_ptr = boost::shared_ptr<PointcloudXYZIR>(
|
||||
new PointcloudXYZIR(config_.max_range, config_.min_range,
|
||||
config_.target_frame, config_.fixed_frame,
|
||||
data_->scansPerPacket()));
|
||||
}
|
||||
}
|
||||
container_ptr->configure(config_.max_range, config_.min_range, config_.fixed_frame, config_.target_frame);
|
||||
}
|
||||
|
||||
/** @brief Callback for raw scan messages.
|
||||
*
|
||||
* @pre TF message filter has already waited until the transform to
|
||||
* the configured @c frame_id can succeed.
|
||||
*/
|
||||
void
|
||||
Transform::processScan(const velodyne_msgs::VelodyneScan::ConstPtr &scanMsg)
|
||||
{
|
||||
if (output_.getNumSubscribers() == 0) // no one listening?
|
||||
return; // avoid much work
|
||||
|
||||
boost::lock_guard<boost::mutex> guard(reconfigure_mtx_);
|
||||
|
||||
// allocate a point cloud with same time and frame ID as raw data
|
||||
container_ptr->setup(scanMsg);
|
||||
|
||||
// process each packet provided by the driver
|
||||
for (size_t i = 0; i < scanMsg->packets.size(); ++i)
|
||||
{
|
||||
container_ptr->computeTransformation(scanMsg->packets[i].stamp);
|
||||
data_->unpack(scanMsg->packets[i], *container_ptr, scanMsg->header.stamp);
|
||||
}
|
||||
// publish the accumulated cloud message
|
||||
output_.publish(container_ptr->finishCloud());
|
||||
|
||||
diag_topic_->tick(scanMsg->header.stamp);
|
||||
diagnostics_.update();
|
||||
}
|
||||
|
||||
} // namespace velodyne_pointcloud
|
||||
@@ -0,0 +1,31 @@
|
||||
/*
|
||||
* Copyright (C) 2012 Austin Robot Technology, Jack O'Quin
|
||||
* License: Modified BSD Software License Agreement
|
||||
*
|
||||
* $Id$
|
||||
*/
|
||||
|
||||
/** \file
|
||||
|
||||
This ROS node transforms raw Velodyne LIDAR packets to PointCloud2
|
||||
in the /map frame of reference.
|
||||
|
||||
*/
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include "velodyne_pointcloud/transform.h"
|
||||
|
||||
/** Main node entry point. */
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "transform_node");
|
||||
|
||||
// create conversion class, which subscribes to raw data
|
||||
velodyne_pointcloud::Transform transform(ros::NodeHandle(),
|
||||
ros::NodeHandle("~"));
|
||||
|
||||
// handle callbacks until shut down
|
||||
ros::spin();
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,48 @@
|
||||
/*
|
||||
* Copyright (C) 2012 Austin Robot Technology, Jack O'Quin
|
||||
* License: Modified BSD Software License Agreement
|
||||
*
|
||||
* $Id$
|
||||
*/
|
||||
|
||||
/** @file
|
||||
|
||||
This ROS nodelet transforms raw Velodyne 3D LIDAR packets to a
|
||||
PointCloud2 in the /map frame.
|
||||
|
||||
*/
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include <nodelet/nodelet.h>
|
||||
|
||||
#include "velodyne_pointcloud/transform.h"
|
||||
|
||||
namespace velodyne_pointcloud
|
||||
{
|
||||
class TransformNodelet: public nodelet::Nodelet
|
||||
{
|
||||
public:
|
||||
|
||||
TransformNodelet() {}
|
||||
~TransformNodelet() {}
|
||||
|
||||
private:
|
||||
|
||||
virtual void onInit();
|
||||
boost::shared_ptr<Transform> tf_;
|
||||
};
|
||||
|
||||
/** @brief Nodelet initialization. */
|
||||
void TransformNodelet::onInit()
|
||||
{
|
||||
tf_.reset(new Transform(getNodeHandle(), getPrivateNodeHandle(), getName()));
|
||||
}
|
||||
|
||||
} // namespace velodyne_pointcloud
|
||||
|
||||
|
||||
// Register this plugin with pluginlib. Names must match nodelets.xml.
|
||||
//
|
||||
// parameters: class type, base class type
|
||||
PLUGINLIB_EXPORT_CLASS(velodyne_pointcloud::TransformNodelet, nodelet::Nodelet)
|
||||
@@ -0,0 +1,8 @@
|
||||
add_library(velodyne_rawdata rawdata.cc calibration.cc)
|
||||
target_link_libraries(velodyne_rawdata
|
||||
${catkin_LIBRARIES}
|
||||
${YAML_CPP_LIBRARIES})
|
||||
install(TARGETS velodyne_rawdata
|
||||
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
|
||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
|
||||
@@ -0,0 +1,283 @@
|
||||
/**
|
||||
* \file calibration.cc
|
||||
* \brief
|
||||
*
|
||||
* \author Piyush Khandelwal (piyushk@cs.utexas.edu)
|
||||
* Copyright (C) 2012, Austin Robot Technology,
|
||||
* The University of Texas at Austin
|
||||
*
|
||||
* License: Modified BSD License
|
||||
*
|
||||
* $ Id: 02/14/2012 11:36:36 AM piyushk $
|
||||
*/
|
||||
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
#include <string>
|
||||
#include <cmath>
|
||||
#include <limits>
|
||||
#include <yaml-cpp/yaml.h>
|
||||
|
||||
#ifdef HAVE_NEW_YAMLCPP
|
||||
namespace YAML {
|
||||
|
||||
// The >> operator disappeared in yaml-cpp 0.5, so this function is
|
||||
// added to provide support for code written under the yaml-cpp 0.3 API.
|
||||
template<typename T>
|
||||
void operator >> (const YAML::Node& node, T& i) {
|
||||
i = node.as<T>();
|
||||
}
|
||||
} /* YAML */
|
||||
#endif // HAVE_NEW_YAMLCPP
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <velodyne_pointcloud/calibration.h>
|
||||
|
||||
namespace velodyne_pointcloud
|
||||
{
|
||||
|
||||
const std::string NUM_LASERS = "num_lasers";
|
||||
const std::string DISTANCE_RESOLUTION = "distance_resolution";
|
||||
const std::string LASERS = "lasers";
|
||||
const std::string LASER_ID = "laser_id";
|
||||
const std::string ROT_CORRECTION = "rot_correction";
|
||||
const std::string VERT_CORRECTION = "vert_correction";
|
||||
const std::string DIST_CORRECTION = "dist_correction";
|
||||
const std::string TWO_PT_CORRECTION_AVAILABLE =
|
||||
"two_pt_correction_available";
|
||||
const std::string DIST_CORRECTION_X = "dist_correction_x";
|
||||
const std::string DIST_CORRECTION_Y = "dist_correction_y";
|
||||
const std::string VERT_OFFSET_CORRECTION = "vert_offset_correction";
|
||||
const std::string HORIZ_OFFSET_CORRECTION = "horiz_offset_correction";
|
||||
const std::string MAX_INTENSITY = "max_intensity";
|
||||
const std::string MIN_INTENSITY = "min_intensity";
|
||||
const std::string FOCAL_DISTANCE = "focal_distance";
|
||||
const std::string FOCAL_SLOPE = "focal_slope";
|
||||
|
||||
/** Read calibration for a single laser. */
|
||||
void operator >> (const YAML::Node& node,
|
||||
std::pair<int, LaserCorrection>& correction)
|
||||
{
|
||||
node[LASER_ID] >> correction.first;
|
||||
node[ROT_CORRECTION] >> correction.second.rot_correction;
|
||||
node[VERT_CORRECTION] >> correction.second.vert_correction;
|
||||
node[DIST_CORRECTION] >> correction.second.dist_correction;
|
||||
#ifdef HAVE_NEW_YAMLCPP
|
||||
if (node[TWO_PT_CORRECTION_AVAILABLE])
|
||||
node[TWO_PT_CORRECTION_AVAILABLE] >>
|
||||
correction.second.two_pt_correction_available;
|
||||
#else
|
||||
if (const YAML::Node *pName = node.FindValue(TWO_PT_CORRECTION_AVAILABLE))
|
||||
*pName >> correction.second.two_pt_correction_available;
|
||||
#endif
|
||||
else
|
||||
correction.second.two_pt_correction_available = false;
|
||||
node[DIST_CORRECTION_X] >> correction.second.dist_correction_x;
|
||||
node[DIST_CORRECTION_Y] >> correction.second.dist_correction_y;
|
||||
node[VERT_OFFSET_CORRECTION] >> correction.second.vert_offset_correction;
|
||||
#ifdef HAVE_NEW_YAMLCPP
|
||||
if (node[HORIZ_OFFSET_CORRECTION])
|
||||
node[HORIZ_OFFSET_CORRECTION] >>
|
||||
correction.second.horiz_offset_correction;
|
||||
#else
|
||||
if (const YAML::Node *pName = node.FindValue(HORIZ_OFFSET_CORRECTION))
|
||||
*pName >> correction.second.horiz_offset_correction;
|
||||
#endif
|
||||
else
|
||||
correction.second.horiz_offset_correction = 0;
|
||||
|
||||
const YAML::Node * max_intensity_node = NULL;
|
||||
#ifdef HAVE_NEW_YAMLCPP
|
||||
if (node[MAX_INTENSITY]) {
|
||||
const YAML::Node max_intensity_node_ref = node[MAX_INTENSITY];
|
||||
max_intensity_node = &max_intensity_node_ref;
|
||||
}
|
||||
#else
|
||||
if (const YAML::Node *pName = node.FindValue(MAX_INTENSITY))
|
||||
max_intensity_node = pName;
|
||||
#endif
|
||||
if (max_intensity_node) {
|
||||
float max_intensity_float;
|
||||
*max_intensity_node >> max_intensity_float;
|
||||
correction.second.max_intensity = floor(max_intensity_float);
|
||||
}
|
||||
else {
|
||||
correction.second.max_intensity = 255;
|
||||
}
|
||||
|
||||
const YAML::Node * min_intensity_node = NULL;
|
||||
#ifdef HAVE_NEW_YAMLCPP
|
||||
if (node[MIN_INTENSITY]) {
|
||||
const YAML::Node min_intensity_node_ref = node[MIN_INTENSITY];
|
||||
min_intensity_node = &min_intensity_node_ref;
|
||||
}
|
||||
#else
|
||||
if (const YAML::Node *pName = node.FindValue(MIN_INTENSITY))
|
||||
min_intensity_node = pName;
|
||||
#endif
|
||||
if (min_intensity_node) {
|
||||
float min_intensity_float;
|
||||
*min_intensity_node >> min_intensity_float;
|
||||
correction.second.min_intensity = floor(min_intensity_float);
|
||||
}
|
||||
else {
|
||||
correction.second.min_intensity = 0;
|
||||
}
|
||||
node[FOCAL_DISTANCE] >> correction.second.focal_distance;
|
||||
node[FOCAL_SLOPE] >> correction.second.focal_slope;
|
||||
|
||||
// Calculate cached values
|
||||
correction.second.cos_rot_correction =
|
||||
cosf(correction.second.rot_correction);
|
||||
correction.second.sin_rot_correction =
|
||||
sinf(correction.second.rot_correction);
|
||||
correction.second.cos_vert_correction =
|
||||
cosf(correction.second.vert_correction);
|
||||
correction.second.sin_vert_correction =
|
||||
sinf(correction.second.vert_correction);
|
||||
|
||||
correction.second.laser_ring = 0; // clear initially (set later)
|
||||
}
|
||||
|
||||
/** Read entire calibration file. */
|
||||
void operator >> (const YAML::Node& node, Calibration& calibration)
|
||||
{
|
||||
int num_lasers;
|
||||
node[NUM_LASERS] >> num_lasers;
|
||||
float distance_resolution_m;
|
||||
node[DISTANCE_RESOLUTION] >> distance_resolution_m;
|
||||
const YAML::Node& lasers = node[LASERS];
|
||||
calibration.laser_corrections.clear();
|
||||
calibration.num_lasers = num_lasers;
|
||||
calibration.distance_resolution_m = distance_resolution_m;
|
||||
calibration.laser_corrections.resize(num_lasers);
|
||||
for (int i = 0; i < num_lasers; i++) {
|
||||
std::pair<int, LaserCorrection> correction;
|
||||
lasers[i] >> correction;
|
||||
const int index = correction.first;
|
||||
if( index >= calibration.laser_corrections.size() )
|
||||
{
|
||||
calibration.laser_corrections.resize( index+1 );
|
||||
}
|
||||
calibration.laser_corrections[index] = (correction.second);
|
||||
calibration.laser_corrections_map.insert(correction);
|
||||
}
|
||||
|
||||
// For each laser ring, find the next-smallest vertical angle.
|
||||
//
|
||||
// This implementation is simple, but not efficient. That is OK,
|
||||
// since it only runs while starting up.
|
||||
double next_angle = -std::numeric_limits<double>::infinity();
|
||||
for (int ring = 0; ring < num_lasers; ++ring) {
|
||||
|
||||
// find minimum remaining vertical offset correction
|
||||
double min_seen = std::numeric_limits<double>::infinity();
|
||||
int next_index = num_lasers;
|
||||
for (int j = 0; j < num_lasers; ++j) {
|
||||
|
||||
double angle = calibration.laser_corrections[j].vert_correction;
|
||||
if (next_angle < angle && angle < min_seen) {
|
||||
min_seen = angle;
|
||||
next_index = j;
|
||||
}
|
||||
}
|
||||
|
||||
if (next_index < num_lasers) { // anything found in this ring?
|
||||
|
||||
// store this ring number with its corresponding laser number
|
||||
calibration.laser_corrections[next_index].laser_ring = ring;
|
||||
next_angle = min_seen;
|
||||
if (calibration.ros_info) {
|
||||
ROS_INFO("laser_ring[%2u] = %2u, angle = %+.6f",
|
||||
next_index, ring, next_angle);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
YAML::Emitter& operator << (YAML::Emitter& out,
|
||||
const std::pair<int, LaserCorrection> correction)
|
||||
{
|
||||
out << YAML::BeginMap;
|
||||
out << YAML::Key << LASER_ID << YAML::Value << correction.first;
|
||||
out << YAML::Key << ROT_CORRECTION <<
|
||||
YAML::Value << correction.second.rot_correction;
|
||||
out << YAML::Key << VERT_CORRECTION <<
|
||||
YAML::Value << correction.second.vert_correction;
|
||||
out << YAML::Key << DIST_CORRECTION <<
|
||||
YAML::Value << correction.second.dist_correction;
|
||||
out << YAML::Key << TWO_PT_CORRECTION_AVAILABLE <<
|
||||
YAML::Value << correction.second.two_pt_correction_available;
|
||||
out << YAML::Key << DIST_CORRECTION_X <<
|
||||
YAML::Value << correction.second.dist_correction_x;
|
||||
out << YAML::Key << DIST_CORRECTION_Y <<
|
||||
YAML::Value << correction.second.dist_correction_y;
|
||||
out << YAML::Key << VERT_OFFSET_CORRECTION <<
|
||||
YAML::Value << correction.second.vert_offset_correction;
|
||||
out << YAML::Key << HORIZ_OFFSET_CORRECTION <<
|
||||
YAML::Value << correction.second.horiz_offset_correction;
|
||||
out << YAML::Key << MAX_INTENSITY <<
|
||||
YAML::Value << correction.second.max_intensity;
|
||||
out << YAML::Key << MIN_INTENSITY <<
|
||||
YAML::Value << correction.second.min_intensity;
|
||||
out << YAML::Key << FOCAL_DISTANCE <<
|
||||
YAML::Value << correction.second.focal_distance;
|
||||
out << YAML::Key << FOCAL_SLOPE <<
|
||||
YAML::Value << correction.second.focal_slope;
|
||||
out << YAML::EndMap;
|
||||
return out;
|
||||
}
|
||||
|
||||
YAML::Emitter& operator <<
|
||||
(YAML::Emitter& out, const Calibration& calibration)
|
||||
{
|
||||
out << YAML::BeginMap;
|
||||
out << YAML::Key << NUM_LASERS <<
|
||||
YAML::Value << calibration.laser_corrections.size();
|
||||
out << YAML::Key << DISTANCE_RESOLUTION <<
|
||||
YAML::Value << calibration.distance_resolution_m;
|
||||
out << YAML::Key << LASERS << YAML::Value << YAML::BeginSeq;
|
||||
for (std::map<int, LaserCorrection>::const_iterator
|
||||
it = calibration.laser_corrections_map.begin();
|
||||
it != calibration.laser_corrections_map.end(); it++)
|
||||
{
|
||||
out << *it;
|
||||
}
|
||||
out << YAML::EndSeq;
|
||||
out << YAML::EndMap;
|
||||
return out;
|
||||
}
|
||||
|
||||
void Calibration::read(const std::string& calibration_file) {
|
||||
std::ifstream fin(calibration_file.c_str());
|
||||
if (!fin.is_open()) {
|
||||
initialized = false;
|
||||
return;
|
||||
}
|
||||
initialized = true;
|
||||
try {
|
||||
YAML::Node doc;
|
||||
#ifdef HAVE_NEW_YAMLCPP
|
||||
fin.close();
|
||||
doc = YAML::LoadFile(calibration_file);
|
||||
#else
|
||||
YAML::Parser parser(fin);
|
||||
parser.GetNextDocument(doc);
|
||||
#endif
|
||||
doc >> *this;
|
||||
} catch (YAML::Exception &e) {
|
||||
std::cerr << "YAML Exception: " << e.what() << std::endl;
|
||||
initialized = false;
|
||||
}
|
||||
fin.close();
|
||||
}
|
||||
|
||||
void Calibration::write(const std::string& calibration_file) {
|
||||
std::ofstream fout(calibration_file.c_str());
|
||||
YAML::Emitter out;
|
||||
out << *this;
|
||||
fout << out.c_str();
|
||||
fout.close();
|
||||
}
|
||||
|
||||
} /* velodyne_pointcloud */
|
||||
@@ -0,0 +1,620 @@
|
||||
/*
|
||||
* Copyright (C) 2007 Austin Robot Technology, Patrick Beeson
|
||||
* Copyright (C) 2009, 2010, 2012 Austin Robot Technology, Jack O'Quin
|
||||
* Copyright (C) 2019, Kaarta Inc, Shawn Hanna
|
||||
*
|
||||
* License: Modified BSD Software License Agreement
|
||||
*
|
||||
* $Id$
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file
|
||||
*
|
||||
* Velodyne 3D LIDAR data accessor class implementation.
|
||||
*
|
||||
* Class for unpacking raw Velodyne LIDAR packets into useful
|
||||
* formats.
|
||||
*
|
||||
* Derived classes accept raw Velodyne data for either single packets
|
||||
* or entire rotations, and provide it in various formats for either
|
||||
* on-line or off-line processing.
|
||||
*
|
||||
* @author Patrick Beeson
|
||||
* @author Jack O'Quin
|
||||
* @author Shawn Hanna
|
||||
*
|
||||
* HDL-64E S2 calibration support provided by Nick Hillier
|
||||
*/
|
||||
|
||||
#include <fstream>
|
||||
#include <math.h>
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <ros/package.h>
|
||||
#include <angles/angles.h>
|
||||
|
||||
#include <velodyne_pointcloud/rawdata.h>
|
||||
|
||||
namespace velodyne_rawdata
|
||||
{
|
||||
inline float SQR(float val) { return val*val; }
|
||||
|
||||
////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
// RawData base class implementation
|
||||
//
|
||||
////////////////////////////////////////////////////////////////////////
|
||||
|
||||
RawData::RawData() {}
|
||||
|
||||
/** Update parameters: conversions and update */
|
||||
void RawData::setParameters(double min_range,
|
||||
double max_range,
|
||||
double view_direction,
|
||||
double view_width)
|
||||
{
|
||||
config_.min_range = min_range;
|
||||
config_.max_range = max_range;
|
||||
|
||||
//converting angle parameters into the velodyne reference (rad)
|
||||
config_.tmp_min_angle = view_direction + view_width/2;
|
||||
config_.tmp_max_angle = view_direction - view_width/2;
|
||||
|
||||
//computing positive modulo to keep theses angles into [0;2*M_PI]
|
||||
config_.tmp_min_angle = fmod(fmod(config_.tmp_min_angle,2*M_PI) + 2*M_PI,2*M_PI);
|
||||
config_.tmp_max_angle = fmod(fmod(config_.tmp_max_angle,2*M_PI) + 2*M_PI,2*M_PI);
|
||||
|
||||
//converting into the hardware velodyne ref (negative yaml and degrees)
|
||||
//adding 0.5 perfomrs a centered double to int conversion
|
||||
config_.min_angle = 100 * (2*M_PI - config_.tmp_min_angle) * 180 / M_PI + 0.5;
|
||||
config_.max_angle = 100 * (2*M_PI - config_.tmp_max_angle) * 180 / M_PI + 0.5;
|
||||
if (config_.min_angle == config_.max_angle)
|
||||
{
|
||||
//avoid returning empty cloud if min_angle = max_angle
|
||||
config_.min_angle = 0;
|
||||
config_.max_angle = 36000;
|
||||
}
|
||||
}
|
||||
|
||||
int RawData::scansPerPacket() const
|
||||
{
|
||||
if( calibration_.num_lasers == 16)
|
||||
{
|
||||
return BLOCKS_PER_PACKET * VLP16_FIRINGS_PER_BLOCK *
|
||||
VLP16_SCANS_PER_FIRING;
|
||||
}
|
||||
else{
|
||||
return BLOCKS_PER_PACKET * SCANS_PER_BLOCK;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Build a timing table for each block/firing. Stores in timing_offsets vector
|
||||
*/
|
||||
bool RawData::buildTimings(){
|
||||
// vlp16
|
||||
if (config_.model == "VLP16"){
|
||||
// timing table calculation, from velodyne user manual
|
||||
timing_offsets.resize(12);
|
||||
for (size_t i=0; i < timing_offsets.size(); ++i){
|
||||
timing_offsets[i].resize(32);
|
||||
}
|
||||
// constants
|
||||
double full_firing_cycle = 55.296 * 1e-6; // seconds
|
||||
double single_firing = 2.304 * 1e-6; // seconds
|
||||
double dataBlockIndex, dataPointIndex;
|
||||
bool dual_mode = false;
|
||||
// compute timing offsets
|
||||
for (size_t x = 0; x < timing_offsets.size(); ++x){
|
||||
for (size_t y = 0; y < timing_offsets[x].size(); ++y){
|
||||
if (dual_mode){
|
||||
dataBlockIndex = (x - (x % 2)) + (y / 16);
|
||||
}
|
||||
else{
|
||||
dataBlockIndex = (x * 2) + (y / 16);
|
||||
}
|
||||
dataPointIndex = y % 16;
|
||||
//timing_offsets[block][firing]
|
||||
timing_offsets[x][y] = (full_firing_cycle * dataBlockIndex) + (single_firing * dataPointIndex);
|
||||
}
|
||||
}
|
||||
}
|
||||
// vlp32
|
||||
else if (config_.model == "32C"){
|
||||
// timing table calculation, from velodyne user manual
|
||||
timing_offsets.resize(12);
|
||||
for (size_t i=0; i < timing_offsets.size(); ++i){
|
||||
timing_offsets[i].resize(32);
|
||||
}
|
||||
// constants
|
||||
double full_firing_cycle = 55.296 * 1e-6; // seconds
|
||||
double single_firing = 2.304 * 1e-6; // seconds
|
||||
double dataBlockIndex, dataPointIndex;
|
||||
bool dual_mode = false;
|
||||
// compute timing offsets
|
||||
for (size_t x = 0; x < timing_offsets.size(); ++x){
|
||||
for (size_t y = 0; y < timing_offsets[x].size(); ++y){
|
||||
if (dual_mode){
|
||||
dataBlockIndex = x / 2;
|
||||
}
|
||||
else{
|
||||
dataBlockIndex = x;
|
||||
}
|
||||
dataPointIndex = y / 2;
|
||||
timing_offsets[x][y] = (full_firing_cycle * dataBlockIndex) + (single_firing * dataPointIndex);
|
||||
}
|
||||
}
|
||||
}
|
||||
// hdl32
|
||||
else if (config_.model == "32E"){
|
||||
// timing table calculation, from velodyne user manual
|
||||
timing_offsets.resize(12);
|
||||
for (size_t i=0; i < timing_offsets.size(); ++i){
|
||||
timing_offsets[i].resize(32);
|
||||
}
|
||||
// constants
|
||||
double full_firing_cycle = 46.080 * 1e-6; // seconds
|
||||
double single_firing = 1.152 * 1e-6; // seconds
|
||||
double dataBlockIndex, dataPointIndex;
|
||||
bool dual_mode = false;
|
||||
// compute timing offsets
|
||||
for (size_t x = 0; x < timing_offsets.size(); ++x){
|
||||
for (size_t y = 0; y < timing_offsets[x].size(); ++y){
|
||||
if (dual_mode){
|
||||
dataBlockIndex = x / 2;
|
||||
}
|
||||
else{
|
||||
dataBlockIndex = x;
|
||||
}
|
||||
dataPointIndex = y / 2;
|
||||
timing_offsets[x][y] = (full_firing_cycle * dataBlockIndex) + (single_firing * dataPointIndex);
|
||||
}
|
||||
}
|
||||
}
|
||||
else{
|
||||
timing_offsets.clear();
|
||||
ROS_WARN("Timings not supported for model %s", config_.model.c_str());
|
||||
}
|
||||
|
||||
if (timing_offsets.size()){
|
||||
// ROS_INFO("VELODYNE TIMING TABLE:");
|
||||
for (size_t x = 0; x < timing_offsets.size(); ++x){
|
||||
for (size_t y = 0; y < timing_offsets[x].size(); ++y){
|
||||
printf("%04.3f ", timing_offsets[x][y] * 1e6);
|
||||
}
|
||||
printf("\n");
|
||||
}
|
||||
return true;
|
||||
}
|
||||
else{
|
||||
ROS_WARN("NO TIMING OFFSETS CALCULATED. ARE YOU USING A SUPPORTED VELODYNE SENSOR?");
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/** Set up for on-line operation. */
|
||||
boost::optional<velodyne_pointcloud::Calibration> RawData::setup(ros::NodeHandle private_nh)
|
||||
{
|
||||
private_nh.param("model", config_.model, std::string("64E"));
|
||||
buildTimings();
|
||||
|
||||
// get path to angles.config file for this device
|
||||
if (!private_nh.getParam("calibration", config_.calibrationFile))
|
||||
{
|
||||
ROS_ERROR_STREAM("No calibration angles specified! Using test values!");
|
||||
|
||||
// have to use something: grab unit test version as a default
|
||||
std::string pkgPath = ros::package::getPath("velodyne_pointcloud");
|
||||
config_.calibrationFile = pkgPath + "/params/64e_utexas.yaml";
|
||||
}
|
||||
|
||||
ROS_INFO_STREAM("correction angles: " << config_.calibrationFile);
|
||||
|
||||
calibration_.read(config_.calibrationFile);
|
||||
if (!calibration_.initialized) {
|
||||
ROS_ERROR_STREAM("Unable to open calibration file: " <<
|
||||
config_.calibrationFile);
|
||||
return boost::none;
|
||||
}
|
||||
|
||||
ROS_INFO_STREAM("Number of lasers: " << calibration_.num_lasers << ".");
|
||||
|
||||
// Set up cached values for sin and cos of all the possible headings
|
||||
for (uint16_t rot_index = 0; rot_index < ROTATION_MAX_UNITS; ++rot_index) {
|
||||
float rotation = angles::from_degrees(ROTATION_RESOLUTION * rot_index);
|
||||
cos_rot_table_[rot_index] = cosf(rotation);
|
||||
sin_rot_table_[rot_index] = sinf(rotation);
|
||||
}
|
||||
return calibration_;
|
||||
}
|
||||
|
||||
/** Set up for offline operation */
|
||||
int RawData::setupOffline(std::string calibration_file, double max_range_, double min_range_)
|
||||
{
|
||||
|
||||
config_.max_range = max_range_;
|
||||
config_.min_range = min_range_;
|
||||
ROS_INFO_STREAM("data ranges to publish: ["
|
||||
<< config_.min_range << ", "
|
||||
<< config_.max_range << "]");
|
||||
|
||||
config_.calibrationFile = calibration_file;
|
||||
|
||||
ROS_INFO_STREAM("correction angles: " << config_.calibrationFile);
|
||||
|
||||
calibration_.read(config_.calibrationFile);
|
||||
if (!calibration_.initialized) {
|
||||
ROS_ERROR_STREAM("Unable to open calibration file: " << config_.calibrationFile);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Set up cached values for sin and cos of all the possible headings
|
||||
for (uint16_t rot_index = 0; rot_index < ROTATION_MAX_UNITS; ++rot_index) {
|
||||
float rotation = angles::from_degrees(ROTATION_RESOLUTION * rot_index);
|
||||
cos_rot_table_[rot_index] = cosf(rotation);
|
||||
sin_rot_table_[rot_index] = sinf(rotation);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
/** @brief convert raw packet to point cloud
|
||||
*
|
||||
* @param pkt raw packet to unpack
|
||||
* @param pc shared pointer to point cloud (points are appended)
|
||||
*/
|
||||
void RawData::unpack(const velodyne_msgs::VelodynePacket &pkt, DataContainerBase& data, const ros::Time& scan_start_time)
|
||||
{
|
||||
using velodyne_pointcloud::LaserCorrection;
|
||||
ROS_DEBUG_STREAM("Received packet, time: " << pkt.stamp);
|
||||
|
||||
/** special parsing for the VLP16 **/
|
||||
if (calibration_.num_lasers == 16)
|
||||
{
|
||||
unpack_vlp16(pkt, data, scan_start_time);
|
||||
return;
|
||||
}
|
||||
|
||||
float time_diff_start_to_this_packet = (pkt.stamp - scan_start_time).toSec();
|
||||
|
||||
const raw_packet_t *raw = (const raw_packet_t *) &pkt.data[0];
|
||||
|
||||
for (int i = 0; i < BLOCKS_PER_PACKET; i++) {
|
||||
|
||||
// upper bank lasers are numbered [0..31]
|
||||
// NOTE: this is a change from the old velodyne_common implementation
|
||||
|
||||
int bank_origin = 0;
|
||||
if (raw->blocks[i].header == LOWER_BANK) {
|
||||
// lower bank lasers are [32..63]
|
||||
bank_origin = 32;
|
||||
}
|
||||
|
||||
for (int j = 0, k = 0; j < SCANS_PER_BLOCK; j++, k += RAW_SCAN_SIZE) {
|
||||
|
||||
float x, y, z;
|
||||
float intensity;
|
||||
const uint8_t laser_number = j + bank_origin;
|
||||
float time = 0;
|
||||
|
||||
const LaserCorrection &corrections = calibration_.laser_corrections[laser_number];
|
||||
|
||||
/** Position Calculation */
|
||||
const raw_block_t &block = raw->blocks[i];
|
||||
union two_bytes tmp;
|
||||
tmp.bytes[0] = block.data[k];
|
||||
tmp.bytes[1] = block.data[k+1];
|
||||
|
||||
/*condition added to avoid calculating points which are not
|
||||
in the interesting defined area (min_angle < area < max_angle)*/
|
||||
if ((block.rotation >= config_.min_angle
|
||||
&& block.rotation <= config_.max_angle
|
||||
&& config_.min_angle < config_.max_angle)
|
||||
||(config_.min_angle > config_.max_angle
|
||||
&& (raw->blocks[i].rotation <= config_.max_angle
|
||||
|| raw->blocks[i].rotation >= config_.min_angle))){
|
||||
|
||||
if (timing_offsets.size())
|
||||
{
|
||||
time = timing_offsets[i][j] + time_diff_start_to_this_packet;
|
||||
}
|
||||
|
||||
if (tmp.uint == 0) // no valid laser beam return
|
||||
{
|
||||
// call to addPoint is still required since output could be organized
|
||||
data.addPoint(nanf(""), nanf(""), nanf(""), corrections.laser_ring, raw->blocks[i].rotation, nanf(""), nanf(""), time);
|
||||
continue;
|
||||
}
|
||||
|
||||
float distance = tmp.uint * calibration_.distance_resolution_m;
|
||||
distance += corrections.dist_correction;
|
||||
|
||||
float cos_vert_angle = corrections.cos_vert_correction;
|
||||
float sin_vert_angle = corrections.sin_vert_correction;
|
||||
float cos_rot_correction = corrections.cos_rot_correction;
|
||||
float sin_rot_correction = corrections.sin_rot_correction;
|
||||
|
||||
// cos(a-b) = cos(a)*cos(b) + sin(a)*sin(b)
|
||||
// sin(a-b) = sin(a)*cos(b) - cos(a)*sin(b)
|
||||
float cos_rot_angle =
|
||||
cos_rot_table_[block.rotation] * cos_rot_correction +
|
||||
sin_rot_table_[block.rotation] * sin_rot_correction;
|
||||
float sin_rot_angle =
|
||||
sin_rot_table_[block.rotation] * cos_rot_correction -
|
||||
cos_rot_table_[block.rotation] * sin_rot_correction;
|
||||
|
||||
float horiz_offset = corrections.horiz_offset_correction;
|
||||
float vert_offset = corrections.vert_offset_correction;
|
||||
|
||||
// Compute the distance in the xy plane (w/o accounting for rotation)
|
||||
/**the new term of 'vert_offset * sin_vert_angle'
|
||||
* was added to the expression due to the mathemathical
|
||||
* model we used.
|
||||
*/
|
||||
float xy_distance = distance * cos_vert_angle - vert_offset * sin_vert_angle;
|
||||
|
||||
// Calculate temporal X, use absolute value.
|
||||
float xx = xy_distance * sin_rot_angle - horiz_offset * cos_rot_angle;
|
||||
// Calculate temporal Y, use absolute value
|
||||
float yy = xy_distance * cos_rot_angle + horiz_offset * sin_rot_angle;
|
||||
if (xx < 0) xx=-xx;
|
||||
if (yy < 0) yy=-yy;
|
||||
|
||||
// Get 2points calibration values,Linear interpolation to get distance
|
||||
// correction for X and Y, that means distance correction use
|
||||
// different value at different distance
|
||||
float distance_corr_x = 0;
|
||||
float distance_corr_y = 0;
|
||||
if (corrections.two_pt_correction_available) {
|
||||
distance_corr_x =
|
||||
(corrections.dist_correction - corrections.dist_correction_x)
|
||||
* (xx - 2.4) / (25.04 - 2.4)
|
||||
+ corrections.dist_correction_x;
|
||||
distance_corr_x -= corrections.dist_correction;
|
||||
distance_corr_y =
|
||||
(corrections.dist_correction - corrections.dist_correction_y)
|
||||
* (yy - 1.93) / (25.04 - 1.93)
|
||||
+ corrections.dist_correction_y;
|
||||
distance_corr_y -= corrections.dist_correction;
|
||||
}
|
||||
|
||||
float distance_x = distance + distance_corr_x;
|
||||
/**the new term of 'vert_offset * sin_vert_angle'
|
||||
* was added to the expression due to the mathemathical
|
||||
* model we used.
|
||||
*/
|
||||
xy_distance = distance_x * cos_vert_angle - vert_offset * sin_vert_angle ;
|
||||
///the expression wiht '-' is proved to be better than the one with '+'
|
||||
x = xy_distance * sin_rot_angle - horiz_offset * cos_rot_angle;
|
||||
|
||||
float distance_y = distance + distance_corr_y;
|
||||
xy_distance = distance_y * cos_vert_angle - vert_offset * sin_vert_angle ;
|
||||
/**the new term of 'vert_offset * sin_vert_angle'
|
||||
* was added to the expression due to the mathemathical
|
||||
* model we used.
|
||||
*/
|
||||
y = xy_distance * cos_rot_angle + horiz_offset * sin_rot_angle;
|
||||
|
||||
// Using distance_y is not symmetric, but the velodyne manual
|
||||
// does this.
|
||||
/**the new term of 'vert_offset * cos_vert_angle'
|
||||
* was added to the expression due to the mathemathical
|
||||
* model we used.
|
||||
*/
|
||||
z = distance_y * sin_vert_angle + vert_offset*cos_vert_angle;
|
||||
|
||||
/** Use standard ROS coordinate system (right-hand rule) */
|
||||
float x_coord = y;
|
||||
float y_coord = -x;
|
||||
float z_coord = z;
|
||||
|
||||
/** Intensity Calculation */
|
||||
|
||||
float min_intensity = corrections.min_intensity;
|
||||
float max_intensity = corrections.max_intensity;
|
||||
|
||||
intensity = raw->blocks[i].data[k+2];
|
||||
|
||||
float focal_offset = 256
|
||||
* (1 - corrections.focal_distance / 13100)
|
||||
* (1 - corrections.focal_distance / 13100);
|
||||
float focal_slope = corrections.focal_slope;
|
||||
intensity += focal_slope * (std::abs(focal_offset - 256 *
|
||||
SQR(1 - static_cast<float>(tmp.uint)/65535)));
|
||||
intensity = (intensity < min_intensity) ? min_intensity : intensity;
|
||||
intensity = (intensity > max_intensity) ? max_intensity : intensity;
|
||||
|
||||
data.addPoint(x_coord, y_coord, z_coord, corrections.laser_ring, raw->blocks[i].rotation, distance, intensity, time);
|
||||
}
|
||||
}
|
||||
data.newLine();
|
||||
}
|
||||
}
|
||||
|
||||
/** @brief convert raw VLP16 packet to point cloud
|
||||
*
|
||||
* @param pkt raw packet to unpack
|
||||
* @param pc shared pointer to point cloud (points are appended)
|
||||
*/
|
||||
void RawData::unpack_vlp16(const velodyne_msgs::VelodynePacket &pkt, DataContainerBase& data, const ros::Time& scan_start_time)
|
||||
{
|
||||
float azimuth;
|
||||
float azimuth_diff;
|
||||
int raw_azimuth_diff;
|
||||
float last_azimuth_diff=0;
|
||||
float azimuth_corrected_f;
|
||||
int azimuth_corrected;
|
||||
float x, y, z;
|
||||
float intensity;
|
||||
|
||||
float time_diff_start_to_this_packet = (pkt.stamp - scan_start_time).toSec();
|
||||
|
||||
const raw_packet_t *raw = (const raw_packet_t *) &pkt.data[0];
|
||||
|
||||
for (int block = 0; block < BLOCKS_PER_PACKET; block++) {
|
||||
|
||||
// ignore packets with mangled or otherwise different contents
|
||||
if (UPPER_BANK != raw->blocks[block].header) {
|
||||
// Do not flood the log with messages, only issue at most one
|
||||
// of these warnings per minute.
|
||||
ROS_WARN_STREAM_THROTTLE(60, "skipping invalid VLP-16 packet: block "
|
||||
<< block << " header value is "
|
||||
<< raw->blocks[block].header);
|
||||
return; // bad packet: skip the rest
|
||||
}
|
||||
|
||||
// Calculate difference between current and next block's azimuth angle.
|
||||
azimuth = (float)(raw->blocks[block].rotation);
|
||||
if (block < (BLOCKS_PER_PACKET-1)){
|
||||
raw_azimuth_diff = raw->blocks[block+1].rotation - raw->blocks[block].rotation;
|
||||
azimuth_diff = (float)((36000 + raw_azimuth_diff)%36000);
|
||||
// some packets contain an angle overflow where azimuth_diff < 0
|
||||
if(raw_azimuth_diff < 0)//raw->blocks[block+1].rotation - raw->blocks[block].rotation < 0)
|
||||
{
|
||||
ROS_WARN_STREAM_THROTTLE(60, "Packet containing angle overflow, first angle: " << raw->blocks[block].rotation << " second angle: " << raw->blocks[block+1].rotation);
|
||||
// if last_azimuth_diff was not zero, we can assume that the velodyne's speed did not change very much and use the same difference
|
||||
if(last_azimuth_diff > 0){
|
||||
azimuth_diff = last_azimuth_diff;
|
||||
}
|
||||
// otherwise we are not able to use this data
|
||||
// TODO: we might just not use the second 16 firings
|
||||
else{
|
||||
continue;
|
||||
}
|
||||
}
|
||||
last_azimuth_diff = azimuth_diff;
|
||||
}else{
|
||||
azimuth_diff = last_azimuth_diff;
|
||||
}
|
||||
|
||||
for (int firing=0, k=0; firing < VLP16_FIRINGS_PER_BLOCK; firing++){
|
||||
for (int dsr=0; dsr < VLP16_SCANS_PER_FIRING; dsr++, k+=RAW_SCAN_SIZE){
|
||||
velodyne_pointcloud::LaserCorrection &corrections = calibration_.laser_corrections[dsr];
|
||||
|
||||
/** Position Calculation */
|
||||
union two_bytes tmp;
|
||||
tmp.bytes[0] = raw->blocks[block].data[k];
|
||||
tmp.bytes[1] = raw->blocks[block].data[k+1];
|
||||
|
||||
/** correct for the laser rotation as a function of timing during the firings **/
|
||||
azimuth_corrected_f = azimuth + (azimuth_diff * ((dsr*VLP16_DSR_TOFFSET) + (firing*VLP16_FIRING_TOFFSET)) / VLP16_BLOCK_TDURATION);
|
||||
azimuth_corrected = ((int)round(azimuth_corrected_f)) % 36000;
|
||||
|
||||
/*condition added to avoid calculating points which are not
|
||||
in the interesting defined area (min_angle < area < max_angle)*/
|
||||
if ((azimuth_corrected >= config_.min_angle
|
||||
&& azimuth_corrected <= config_.max_angle
|
||||
&& config_.min_angle < config_.max_angle)
|
||||
||(config_.min_angle > config_.max_angle
|
||||
&& (azimuth_corrected <= config_.max_angle
|
||||
|| azimuth_corrected >= config_.min_angle))){
|
||||
|
||||
// convert polar coordinates to Euclidean XYZ
|
||||
float distance = tmp.uint * calibration_.distance_resolution_m;
|
||||
distance += corrections.dist_correction;
|
||||
|
||||
float cos_vert_angle = corrections.cos_vert_correction;
|
||||
float sin_vert_angle = corrections.sin_vert_correction;
|
||||
float cos_rot_correction = corrections.cos_rot_correction;
|
||||
float sin_rot_correction = corrections.sin_rot_correction;
|
||||
|
||||
// cos(a-b) = cos(a)*cos(b) + sin(a)*sin(b)
|
||||
// sin(a-b) = sin(a)*cos(b) - cos(a)*sin(b)
|
||||
float cos_rot_angle =
|
||||
cos_rot_table_[azimuth_corrected] * cos_rot_correction +
|
||||
sin_rot_table_[azimuth_corrected] * sin_rot_correction;
|
||||
float sin_rot_angle =
|
||||
sin_rot_table_[azimuth_corrected] * cos_rot_correction -
|
||||
cos_rot_table_[azimuth_corrected] * sin_rot_correction;
|
||||
|
||||
float horiz_offset = corrections.horiz_offset_correction;
|
||||
float vert_offset = corrections.vert_offset_correction;
|
||||
|
||||
// Compute the distance in the xy plane (w/o accounting for rotation)
|
||||
/**the new term of 'vert_offset * sin_vert_angle'
|
||||
* was added to the expression due to the mathemathical
|
||||
* model we used.
|
||||
*/
|
||||
float xy_distance = distance * cos_vert_angle - vert_offset * sin_vert_angle;
|
||||
|
||||
// Calculate temporal X, use absolute value.
|
||||
float xx = xy_distance * sin_rot_angle - horiz_offset * cos_rot_angle;
|
||||
// Calculate temporal Y, use absolute value
|
||||
float yy = xy_distance * cos_rot_angle + horiz_offset * sin_rot_angle;
|
||||
if (xx < 0) xx=-xx;
|
||||
if (yy < 0) yy=-yy;
|
||||
|
||||
// Get 2points calibration values,Linear interpolation to get distance
|
||||
// correction for X and Y, that means distance correction use
|
||||
// different value at different distance
|
||||
float distance_corr_x = 0;
|
||||
float distance_corr_y = 0;
|
||||
if (corrections.two_pt_correction_available) {
|
||||
distance_corr_x =
|
||||
(corrections.dist_correction - corrections.dist_correction_x)
|
||||
* (xx - 2.4) / (25.04 - 2.4)
|
||||
+ corrections.dist_correction_x;
|
||||
distance_corr_x -= corrections.dist_correction;
|
||||
distance_corr_y =
|
||||
(corrections.dist_correction - corrections.dist_correction_y)
|
||||
* (yy - 1.93) / (25.04 - 1.93)
|
||||
+ corrections.dist_correction_y;
|
||||
distance_corr_y -= corrections.dist_correction;
|
||||
}
|
||||
|
||||
float distance_x = distance + distance_corr_x;
|
||||
/**the new term of 'vert_offset * sin_vert_angle'
|
||||
* was added to the expression due to the mathemathical
|
||||
* model we used.
|
||||
*/
|
||||
xy_distance = distance_x * cos_vert_angle - vert_offset * sin_vert_angle ;
|
||||
x = xy_distance * sin_rot_angle - horiz_offset * cos_rot_angle;
|
||||
|
||||
float distance_y = distance + distance_corr_y;
|
||||
/**the new term of 'vert_offset * sin_vert_angle'
|
||||
* was added to the expression due to the mathemathical
|
||||
* model we used.
|
||||
*/
|
||||
xy_distance = distance_y * cos_vert_angle - vert_offset * sin_vert_angle ;
|
||||
y = xy_distance * cos_rot_angle + horiz_offset * sin_rot_angle;
|
||||
|
||||
// Using distance_y is not symmetric, but the velodyne manual
|
||||
// does this.
|
||||
/**the new term of 'vert_offset * cos_vert_angle'
|
||||
* was added to the expression due to the mathemathical
|
||||
* model we used.
|
||||
*/
|
||||
z = distance_y * sin_vert_angle + vert_offset*cos_vert_angle;
|
||||
|
||||
|
||||
/** Use standard ROS coordinate system (right-hand rule) */
|
||||
float x_coord = y;
|
||||
float y_coord = -x;
|
||||
float z_coord = z;
|
||||
|
||||
/** Intensity Calculation */
|
||||
float min_intensity = corrections.min_intensity;
|
||||
float max_intensity = corrections.max_intensity;
|
||||
|
||||
intensity = raw->blocks[block].data[k+2];
|
||||
|
||||
float focal_offset = 256 * SQR(1 - corrections.focal_distance / 13100);
|
||||
float focal_slope = corrections.focal_slope;
|
||||
intensity += focal_slope * (std::abs(focal_offset - 256 *
|
||||
SQR(1 - tmp.uint/65535)));
|
||||
intensity = (intensity < min_intensity) ? min_intensity : intensity;
|
||||
intensity = (intensity > max_intensity) ? max_intensity : intensity;
|
||||
|
||||
float time = 0;
|
||||
if (timing_offsets.size())
|
||||
time = timing_offsets[block][firing * 16 + dsr] + time_diff_start_to_this_packet;
|
||||
|
||||
data.addPoint(x_coord, y_coord, z_coord, corrections.laser_ring, azimuth_corrected, distance, intensity, time);
|
||||
}
|
||||
}
|
||||
data.newLine();
|
||||
}
|
||||
}
|
||||
}
|
||||
} // namespace velodyne_rawdata
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,52 @@
|
||||
### Unit tests
|
||||
#
|
||||
# Only configured when CATKIN_ENABLE_TESTING is true.
|
||||
|
||||
# these dependencies are only needed for unit testing
|
||||
find_package(roslaunch REQUIRED)
|
||||
find_package(rostest REQUIRED)
|
||||
find_package(tf2_ros REQUIRED)
|
||||
|
||||
# C++ gtests
|
||||
catkin_add_gtest(test_calibration test_calibration.cpp)
|
||||
add_dependencies(test_calibration ${catkin_EXPORTED_TARGETS})
|
||||
target_link_libraries(test_calibration velodyne_rawdata ${catkin_LIBRARIES})
|
||||
|
||||
# Download packet capture (PCAP) files containing test data.
|
||||
# Store them in devel-space, so rostest can easily find them.
|
||||
catkin_download_test_data(
|
||||
${PROJECT_NAME}_tests_class.pcap
|
||||
http://download.ros.org/data/velodyne/class.pcap
|
||||
DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/tests
|
||||
MD5 65808d25772101358a3719b451b3d015)
|
||||
catkin_download_test_data(
|
||||
${PROJECT_NAME}_tests_32e.pcap
|
||||
http://download.ros.org/data/velodyne/32e.pcap
|
||||
DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/tests
|
||||
MD5 e41d02aac34f0967c03a5597e1d554a9)
|
||||
catkin_download_test_data(
|
||||
${PROJECT_NAME}_tests_64e_s2.1-300-sztaki.pcap
|
||||
http://download.ros.org/data/velodyne/64e_s2.1-300-sztaki.pcap
|
||||
DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/tests
|
||||
MD5 176c900ffb698f9b948a13e281ffc1a2)
|
||||
catkin_download_test_data(
|
||||
${PROJECT_NAME}_tests_vlp16.pcap
|
||||
http://download.ros.org/data/velodyne/vlp16.pcap
|
||||
DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/tests
|
||||
MD5 f45c2bb1d7ee358274e423ea3b66fd73)
|
||||
|
||||
# run rostests
|
||||
add_rostest(cloud_node_hz.test)
|
||||
add_rostest(cloud_nodelet_hz.test)
|
||||
add_rostest(cloud_node_32e_hz.test)
|
||||
add_rostest(cloud_nodelet_32e_hz.test)
|
||||
add_rostest(cloud_node_64e_s2.1_hz.test)
|
||||
add_rostest(cloud_nodelet_64e_s2.1_hz.test)
|
||||
add_rostest(cloud_node_vlp16_hz.test)
|
||||
add_rostest(cloud_nodelet_vlp16_hz.test)
|
||||
add_rostest(transform_node_hz.test)
|
||||
add_rostest(transform_nodelet_hz.test)
|
||||
add_rostest(two_nodelet_managers.test)
|
||||
|
||||
# parse check all the launch/*.launch files
|
||||
roslaunch_add_file_check(../launch)
|
||||
@@ -0,0 +1,835 @@
|
||||
num_lasers: 64
|
||||
distance_resolution: 0.002
|
||||
lasers:
|
||||
- laser_id: 0
|
||||
rot_correction: -0.122173048555851
|
||||
vert_correction: -0.401850968599319
|
||||
dist_correction: 0.119999997317791
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 1
|
||||
rot_correction: -0.0610865242779255
|
||||
vert_correction: -0.39391815662384
|
||||
dist_correction: 0.0199999995529652
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 2
|
||||
rot_correction: 0.0959931090474129
|
||||
vert_correction: -0.200955957174301
|
||||
dist_correction: 0.100000001490116
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 3
|
||||
rot_correction: 0.148352980613708
|
||||
vert_correction: -0.188023120164871
|
||||
dist_correction: 0.230000004172325
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 4
|
||||
rot_correction: -0.00872664619237185
|
||||
vert_correction: -0.384992778301239
|
||||
dist_correction: 0.170000001788139
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 5
|
||||
rot_correction: 0.0610865242779255
|
||||
vert_correction: -0.379074156284332
|
||||
dist_correction: 0.150000005960464
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 6
|
||||
rot_correction: -0.0174532923847437
|
||||
vert_correction: -0.445288896560669
|
||||
dist_correction: 0.0500000007450581
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 7
|
||||
rot_correction: 0.0349065847694874
|
||||
vert_correction: -0.43470174074173
|
||||
dist_correction: 0.270000010728836
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 8
|
||||
rot_correction: 0.122173048555851
|
||||
vert_correction: -0.3671615421772
|
||||
dist_correction: 0.180000007152557
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 9
|
||||
rot_correction: 0.174532920122147
|
||||
vert_correction: -0.357254236936569
|
||||
dist_correction: 0.189999997615814
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 10
|
||||
rot_correction: 0.104719758033752
|
||||
vert_correction: -0.423742026090622
|
||||
dist_correction: 0.100000001490116
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 11
|
||||
rot_correction: 0.165806278586388
|
||||
vert_correction: -0.414792060852051
|
||||
dist_correction: 0.200000002980232
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 12
|
||||
rot_correction: -0.113446399569511
|
||||
vert_correction: -0.288992017507553
|
||||
dist_correction: 0.219999998807907
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 13
|
||||
rot_correction: -0.0567231997847557
|
||||
vert_correction: -0.28310164809227
|
||||
dist_correction: 0.200000002980232
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 14
|
||||
rot_correction: -0.13962633907795
|
||||
vert_correction: -0.347351551055908
|
||||
dist_correction: 0.150000005960464
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 15
|
||||
rot_correction: -0.0872664600610733
|
||||
vert_correction: -0.332452714443207
|
||||
dist_correction: 0.219999998807907
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 16
|
||||
rot_correction: 0
|
||||
vert_correction: -0.274210214614868
|
||||
dist_correction: 0.0599999986588955
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 17
|
||||
rot_correction: 0.0558505356311798
|
||||
vert_correction: -0.266317009925842
|
||||
dist_correction: 0.129999995231628
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 18
|
||||
rot_correction: -0.0174532923847437
|
||||
vert_correction: -0.331895291805267
|
||||
dist_correction: 0.00999999977648258
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 19
|
||||
rot_correction: 0.0349065847694874
|
||||
vert_correction: -0.323663860559464
|
||||
dist_correction: 0.150000005960464
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 20
|
||||
rot_correction: 0.122173048555851
|
||||
vert_correction: -0.256421387195587
|
||||
dist_correction: 0.200000002980232
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 21
|
||||
rot_correction: 0.165806278586388
|
||||
vert_correction: -0.244522526860237
|
||||
dist_correction: 0.239999994635582
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 22
|
||||
rot_correction: 0.104719758033752
|
||||
vert_correction: -0.31077241897583
|
||||
dist_correction: 0.180000007152557
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 23
|
||||
rot_correction: 0.157079637050629
|
||||
vert_correction: -0.302882075309753
|
||||
dist_correction: 0.219999998807907
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 24
|
||||
rot_correction: -0.104719758033752
|
||||
vert_correction: -0.180082052946091
|
||||
dist_correction: 0.140000000596046
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 25
|
||||
rot_correction: -0.0610865242779255
|
||||
vert_correction: -0.172132045030594
|
||||
dist_correction: 0.319999992847443
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 26
|
||||
rot_correction: -0.13962633907795
|
||||
vert_correction: -0.235619828104973
|
||||
dist_correction: 0.200000002980232
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 27
|
||||
rot_correction: -0.0785398185253143
|
||||
vert_correction: -0.228712558746338
|
||||
dist_correction: 0.25
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 28
|
||||
rot_correction: 0
|
||||
vert_correction: -0.16517236828804
|
||||
dist_correction: 0.170000001788139
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 29
|
||||
rot_correction: 0.0436332300305367
|
||||
vert_correction: -0.154202222824097
|
||||
dist_correction: 0.230000004172325
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 30
|
||||
rot_correction: -0.0174532923847437
|
||||
vert_correction: -0.219799935817719
|
||||
dist_correction: 0.150000005960464
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 31
|
||||
rot_correction: 0.0314159244298935
|
||||
vert_correction: -0.21088133752346
|
||||
dist_correction: 0.230000004172325
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 32
|
||||
rot_correction: -0.0698131695389748
|
||||
vert_correction: -0.121932752430439
|
||||
dist_correction: 0.100000001490116
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 33
|
||||
rot_correction: -0.0392699092626572
|
||||
vert_correction: -0.113993428647518
|
||||
dist_correction: 0.280000001192093
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 34
|
||||
rot_correction: 0.0698131695389748
|
||||
vert_correction: 0.0055470340885222
|
||||
dist_correction: 0.319999992847443
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 35
|
||||
rot_correction: 0.104719758033752
|
||||
vert_correction: 0.0114863449707627
|
||||
dist_correction: 0.230000004172325
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 36
|
||||
rot_correction: 0.013962633907795
|
||||
vert_correction: -0.109056323766708
|
||||
dist_correction: 0.0700000002980232
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 37
|
||||
rot_correction: 0.0392699092626572
|
||||
vert_correction: -0.104121200740337
|
||||
dist_correction: 0.0900000035762787
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 38
|
||||
rot_correction: 0
|
||||
vert_correction: -0.14471623301506
|
||||
dist_correction: 0.119999997317791
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 39
|
||||
rot_correction: 0.0226892791688442
|
||||
vert_correction: -0.1387659907341
|
||||
dist_correction: 0.200000002980232
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 40
|
||||
rot_correction: 0.0820304751396179
|
||||
vert_correction: -0.0981878563761711
|
||||
dist_correction: 0.129999995231628
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 41
|
||||
rot_correction: 0.113446399569511
|
||||
vert_correction: -0.0952560678124428
|
||||
dist_correction: 0.159999996423721
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 42
|
||||
rot_correction: 0.0698131695389748
|
||||
vert_correction: -0.132818818092346
|
||||
dist_correction: 0.159999996423721
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 43
|
||||
rot_correction: 0.104719758033752
|
||||
vert_correction: -0.127874463796616
|
||||
dist_correction: 0.159999996423721
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 44
|
||||
rot_correction: -0.068067841231823
|
||||
vert_correction: -0.05375986546278
|
||||
dist_correction: 0.129999995231628
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 45
|
||||
rot_correction: -0.0349065847694874
|
||||
vert_correction: -0.0478330813348293
|
||||
dist_correction: 0.200000002980232
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 46
|
||||
rot_correction: -0.0837758108973503
|
||||
vert_correction: -0.0893256440758705
|
||||
dist_correction: 0.170000001788139
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 47
|
||||
rot_correction: -0.0479965545237064
|
||||
vert_correction: -0.0833963677287102
|
||||
dist_correction: 0.239999994635582
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 48
|
||||
rot_correction: 0.00872664619237185
|
||||
vert_correction: -0.0419059917330742
|
||||
dist_correction: 0.180000007152557
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 49
|
||||
rot_correction: 0.0392699092626572
|
||||
vert_correction: -0.035978376865387
|
||||
dist_correction: 0.0599999986588955
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 50
|
||||
rot_correction: 0
|
||||
vert_correction: -0.0774680152535439
|
||||
dist_correction: 0.140000000596046
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 51
|
||||
rot_correction: 0.0305432621389627
|
||||
vert_correction: -0.0715404152870178
|
||||
dist_correction: 0.150000005960464
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 52
|
||||
rot_correction: 0.0855211317539215
|
||||
vert_correction: -0.0300500374287367
|
||||
dist_correction: 0.219999998807907
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 53
|
||||
rot_correction: 0.109083078801632
|
||||
vert_correction: -0.024120757356286
|
||||
dist_correction: 0.140000000596046
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 54
|
||||
rot_correction: 0.0698131695389748
|
||||
vert_correction: -0.0656133219599724
|
||||
dist_correction: 0.0799999982118607
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 55
|
||||
rot_correction: 0.104719758033752
|
||||
vert_correction: -0.0596865378320217
|
||||
dist_correction: 0.159999996423721
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 56
|
||||
rot_correction: -0.0610865242779255
|
||||
vert_correction: 0.0174280721694231
|
||||
dist_correction: 0.119999997317791
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 57
|
||||
rot_correction: -0.0349065847694874
|
||||
vert_correction: 0.0233724191784859
|
||||
dist_correction: 0.219999998807907
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 58
|
||||
rot_correction: -0.0785398185253143
|
||||
vert_correction: -0.0181903336197138
|
||||
dist_correction: 0.159999996423721
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 59
|
||||
rot_correction: -0.0471238903701305
|
||||
vert_correction: -0.0122585473582149
|
||||
dist_correction: 0.259999990463257
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 60
|
||||
rot_correction: 0.0104719763621688
|
||||
vert_correction: 0.0293195936828852
|
||||
dist_correction: 0.140000000596046
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 61
|
||||
rot_correction: 0.0349065847694874
|
||||
vert_correction: 0.0352698266506195
|
||||
dist_correction: 0.219999998807907
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 62
|
||||
rot_correction: -0.00436332309618592
|
||||
vert_correction: -0.00632520206272602
|
||||
dist_correction: 0.209999993443489
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
- laser_id: 63
|
||||
rot_correction: 0.0296705979853868
|
||||
vert_correction: -0.000390077300835401
|
||||
dist_correction: 0.140000000596046
|
||||
two_pt_correction_available: false
|
||||
dist_correction_x: 0
|
||||
dist_correction_y: 0
|
||||
vert_offset_correction: 0
|
||||
vert_offset_correction: 0
|
||||
max_intensity: 255
|
||||
min_intensity: 0
|
||||
focal_distance: 0
|
||||
focal_slope: 0
|
||||
@@ -0,0 +1,258 @@
|
||||
lasers:
|
||||
- {dist_correction: 0.100000001490116, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 0, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.0698131695389748, two_pt_correction_available: false, vert_correction: -0.124932751059532,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.280000001192093, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 1, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.0392699092626572, two_pt_correction_available: false, vert_correction: -0.118993431329727,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.319999992847443, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 2, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.0698131695389748, two_pt_correction_available: false, vert_correction: 0.0055470340885222,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.230000004172325, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 3, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.104719758033752, two_pt_correction_available: false, vert_correction: 0.0114863449707627,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.0700000002980232, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 4, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.013962633907795, two_pt_correction_available: false, vert_correction: -0.113056324422359,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.0900000035762787, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 5, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.0392699092626572, two_pt_correction_available: false, vert_correction: -0.107121199369431,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.119999997317791, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 6, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0, two_pt_correction_available: false, vert_correction: -0.148716226220131,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.200000002980232, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 7, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.0226892791688442, two_pt_correction_available: false, vert_correction: -0.142765983939171,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.129999995231628, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 8, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.0820304751396179, two_pt_correction_available: false, vert_correction: -0.101187855005264,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.159999996423721, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 9, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.113446399569511, two_pt_correction_available: false, vert_correction: -0.0952560678124428,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.159999996423721, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 10, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.0698131695389748, two_pt_correction_available: false, vert_correction: -0.136818811297417,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.159999996423721, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 11, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.104719758033752, two_pt_correction_available: false, vert_correction: -0.130874469876289,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.129999995231628, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 12, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.068067841231823, two_pt_correction_available: false, vert_correction: -0.05375986546278,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.200000002980232, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 13, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.0349065847694874, two_pt_correction_available: false, vert_correction: -0.0478330813348293,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.170000001788139, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 14, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.0837758108973503, two_pt_correction_available: false, vert_correction: -0.0893256440758705,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.239999994635582, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 15, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.0479965545237064, two_pt_correction_available: false, vert_correction: -0.0833963677287102,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.180000007152557, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 16, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.00872664619237185, two_pt_correction_available: false, vert_correction: -0.0419059917330742,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.0599999986588955, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 17, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.0392699092626572, two_pt_correction_available: false, vert_correction: -0.035978376865387,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.140000000596046, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 18, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0, two_pt_correction_available: false, vert_correction: -0.0774680152535439,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.150000005960464, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 19, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.0305432621389627, two_pt_correction_available: false, vert_correction: -0.0715404152870178,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.219999998807907, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 20, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.0855211317539215, two_pt_correction_available: false, vert_correction: -0.0300500374287367,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.140000000596046, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 21, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.109083078801632, two_pt_correction_available: false, vert_correction: -0.024120757356286,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.0799999982118607, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 22, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.0698131695389748, two_pt_correction_available: false, vert_correction: -0.0656133219599724,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.159999996423721, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 23, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.104719758033752, two_pt_correction_available: false, vert_correction: -0.0596865378320217,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.119999997317791, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 24, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.0610865242779255, two_pt_correction_available: false, vert_correction: 0.0174280721694231,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.219999998807907, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 25, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.0349065847694874, two_pt_correction_available: false, vert_correction: 0.0233724191784859,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.159999996423721, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 26, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.0785398185253143, two_pt_correction_available: false, vert_correction: -0.0181903336197138,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.259999990463257, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 27, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.0471238903701305, two_pt_correction_available: false, vert_correction: -0.0122585473582149,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.140000000596046, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 28, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.0104719763621688, two_pt_correction_available: false, vert_correction: 0.0293195936828852,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.219999998807907, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 29, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.0349065847694874, two_pt_correction_available: false, vert_correction: 0.0352698266506195,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.209999993443489, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 30, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.00436332309618592, two_pt_correction_available: false, vert_correction: -0.00632520206272602,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.140000000596046, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 31, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.0296705979853868, two_pt_correction_available: false, vert_correction: -0.000390077300835401,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.119999997317791, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 32, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.122173048555851, two_pt_correction_available: false, vert_correction: -0.396850973367691,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.0199999995529652, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 33, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.0610865242779255, two_pt_correction_available: false, vert_correction: -0.387918144464493,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.100000001490116, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 34, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.0959931090474129, two_pt_correction_available: false, vert_correction: -0.200955957174301,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.230000004172325, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 35, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.148352980613708, two_pt_correction_available: false, vert_correction: -0.192023113369942,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.170000001788139, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 36, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.00872664619237185, two_pt_correction_available: false, vert_correction: -0.378992766141891,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.150000005960464, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 37, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.0610865242779255, two_pt_correction_available: false, vert_correction: -0.370074152946472,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.0500000007450581, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 38, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.0174532923847437, two_pt_correction_available: false, vert_correction: -0.43128889799118,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.270000010728836, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 39, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.0349065847694874, two_pt_correction_available: false, vert_correction: -0.423701733350754,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.180000007152557, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 40, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.122173048555851, two_pt_correction_available: false, vert_correction: -0.361161530017853,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.189999997615814, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 41, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.174532920122147, two_pt_correction_available: false, vert_correction: -0.352254241704941,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.100000001490116, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 42, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.104719758033752, two_pt_correction_available: false, vert_correction: -0.414742022752762,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.200000002980232, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 43, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.165806278586388, two_pt_correction_available: false, vert_correction: -0.405792057514191,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.219999998807907, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 44, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.113446399569511, two_pt_correction_available: false, vert_correction: -0.28999200463295,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.200000002980232, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 45, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.0567231997847557, two_pt_correction_available: false, vert_correction: -0.281101644039154,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.150000005960464, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 46, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.13962633907795, two_pt_correction_available: false, vert_correction: -0.343351542949677,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.219999998807907, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 47, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.0872664600610733, two_pt_correction_available: false, vert_correction: -0.334452718496323,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.0599999986588955, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 48, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0, two_pt_correction_available: false, vert_correction: -0.272210210561752,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.129999995231628, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 49, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.0558505356311798, two_pt_correction_available: false, vert_correction: -0.26331701874733,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.00999999977648258, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 50, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.0174532923847437, two_pt_correction_available: false, vert_correction: -0.323895305395126,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.150000005960464, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 51, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.0349065847694874, two_pt_correction_available: false, vert_correction: -0.316663861274719,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.200000002980232, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 52, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.122173048555851, two_pt_correction_available: false, vert_correction: -0.254421383142471,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.239999994635582, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 53, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.165806278586388, two_pt_correction_available: false, vert_correction: -0.245522528886795,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.180000007152557, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 54, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.104719758033752, two_pt_correction_available: false, vert_correction: -0.307772427797318,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.219999998807907, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 55, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.157079637050629, two_pt_correction_available: false, vert_correction: -0.298882067203522,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.140000000596046, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 56, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.104719758033752, two_pt_correction_available: false, vert_correction: -0.183082059025764,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.319999992847443, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 57, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.0610865242779255, two_pt_correction_available: false, vert_correction: -0.17413204908371,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.200000002980232, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 58, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.13962633907795, two_pt_correction_available: false, vert_correction: -0.236619830131531,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.25, dist_correction_x: 0, dist_correction_y: 0, focal_distance: 0,
|
||||
focal_slope: 0, laser_id: 59, max_intensity: 255, min_intensity: 0, rot_correction: -0.0785398185253143,
|
||||
two_pt_correction_available: false, vert_correction: -0.22771255671978, vert_offset_correction: 0}
|
||||
- {dist_correction: 0.170000001788139, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 60, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0, two_pt_correction_available: false, vert_correction: -0.16517236828804,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.230000004172325, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 61, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.0436332300305367, two_pt_correction_available: false, vert_correction: -0.156202226877213,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.150000005960464, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 62, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: -0.0174532923847437, two_pt_correction_available: false, vert_correction: -0.218799933791161,
|
||||
vert_offset_correction: 0}
|
||||
- {dist_correction: 0.230000004172325, dist_correction_x: 0, dist_correction_y: 0,
|
||||
focal_distance: 0, focal_slope: 0, laser_id: 63, max_intensity: 255, min_intensity: 0,
|
||||
rot_correction: 0.0314159244298935, two_pt_correction_available: false, vert_correction: -0.209881335496902,
|
||||
vert_offset_correction: 0}
|
||||
num_lasers: 64
|
||||
distance_resolution: 0.002
|
||||
@@ -0,0 +1,28 @@
|
||||
<!-- -*- mode: XML -*- -->
|
||||
<!-- rostest of reading Velodyne 32E PCAP files -->
|
||||
|
||||
<launch>
|
||||
|
||||
<!-- start driver with example PCAP file -->
|
||||
<node pkg="velodyne_driver" type="velodyne_node" name="velodyne_node">
|
||||
<param name="model" value="32E"/>
|
||||
<param name="pcap" value="$(find velodyne_pointcloud)/tests/32e.pcap"/>
|
||||
</node>
|
||||
|
||||
<!-- start cloud node with test calibration file -->
|
||||
<node pkg="velodyne_pointcloud" type="cloud_node" name="cloud_node">
|
||||
<param name="calibration"
|
||||
value="$(find velodyne_pointcloud)/params/32db.yaml"/>
|
||||
</node>
|
||||
|
||||
<!-- verify PointCloud2 publication rate -->
|
||||
<test test-name="cloud_node_32e_hz_test" pkg="rostest"
|
||||
type="hztest" name="hztest_cloud_node_32e" >
|
||||
<param name="hz" value="10.0" />
|
||||
<param name="hzerror" value="3.0" />
|
||||
<param name="test_duration" value="5.0" />
|
||||
<param name="topic" value="velodyne_points" />
|
||||
<param name="wait_time" value="2.0" />
|
||||
</test>
|
||||
|
||||
</launch>
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user