async connection and reconnection has been fully tested, wait for the test with real wj lidar.

.gitignore

@@ -31,3 +31,4 @@ _ReSharper*/
 #Nuget packages folder
 packages/
 .vscode/
+build/

CMakeLists.txt

@@ -7,6 +7,9 @@ add_compile_options(-std=c++11)
 find_package(PCL REQUIRED)
 find_package(Boost REQUIRED)
 find_package(Eigen3 REQUIRED)
+find_package(Glog REQUIRED)
+find_package(GFlags REQUIRED)
+FIND_PACKAGE(Protobuf REQUIRED)
 
 ###################################
 ## catkin specific configuration ##
@@ -32,10 +35,12 @@ include_directories(
   ./src
 )
 
-aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/src WJ_LIDAR_SRC)
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/src/wj_lidar WJ_LIDAR_SRC)
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/src/test WJ_TEST_SRC)
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/src/error_code ERROR_CODE_SRC)
 
-add_executable(wj_716_lidar ${WJ_LIDAR_SRC} )
-target_link_libraries(wj_716_lidar ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${PROTOBUF_LIBRARIES})
+add_executable(wj_716_lidar ${WJ_TEST_SRC} ${ERROR_CODE_SRC} ${WJ_LIDAR_SRC})
+target_link_libraries(wj_716_lidar ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${PROTOBUF_LIBRARIES} glog gflags)
 
 #add_dependencies(wj_716_lidar ${PROJECT_NAME}_gencfg)
 

src/error_code/error_code.h

@@ -161,6 +161,7 @@ enum Error_code
     WJ_LIDAR_UNINITIALIZED,
     WJ_LIDAR_READ_FAILED,
     WJ_LIDAR_WRITE_FAILED,
+    WJ_LIDAR_GET_CLOUD_TIMEOUT,
 
     //wj lidar protocol error from 0x05020000-0x0502FFFF
     WJ_PROTOCOL_ERROR_BASE=0x05020000,

src/test/wj_lidar_test.cpp

@@ -0,0 +1,48 @@
+#include "../wj_lidar/wj_lidar_encapsulation.h"
+// #include <pcl/visualization/cloud_viewer.h>
+#include <fstream>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <stdint.h>
+
+void save_cloud_txt(std::string txt, pcl::PointCloud<pcl::PointXYZ>::Ptr pCloud)
+{
+    std::ofstream os;
+    os.open(txt, std::ios::out);
+    for (int i = 0; i < pCloud->points.size(); i++)
+    {
+        pcl::PointXYZ point = pCloud->points[i];
+        char buf[255];
+        memset(buf, 0, 255);
+        sprintf(buf, "%f %f %f\n", point.x, point.y, point.z);
+        os.write(buf, strlen(buf));
+    }
+    os.close();
+}
+
+int main(int argc,char* argv[]){
+    Wj_lidar_encapsulation* lidar = new Wj_lidar_encapsulation();
+    wj::wjLidarParams params;
+    params.mutable_net_config()->set_ip_address("127.0.0.1");
+    params.mutable_net_config()->set_port(8100);
+    if(lidar!=0){
+        Error_manager code = lidar->initialize(params);
+        LOG(INFO) << code.to_string();
+        if(code == SUCCESS){
+            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+            for (size_t i = 0; i < 10; i++)
+            {
+                code = lidar->get_cloud(cloud, std::chrono::steady_clock::now(), 3000);
+                if(code == SUCCESS){
+                    save_cloud_txt("cloud.txt", cloud);
+                    LOG(INFO) << "cloud saved to clout.txt";
+                }else{
+                    LOG(WARNING) << code.to_string();
+                }
+                usleep(1000 * 1000);
+            }
+        }
+    }
+    getchar();
+    return 0;
+}

src/wj_lidar/async_client.cpp

