Control the YDLIDAR on C++

※이번 포스팅에선 C++을 활용하여 YDLidar를 제어하는 방법을 정리했다.

 

위치

아래 명령어를 통해 폴더를 살펴보면 tri_test.cpp라는 소스코드가 있다.

이번 포스팅에서는 tri_test.cpp를 기반으로 한 소스코드를 작성해 보았다.

$ ls ~/YDLidar-SDK/examples

examples list

tri_test.cpp 소스코드를 기본 기반으로 한 이유는 다른 소스코드는 필요 없는 filtering 라이브러리나 uart baudrate가 내가 사용하는 YdLidar G2와 다르기 때문이다.

 

CMakeLists.txt

소스코드를 컴파일하기 위해 CMakeLists.txt를 작성해야한다.

기본 코드는 주어져 있으며 필요 없는 것을 제거하고 필요한 것을 추가하여 사용했다.

(코드에 Control_Lidar 부분은 각자 파일에 맞게 수정)

※여기서 주의해야  부분은 CYdLidar.h 헤더 파일이 소스코드와 같은 폴더에 있어야한다.

위치는 아래 명령어로 찾은 뒤 복사하면 된다.

find / -name "CYdLidar.h"

cmake_minimum_required(VERSION 2.8)
project(Control_Lidar)
add_compile_options(-std=c++11) # Use C++11

# Include directories
include_directories(
     ${CMAKE_SOURCE_DIR}
     ${CMAKE_SOURCE_DIR}/../../
     ${CMAKE_CURRENT_BINARY_DIR}
)

set(EXECUTABLE_OUTPUT_PATH ${CMAKE_BINARY_DIR})

set(APP_NAME Control_Lidar)

add_executable(${APP_NAME} ../Control_Lidar.cpp)

target_link_libraries(${APP_NAME} ydlidar_sdk pthread)

install(TARGETS ${APP_NAME}
    RUNTIME DESTINATION bin
)

 

코드

아래는 tri_test.cpp를 기반으로 한 소스코드이다.

출력결과는 angle마다 distanse(range)를 출력하게 된다.

#include <iostream>
#include <string>
#include "CYdLidar.h"
#include <core/base/timer.h>
#include <core/common/ydlidar_help.h>

using namespace std;
using namespace ydlidar;

