Driver: added check angle function in velodyne driver.

modules/drivers/velodyne/velodyne_driver/CMakeLists.txt

@@ -40,7 +40,7 @@ if (CATKIN_ENABLE_TESTING)
   # these dependencies are only needed for unit testing
   find_package(roslaunch REQUIRED)
   find_package(rostest REQUIRED)
-    
+
   # parse check all the launch/*.launch files
   roslaunch_add_file_check(launch)
 

modules/drivers/velodyne/velodyne_driver/src/driver/CMakeLists.txt

 # build the driver node
-add_executable(driver_node 
-  driver_node.cpp 
-  driver.cpp 
+add_executable(driver_node
+  driver_node.cpp
+  driver.cpp
   driver16.cpp
   driver32.cpp
   driver64.cpp
@@ -12,14 +12,16 @@ target_link_libraries(driver_node
 )
 
 # build the nodelet version
-add_library(driver_nodelet 
+add_library(
+  driver_nodelet
   driver_nodelet.cpp
-  driver.cpp 
+  driver.cpp
   driver16.cpp
   driver32.cpp
   driver64.cpp
 )
-target_link_libraries(driver_nodelet
+target_link_libraries(
+  driver_nodelet
   velodyne_input
   ${catkin_LIBRARIES}
 )

modules/drivers/velodyne/velodyne_driver/src/driver/driver.cpp

@@ -16,11 +16,12 @@
 
 #include "driver.h"
 
-#include <ros/ros.h>
-#include <time.h>
 #include <cmath>
+#include <ctime>
 #include <string>
 
+#include <ros/ros.h>
+
 namespace apollo {
 namespace drivers {
 namespace velodyne {
@@ -106,6 +107,24 @@ void VelodyneDriver::update_gps_top_hour(uint32_t current_time) {
   last_gps_time_ = current_time;
 }
 
+bool VelodyneDriver::check_angle(velodyne_msgs::VelodynePacket& packet) {
+  // check the angel in every packet
+  // for each model of velodyne 64 the data struct is same ,
+  // so we don't need to check the lidar model
+  const unsigned char* raw_ptr = (const unsigned char*)&packet.data[0];
+  for (int i = 0; i < BLOCKS_PER_PACKET; ++i) {
+    uint16_t angle =
+        raw_ptr[i * BLOCK_SIZE + 3] * 256 + raw_ptr[i * BLOCK_SIZE + 2];
+    // for the velodyne64 angle resolution is 0.17~0.2 , so take the angle diff
+    // at 0.2~0.3 should be a good choice
+    if (angle > config_.prefix_angle &&
+        std::abs(angle - config_.prefix_angle) < 30) {
+      return true;
+    }
+  }
+  return false;
+}
+
 VelodyneDriver* VelodyneDriverFactory::create_driver(
     ros::NodeHandle private_nh) {
   Config config;

modules/drivers/velodyne/velodyne_driver/src/driver/driver.h

@@ -14,8 +14,8 @@
  * limitations under the License.
  *****************************************************************************/
 
-#ifndef VELODYNE_DRIVER_H
-#define VELODYNE_DRIVER_H
+#ifndef VELODYNE_DRIVER_H_
+#define VELODYNE_DRIVER_H_
 
 #include <ros/ros.h>
 #include <string>
@@ -27,17 +27,19 @@ namespace apollo {
 namespace drivers {
 namespace velodyne {
 
+constexpr int BLOCKS_PER_PACKET = 12;
+constexpr int BLOCK_SIZE = 100;
+
 // configuration parameters
 struct Config {
-  Config()
-      : npackets(0), rpm(0.0), firing_data_port(0), positioning_data_port(0) {}
   std::string frame_id;  ///< tf frame ID
   std::string model;     ///< device model name
   std::string topic;
-  int npackets;  ///< number of packets to collect
-  double rpm;    ///< device rotation rate (RPMs)
-  int firing_data_port;
-  int positioning_data_port;
+  int npackets = 0;  ///< number of packets to collect
+  double rpm = 0.0;  ///< device rotation rate (RPMs)
+  int firing_data_port = 0;
+  int positioning_data_port = 0;
+  int prefix_angle = 0;  // prefix angle to recv
 };
 
 class VelodyneDriver {
@@ -61,6 +63,8 @@ class VelodyneDriver {
   void set_base_time_from_nmea_time(NMEATimePtr nmea_time,
                                     uint64_t &basetime);
   void update_gps_top_hour(unsigned int current_time);
+
+  bool check_angle(velodyne_msgs::VelodynePacket &packet);
 };
 
 class Velodyne64Driver : public VelodyneDriver {
@@ -75,13 +79,14 @@ class Velodyne64Driver : public VelodyneDriver {
 };
 
 class Velodyne32Driver : public VelodyneDriver {
-public:
-    explicit Velodyne32Driver(const Config &config);
-    virtual ~Velodyne32Driver() {}
-    void init(ros::NodeHandle &node);
-    bool poll(void);
-    void poll_positioning_packet();
-private:
+ public:
+  explicit Velodyne32Driver(const Config &config);
+  virtual ~Velodyne32Driver() {}
+  void init(ros::NodeHandle &node);
+  bool poll(void);
+  void poll_positioning_packet();
+
+ private:
   std::shared_ptr<Input> positioning_input_;
 };
 
@@ -107,4 +112,4 @@ class VelodyneDriverFactory {
 }  // namespace drivers
 }  // namespace apollo
 
-#endif  // VELODYNE_DRIVER_H__
+#endif  // VELODYNE_DRIVER_H_

modules/drivers/velodyne/velodyne_driver/src/driver/driver32.cpp

@@ -15,26 +15,23 @@
  *****************************************************************************/
 #include "driver.h"
 
-#include <ros/ros.h>
-#include <tf/transform_listener.h>
 #include <time.h>
 #include <unistd.h>
 #include <cmath>
 #include <string>
 #include <thread>
 
+#include <ros/ros.h>
+#include <tf/transform_listener.h>
+
 namespace apollo {
 namespace drivers {
 namespace velodyne {
 
-Velodyne32Driver::Velodyne32Driver(const Config& config) { 
-  config_ = config; 
-}
+Velodyne32Driver::Velodyne32Driver(const Config& config) { config_ = config; }
 
 void Velodyne32Driver::init(ros::NodeHandle& node) {
-  double packet_rate = 0.0;  // packet frequency (Hz)
-  packet_rate = 1808.0;
-
+  double packet_rate = 1808.0;              // packet frequency (Hz)
   double frequency = (config_.rpm / 60.0);  // expected Hz rate
 
   // default number of packets for each scan is a single revolution

modules/drivers/velodyne/velodyne_driver/src/driver/driver64.cpp

@@ -25,9 +25,7 @@ namespace apollo {
 namespace drivers {
 namespace velodyne {
 
-Velodyne64Driver::Velodyne64Driver(const Config& config) {
-  config_ = config;
-}
+Velodyne64Driver::Velodyne64Driver(const Config& config) { config_ = config; }
 
 void Velodyne64Driver::init(ros::NodeHandle& node) {
   double packet_rate = 0;  // packet frequency (Hz)