windows VS2019 sick_tim系列雷达sdk配置

本教程针对于西克(sick)雷达官方sdk在window下(无ros的情况下)配置使用。

visaul stodio2019的安装教程很多,这里略过。

第一步:下载官网的sdk 配置编译环境

网址如下:

https://github.com/SICKAG/sick_scan_xd

https://github.com/microsoft/vcpkg/archive/master.zip

GitHub - SICKAG/msgpack11: A tiny MessagePack library for C++11 (msgpack.org[C++11])

下载完成后到d盘新建一个文件夹,命名为sick_scan_ws,随后分别将上述下载的文件解压到sick_scan_ws文件夹下并分别重命名为sick_scan_xd vcpkg msgpack11

Fig.1 sdk下载 

第二步:配置sdk

打开CMD(命令提示符)

输入以下命令(注意自己下载的文件夹的位置)

注意自己下载的文件夹的位置!!!注意自己下载的文件夹的位置!!!注意自己下载的文件夹的位置!!!

d: 
cd d:\sick_scan_ws\vcpkg                      //注意切换到自己vcpkg文件所在的文件夹
.\bootstrap-vcpkg.bat
.\vcpkg integrate install                     //以下过程会从github下载文件过程较长 注意(科学的)网络
vcpkg.exe install pthread:x86-windows    
vcpkg.exe install pthread:x64-windows
vcpkg.exe install boost:x64-windows          //这个过程可能会因为网络延时报错,多运行几下这行代码就没问题了
set PATH=d:\sick_scan_ws\vcpkg\installed\x64-windows\bin;%PATH%
cd d:\sick_scan_ws\vcpkg
bootstrap-vcpkg.bat
vcpkg integrate install
set PATH=d:\sick_scan_ws\vcpkg\installed\x64-windows\bin;%PATH%
cd d:\sick_scan_ws
mkdir sick_scan_xd\build\msgpack11
pushd sick_scan_xd\build\msgpack11
cmake -DMSGPACK11_BUILD_TESTS=0 -G "Visual Studio 16 2019" ../../../msgpack11
cmake --build . --clean-first --config Debug
cmake --build . --clean-first --config Release
REM open msgpack11.sln in Visual Studio 2019
popd
cd sick_scan_xd
set _os=x64
set _cmake_string=Visual Studio 16 2019
set _msvc=Visual Studio 2019
set _cmake_build_dir=build
if not exist %_cmake_build_dir% mkdir %_cmake_build_dir%
pushd %_cmake_build_dir%
cmake -DROS_VERSION=0 -G "%_cmake_string%" ..
cmake --build . --clean-first --config Debug //这行代码可能会编译不通过,需要找到报错的cpp文件 将注释中的一个乱码符号删除 cpp文件修改后 编译通过
cmake --build . --clean-first --config Release
REM open sick_scan.sln in Visual Studio 2019 for development and debugging
popd

以上配置完成后,可以在build文件夹看到sick_scan.sln的项目文件, 用vs2019打开该项目,右键选中sick_scan_shared_lib选择生成,生成成功后可以在build\Debug\目录下找到sick_scan_shared_lib.dll 。

至此,sdk配置就算完成了

如果觉得配置太麻烦可以选择下载我已经配置好的dll文件。

链接如下:https://download.csdn.net/download/qq_43222917/87155967

**补充:其他的项目都可以生成 其中sick_generic_caller需要在cmd模式下输入参数运行。(这里我觉得不方便使用,就略过cmd的调用模式,感兴趣的可以私我)

Fig.2 sick_scan_sln的项目

第三步:在新项目中配置sdk并在Debug模式下读取sick_timxx系列的点云数据

打开vs2019新建一个项目 注意项目是Debug x64位的

项目新建完成,右键项目选择属性进行配置

首先在VC++目录中的包含目录中添加sick_scan_xd所在的文件夹,然后在C/C++的附加包含目录下添加sick_scan_xd\include所在的位置。同时在预处理器中添加如下内容:

%(PreprocessorDefinitions)
WIN32
_WINDOWS
CMAKE_INTDIR="Debug"
_CRT_SECURE_NO_WARNINGS

在链接器的输入添加如下内容:

kernel32.lib
user32.lib
gdi32.lib
winspool.lib
shell32.lib
ole32.lib
oleaut32.lib
uuid.lib
comdlg32.lib
advapi32.lib

最后将之前生成的sick_scan_shared_lib.dll文件与对应的sick_timxx的launch文件(在sick_scan_xd\launch下)复制到项目的Debug目录下。

环境到这里配置完成

在源文件中新建一个c文件,命名为:sick_scan_xd_api_wrapper.c 代码如下:

#if defined _MSC_VER && _MSC_VER >= 1300
#   pragma warning( disable : 4996 ) // suppress warning 4996 about unsafe string functions like strcpy, sprintf, etc.
#   ifndef _CRT_SECURE_NO_DEPRECATE
#   define  _CRT_SECURE_NO_DEPRECATE // suppress warning 4996 about unsafe string functions like strcpy, sprintf, etc.
#   endif
#endif
#ifdef WIN32
#  include <windows.h>
#else
#  include <dlfcn.h>
#endif

#include  <stdio.h>
#include <stdlib.h>
#include <string.h>
#include  <ctype.h>

#include "sick_scan_xd_api/sick_scan_api.h"

#ifdef WIN32
#else
typedef void* HINSTANCE;
static HINSTANCE LoadLibrary(const char* szLibFilename)
{
  return dlopen(szLibFilename,RTLD_GLOBAL|RTLD_LAZY);
}
static int FreeLibrary ( HINSTANCE hLib )
{
  return !dlclose(hLib);
}
static void* GetProcAddress(HINSTANCE hLib, const char* szFunctionName)
{
  return dlsym(hLib,szFunctionName);
}
#endif

static HINSTANCE hinstLib = NULL;

#ifdef WIN32
typedef SickScanApiHandle(__stdcall* SickScanApiCreate_PROCTYPE)(int argc, char** argv);
static SickScanApiCreate_PROCTYPE ptSickScanApiCreate = 0;
#else
typedef SickScanApiHandle(*SickScanApiCreate_PROCTYPE)(int argc, char** argv);
static SickScanApiCreate_PROCTYPE ptSickScanApiCreate = 0;
#endif

typedef int32_t(*SickScanApiRelease_PROCTYPE)(SickScanApiHandle apiHandle);
static SickScanApiRelease_PROCTYPE ptSickScanApiRelease = 0;

typedef int32_t(*SickScanApiInitByLaunchfile_PROCTYPE)(SickScanApiHandle apiHandle, const char* launchfile);
static SickScanApiInitByLaunchfile_PROCTYPE ptSickScanApiInitByLaunchfile = 0;

typedef int32_t(*SickScanApiInitByCli_PROCTYPE)(SickScanApiHandle apiHandle, int argc, char** argv);
static SickScanApiInitByCli_PROCTYPE ptSickScanApiInitByCli = 0;