int main(){
        string port;
        os_init();

        map<string, string> ports = lidarPortList();
        map<string, string>::iterator it;


        if(ports.size() == 1)   port = ports.begin()->second;
        else{
                int id = 0;
                for (it = ports.begin(); it != ports.end(); it++)
                {
                        printf("[%d] %s %s\n", id, it->first.c_str(), it->second.c_str());
                        id++;
                }

                if(ports.empty()){
                        cout << "Not Lidar was detected. Please enter the lidar serial port : ";
                        cin >> port;
                }
                else{
                        while(os_isOk()){
                                cout << "Please select the lidar port : ";
                                string number;
                                cin >> number;

                                if ((size_t)atoi(number.c_str()) >= ports.size())
                                        continue;

                                it = ports.begin();
                                id = atoi(number.c_str());

                                while (id)
                                {
                                        id--;
                                        it++;
                                }

                                port = it->second;
                                break;
                        }
                }
        }

        int baudrate = 230400;
        cout << "Baudrate : " << baudrate << endl;

        bool isSingleChannel = true;
        cout << "One-way communication : " << isSingleChannel  << endl;

        float frequency = 5.0;
        cout << "Frequency : " << frequency << "Hz" << endl;

        if(!os_isOk())  return -1;

        CYdLidar laser;
        int optval = TYPE_TRIANGLE;
        string ignore_array;
        ignore_array.clear();
        laser.setlidaropt(LidarPropSerialPort, port.c_str(), port.size());
        laser.setlidaropt(LidarPropIgnoreArray, ignore_array.c_str(), ignore_array.size());
        laser.setlidaropt(LidarPropSerialBaudrate, &baudrate, sizeof(int));
        laser.setlidaropt(LidarPropLidarType, &optval, sizeof(int));
        optval = YDLIDAR_TYPE_SERIAL;
        laser.setlidaropt(LidarPropDeviceType, &optval, sizeof(int));
        optval = 5;
        laser.setlidaropt(LidarPropSampleRate, &optval, sizeof(int));
        optval = 4;
        laser.setlidaropt(LidarPropAbnormalCheckCount, &optval, sizeof(int));
        optval = 10;
        laser.setlidaropt(LidarPropIntenstiyBit, &optval, sizeof(int));

        bool b_optvalue = false;
        laser.setlidaropt(LidarPropFixedResolution, &b_optvalue, sizeof(bool));
        b_optvalue = false;
        laser.setlidaropt(LidarPropReversion, &b_optvalue, sizeof(bool));
        b_optvalue = false;
        laser.setlidaropt(LidarPropInverted, &b_optvalue, sizeof(bool));
        b_optvalue = true;
        laser.setlidaropt(LidarPropAutoReconnect, &b_optvalue, sizeof(bool));
        laser.setlidaropt(LidarPropSingleChannel, &isSingleChannel, sizeof(bool));
        b_optvalue = false;
        laser.setlidaropt(LidarPropIntenstiy, &b_optvalue, sizeof(bool));
        b_optvalue = true;
        laser.setlidaropt(LidarPropSupportMotorDtrCtrl, &b_optvalue, sizeof(bool));
        b_optvalue = false;
        laser.setlidaropt(LidarPropSupportHeartBeat, &b_optvalue, sizeof(bool));

        /// unit: °
        float f_optvalue = 180.0f;
        laser.setlidaropt(LidarPropMaxAngle, &f_optvalue, sizeof(float));
        f_optvalue = -180.0f;
        laser.setlidaropt(LidarPropMinAngle, &f_optvalue, sizeof(float));
        /// unit: m
        f_optvalue = 64.f;
        laser.setlidaropt(LidarPropMaxRange, &f_optvalue, sizeof(float));
        f_optvalue = 0.05f;
        laser.setlidaropt(LidarPropMinRange, &f_optvalue, sizeof(float));
        /// unit: Hz
        laser.setlidaropt(LidarPropScanFrequency, &frequency, sizeof(float));

        laser.enableGlassNoise(false);
        laser.enableSunNoise(false);

        laser.setBottomPriority(true);

        int c = 0;
        uint32_t t = getms();

        bool ret = laser.initialize();
        if(!ret){
                cerr << "Fail to initalize " << laser.DescribeError() << endl;
                return -1;
        }

        ret = laser.turnOn();
        if(!ret){
                cerr << "Fail to start "<< laser.DescribeError() << endl;
                return -1;
        }

        if(ret && os_isOk()){
                string userVersion;
                if(laser.getUserVersion(userVersion))
                        cout << "User Version : " << userVersion.c_str() << endl;
        }

        if(ret){
                device_info di;
                memset(&di, 0, DEVICEINFOSIZE);
                if(laser.getDeviceInfo(di, EPT_Module)) core::common::printfDeviceInfo(di, EPT_Module);
                else    cout << "Fail to get module device info" << endl;
                if(laser.getDeviceInfo(di, EPT_Base))   core::common::printfDeviceInfo(di, EPT_Base);
                else    cout << "Fail to get baseplate device info" << endl;
        }

        LaserScan scan;
        while(os_isOk()){
                if(laser.doProcessSimple(scan)){
                        cout << "Scan received [" << scan.points.size() <<
                                "] points scanFreq [" << scan.scanFreq << ']' << endl;

/*                      const LaserPoint &p = scan.points.at(0);
                        cout << 0 << " " << "dis : " << p.range << ", angle : " <<
                                p.angle * 180.0 / M_PI << endl;
*/
                        for(size_t i = 0; i < scan.points.size(); i++){
                                const LaserPoint &p = scan.points.at(i);
                                cout << i << " " << "dis : " << p.range << ", angle : " <<
                                        p.angle * 180.0 / M_PI << endl;
                        }
                }
                else{
                        cerr << "Failed to get Lidar Data" << endl;
                }

                if(!c++){
                        cout << "Time consuming " << getms() - t <<
                                " from initialization to parsing to point cloud data" << endl;
                }
        }

        laser.turnOff();
        laser.disconnecting();

        return 0;
}

 

컴파일

기본적으로 소스코드와 헤더파일이 있는 폴더에서 build 폴더를 생성한 뒤 cmake .. && make를 실행하여 컴파일을 진행한다.

$ mkdir build
$ cd build
$ cmake ..
$ make

컴파일하기 직전에 폴더 상태

정상적으로 컴파일된 폴더 상태(build 폴더 안)

위 사진 Vision_Lidar.cpp와 Control_Lidar가 같은 이름으로 생각하면 된다.(캡쳐를 잘 못 함.)

 

Control_Lidar 파일이 실행 파일이다.

실행 파일 이름은 CMakeLists.txt에 작성한 데로 생성된다.

 

 

다음 포스팅에서는 OpenCV에 Mat 클래스를 활용하여 뷰어를 만들 예정이다.