Skip to content

Commit

Permalink
针对Tmini Pro系列屏蔽自动识别强度标识
Browse files Browse the repository at this point in the history
  • Loading branch information
@zhanyiaini
zhanyiaini committed Apr 24, 2023
1 parent 05b39d4 commit 6e7e166
Show file tree
Hide file tree
Showing 5 changed files with 221 additions and 73 deletions.
4 changes: 2 additions & 2 deletions python/examples/ydlidar_test.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,11 +11,11 @@
print(port);
laser = ydlidar.CYdLidar();
laser.setlidaropt(ydlidar.LidarPropSerialPort, port);
laser.setlidaropt(ydlidar.LidarPropSerialBaudrate, 128000);
laser.setlidaropt(ydlidar.LidarPropSerialBaudrate, 115200);
laser.setlidaropt(ydlidar.LidarPropLidarType, ydlidar.TYPE_TRIANGLE);
laser.setlidaropt(ydlidar.LidarPropDeviceType, ydlidar.YDLIDAR_TYPE_SERIAL);
laser.setlidaropt(ydlidar.LidarPropScanFrequency, 10.0);
laser.setlidaropt(ydlidar.LidarPropSampleRate, 5);
laser.setlidaropt(ydlidar.LidarPropSampleRate, 3);
laser.setlidaropt(ydlidar.LidarPropSingleChannel, True);
laser.setlidaropt(ydlidar.LidarPropMaxAngle, 180.0);
laser.setlidaropt(ydlidar.LidarPropMinAngle, -180.0);
Expand Down
115 changes: 57 additions & 58 deletions samples/tmini_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -111,72 +111,70 @@ int main(int argc, char *argv[]) {
}

