Unreal Build Source

///////Header//////////

// Fill out your copyright notice in the Description page of Project Settings.

#pragma once

#include "GameFramework/PlayerController.h"
#include <rplidar.h> //RPLIDAR standard sdk, all-in-one header
#include "LidarPlayerController.generated.h"


#ifndef _countof
#define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0]))
#endif

//using namespace rp::standalone::rplidar;

/**
 *
 */
UCLASS()
class OCULUSTEST_API ALidarPlayerController : public APlayerController
{
    GENERATED_BODY()

private:
    const char * opt_com_path = nullptr;
    _u32         opt_com_baudrate = 115200;
    u_result     op_result;
    rp::standalone::rplidar::RPlidarDriver * drv;
    bool checkRPLIDARHealth(rp::standalone::rplidar::RPlidarDriver * driver);

public:
    ALidarPlayerController(const class FObjectInitializer& PCIP);
    ~ALidarPlayerController();
    FVector2D poll();
};


///////CPP//////////

ALidarPlayerController::ALidarPlayerController(const class FObjectInitializer& PCIP) : Super(PCIP)
{
    opt_com_path = "\\\\.\\com3"; //Windows specific

    // create the driver instance
    drv = rp::standalone::rplidar::RPlidarDriver::CreateDriver(rp::standalone::rplidar::RPlidarDriver::DRIVER_TYPE_SERIALPORT);

    if (!drv) {
        if (GEngine)
            GEngine->AddOnScreenDebugMessage(-1, 5.f, FColor::Yellow, "insufficent memory, exit\n");
        exit(-2);
    }


    // make connection...
    if (IS_FAIL(drv->connect(opt_com_path, opt_com_baudrate))) {
        if (GEngine)
            GEngine->AddOnScreenDebugMessage(-1, 5.f, FColor::Yellow, "Error, cannot bind to the specified serial port %s.\n");
    }

    // check health...
    if (!checkRPLIDARHealth(drv)) {
        if (GEngine)
            GEngine->AddOnScreenDebugMessage(-1, 5.f, FColor::Yellow, "Lidar health warning.");
    }


    // start scan...
    drv->startScan();

    if (GEngine)
        GEngine->AddOnScreenDebugMessage(-1, 5.f, FColor::Yellow, "Lidar loaded\n");
};
ALidarPlayerController::~ALidarPlayerController()
{
    rp::standalone::rplidar::RPlidarDriver::DisposeDriver(drv);
};


FVector2D ALidarPlayerController::poll()
{
    rplidar_response_measurement_node_t nodes[360];
    size_t   count = _countof(nodes);
    printf("%u\n", count);
    op_result = drv->grabScanData(nodes, count);

    if (IS_OK(op_result)) {
        drv->ascendScanData(nodes, count);
        for (int pos = 0; pos < (int)count; ++pos) {
            float angle = (nodes[pos].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) / 64.0f;
            float distance = nodes[pos].distance_q2 / 4.0f;
        }
    }
    return FVector2D(0.0f, 0.0f);
}

bool ALidarPlayerController::checkRPLIDARHealth(rp::standalone::rplidar::RPlidarDriver * driver)
{

    u_result     op_result;
    rplidar_response_device_health_t healthinfo;


    op_result = drv->getHealth(healthinfo);
    if (IS_OK(op_result)) { // the macro IS_OK is the preperred way to judge whether the operation is succeed.
        if (GEngine)
            GEngine->AddOnScreenDebugMessage(-1, 5.f, FColor::Yellow, "RPLidar health status : %d\n");
        if (healthinfo.status == RPLIDAR_STATUS_ERROR) {
            if (GEngine)
                GEngine->AddOnScreenDebugMessage(-1, 5.f, FColor::Yellow, "Error, rplidar internal error detected. Please reboot the device to retry.\n");
            // enable the following code if you want rplidar to be reboot by software
            // driver->reset();
            return false;
        } else {
            return true;
        }

    } else {
        if (GEngine)
            GEngine->AddOnScreenDebugMessage(-1, 5.f, FColor::Yellow, "Error, cannot retrieve the lidar health code: %x\n");
        return false;
    }
}


///////////Project.Build.cs///////////////////

// Copyright 1998-2014 Epic Games, Inc. All Rights Reserved.

using UnrealBuildTool;
using System.IO;
public class OculusTest : ModuleRules
{
    private string ModulePath
    {
        get { return Path.GetDirectoryName(RulesCompiler.GetModuleFilename(this.GetType().Name)); }
    }

    private string ThirdPartyPath
    {
        get { return Path.GetFullPath(Path.Combine( ModulePath, "../../ThirdParty/")); }
    }


    public OculusTest(TargetInfo Target)
    {
        PublicDependencyModuleNames.AddRange(new string[] { "Core", "CoreUObject", "Engine", "InputCore" });
        // Add static libs
        PublicAdditionalLibraries.Add(Path.Combine(ThirdPartyPath, "RPLidar/Libraries", "rplidar_driver.lib"));
        // Add a path with the .h that has the function definitions
        PublicIncludePaths.Add(Path.Combine(ThirdPartyPath, "RPLidar", "Includes"));
    }
}