@@ -11,6 +11,11 @@ Async_Client::Async_Client(boost::asio::io_service &io_service, ip::tcp::endpoin
       mp_handle(p)
 {
 }
+
+Async_Client::~Async_Client(){
+  mp_handle = 0;
+}
+
 void Async_Client::client_async_write(char *buf, int len)
 {
   boost::asio::async_write(socket,
@@ -20,7 +25,7 @@ void Async_Client::client_async_write(char *buf, int len)
 }
 void Async_Client::client_async_read()
 {
-  socket.async_read_some(boost::asio::buffer(data_, MAX_LENGTH),
+  socket.async_read_some(boost::asio::buffer(m_origin_data_, MAX_LENGTH),
                          boost::bind(&Async_Client::handle_read, this,
                                      boost::asio::placeholders::error,
                                      boost::asio::placeholders::bytes_transferred));
@@ -35,28 +40,39 @@ bool Async_Client::client_connection_status()
 }
 
 /**
- * 异步连接回调
+ * 返回初始化状态
+ * */
+bool Async_Client::client_initialize_status()
+{
+  return mb_initialized;
+}
+
+/**
+ * 异步连接回调, 失败后重连
  * */
 void Async_Client::handle_connect(const boost::system::error_code &error)
 {
   if (!error)
   {
-    std::cout << " Connection Success! " << m_ep.address().to_string() << std::endl;
+    LOG(INFO) << " Connection Success! " << m_ep.address().to_string() << ":" << m_ep.port();
     mb_connected = true;
     // 连接成功后开启异步读
     this->client_async_read();
   }
   else
   {
-    std::cout << boost::system::system_error(error).what() << " start reconnect" << std::endl;
+    LOG(WARNING) << boost::system::system_error(error).what() << ", start reconnect "  << m_ep.address().to_string() << ":" << m_ep.port();
     mb_connected = false;
     socket_close();
-    socket_connect();
+    if(mb_with_reconnection){
+      socket_connect();
+      usleep(1000 * 1000);
+    }
   }
 }
 
 /**
- * 异步读取回调
+ * 异步读取回调, 失败后重连
  * */
 void Async_Client::handle_read(const boost::system::error_code &error,
                                size_t bytes_transferred)
@@ -68,25 +84,28 @@ void Async_Client::handle_read(const boost::system::error_code &error,
     if (mp_handle != 0)
     {
       // 调用外部回调处理函数后再次进入异步读
-      (*m_fundata)(this->m_ep.address().to_string().c_str(), this->m_ep.port(), data_, bytes_transferred, mp_handle);
+      (*m_fundata)(this->m_ep.address().to_string().c_str(), this->m_ep.port(), m_origin_data_, bytes_transferred, mp_handle);
       this->client_async_read();
     }
     else
     {
-      std::cout << "async client, handle null pointer" << std::endl;
+      LOG(ERROR) << "async client, handle null pointer" << m_ep.address().to_string() << ":" << m_ep.port();
     }
   }
   else
   {
-    std::cout << boost::system::system_error(error).what() << " start reconnect" << std::endl;
+    LOG(WARNING) << boost::system::system_error(error).what() << ", start reconnect " << m_ep.address().to_string() << ":" << m_ep.port();
     mb_connected = false;
     socket_close();
-    socket_connect();
+    if(mb_with_reconnection){
+      socket_connect();
+      usleep(1000 * 1000);
+    }
   }
 }
 
 /**
- * 异步写入回调
+ * 异步写入回调, 失败后重连
  * */
 void Async_Client::handle_write(const boost::system::error_code &error)
 {
@@ -95,10 +114,13 @@ void Async_Client::handle_write(const boost::system::error_code &error)
   }
   else
   {
-    printf("%s. start reconnect", error.message().c_str());
+    LOG(WARNING) << boost::system::system_error(error).what() << ", start reconnect " << m_ep.address().to_string() << ":" << m_ep.port();
     mb_connected = false;
     socket_close();
-    socket_connect();
+    if(mb_with_reconnection){
+      socket_connect();
+      usleep(1000 * 1000);
+    }
   }
 }
 
@@ -113,7 +135,7 @@ void Async_Client::socket_close()
 /**
  * 开始连接
  * */
-bool Async_Client::socket_connect()
+void Async_Client::socket_connect()
 {
   socket.async_connect(m_ep,
                        boost::bind(&Async_Client::handle_connect, this, boost::asio::placeholders::error));
@@ -128,4 +150,4 @@ bool Async_Client::initialize(bool with_reconnection)
   socket_connect();
   mb_initialized = true;
   return mb_initialized;
-}
+}

