Skip to content

Commit

Permalink
单线程改回多线程
Browse files Browse the repository at this point in the history
  • Loading branch information
@zhanyiaini
zhanyiaini committed Mar 8, 2024
1 parent 4e77c79 commit c66339f
Show file tree
Hide file tree
Showing 4 changed files with 78 additions and 71 deletions.
4 changes: 2 additions & 2 deletions examples/tmini_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -163,7 +163,7 @@ int main(int argc, char *argv[]) {

std::string input_frequency;

float frequency = 6.0;
float frequency = 10.0;

// while (ydlidar::os_isOk() && !isSingleChannel) {
// printf("Please enter the lidar scan frequency[5-12]:");
Expand Down Expand Up @@ -211,7 +211,7 @@ 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
Expand Down
4 changes: 3 additions & 1 deletion examples/tri_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -270,7 +270,9 @@ int main(int argc, char *argv[])
laser.enableSunNoise(false);

//设置是否获取底板设备信息（默认仅尝试获取模组设备信息）
laser.setBottomPriority(true);
laser.setBottomPriority(false);
//启用调试
laser.setEnableDebug(true);

uint32_t t = getms(); //时间
int c = 0; //计数
Expand Down
21 changes: 13 additions & 8 deletions src/GSLidarDriver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -980,10 +980,10 @@ void GSLidarDriver::angTransform(
}
tempDist = (dist - Angle_Px) / cos(((Angle_PAngle + bias[mdNum]) - (tempTheta)) * M_PI / 180);
tempTheta = tempTheta * M_PI / 180;
tempX = cos((Angle_PAngle + bias[mdNum]) * M_PI / 180) * tempDist * cos(tempTheta) + sin((Angle_PAngle + bias[mdNum]) * M_PI / 180) * (tempDist *
sin(tempTheta));
tempY = -sin((Angle_PAngle + bias[mdNum]) * M_PI / 180) * tempDist * cos(tempTheta) + cos((Angle_PAngle + bias[mdNum]) * M_PI / 180) * (tempDist *
sin(tempTheta));
tempX = cos((Angle_PAngle + bias[mdNum]) * M_PI / 180) * tempDist * cos(tempTheta) +
sin((Angle_PAngle + bias[mdNum]) * M_PI / 180) * (tempDist * sin(tempTheta));
tempY = -sin((Angle_PAngle + bias[mdNum]) * M_PI / 180) * tempDist * cos(tempTheta) +
cos((Angle_PAngle + bias[mdNum]) * M_PI / 180) * (tempDist * sin(tempTheta));
tempX = tempX + Angle_Px;
tempY = tempY - Angle_Py; //5.315
Dist = sqrt(tempX * tempX + tempY * tempY);
Expand All @@ -1002,10 +1002,10 @@ void GSLidarDriver::angTransform(
}
tempDist = (dist - Angle_Px) / cos(((Angle_PAngle + bias[mdNum]) + (tempTheta)) * M_PI / 180);
tempTheta = tempTheta * M_PI / 180;
tempX = cos(-(Angle_PAngle + bias[mdNum]) * M_PI / 180) * tempDist * cos(tempTheta) + sin(-(Angle_PAngle + bias[mdNum]) * M_PI / 180) * (tempDist *
sin(tempTheta));
tempY = -sin(-(Angle_PAngle + bias[mdNum]) * M_PI / 180) * tempDist * cos(tempTheta) + cos(-(Angle_PAngle + bias[mdNum]) * M_PI / 180) * (tempDist *
sin(tempTheta));
tempX = cos(-(Angle_PAngle + bias[mdNum]) * M_PI / 180) * tempDist * cos(tempTheta) +
sin(-(Angle_PAngle + bias[mdNum]) * M_PI / 180) * (tempDist * sin(tempTheta));
tempY = -sin(-(Angle_PAngle + bias[mdNum]) * M_PI / 180) * tempDist * cos(tempTheta) +
cos(-(Angle_PAngle + bias[mdNum]) * M_PI / 180) * (tempDist * sin(tempTheta));
tempX = tempX + Angle_Px;
tempY = tempY + Angle_Py; //5.315
Dist = sqrt(tempX * tempX + tempY * tempY);
Expand All @@ -1017,6 +1017,8 @@ void GSLidarDriver::angTransform(
}
*dstTheta = theta;
*dstDist = Dist;

// printf("%d %d %f %d\n", n, dist, (float)theta, (int)Dist);
}