typedef int32_t(*SickScanApiClose_PROCTYPE)(SickScanApiHandle apiHandle);
static SickScanApiClose_PROCTYPE ptSickScanApiClose = 0;

typedef int32_t(*SickScanApiRegisterCartesianPointCloudMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanPointCloudMsgCallback callback);
static SickScanApiRegisterCartesianPointCloudMsg_PROCTYPE ptSickScanApiRegisterCartesianPointCloudMsg = 0;

typedef int32_t(*SickScanApiDeregisterCartesianPointCloudMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanPointCloudMsgCallback callback);
static SickScanApiDeregisterCartesianPointCloudMsg_PROCTYPE ptSickScanApiDeregisterCartesianPointCloudMsg = 0;

typedef int32_t(*SickScanApiRegisterPolarPointCloudMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanPointCloudMsgCallback callback);
static SickScanApiRegisterPolarPointCloudMsg_PROCTYPE ptSickScanApiRegisterPolarPointCloudMsg = 0;

typedef int32_t(*SickScanApiDeregisterPolarPointCloudMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanPointCloudMsgCallback callback);
static SickScanApiDeregisterPolarPointCloudMsg_PROCTYPE ptSickScanApiDeregisterPolarPointCloudMsg = 0;

typedef int32_t(*SickScanApiRegisterImuMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanImuMsgCallback callback);
static SickScanApiRegisterImuMsg_PROCTYPE ptSickScanApiRegisterImuMsg = 0;

typedef int32_t(*SickScanApiDeregisterImuMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanImuMsgCallback callback);
static SickScanApiDeregisterImuMsg_PROCTYPE ptSickScanApiDeregisterImuMsg = 0;

typedef int32_t(*SickScanApiRegisterLFErecMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanLFErecMsgCallback callback);
static SickScanApiRegisterLFErecMsg_PROCTYPE ptSickScanApiRegisterLFErecMsg = 0;

typedef int32_t(*SickScanApiDeregisterLFErecMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanLFErecMsgCallback callback);
static SickScanApiDeregisterLFErecMsg_PROCTYPE ptSickScanApiDeregisterLFErecMsg = 0;

typedef int32_t(*SickScanApiRegisterLIDoutputstateMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanLIDoutputstateMsgCallback callback);
static SickScanApiRegisterLIDoutputstateMsg_PROCTYPE ptSickScanApiRegisterLIDoutputstateMsg = 0;

typedef int32_t(*SickScanApiDeregisterLIDoutputstateMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanLIDoutputstateMsgCallback callback);
static SickScanApiDeregisterLIDoutputstateMsg_PROCTYPE ptSickScanApiDeregisterLIDoutputstateMsg = 0;

typedef int32_t(*SickScanApiRegisterRadarScanMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanRadarScanCallback callback);
static SickScanApiRegisterRadarScanMsg_PROCTYPE ptSickScanApiRegisterRadarScanMsg = 0;

typedef int32_t(*SickScanApiDeregisterRadarScanMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanRadarScanCallback callback);
static SickScanApiDeregisterRadarScanMsg_PROCTYPE ptSickScanApiDeregisterRadarScanMsg = 0;

typedef int32_t(*SickScanApiRegisterLdmrsObjectArrayMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanLdmrsObjectArrayCallback callback);
static SickScanApiRegisterLdmrsObjectArrayMsg_PROCTYPE ptSickScanApiRegisterLdmrsObjectArrayMsg = 0;

typedef int32_t(*SickScanApiDeregisterLdmrsObjectArrayMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanLdmrsObjectArrayCallback callback);
static SickScanApiDeregisterLdmrsObjectArrayMsg_PROCTYPE ptSickScanApiDeregisterLdmrsObjectArrayMsg = 0;

typedef int32_t(*SickScanApiRegisterVisualizationMarkerMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanVisualizationMarkerCallback callback);
static SickScanApiRegisterVisualizationMarkerMsg_PROCTYPE ptSickScanApiRegisterVisualizationMarkerMsg = 0;

typedef int32_t(*SickScanApiDeregisterVisualizationMarkerMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanVisualizationMarkerCallback callback);
static SickScanApiDeregisterVisualizationMarkerMsg_PROCTYPE ptSickScanApiDeregisterVisualizationMarkerMsg = 0;

typedef int32_t(*SickScanApiWaitNextCartesianPointCloudMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanPointCloudMsg* msg, double timeout_sec);
static SickScanApiWaitNextCartesianPointCloudMsg_PROCTYPE ptSickScanApiWaitNextCartesianPointCloudMsg = 0;

typedef int32_t(*SickScanApiWaitNextPolarPointCloudMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanPointCloudMsg* msg, double timeout_sec);
static SickScanApiWaitNextPolarPointCloudMsg_PROCTYPE ptSickScanApiWaitNextPolarPointCloudMsg = 0;

typedef int32_t(*SickScanApiFreePointCloudMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanPointCloudMsg* msg);
static SickScanApiFreePointCloudMsg_PROCTYPE ptSickScanApiFreePointCloudMsg = 0;

typedef int32_t(*SickScanApiWaitNextImuMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanImuMsg* msg, double timeout_sec);
static SickScanApiWaitNextImuMsg_PROCTYPE ptSickScanApiWaitNextImuMsg = 0;

typedef int32_t(*SickScanApiFreeImuMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanImuMsg* msg);
static SickScanApiFreeImuMsg_PROCTYPE ptSickScanApiFreeImuMsg = 0;

typedef int32_t(*SickScanApiWaitNextLFErecMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanLFErecMsg* msg, double timeout_sec);
static SickScanApiWaitNextLFErecMsg_PROCTYPE ptSickScanApiWaitNextLFErecMsg = 0;

typedef int32_t(*SickScanApiFreeLFErecMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanLFErecMsg* msg);
static SickScanApiFreeLFErecMsg_PROCTYPE ptSickScanApiFreeLFErecMsg = 0;

typedef int32_t(*SickScanApiWaitNextLIDoutputstateMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanLIDoutputstateMsg* msg, double timeout_sec);
static SickScanApiWaitNextLIDoutputstateMsg_PROCTYPE ptSickScanApiWaitNextLIDoutputstateMsg = 0;

typedef int32_t(*SickScanApiFreeLIDoutputstateMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanLIDoutputstateMsg* msg);
static SickScanApiFreeLIDoutputstateMsg_PROCTYPE ptSickScanApiFreeLIDoutputstateMsg = 0;

typedef int32_t(*SickScanApiWaitNextRadarScanMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanRadarScan* msg, double timeout_sec);
static SickScanApiWaitNextRadarScanMsg_PROCTYPE ptSickScanApiWaitNextRadarScanMsg = 0;

typedef int32_t(*SickScanApiFreeRadarScanMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanRadarScan* msg);
static SickScanApiFreeRadarScanMsg_PROCTYPE ptSickScanApiFreeRadarScanMsg = 0;

typedef int32_t(*SickScanApiWaitNextLdmrsObjectArrayMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanLdmrsObjectArray* msg, double timeout_sec);
static SickScanApiWaitNextLdmrsObjectArrayMsg_PROCTYPE ptSickScanApiWaitNextLdmrsObjectArrayMsg = 0;

