add driver/driver_lidar_rsm1. base from ros driver,but not test.

src/driver/driver_lidar_rs16/lidar_driver_rs16.cpp

@@ -9,8 +9,7 @@
 
 
 //#include <pcl/conversions.h>
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
+"the format of " << limitPath << " is not correct, use default data"
 
 #include "modulecomm.h"
 

src/driver/driver_lidar_rsm1/.gitignore

@@ -0,0 +1,73 @@
+# This file is used to ignore files which are generated
+# ----------------------------------------------------------------------------
+
+*~
+*.autosave
+*.a
+*.core
+*.moc
+*.o
+*.obj
+*.orig
+*.rej
+*.so
+*.so.*
+*_pch.h.cpp
+*_resource.rc
+*.qm
+.#*
+*.*#
+core
+!core/
+tags
+.DS_Store
+.directory
+*.debug
+Makefile*
+*.prl
+*.app
+moc_*.cpp
+ui_*.h
+qrc_*.cpp
+Thumbs.db
+*.res
+*.rc
+/.qmake.cache
+/.qmake.stash
+
+# qtcreator generated files
+*.pro.user*
+
+# xemacs temporary files
+*.flc
+
+# Vim temporary files
+.*.swp
+
+# Visual Studio generated files
+*.ib_pdb_index
+*.idb
+*.ilk
+*.pdb
+*.sln
+*.suo
+*.vcproj
+*vcproj.*.*.user
+*.ncb
+*.sdf
+*.opensdf
+*.vcxproj
+*vcxproj.*
+
+# MinGW generated files
+*.Debug
+*.Release
+
+# Python byte code
+*.pyc
+
+# Binaries
+# --------
+*.dll
+*.exe
+

src/driver/driver_lidar_rsm1/ChannelNum.csv

@@ -0,0 +1,32 @@
+0 # distance correction offset of channel 0
+0 # distance correction offset of channel 1
+0 # distance correction offset of channel 2
+0 # distance correction offset of channel 3
+0 # distance correction offset of channel 4
+0 # distance correction offset of channel 5
+1 # pitch rate
+1 # yaw rate
+0 # pitch offset of channel 0
+0 # pitch offset of channel 1
+0 # pitch offset of channel 2
+0 # pitch offset of channel 3
+0 # pitch offset of channel 4
+0 # pitch offset of channel 5
+0 # yaw offset of channel 0
+0 # yaw offset of channel 1
+0 # yaw offset of channel 2
+0 # yaw offset of channel 3
+0 # yaw offset of channel 4
+0 # yaw offset of channel 5
+-6.75 # yaw start angle of channel 0, -6.75~6.75
+6.75 # yaw end angle of channel 0, -6.75~6.75
+-6.75 # yaw start angle of channel 1, -6.75~6.75
+6.75 # yaw end angle of channel 1, -6.75~6.75
+-6.75 # yaw start angle of channel 2, -6.75~6.75
+6.75 # yaw end angle of channel 2, -6.75~6.75
+-6.75 # yaw start angle of channel 3, -6.75~6.75
+6.75 # yaw end angle of channel 3, -6.75~6.75
+-6.75 # yaw start angle of channel 4, -6.75~6.75
+6.75 # yaw end angle of channel 4, -6.75~6.75
+-6.75 # yaw start angle of channel 5, -6.75~6.75
+6.75 # yaw end angle of channel 5, -6.75~6.75

src/driver/driver_lidar_rsm1/convert.cc

@@ -0,0 +1,85 @@
+/*
+ *  Copyright (C) 2009, 2010 Austin Robot Technology, Jack O'Quin
+ *  Copyright (C) 2011 Jesse Vera
+ *  Copyright (C) 2012 Austin Robot Technology, Jack O'Quin
+ *  Copyright (C) 2017 Robosense, Tony Zhang
+ *  License: Modified BSD Software License Agreement
+ *
+ *  $Id$
+ */
+
+/** @file
+
+    This class converts raw RSLIDAR 3D LIDAR packets to PointCloud2.
+
+*/
+#include "convert.h"
+//#include <pcl_conversions/pcl_conversions.h>
+
+namespace rslidar_pointcloud {
+/** @brief Constructor. */
+//Convert(std::string strchannelPath, std::string strlimitPath);
+//Convert::Convert(ros::NodeHandle node, ros::NodeHandle private_nh)
+Convert::Convert(std::string strchannelPath, std::string strlimitPath)
+    : data_(new rslidar_rawdata::RawData()), out_points(new pcl::PointCloud<pcl::PointXYZI>)
+{
+  // load lidar parameters
+  data_->loadConfigFile(strchannelPath,strlimitPath);
+//  data_->loadConfigFile(node, private_nh); // load lidar parameters
+
+  std::string model;
+  model = "MEMS";
+//  private_nh.param("model", model, std::string("MEMS"));
+
+  // advertise output point cloud (before subscribing to input data)
+//  point_output_ = node.advertise<sensor_msgs::PointCloud2>("rslidar_points", 10);
+
+//  srv_ = boost::make_shared<
+//      dynamic_reconfigure::Server<rslidar_pointcloud::CloudNodeConfig>>(
+//      private_nh);
+//  dynamic_reconfigure::Server<rslidar_pointcloud::CloudNodeConfig>::CallbackType
+//      f;
+//  f = boost::bind(&Convert::callback, this, _1, _2);
+//  srv_->setCallback(f);
+
+//  // subscribe to rslidarScan packets
+//  rslidar_scan_ = node.subscribe("rslidar_packets", 10, &Convert::processScan,
+//                     (Convert *)this, ros::TransportHints().tcpNoDelay(true));
+}
+
+//void Convert::callback(rslidar_pointcloud::CloudNodeConfig &config,
+//                       uint32_t level) {
+//  ROS_INFO("Reconfigure Request");
+//  // config_.time_offset = config.time_offset;
+//}
+
+/** @brief Callback for raw scan messages. */
+void Convert::processScan(const std::shared_ptr<rs_driver::lidarscan> &scanMsg,pcl::PointCloud<pcl::PointXYZI>::Ptr & pointc) {
+//void Convert::processScan(const rslidar_msgs::rslidarScan::ConstPtr &scanMsg) {
+
+  out_points->header.stamp =  scanMsg->stamp; //pcl_conversions::toPCL(scanMsg->header).stamp;
+  out_points->header.frame_id = scanMsg->frame_id;//scanMsg->header.frame_id;
+
+  // out_points init
+  out_points->height = 5;
+  out_points->width = 15750; // 630 packets * 25 blocks
+  out_points->is_dense = false;
+  out_points->resize(out_points->height * out_points->width);
+
+  // unpack packet repeatly
+  for (size_t i = 0; i < scanMsg->packets.size(); ++i) {
+    data_->unpack_MEMS(scanMsg->packets[i], out_points);
+  }
+
+  // publish point cloud data 
+//  sensor_msgs::PointCloud2 outMsg;
+//  pcl::toROSMsg(*out_points, outMsg);
+//  point_output_.publish(outMsg);
+
+  pointc = out_points;
+
+  // reprocesss the last packet to get start points of next frame
+  size_t last_pkt_index = scanMsg->packets.size() - 1;
+  data_->unpack_MEMS(scanMsg->packets[last_pkt_index], out_points);
+}
+} // namespace rslidar_pointcloud

src/driver/driver_lidar_rsm1/convert.h

