Skip to content

Commit

Permalink
修改自动重连时可能引起失败问题
Browse files Browse the repository at this point in the history
  • Loading branch information
@zhanyiaini
zhanyiaini committed Apr 2, 2024
1 parent 847f645 commit 97a8b0d
Show file tree
Hide file tree
Showing 6 changed files with 62 additions and 32 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 2)
set(YDLIDAR_SDK_VERSION_PATCH 3)
set(YDLIDAR_SDK_VERSION_PATCH 4)
set(YDLIDAR_SDK_VERSION ${YDLIDAR_SDK_VERSION_MAJOR}.${YDLIDAR_SDK_VERSION_MINOR}.${YDLIDAR_SDK_VERSION_PATCH})

##########################################################
Expand Down
48 changes: 26 additions & 22 deletions examples/gs_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -199,20 +199,21 @@ int main(int argc, char *argv[])
laser.setlidaropt(LidarPropSupportHeartBeat, &b_optvalue, sizeof(bool));

//////////////////////float property/////////////////
/// unit: °
// 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
// unit: m
f_optvalue = 1.f;
laser.setlidaropt(LidarPropMaxRange, &f_optvalue, sizeof(float));
f_optvalue = 0.025f;
laser.setlidaropt(LidarPropMinRange, &f_optvalue, sizeof(float));
/// unit: Hz
// unit: Hz
laser.setlidaropt(LidarPropScanFrequency, &frequency, sizeof(float));

laser.setEnableDebug(false);
//是否启用调试
laser.setEnableDebug(false);

//雷达初始化
bool ret = laser.initialize();
Expand All @@ -223,15 +224,15 @@ int main(int argc, char *argv[])
return -1;
}
//设置雷达工作模式（0表示避障模式，1表示沿边模式）
// ret &= laser.setWorkMode(0, 0x01);
ret &= laser.setWorkMode(1, 0x01);
// ret &= laser.setWorkMode(0, 0x02);
// ret &= laser.setWorkMode(1, 0x04);
// if (!ret)
// {
// fprintf(stderr, "Fail to set work mode %s\n", laser.DescribeError());
// fflush(stderr);
// return -1;
// }
if (!ret)
{
fprintf(stderr, "Fail to set work mode %s\n", laser.DescribeError());
fflush(stderr);
return -1;
}
//获取级联雷达设备信息
// std::vector<device_info_ex> dis;
// ret = laser.getDeviceInfo(dis);
Expand All @@ -258,11 +259,11 @@ int main(int argc, char *argv[])
}

LaserScan scan;
//打印帧间隔
// std::map<int, uint32_t> ts;
// ts[0] = getms();
// ts[1] = getms();
// ts[2] = getms();
//打印帧间隔相关
std::map<int, uint32_t> ts;
ts[0] = getms();
ts[1] = getms();
ts[2] = getms();