typedef int32_t(*SickScanApiFreeLdmrsObjectArrayMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanLdmrsObjectArray* msg);
static SickScanApiFreeLdmrsObjectArrayMsg_PROCTYPE ptSickScanApiFreeLdmrsObjectArrayMsg = 0;

typedef int32_t(*SickScanApiWaitNextVisualizationMarkerMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanVisualizationMarkerMsg* msg, double timeout_sec);
static SickScanApiWaitNextVisualizationMarkerMsg_PROCTYPE ptSickScanApiWaitNextVisualizationMarkerMsg = 0;

typedef int32_t(*SickScanApiFreeVisualizationMarkerMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanVisualizationMarkerMsg* msg);
static SickScanApiFreeVisualizationMarkerMsg_PROCTYPE ptSickScanApiFreeVisualizationMarkersg = 0;

/*
*  Functions and macros to initialize and close the API and a lidar
*/

// load a function by its name using GetProcAddress if not done before (i.e. if ptFunction is 0)
#define CACHE_FUNCTION_PTR(apiHandle, ptFunction, szFunctionName, procType)                              \
do                                                                                                       \
{                                                                                                        \
    if (hinstLib == 0 || apiHandle == 0)                                                                 \
    {                                                                                                    \
        printf("## ERROR SickScanApi, cacheFunctionPtr(%s): library not initialized\n", szFunctionName); \
        ptFunction = 0;                                                                                  \
    }                                                                                                    \
    else if (ptFunction == 0)                                                                            \
    {                                                                                                    \
        ptFunction = (procType)GetProcAddress(hinstLib, szFunctionName);                                 \
    }                                                                                                    \
    if (ptFunction == 0)                                                                                 \
    {                                                                                                    \
        printf("## ERROR SickScanApi, cacheFunctionPtr(%s): GetProcAddress failed\n", szFunctionName);   \
    }                                                                                                    \
}   while(0)

// Load sick_scan_xd api library (dll or so file)
int32_t SickScanApiLoadLibrary(const char* library_filepath)
{
    int32_t ret = SICK_SCAN_API_SUCCESS;
    if (hinstLib == NULL)
    {
        hinstLib = LoadLibrary(library_filepath);
    }
    if (hinstLib == NULL)
    {
        printf("## ERROR SickScanApiLoadLibrary: LoadLibrary(%s) failed\n", library_filepath);
        ret = SICK_SCAN_API_NOT_LOADED;
    }
    return ret;
}

// Unload sick_scan_xd api library
int32_t SickScanApiUnloadLibrary()
{
    int32_t ret = SICK_SCAN_API_SUCCESS;
    if (hinstLib != 0)
    {
        if (!FreeLibrary(hinstLib))
        {
            printf("## ERROR SickScanApiUnloadLibrary: FreeLibrary() failed\n");
            ret = SICK_SCAN_API_ERROR;
        }
    }
    hinstLib = 0;
    ptSickScanApiCreate = 0;
    ptSickScanApiRelease = 0;
    ptSickScanApiInitByLaunchfile = 0;
    ptSickScanApiInitByCli = 0;
    ptSickScanApiClose = 0;
    ptSickScanApiRegisterCartesianPointCloudMsg = 0;
    ptSickScanApiDeregisterCartesianPointCloudMsg = 0;
    ptSickScanApiRegisterPolarPointCloudMsg = 0;
    ptSickScanApiDeregisterPolarPointCloudMsg = 0;
    ptSickScanApiRegisterImuMsg = 0;
    ptSickScanApiDeregisterImuMsg = 0;
    ptSickScanApiRegisterLFErecMsg = 0;
    ptSickScanApiDeregisterLFErecMsg = 0;
    ptSickScanApiRegisterLIDoutputstateMsg = 0;
    ptSickScanApiDeregisterLIDoutputstateMsg = 0;
    ptSickScanApiRegisterRadarScanMsg = 0;
    ptSickScanApiDeregisterRadarScanMsg = 0;
    ptSickScanApiRegisterLdmrsObjectArrayMsg = 0;
    ptSickScanApiDeregisterLdmrsObjectArrayMsg = 0;
    ptSickScanApiRegisterVisualizationMarkerMsg = 0;
    ptSickScanApiDeregisterVisualizationMarkerMsg = 0;
    ptSickScanApiWaitNextCartesianPointCloudMsg = 0;
    ptSickScanApiWaitNextPolarPointCloudMsg = 0;
    ptSickScanApiFreePointCloudMsg = 0;
    ptSickScanApiWaitNextImuMsg = 0;
    ptSickScanApiFreeImuMsg = 0;
    ptSickScanApiWaitNextLFErecMsg = 0;
    ptSickScanApiFreeLFErecMsg = 0;
    ptSickScanApiWaitNextLIDoutputstateMsg = 0;
    ptSickScanApiFreeLIDoutputstateMsg = 0;
    ptSickScanApiWaitNextRadarScanMsg = 0;
    ptSickScanApiFreeRadarScanMsg = 0;
    ptSickScanApiWaitNextLdmrsObjectArrayMsg = 0;
    ptSickScanApiFreeLdmrsObjectArrayMsg = 0;
    ptSickScanApiWaitNextVisualizationMarkerMsg = 0;
    ptSickScanApiFreeVisualizationMarkersg = 0;
    return ret;
}

/*
*  Create an instance of sick_scan_xd api.
*  Optional commandline arguments argc, argv identical to sick_generic_caller.
*  Call SickScanApiInitByLaunchfile or SickScanApiInitByCli to process a lidar.
*/
SickScanApiHandle SickScanApiCreate(int argc, char** argv)
{
    if (hinstLib == 0)
    {
        printf("## ERROR SickScanApiCreate: library not loaded\n");
        return 0;
    }
    if (ptSickScanApiCreate == 0)
    {
        ptSickScanApiCreate = (SickScanApiCreate_PROCTYPE)GetProcAddress(hinstLib, "SickScanApiCreate");
    }
    if (ptSickScanApiCreate == 0)
    {
        printf("## ERROR SickScanApiCreate: GetProcAddress failed\n");
        return 0;
    }
    SickScanApiHandle apiHandle = ptSickScanApiCreate(argc, argv);
    if (apiHandle == 0)
    {
        printf("## ERROR SickScanApiCreate: library call SickScanApiCreate() returned 0\n");
    }
    return apiHandle;
}

// Release and free all resources of a handle; the handle is invalid after SickScanApiRelease
int32_t SickScanApiRelease(SickScanApiHandle apiHandle)
{
    CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiRelease, "SickScanApiRelease", SickScanApiRelease_PROCTYPE);
    int32_t ret = (ptSickScanApiRelease ? (ptSickScanApiRelease(apiHandle)) : SICK_SCAN_API_NOT_INITIALIZED);
    if (ret != SICK_SCAN_API_SUCCESS)
        printf("## ERROR SickScanApiRelease: library call SickScanApiRelease() failed, error code %d\n", ret);
    return ret;
}