int baudrate = 230400;
std::map<int, int> baudrateList;
baudrateList[0] = 115200;
baudrateList[1] = 128000;
baudrateList[2] = 153600;
baudrateList[3] = 230400;
baudrateList[4] = 460800;
baudrateList[5] = 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;
}
// std::map<int, int> baudrateList;
// baudrateList[0] = 115200;
// baudrateList[1] = 128000;
// baudrateList[2] = 153600;
// baudrateList[3] = 230400;
// baudrateList[4] = 460800;
// baudrateList[5] = 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_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 = 8.0;
float frequency = 4.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");
}
// 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;
Expand Down Expand Up @@ -213,8 +211,9 @@ int main(int argc, char *argv[]) {

//////////////////////bool property/////////////////
/// fixed angle resolution
bool b_optvalue = false;
bool b_optvalue = true;
laser.setlidaropt(LidarPropFixedResolution, &b_optvalue, sizeof(bool));
b_optvalue = false;
/// rotate 180
laser.setlidaropt(LidarPropReversion, &b_optvalue, sizeof(bool));
/// Counterclockwise
Expand All @@ -227,7 +226,7 @@ int main(int argc, char *argv[]) {
b_optvalue = true;
laser.setlidaropt(LidarPropIntenstiy, &b_optvalue, sizeof(bool));
/// Motor DTR
b_optvalue = true;
b_optvalue = false;
laser.setlidaropt(LidarPropSupportMotorDtrCtrl, &b_optvalue, sizeof(bool));
/// HeartBeat
b_optvalue = false;
Expand Down
142 changes: 142 additions & 0 deletions samples/tri_restart.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,142 @@
#include "CYdLidar.h"
#include <iostream>
#include <string>
#include <algorithm>
#include <cctype>
#include "core/base/timer.h"

using namespace std;
using namespace ydlidar;

#if defined(_MSC_VER)
#pragma comment(lib, "ydlidar_sdk.lib")
#endif

int main(int argc, char *argv[])
{
printf("__ ______ _ ___ ____ _ ____ \n");
printf("\\ \\ / / _ \\| | |_ _| _ \\ / \\ | _ \\ \n");
printf(" \\ V /| | | | | | || | | |/ _ \\ | |_) | \n");
printf(" | | | |_| | |___ | || |_| / ___ \\| _ < \n");
printf(" |_| |____/|_____|___|____/_/ \\_\\_| \\_\\ \n");
printf("\n");
fflush(stdout);

ydlidar::os_init();

bool ret = false;

CYdLidar lidarS2; //S2雷达
{
bool isSingleChannel = false;
float frequency = 8.0;
std::string port = "/dev/ttyUSB0";
int baudrate = 115200;
//////////////////////string property/////////////////
/// lidar port
lidarS2.setlidaropt(LidarPropSerialPort, port.c_str(), port.size());
//////////////////////int property/////////////////
/// lidar baudrate
lidarS2.setlidaropt(LidarPropSerialBaudrate, &baudrate, sizeof(int));
/// tof lidar
int optval = TYPE_TRIANGLE;
lidarS2.setlidaropt(LidarPropLidarType, &optval, sizeof(int));
/// device type
optval = YDLIDAR_TYPE_SERIAL;
lidarS2.setlidaropt(LidarPropDeviceType, &optval, sizeof(int));
/// sample rate
optval = isSingleChannel ? 3 : 4;
lidarS2.setlidaropt(LidarPropSampleRate, &optval, sizeof(int));
/// abnormal count
optval = 4;
lidarS2.setlidaropt(LidarPropAbnormalCheckCount, &optval, sizeof(int));
/// Intenstiy bit count
optval = 10;
lidarS2.setlidaropt(LidarPropIntenstiyBit, &optval, sizeof(int));
//////////////////////bool property/////////////////
/// fixed angle resolution
bool b_optvalue = false;
lidarS2.setlidaropt(LidarPropFixedResolution, &b_optvalue, sizeof(bool));
b_optvalue = false;
/// rotate 180
lidarS2.setlidaropt(LidarPropReversion, &b_optvalue, sizeof(bool));
/// Counterclockwise
lidarS2.setlidaropt(LidarPropInverted, &b_optvalue, sizeof(bool));
b_optvalue = true;
lidarS2.setlidaropt(LidarPropAutoReconnect, &b_optvalue, sizeof(bool));
/// one-way communication
lidarS2.setlidaropt(LidarPropSingleChannel, &isSingleChannel, sizeof(bool));
/// intensity
b_optvalue = true;
lidarS2.setlidaropt(LidarPropIntenstiy, &b_optvalue, sizeof(bool));
/// Motor DTR
b_optvalue = false;
lidarS2.setlidaropt(LidarPropSupportMotorDtrCtrl, &b_optvalue, sizeof(bool));
/// HeartBeat
b_optvalue = false;
lidarS2.setlidaropt(LidarPropSupportHeartBeat, &b_optvalue, sizeof(bool));
//////////////////////float property/////////////////
/// unit: °
float f_optvalue = 180.0f;
lidarS2.setlidaropt(LidarPropMaxAngle, &f_optvalue, sizeof(float));
f_optvalue = -180.0f;
lidarS2.setlidaropt(LidarPropMinAngle, &f_optvalue, sizeof(float));
/// unit: m
f_optvalue = 64.f;
lidarS2.setlidaropt(LidarPropMaxRange, &f_optvalue, sizeof(float));
f_optvalue = 0.05f;
lidarS2.setlidaropt(LidarPropMinRange, &f_optvalue, sizeof(float));
/// unit: Hz
lidarS2.setlidaropt(LidarPropScanFrequency, &frequency, sizeof(float));
}

LaserScan scanGs; //GS2点云数据
LaserScan scanS2; //S2雷达点云数据
while (ydlidar::os_isOk())
{
ret = lidarS2.initialize();
if (!ret)
{
fprintf(stderr, "Fail to initialize %s\n", lidarS2.DescribeError());
fflush(stderr);
return -1;
}
//启动S2
ret = lidarS2.turnOn();
if (!ret)
{
fprintf(stderr, "Fail to turn on S2 %s\n", lidarS2.DescribeError());
fflush(stderr);
return -1;
}
//启动后运行5秒然后停止扫描
uint64_t t = getms();
while (getms() - t < 5000)
{
//获取S2点云数据
if (lidarS2.doProcessSimple(scanS2))
{
printf("[%u] points inc [%f]\n",
(unsigned int)scanS2.points.size(),
scanS2.config.angle_increment);
fflush(stdout);
}
else
{
fprintf(stderr, "Failed to get S2 ldiar data\n");
fflush(stderr);
static int s_errorCount = 0;
if (s_errorCount++ > 10)
return -1;
}
}

//停止S2
lidarS2.turnOff();
}

lidarS2.turnOff();
lidarS2.disconnecting();

return 0;
}
4 changes: 2 additions & 2 deletions src/CYdLidar.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1318,8 +1318,8 @@ bool CYdLidar::getDeviceHealth()
if (healthinfo.status == 2)
{
fprintf(stderr,
"[YDLIDAR] Error, YDLidar internal error detected. "
"Please reboot the device to retry.\n");
"[YDLIDAR] Error, YDLidar internal error[0x%X] detected. "
"Please reboot the device to retry.\n", healthinfo.error_code);
return false;
}
else
Expand Down
29 changes: 18 additions & 11 deletions src/YDlidarDriver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1901,8 +1901,12 @@ result_t YDlidarDriver::startScan(bool force, uint32_t timeout)
}
}

