From 5e4a52eb8bc4b39364c818ec921c1fd3c3796431 Mon Sep 17 00:00:00 2001 From: Andreas Wachaja Date: Wed, 20 Apr 2016 11:31:21 +0200 Subject: [PATCH 1/3] velodyne_pointcloud: Fix compile warning "Wrong initialization order" --- velodyne_pointcloud/src/conversions/transform.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/velodyne_pointcloud/src/conversions/transform.cc b/velodyne_pointcloud/src/conversions/transform.cc index 4bf5a2e99..bce8b035e 100644 --- a/velodyne_pointcloud/src/conversions/transform.cc +++ b/velodyne_pointcloud/src/conversions/transform.cc @@ -25,8 +25,8 @@ namespace velodyne_pointcloud { /** @brief Constructor. */ Transform::Transform(ros::NodeHandle node, ros::NodeHandle private_nh): - data_(new velodyne_rawdata::RawData()), - tf_prefix_(tf::getPrefixParam(private_nh)) + tf_prefix_(tf::getPrefixParam(private_nh)), + data_(new velodyne_rawdata::RawData()) { // Read calibration. data_->setup(private_nh); From 094db6975f41191690e354fe24b201b5a907edd6 Mon Sep 17 00:00:00 2001 From: Andreas Wachaja Date: Fri, 22 Apr 2016 13:51:11 +0200 Subject: [PATCH 2/3] velodyne_driver: Make input destructors virtual --- velodyne_driver/include/velodyne_driver/input.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/velodyne_driver/include/velodyne_driver/input.h b/velodyne_driver/include/velodyne_driver/input.h index eaab2e2a1..adea8fab0 100644 --- a/velodyne_driver/include/velodyne_driver/input.h +++ b/velodyne_driver/include/velodyne_driver/input.h @@ -48,6 +48,7 @@ namespace velodyne_driver { public: Input() {} + virtual ~Input() {} /** @brief Read one Velodyne packet. * @@ -75,7 +76,7 @@ namespace velodyne_driver public: InputSocket(ros::NodeHandle private_nh, uint16_t udp_port = UDP_PORT_NUMBER); - ~InputSocket(); + virtual ~InputSocket(); virtual int getPacket(velodyne_msgs::VelodynePacket *pkt); void setDeviceIP( const std::string& ip ); @@ -100,7 +101,7 @@ namespace velodyne_driver bool read_once=false, bool read_fast=false, double repeat_delay=0.0); - ~InputPCAP(); + virtual ~InputPCAP(); virtual int getPacket(velodyne_msgs::VelodynePacket *pkt); void setDeviceIP( const std::string& ip ); From ed051a80c16852dff5ec1fa581aa3566aeaef499 Mon Sep 17 00:00:00 2001 From: Andreas Wachaja Date: Fri, 22 Apr 2016 13:54:20 +0200 Subject: [PATCH 3/3] 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/CMakeLists.txt | 4 ++++ velodyne_driver/cfg/VelodyneNode.cfg | 14 ++++++++++++++ .../include/velodyne_driver/input.h | 6 +++--- velodyne_driver/package.xml | 2 ++ velodyne_driver/src/driver/CMakeLists.txt | 2 ++ velodyne_driver/src/driver/driver.cc | 19 +++++++++++++++++-- velodyne_driver/src/driver/driver.h | 11 +++++++++++ velodyne_driver/src/lib/input.cc | 10 +++++----- 8 files changed, 58 insertions(+), 10 deletions(-) create mode 100755 velodyne_driver/cfg/VelodyneNode.cfg diff --git a/velodyne_driver/CMakeLists.txt b/velodyne_driver/CMakeLists.txt index 319a4b4b5..e8901f234 100644 --- a/velodyne_driver/CMakeLists.txt +++ b/velodyne_driver/CMakeLists.txt @@ -3,6 +3,7 @@ project(velodyne_driver) set(${PROJECT_NAME}_CATKIN_DEPS diagnostic_updater + dynamic_reconfigure nodelet roscpp tf @@ -18,6 +19,9 @@ 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 diff --git a/velodyne_driver/cfg/VelodyneNode.cfg b/velodyne_driver/cfg/VelodyneNode.cfg new file mode 100755 index 000000000..16196722a --- /dev/null +++ b/velodyne_driver/cfg/VelodyneNode.cfg @@ -0,0 +1,14 @@ +#!/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, 0, "A manually calibrated offset (in seconds) to add to the timestamp before publication of a message.", + 0.0, -1.0, 1.0) + +exit(gen.generate(PACKAGE, NODE_NAME, PARAMS_NAME)) diff --git a/velodyne_driver/include/velodyne_driver/input.h b/velodyne_driver/include/velodyne_driver/input.h index adea8fab0..cc6823940 100644 --- a/velodyne_driver/include/velodyne_driver/input.h +++ b/velodyne_driver/include/velodyne_driver/input.h @@ -58,7 +58,7 @@ namespace velodyne_driver * -1 if end of file * > 0 if incomplete packet (is this possible?) */ - virtual int getPacket(velodyne_msgs::VelodynePacket *pkt) = 0; + virtual int getPacket(velodyne_msgs::VelodynePacket *pkt, const double time_offset) = 0; /** @brief Set source IP, from where packets are accepted @@ -78,7 +78,7 @@ namespace velodyne_driver uint16_t udp_port = UDP_PORT_NUMBER); virtual ~InputSocket(); - virtual int getPacket(velodyne_msgs::VelodynePacket *pkt); + virtual int getPacket(velodyne_msgs::VelodynePacket *pkt, const double time_offset); void setDeviceIP( const std::string& ip ); private: @@ -103,7 +103,7 @@ namespace velodyne_driver double repeat_delay=0.0); virtual ~InputPCAP(); - virtual int getPacket(velodyne_msgs::VelodynePacket *pkt); + virtual int getPacket(velodyne_msgs::VelodynePacket *pkt, const double time_offset); void setDeviceIP( const std::string& ip ); private: diff --git a/velodyne_driver/package.xml b/velodyne_driver/package.xml index e83160555..4779ef9ec 100644 --- a/velodyne_driver/package.xml +++ b/velodyne_driver/package.xml @@ -20,6 +20,7 @@ catkin diagnostic_updater + dynamic_reconfigure libpcap nodelet pluginlib @@ -32,6 +33,7 @@ rostest diagnostic_updater + dynamic_reconfigure libpcap nodelet pluginlib diff --git a/velodyne_driver/src/driver/CMakeLists.txt b/velodyne_driver/src/driver/CMakeLists.txt index 2102e3353..8d4179d7c 100644 --- a/velodyne_driver/src/driver/CMakeLists.txt +++ b/velodyne_driver/src/driver/CMakeLists.txt @@ -1,5 +1,6 @@ # 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} @@ -8,6 +9,7 @@ target_link_libraries(velodyne_node # 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} diff --git a/velodyne_driver/src/driver/driver.cc b/velodyne_driver/src/driver/driver.cc index 7bfa8eaa4..ee3eddcd7 100644 --- a/velodyne_driver/src/driver/driver.cc +++ b/velodyne_driver/src/driver/driver.cc @@ -81,6 +81,14 @@ VelodyneDriver::VelodyneDriver(ros::NodeHandle node, int udp_port; private_nh.param("port", udp_port, (int)UDP_PORT_NUMBER); + // Initialize dynamic reconfigure + srv_ = boost::make_shared > (private_nh); + dynamic_reconfigure::Server:: + 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; @@ -134,7 +142,7 @@ bool VelodyneDriver::poll(void) while (true) { // keep reading until full packet received - int rc = input_->getPacket(&scan->packets[i]); + 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? } @@ -142,7 +150,7 @@ bool VelodyneDriver::poll(void) // publish message using time of last packet read ROS_DEBUG("Publishing a full Velodyne scan."); - scan->header.stamp = ros::Time(scan->packets[config_.npackets - 1].stamp); + scan->header.stamp = scan->packets[config_.npackets - 1].stamp; scan->header.frame_id = config_.frame_id; output_.publish(scan); @@ -154,4 +162,11 @@ bool VelodyneDriver::poll(void) return true; } +void VelodyneDriver::callback(velodyne_driver::VelodyneNodeConfig &config, + uint32_t level) +{ + ROS_INFO("Reconfigure Request"); + config_.time_offset = config.time_offset; +} + } // namespace velodyne_driver diff --git a/velodyne_driver/src/driver/driver.h b/velodyne_driver/src/driver/driver.h index 413b4bc51..6eda25a50 100644 --- a/velodyne_driver/src/driver/driver.h +++ b/velodyne_driver/src/driver/driver.h @@ -19,8 +19,10 @@ #include #include #include +#include #include +#include namespace velodyne_driver { @@ -37,6 +39,14 @@ class VelodyneDriver private: + ///Callback for dynamic reconfigure + void callback(velodyne_driver::VelodyneNodeConfig &config, + uint32_t level); + + ///Pointer to dynamic reconfigure service srv_ + boost::shared_ptr > srv_; + // configuration parameters struct { @@ -44,6 +54,7 @@ class VelodyneDriver std::string model; ///< device model name int npackets; ///< number of packets to collect double rpm; ///< device rotation rate (RPMs) + double time_offset; ///< time in seconds added to each velodyne time stamp } config_; boost::shared_ptr input_; diff --git a/velodyne_driver/src/lib/input.cc b/velodyne_driver/src/lib/input.cc index 383401d2a..885fd3b8e 100644 --- a/velodyne_driver/src/lib/input.cc +++ b/velodyne_driver/src/lib/input.cc @@ -91,7 +91,7 @@ namespace velodyne_driver } /** @brief Get one velodyne packet. */ - int InputSocket::getPacket(velodyne_msgs::VelodynePacket *pkt) + int InputSocket::getPacket(velodyne_msgs::VelodynePacket *pkt, const double time_offset) { double time1 = ros::Time::now().toSec(); @@ -177,9 +177,9 @@ namespace velodyne_driver } // Average the times at which we begin and end reading. Use that to - // estimate when the scan occurred. + // estimate when the scan occurred. Add the time offset. double time2 = ros::Time::now().toSec(); - pkt->stamp = ros::Time((time2 + time1) / 2.0); + pkt->stamp = ros::Time((time2 + time1) / 2.0 + time_offset); return 0; } @@ -248,7 +248,7 @@ namespace velodyne_driver } /** @brief Get one velodyne packet. */ - int InputPCAP::getPacket(velodyne_msgs::VelodynePacket *pkt) + int InputPCAP::getPacket(velodyne_msgs::VelodynePacket *pkt, const double time_offset) { struct pcap_pkthdr *header; const u_char *pkt_data; @@ -267,7 +267,7 @@ namespace velodyne_driver packet_rate_.sleep(); memcpy(&pkt->data[0], pkt_data+42, packet_size); - pkt->stamp = ros::Time::now(); + pkt->stamp = ros::Time::now(); // time_offset not considered here, as no synchronization required empty_ = false; return 0; // success }