// Initializes a lidar by launchfile and starts message receiving and processing
int32_t SickScanApiInitByLaunchfile(SickScanApiHandle apiHandle, const char* launchfile_args)
{
    CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiInitByLaunchfile, "SickScanApiInitByLaunchfile", SickScanApiInitByLaunchfile_PROCTYPE);
    int32_t ret = (ptSickScanApiInitByLaunchfile ? (ptSickScanApiInitByLaunchfile(apiHandle, launchfile_args)) : SICK_SCAN_API_NOT_INITIALIZED);
    if (ret != SICK_SCAN_API_SUCCESS)
        printf("## ERROR SickScanApiInitByLaunchfile: library call SickScanApiInitByLaunchfile() failed, error code %d\n", ret);
    return ret;
}

// Initializes a lidar by commandline arguments and starts message receiving and processing
int32_t SickScanApiInitByCli(SickScanApiHandle apiHandle, int argc, char** argv)
{
    CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiInitByCli, "SickScanApiInitByCli", SickScanApiInitByCli_PROCTYPE);
    int32_t ret = (ptSickScanApiInitByCli ? (ptSickScanApiInitByCli(apiHandle, argc, argv)) : SICK_SCAN_API_NOT_INITIALIZED);
    if (ret != SICK_SCAN_API_SUCCESS)
        printf("## ERROR SickScanApiInitByCli: library call SickScanApiInitByCli() failed, error code %d\n", ret);
    return ret;
}

// Stops message receiving and processing and closes a lidar
int32_t SickScanApiClose(SickScanApiHandle apiHandle)
{
    CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiClose, "SickScanApiClose", SickScanApiClose_PROCTYPE);
    int32_t ret = (ptSickScanApiClose ? (ptSickScanApiClose(apiHandle)) : SICK_SCAN_API_NOT_INITIALIZED);
    if (ret != SICK_SCAN_API_SUCCESS)
        printf("## ERROR SickScanApiClose: library call SickScanApiClose() failed, error code %d\n", ret);
    return ret;
}

/*
*  Registration / deregistration of message callbacks
*/

// Register / deregister a callback for cartesian PointCloud messages, pointcloud in cartesian coordinates with fields x, y, z, intensity
int32_t SickScanApiRegisterCartesianPointCloudMsg(SickScanApiHandle apiHandle, SickScanPointCloudMsgCallback callback)
{
    CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiRegisterCartesianPointCloudMsg, "SickScanApiRegisterCartesianPointCloudMsg", SickScanApiRegisterCartesianPointCloudMsg_PROCTYPE);
    int32_t ret = (ptSickScanApiRegisterCartesianPointCloudMsg ? (ptSickScanApiRegisterCartesianPointCloudMsg(apiHandle, callback)) : SICK_SCAN_API_NOT_INITIALIZED);
    if (ret != SICK_SCAN_API_SUCCESS)
        printf("## ERROR SickScanApiRegisterCartesianPointCloudMsg: library call SickScanApiRegisterCartesianPointCloudMsg() failed, error code %d\n", ret);
    return ret;
}
int32_t SickScanApiDeregisterCartesianPointCloudMsg(SickScanApiHandle apiHandle, SickScanPointCloudMsgCallback callback)
{
    CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiDeregisterCartesianPointCloudMsg, "SickScanApiDeregisterCartesianPointCloudMsg", SickScanApiDeregisterCartesianPointCloudMsg_PROCTYPE);
    int32_t ret = (ptSickScanApiDeregisterCartesianPointCloudMsg ? (ptSickScanApiDeregisterCartesianPointCloudMsg(apiHandle, callback)) : SICK_SCAN_API_NOT_INITIALIZED);
    if (ret != SICK_SCAN_API_SUCCESS)
        printf("## ERROR SickScanApiDeregisterCartesianPointCloudMsg: library call SickScanApiDeregisterCartesianPointCloudMsg() failed, error code %d\n", ret);
    return ret;
}

// Register / deregister a callback for polar PointCloud messages, pointcloud in polar coordinates with fields range, azimuth, elevation, intensity
int32_t SickScanApiRegisterPolarPointCloudMsg(SickScanApiHandle apiHandle, SickScanPointCloudMsgCallback callback)
{
    CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiRegisterPolarPointCloudMsg, "SickScanApiRegisterPolarPointCloudMsg", SickScanApiRegisterPolarPointCloudMsg_PROCTYPE);
    int32_t ret = (ptSickScanApiRegisterPolarPointCloudMsg ? (ptSickScanApiRegisterPolarPointCloudMsg(apiHandle, callback)) : SICK_SCAN_API_NOT_INITIALIZED);
    if (ret != SICK_SCAN_API_SUCCESS)
        printf("## ERROR SickScanApiRegisterPolarPointCloudMsg: library call SickScanApiRegisterPolarPointCloudMsg() failed, error code %d\n", ret);
    return ret;
}
int32_t SickScanApiDeregisterPolarPointCloudMsg(SickScanApiHandle apiHandle, SickScanPointCloudMsgCallback callback)
{
    CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiDeregisterPolarPointCloudMsg, "SickScanApiDeregisterPolarPointCloudMsg", SickScanApiDeregisterPolarPointCloudMsg_PROCTYPE);
    int32_t ret = (ptSickScanApiDeregisterPolarPointCloudMsg ? (ptSickScanApiDeregisterPolarPointCloudMsg(apiHandle, callback)) : SICK_SCAN_API_NOT_INITIALIZED);
    if (ret != SICK_SCAN_API_SUCCESS)
        printf("## ERROR SickScanApiDeregisterPolarPointCloudMsg: library call SickScanApiDeregisterPolarPointCloudMsg() failed, error code %d\n", ret);
    return ret;
}

// Register / deregister a callback for Imu messages
int32_t SickScanApiRegisterImuMsg(SickScanApiHandle apiHandle, SickScanImuMsgCallback callback)
{
    CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiRegisterImuMsg, "SickScanApiRegisterImuMsg", SickScanApiRegisterImuMsg_PROCTYPE);
    int32_t ret = (ptSickScanApiRegisterImuMsg ? (ptSickScanApiRegisterImuMsg(apiHandle, callback)) : SICK_SCAN_API_NOT_INITIALIZED);
    if (ret != SICK_SCAN_API_SUCCESS)
        printf("## ERROR SickScanApiRegisterImuMsg: library call SickScanApiRegisterImuMsg() failed, error code %d\n", ret);
    return ret;
}
int32_t SickScanApiDeregisterImuMsg(SickScanApiHandle apiHandle, SickScanImuMsgCallback callback)
{
    CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiDeregisterImuMsg, "SickScanApiDeregisterImuMsg", SickScanApiDeregisterImuMsg_PROCTYPE);
    int32_t ret = (ptSickScanApiDeregisterImuMsg ? (ptSickScanApiDeregisterImuMsg(apiHandle, callback)) : SICK_SCAN_API_NOT_INITIALIZED);
    if (ret != SICK_SCAN_API_SUCCESS)
        printf("## ERROR SickScanApiDeregisterImuMsg: library call SickScanApiDeregisterImuMsg() failed, error code %d\n", ret);
    return ret;
}

