Initial commit
This commit is contained in:
@@ -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;
|
||||
}
|
||||
Reference in New Issue
Block a user