while (ret && ydlidar::os_isOk())
{
Expand All @@ -271,15 +272,18 @@ int main(int argc, char *argv[])
printf("Module [%d] [%d] points\n",
scan.moduleNum,
int(scan.points.size()));

//打印帧间隔
// uint32_t t = getms();
// printf("module[%d] time[%lld]\n", scan.moduleNum, t - ts[scan.moduleNum]);
// ts[scan.moduleNum] = t;

for (size_t i = 0; i < scan.points.size(); ++i)
{
const LaserPoint &p = scan.points.at(i);
printf("%d a %.01f r %.01f\n", int(i), p.angle * 180.0f / M_PI, p.range * 1000.0f);
}

//打印点云
// for (size_t i = 0; i < scan.points.size(); ++i)
// {
// const LaserPoint &p = scan.points.at(i);
// printf("%d a %.01f r %.01f\n", int(i), p.angle * 180.0f / M_PI, p.range * 1000.0f);
// }
// fflush(stdout);
}
else
Expand Down
2 changes: 1 addition & 1 deletion src/DTSLidarDriver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,7 +66,7 @@ result_t DTSLidarDriver::connect(const char *port, uint32_t baudrate)
if (!_serial)
{
_serial = new serial::Serial(
port,
m_port,
m_baudrate,
serial::Timeout::simpleTimeout(DEFAULT_TIMEOUT));
}
Expand Down
38 changes: 32 additions & 6 deletions src/GSLidarDriver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -127,7 +127,7 @@ result_t GSLidarDriver::connect(const char *port_path, uint32_t baudrate)
}
else
{
_comm = new serial::Serial(port_path, m_baudrate,
_comm = new serial::Serial(m_port, m_baudrate,
serial::Timeout::simpleTimeout(DEFAULT_TIMEOUT));
}
_comm->bindport(port_path, baudrate);
Expand Down Expand Up @@ -1089,7 +1089,7 @@ result_t GSLidarDriver::waitScanData(
}

result_t GSLidarDriver::grabScanData(
node_info *nodebuffer,
node_info *nodes,
size_t &count,
uint32_t timeout)
{
Expand All @@ -1102,17 +1102,41 @@ result_t GSLidarDriver::grabScanData(
if (datas.size())
{
//从数组中取出点云数据
gs_module_nodes nodes = datas.front();
gs_module_nodes ns = datas.front();
datas.pop_front();
size_t size = min(int(count), nodes.pointCount);
memcpy(nodebuffer, nodes.points, size * SDKNODESIZE);
size_t size = min(int(count), ns.pointCount);
memcpy(nodes, ns.points, size * SDKNODESIZE);
count = size;
return RESULT_OK;
}
}
delay(1); //延时
}
return RESULT_TIMEOUT;

// node_info packNodes[LIDAR_PACKMAXPOINTSIZE];
// size_t packCount = 0; //单包点数
// size_t currCount = 0; //当前点数
// result_t ans = RESULT_FAIL;
// uint32_t st = getms();
// uint32_t wt = 0;
// while ((wt = getms() - st) < timeout)
// {
// packCount = LIDAR_PACKMAXPOINTSIZE;
// ans = waitScanData(packNodes, packCount, timeout - wt);
// if (!IS_OK(ans))
// {
// return ans; //失败时直接返回
// }
// else
// {
// count = packCount;
// memcpy(nodes, packNodes, count * SDKNODESIZE);
// return RESULT_OK;
// }
// }

// return RESULT_TIMEOUT;
}

result_t GSLidarDriver::ascendScanData(node_info *nodebuffer, size_t count) {
Expand Down Expand Up @@ -1395,12 +1419,14 @@ result_t GSLidarDriver::startScan(bool force, uint32_t timeout)
}
//启动线程
ans = createThread();
m_isScanning = true;
}

return ans;
}

result_t GSLidarDriver::stopScan(uint32_t timeout) {
result_t GSLidarDriver::stopScan(uint32_t timeout)
{
UNUSED(timeout);
result_t ans;

Expand Down
2 changes: 1 addition & 1 deletion src/SDMLidarDriver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -98,7 +98,7 @@ result_t SDMLidarDriver::connect(const char *port, uint32_t baudrate)
if (!_serial)
{
_serial = new serial::Serial(
port,
m_port,
m_baudrate,
serial::Timeout::simpleTimeout(DEFAULT_TIMEOUT));
}
Expand Down
2 changes: 1 addition & 1 deletion src/YDlidarDriver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -148,7 +148,7 @@ result_t YDlidarDriver::connect(const char *port_path, uint32_t baudrate)
}
else
{
_serial = new serial::Serial(port_path, m_baudrate,
_serial = new serial::Serial(m_port, m_baudrate,
serial::Timeout::simpleTimeout(DEFAULT_TIMEOUT));
}
_serial->bindport(port_path, baudrate);
Expand Down

0 comments on commit 97a8b0d

Please sign in to comment.