// Register / deregister a callback for SickScanLFErecMsg messages
int32_t SickScanApiRegisterLFErecMsg(SickScanApiHandle apiHandle, SickScanLFErecMsgCallback callback)
{
    CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiRegisterLFErecMsg, "SickScanApiRegisterLFErecMsg", SickScanApiRegisterLFErecMsg_PROCTYPE);
    int32_t ret = (ptSickScanApiRegisterLFErecMsg ? (ptSickScanApiRegisterLFErecMsg(apiHandle, callback)) : SICK_SCAN_API_NOT_INITIALIZED);
    if (ret != SICK_SCAN_API_SUCCESS)
        printf("## ERROR SickScanApiRegisterLFErecMsg: library call SickScanApiRegisterLFErecMsg() failed, error code %d\n", ret);
    return ret;
}
int32_t SickScanApiDeregisterLFErecMsg(SickScanApiHandle apiHandle, SickScanLFErecMsgCallback callback)
{
    CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiDeregisterLFErecMsg, "SickScanApiDeregisterLFErecMsg", SickScanApiDeregisterLFErecMsg_PROCTYPE);
    int32_t ret = (ptSickScanApiDeregisterLFErecMsg ? (ptSickScanApiDeregisterLFErecMsg(apiHandle, callback)) : SICK_SCAN_API_NOT_INITIALIZED);
    if (ret != SICK_SCAN_API_SUCCESS)
        printf("## ERROR SickScanApiDeregisterLFErecMsg: library call SickScanApiDeregisterLFErecMsg() failed, error code %d\n", ret);
    return ret;
}

// Register / deregister a callback for SickScanLIDoutputstateMsg messages
int32_t SickScanApiRegisterLIDoutputstateMsg(SickScanApiHandle apiHandle, SickScanLIDoutputstateMsgCallback callback)
{
    CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiRegisterLIDoutputstateMsg, "SickScanApiRegisterLIDoutputstateMsg", SickScanApiRegisterLIDoutputstateMsg_PROCTYPE);
    int32_t ret = (ptSickScanApiRegisterLIDoutputstateMsg ? (ptSickScanApiRegisterLIDoutputstateMsg(apiHandle, callback)) : SICK_SCAN_API_NOT_INITIALIZED);
    if (ret != SICK_SCAN_API_SUCCESS)
        printf("## ERROR SickScanApiRegisterLIDoutputstateMsg: library call SickScanApiRegisterLIDoutputstateMsg() failed, error code %d\n", ret);
    return ret;
}
int32_t SickScanApiDeregisterLIDoutputstateMsg(SickScanApiHandle apiHandle, SickScanLIDoutputstateMsgCallback callback)
{
    CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiDeregisterLIDoutputstateMsg, "SickScanApiDeregisterLIDoutputstateMsg", SickScanApiDeregisterLIDoutputstateMsg_PROCTYPE);
    int32_t ret = (ptSickScanApiDeregisterLIDoutputstateMsg ? (ptSickScanApiDeregisterLIDoutputstateMsg(apiHandle, callback)) : SICK_SCAN_API_NOT_INITIALIZED);
    if (ret != SICK_SCAN_API_SUCCESS)
        printf("## ERROR SickScanApiDeregisterLIDoutputstateMsg: library call SickScanApiDeregisterLIDoutputstateMsg() failed, error code %d\n", ret);
    return ret;
}

// Register / deregister a callback for SickScanRadarScan messages
int32_t SickScanApiRegisterRadarScanMsg(SickScanApiHandle apiHandle, SickScanRadarScanCallback callback)
{
    CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiRegisterRadarScanMsg, "SickScanApiRegisterRadarScanMsg", SickScanApiRegisterRadarScanMsg_PROCTYPE);
    int32_t ret = (ptSickScanApiRegisterRadarScanMsg ? (ptSickScanApiRegisterRadarScanMsg(apiHandle, callback)) : SICK_SCAN_API_NOT_INITIALIZED);
    if (ret != SICK_SCAN_API_SUCCESS)
        printf("## ERROR SickScanApiRegisterRadarScanMsg: library call SickScanApiRegisterRadarScanMsg() failed, error code %d\n", ret);
    return ret;
}
int32_t SickScanApiDeregisterRadarScanMsg(SickScanApiHandle apiHandle, SickScanRadarScanCallback callback)
{
    CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiDeregisterRadarScanMsg, "SickScanApiDeregisterRadarScanMsg", SickScanApiDeregisterRadarScanMsg_PROCTYPE);
    int32_t ret = (ptSickScanApiDeregisterRadarScanMsg ? (ptSickScanApiDeregisterRadarScanMsg(apiHandle, callback)) : SICK_SCAN_API_NOT_INITIALIZED);
    if (ret != SICK_SCAN_API_SUCCESS)
        printf("## ERROR SickScanApiDeregisterRadarScanMsg: library call SickScanApiDeregisterRadarScanMsg() failed, error code %d\n", ret);
    return ret;
}

// Register / deregister a callback for SickScanLdmrsObjectArray messages
int32_t SickScanApiRegisterLdmrsObjectArrayMsg(SickScanApiHandle apiHandle, SickScanLdmrsObjectArrayCallback callback)
{
    CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiRegisterLdmrsObjectArrayMsg, "SickScanApiRegisterLdmrsObjectArrayMsg", SickScanApiRegisterLdmrsObjectArrayMsg_PROCTYPE);
    int32_t ret = (ptSickScanApiRegisterLdmrsObjectArrayMsg ? (ptSickScanApiRegisterLdmrsObjectArrayMsg(apiHandle, callback)) : SICK_SCAN_API_NOT_INITIALIZED);
    if (ret != SICK_SCAN_API_SUCCESS)
        printf("## ERROR SickScanApiRegisterLdmrsObjectArrayMsg: library call SickScanApiRegisterLdmrsObjectArrayMsg() failed, error code %d\n", ret);
    return ret;
}
int32_t SickScanApiDeregisterLdmrsObjectArrayMsg(SickScanApiHandle apiHandle, SickScanLdmrsObjectArrayCallback callback)
{
    CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiDeregisterLdmrsObjectArrayMsg, "SickScanApiDeregisterLdmrsObjectArrayMsg", SickScanApiDeregisterLdmrsObjectArrayMsg_PROCTYPE);
    int32_t ret = (ptSickScanApiDeregisterLdmrsObjectArrayMsg ? (ptSickScanApiDeregisterLdmrsObjectArrayMsg(apiHandle, callback)) : SICK_SCAN_API_NOT_INITIALIZED);
    if (ret != SICK_SCAN_API_SUCCESS)
        printf("## ERROR SickScanApiDeregisterLdmrsObjectArrayMsg: library call SickScanApiDeregisterLdmrsObjectArrayMsg() failed, error code %d\n", ret);
    return ret;
}