//获取强度标识
getIntensityFlag();
//非Tmini系列雷达才自动获取强度标识
if (!isTminiLidar(model))
{
// 获取强度标识
getIntensityFlag();
}

//创建数据解析线程
ans = createThread();
Expand Down Expand Up @@ -2624,6 +2628,8 @@ result_t YDlidarDriver::parseHeader(
return ans;
}

#define ZERO_OFFSET12 12 //零位包数据长度12（不带光强）
#define ZERO_OFFSET13 13 //零位包数据长度13（带光强）
result_t YDlidarDriver::getIntensityFlag()
{
//只针对三角雷达
Expand All @@ -2636,7 +2642,7 @@ result_t YDlidarDriver::getIntensityFlag()
m_dataPos = 0;
uint32_t lastOffset = 0;
//遍历5圈，如果5圈结果一致则认为准确
int i = 2;
int i = 5;
while (i-- > 0)
{
uint8_t zero = 0; //零位包标记
Expand All @@ -2659,16 +2665,19 @@ result_t YDlidarDriver::getIntensityFlag()
lastZero = 0;

offset = headPos - lastPos;
//printf("lastPos %u currPos %u offset %u\n", lastPos, headPos, offset);
// printf("lastPos %u currPos %u offset %u\n", lastPos, headPos, offset);
//fflush(stdout);

if (offset != 12 &&
offset != 13)
continue;
if (offset != ZERO_OFFSET12 &&
offset != ZERO_OFFSET13)
break;

if (lastOffset &&
lastOffset != offset)
{
printf("[YDLIDAR] Fail to getting intensity\n");
return RESULT_FAIL;
}

lastOffset = offset;
break;
Expand All @@ -2681,18 +2690,16 @@ result_t YDlidarDriver::getIntensityFlag()

if (lastOffset)
{
if (lastOffset == 12)
if (lastOffset == ZERO_OFFSET12)
{
setIntensities(false);
}
else if (lastOffset == 13)
else if (lastOffset == ZERO_OFFSET13)
{
setIntensities(true);
m_intensityBit = 8;
}

printf("[YDLIDAR] Auto set intensity %d\n", m_intensities);
fflush(stdout);
}

printf("[YDLIDAR] End to getting intensity flag\n");
Expand Down

0 comments on commit 6e7e166

Please sign in to comment.