Skip to content

Commit

Permalink
修改TEA、GS1等时间增量值无效导致ROS崩溃问题
Browse files Browse the repository at this point in the history
  • Loading branch information
@zhanyiaini
zhanyiaini committed Jan 9, 2024
1 parent 8d4c136 commit dde846c
Show file tree
Hide file tree
Showing 8 changed files with 83 additions and 67 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 21)
set(YDLIDAR_SDK_VERSION_PATCH 22)
set(YDLIDAR_SDK_VERSION ${YDLIDAR_SDK_VERSION_MAJOR}.${YDLIDAR_SDK_VERSION_MINOR}.${YDLIDAR_SDK_VERSION_PATCH})

##########################################################
Expand Down
12 changes: 4 additions & 8 deletions core/common/ydlidar_protocol.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,15 +41,12 @@
#define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0]))
#endif

/**
* @name PI constant
* @{
*/
#ifndef M_PI
#define M_PI 3.1415926
#endif
/** @}
*/

//浮点型判断是否为0
#define ISZERO(v) (abs(v) < 1e-6)

/**
* @name sun noise flag constant
Expand Down Expand Up @@ -140,8 +137,7 @@
#define LIDAR_MODULE_3 0x04
#define LIDAR_MODULE_ALL 0x00
#define LIDAR_MAXCOUNT 3 //最大模组数

/** @} LIDAR CMD Protocol */
#define GSMAXPOINTCOUNT 160 //GS数据包中最大点云数