// Register / deregister a callback for VisualizationMarker messages
int32_t SickScanApiRegisterVisualizationMarkerMsg(SickScanApiHandle apiHandle, SickScanVisualizationMarkerCallback callback)
{
    CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiRegisterVisualizationMarkerMsg, "SickScanApiRegisterVisualizationMarkerMsg", SickScanApiRegisterVisualizationMarkerMsg_PROCTYPE);
    int32_t ret = (ptSickScanApiRegisterVisualizationMarkerMsg ? (ptSickScanApiRegisterVisualizationMarkerMsg(apiHandle, callback)) : SICK_SCAN_API_NOT_INITIALIZED);
    if (ret != SICK_SCAN_API_SUCCESS)
        printf("## ERROR SickScanApiRegisterVisualizationMarkerMsg: library call SickScanApiRegisterVisualizationMarkerMsg() failed, error code %d\n", ret);
    return ret;
}
int32_t SickScanApiDeregisterVisualizationMarkerMsg(SickScanApiHandle apiHandle, SickScanVisualizationMarkerCallback callback)
{
    CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiDeregisterVisualizationMarkerMsg, "SickScanApiDeregisterVisualizationMarkerMsg", SickScanApiDeregisterVisualizationMarkerMsg_PROCTYPE);
    int32_t ret = (ptSickScanApiDeregisterVisualizationMarkerMsg ? (ptSickScanApiDeregisterVisualizationMarkerMsg(apiHandle, callback)) : SICK_SCAN_API_NOT_INITIALIZED);
    if (ret != SICK_SCAN_API_SUCCESS)
        printf("## ERROR SickScanApiDeregisterVisualizationMarkerMsg: library call SickScanApiDeregisterVisualizationMarkerMsg() failed, error code %d\n", ret);
    return ret;
}

/*
*  Polling functions
*/

// Wait for and return the next cartesian resp. polar PointCloud messages. Note: SickScanApiWait...Msg() allocates a message. Use function SickScanApiFree...Msg() to deallocate it after use.
int32_t SickScanApiWaitNextCartesianPointCloudMsg(SickScanApiHandle apiHandle, SickScanPointCloudMsg* msg, double timeout_sec)
{
    CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiWaitNextCartesianPointCloudMsg, "SickScanApiWaitNextCartesianPointCloudMsg", SickScanApiWaitNextCartesianPointCloudMsg_PROCTYPE);
    int32_t ret = (ptSickScanApiWaitNextCartesianPointCloudMsg ? (ptSickScanApiWaitNextCartesianPointCloudMsg(apiHandle, msg, timeout_sec)) : SICK_SCAN_API_NOT_INITIALIZED);
    if (ret != SICK_SCAN_API_SUCCESS && ret != SICK_SCAN_API_TIMEOUT)
        printf("## ERROR SickScanApiWaitNextCartesianPointCloudMsg: library call SickScanApiWaitNextCartesianPointCloudMsg() failed, error code %d\n", ret);
    return ret;
}
int32_t SickScanApiWaitNextPolarPointCloudMsg(SickScanApiHandle apiHandle, SickScanPointCloudMsg* msg, double timeout_sec)
{
    CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiWaitNextPolarPointCloudMsg, "SickScanApiWaitNextPolarPointCloudMsg", SickScanApiWaitNextPolarPointCloudMsg_PROCTYPE);
    int32_t ret = (ptSickScanApiWaitNextPolarPointCloudMsg ? (ptSickScanApiWaitNextPolarPointCloudMsg(apiHandle, msg, timeout_sec)) : SICK_SCAN_API_NOT_INITIALIZED);
    if (ret != SICK_SCAN_API_SUCCESS && ret != SICK_SCAN_API_TIMEOUT)
        printf("## ERROR SickScanApiWaitNextPolarPointCloudMsg: library call SickScanApiWaitNextPolarPointCloudMsg() failed, error code %d\n", ret);
    return ret;
}
int32_t SickScanApiFreePointCloudMsg(SickScanApiHandle apiHandle, SickScanPointCloudMsg* msg)
{
    CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiFreePointCloudMsg, "SickScanApiFreePointCloudMsg", SickScanApiFreePointCloudMsg_PROCTYPE);
    int32_t ret = (ptSickScanApiFreePointCloudMsg ? (ptSickScanApiFreePointCloudMsg(apiHandle, msg)) : SICK_SCAN_API_NOT_INITIALIZED);
    if (ret != SICK_SCAN_API_SUCCESS)
        printf("## ERROR SickScanApiFreePointCloudMsg: library call SickScanApiFreePointCloudMsg() failed, error code %d\n", ret);
    return ret;
}

// Wait for and return the next Imu messages. Note: SickScanApiWait...Msg() allocates a message. Use function SickScanApiFree...Msg() to deallocate it after use.
int32_t SickScanApiWaitNextImuMsg(SickScanApiHandle apiHandle, SickScanImuMsg* msg, double timeout_sec)
{
    CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiWaitNextImuMsg, "SickScanApiWaitNextImuMsg", SickScanApiWaitNextImuMsg_PROCTYPE);
    int32_t ret = (ptSickScanApiWaitNextImuMsg ? (ptSickScanApiWaitNextImuMsg(apiHandle, msg, timeout_sec)) : SICK_SCAN_API_NOT_INITIALIZED);
    if (ret != SICK_SCAN_API_SUCCESS && ret != SICK_SCAN_API_TIMEOUT)
        printf("## ERROR SickScanApiWaitNextImuMsg: library call SickScanApiWaitNextImuMsg() failed, error code %d\n", ret);
    return ret;
}
int32_t SickScanApiFreeImuMsg(SickScanApiHandle apiHandle, SickScanImuMsg* msg)
{
    CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiFreeImuMsg, "SickScanApiFreeImuMsg", SickScanApiFreeImuMsg_PROCTYPE);
    int32_t ret = (ptSickScanApiFreeImuMsg ? (ptSickScanApiFreeImuMsg(apiHandle, msg)) : SICK_SCAN_API_NOT_INITIALIZED);
    if (ret != SICK_SCAN_API_SUCCESS)
        printf("## ERROR SickScanApiFreeImuMsg: library call SickScanApiFreeImuMsg() failed, error code %d\n", ret);
    return ret;
}

