gistfile1.txt

#include "ldlidar_driver.h"
#include <iostream>
#include <cstring>
#include <unistd.h>
#include <arpa/inet.h>
#include <stdio.h>
#include <pigpio.h>

const int PORT = 8080;
const int BUFFER_SIZE = 1024;

int serverSocket;
int clientSocket;

ldlidar::LDLidarDriver* node;

uint64_t GetSystemTimeStamp(void) {
  std::chrono::time_point<std::chrono::system_clock, std::chrono::nanoseconds> tp = 
    std::chrono::time_point_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now());
  auto tmp = std::chrono::duration_cast<std::chrono::nanoseconds>(tp.time_since_epoch());
  return ((uint64_t)tmp.count());
}

void FinishServer();


// void LidarPowerOn(void) {
//   LDS_LOG_DEBUG("Please Lidar Power On","");
//   // ...
// }

// void LidarPowerOff(void) {
//   LDS_LOG_DEBUG("Please Lidar Power Off","");
//   // ...
// }
//17,27,22

#define PIN_R 17
#define PIN_G 27
#define PIN_B 22

#define MOTOR_A 18
#define MOTOR_B 23
#define MOTOR_C 24
#define MOTOR_D 25

void car_forward();
void car_backward();
void car_left();
void car_right();
void car_stop();

void setupGPIO(){
  if (gpioInitialise() < 0) {
        std::cerr << "Erro ao inicializar pigpio." << std::endl;
        return;
  }
  
  gpioSetMode(PIN_R, PI_OUTPUT);
  gpioSetMode(PIN_G, PI_OUTPUT);
  gpioSetMode(PIN_B, PI_OUTPUT);
  
  gpioSetMode(MOTOR_A, PI_OUTPUT);
  gpioSetMode(MOTOR_B, PI_OUTPUT);
  gpioSetMode(MOTOR_C, PI_OUTPUT);
  gpioSetMode(MOTOR_D, PI_OUTPUT);

}

void setColor(int r, int g, int b){
  gpioWrite(PIN_R, r);
  gpioWrite(PIN_G, g);
  gpioWrite(PIN_B, b);
}

