Skip to content

Commit

Permalink
针对SCL增加强光滤波器功能
Browse files Browse the repository at this point in the history
  • Loading branch information
@zhanyiaini
zhanyiaini committed Jun 25, 2023
1 parent 5bf99a3 commit 33904a0
Show file tree
Hide file tree
Showing 6 changed files with 597 additions and 3 deletions.
2 changes: 1 addition & 1 deletion CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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})

##########################################################
Expand Down
362 changes: 362 additions & 0 deletions samples/scl_test.cpp
View file
Original file line number Diff line number Diff line change
@@ -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 <iostream>
#include <string>
#include <algorithm>
#include <cctype>
#include <core/base/timer.h>
#include <core/common/ydlidar_help.h>
#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<std::string, std::string> ports =
ydlidar::lidarPortList();
std::map<std::string, std::string>::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<int, int> 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<int, int>::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;
}
Loading

0 comments on commit 33904a0

Please sign in to comment.