@@ -0,0 +1,55 @@
+/* -*- mode: C++ -*- */
+/*
+ *  Copyright (C) 2009, 2010 Austin Robot Technology, Jack O'Quin
+ *  Copyright (C) 2011 Jesse Vera
+ *  Copyright (C) 2012 Austin Robot Technology, Jack O'Quin
+ *  Copyright (C) 2017 Robosense, Tony Zhang
+ *  License: Modified BSD Software License Agreement
+ *
+ *  $Id$
+ */
+
+/** @file
+
+    This class converts raw Robosense 3D LIDAR packets to PointCloud2.
+
+*/
+#ifndef _CONVERT_H_
+#define _CONVERT_H_
+
+//#include <sensor_msgs/PointCloud2.h>
+//#include <dynamic_reconfigure/server.h>
+//#include <rslidar_pointcloud/CloudNodeConfig.h>
+#include "rawdata.h"
+#include "rsdriver.h"
+
+namespace rslidar_pointcloud {
+    class Convert {
+    public:
+
+        Convert(std::string strchannelPath, std::string strlimitPath);
+ //       Convert(ros::NodeHandle node, ros::NodeHandle private_nh);
+
+        ~Convert() {}
+
+        void processScan(const std::shared_ptr<rs_driver::lidarscan> &scanMsg,pcl::PointCloud<pcl::PointXYZI>::Ptr & pointc);
+    private:
+
+//        void callback(rslidar_pointcloud::CloudNodeConfig &config,
+//                      uint32_t level);
+
+ //       void processScan(const rslidar_msgs::rslidarScan::ConstPtr &scanMsg);
+
+
+        ///Pointer to dynamic reconfigure service srv_
+//        boost::shared_ptr<dynamic_reconfigure::Server<rslidar_pointcloud::
+//        CloudNodeConfig> > srv_;
+
+        pcl::PointCloud<pcl::PointXYZI>::Ptr out_points;
+        boost::shared_ptr<rslidar_rawdata::RawData> data_;
+//        ros::Subscriber rslidar_scan_;
+//        ros::Publisher point_output_;
+    };
+
+}//namespace rslidar_pointcloud
+#endif

src/driver/driver_lidar_rsm1/driver_lidar_rsm1.pro

@@ -0,0 +1,57 @@
+QT -= gui
+
+CONFIG += c++11 console
+CONFIG -= app_bundle
+
+# The following define makes your compiler emit warnings if you use
+# any Qt feature that has been marked deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+
+# You can also make your code fail to compile if it uses deprecated APIs.
+# In order to do so, uncomment the following line.
+# You can also select to disable deprecated APIs only up to a certain version of Qt.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += \
+        convert.cc \
+        input.cc \
+        main.cpp \
+        rawdata.cc \
+        rsdriver.cpp
+
+# Default rules for deployment.
+qnx: target.path = /tmp/$${TARGET}/bin
+else: unix:!android: target.path = /opt/$${TARGET}/bin
+!isEmpty(target.path): INSTALLS += target
+
+HEADERS += \
+    convert.h \
+    input.h \
+    rawdata.h \
+    rsdriver.h
+
+INCLUDEPATH += $$PWD/../common
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+!include(../../../include/ivprotobuf.pri ) {
+    error( "Couldn't find the ivprotobuf.pri file!" )
+}
+
+!include(../../../include/ivboost.pri ) {
+    error( "Couldn't find the ivboost.pri file!" )
+}
+
+!include(../../../include/iveigen.pri ) {
+    error( "Couldn't find the iveigen.pri file!" )
+}
+
+!include(../../../include/ivpcl.pri ) {
+    error( "Couldn't find the ivpcl.pri file!" )
+}
+
+LIBS += -lpcap

src/driver/driver_lidar_rsm1/input.cc