src/wj_lidar/async_client.h

@@ -8,6 +8,7 @@
 #include <boost/thread.hpp>
 #include <string>
 #include <unistd.h>
+#include "glog/logging.h"
 
 // using namespace std ;
 using boost::asio::ip::udp;
@@ -22,20 +23,24 @@ typedef void (*fundata_t)(const char* addr,int port,const char* data,const int l
 class Async_Client
 {
 public:
-  // 无参构造
-  Async_Client();
+  // // 无参构造
+  // Async_Client();
   // 有参构造
   Async_Client(boost::asio::io_service& io_service,ip::tcp::endpoint endpoint,fundata_t fundata_ , void *p);
+  // 析构
+  ~Async_Client();
   // 异步写入
   void client_async_write(char *buf,int len);
   // 异步读取
   void client_async_read();
   // 连接状态返回
   bool client_connection_status();
+  // 初始化状态返回
+  bool client_initialize_status();
   // 初始化
   bool initialize(bool with_reconnection=true);
   // 连接
-  bool socket_connect();
+  void socket_connect();
   // 关闭连接
   void socket_close();
 
@@ -53,9 +58,8 @@ private:
   io_service &iosev;                      // 异步控制服务
   ip::tcp::socket socket;                 // socket句柄
   ip::tcp::endpoint m_ep;                 // 连接参数
-  boost::system::error_code m_ec;         // 错误码
 
-  char        data_[MAX_LENGTH];          // 原始数据
+  char        m_origin_data_[MAX_LENGTH];          // 原始数据
   bool        mb_with_reconnection;       // 开启自动重连 
   bool        mb_connected;               // 连接状态
   bool        mb_initialized;             // 初始化是否成功

src/wj_lidar/wj_716_lidar_01.cpp

@@ -1,145 +0,0 @@
-#include <ros/ros.h>
-#include "async_client.h"
-#include "wj_716_lidar_protocol.h"
-using namespace wj_lidar;
-
-int InitTcpConnection(const char* addr,int port,Async_Client **client_,fundata_t fundata_)
-{
-
-  io_service iosev;
-
-  ip::tcp::endpoint ep(ip::address_v4::from_string(addr),port);
-  *client_ = new Async_Client(iosev,ep,fundata_);
-
-  iosev.run();
-
-  return 1 ;
-}
-
-int boost_tcp_init_connection(const char* addr,int port,Async_Client **client_,fundata_t fundata_)
-{
-  int timecnt=0 ;
-  *client_ = NULL ;
-
-  boost::thread tmp(&InitTcpConnection,addr,port,client_,fundata_);
-  tmp.detach() ;
-
-  while(timecnt<50){
-    timecnt++ ;
-    usleep(20000); //20 ms
-    if((*client_)->client_return_status()){
-      return 0 ;
-    }
-  }
-  *client_ = NULL ;
-  return -1 ;
-
-}
-
-int boost_tcp_sync_send(Async_Client *client_ ,const char* msg,const int len)
-{
-  if(client_==NULL || client_->client_return_status()==0 ){
-    printf("not connected , please connect first \n");
-    return -1 ;
-  }
-  else{
-    client_->client_async_write((char*)msg,len);
-    return 0 ;
-  }
-
-  return 1;
-}
-
-int boost_tcp_sync_read(Async_Client *client_ )
-{
-  if(client_==NULL || client_->client_return_status()==0 ){
-    printf("not connected , please connect first \n");
-    return -1 ;
-  }
-  else{
-    client_->client_async_read();
-    return 0 ;
-  }
-  return 1;
-}
-
-/* ------------------------------------------------------------------------------------------
- *  show demo --
- * ------------------------------------------------------------------------------------------ */
-wj_716_lidar_protocol *protocol;
-Async_Client *client;
-void CallBackRead(const char* addr,int port,const char* data,const int len)
-{
-  protocol->data_process(data,len);
-}
-
-void callback(wj_716_lidar::wj_716_lidarConfig &config,uint32_t level)
-{
-  protocol->setConfig(config,level);
-}
-
-void timerCallback(const ros::TimerEvent&)
-{
-//  char heartframe[34]={0xFF,0xAA,0x00,0x1E,0x00,0x00,0x00,0x00,
-//                       0x00,0x00,0x01,0x01,0x00,0x05,0x00,0x00,
-//                       0x00,0x00,0x00,0x00,0x00,0x00,0x05,0x04,
-//                       0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x1A,
-//                       0xEE,0xEE};
-
-//  //cout << "20ms" <<endl;
-//  boost_tcp_sync_send(client,heartframe,34);
-
-  if(protocol->heartstate)
-  {
-    protocol->heartstate=false;
-    cout <<"once heart cycle "<<endl;
-  }
-  else
-  {
-    client->socket_close();
-    client=NULL;
-    ros::NodeHandle nh("~");
-    std::string hostname;
-    nh.getParam("hostname",hostname);
-    std::string port;
-    nh.getParam("port",port);
-
-
-    //InitTcpConnection(hostname.c_str(),atoi(port.c_str()),&client,&CallBackRead);
-   //boost_tcp_init_connection(hostname.c_str(),atoi(port.c_str()),&client,&CallBackRead);
-    boost_tcp_sync_read(client);
-
-    cout <<"try to reconect "<<endl;
-    cout << hostname.c_str()<<endl;
-    cout << port.c_str()<<endl;
-    //ROS_INFO("Hello wj_716_lidar!");
-  }
-
-}
-
-int main(int argc, char **argv)
-{
-  ros::init(argc, argv, "wj_716_lidar_01");
-  ros::NodeHandle nh("~");
-//  ros::Timer timer;
-  std::string hostname;
-  nh.getParam("hostname",hostname);
-  cout << hostname <<endl;
-  std::string port;
-  nh.getParam("port",port);
-
-  protocol = new wj_716_lidar_protocol();
-  dynamic_reconfigure::Server<wj_716_lidar::wj_716_lidarConfig> server;
-  dynamic_reconfigure::Server<wj_716_lidar::wj_716_lidarConfig>::CallbackType f;
-  f = boost::bind(&callback,_1,_2);
-  server.setCallback(f);
-  client=NULL;
-
-  boost_tcp_init_connection(hostname.c_str(),atoi(port.c_str()),&client,&CallBackRead);
-//  timer= nh.createTimer(ros::Duration(2), timerCallback);
-  boost_tcp_sync_read(client);
-
-  ROS_INFO("Hello wj_716_lidar!");
-  ros::spin();
-
-}

src/wj_lidar/wj_716_lidar_protocol.cpp

@@ -5,14 +5,17 @@ namespace wj_lidar
 {
 
 /**
- * 扫描点云转换为欧式坐标,并根据标定参数移动
+ * 扫描点云转换为欧式坐标,并根据标定参数进行坐标变换
  * */
 Error_manager scan_transform(wj::wjLidarParams params, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_out)
 {
-  if (!params.has_net_config() || !params.has_transform() || !params.has_scan_limit()){
+  // 1. 合法性检验
+  if (!params.has_net_config() || !params.has_transform() || !params.has_scan_limit())
+  {
     return Error_manager(Error_code::PARAMETER_ERROR);
   }
 
+  // 2. 将极坐标转换为欧式坐标，其中原极坐标r存于点云x坐标中，原极坐标theta通过角度最小值与角分辨率计算
   float angle = params.angle_min();
   cloud_out->clear();
   for (size_t i = 0; i < cloud_in->size(); ++i)
@@ -64,27 +67,31 @@ void Wj_716_lidar_protocol::set_config(wj::wjLidarParams params)
 /**
  * 获取更新扫描点云时间
  * */
-std::chrono::steady_clock::time_point Wj_716_lidar_protocol::get_scan_time(){
+std::chrono::steady_clock::time_point Wj_716_lidar_protocol::get_scan_time()
+{
   return m_scan_cloud_time;
 }
 
 /**
  * 获取更新二次回波时间
  * */
-std::chrono::steady_clock::time_point Wj_716_lidar_protocol::get_two_echo_time(){
+std::chrono::steady_clock::time_point Wj_716_lidar_protocol::get_two_echo_time()
+{
   return m_two_echo_cloud_time;
 }
 
 /**
  * 获取扫描点云
  * */
-Error_manager Wj_716_lidar_protocol::get_scan_points(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_out){
+Error_manager Wj_716_lidar_protocol::get_scan_points(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_out)
+{
   Error_manager result;
   m_scan_mutex.lock();
   result = scan_transform(m_lidar_params, mp_scan_points, cloud_out);
   m_scan_mutex.unlock();
-  if(result!=SUCCESS){
-    LOG(WARNING)<<"获取点云失败，参数错误";
+  if (result != SUCCESS)
+  {
+    LOG(WARNING) << "获取点云失败，参数错误";
   }
   return result;
 }
@@ -92,13 +99,15 @@ Error_manager Wj_716_lidar_protocol::get_scan_points(pcl::PointCloud<pcl::PointX
 /**
  * 获取二次回波点云
  * */
-Error_manager Wj_716_lidar_protocol::get_two_echo_points(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_out){
+Error_manager Wj_716_lidar_protocol::get_two_echo_points(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_out)
+{
   Error_manager result;
   m_two_echo_mutex.lock();
   result = scan_transform(m_lidar_params, mp_two_echo_points, cloud_out);
   m_two_echo_mutex.unlock();
-  if(result!=SUCCESS){
-    LOG(WARNING)<<"获取点云失败，参数错误";
+  if (result != SUCCESS)
+  {
+    LOG(WARNING) << "获取点云失败，参数错误";
   }
   return result;
 }
@@ -123,15 +132,21 @@ Error_manager Wj_716_lidar_protocol::initialize(wj::wjLidarParams params)
   m_lidar_params.set_range_min(0);
   m_lidar_params.set_range_max(30);
 
-  m_scan_cloud_time = std::chrono::steady_clock::now();
-  m_two_echo_cloud_time = std::chrono::steady_clock::now();
+  // 设置初始时间为昨天，以避免第一次点云更新前获取到空点云
+  m_scan_cloud_time = std::chrono::steady_clock::now() - std::chrono::duration<int, std::ratio<60 * 60 * 24>>(1);
+  m_two_echo_cloud_time = std::chrono::steady_clock::now() - std::chrono::duration<int, std::ratio<60 * 60 * 24>>(1);
 
   set_config(params);
   LOG(INFO) << "wj_716_lidar_protocol start success";
+  mb_initialized = true;
   return Error_manager(Error_code::SUCCESS);
 }
 
-Wj_716_lidar_protocol::Wj_716_lidar_protocol()
+Wj_716_lidar_protocol::Wj_716_lidar_protocol() : mb_initialized(0)
+{
+}
+
+Wj_716_lidar_protocol::~Wj_716_lidar_protocol()
 {
 }
 
@@ -140,6 +155,7 @@ Wj_716_lidar_protocol::Wj_716_lidar_protocol()
  **/
 Error_manager Wj_716_lidar_protocol::data_process(const char *data, const int reclen)
 {
+  // 1. 判断长度合法性
   if (reclen > MAX_LENGTH_DATA_PROCESS)
   {
     return Error_manager(Error_code::WJ_PROTOCOL_EXCEED_MAX_SIZE);
@@ -152,6 +168,7 @@ Error_manager Wj_716_lidar_protocol::data_process(const char *data, const int re
   }
   memcpy(&m_sdata.m_acdata[m_sdata.m_u32in], data, reclen * sizeof(char));
   m_sdata.m_u32in += reclen;
+  // 2. 数据处理
   while (m_sdata.m_u32out < m_sdata.m_u32in)
   {
     if (m_sdata.m_acdata[m_sdata.m_u32out] == 0x02 && m_sdata.m_acdata[m_sdata.m_u32out + 1] == 0x02 &&
@@ -217,21 +234,24 @@ Error_manager Wj_716_lidar_protocol::data_process(const char *data, const int re
 }
 
 /**
- * 获取消息处理
+ * 获取消息处理, 判断参数合法性，完整性检验并调用协议栈解析
  * */
 Error_manager Wj_716_lidar_protocol::on_recv_process(char *data, int len)
 {
   if (len > 0)
   {
     Error_manager code = check_xor(data, len);
-    if (code==SUCCESS)
+    if (code == SUCCESS)
     {
       code = protocol(data, len);
-      if(code!=SUCCESS){
-        LOG(WARNING)<< "万集雷达协议解析失败";
+      if (code != SUCCESS)
+      {
+        LOG(WARNING) << "万集雷达协议解析失败";
       }
-    }else{
-      LOG(WARNING)<< "万集雷达信息完整性验证失败";
+    }
+    else
+    {
+      LOG(WARNING) << "万集雷达信息完整性验证失败";
     }
   }
   else
@@ -419,7 +439,8 @@ Error_manager Wj_716_lidar_protocol::protocol(const char *data, const int len)
         return Error_manager(Error_code::WJ_PROTOCOL_PARSE_FAILED);
       }
     }
-    return Error_manager(Error_code::SUCCESS);;
+    return Error_manager(Error_code::SUCCESS);
+    ;
   }
   else
   {
@@ -457,4 +478,12 @@ Error_manager Wj_716_lidar_protocol::check_xor(char *recvbuf, int recvlen)
   }
 }
 
-} // namespace wj_lidar
+/**
+ * 初始化状态获取
+ * */
+bool Wj_716_lidar_protocol::get_initialize_status()
+{
+  return mb_initialized;
+}
+
+} // namespace wj_lidar

src/wj_lidar/wj_716_lidar_protocol.h

@@ -10,7 +10,6 @@
 #include <mutex>
 #include <chrono>
 
-#include <pcl_conversions/pcl_conversions.h>
 #include <pcl/point_types.h>
 #include <pcl/PCLPointCloud2.h>
 #include <pcl/conversions.h>
@@ -46,7 +45,10 @@ typedef struct tag_data_cache
 class Wj_716_lidar_protocol
 {
 public:
+  // 构造
   Wj_716_lidar_protocol();
+  // 析构
+  ~Wj_716_lidar_protocol();
   // 初始化
   Error_manager initialize(wj::wjLidarParams params);
   // 数据处理
@@ -57,6 +59,8 @@ public:
   Error_manager on_recv_process(char *data, int len);
   // 消息完整性检查
   Error_manager check_xor(char *recvbuf, int recvlen);
+  // 获取初始化状态
+  bool get_initialize_status();
   // 获取扫描点云
   Error_manager get_scan_points(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_out);
   Error_manager get_two_echo_points(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_out);
@@ -69,6 +73,7 @@ private:
   void set_config(wj::wjLidarParams params);
 
 private:
+  bool mb_initialized; // 初始化状态flag
   // 万集雷达参数
   wj::wjLidarParams m_lidar_params;
   data_cache m_sdata;

src/wj_lidar/wj_lidar_encapsulation.cpp

@@ -3,16 +3,17 @@
 /**
  * 初始化连接
  * */
-void Wj_lidar_encapsulation::init_tcp_connection(const char *addr, int port, Async_Client **client_, fundata_t fundata_)
+void Wj_lidar_encapsulation::init_tcp_connection(const char *addr, int port, Async_Client **client_, fundata_t fundata_, Wj_lidar_encapsulation *p_handle)
 {
 
   io_service iosev;
 
   ip::tcp::endpoint ep(ip::address_v4::from_string(addr), port);
-  *client_ = new Async_Client(iosev, ep, fundata_, this);
+  *client_ = new Async_Client(iosev, ep, fundata_, p_handle);
+  (*client_)->initialize();
 
   iosev.run();
-
+  LOG(WARNING) << "ip: " << addr << " 万集异步读写线程退出";
 }
 
 /**
@@ -23,55 +24,56 @@ Error_manager Wj_lidar_encapsulation::boost_tcp_init_connection(const char *addr
   int timecnt = 0;
   *client_ = NULL;
 
-  boost::thread tmp(&init_tcp_connection, addr, port, client_, fundata_);
+  boost::thread tmp(&init_tcp_connection, addr, port, client_, fundata_, this);
   tmp.detach();
 
   while (timecnt < 50)
   {
     timecnt++;
     usleep(20000); //20 ms
-    if ((*client_)->client_connection_status())
+    if ((*client_)->client_initialize_status() && (*client_)->client_connection_status())
     {
       return Error_manager(Error_code::SUCCESS);
     }
   }
-  *client_ = NULL;
+  // delete *client_;
+  // *client_ = NULL;
   return Error_manager(Error_code::WJ_LIDAR_CONNECT_FAILED);
 }
 
-/**
- * 异步写指令
- * */
-Error_manager Wj_lidar_encapsulation::boost_tcp_async_send(Async_Client *client_, const char *msg, const int len)
-{
-  if (client_ == NULL || client_->client_connection_status() == 0)
-  {
-    LOG(WARNING) << "not connected , need to connect first \n";
-    return Error_manager(Error_code::WJ_LIDAR_WRITE_FAILED);
-  }
-  else
-  {
-    client_->client_async_write((char *)msg, len);
-    return Error_manager(Error_code::SUCCESS);
-  }
-}
-
-/**
- * 异步读数据
- * */
-Error_manager Wj_lidar_encapsulation::boost_tcp_async_read(Async_Client *client_)
-{
-  if (client_ == NULL || client_->client_connection_status() == 0)
-  {
-    LOG(WARNING) << "not connected , need to connect first \n";
-    return Error_manager(Error_code::WJ_LIDAR_READ_FAILED);
-  }
-  else
-  {
-    client_->client_async_read();
-    return Error_manager(Error_code::SUCCESS);;
-  }
-}
+// /**
+//  * 异步写指令
+//  * */
+// Error_manager Wj_lidar_encapsulation::boost_tcp_async_send(Async_Client *client_, const char *msg, const int len)
+// {
+//   if (client_ == NULL || client_->client_connection_status() == 0)
+//   {
+//     LOG(WARNING) << "not connected , need to connect first \n";
+//     return Error_manager(Error_code::WJ_LIDAR_WRITE_FAILED);
+//   }
+//   else
+//   {
+//     client_->client_async_write((char *)msg, len);
+//     return Error_manager(Error_code::SUCCESS);
+//   }
+// }
+
+// /**
+//  * 异步读数据
+//  * */
+// Error_manager Wj_lidar_encapsulation::boost_tcp_async_read(Async_Client *client_)
+// {
+//   if (client_ == NULL || client_->client_connection_status() == 0)
+//   {
+//     LOG(WARNING) << "not connected , need to connect first \n";
+//     return Error_manager(Error_code::WJ_LIDAR_READ_FAILED);
+//   }
+//   else
+//   {
+//     client_->client_async_read();
+//     return Error_manager(Error_code::SUCCESS);;
+//   }
+// }
 
 /**
  * 万集读数据回调
@@ -99,39 +101,32 @@ void Wj_lidar_encapsulation::read_callback(const char *addr, int port, const cha
 Error_manager Wj_lidar_encapsulation::get_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud,
                                                 std::chrono::steady_clock::time_point command_time, int timeout_milli)
 {
-  if (!mb_initialized || mp_protocol == 0)
+  // 1. 检查参数合法性
+  if (!mb_initialized || mp_protocol == 0 || !mp_protocol->get_initialize_status())
   {
     return Error_manager(Error_code::WJ_LIDAR_UNINITIALIZED);
   }
-  std::chrono::steady_clock::time_point scan_time_temp = mp_protocol->get_scan_time();
-  while (std::chrono::duration_cast<std::chrono::milliseconds>(scan_time_temp - command_time).count() < timeout_milli)
+
+  // 2. 获取扫描时间，时间点位于指令时刻之后，则获得的点云合法
+  // 若当前时间超过(指令时刻+超时时间段)，则获取点云函数超时退出
+  int current_duration = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - command_time).count();
+  while (current_duration < timeout_milli)
   {
+    current_duration = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - command_time).count();
 
-  }
+    std::chrono::steady_clock::time_point scan_time_temp = mp_protocol->get_scan_time();
+    int cloud_duration = std::chrono::duration_cast<std::chrono::milliseconds>(scan_time_temp - command_time).count();
 
-  if (!mp_protocol->get_scan_points(cloud))
-  {
-    return Error_manager(Error_code::WJ_LIDAR_PROTOCOL_FAILED);
-  }
-  
-
-  cloud->clear();
-  // m_mutex.lock();
-  cloud->operator+=(*m_cloud);
-  //    std::cout<<cloud->size()<<std::endl;
-  //    return true;
-  ros::Duration duration = ros::Time::now() - m_cloud_time;
-  if (print)
-  {
-    // if (m_lidar_param.topic() == "/scan3" || m_lidar_param.topic() == "/scan4" || m_lidar_param.topic() == "/scan5")
-    // {
-    std::cout << m_lidar_param.topic() << " duration: " << duration.toSec() * 1000 << ", cloud size: " << cloud->size() << std::endl;
-    // }
+    // 扫描时刻位于指令时刻之后
+    if (cloud_duration > 0)
+    {
+      Error_manager code = mp_protocol->get_scan_points(cloud);
+      // 内部已打印错误信息
+      return code;
+    }
+    usleep(1000 * 50);
   }
-  if (duration.toSec() * 1000 > timeout_milli)
-    return false;
-  else
-    return true;
+  return Error_manager(Error_code::WJ_LIDAR_GET_CLOUD_TIMEOUT);
 }
 
 /**
@@ -144,29 +139,27 @@ Error_manager Wj_lidar_encapsulation::initialize(wj::wjLidarParams params)
   {
     m_ip = params.net_config().ip_address();
     m_port = params.net_config().port();
-    if (boost_tcp_init_connection(m_ip.c_str(), m_port, &mp_client, &read_callback) == SUCCESS)
+    if (boost_tcp_init_connection(m_ip.c_str(), m_port, &mp_client, &read_callback) != SUCCESS)
     {
-      if (boost_tcp_async_read(mp_client) != SUCCESS)
-      {
-        LOG(WARNING) << m_ip << " null or disconnected.";
-        return Error_manager(Error_code::WJ_LIDAR_CONNECT_FAILED);
-      }
-    }
-    else
-    {
-      LOG(WARNING) << m_ip << " 1 sec timeout, connection failed.";
+      LOG(WARNING) << m_ip << " 1 sec timeout, connection failed, will try to reconnect";
       return Error_manager(Error_code::WJ_LIDAR_CONNECT_FAILED);
     }
-  }else{
+  }
+  else
+  {
     return Error_manager(Error_code::PARAMETER_ERROR);
   }
   // 2. 创建协议栈对象
   mp_protocol = new Wj_716_lidar_protocol();
   Error_manager code = mp_protocol->initialize(params);
-  if(code != SUCCESS){
-    LOG(ERROR) << code.to_string();
-  }else{
+  if (code != SUCCESS)
+  {
+    LOG(ERROR) << "wj_lidar " << m_ip << ":" << m_port << " initialize failed. " << code.to_string();
+  }
+  else
+  {
     mb_initialized = true;
+    LOG(INFO) << "wj_lidar " << m_ip << ":" << m_port << " successfully initialized.";
   }
   return code;
 }
@@ -187,6 +180,7 @@ Wj_lidar_encapsulation::~Wj_lidar_encapsulation()
     delete mp_client;
     mp_client = 0;
   }
+
   if (mp_protocol != 0)
   {
     delete mp_protocol;

src/wj_lidar/wj_lidar_encapsulation.h

@@ -21,16 +21,15 @@ public:
     static void read_callback(const char* addr,int port,const char* data,const int len, void *handle);
     // 外部调用获取点云
     Error_manager get_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, std::chrono::steady_clock::time_point command_time, int timeout_milli=1500);
-
+    // 初始化雷达连接线程函数
+    void static init_tcp_connection(const char* addr,int port,Async_Client **client_,fundata_t fundata_, Wj_lidar_encapsulation* p_handle);
 private:
-    // 初始化雷达连接
-    void init_tcp_connection(const char* addr,int port,Async_Client **client_,fundata_t fundata_);
     // 连接与异步读写线程创建
     Error_manager boost_tcp_init_connection(const char* addr,int port,Async_Client **client_,fundata_t fundata_);
-    // 异步写入
-    Error_manager boost_tcp_async_send(Async_Client *client_ ,const char* msg,const int len);
-    // 异步读取
-    Error_manager boost_tcp_async_read(Async_Client *client_ );
+    // // 异步写入
+    // Error_manager boost_tcp_async_send(Async_Client *client_ ,const char* msg,const int len);
+    // // 异步读取
+    // Error_manager boost_tcp_async_read(Async_Client *client_ );
 
 private:
     Wj_716_lidar_protocol*  mp_protocol;        // 万集雷达协议句柄