int main(int argc, char **argv) {
  setupGPIO();
  setColor(0,0,1);
  
  
  if (argc < 4) {
    LDS_LOG_WARN("Terminal >>: ./ldlidar_stl_node <product_name> serialcom <serial_number>","");
    LDS_LOG_WARN("or","");
    LDS_LOG_WARN("Terminal >>: ./ldlidar_stl_node <product_name> networkcom_tcpclient <server_ip> <server_port>","");
    LDS_LOG_WARN("For example:","");
    LDS_LOG_WARN("./ldlidar_stl_node LD06 serialcom /dev/ttyUSB0","");
    LDS_LOG_WARN("./ldlidar_stl__node LD19 serialcom /dev/ttyUSB0","");
    LDS_LOG_WARN("-----------------","");
    LDS_LOG_WARN("./ldlidar_stl_node LD06 networkcom_tcpclient 192.168.1.200 2000","");
    LDS_LOG_WARN("./ldlidar_stl_node LD19 networkcom_tcpclient 192.168.1.200 2000","");
    exit(EXIT_FAILURE);
  }

  std::string product_name(argv[1]);
  std::string communication_mode(argv[2]);
  std::string port_name;
  std::string server_ip;
  std::string server_port;
  uint32_t serial_baudrate = 0;
  ldlidar::LDType type_name;

  if ((communication_mode != "serialcom") && 
      (communication_mode != "networkcom_tcpclient")) {
    LD_LOG_ERROR("Terminal:input argc(3) value is error","");
    LDS_LOG_WARN("Terminal >>: ./ldlidar_stl_node <product_name> serialcom <serial_number>","");
    LDS_LOG_WARN("or","");
    LDS_LOG_WARN("Terminal >>: ./ldlidar_stl_node <product_name> networkcom_tcpclient <server_ip> <server_port>","");
    LDS_LOG_WARN("For example:","");
    LDS_LOG_WARN("./ldlidar_stl_node LD06 serialcom /dev/ttyUSB0","");
    LDS_LOG_WARN("./ldlidar_stl_node LD19 serialcom /dev/ttyUSB0","");
    LDS_LOG_WARN("-----------------","");
    LDS_LOG_WARN("./ldlidar_stl_node LD06 networkcom_tcpclient 192.168.1.200 2000","");
    LDS_LOG_WARN("./ldlidar_stl_node LD19 networkcom_tcpclient 192.168.1.200 2000","");
    exit(EXIT_FAILURE);
  } else {
    if (communication_mode == "serialcom") {
      if (argc != 4) {
        LD_LOG_ERROR("Terminal:input argc != 4","");
        exit(EXIT_FAILURE);
      }
      port_name = argv[3];
    } else if (communication_mode == "networkcom_tcpclient") {
      if (argc != 5) {
        LD_LOG_ERROR("Terminal:input argc != 5","");
        exit(EXIT_FAILURE);
      }
      server_ip = argv[3];
      server_port = argv[4];
    }
  }
  // Criação do socket
    serverSocket = socket(AF_INET, SOCK_STREAM, 0);
    if (serverSocket == -1) {
        std::cerr << "Erro ao criar o socket\n";
        return -1;
    }

    // Configuração do endereço do servidor
    sockaddr_in serverAddr;
    serverAddr.sin_family = AF_INET;
    serverAddr.sin_addr.s_addr = INADDR_ANY;
    serverAddr.sin_port = htons(PORT);

    // Vinculação do socket ao endereço
    if (bind(serverSocket, (struct sockaddr*)&serverAddr, sizeof(serverAddr)) == -1) {
        std::cerr << "Erro ao vincular o socket ao endereço\n";
        close(serverSocket);
        return -1;
    }

    // Aguarda por conexões
    if (listen(serverSocket, 10) == -1) {
        std::cerr << "Erro ao aguardar por conexões\n";
        close(serverSocket);
        return -1;
    }

    std::cout << "Servidor aguardando por conexões na porta " << PORT << std::endl;

    // Aceita a conexão de um cliente
    clientSocket = accept(serverSocket, nullptr, nullptr);
    if (clientSocket == -1) {
        std::cerr << "Erro ao aceitar a conexão do cliente\n";
        close(serverSocket);
        return -1;
    }
    // Thread para receber mensagens do cliente
    std::thread receiveThread([&clientSocket]() {
      char buffer[BUFFER_SIZE];
      while (true) {
        // Recebe a mensagem do cliente
        ssize_t bytesRead = recv(clientSocket, buffer, BUFFER_SIZE, 0);
        if (bytesRead <= 0) {
          perror("Erro ao receber dados do cliente\n");
          break;
        }

        // Imprime a mensagem recebida
        std::string message(buffer, bytesRead);
        if(message == "CS"){
          car_stop();
        } else if (message == "CB") {
          car_backward();
        } else if (message == "CR") {
          car_right();
        } else if (message == "CL") {
          car_left();
        } else if (message == "CF") {
          car_forward();
        }
        std::cout << "Cliente diz: " << message << std::endl;
      }
    });

  node = new ldlidar::LDLidarDriver();

  LDS_LOG_INFO("LDLiDAR SDK Pack Version is %s", node->GetLidarSdkVersionNumber().c_str());

  node->RegisterGetTimestampFunctional(std::bind(&GetSystemTimeStamp)); 

  node->EnableFilterAlgorithnmProcess(true);

  if (product_name == "LD06") {
    serial_baudrate = 230400;
    type_name = ldlidar::LDType::LD_06; 
  } else if (product_name == "LD19") {
    serial_baudrate = 230400;
    type_name = ldlidar::LDType::LD_19;
  } else {
    LDS_LOG_ERROR("input <product_name> is error!","");
    exit(EXIT_FAILURE);
  }

  if (communication_mode == "serialcom") {
    if (node->Start(type_name, port_name, serial_baudrate, ldlidar::COMM_SERIAL_MODE)) {
      LDS_LOG_INFO("ldlidar node start is success","");
      // LidarPowerOn();
    } else {
      LD_LOG_ERROR("ldlidar node start is fail","");
      exit(EXIT_FAILURE);
    }
  } else if (communication_mode == "networkcom_tcpclient") {
    if (node->Start(type_name, server_ip.c_str(), server_port.c_str(), ldlidar::COMM_TCP_CLIENT_MODE)) {
      LDS_LOG_INFO("ldldiar node start is success","");
      // LidarPowerOn();
    } else {
      LD_LOG_ERROR("ldlidar node start is fail","");
      exit(EXIT_FAILURE);
    }
  }

  if (node->WaitLidarCommConnect(3500)) {
    LDS_LOG_INFO("ldlidar communication is normal.","");
  } else {
    LDS_LOG_ERROR("ldlidar communication is abnormal.","");
    node->Stop();
  }

  ldlidar::Points2D laser_scan_points;
  
  setColor(0,1,0);
  /*
  char buffer[BUFFER_SIZE];
  std::memset(buffer, 0, BUFFER_SIZE);

  // Recebe a string do cliente
  ssize_t bytesRead = recv(clientSocket, buffer, BUFFER_SIZE, 0);
  if (bytesRead <= 0) {
      std::cerr << "Erro ao receber dados do cliente\n";
      break;
  }

  std::cout << "Cliente diz: " << buffer << std::endl;
  */
  // Envia uma resposta de volta para o cliente
  
  
  while (ldlidar::LDLidarDriver::IsOk()) {

    switch (node->GetLaserScanData(laser_scan_points, 1500)){
      case ldlidar::LidarStatus::NORMAL: {
        double lidar_scan_freq = 0;
        node->GetLidarScanFreq(lidar_scan_freq);
#include <pigpio.h>
/*
#ifdef __LP64__
        LDS_LOG_INFO("speed(Hz):%f,size:%d,stamp_front:%lu, stamp_back:%lu",
          lidar_scan_freq, laser_scan_points.size(), laser_scan_points.front().stamp, laser_scan_points.back().stamp);
#else
        LDS_LOG_INFO("speed(Hz):%f,size:%d,stamp_front:%llu, stamp_back:%llu",
          lidar_scan_freq, laser_scan_points.size(), laser_scan_points.front().stamp, laser_scan_points.back().stamp);
#endif
*/

        //  output 2d point cloud data
        for (auto point : laser_scan_points) {
          if(!point.intensity){
            continue;
          }
          char buffer[50];
          sprintf(buffer, "%.2f,%d", point.angle, point.distance);
          std::string response(buffer);
  
          ssize_t bytesSent = send(clientSocket, response.c_str(), response.size(), 0);
          if (bytesSent == -1) {
              std::cout << "Disconnected" << std::endl;
              FinishServer();
              return 1;
          }
        
        /*
#ifdef __LP64__
          LDS_LOG_INFO("stamp:%lu,angle:%f,distance(mm):%d,intensity:%d", 
            point.stamp, point.angle, point.distance, point.intensity);
#else
          LDS_LOG_INFO("stamp:%llu,angle:%f,distance(mm):%d,intensity:%d", 
            point.stamp, point.angle, point.distance, point.intensity);
#endif
*/

        }
        
        break;
      }
      case ldlidar::LidarStatus::DATA_TIME_OUT: {
        LDS_LOG_ERROR("ldlidar publish data is time out, please check your lidar device.","");
        node->Stop();
        break;
      }
      case ldlidar::LidarStatus::DATA_WAIT: {
        break;
      }
      default: {
        break;
      }
    }

    usleep(1000 * 100);  // sleep 100ms  == 10Hz
  }

  
  // Fecha os sockets
  FinishServer();
  receiveThread.join();
  std::cout << "Goodbye" << std::endl;

  return 0;
}