@@ -0,0 +1,337 @@
+/*
+ *  Copyright (C) 2007 Austin Robot Technology, Patrick Beeson
+ *  Copyright (C) 2009, 2010 Austin Robot Technology, Jack O'Quin
+ *  Copyright (C) 2015, Jack O'Quin
+ *	Copyright (C) 2017, Robosense, Tony Zhang
+ *
+ *
+ *  License: Modified BSD Software License Agreement
+ *
+ *  $Id$
+ */
+
+/** \file
+ *
+ *  Input classes for the RSLIDAR RS-16 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 "input.h"
+
+
+#include <iostream>
+
+#include <QDebug>
+#include <QDateTime>
+
+extern volatile sig_atomic_t flag;
+namespace rs_driver {
+static const size_t data_packet_size = 1400;//sizeof(rslidar_msgs::rslidarPacket().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) {
+
+Input::Input( uint16_t port)
+    : port_(port) {
+    devip_str_ = "192.168.1.102";
+//  private_nh.param("device_ip", devip_str_, std::string(""));
+//  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) {
+
+InputSocket::InputSocket(uint16_t port)
+    : Input( port) {
+  sockfd_ = -1;
+
+  if (!devip_str_.empty()) {
+    inet_aton(devip_str_.c_str(), &devip_);
+  }
+
+  std::cout<<"Opening UDP socket: port " << port<<std::endl;
+//  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;
+  }
+
+  int opt = 1;
+  if (setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (const void *)&opt,
+                 sizeof(opt))) {
+    perror("setsockopt error!\n");
+    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;
+  }
+}
+
+/** @brief destructor */
+InputSocket::~InputSocket(void) { (void)close(sockfd_); }
+
+/** @brief Get one rslidar msop or difop packet. */
+//int InputSocket::getPacket(rslidar_msgs::rslidarPacket *pkt,
+//                           const double time_offset) {
+int InputSocket::getPacket(m1packet * pkt,
+                               const double time_offset) {
+//  double time1 = ros::Time::now().toSec();
+  double time1 = QDateTime::currentMSecsSinceEpoch();
+  time1 = time1/1000.0;
+  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 (flag == 1) {
+    // Receive packets that should now be available from the
+    // socket using a blocking read.
+    // poll() until input available
+    do {
+      int retval = poll(fds, 1, POLL_TIMEOUT);
+      if (retval < 0) // poll() error?
+      {
+        if (errno != EINTR)
+        {
+            std::cout<<"poll() error:"<< strerror(errno)<<std::endl;
+        }
+//          ROS_ERROR("poll() error: %s", strerror(errno));
+        return 1;
+      }
+      if (retval == 0) // poll() timeout?
+      {
+          std::cout<<"Rslidar Data Packet poll() timeout"<<std::endl;
+  //      ROS_WARN("Rslidar Data Packet poll() timeout");
+
+        char buffer_data[8] = "re-con";
+        memset(&sender_address, 0, sender_address_len); // initialize to zeros
+        sender_address.sin_family = AF_INET;            // host byte order
+        sender_address.sin_port = htons(
+            MSOP_DATA_PORT_NUMBER); // port in network byte order, set any value
+        sender_address.sin_addr.s_addr =
+            devip_.s_addr; // automatically fill in my IP
+        sendto(sockfd_, &buffer_data, strlen(buffer_data), 0,
+               (sockaddr *)&sender_address, sender_address_len);
+        return 1;
+      }
+      if ((fds[0].revents & POLLERR) || (fds[0].revents & POLLHUP) ||
+          (fds[0].revents & POLLNVAL)) // device error?
+      {
+          std::cout<<"poll() reports Rslidar error"<<std::endl;
+//        ROS_ERROR("poll() reports Rslidar error");
+        return 1;
+      }
+    } while ((fds[0].revents & POLLIN) == 0);
+    ssize_t nbytes = recvfrom(sockfd_, &pkt->data[0], data_packet_size, 0,
+                              (sockaddr *)&sender_address, &sender_address_len);
+
+    if (nbytes < 0) {
+      if (errno != EWOULDBLOCK) {
+        perror("recvfail");
+        std::cout<<"recvfail"<<std::endl;
+ //       ROS_INFO("recvfail");
+        return 1;
+      }
+    } else if ((size_t)nbytes == data_packet_size) {
+      if (devip_str_ != "" && sender_address.sin_addr.s_addr != devip_.s_addr) {
+        continue;
+      } else {
+        if (pkt->data[0] != 0x55 && pkt->data[0] != 0xa5) {
+            std::cout<<"No right packets!"<<std::endl;
+ //         ROS_INFO_STREAM("No right packets!");
+          continue;
+        }
+        break; // done
+      }
+    }
+
+    std::cout<<"incomplete rslidar packet read: " << nbytes << " bytes"<<std::endl;
+ //   ROS_DEBUG_STREAM("incomplete rslidar packet read: " << nbytes << " bytes");
+  }
+  if (flag == 0) {
+    abort();
+  }
+  // Average the times at which we begin and end reading.  Use that to
+  // estimate when the scan occurred. Add the time offset.
+
+  double time2 = QDateTime::currentMSecsSinceEpoch();
+  time2 = time2/1000.0;
+  pkt->stamp = (time2 + time1)/2.0 + time_offset;
+//  double time2 = ros::Time::now().toSec();
+//  pkt->stamp = ros::Time((time2 + time1) / 2.0 + time_offset);
+
+  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)
+InputPCAP::InputPCAP( uint16_t port,
+                     double packet_rate, std::string filename, bool read_once,
+                     bool read_fast, double repeat_delay)
+    : Input( 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);
+  read_once_= false;
+  read_fast_ = false;
+  repeat_delay_ = 0.0;
+
+  if (read_once_)
+      std::cout<<"Read input file only once."<<std::endl;
+ //   ROS_INFO("Read input file only once.");
+  if (read_fast_)
+      std::cout<<"Read input file as quickly as possible."<<std::endl;
+//    ROS_INFO("Read input file as quickly as possible.");
+  if (repeat_delay_ > 0.0)
+      std::cout<<"Delay "<<repeat_delay_<<" %.3f seconds before repeating input file."<<std::endl;
+//    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());
+  std::cout<<"Opening PCAP file "<<filename_<<std::endl;
+//  ROS_INFO_STREAM("Opening PCAP file " << filename_);
+  if ((pcap_ = pcap_open_offline(filename_.c_str(), errbuf_)) == NULL) {
+      std::cout<<"Error opening rslidar socket dump file."<<std::endl;
+//    ROS_FATAL("Error opening rslidar 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 rslidar msop or difiop packet. */
+//int InputPCAP::getPacket(rslidar_msgs::rslidarPacket *pkt,
+//                         const double time_offset) {
+int InputPCAP::getPacket(m1packet * pkt,const double time_offset) {
+  struct pcap_pkthdr *header;
+  const u_char *pkt_data;
+
+  while (flag == 1) {
+    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 (!devip_str_.empty() &&
+          (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, data_packet_size);
+
+      double timex = QDateTime::currentMSecsSinceEpoch();
+      timex = timex/1000.0;
+      pkt->stamp = timex;
+//      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?
+    {
+        std::cout<<"Error "<<res<<" reading rslidar packet:"<<pcap_geterr(pcap_)<<std::endl;
+//      ROS_WARN("Error %d reading rslidar packet: %s", res, pcap_geterr(pcap_));
+      return -1;
+    }
+
+    if (read_once_) {
+        std::cout<<"end of file reached -- done reading."<<std::endl;
+ //     ROS_INFO("end of file reached -- done reading.");
+      return -1;
+    }
+
+    if (repeat_delay_ > 0.0) {
+        std::cout<<"end of file reached -- delaying "<<repeat_delay_<<" seconds."<<std::endl;
+//      ROS_INFO("end of file reached -- delaying %.3f seconds.", repeat_delay_);
+//      usleep(rint(repeat_delay_ * 1000000.0));
+    }
+
+    std::cout<<"replaying rslidar dump file"<<std::endl;
+//    ROS_DEBUG("replaying rslidar 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
+
+  if (flag == 0) {
+    abort();
+  }
+}
+}

src/driver/driver_lidar_rsm1/input.h

@@ -0,0 +1,160 @@
+/*
+ *  Copyright (C) 2007 Austin Robot Technology, Patrick Beeson
+ *  Copyright (C) 2009, 2010 Austin Robot Technology, Jack O'Quin
+ *  Copyright (C) 2015, Jack O'Quin
+ *	Copyright (C) 2017, Robosense, Tony Zhang
+ *
+ *
+ *  License: Modified BSD Software License Agreement
+ *
+ *  $Id$
+ */
+
+/** \file
+ *
+ *  Input classes for the RSLIDAR RS-16 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
+ */
+
+#ifndef __RSLIDAR_INPUT_H_
+#define __RSLIDAR_INPUT_H_
+
+#include <unistd.h>
+#include <stdio.h>
+#include <pcap.h>
+#include <netinet/in.h>
+//#include <ros/ros.h>
+//#include <rslidar_msgs/rslidarPacket.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 <signal.h>
+
+
+namespace rs_driver {
+    static uint16_t MSOP_DATA_PORT_NUMBER = 6699;     // rslidar default msop data port on PC
+    static uint16_t DIFOP_DATA_PORT_NUMBER = 7788;     // rslidar default difop data port on PC
+
+    struct m1packet
+    {
+        double stamp; //second
+        unsigned char data[1400];
+    };
+
+/**
+ *  从在线的网络数据或离线的网络抓包数据（pcap文件）中提取出lidar的原始数据，即packet数据包
+ * @brief The Input class,
+     *
+     * @param private_nh  一个NodeHandled,用于通过节点传递参数
+     * @param port
+     * @returns 0 if successful,
+     *          -1 if end of file
+     *          >0 if incomplete packet (is this possible?)
+ */
+    class Input {
+    public:
+//        Input(ros::NodeHandle private_nh, uint16_t port);
+
+        Input( uint16_t port);
+
+        virtual ~Input() {}
+
+//        virtual int getPacket(rslidar_msgs::rslidarPacket *pkt,
+//                              const double time_offset) = 0;
+
+        virtual int getPacket(m1packet * pkt,
+                              const double time_offset) = 0;
+
+    protected:
+//        ros::NodeHandle private_nh_;
+        uint16_t port_;
+        std::string devip_str_;
+    };
+
+/** @brief Live rslidar input from socket. */
+    class InputSocket : public Input {
+    public:
+//        InputSocket(ros::NodeHandle private_nh,
+//                    uint16_t port = MSOP_DATA_PORT_NUMBER);
+
+        InputSocket(
+                    uint16_t port = MSOP_DATA_PORT_NUMBER);
+
+        virtual ~InputSocket();
+
+//        virtual int getPacket(rslidar_msgs::rslidarPacket *pkt,
+//                              const double time_offset);
+
+        virtual int getPacket(m1packet * pkt,
+                              const double time_offset);
+
+
+    private:
+
+    private:
+        int sockfd_;
+        in_addr devip_;
+
+        int Ret;
+        int len;
+    };
+
+
+/** @brief rslidar input from PCAP dump file.
+   *
+   * Dump files can be grabbed by libpcap
+   */
+    class InputPCAP : public Input {
+    public:
+//        InputPCAP(ros::NodeHandle private_nh,
+//                  uint16_t port = MSOP_DATA_PORT_NUMBER,
+//                  double packet_rate = 0.0,
+//                  std::string filename = "",
+//                  bool read_once = false,
+//                  bool read_fast = false,
+//                  double repeat_delay = 0.0);
+
+        InputPCAP(uint16_t port = MSOP_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(rslidar_msgs::rslidarPacket *pkt,
+//                              const double time_offset);
+
+        virtual int getPacket(m1packet * pkt,const double time_offset);
+
+
+    private:
+ //       ros::Rate packet_rate_;
+        double 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_;
+    };
+
+}
+
+#endif // __RSLIDAR_INPUT_H

src/driver/driver_lidar_rsm1/limit.csv

@@ -0,0 +1,2 @@
+0
+20000

src/driver/driver_lidar_rsm1/main.cpp

@@ -0,0 +1,102 @@
+#include <QCoreApplication>
+
+
+#include "modulecomm.h"
+
+#include "input.h"
+
+#include "rsdriver.h"
+#include "convert.h"
+
+#include <QMutex>
+#include <QWaitCondition>
+#include <thread>
+
+volatile sig_atomic_t flag = 1;
+
+
+std::shared_ptr<rs_driver::lidarscan>  grawscan;
+QMutex gMutexScan;
+bool gbUpdate = false;
+
+QMutex gMutexWait;
+QWaitCondition gWait;
+
+rslidar_pointcloud::Convert * gpConvert;
+
+void * g_lidar_pc;
+
+std::thread * gthreadpoll,*gthreadconvert;
+
+
+void PublishPointCloud(std::shared_ptr<rs_driver::lidarscan>  rawscan)
+{
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud;
+    gpConvert->processScan(rawscan,point_cloud);
+
+    char * strOut = new char[4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->width * sizeof(pcl::PointXYZI)];
+
+    int * pHeadSize = (int *)strOut;
+    *pHeadSize = 4 + point_cloud->header.frame_id.size()+4+8;
+    memcpy(strOut+4,point_cloud->header.frame_id.c_str(),point_cloud->header.frame_id.size());
+    pcl::uint32_t * pu32 = (pcl::uint32_t *)(strOut+4+sizeof(point_cloud->header.frame_id.size()));
+    *pu32 = point_cloud->header.seq;
+    memcpy(strOut+4+sizeof(point_cloud->header.frame_id.size()) + 4,&point_cloud->header.stamp,8);
+    pcl::PointXYZI * p;
+    p = (pcl::PointXYZI *)(strOut +4+sizeof(point_cloud->header.frame_id.size()) + 4+8 );
+    memcpy(p,point_cloud->points.data(),point_cloud->width * sizeof(pcl::PointXYZI));
+
+    iv::modulecomm::ModuleSendMsg(g_lidar_pc,strOut,4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->width * sizeof(pcl::PointXYZI));
+    delete strOut;
+}
+
+void pollthread()
+{
+    rs_driver::rslidarDriver lidarm1;
+    while(true)
+    {
+        std::shared_ptr<rs_driver::lidarscan>  rawscan;
+        if(true == lidarm1.poll(rawscan))
+        {
+            gMutexScan.lock();
+            grawscan = rawscan;
+            gbUpdate = true;
+            gMutexScan.unlock();
+            gWait.wakeAll();
+
+        }
+    }
+}
+
+void convertthread()
+{
+    while(true)
+    {
+        gMutexWait.lock();
+        gWait.wait(&gMutexWait,300);
+        gMutexWait.unlock();
+        if(gbUpdate)
+        {
+            gMutexScan.lock();
+            std::shared_ptr<rs_driver::lidarscan>  rawscan;
+            rawscan = grawscan;
+            gbUpdate = false;
+            gMutexScan.unlock();
+            PublishPointCloud(rawscan);
+        }
+    }
+}
+
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
+
+    g_lidar_pc = iv::modulecomm::RegisterSend("lidar_pc",20000000,1);
+
+    gpConvert = new rslidar_pointcloud::Convert("ChannelNum.csv","limit.csv");
+
+    gthreadpoll = new std::thread(pollthread);
+    gthreadconvert = new std::thread(convertthread);
+
+    return a.exec();
+}

src/driver/driver_lidar_rsm1/rawdata.cc

@@ -0,0 +1,417 @@
+/*
+ *  Copyright (C) 2007 Austin Robot Technology, Patrick Beeson
+ *  Copyright (C) 2009, 2010, 2012 Austin Robot Technology, Jack O'Quin
+ *	Copyright (C) 2017 Robosense, Tony Zhang
+ *
+ *  License: Modified BSD Software License Agreement
+ *
+ *  $Id$
+ */
+
+/**
+ *  @file
+ *
+ *  RSLIDAR 3D LIDAR data accessor class implementation.
+ *
+ *  Class for unpacking raw RSLIDAR LIDAR packets into useful
+ *  formats.
+ *
+ */
+#include "rawdata.h"
+
+#define RS_Grabber_toRadians(x) ((x)*M_PI / 180.0)
+#define RS_SWAP_HIGHLOW(x) ((((x)&0xFF) << 8) | (((x)&0xFF00) >> 8))
+// #define RS_SWAP_HIGHLOW(x) (((x) / 256) + (((x) % 256) * 256))
+
+namespace rslidar_rawdata
+{
+RawData::RawData()
+{
+  point_idx_ = 0;
+  last_pitch_index_ = 0;
+  skip_block_ = 0;
+  last_pkt_index_ = -1;
+  pitch_max_ = 25000;
+  is_init_difop_ = false;
+
+}
+
+void RawData::loadConfigFile(std::string strchannelPath, std::string strlimitPath)
+//void RawData::loadConfigFile(ros::NodeHandle node, ros::NodeHandle private_nh)
+{
+  std::string channelPath, limitPath, slowPath;
+  std::string input_difop_packets_topic;
+
+  channelPath = strchannelPath;
+  limitPath = strlimitPath;
+
+//  private_nh.param("channel_path", channelPath, std::string(""));
+//  private_nh.param("limit_path", limitPath, std::string(""));
+//  private_nh.param("input_difop_packets_topic", input_difop_packets_topic, std::string("rslidar_difop_packets"));
+
+  //=============================================================
+  FILE* f_channel = fopen(channelPath.c_str(), "r");
+  if (!f_channel)
+  {
+      std::cout<<"channelPath: " << channelPath << " does not exist"<<std::endl;
+ //   ROS_ERROR_STREAM("channelPath: " << channelPath << " does not exist");
+
+    for (int i = 0; i < MEMS_SCANS_PER_FIRING; i++)
+    {
+      channel_num_[i] = 0;
+      pitch_offset_[i] = 0;
+      yaw_offset_[i] = 0;
+    }
+    pitch_rate_ = 1;
+    yaw_rate_ = 1;
+  }
+  else
+  {
+      std::cout<<"Loading channelNum corrections file!"<<std::endl;
+//    ROS_INFO_STREAM("Loading channelNum corrections file!");
+
+    int loopm = 0;
+    float tmpBuf[32];
+    const float defaultTmpBuf[32] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, // distance correction offset
+                        1.0f, 1.0f, // pitch yaw rate
+                        0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, // pitch offset
+                        0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, // yaw offset
+                        -6.75f, 6.75f, -6.75f, 6.75f, -6.75f, 6.75f, -6.75f, 6.75f, -6.75f, 6.75f, -6.75f, 6.75f};// yaw start angle
+    while (!feof(f_channel))
+    {
+      if (1 != fscanf(f_channel, "%f%*[^\n]%*c\n", &tmpBuf[loopm]))
+      {
+        break;
+      }
+      loopm++;
+      if (loopm >= 32)
+      {
+        break;
+      }
+    }
+    if (loopm != 32)
+    {
+        std::cout<<"the format of " << channelPath << " is not correct, use default data"<<std::endl;
+//      ROS_ERROR_STREAM("the format of " << channelPath << " is not correct, use default data");
+      for (int i = 0; i < 32; ++i)
+      {
+        tmpBuf[i] = defaultTmpBuf[i];
+      }
+    }
+
+    for (int i = 0; i < MEMS_SCANS_PER_FIRING; i++)
+    {
+      channel_num_[i] = (int)(tmpBuf[i]);
+      pitch_offset_[i] = tmpBuf[8 + i];
+      yaw_offset_[i] = tmpBuf[14 + i];
+    }
+    pitch_rate_ = tmpBuf[6];
+    yaw_rate_ = tmpBuf[7];
+
+    fclose(f_channel);
+  }
+
+  //=============================================================
+  FILE* f_limit = fopen(limitPath.c_str(), "r");
+  if (!f_limit)
+  {
+      std::cout<<"limitPath: " << limitPath <<" does not exist"<<std::endl;
+//    ROS_ERROR_STREAM("limitPath: " << limitPath <<" does not exist");
+    distance_max_thd_ = 200.0;
+    distance_min_thd_ = 0.0;
+  }
+  else
+  {
+      std::cout<<"Loading limit file!"<<std::endl;
+//    ROS_INFO_STREAM("Loading limit file!");
+    float tmp_min;
+    float tmp_max;
+
+    if (1 == fscanf(f_limit, "%f\n", &tmp_min) &&
+        1 == fscanf(f_limit, "%f\n", &tmp_max))
+    {
+      distance_max_thd_ = tmp_max / 100.0;
+      distance_min_thd_ = tmp_min / 100.0;
+    }
+    else
+    {
+        std::cout<<"the format of " << limitPath << " is not correct, use default data"<<std::endl;
+  //    ROS_ERROR_STREAM("the format of " << limitPath << " is not correct, use default data");
+      distance_max_thd_ = 200.0;
+      distance_min_thd_ = 0.0;
+    }
+
+    fclose(f_limit);
+  }
+
+  // lookup table init, -10 ~ 10 deg, 0.01 resolution
+  this->tan_lookup_table_.resize(2000);
+
+  for (int i = -1000; i < 1000; i++)
+  {
+    double rad = RS_Grabber_toRadians(i / 100.0f);
+    this->tan_lookup_table_[i + 1000] = std::tan(rad);
+  }
+
+  input_ref_ << 0.748956, 0.414693, -0.414693, -0.748956, 0,
+           0.33131,  0.454981,  0.454981,  0.33131,  0.5,
+          -0.573846,-0.78805,  -0.78805,  -0.573846,-0.866025;
+
+  rotate_gal_ << 1, 0, 0, 0, 0.965926, -0.258819, 0, 0.258819, 0.965926;
+  // std::cout << "input_ref_" << input_ref_ << std::endl;
+  // std::cout << "rotate_gal_" << rotate_gal_ << std::endl;
+  // advertise output temp info
+  std::string temp_info_topic = "temperature";
+//  private_nh.param("output_temp_topic", temp_info_topic, std::string("temperature"));
+//  temp_output_ = node.advertise<std_msgs::Float32>(temp_info_topic, 10);
+
+//  // subscribe to difop topic
+//  difop_sub_ = node.subscribe(input_difop_packets_topic, 10, &RawData::processDifop, (RawData*)this);
+}
+
+void RawData::processDifop(const std::shared_ptr<rs_driver::m1packet> & difop_msg)
+//void RawData::processDifop(const rslidar_msgs::rslidarPacket::ConstPtr& difop_msg)
+{
+  const rs_difop* difop_ptr = (const rs_difop*)&difop_msg->data[0];
+
+  if (difop_ptr->framHeader[0] == 0xa5 && difop_ptr->framHeader[1] == 0xff &&
+      difop_ptr->framHeader[2] == 0x00 && difop_ptr->framHeader[3] == 0x5a)
+  {
+    if (!this->is_init_difop_)
+    {
+      bool cali_param_flag = true;
+      // check difop reigon has been flashed the right data
+      if ((difop_ptr->caliParam[9].data[0] == 0x00 && difop_ptr->caliParam[9].data[1] == 0x00) && 
+          (difop_ptr->caliParam[10].data[0] == 0x00 && difop_ptr->caliParam[10].data[1] == 0x00))
+      {
+        cali_param_flag = false;
+      }
+      else
+      {
+        cali_param_flag = true;
+      }
+      
+      // calibration parameter
+      if (cali_param_flag)
+      {
+        int bit1, bit2, bit3, symbolbit;
+        for (int i = 0; i < MEMS_SCANS_PER_FIRING; ++i)
+        {
+          // distance cor
+          bit1 = static_cast<int>(difop_ptr->caliParam[0 + i].paramSign);
+          bit2 = static_cast<int>(difop_ptr->caliParam[0 + i].data[0]);
+          bit3 = static_cast<int>(difop_ptr->caliParam[0 + i].data[1]);
+          if (bit1 == 0)
+            symbolbit = 1;
+          else if (bit1 == 1)
+            symbolbit = -1;
+          channel_num_[i] = 0.01 * (bit2 * 256 + bit3) * symbolbit; 
+
+          // pitch offset angle
+          bit1 = static_cast<int>(difop_ptr->caliParam[8 + i].paramSign);
+          bit2 = static_cast<int>(difop_ptr->caliParam[8 + i].data[0]);
+          bit3 = static_cast<int>(difop_ptr->caliParam[8 + i].data[1]);
+          if (bit1 == 0)
+            symbolbit = 1;
+          else if (bit1 == 1)
+            symbolbit = -1;
+          pitch_offset_[i] = 0.01 * (bit2 * 256 + bit3) * symbolbit; 
+
+          // yaw offset angle
+          bit1 = static_cast<int>(difop_ptr->caliParam[14 + i].paramSign);  
+          bit2 = static_cast<int>(difop_ptr->caliParam[14 + i].data[0]);
+          bit3 = static_cast<int>(difop_ptr->caliParam[14 + i].data[1]);
+          if (bit1 == 0) 
+            symbolbit = 1;
+          else if (bit1 == 1)
+            symbolbit = -1;
+          yaw_offset_[i] = 0.01 * (bit2 * 256 + bit3) * symbolbit;
+          // printf("index: %d, dis_cor: %d, pitch_offset_: %f, yaw_offset_: %f\n", i, channel_num_[i], pitch_offset_[i], yaw_offset_[i]);
+        }
+        // pitch_rate
+        bit1 = static_cast<int>(difop_ptr->caliParam[6].paramSign);
+        bit2 = static_cast<int>(difop_ptr->caliParam[6].data[0]);
+        bit3 = static_cast<int>(difop_ptr->caliParam[6].data[1]);
+        if (bit1 == 0)
+          symbolbit = 1;
+        else if (bit1 == 1)
+          symbolbit = -1;
+        pitch_rate_ = 0.01 * (bit2 * 256 + bit3) * symbolbit; 
+
+        // yaw_rate
+        bit1 = static_cast<int>(difop_ptr->caliParam[7].paramSign);
+        bit2 = static_cast<int>(difop_ptr->caliParam[7].data[0]);
+        bit3 = static_cast<int>(difop_ptr->caliParam[7].data[1]);
+        if (bit1 == 0)
+          symbolbit = 1;
+        else if (bit1 == 1)
+          symbolbit = -1;
+        yaw_rate_ = 0.01 * (bit2 * 256 + bit3) * symbolbit; 
+        // printf("pitch_rate: %f, yaw_rate: %f\n", pitch_rate_, yaw_rate_);
+      }
+      this->is_init_difop_ = true;
+      std::cout<<"read angle from difop!"<<std::endl;
+ //     ROS_INFO_STREAM("read angle from difop!");
+    }
+
+  }
+  else
+  {
+    return;
+  }
+  
+}
+
+//------------------------------------------------------------
+
+/** @brief convert raw packet to point cloud
+ *
+ *  @param pkt raw packet to unpack
+ *  @param pc shared pointer to point cloud (points are appended)
+ */
+
+float RawData::pitchConvertDeg(int deg)  // convert deg into  -625 ~ +625
+{
+  float result_f;
+  float deg_f = deg;
+
+  result_f = (deg_f * (1250.0f / pitch_max_) - 625.0f);
+  // printf("%d,%f;%f\n",deg,deg_f,result_f);
+  return result_f;
+}
+
+float RawData::yawConvertDeg(int deg)  // convert deg into  -675 ~ 675
+{
+  float result_f;
+  float deg_f = deg;
+
+  result_f = (deg_f * (1350.0f / 65534.0f) - 675.0f);
+  // printf("%d,%f;%f\n",deg,deg_f,result_f);
+  return result_f;
+}
+
+float RawData::pixelToDistance(int distance, int dsr)
+{
+  float result;
+  int cor = channel_num_[dsr];
+
+  if (distance <= cor)
+  {
+    result = 0.0;
+  }
+  else
+  {
+    result = distance - cor;
+  }
+  return result;
+}
+
+void RawData::unpack_MEMS(const rs_driver::m1packet &pkt, pcl::PointCloud<pcl::PointXYZI>::Ptr pointcloud)
+//void RawData::unpack_MEMS(const rslidar_msgs::rslidarPacket& pkt, pcl::PointCloud<pcl::PointXYZI>::Ptr pointcloud)
+{
+  float pitch, yaw;  // degree
+  int index_temp;
+  pcl::PointXYZI point;
+
+  const raw_mems_packet_t* raw = (const raw_mems_packet_t*)&pkt.data[0];
+  
+  // check if it is lost udp packet, and need to set nan data and increase index of point
+  // normally, index of packet will increase one if it doesn't lose packet
+  int pkt_index = 256 * raw->cmd[0] + raw->cmd[1];
+  // lose packets limit: 1 ~ 630
+  if (pkt_index - last_pkt_index_ > 1 && pkt_index - last_pkt_index_ <  POINT_COUNT_PER_VIEWFIELD / MEMS_BLOCKS_PER_PACKET 
+      && last_pkt_index_ != -1)
+  {
+    int lose_pkt_count = pkt_index - last_pkt_index_ - 1;
+    while (lose_pkt_count--)
+    {
+      for (int block = 0; block < MEMS_BLOCKS_PER_PACKET; block ++)
+      {
+        for (int dsr = 1; dsr < MEMS_SCANS_PER_FIRING; dsr++)
+        {
+          point.x = NAN;
+          point.y = NAN;
+          point.z = NAN;
+          point.intensity = 0;
+          pointcloud->at(point_idx_, dsr - 1) = point;
+        }
+        point_idx_ ++;
+      }
+    } 
+  }
+  
+  // unpack block
+  for (int block = skip_block_; block < MEMS_BLOCKS_PER_PACKET; block++)  // 1 packet:25 data blocks
+  {
+    raw_mems_block_t* pBlockPkt = (raw_mems_block_t*)(raw->blocks + block);
+    index_temp = RS_SWAP_HIGHLOW(pBlockPkt->pitch);
+    // pitch index will increase until new frame
+    if (last_pitch_index_ > index_temp)
+    {
+      if (last_pkt_index_ == POINT_COUNT_PER_VIEWFIELD / MEMS_BLOCKS_PER_PACKET)
+      {
+        pitch_max_ = last_pitch_index_;
+      }
+      point_idx_ = 0;
+      skip_block_ = block;
+      last_pitch_index_ = index_temp;
+
+      //temperature publish
+//      std_msgs::Float32 tempMsg; // temperature value, Celsius degree
+//      tempMsg.data = raw->temp;
+//      temp_output_.publish(tempMsg);
+      break;
+    }
+    last_pitch_index_ = index_temp;
+
+    pitch = pitchConvertDeg(RS_SWAP_HIGHLOW(pBlockPkt->pitch)) * ANGLE_RESOLUTION;
+    yaw = yawConvertDeg(RS_SWAP_HIGHLOW(pBlockPkt->yaw)) * ANGLE_RESOLUTION;
+
+    // unpack points
+    Eigen::Matrix<float, 3, 1> n_gal;
+    Eigen::Matrix<float, 3, 1> i_out;
+    for (int dsr = 1; dsr < MEMS_SCANS_PER_FIRING; dsr++)  // 5 channels, channel index 1 ~ 5
+    {
+      float temp_a, temp_b, temp_c, tanax, tanay;
+      int ax, ay;
+      
+      ax = (int)(100 * pitch_rate_ * (pitch + pitch_offset_[dsr]));  // ax: -1000 ~ 1000, 0.01deg
+      ay = (int)(100 * yaw_rate_ * (yaw + yaw_offset_[dsr]));        // ay: -1000 ~ 1000, 0.01deg
+      tanax = this->tan_lookup_table_[ax + 1000];
+      tanay = this->tan_lookup_table_[ay + 1000];
+      tanax = -tanax;
+      tanay = -tanay;
+
+      // calc i_out
+      n_gal << tanay, -tanax, 1;
+      n_gal.normalize();
+      n_gal = rotate_gal_ * n_gal;
+      i_out = input_ref_.col(dsr - 1) - 2 * n_gal * n_gal.transpose() * input_ref_.col(dsr - 1);
+
+      float distance = pixelToDistance(RS_SWAP_HIGHLOW(pBlockPkt->channel[dsr].distance_1), dsr);
+      distance = distance * DISTANCE_RESOLUTION;
+
+      if (distance > distance_max_thd_ || distance < distance_min_thd_)  // invalid data
+      {
+        point.x = NAN;
+        point.y = NAN;
+        point.z = NAN;
+        point.intensity = 0;
+      }
+      else
+      {
+        point.x = i_out(2) * distance;
+        point.y = i_out(0) * distance;
+        point.z = i_out(1) * distance;
+        point.intensity = RS_SWAP_HIGHLOW(pBlockPkt->channel[dsr].intensity_1);
+      }
+
+      if (point_idx_ < POINT_COUNT_PER_VIEWFIELD)
+        pointcloud->at(point_idx_, dsr - 1) = point;
+    }
+    point_idx_ ++;
+    skip_block_ = 0;
+  }
+  last_pkt_index_ = pkt_index;
+}
+}  // namespace rslidar_rawdata

src/driver/driver_lidar_rsm1/rawdata.h

@@ -0,0 +1,253 @@
+/* -*- mode: C++ -*-
+ *
+ *  Copyright (C) 2007 Austin Robot Technology, Yaxin Liu, Patrick Beeson
+ *  Copyright (C) 2009, 2010, 2012 Austin Robot Technology, Jack O'Quin
+ *	Copyright (C) 2017 Robosense, Tony Zhang
+ *
+ *  License: Modified BSD Software License Agreement
+ *
+ *  $Id$
+ */
+
+/** @file
+ *
+ *  @brief Interfaces for interpreting raw packets from the Velodyne 3D LIDAR.
+ *
+ *  @author Yaxin Liu
+ *  @author Patrick Beeson
+ *  @author Jack O'Quin
+ *	@author Tony Zhang
+ */
+
+#ifndef _RAWDATA_H
+#define _RAWDATA_H
+
+
+#include  "stdint.h"
+#include <vector>
+#include <string>
+#include <memory>
+#include <input.h>
+
+//#include "std_msgs/String.h"
+//#include "std_msgs/Float32.h"
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+//#include <pcl_conversions/pcl_conversions.h>
+//#include <pcl_ros/impl/transforms.hpp>
+//#include <pcl_ros/point_cloud.h>
+//#include <ros/package.h>
+//#include <ros/ros.h>
+//#include <rslidar_msgs/rslidarPacket.h>
+//#include <rslidar_msgs/rslidarScan.h>
+
+namespace rslidar_rawdata
+{
+
+static const float ANGLE_RESOLUTION = 0.01f; /**< degrees */
+
+static const float DISTANCE_MAX = 200.0f;       /**< meters */
+static const float DISTANCE_MIN = 0.5f;         /**< meters */
+static const float DISTANCE_RESOLUTION = 0.01f; /**< meters */
+static const float DISTANCE_MAX_UNITS = (DISTANCE_MAX / DISTANCE_RESOLUTION + 1.0f);
+
+/** Special Defines for MEMS support **/
+static const int MEMS_BLOCKS_PER_PACKET = 25;
+static const int MEMS_SCANS_PER_FIRING = 6;
+static const int POINT_COUNT_PER_VIEWFIELD = 15750;
+/** used for unpacking the first two data bytes in a block
+ *
+ *  They are packed into the actual data stream misaligned.  I doubt
+ *  this works on big endian machines.
+ */
+// union two_bytes {
+//  uint16_t uint;
+//  uint8_t bytes[2];
+//};
+
+static const int PACKET_SIZE = 1400;
+static const int PACKET_STATUS_SIZE = 80;
+
+/** \brief Raw Rsldar packet.
+ *
+ *  revolution is described in the device manual as incrementing
+ *    (mod 65536) for each physical turn of the device.  Our device
+ *    seems to alternate between two different values every third
+ *    packet.  One value increases, the other decreases.
+ *
+ *  \todo figure out if revolution is only present for one of the
+ *  two types of status fields
+ *
+ *  status has either a temperature encoding or the microcode level
+ */
+
+// MEMS channel return
+typedef struct raw_mems_channel
+{
+  uint16_t intensity_1;
+  uint16_t distance_1;
+  uint16_t intensity_2;
+  uint16_t distance_2;
+} raw_mems_channel_t;
+
+// MEMS block
+typedef struct raw_mems_block
+{
+  uint16_t pitch;
+  uint16_t yaw;
+  raw_mems_channel_t channel[MEMS_SCANS_PER_FIRING];
+} raw_mems_block_t;
+
+// MEMS timestamp
+typedef struct raw_mems_timestamp
+{
+  uint8_t year;
+  uint8_t month;
+  uint8_t day;
+  uint8_t hour;
+  uint8_t minute;
+  uint8_t second;
+  uint16_t ms;
+  uint16_t us;
+}raw_mems_timestamp_t;
+
+// MEMS packet
+typedef struct raw_mems_packet
+{
+  uint8_t sync[4];
+  uint8_t cmd[4];
+  int8_t temp;
+  uint8_t reserved;
+  raw_mems_timestamp_t timestamp;
+  raw_mems_block_t blocks[MEMS_BLOCKS_PER_PACKET];
+  uint8_t status[PACKET_STATUS_SIZE];
+} raw_mems_packet_t;
+
+typedef struct rs_difop_ether
+{
+  unsigned char  ip_local[4];
+  unsigned char  ip_remote[4];
+  unsigned char  mac[6];
+  unsigned char  ip_msop_port[2];
+  unsigned char  net_reserve1[2];
+  unsigned char  ip_difop_port[2];
+  unsigned char  net_reserve2[2];
+}rs_difop_ether;
+
+typedef struct rs_difop_fov
+{
+  unsigned char fov_start[2];
+  unsigned char fov_end[2];
+}rs_difop_fov;
+
+typedef struct rs_difop_ver_info
+{
+  unsigned char pl_ver[5];
+  unsigned char ps_ver[5];
+}rs_difop_ver_info;
+
+typedef struct rs_difop_time_info
+{
+  unsigned char syn_methord;
+  unsigned char syn_status;
+  unsigned char current_time[10];
+}rs_difop_time_info;
+
+typedef struct rs_difop_run_sts
+{
+  unsigned char current1[3];
+  unsigned char current2[3];
+  unsigned char voltage1[2];
+  unsigned char voltage2[2];
+  unsigned char reserve[8];
+}rs_difop_run_sts;
+
+typedef struct rs_difop_cali_param
+{
+  unsigned char paramSign;
+  unsigned char data[2];
+}rs_difop_cali_param;
+
+
+typedef struct rs_difop
+{
+  unsigned char framHeader[8];
+  unsigned char reserve1;
+  unsigned char framRate;
+  rs_difop_ether ether;
+  rs_difop_fov fovSetting;
+  unsigned char reserve2[4];
+  rs_difop_ver_info verInfo;
+  unsigned char reserve3[242];
+  unsigned char serialNumber[6];
+  unsigned char reserve4[2];
+  unsigned char waveMode;
+  rs_difop_time_info timeInfo;
+  rs_difop_run_sts runStatus;
+  unsigned char reserve5[11];
+  unsigned char diagReserve[40];
+  unsigned char reserve6[86];
+  rs_difop_cali_param caliParam[20];
+  unsigned char reserve7[718];
+  unsigned char framTail[2];
+}rs_difop;
+
+
+/** \brief RSLIDAR data conversion class */
+class RawData
+{
+public:
+  RawData();
+
+  ~RawData()
+  {
+    this->tan_lookup_table_.clear();
+  }
+
+  /*load the cablibrated files: ChannelNum, limit, look-up table for pitch*/
+//  void loadConfigFile(ros::NodeHandle node, ros::NodeHandle private_nh);
+  void loadConfigFile(std::string strchannelPath, std::string strlimitPath);
+
+  /*unpack the MEMS UDP packet and opuput PCL PointXYZI type*/
+
+  void unpack_MEMS(const  rs_driver::m1packet & pkt, pcl::PointCloud<pcl::PointXYZI>::Ptr pointcloud);
+//  void unpack_MEMS(const rslidar_msgs::rslidarPacket& pkt, pcl::PointCloud<pcl::PointXYZI>::Ptr pointcloud);
+
+  /*convert the deg */
+  float pitchConvertDeg(int deg); 
+  float yawConvertDeg(int deg);
+  
+  /*calibrated the disctance for mems */
+  float pixelToDistance(int distance, int dsr); 
+
+  /* process difop packet*/
+  void processDifop(const std::shared_ptr<rs_driver::m1packet> & difop_msg);
+//  void processDifop(const rslidar_msgs::rslidarPacket::ConstPtr& difop_msg);
+
+private:
+  int channel_num_[MEMS_SCANS_PER_FIRING];
+  float pitch_rate_;
+  float yaw_rate_;
+  float pitch_offset_[MEMS_SCANS_PER_FIRING];
+  float yaw_offset_[MEMS_SCANS_PER_FIRING];
+  float distance_max_thd_;
+  float distance_min_thd_;
+
+  uint32_t point_idx_;
+  int last_pitch_index_;
+  int skip_block_;
+  int last_pkt_index_;
+  int pitch_max_;
+  bool is_init_difop_;
+
+  /* tan lookup table */
+  std::vector<float> tan_lookup_table_;
+
+  Eigen::Matrix<float, 3, 5> input_ref_;
+  Eigen::Matrix<float, 3, 3> rotate_gal_;
+//  ros::Publisher temp_output_;
+//  ros::Subscriber difop_sub_;
+};
+}  // namespace rslidar_rawdata
+
+#endif  // __RAWDATA_H

src/driver/driver_lidar_rsm1/rsdriver.cpp

@@ -0,0 +1,228 @@
+/*
+ *  Copyright (C) 2007 Austin Robot Technology, Patrick Beeson
+ *  Copyright (C) 2009-2012 Austin Robot Technology, Jack O'Quin
+ *	Copyright (C) 2017 Robosense, Tony Zhang
+ *
+ *  License: Modified BSD Software License Agreement
+ *
+ *  $Id$
+ */
+
+/** \file
+ *
+ *  ROS driver implementation for the RILIDAR 3D LIDARs
+ */
+#include "rsdriver.h"
+
+#include <iostream>
+//#include <rslidar_msgs/rslidarScan.h>
+
+namespace rs_driver {
+
+//rslidarDriver::rslidarDriver(ros::NodeHandle node, ros::NodeHandle private_nh) {
+    rslidarDriver::rslidarDriver() {
+  // use private node handle to get parameters
+//  private_nh.param("frame_id", config_.frame_id, std::string("rslidar"));
+
+  config_.frame_id  = "rslidar";
+
+  std::string tf_prefix =  "robo" ;//tf::getPrefixParam(private_nh);
+//  ROS_DEBUG_STREAM("tf_prefix: " << tf_prefix);
+//  config_.frame_id = tf::resolve(tf_prefix, config_.frame_id);
+  config_.time_offset = 0.0;
+  
+  // get model name, validate string, determine packet rate
+  config_.model = "MEMS";
+  config_.start_from_edge = true;
+//  private_nh.param("model", config_.model, std::string("MEMS"));
+//  private_nh.param("start_from_edge", config_.start_from_edge, true);
+  double packet_rate;  // packet frequency (Hz)
+  std::string model_full_name;
+
+  // product model
+  if (config_.model == "MEMS") 
+  {
+    packet_rate = 9450;
+    model_full_name = "RS-LiDAR-MEMS";
+  } 
+  else 
+  {
+      std::cout<<"unknown LIDAR model: " << config_.model<<std::endl;
+//    ROS_ERROR_STREAM("unknown LIDAR model: " << config_.model);
+    packet_rate = 4000;
+  }
+  std::string deviceName(std::string("Robosense ") + model_full_name);
+
+  double frequency = 15; // expected Hz rate
+
+  // default number of packets for each scan is a single revolution
+  // (fractions rounded up)
+
+  int npackets = (int)ceil(packet_rate / frequency);
+  config_.npackets = npackets;
+//  private_nh.param("npackets", config_.npackets, npackets);
+  std::cout<<config_.npackets << " packets per scan"<<std::endl;
+ // ROS_INFO_STREAM("publishing " << config_.npackets << " packets per scan");
+
+  std::string dump_file;
+  dump_file = "";
+//  private_nh.param("pcap", dump_file, std::string(""));
+
+  int msop_udp_port;
+  int difop_udp_port;
+  msop_udp_port = (int)MSOP_DATA_PORT_NUMBER;
+  difop_udp_port = (int)DIFOP_DATA_PORT_NUMBER;
+//  private_nh.param("msop_port", msop_udp_port, (int)MSOP_DATA_PORT_NUMBER);
+//  private_nh.param("difop_port", difop_udp_port, (int)DIFOP_DATA_PORT_NUMBER);
+
+  // Initialize dynamic reconfigure
+//  srv_ = boost::make_shared<
+//      dynamic_reconfigure::Server<rslidar_driver::rslidarNodeConfig>>(
+//      private_nh);
+//  dynamic_reconfigure::Server<rslidar_driver::rslidarNodeConfig>::CallbackType
+//      f;
+//  f = boost::bind(&rslidarDriver::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(
+//      "rslidar_packets", diagnostics_,
+//      FrequencyStatusParam(&diag_min_freq_, &diag_max_freq_, 0.1, 10),
+//      TimeStampStatusParam()));
+
+  // open rslidar input device or file
+  if (dump_file != "") // have PCAP file?
+  {
+     printf("dump_file != null\n");
+    // read data from packet capture file
+    msop_input_.reset(new rs_driver::InputPCAP( msop_udp_port, packet_rate, dump_file));
+    difop_input_.reset(new rs_driver::InputPCAP( difop_udp_port, packet_rate, dump_file));
+  }
+  else
+  {
+    printf("dump_file == null\n");
+    // read data from live socket
+    msop_input_.reset(new rs_driver::InputSocket( msop_udp_port));
+    difop_input_.reset(new rs_driver::InputSocket(difop_udp_port));
+  }
+
+//  // raw packet output topic
+//  msop_output_ = node.advertise<rslidar_msgs::rslidarScan>("rslidar_packets", 10);
+//  difop_output_ = node.advertise<rslidar_msgs::rslidarPacket>("rslidar_difop_packets", 10);
+
+  // receive difop packet thread
+  difop_thread_ = boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&rslidarDriver::difopPoll, this)));
+}
+
+/** poll the device
+ *
+ *  @returns true unless end of file reached
+ */
+bool rslidarDriver::poll(std::shared_ptr<lidarscan> & rawscan) {
+  // Allocate a new shared pointer for zero-copy sharing with other nodelets.
+//  rslidar_msgs::rslidarScanPtr scan(new rslidar_msgs::rslidarScan);
+  std::shared_ptr<lidarscan> scan(new lidarscan);
+  if (config_.start_from_edge) {
+    scan->packets.reserve(config_.npackets);
+ //   rslidar_msgs::rslidarPacket tmp_packet;
+    m1packet tmp_packet;
+    while (true) {
+      bool isNewFrame = false;
+      while (true) {
+        int rc = msop_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);
+      // is first pkt of new frame? one packet include 25 blocks, one block offset: 52bytes, first block offset: 20
+      for (int i = 0; i < 25; i++)
+      {
+        int index_temp = 256 * tmp_packet.data[20 + 52 * i] + tmp_packet.data[21 + 52 * i];
+        static int index_temp_zero = index_temp;
+
+        if ( index_temp < index_temp_zero)
+        {
+          index_temp_zero = index_temp;
+          isNewFrame = true;
+          break;
+        }
+        index_temp_zero = index_temp;
+      }
+
+      if (isNewFrame)
+        break;
+    }
+  } else {
+    scan->packets.resize(config_.npackets);
+    // Since the rslidar delivers data at a very high rate, keep
+    // reading and publishing scans as fast as possible.
+    for (int i = 0; i < config_.npackets; i++) {
+      while (true) {
+        // keep reading until full packet received
+        int rc = msop_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 rslidar scan.");
+//  scan->header.stamp = scan->packets.back().stamp;
+//  scan->header.frame_id = config_.frame_id;
+  scan->stamp = scan->packets.back().stamp;
+  scan->frame_id = config_.frame_id;
+
+  rawscan = scan;
+//  msop_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 rslidarDriver::difopPoll(void)
+{
+  // reading and publishing scans as fast as possible.
+ // rslidar_msgs::rslidarPacketPtr difop_packet_ptr(new rslidar_msgs::rslidarPacket);
+  std::shared_ptr<m1packet> difop_packet_ptr(new m1packet);
+ // while (ros::ok())
+  while(true)
+  {
+    // keep reading
+//    rslidar_msgs::rslidarPacket difop_packet_msg;
+    m1packet difop_packet_msg;
+    int rc = difop_input_->getPacket(&difop_packet_msg, config_.time_offset);
+    if (rc == 0)
+    {
+       std::cout << "Publishing a difop data." << std::endl;
+     // ROS_DEBUG("Publishing a difop data.");
+      *difop_packet_ptr = difop_packet_msg;
+  //    difop_output_.publish(difop_packet_ptr);
+    }
+    if (rc < 0)
+      return;  // end of file reached?
+//    ros::spinOnce();
+  }
+}
+
+//void rslidarDriver::callback(rslidar_driver::rslidarNodeConfig &config,
+//                             uint32_t level) {
+//  ROS_INFO("Reconfigure Request");
+//  config_.time_offset = config.time_offset;
+//}
+
+}

src/driver/driver_lidar_rsm1/rsdriver.h

@@ -0,0 +1,92 @@
+/* -*- mode: C++ -*- */
+/*
+ *  Copyright (C) 2012 Austin Robot Technology, Jack O'Quin
+ *	Copyright (C) 2017 Robosense, Tony Zhang
+ *
+ *  License: Modified BSD Software License Agreement
+ *
+ *  $Id$
+ */
+
+/** \file
+ *
+ *  ROS driver interface for the RSLIDAR 3D LIDARs
+ */
+#ifndef _RSDRIVER_H_
+#define _RSDRIVER_H_
+
+#include "input.h"
+//#include <diagnostic_updater/diagnostic_updater.h>
+//#include <diagnostic_updater/publisher.h>
+//#include <dynamic_reconfigure/server.h>
+//#include <pcl/point_types.h>
+//#include <pcl_conversions/pcl_conversions.h>
+//#include <pcl_ros/impl/transforms.hpp>
+//#include <ros/package.h>
+//#include <ros/ros.h>
+//#include <rslidar_driver/rslidarNodeConfig.h>
+#include <string>
+
+#include "boost/thread.hpp"
+
+namespace rs_driver {
+
+struct lidarscan
+{
+    double stamp;
+    std::string frame_id;
+    std::vector<m1packet> packets;
+
+};
+
+class rslidarDriver {
+public:
+  /**
+ * @brief rslidarDriver
+ * @param node          raw packet output topic
+ * @param private_nh    通过这个节点传参数
+ */
+//  rslidarDriver(ros::NodeHandle node, ros::NodeHandle private_nh);
+  rslidarDriver();
+
+  ~rslidarDriver() {}
+
+  bool poll(std::shared_ptr<lidarscan> & rawscan);
+  void difopPoll(void);
+
+private:
+ /// Callback for dynamic reconfigure
+//  void callback(rslidar_driver::rslidarNodeConfig &config, uint32_t level);
+
+//  /// Pointer to dynamic reconfigure service srv_
+//  boost::shared_ptr<
+//      dynamic_reconfigure::Server<rslidar_driver::rslidarNodeConfig>>
+//      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)
+    double time_offset;   ///< time in seconds added to each  time stamp
+    bool start_from_edge;
+  } config_;
+
+  boost::shared_ptr<Input> msop_input_;
+  boost::shared_ptr<Input> difop_input_;
+//  ros::Publisher msop_output_;
+//  ros::Publisher difop_output_;
+  boost::shared_ptr<boost::thread> difop_thread_;
+
+  // Converter convtor_;
+  /** diagnostics updater */
+//  diagnostic_updater::Updater diagnostics_;
+//  double diag_min_freq_;
+//  double diag_max_freq_;
+//  boost::shared_ptr<diagnostic_updater::TopicDiagnostic> diag_topic_;
+};
+
+} // namespace rs_driver
+
+#endif