← Back to ratslair.com
aboutsummaryrefslogtreecommitdiff
path: root/LIDAR/LIDAR.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'LIDAR/LIDAR.cpp')
-rw-r--r--LIDAR/LIDAR.cpp303
1 files changed, 303 insertions, 0 deletions
diff --git a/LIDAR/LIDAR.cpp b/LIDAR/LIDAR.cpp
new file mode 100644
index 0000000..aa67cf8
--- /dev/null
+++ b/LIDAR/LIDAR.cpp
@@ -0,0 +1,303 @@
+/*
+ * SLAMTEC LIDAR
+ * Ultra Simple Data Grabber Demo App
+ *
+ * Copyright (c) 2009 - 2014 RoboPeak Team
+ * http://www.robopeak.com
+ * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd.
+ * http://www.slamtec.com
+ *
+ */
+/*
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <signal.h>
+#include <string.h>
+#include <cmath>
+
+#include "sl_lidar.h"
+#include "sl_lidar_driver.h"
+#ifndef _countof
+#define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0]))
+#endif
+
+#ifdef _WIN32
+#include <Windows.h>
+#define delay(x) ::Sleep(x)
+#else
+#include <unistd.h>
+static inline void delay(sl_word_size_t ms){
+ while (ms>=1000){
+ usleep(1000*1000);
+ ms-=1000;
+ };
+ if (ms!=0)
+ usleep(ms*1000);
+}
+#endif
+
+using namespace sl;
+
+void print_usage(int argc, const char * argv[])
+{
+ printf("Usage:\n"
+ " For serial channel\n %s --channel --serial <com port> [baudrate]\n"
+ " The baudrate used by different models is as follows:\n"
+ " A1(115200),A2M7(256000),A2M8(115200),A2M12(256000),"
+ "A3(256000),S1(256000),S2(1000000),S3(1000000),C1(460800)\n"
+ " For udp channel\n %s --channel --udp <ipaddr> [port NO.]\n"
+ " The T1 default ipaddr is 192.168.11.2,and the port NO.is 8089. Please refer to the datasheet for details.\n"
+ , argv[0], argv[0]);
+}
+
+bool checkSLAMTECLIDARHealth(ILidarDriver * drv)
+{
+ sl_result op_result;
+ sl_lidar_response_device_health_t healthinfo;
+
+ op_result = drv->getHealth(healthinfo);
+ if (SL_IS_OK(op_result)) { // the macro IS_OK is the preperred way to judge whether the operation is succeed.
+ printf("SLAMTEC Lidar health status : %d\n", healthinfo.status);
+ if (healthinfo.status == SL_LIDAR_STATUS_ERROR) {
+ fprintf(stderr, "Error, slamtec lidar internal error detected. Please reboot the device to retry.\n");
+ // enable the following code if you want slamtec lidar to be reboot by software
+ // drv->reset();
+ return false;
+ } else {
+ return true;
+ }
+
+ } else {
+ fprintf(stderr, "Error, cannot retrieve the lidar health code: %x\n", op_result);
+ return false;
+ }
+}
+
+bool ctrl_c_pressed;
+void ctrlc(int)
+{
+ ctrl_c_pressed = true;
+}
+
+int main(int argc, const char * argv[]) {
+ const char * opt_is_channel = NULL;
+ const char * opt_channel = NULL;
+ const char * opt_channel_param_first = NULL;
+ sl_u32 opt_channel_param_second = 0;
+ sl_u32 baudrateArray[2] = {115200, 256000};
+ sl_result op_result;
+ int opt_channel_type = CHANNEL_TYPE_SERIALPORT;
+
+ bool useArgcBaudrate = false;
+
+ IChannel* _channel;
+
+ printf("Ultra simple LIDAR data grabber for SLAMTEC LIDAR.\n"
+ "Version: %s\n", SL_LIDAR_SDK_VERSION);
+
+
+ if (argc>1)
+ {
+ opt_is_channel = argv[1];
+ }
+ else
+ {
+ print_usage(argc, argv);
+ return -1;
+ }
+
+ if(strcmp(opt_is_channel, "--channel")==0){
+ opt_channel = argv[2];
+ if(strcmp(opt_channel, "-s")==0||strcmp(opt_channel, "--serial")==0)
+ {
+ // read serial port from the command line...
+ opt_channel_param_first = argv[3];// or set to a fixed value: e.g. "com3"
+ // read baud rate from the command line if specified...
+ if (argc>4) opt_channel_param_second = strtoul(argv[4], NULL, 10);
+ useArgcBaudrate = true;
+ }
+ else if(strcmp(opt_channel, "-u")==0||strcmp(opt_channel, "--udp")==0)
+ {
+ // read ip addr from the command line...
+ opt_channel_param_first = argv[3];//or set to a fixed value: e.g. "192.168.11.2"
+ if (argc>4) opt_channel_param_second = strtoul(argv[4], NULL, 10);//e.g. "8089"
+ opt_channel_type = CHANNEL_TYPE_UDP;
+ }
+ else
+ {
+ print_usage(argc, argv);
+ return -1;
+ }
+ }
+ else
+ {
+ print_usage(argc, argv);
+ return -1;
+ }
+
+ if(opt_channel_type == CHANNEL_TYPE_SERIALPORT)
+ {
+ if (!opt_channel_param_first) {
+#ifdef _WIN32
+ // use default com port
+ opt_channel_param_first = "\\\\.\\com3";
+#elif __APPLE__
+ opt_channel_param_first = "/dev/tty.SLAB_USBtoUART";
+#else
+ opt_channel_param_first = "/dev/ttyUSB0";
+#endif
+ }
+ }
+
+
+ // create the driver instance
+ ILidarDriver * drv = *createLidarDriver();
+
+ if (!drv) {
+ fprintf(stderr, "insufficent memory, exit\n");
+ exit(-2);
+ }
+
+ sl_lidar_response_device_info_t devinfo;
+ bool connectSuccess = false;
+
+ if(opt_channel_type == CHANNEL_TYPE_SERIALPORT){
+ if(useArgcBaudrate){
+ _channel = (*createSerialPortChannel(opt_channel_param_first, opt_channel_param_second));
+ if (SL_IS_OK((drv)->connect(_channel))) {
+ op_result = drv->getDeviceInfo(devinfo);
+
+ if (SL_IS_OK(op_result))
+ {
+ connectSuccess = true;
+ }
+ else{
+ delete drv;
+ drv = NULL;
+ }
+ }
+ }
+ else{
+ size_t baudRateArraySize = (sizeof(baudrateArray))/ (sizeof(baudrateArray[0]));
+ for(size_t i = 0; i < baudRateArraySize; ++i)
+ {
+ _channel = (*createSerialPortChannel(opt_channel_param_first, baudrateArray[i]));
+ if (SL_IS_OK((drv)->connect(_channel))) {
+ op_result = drv->getDeviceInfo(devinfo);
+
+ if (SL_IS_OK(op_result))
+ {
+ connectSuccess = true;
+ break;
+ }
+ else{
+ delete drv;
+ drv = NULL;
+ }
+ }
+ }
+ }
+ }
+ else if(opt_channel_type == CHANNEL_TYPE_UDP){
+ _channel = *createUdpChannel(opt_channel_param_first, opt_channel_param_second);
+ if (SL_IS_OK((drv)->connect(_channel))) {
+ op_result = drv->getDeviceInfo(devinfo);
+
+ if (SL_IS_OK(op_result))
+ {
+ connectSuccess = true;
+ }
+ else{
+ delete drv;
+ drv = NULL;
+ }
+ }
+ }
+
+
+ if (!connectSuccess) {
+ (opt_channel_type == CHANNEL_TYPE_SERIALPORT)?
+ (fprintf(stderr, "Error, cannot bind to the specified serial port %s.\n"
+ , opt_channel_param_first)):(fprintf(stderr, "Error, cannot connect to the specified ip addr %s.\n"
+ , opt_channel_param_first));
+
+ goto on_finished;
+ }
+
+ // print out the device serial number, firmware and hardware version number..
+ printf("SLAMTEC LIDAR S/N: ");
+ for (int pos = 0; pos < 16 ;++pos) {
+ printf("%02X", devinfo.serialnum[pos]);
+ }
+
+ printf("\n"
+ "Firmware Ver: %d.%02d\n"
+ "Hardware Rev: %d\n"
+ , devinfo.firmware_version>>8
+ , devinfo.firmware_version & 0xFF
+ , (int)devinfo.hardware_version);
+
+
+
+ // check health...
+ if (!checkSLAMTECLIDARHealth(drv)) {
+ goto on_finished;
+ }
+
+ signal(SIGINT, ctrlc);
+
+ if(opt_channel_type == CHANNEL_TYPE_SERIALPORT)
+ drv->setMotorSpeed();
+ // start scan...
+ drv->startScan(0,1);
+
+ // fetech result and print it out...
+ while (1) {
+ sl_lidar_response_measurement_node_hq_t nodes[8192];
+ size_t count = _countof(nodes);
+
+ op_result = drv->grabScanDataHq(nodes, count);
+
+ if (SL_IS_OK(op_result)) {
+ drv->ascendScanData(nodes, count);
+ for (int pos = 0; pos < (int)count ; ++pos) {
+ int angle = std::floor((nodes[pos].angle_z_q14 * 90.f) / 16384.f);
+ float dist = nodes[pos].dist_mm_q2/4.0f;
+ if (dist > 0.0) {
+ printf("Angle: %i, Dist: %f\n", angle, dist);
+ }
+ }
+ }
+
+ if (ctrl_c_pressed){
+ break;
+ }
+ }
+
+ drv->stop();
+ delay(200);
+ if(opt_channel_type == CHANNEL_TYPE_SERIALPORT)
+ drv->setMotorSpeed(0);
+ // done!
+on_finished:
+ if(drv) {
+ delete drv;
+ drv = NULL;
+ }
+ return 0;
+}
generated by cgit v1.2.3 (git 2.46.0) at 2026-09-27 08:28:04 +0000