LidarMeasure/wj_lidar/wj_lidar_encapsulation.cpp

#include "wj_lidar_encapsulation.h"

/**
 * 初始化连接
 * */
void Wj_lidar_encapsulation::init_tcp_connection(const char *addr, int port, Async_Client **client_, fundata_t fundata_, Wj_lidar_encapsulation *p_handle)
{
  if (p_handle == 0)
  {
    LOG(WARNING) << "ip: " << addr << " 万集异步读写线程参数错误";
    return;
  }
  boost::asio::ip::tcp::endpoint ep(boost::asio::ip::address_v4::from_string(addr), port);
  while (!p_handle->mb_exit)
  {
    if (*client_ == 0)
    {
      *client_ = new Async_Client(p_handle->m_io_service, ep, fundata_, p_handle);
    }
    if (*client_ != 0)
    {
      if(!(*client_)->initialize())
      {
        LOG(WARNING) << "ip: " << addr << " 万集异步读写客户端初始化失败";
      }
      if((*client_)->mb_exit){
        break;
      }
    }

    p_handle->m_io_service.run();
    LOG(WARNING) << "ip: " << addr << " 万集异步读写服务中止";
    usleep(1000 * 1000);
  }
  LOG(WARNING) << "ip: " << addr << " 万集异步读写线程退出";
}

/**
 * 创建初始化与读写线程以及连接超时判断
 * */
Error_manager Wj_lidar_encapsulation::boost_tcp_init_connection(const char *addr, int port, Async_Client **client_, fundata_t fundata_)
{
  int timecnt = 0;
  *client_ = NULL;

  m_client_thread = new std::thread(&init_tcp_connection, addr, port, client_, fundata_, this);
  m_client_thread->detach();
  // boost::thread tmp(&init_tcp_connection, addr, port, client_, fundata_, this);
  // tmp.detach();

  while (timecnt < 50)
  {
    timecnt++;
    usleep(20000); //20 ms
    if ((*client_)->client_initialize_status() && (*client_)->client_connection_status())
    {
      return Error_manager(Error_code::SUCCESS);
    }
  }
  // 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);;
//   }
// }

/**
 * 万集读数据回调
 * 
 * */
void Wj_lidar_encapsulation::read_callback(const char *data, const int len, const void *handle)
{
  if (handle != 0)
  {
    Wj_lidar_encapsulation *p_wj_temp = (Wj_lidar_encapsulation *)handle;
    if (p_wj_temp->mp_protocol != 0 && p_wj_temp->mp_protocol->get_initialize_status())
    {
      // LOG(INFO) << "read callback, try to process data";
      if (p_wj_temp->mp_protocol->data_process(data, len) != SUCCESS)
      {
        LOG(WARNING) << p_wj_temp->m_ip << " data process failed, exceed max length";
      }
      else
      {
        // LOG(INFO) << "read callback, data processed";
      }
    }
    else
    {
      LOG(WARNING) << p_wj_temp->m_ip << " protocol null pointer or not initialized";
    }
  }
}

/**
 * 外部调用获取连接状态
 * */
bool Wj_lidar_encapsulation::get_connection_status(){
  if(mp_client== 0 || !mp_client->client_initialize_status()){
    return false;
  }
  return mp_client->client_connection_status();
}

/**
 * 外部调用获取初始化状态
 * */
bool Wj_lidar_encapsulation::get_initialize_status()
{
  return mb_initialized;
}

/**
 * 外部调用获取点云
 * */
Error_manager Wj_lidar_encapsulation::get_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud,
                                                std::chrono::steady_clock::time_point command_time, int timeout_milli)
{
  // 1. 检查参数合法性
  if (!mb_initialized || mp_protocol == 0 || !mp_protocol->get_initialize_status())
  {
    return Error_manager(Error_code::WJ_LIDAR_UNINITIALIZED);
  }

  // 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 (cloud_duration > 0)
    {
      Error_manager code = mp_protocol->get_scan_points(cloud);
      // LOG(INFO) << "耗时(ms): " << current_duration;
      // 内部已打印错误信息
      return code;
    }
    usleep(1000 * 50);
  }
  return Error_manager(Error_code::WJ_LIDAR_GET_CLOUD_TIMEOUT);
}

/**
 * 万集雷达封装类初始化
 * */
Error_manager Wj_lidar_encapsulation::initialize(wj::wjLidarParams params)
{
  // 1. 判断网络参数合法性并初始化异步读写实例
  if (params.has_net_config())
  {
    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)
    {
      LOG(WARNING) << m_ip << " 1 sec timeout, connection failed, will try to reconnect";
      return Error_manager(Error_code::WJ_LIDAR_CONNECT_FAILED);
    }
    else
    {
      LOG(INFO) << m_ip << " is connected";
    }
  }
  else
  {
    LOG(ERROR) << "parameter error, without net config params";
    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) << "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;
}

// 万集雷达封装类构造函数
Wj_lidar_encapsulation::Wj_lidar_encapsulation() : mb_initialized(false),
                                                   mp_protocol(0),
                                                   mp_client(0),
                                                   mb_exit(0)
{
}

// 万集雷达封装类析构函数
Wj_lidar_encapsulation::~Wj_lidar_encapsulation()
{
  mb_exit = true;
  if (mp_client != 0)
  {
    mp_client->close();
    delete mp_client;
    mp_client = 0;
  }
  LOG(INFO) << "client released";

  if (m_client_thread != 0)
  {
    if (m_client_thread->joinable())
    {
      m_client_thread->join();
    }
    delete m_client_thread;
    m_client_thread = 0;
  }
  LOG(INFO) << "client thread released";

  if (mp_protocol != 0)
  {
    delete mp_protocol;
    mp_protocol = 0;
  }
  LOG(INFO) << "protocol released";
}