void GSLidarDriver::angTransform2(
Expand Down Expand Up @@ -1273,6 +1275,9 @@ result_t GSLidarDriver::getDevicePara(gs_device_para &info, uint32_t timeout) {
b0[mdNum] = info.b0 / 10000.00;
b1[mdNum] = info.b1 / 10000.00;
bias[mdNum] = double(info.bias) * 0.1;

// debug("k0 %lf k1 %lf b0 %lf b1 %lf bias %lf",
// k0[mdNum], k1[mdNum], b0[mdNum], b1[mdNum], bias[mdNum]);
delay(5);
}
}
Expand Down
120 changes: 60 additions & 60 deletions src/YDlidarDriver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1426,68 +1426,68 @@ result_t YDlidarDriver::grabScanData(
size_t &count,
uint32_t timeout)
{
// switch (_dataEvent.wait(timeout))
// {
// case Event::EVENT_TIMEOUT:
// count = 0;
// return RESULT_TIMEOUT;
// case Event::EVENT_OK:
// {
// ScopedLocker l(_lock);
// size_t size_to_copy = min(count, scan_node_count);
// memcpy(nodes, scan_node_buf, size_to_copy * sizeof(node_info));
// count = size_to_copy;
// scan_node_count = 0;
// return RESULT_OK;
// }
// default:
// count = 0;
// return RESULT_FAIL;
// }

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)
switch (_dataEvent.wait(timeout))
{
packCount = LIDAR_PACKMAXPOINTSIZE;
ans = waitScanData(packNodes, packCount, timeout - wt);
if (!IS_OK(ans))
{
return ans; //失败时直接返回
}
else
{
bool hasZero = false; //当前包中是否有零位标记
for (size_t i = 0; i < packCount; ++i)
{
if (packNodes[i].sync & LIDAR_RESP_SYNCBIT)
{
hasZero = true;
}

nodes[currCount ++] = packNodes[i];
if (currCount >= count)
{
hasZero = true;
printf("[YDLIDAR] Current points count %d > buffer size %d\n",
currCount, count);
fflush(stdout);
break;
}
}
if (hasZero)
{
count = currCount;
return RESULT_OK;
}
}
case Event::EVENT_TIMEOUT:
count = 0;
return RESULT_TIMEOUT;
case Event::EVENT_OK:
{
ScopedLocker l(_lock);
size_t size_to_copy = min(count, scan_node_count);
memcpy(nodes, scan_node_buf, size_to_copy * sizeof(node_info));
count = size_to_copy;
scan_node_count = 0;
return RESULT_OK;
}
default:
count = 0;
return RESULT_FAIL;
}

// 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
// {
// bool hasZero = false; //当前包中是否有零位标记
// for (size_t i = 0; i < packCount; ++i)
// {
// if (packNodes[i].sync & LIDAR_RESP_SYNCBIT)
// {
// hasZero = true;
// }

// nodes[currCount ++] = packNodes[i];
// if (currCount >= count)
// {
// hasZero = true;
// printf("[YDLIDAR] Current points count %d > buffer size %d\n",
// currCount, count);
// fflush(stdout);
// break;
// }
// }
// if (hasZero)
// {
// count = currCount;
// return RESULT_OK;
// }
// }
// }

return RESULT_TIMEOUT;
// return RESULT_TIMEOUT;
}

result_t YDlidarDriver::ascendScanData(node_info *nodebuffer, size_t count) {
Expand Down Expand Up @@ -1871,7 +1871,7 @@ result_t YDlidarDriver::startScan(bool force, uint32_t timeout)
}

//创建数据解析线程
// ret = createThread();
ret = createThread();
}

if (isSupportMotorCtrl(model))
Expand Down

0 comments on commit c66339f

Please sign in to comment.