diff --git a/CMakeLists.txt b/CMakeLists.txt index 9293fe5..af1e5fb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,7 +5,7 @@ project(ydlidar_sdk C CXX) # version set(YDLIDAR_SDK_VERSION_MAJOR 1) set(YDLIDAR_SDK_VERSION_MINOR 1) -set(YDLIDAR_SDK_VERSION_PATCH 13) +set(YDLIDAR_SDK_VERSION_PATCH 14) set(YDLIDAR_SDK_VERSION ${YDLIDAR_SDK_VERSION_MAJOR}.${YDLIDAR_SDK_VERSION_MINOR}.${YDLIDAR_SDK_VERSION_PATCH}) ########################################################## diff --git a/samples/scl_test.cpp b/samples/scl_test.cpp new file mode 100644 index 0000000..ca560b2 --- /dev/null +++ b/samples/scl_test.cpp @@ -0,0 +1,362 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, EAIBOT, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * 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. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 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 OWNER 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 "CYdLidar.h" +#include +#include +#include +#include +#include +#include +#include "filters/StrongLightFilter.h" + +using namespace std; +using namespace ydlidar; + +#if defined(_MSC_VER) +#pragma comment(lib, "ydlidar_sdk.lib") +#endif + +/** + * @brief ydlidar test + * @param argc + * @param argv + * @return + * @par Flow chart + * Step1: instance CYdLidar.\n + * Step2: set paramters.\n + * Step3: initialize SDK and LiDAR.(::CYdLidar::initialize)\n + * Step4: Start the device scanning routine which runs on a separate thread and enable motor.(::CYdLidar::turnOn)\n + * Step5: Get the LiDAR Scan Data.(::CYdLidar::doProcessSimple)\n + * Step6: Stop the device scanning thread and disable motor.(::CYdLidar::turnOff)\n + * Step7: Uninitialize the SDK and Disconnect the LiDAR.(::CYdLidar::disconnecting)\n + */ + +int main(int argc, char *argv[]) +{ + printf("__ ______ _ ___ ____ _ ____ \n"); + printf("\\ \\ / / _ \\| | |_ _| _ \\ / \\ | _ \\ \n"); + printf(" \\ V /| | | | | | || | | |/ _ \\ | |_) | \n"); + printf(" | | | |_| | |___ | || |_| / ___ \\| _ < \n"); + printf(" |_| |____/|_____|___|____/_/ \\_\\_| \\_\\ \n"); + printf("\n"); + fflush(stdout); + std::string port; + ydlidar::os_init(); + + std::map ports = + ydlidar::lidarPortList(); + std::map::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()) + { + printf("Not Lidar was detected. Please enter the lidar serial port:"); + std::cin >> port; + } + else + { + while (ydlidar::os_isOk()) + { + printf("Please select the lidar port:"); + std::string number; + std::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; + std::map baudrateList; + baudrateList[0] = 115200; + baudrateList[1] = 128000; + baudrateList[2] = 150000; + baudrateList[3] = 153600; + baudrateList[4] = 230400; + baudrateList[5] = 460800; + baudrateList[6] = 512000; + + printf("Baudrate:\n"); + + for (std::map::iterator it = baudrateList.begin(); + it != baudrateList.end(); it++) + { + printf("[%d] %d\n", it->first, it->second); + } + + while (ydlidar::os_isOk()) + { + printf("Please select the lidar baudrate:"); + std::string number; + std::cin >> number; + + if ((size_t)atoi(number.c_str()) > baudrateList.size()) + { + continue; + } + + baudrate = baudrateList[atoi(number.c_str())]; + break; + } + + if (!ydlidar::os_isOk()) + { + return 0; + } + + bool isSingleChannel = false; + std::string input_channel; + printf("Whether the Lidar is one-way communication [yes/no]:"); + std::cin >> input_channel; + std::transform(input_channel.begin(), input_channel.end(), + input_channel.begin(), + [](unsigned char c) + { + return std::tolower(c); // correct + }); + + if (input_channel.find("y") != std::string::npos) + { + isSingleChannel = true; + } + + if (!ydlidar::os_isOk()) + { + return 0; + } + + std::string input_frequency; + + float frequency = 5.0; + + while (ydlidar::os_isOk() && !isSingleChannel) + { + printf("Please enter the lidar scan frequency[5-12]:"); + std::cin >> input_frequency; + frequency = atof(input_frequency.c_str()); + + if (frequency <= 12 && frequency >= 5.0) + { + break; + } + + fprintf(stderr, "Invalid scan frequency," + "The scanning frequency range is 5 to 12 HZ, Please re-enter.\n"); + } + + if (!ydlidar::os_isOk()) + { + return 0; + } + + CYdLidar laser; + //////////////////////string property///////////////// + /// lidar port + laser.setlidaropt(LidarPropSerialPort, port.c_str(), port.size()); + /// ignore array + std::string ignore_array; + ignore_array.clear(); + laser.setlidaropt(LidarPropIgnoreArray, ignore_array.c_str(), + ignore_array.size()); + + //////////////////////int property///////////////// + /// lidar baudrate + laser.setlidaropt(LidarPropSerialBaudrate, &baudrate, sizeof(int)); + /// tof lidar + int optval = TYPE_SCL; + laser.setlidaropt(LidarPropLidarType, &optval, sizeof(int)); + /// device type + optval = YDLIDAR_TYPE_SERIAL; + laser.setlidaropt(LidarPropDeviceType, &optval, sizeof(int)); + /// sample rate + optval = isSingleChannel ? 3 : 4; + laser.setlidaropt(LidarPropSampleRate, &optval, sizeof(int)); + /// abnormal count + optval = 4; + laser.setlidaropt(LidarPropAbnormalCheckCount, &optval, sizeof(int)); + /// Intenstiy bit count + optval = 10; + laser.setlidaropt(LidarPropIntenstiyBit, &optval, sizeof(int)); + + //////////////////////bool property///////////////// + /// fixed angle resolution + bool b_optvalue = true; + laser.setlidaropt(LidarPropFixedResolution, &b_optvalue, sizeof(bool)); + /// rotate 180 + laser.setlidaropt(LidarPropReversion, &b_optvalue, sizeof(bool)); + /// Counterclockwise + laser.setlidaropt(LidarPropInverted, &b_optvalue, sizeof(bool)); + b_optvalue = true; + laser.setlidaropt(LidarPropAutoReconnect, &b_optvalue, sizeof(bool)); + /// one-way communication + laser.setlidaropt(LidarPropSingleChannel, &isSingleChannel, sizeof(bool)); + /// intensity + b_optvalue = false; + laser.setlidaropt(LidarPropIntenstiy, &b_optvalue, sizeof(bool)); + /// Motor DTR + b_optvalue = false; + laser.setlidaropt(LidarPropSupportMotorDtrCtrl, &b_optvalue, sizeof(bool)); + /// HeartBeat + b_optvalue = false; + laser.setlidaropt(LidarPropSupportHeartBeat, &b_optvalue, sizeof(bool)); + + //////////////////////float property///////////////// + /// 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); + + uint32_t t = getms(); //时间 + int c = 0; //计数 + + bool ret = laser.initialize(); + if (!ret) + { + fprintf(stderr, "Fail to initialize %s\n", laser.DescribeError()); + fflush(stderr); + return -1; + } + + ret = laser.turnOn(); + if (!ret) + { + fprintf(stderr, "Fail to start %s\n", laser.DescribeError()); + fflush(stderr); + return -1; + } + + //获取用户版本 + // if (ret && ydlidar::os_isOk()) + // { + // std::string userVersion; + // if (laser.getUserVersion(userVersion)) + // { + // printf("User version %s\n", userVersion.c_str()); + // } + // } + + //获取设备信息 + // if (ret) + // { + // device_info di; + // memset(&di, 0, DEVICEINFOSIZE); + // if (!laser.getDeviceInfo(di)) { + // ydlidar::core::common::printfVersionInfo(di, "", 0); + // } + // else { + // printf("Fail to get device info\n"); + // } + // } + + LaserScan scan; + LaserScan outScan; + StrongLightFilter filter; //强光滤波器 + + while (ydlidar::os_isOk()) + { + if (laser.doProcessSimple(scan)) + { + printf("Scan received [%llu] points scanFreq [%.02f]\n", + scan.points.size(), + scan.scanFreq); + // for (size_t i = 0; i < scan.points.size(); ++i) + // { + // const LaserPoint &p = scan.points.at(i); + // printf("%d d %.05f a %.02f\n", i, p.range, p.angle * 180.0 / M_PI); + // } + //使用强光滤波器 + filter.filter(scan, 0, 0, outScan); + + fflush(stdout); + } + else + { + fprintf(stderr, "Failed to get Lidar Data\n"); + fflush(stderr); + } + if (!c++) + { + printf("Time consuming [%u] from initialization to parsing to point cloud data", + getms() - t); + } + } + + laser.turnOff(); + laser.disconnecting(); + + return 0; +} diff --git a/samples/tri_test.cpp b/samples/tri_test.cpp index 72a47af..d13a5c7 100644 --- a/samples/tri_test.cpp +++ b/samples/tri_test.cpp @@ -221,7 +221,7 @@ int main(int argc, char *argv[]) /// lidar baudrate laser.setlidaropt(LidarPropSerialBaudrate, &baudrate, sizeof(int)); /// tof lidar - int optval = TYPE_SCL; + int optval = TYPE_TRIANGLE; laser.setlidaropt(LidarPropLidarType, &optval, sizeof(int)); /// device type optval = YDLIDAR_TYPE_SERIAL; @@ -279,6 +279,9 @@ int main(int argc, char *argv[]) //设置是否底板优先 laser.setBottomPriority(true); + uint32_t t = getms(); //时间 + int c = 0; //计数 + bool ret = laser.initialize(); if (!ret) { @@ -339,6 +342,11 @@ int main(int argc, char *argv[]) fprintf(stderr, "Failed to get Lidar Data\n"); fflush(stderr); } + if (!c++) + { + printf("Time consuming [%u] from initialization to parsing to point cloud data", + getms() - t); + } } laser.turnOff(); diff --git a/src/filters/FilterInterface.h b/src/filters/FilterInterface.h index cc700b0..4b9b288 100644 --- a/src/filters/FilterInterface.h +++ b/src/filters/FilterInterface.h @@ -9,6 +9,7 @@ class FilterInterface public: FilterInterface() {} virtual ~FilterInterface() {} + virtual void filter(const LaserScan &in, int lidarType, int version, @@ -16,7 +17,9 @@ class FilterInterface virtual std::string name() const { return m_name; } - virtual std::string version() const = 0; + virtual std::string version() const { + return "V1.0"; + } virtual void setName(const std::string &name) { m_name = name; } diff --git a/src/filters/StrongLightFilter.cpp b/src/filters/StrongLightFilter.cpp new file mode 100644 index 0000000..97e6dc4 --- /dev/null +++ b/src/filters/StrongLightFilter.cpp @@ -0,0 +1,186 @@ +#include +#include +#include "core/math/angles.h" +#include "StrongLightFilter.h" + +#define MAX_DIST 0.1f +#define MIN_NOISE 1 +#define MIN_VALUE 1e-8 +#define IS_ZERO(d) (abs(d) < MIN_VALUE) +#define IS_EQUAL(d1, d2) IS_ZERO(d1 - d2) + + +StrongLightFilter::StrongLightFilter() +{ +} + +StrongLightFilter::~StrongLightFilter() +{ +} + +void StrongLightFilter::filter( + const LaserScan &in, + int lidarType, + int version, + LaserScan &out) +{ + //转 + int size = in.points.size(); //点数 + //按角度排序 + std::map ais; + for (int i=0; i in.config.max_range) + continue; + if (IS_ZERO(p.range)) + continue; + ais[p.angle] = i; + } + + // printf("按角度排序从[%d]点到[%d]点\n", size, ais.size()); + + size = ais.size(); //更新点数(过滤无效点后的点数) + out = in; + out.points.resize(size); //更新点数 + std::map::iterator it; + int i = 0; + for (it = ais.begin(); it != ais.end(); ++it) + { + out.points[i++] = in.points.at(it->second); + } + //初始化变量 + std::vector noises(size, false); //是否为噪点的标记 + //将遍历范围扩大到原数组的104%以便处理首尾部分的点 + int sizeEx = int(size * 1.04); + int startI = -1; //标记拖尾起始点下标位置 + LaserPoint lastP; //上一个点信息 + + //主循环函数 + for (int i = 0; i < sizeEx; ++i) + { + const LaserPoint& p = out.points.at(i % size); + if (i != 0) + { + //计算两点连线到原点的距离(直角坐标系) + Point p1 = Point::polar2Angular(Point(p.angle, p.range)); + Point p2 = Point::polar2Angular(Point(lastP.angle, lastP.range)); + float d = Point::calcDist(Point(0, 0), p1, p2); + + // printf("点[%d]距离[%.03f]\n", i % size, d); + //如果当前距离小于标准则认为是拖尾点 + if (d < MAX_DIST) + { + //如果起始点无效则标记 + if (-1 == startI) + startI = i; + } + //如果点的距离是在增加的,则当前距离小于2倍标准也认为是拖尾点 + else if (-1 != startI && + p.range > lastP.range && + d < MAX_DIST * 2) + { + //无处理 + } + else + { + //判断统计位置是否有效,如果有效需要标记 + if (-1 != startI && + i - startI >= MIN_NOISE) + { + for (int j=startI; j<=i; ++j) + { + noises[j % size] = true; + const LaserPoint& pp = out.points.at(j % size); + // printf("噪点[%d] a[%.02f] r[%.02f]\n", + // j % size, ydlidar::core::math::to_degrees(pp.angle), pp.range); + } + } + + startI = -1; + } + } + lastP = p; + } + + //处理被标记的点 + int noiseCount = 0; + for (int i = 0; i < size; ++i) + { + if (noises[i]) + { + out.points[i].range = 0.0f; + noiseCount ++; + } + } + + // printf("强光过滤噪点数[%d]\n", noiseCount); +} + +StrongLightFilter::Point::Point(float x, float y) + : x(x), + y(y) +{ +} + +StrongLightFilter::Point StrongLightFilter::Point::angular2Polar( + const StrongLightFilter::Point &p) +{ + //1.极坐标系中的两个坐标 r 和 θ 可以由下面的公式转换为直角坐标系下的坐标值x = r*cos(θ),y = r*sin(θ)。 + //2.由上述二公式,可得到从直角坐标系中x和y两坐标如何计算出极坐标下的坐标,r = sqrt(x^2 + y^2),θ = arctan(y/x) +// float r = qSqrt(x * x + y * y); +// float theta = qAtan(y / x); + + //将弧度值从[-M_PI/2,M_PI/2]转成[0, 2*M_PI] + float theta = .0; + if (!IS_ZERO(p.x)) //x不为0时 + { + theta = atan(p.y / p.x); + if (p.x > 0.0) + { + if (p.y < 0.0) + theta += (M_PI * 2.0); + } + else + { + theta += M_PI; + } + } + return Point(theta, sqrt(p.x * p.x + p.y * p.y)); +} + +StrongLightFilter::Point StrongLightFilter::Point::polar2Angular( + const StrongLightFilter::Point &p) +{ + return Point(p.y * cos(p.x), p.y * sin(p.x)); +} + +float StrongLightFilter::Point::calcDist( + const StrongLightFilter::Point &p, + const StrongLightFilter::Point &p1, + const StrongLightFilter::Point &p2) +{ + //计算点到直线的最近距离 + //输入点P(x0,y0)和直线AB(x1,y1,x2,y2),输出点到直线的最近距离 + //# 如果两点相同,则输出一个点的坐标为垂足 + //if x1 == x2 and y1 == y2: + // return math.sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0)) + if (IS_EQUAL(p1.x, p2.x) && + IS_EQUAL(p1.y, p2.y)) + return sqrt((p1.x - p.x) * (p1.x - p.x) + + (p1.y - p.y) * (p1.y - p.y)); + + //# 根据向量外积计算面积 + //s = (x0 - x1) * (y2 - y1) - (y0 - y1) * (x2 - x1) + float s = (p.x - p1.x) * (p2.y - p1.y) - + (p.y - p1.y) * (p2.x - p1.x); + //# 计算直线上两点之间的距离 + //d = math.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2) + float d = sqrt((p2.x - p1.x) * (p2.x - p1.x) + + (p2.y - p1.y) * (p2.y - p1.y)); + + //return math.fabs(s / d) + return fabs(s / d); +} diff --git a/src/filters/StrongLightFilter.h b/src/filters/StrongLightFilter.h new file mode 100644 index 0000000..bfd9990 --- /dev/null +++ b/src/filters/StrongLightFilter.h @@ -0,0 +1,35 @@ +#ifndef STRONGLIGHTFILTER_H +#define STRONGLIGHTFILTER_H +#include "FilterInterface.h" + + +class StrongLightFilter : public FilterInterface +{ +public: + StrongLightFilter(); + virtual ~StrongLightFilter(); + + virtual void filter(const LaserScan &in, + int lidarType, + int version, + LaserScan &out); + +protected: + struct Point + { + float x = .0; + float y = .0; + + Point(float x = .0, float y = .0); + + static Point angular2Polar(const Point &p); // 直角坐标转极坐标 + static Point polar2Angular(const Point &p); // 极坐标转直角坐标 + // 计算直角坐标系中点到直线的距离 + static float calcDist( + const Point &p, + const Point &p1, + const Point &p2); + }; +}; + +#endif // STRONGLIGHTFILTER_H