// Wait for and return the next LFErec messages. Note: SickScanApiWait...Msg() allocates a message. Use function SickScanApiFree...Msg() to deallocate it after use.
int32_t SickScanApiWaitNextLFErecMsg(SickScanApiHandle apiHandle, SickScanLFErecMsg* msg, double timeout_sec)
{
    CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiWaitNextLFErecMsg, "SickScanApiWaitNextLFErecMsg", SickScanApiWaitNextLFErecMsg_PROCTYPE);
    int32_t ret = (ptSickScanApiWaitNextLFErecMsg ? (ptSickScanApiWaitNextLFErecMsg(apiHandle, msg, timeout_sec)) : SICK_SCAN_API_NOT_INITIALIZED);
    if (ret != SICK_SCAN_API_SUCCESS && ret != SICK_SCAN_API_TIMEOUT)
        printf("## ERROR SickScanApiWaitNextLFErecMsg: library call SickScanApiWaitNextLFErecMsg() failed, error code %d\n", ret);
    return ret;
}
int32_t SickScanApiFreeLFErecMsg(SickScanApiHandle apiHandle, SickScanLFErecMsg* msg)
{
    CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiFreeLFErecMsg, "SickScanApiFreeLFErecMsg", SickScanApiFreeLFErecMsg_PROCTYPE);
    int32_t ret = (ptSickScanApiFreeLFErecMsg ? (ptSickScanApiFreeLFErecMsg(apiHandle, msg)) : SICK_SCAN_API_NOT_INITIALIZED);
    if (ret != SICK_SCAN_API_SUCCESS)
        printf("## ERROR SickScanApiFreeLFErecMsg: library call SickScanApiFreeLFErecMsg() failed, error code %d\n", ret);
    return ret;
}

// Wait for and return the next LIDoutputstate messages. Note: SickScanApiWait...Msg() allocates a message. Use function SickScanApiFree...Msg() to deallocate it after use.
int32_t SickScanApiWaitNextLIDoutputstateMsg(SickScanApiHandle apiHandle, SickScanLIDoutputstateMsg* msg, double timeout_sec)
{
    CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiWaitNextLIDoutputstateMsg, "SickScanApiWaitNextLIDoutputstateMsg", SickScanApiWaitNextLIDoutputstateMsg_PROCTYPE);
    int32_t ret = (ptSickScanApiWaitNextLIDoutputstateMsg ? (ptSickScanApiWaitNextLIDoutputstateMsg(apiHandle, msg, timeout_sec)) : SICK_SCAN_API_NOT_INITIALIZED);
    if (ret != SICK_SCAN_API_SUCCESS && ret != SICK_SCAN_API_TIMEOUT)
        printf("## ERROR SickScanApiWaitNextLIDoutputstateMsg: library call SickScanApiWaitNextLIDoutputstateMsg() failed, error code %d\n", ret);
    return ret;
}
int32_t SickScanApiFreeLIDoutputstateMsg(SickScanApiHandle apiHandle, SickScanLIDoutputstateMsg* msg)
{
    CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiFreeLIDoutputstateMsg, "SickScanApiFreeLIDoutputstateMsg", SickScanApiFreeLIDoutputstateMsg_PROCTYPE);
    int32_t ret = (ptSickScanApiFreeLIDoutputstateMsg ? (ptSickScanApiFreeLIDoutputstateMsg(apiHandle, msg)) : SICK_SCAN_API_NOT_INITIALIZED);
    if (ret != SICK_SCAN_API_SUCCESS)
        printf("## ERROR SickScanApiFreeLIDoutputstateMsg: library call SickScanApiFreeLIDoutputstateMsg() failed, error code %d\n", ret);
    return ret;
}

// Wait for and return the next RadarScan messages. Note: SickScanApiWait...Msg() allocates a message. Use function SickScanApiFree...Msg() to deallocate it after use.
int32_t SickScanApiWaitNextRadarScanMsg(SickScanApiHandle apiHandle, SickScanRadarScan* msg, double timeout_sec)
{
    CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiWaitNextRadarScanMsg, "SickScanApiWaitNextRadarScanMsg", SickScanApiWaitNextRadarScanMsg_PROCTYPE);
    int32_t ret = (ptSickScanApiWaitNextRadarScanMsg ? (ptSickScanApiWaitNextRadarScanMsg(apiHandle, msg, timeout_sec)) : SICK_SCAN_API_NOT_INITIALIZED);
    if (ret != SICK_SCAN_API_SUCCESS && ret != SICK_SCAN_API_TIMEOUT)
        printf("## ERROR SickScanApiWaitNextRadarScanMsg: library call SickScanApiWaitNextRadarScanMsg() failed, error code %d\n", ret);
    return ret;
}
int32_t SickScanApiFreeRadarScanMsg(SickScanApiHandle apiHandle, SickScanRadarScan* msg)
{
    CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiFreeRadarScanMsg, "SickScanApiFreeRadarScanMsg", SickScanApiFreeRadarScanMsg_PROCTYPE);
    int32_t ret = (ptSickScanApiFreeRadarScanMsg ? (ptSickScanApiFreeRadarScanMsg(apiHandle, msg)) : SICK_SCAN_API_NOT_INITIALIZED);
    if (ret != SICK_SCAN_API_SUCCESS)
        printf("## ERROR SickScanApiFreeRadarScanMsg: library call SickScanApiFreeRadarScanMsg() failed, error code %d\n", ret);
    return ret;
}

// Wait for and return the next LdmrsObjectArray messages. Note: SickScanApiWait...Msg() allocates a message. Use function SickScanApiFree...Msg() to deallocate it after use.
int32_t SickScanApiWaitNextLdmrsObjectArrayMsg(SickScanApiHandle apiHandle, SickScanLdmrsObjectArray* msg, double timeout_sec)
{
    CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiWaitNextLdmrsObjectArrayMsg, "SickScanApiWaitNextLdmrsObjectArrayMsg", SickScanApiWaitNextLdmrsObjectArrayMsg_PROCTYPE);
    int32_t ret = (ptSickScanApiWaitNextLdmrsObjectArrayMsg ? (ptSickScanApiWaitNextLdmrsObjectArrayMsg(apiHandle, msg, timeout_sec)) : SICK_SCAN_API_NOT_INITIALIZED);
    if (ret != SICK_SCAN_API_SUCCESS && ret != SICK_SCAN_API_TIMEOUT)
        printf("## ERROR SickScanApiWaitNextLdmrsObjectArrayMsg: library call SickScanApiWaitNextLdmrsObjectArrayMsg() failed, error code %d\n", ret);
    return ret;
}
int32_t SickScanApiFreeLdmrsObjectArrayMsg(SickScanApiHandle apiHandle, SickScanLdmrsObjectArray* msg)
{
    CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiFreeLdmrsObjectArrayMsg, "SickScanApiFreeLdmrsObjectArrayMsg", SickScanApiFreeLdmrsObjectArrayMsg_PROCTYPE);
    int32_t ret = (ptSickScanApiFreeLdmrsObjectArrayMsg ? (ptSickScanApiFreeLdmrsObjectArrayMsg(apiHandle, msg)) : SICK_SCAN_API_NOT_INITIALIZED);
    if (ret != SICK_SCAN_API_SUCCESS)
        printf("## ERROR SickScanApiFreeLdmrsObjectArrayMsg: library call SickScanApiFreeLdmrsObjectArrayMsg() failed, error code %d\n", ret);
    return ret;
}

