lidar.cpp

/**
 * @file main.cpp
 * @author LDRobot (support@ldrobot.com)
 * @brief  main process App
 *         This code is only applicable to LDROBOT LiDAR LD06 products
 * sold by Shenzhen LDROBOT Co., LTD
 * @version 0.1
 * @date 2021-10-28
 *
 * @copyright Copyright (c) 2021  SHENZHEN LDROBOT CO., LTD. All rights
 * reserved.
 * Licensed under the MIT License (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License in the file LICENSE
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

//#include "ldlidar_driver.h"
#include <iostream>
#include <thread>
#include <unistd.h>
#include <cstring>
#include <sys/socket.h>
#include <bluetooth/bluetooth.h>
#include <bluetooth/rfcomm.h>

bool continueListening = true;

// Function to send a string via Bluetooth
void sendString(int clientSocket, const std::string& message) {
    // Convert the string to a C-string
    const char* messageCStr = message.c_str();

    // Send the string via Bluetooth
    send(clientSocket, messageCStr, strlen(messageCStr), 0);
}

// Function to receive data via Bluetooth and convert it to a string
std::string receiveString(int clientSocket) {
    char buffer[1024]; // Size of the reception buffer
    memset(buffer, 0, sizeof(buffer)); // Clear the buffer

    // Receive data via Bluetooth
    recv(clientSocket, buffer, sizeof(buffer), 0);

    // Convert the buffer to a C++ string
    std::string receivedMessage(buffer);

    return receivedMessage;
}

// Function executed in a thread to handle Bluetooth message listening
void listenBluetooth(int clientSocket) {
    while (continueListening) {
        std::string receivedMessage = receiveString(clientSocket);

        // Call the desired function to process the received message
        std::cout << "Received message: " << receivedMessage << std::endl;
    }
}

int main() {
    int s; // Socket descriptor

    // Creating a Bluetooth socket
    s = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);

    // Configuring the socket information
    sockaddr_rc loc_addr = {0};
    loc_addr.rc_family = AF_BLUETOOTH;
    loc_addr.rc_bdaddr = {{0,0,0,0,0,0}};
    loc_addr.rc_channel = (uint8_t)1; // RFCOMM channel for communication

    // Binding the socket to the Bluetooth address
    bind(s, (struct sockaddr*)&loc_addr, sizeof(loc_addr));

    // Putting the socket into listening mode
    listen(s, 1);

    // Waiting for a Bluetooth connection
    std::cout << "Waiting for Bluetooth connection..." << std::endl;
    int client = accept(s, (struct sockaddr*)NULL, NULL);
    std::cout << "Bluetooth connection established!" << std::endl;

    // Starting the thread to listen for Bluetooth messages
    std::thread bluetoothThread(listenBluetooth, client);

    // Now, you can perform other tasks in the main thread or wait for the completion of the Bluetooth thread

    // Example: Sending a string upon connection
    std::string sentMessage = "Hello, Bluetooth device!";
    sendString(client, sentMessage);

    // Wait for a while to allow the message to be received by the other thread
    std::this_thread::sleep_for(std::chrono::seconds(5));

    // Terminate the Bluetooth thread
    continueListening = false;
    bluetoothThread.join();

    // Closing the sockets
    close(client);
    close(s);

    return 0;
}
/*
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 LidarPowerOn(void) {
//   LDS_LOG_DEBUG("Please Lidar Power On","");
//   // ...
// }

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

int main(int argc, char **argv) {

  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];
    }
  }

  ldlidar::LDLidarDriver* 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;
  
  while (ldlidar::LDLidarDriver::IsOk()) {
    float closest = -1;
    switch (node->GetLaserScanData(laser_scan_points, 1500)){
      case ldlidar::LidarStatus::NORMAL: {
        double lidar_scan_freq = 0;
        node->GetLidarScanFreq(lidar_scan_freq);

#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(closest == -1 || point.distance <= closest && point.intensity){
            closest = point.distance;
            std::cout << point.angle << " intensity " << point.intensity << std::endl;
          }
#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
  }

  node->Stop();
  // LidarPowerOff();

  delete node;
  node = nullptr;

  return 0;
}

/********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF
 * FILE ********/