void FinishServer(){
  
  close(clientSocket);
  close(serverSocket);
  
  node->Stop();
  // LidarPowerOff();

  delete node;
  node = nullptr;
}


void car_forward(){
  gpioWrite(MOTOR_A, 0);
  gpioWrite(MOTOR_B, 1);
  gpioWrite(MOTOR_C, 1);
  gpioWrite(MOTOR_D, 0);
}

void car_backward(){
  gpioWrite(MOTOR_A, 1);
  gpioWrite(MOTOR_B, 0);
  gpioWrite(MOTOR_C, 0);
  gpioWrite(MOTOR_D, 1);
}

void car_left(){
  gpioWrite(MOTOR_A, 1);
  gpioWrite(MOTOR_B, 0);
  gpioWrite(MOTOR_C, 1);
  gpioWrite(MOTOR_D, 0);
}

void car_right(){
  gpioWrite(MOTOR_A, 0);
  gpioWrite(MOTOR_B, 1);
  gpioWrite(MOTOR_C, 0);
  gpioWrite(MOTOR_D, 1);
}

void car_stop(){
  gpioWrite(MOTOR_A, 0);
  gpioWrite(MOTOR_B, 0);
  gpioWrite(MOTOR_C, 0);
  gpioWrite(MOTOR_D, 0);
}