From b3426867d9749329ff6096e21f9c075d7a119d07 Mon Sep 17 00:00:00 2001 From: Nils Hauke Bussas Date: Mon, 18 Mar 2019 13:29:46 +0100 Subject: [PATCH] Transform static variable into member --- .../include/velodyne_driver/driver.h | 1 + velodyne_driver/src/driver/driver.cc | 19 ++++++++++--------- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/velodyne_driver/include/velodyne_driver/driver.h b/velodyne_driver/include/velodyne_driver/driver.h index b0ff26bf9..97a86b551 100644 --- a/velodyne_driver/include/velodyne_driver/driver.h +++ b/velodyne_driver/include/velodyne_driver/driver.h @@ -62,6 +62,7 @@ class VelodyneDriver boost::shared_ptr input_; ros::Publisher output_; + int last_azimuth_; /** diagnostics updater */ ros::Timer diag_timer_; diff --git a/velodyne_driver/src/driver/driver.cc b/velodyne_driver/src/driver/driver.cc index f5cbdf613..3866793ff 100644 --- a/velodyne_driver/src/driver/driver.cc +++ b/velodyne_driver/src/driver/driver.cc @@ -152,6 +152,8 @@ VelodyneDriver::VelodyneDriver(ros::NodeHandle node, // raw packet output topic output_ = node.advertise("velodyne_packets", 10); + + last_azimuth_ = -1; } /** poll the device @@ -177,24 +179,23 @@ bool VelodyneDriver::poll(void) } scan->packets.push_back(tmp_packet); - static int last_azimuth = -1; // 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; + //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)) + 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; + last_azimuth_ = azimuth; break; // Cut angle passed, one full revolution collected } - last_azimuth = azimuth; + last_azimuth_ = azimuth; } } else // standard behaviour