// Wait for and return the next VisualizationMarker message. Note: SickScanApiWait...Msg() allocates a message. Use function SickScanApiFree...Msg() to deallocate it after use.
int32_t SickScanApiWaitNextVisualizationMarkerMsg(SickScanApiHandle apiHandle, SickScanVisualizationMarkerMsg* msg, double timeout_sec)
{
    CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiWaitNextVisualizationMarkerMsg, "SickScanApiWaitNextVisualizationMarkerMsg", SickScanApiWaitNextVisualizationMarkerMsg_PROCTYPE);
    int32_t ret = (ptSickScanApiWaitNextVisualizationMarkerMsg ? (ptSickScanApiWaitNextVisualizationMarkerMsg(apiHandle, msg, timeout_sec)) : SICK_SCAN_API_NOT_INITIALIZED);
    if (ret != SICK_SCAN_API_SUCCESS && ret != SICK_SCAN_API_TIMEOUT)
        printf("## ERROR SickScanApiWaitNextVisualizationMarkerMsg: library call SickScanApiWaitNextVisualizationMarkerMsg() failed, error code %d\n", ret);
    return ret;
}
int32_t SickScanApiFreeVisualizationMarkerMsg(SickScanApiHandle apiHandle, SickScanVisualizationMarkerMsg* msg)
{
    CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiFreeVisualizationMarkersg, "SickScanApiFreeVisualizationMarkerMsg", SickScanApiFreeVisualizationMarkerMsg_PROCTYPE);
    int32_t ret = (ptSickScanApiFreeVisualizationMarkersg ? (ptSickScanApiFreeVisualizationMarkersg(apiHandle, msg)) : SICK_SCAN_API_NOT_INITIALIZED);
    if (ret != SICK_SCAN_API_SUCCESS)
        printf("## ERROR SickScanApiFreeVisualizationMarkerMsg: library call SickScanApiFreeVisualizationMarkerMsg() failed, error code %d\n", ret);
    return ret;
}

在源文件中新建cpp文件,代码如下:

这里我是通过TCP/IP的形式访问点云数据的,注意在代码中更改为你自己的IP与端口号

#include <chrono>
#include <iostream>
#include <thread>
#include <cassert>

#include "sick_scan_xd_api/sick_scan_api.h"

// Implement a callback to process pointcloud messages
void customizedPointCloudMsgCb(SickScanApiHandle apiHandle, const SickScanPointCloudMsg* msg)
{
	SickScanPointFieldMsg* msg_fields_buffer = (SickScanPointFieldMsg*)msg->fields.buffer;
	int field_offset_x = -1, field_offset_y = -1, field_offset_z = -1, field_offset_intensity = -1;
	for (int n = 0; n < msg->fields.size; n++)
	{
		if (strcmp(msg_fields_buffer[n].name, "x") == 0 && msg_fields_buffer[n].datatype == SICK_SCAN_POINTFIELD_DATATYPE_FLOAT32)
			field_offset_x = msg_fields_buffer[n].offset;
		else if (strcmp(msg_fields_buffer[n].name, "y") == 0 && msg_fields_buffer[n].datatype == SICK_SCAN_POINTFIELD_DATATYPE_FLOAT32)
			field_offset_y = msg_fields_buffer[n].offset;
		else if (strcmp(msg_fields_buffer[n].name, "z") == 0 && msg_fields_buffer[n].datatype == SICK_SCAN_POINTFIELD_DATATYPE_FLOAT32)
			field_offset_z = msg_fields_buffer[n].offset;
		else if (strcmp(msg_fields_buffer[n].name, "intensity") == 0 && msg_fields_buffer[n].datatype == SICK_SCAN_POINTFIELD_DATATYPE_FLOAT32)
			field_offset_intensity = msg_fields_buffer[n].offset;
	}
	assert(field_offset_x >= 0 && field_offset_y >= 0 && field_offset_z >= 0);
	for (int row_idx = 0; row_idx < (int)msg->height; row_idx++)
	{
		for (int col_idx = 0; col_idx < (int)msg->width; col_idx++)
		{
			// Get cartesian point coordinates
			int polar_point_offset = row_idx * msg->row_step + col_idx * msg->point_step;
			float point_x = (*((float*)(msg->data.buffer + polar_point_offset + field_offset_x)))*1000;
			float point_y = (*((float*)(msg->data.buffer + polar_point_offset + field_offset_y)))*1000;
			float point_z = (*((float*)(msg->data.buffer + polar_point_offset + field_offset_z)))*1000;
			std::cout << "--------------------------第" << col_idx << "点的坐标----------------------------" << std::endl;
			std::cout << "点云的坐标: x:" << int(point_x)<<" y:"<< int(point_y) << std::endl;
		}
	}
}

//void customized666(SickScanApiHandle apiHandle, const SickScanVisualizationMarkerMsg* msg)
//{
//	//std::cout << "x:: " << msg-> << " z:: " << msg->markers.buffer->pose_position.z << std::endl; // data processing to be done
//}

int main(int argc, char** argv)
{
	//输入参数
	char** argv_tmp;
	int argc_tmp;
	argc_tmp = argc;
	argv_tmp = argv;

	const int MAX_STR_LEN = 1024;
	char nameTagVal[MAX_STR_LEN] = { 0 };
	char internalDebugTagVal[MAX_STR_LEN] = { 0 };
	char sensorType[MAX_STR_LEN] = { 0 };
	char hostnameTag[MAX_STR_LEN] = { 0 };
	char sensorEmulVal[MAX_STR_LEN] = { 0 };
	char portTag[MAX_STR_LEN] = { 0 };
	argc_tmp = 5;
	argv_tmp = (char**)malloc(sizeof(char*) * argc_tmp);
	strcpy(sensorType, "scanner_type:=sick_tim_5xx");
	strcpy(hostnameTag, "hostname:=169.254.45.93");
	strcpy(portTag, "port:=2112");
	strcpy(internalDebugTagVal, "__internalDebug:=0");

	argv_tmp[0] = argv[0];
	argv_tmp[1] = sensorType;
	argv_tmp[2] = hostnameTag;
	argv_tmp[3] = portTag;
	argv_tmp[4] = internalDebugTagVal;



	 Create a sick_scan instance and initialize lidar with commandline arguments
	const char* sick_scan_api_lib = "sick_scan_shared_lib.dll";
	const char* launch_file = "sick_tim_5xx.launch";
	SickScanApiLoadLibrary(sick_scan_api_lib);
	SickScanApiHandle apiHandle = SickScanApiCreate(argc_tmp, argv_tmp);
	SickScanApiInitByCli(apiHandle, argc_tmp, argv_tmp);

	// Register for pointcloud messages
	//SickScanApiRegisterVisualizationMarkerMsg(apiHandle, &customized666);
	SickScanApiRegisterCartesianPointCloudMsg(apiHandle, &customizedPointCloudMsgCb);


	// Run application or main loop
	//getchar();
	std::this_thread::sleep_for(std::chrono::seconds(15));

	// Close lidar and release sick_scan api
	//SickScanApiDeregisterVisualizationMarkerMsg(apiHandle, &customized666);
	SickScanApiDeregisterCartesianPointCloudMsg(apiHandle, &customizedPointCloudMsgCb);
	SickScanApiClose(apiHandle);
	SickScanApiRelease(apiHandle);
	SickScanApiUnloadLibrary();
}

到这里点击运行就可以读取到点云数据啦!!!

  • 4
    点赞
  • 5
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值