From dcd2dd87ce8034a46e4150a89dc69f6ba1a22d8d Mon Sep 17 00:00:00 2001 From: Edvin Date: Thu, 10 Apr 2025 11:35:25 +0200 Subject: Implemented code for Slamtec rplidar C1 --- LIDAR/sdk/src/rplidar_driver.cpp | 199 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 199 insertions(+) create mode 100644 LIDAR/sdk/src/rplidar_driver.cpp (limited to 'LIDAR/sdk/src/rplidar_driver.cpp') diff --git a/LIDAR/sdk/src/rplidar_driver.cpp b/LIDAR/sdk/src/rplidar_driver.cpp new file mode 100644 index 0000000..beedd39 --- /dev/null +++ b/LIDAR/sdk/src/rplidar_driver.cpp @@ -0,0 +1,199 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include "sdkcommon.h" +#include "hal/abs_rxtx.h" +#include "hal/thread.h" +#include "hal/types.h" +#include "hal/assert.h" +#include "hal/locker.h" +#include "hal/socket.h" +#include "hal/event.h" +#include "rplidar_driver.h" +#include "sl_crc.h" +#include + +namespace rp { namespace standalone{ namespace rplidar { + + RPlidarDriver::RPlidarDriver(){} + + RPlidarDriver::RPlidarDriver(sl_u32 channelType) + :_channelType(channelType) + { + } + + RPlidarDriver::~RPlidarDriver() {} + + RPlidarDriver * RPlidarDriver::CreateDriver(_u32 drivertype) + { + //_channelType = drivertype; + return new RPlidarDriver(drivertype); + } + + void RPlidarDriver::DisposeDriver(RPlidarDriver * drv) + { + delete drv; + } + + u_result RPlidarDriver::connect(const char *path, _u32 portOrBaud, _u32 flag) + { + switch (_channelType) + { + case CHANNEL_TYPE_SERIALPORT: + _channel = (*createSerialPortChannel(path, portOrBaud)); + break; + case CHANNEL_TYPE_TCP: + _channel = *createTcpChannel(path, portOrBaud); + break; + case CHANNEL_TYPE_UDP: + _channel = *createUdpChannel(path, portOrBaud); + break; + } + if (!(bool)_channel) return SL_RESULT_OPERATION_FAIL; + + _lidarDrv = *createLidarDriver(); + + if (!(bool)_lidarDrv) return SL_RESULT_OPERATION_FAIL; + + sl_result ans =(_lidarDrv)->connect(_channel); + return ans; + } + + void RPlidarDriver::disconnect() + { + (_lidarDrv)->disconnect(); + } + + bool RPlidarDriver::isConnected() + { + return (_lidarDrv)->isConnected(); + } + + u_result RPlidarDriver::reset(_u32 timeout) + { + return (_lidarDrv)->reset(); + } + + u_result RPlidarDriver::getAllSupportedScanModes(std::vector& outModes, _u32 timeoutInMs) + { + return (_lidarDrv)->getAllSupportedScanModes(outModes, timeoutInMs); + } + + u_result RPlidarDriver::getTypicalScanMode(_u16& outMode, _u32 timeoutInMs) + { + return (_lidarDrv)->getTypicalScanMode(outMode, timeoutInMs); + } + + u_result RPlidarDriver::startScan(bool force, bool useTypicalScan, _u32 options, RplidarScanMode* outUsedScanMode) + { + return (_lidarDrv)->startScan(force, useTypicalScan, options, outUsedScanMode); + } + + u_result RPlidarDriver::startScanExpress(bool force, _u16 scanMode, _u32 options, RplidarScanMode* outUsedScanMode, _u32 timeout) + { + return (_lidarDrv)->startScanExpress(force, scanMode, options, outUsedScanMode, timeout); + } + + u_result RPlidarDriver::getHealth(rplidar_response_device_health_t & health, _u32 timeout) + { + return (_lidarDrv)->getHealth(health, timeout); + } + + u_result RPlidarDriver::getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout) + { + return (_lidarDrv)->getDeviceInfo(info, timeout); + } + + u_result RPlidarDriver::setMotorPWM(_u16 pwm) + { + return (_lidarDrv)->setMotorSpeed(pwm); + } + + u_result RPlidarDriver::checkMotorCtrlSupport(bool & support, _u32 timeout) + { + MotorCtrlSupport motorSupport; + u_result ans = (_lidarDrv)->checkMotorCtrlSupport(motorSupport, timeout); + if (motorSupport == MotorCtrlSupportNone) + support = false; + return ans; + } + + u_result RPlidarDriver::setLidarIpConf(const rplidar_ip_conf_t& conf, _u32 timeout) + { + return (_lidarDrv)->setLidarIpConf(conf, timeout); + } + + u_result RPlidarDriver::getLidarIpConf(rplidar_ip_conf_t& conf, _u32 timeout) + { + return (_lidarDrv)->getLidarIpConf(conf, timeout); + } + + u_result RPlidarDriver::getDeviceMacAddr(_u8* macAddrArray, _u32 timeoutInMs) + { + return (_lidarDrv)->getDeviceMacAddr(macAddrArray, timeoutInMs); + } + + u_result RPlidarDriver::stop(_u32 timeout) + { + return (_lidarDrv)->stop(timeout); + } + + u_result RPlidarDriver::grabScanDataHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count, _u32 timeout) + { + return (_lidarDrv)->grabScanDataHq(nodebuffer, count, timeout); + } + + u_result RPlidarDriver::ascendScanData(rplidar_response_measurement_node_hq_t * nodebuffer, size_t count) + { + return (_lidarDrv)->ascendScanData(nodebuffer, count); + } + + u_result RPlidarDriver::getScanDataWithInterval(rplidar_response_measurement_node_t * nodebuffer, size_t & count) + { + return RESULT_OPERATION_NOT_SUPPORT; + } + + u_result RPlidarDriver::getScanDataWithIntervalHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count) + { + return (_lidarDrv)->getScanDataWithIntervalHq(nodebuffer, count); + } + + u_result RPlidarDriver::startMotor() + { + return (_lidarDrv)->setMotorSpeed(DEFAULT_MOTOR_SPEED); + } + u_result RPlidarDriver::stopMotor() + { + return (_lidarDrv)->setMotorSpeed(0); + } + +}}} -- cgit v1.2.3