//GS
#define Angle_Px 1.22
Expand Down
4 changes: 2 additions & 2 deletions samples/gs_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -266,10 +266,10 @@ int main(int argc, char *argv[])
{
if (laser.doProcessSimple(scan))
{
printf("[%llu] points in module num [%d] env flag [0x%04X]\n",
printf("[%llu] points in module num [%d] env flag [%f]\n",
scan.points.size(),
scan.moduleNum,
scan.envFlag);
scan.config.time_increment);
// uint32_t t = getms();
// printf("module[%d] time[%lld]\n", scan.moduleNum, t - ts[scan.moduleNum]);
// ts[scan.moduleNum] = t;
Expand Down
5 changes: 3 additions & 2 deletions samples/tea_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -208,9 +208,10 @@ int main(int argc, char *argv[])
/// Turn On success and loop
if (laser.doProcessSimple(scan))
{
fprintf(stdout, "Scan received [%llu] points is [%f]s\n",
fprintf(stdout, "Scan received [%llu] points is [%f]s [%f]\n",
scan.points.size(),
scan.config.scan_time);
scan.config.scan_time,
scan.config.time_increment);
fflush(stdout);
}
else
Expand Down
14 changes: 10 additions & 4 deletions src/CYdLidar.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@
#include "core/serial/common.h"
#include "core/common/DriverInterface.h"
#include "core/common/ydlidar_help.h"
#include "core/common/ydlidar_protocol.h"
#include "YDlidarDriver.h"
#include "ETLidarDriver.h"
#include "GSLidarDriver.h"
Expand Down Expand Up @@ -631,10 +632,15 @@ bool CYdLidar::doProcessSimple(LaserScan &outscan)
outscan.config.min_angle = math::from_degrees(m_MinAngle);
outscan.config.max_angle = math::from_degrees(m_MaxAngle);
//将首末点采集时间差作为采集时长
// printf("stamp [%llu]-[%llu]\n", global_nodes[0].stamp, global_nodes[count - 1].stamp);
outscan.config.scan_time = static_cast<float>((global_nodes[count - 1].stamp - global_nodes[0].stamp)) / 1e9;
// outscan.config.scan_time = static_cast<float>(scan_time * 1.0 / 1e9);
outscan.config.time_increment = outscan.config.scan_time / (double)(count - 1);
outscan.config.scan_time = static_cast<float>((global_nodes[count - 1].stamp -
global_nodes[0].stamp)) / 1e9;
//outscan.config.scan_time = static_cast<float>(scan_time * 1.0 / 1e9);
// if (outscan.config.scan_time > 1)
// printf("stamp [%llu]-[%llu]\n", global_nodes[0].stamp, global_nodes[count - 1].stamp);
if (!ISZERO(outscan.config.scan_time))
outscan.config.time_increment = outscan.config.scan_time / (count - 1);
else
outscan.config.time_increment = .0f;
outscan.config.min_range = m_MinRange;
outscan.config.max_range = m_MaxRange;
//模组编号
Expand Down
86 changes: 49 additions & 37 deletions src/GSLidarDriver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,7 +67,6 @@ GSLidarDriver::GSLidarDriver(uint8_t type)
sample_rate = 5000;
m_PointTime = 1e9 / 5000;
trans_delay = 0;
scan_frequence = 0;
model = YDLIDAR_GS2;
retryCount = 0;
m_SingleChannel = false;
Expand Down Expand Up @@ -587,8 +586,8 @@ result_t GSLidarDriver::checkAutoConnecting()

int GSLidarDriver::cacheScanData()
{
node_info local_buf[200];
size_t count = 200;
node_info local_buf[GSMAXPOINTCOUNT];
size_t count = GSMAXPOINTCOUNT;
size_t scan_count = 0;
result_t ans = RESULT_FAIL;

Expand All @@ -599,10 +598,10 @@ int GSLidarDriver::cacheScanData()

while (m_isScanning)
{
count = 160;
count = GSMAXPOINTCOUNT;
ans = waitScanData(local_buf, count);
Thread::needExit();
if (!IS_OK(ans))
if (!IS_OK(ans))
{
if (IS_FAIL(ans))
{
Expand Down Expand Up @@ -644,30 +643,27 @@ int GSLidarDriver::cacheScanData()
retryCount = 0;
}

if (!isPrepareToSend) {
if (!isPrepareToSend)
continue;
}

// printf("[YDLIDAR] GS2 points stored %lu\n", count);
ScopedLocker l(_lock);
size_t size = packages.size();
for (size_t i = 0;i < size; i++) {
for (size_t i = 0;i < size; i++)
{
if (packages[i].frameNum == frameNum &&
packages[i].moduleNum == moduleNum) {
memcpy(scan_node_buf, packages[i].points, sizeof(node_info) * 160);
packages[i].moduleNum == moduleNum)
{
memcpy(scan_node_buf, packages[i].points,
sizeof(node_info) * GSMAXPOINTCOUNT);
break;
}
}

{
// printf("[YDLIDAR] GS2 points stored %lu\n", count);
ScopedLocker l(_lock);
scan_node_buf[0].stamp = local_buf[0].stamp;
scan_node_buf[0].scanFreq = local_buf[0].scanFreq;
scan_node_buf[0].index = 0x03 & (moduleNum >> 1); // gs2: 1, 2, 4
scan_node_count = 160; //一个包固定160个数据
_dataEvent.set();
scan_count = 0;
isPrepareToSend = false;
}

scan_node_count = count; //包点数
_dataEvent.set();
scan_count = 0;
isPrepareToSend = false;
}

m_isScanning = false;
Expand Down Expand Up @@ -849,13 +845,21 @@ result_t GSLidarDriver::waitPackage(node_info *node, uint32_t timeout)
} //end if (nodeIndex == 0)

(*node).stamp = getTime();
(*node).sync = Node_NotSync;


if (CheckSumResult)
{
(*node).index = 0x03 & (moduleNum >> 1);
(*node).scanFreq = scan_frequence;
//第1个点时间戳使用上一帧的最后1个点的时间戳
if (nodeIndex == 0)
(*node).stamp = stamp ? stamp : getTime();
else
(*node).stamp = getTime();
stamp = (*node).stamp;

(*node).index = 0x03 & (moduleNum >> 1); //模组地址转编号: 1, 2, 4
(*node).scanFreq = 0;
(*node).qual = 0;
(*node).sync = Node_NotSync;

if (YDLIDAR_GS1 == model)
{
Expand Down Expand Up @@ -1045,27 +1049,36 @@ void GSLidarDriver::angTransform2(
*dstDist = Dist;
}

void GSLidarDriver::addPointsToVec(node_info *nodebuffer, size_t &count){
void GSLidarDriver::addPointsToVec(node_info *nodebuffer, size_t &count)
{
size_t size = packages.size();
bool isFound = false;
for(size_t i =0;i < size; i++){
if(packages[i].frameNum == frameNum && packages[i].moduleNum == moduleNum){
for (size_t i =0;i < size; i++)
{
if (packages[i].frameNum == frameNum &&
packages[i].moduleNum == moduleNum)
{
isFound = true;
memcpy(packages[i].points,nodebuffer,sizeof (node_info) * count);
memcpy(packages[i].points, nodebuffer, sizeof (node_info) * count);
isPrepareToSend = true;
if(frameNum > 0){
if (frameNum > 0)
{
int lastFrame = frameNum - 1;
for(size_t j =0;j < size; j++){
if(packages[j].frameNum == lastFrame && packages[j].moduleNum == moduleNum){
for (size_t j =0;j < size; j++)
{
if (packages[j].frameNum == lastFrame &&
packages[j].moduleNum == moduleNum)
{
break;
}
}
}
break;
}
}
if(!isFound){
gs_packages package;
if (!isFound)
{
gs_packages package;
package.frameNum = frameNum;
package.moduleNum = moduleNum;
packages.push_back(package);
Expand Down Expand Up @@ -1129,8 +1142,6 @@ result_t GSLidarDriver::waitScanData(
}
addPointsToVec(nodebuffer, recvNodeCount);

// nodebuffer[recvNodeCount - 1].stamp = size * trans_delay + delayTime;
// nodebuffer[recvNodeCount - 1].scanFreq = node.scanFreq;
count = recvNodeCount;
return RESULT_OK;
}
Expand Down Expand Up @@ -1394,7 +1405,8 @@ void GSLidarDriver::setIntensities(const bool &isintensities)
* true 开启
* false 关闭
*/
void GSLidarDriver::setAutoReconnect(const bool &enable) {
void GSLidarDriver::setAutoReconnect(const bool &enable)
{
isAutoReconnect = enable;
}

Expand Down
2 changes: 1 addition & 1 deletion src/GSLidarDriver.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -493,7 +493,6 @@ namespace ydlidar
gs_node_package package; ///< 带信号质量协议包

uint8_t CheckSum; ///< 校验和
uint8_t scan_frequence; ///< 协议中雷达转速

uint8_t CheckSumCal;
bool CheckSumResult;
Expand All @@ -513,6 +512,7 @@ namespace ydlidar
uint8_t moduleCount = 1; // 当前模组数量
std::vector<gs_packages> packages;
int nodeCount = 0; //当前包点数
uint64_t stamp = 0; //时间戳
};

} // namespace ydlidar
Expand Down
25 changes: 13 additions & 12 deletions src/YDlidarDriver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -692,8 +692,9 @@ int YDlidarDriver::cacheScanData()
if (local_scan[0].sync & LIDAR_RESP_SYNCBIT)
{
ScopedLocker l(_lock);
//将下一圈的第一个点的采集时间作为当前圈数据的采集时间
local_scan[0].delayTime = local_buf[pos].delayTime;
//将下一圈的第一个点的采集时间作为当前圈数据的采集时间

memcpy(scan_node_buf, local_scan, scan_count * sizeof(node_info));
scan_node_count = scan_count;
_dataEvent.set();
Expand Down Expand Up @@ -1321,17 +1322,17 @@ void YDlidarDriver::parseNodeFromeBuffer(node_info *node)
if ((FirstSampleAngle + sampleAngle +
correctAngle) < 0) {
(*node).angle = (((uint16_t)(FirstSampleAngle + sampleAngle +
correctAngle + 23040)) << LIDAR_RESP_ANGLE_SHIFT) +
LIDAR_RESP_CHECKBIT;
correctAngle + 23040)) << LIDAR_RESP_ANGLE_SHIFT) +
LIDAR_RESP_CHECKBIT;
} else {
if ((FirstSampleAngle + sampleAngle + correctAngle) > 23040) {
(*node).angle = (((uint16_t)(FirstSampleAngle + sampleAngle +
correctAngle - 23040)) << LIDAR_RESP_ANGLE_SHIFT) +
LIDAR_RESP_CHECKBIT;
correctAngle - 23040)) << LIDAR_RESP_ANGLE_SHIFT) +
LIDAR_RESP_CHECKBIT;
} else {
(*node).angle = (((uint16_t)(FirstSampleAngle + sampleAngle +
correctAngle)) << LIDAR_RESP_ANGLE_SHIFT) +
LIDAR_RESP_CHECKBIT;
correctAngle)) << LIDAR_RESP_ANGLE_SHIFT) +
LIDAR_RESP_CHECKBIT;
}
}
} else {
Expand Down Expand Up @@ -1398,8 +1399,6 @@ result_t YDlidarDriver::waitScanData(
}
nodebuffer[recvNodeCount - 1].delayTime = size * trans_delay + delayTime;

// nodebuffer[recvNodeCount - 1].scanFreq = node.scanFreq;
// nodebuffer[recvNodeCount - 1].stamp = getTime();
count = recvNodeCount;
CheckLaserStatus();
return RESULT_OK;
Expand Down Expand Up @@ -1710,18 +1709,20 @@ void YDlidarDriver::setIntensities(const bool &isintensities)
PackageSampleBytes = 2;
}
}

/**
* @brief 设置雷达异常自动重新连接 \n
* @param[in] enable 是否开启自动重连:
* true 开启
* false 关闭
*/
void YDlidarDriver::setAutoReconnect(const bool &enable) {
void YDlidarDriver::setAutoReconnect(const bool &enable)
{
isAutoReconnect = enable;
}


void YDlidarDriver::checkTransDelay() {
void YDlidarDriver::checkTransDelay()
{
//calc stamp
trans_delay = _serial->getByteTime();
sample_rate = getDefaultSampleRate(model).front() * 1000;
Expand Down

0 comments on commit dde846c

Please sign in to comment.