Skip to content

L
localize_for_ppk
Commit 3dbb4870 authored by wangdawei's avatar wangdawei
Browse files
Options

test

parent f32b545d
Hide whitespace changes
Inline Side-by-side
Showing with 69 additions and 44 deletions
libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp
View file @ 3dbb4870
......@@ -128,7 +128,9 @@ void AdjustPPK::LoadPPK()
// }
ieReader->Init(ieBaseDir_ + "/gnss.txt");
rawData_ = ieReader->GetData();
LOG(INFO) << "rawData_.size(): " << rawData_.size();
LOG(INFO) << "rawData_.size(): " << rawData_.size()
<< setprecision(15) << " begin: " << rawData_.begin()->unix_time
<< " begin: " << rawData_.back()->unix_time;
}
void AdjustPPK::LoadMapInfo()
......
libs/locate_system/adjust_ppk_by_locate/convert.cc
View file @ 3dbb4870
......@@ -1020,7 +1020,7 @@ void Convert::calcPointXYZIT(pandar_msgs::PandarPacket &packet, int cursor) {
}
else if(azimuthIdx < 0) {
azimuthIdx += CIRCLE;
}
}
point.x = xyDistance * m_fSinAllAngle[azimuthIdx];
point.y = xyDistance * m_fCosAllAngle[azimuthIdx];
point.z = distance * m_fSinAllAngle[pitchIdx];
......@@ -1441,52 +1441,75 @@ bool Convert::processPacket(PPointCloud& pc)
pktCount = 0;
return false;
}
checkClockwise();
if(isNeedPublish()) { // Judging whether pass the start angle
// uint32_t startTick1 = GetTickCount();
moveTaskEndToStartAngle();
doTaskFlow(cursor);
// uint32_t startTick2 = GetTickCount();
// printf("move and taskflow time:%d\n", startTick2 - startTick1);
if(m_bPublishPointsFlag == false) {
m_bPublishPointsFlag = true;
m_iPublishPointsIndex = cursor;
cursor = (cursor + 1) % 2;
#ifdef PRINT_FLAG
ROS_WARN("ts %lf cld size %u", m_dTimestamp, m_OutMsgArray[m_iPublishPointsIndex]->points.size());
#endif
}
else
ROS_WARN("publishPoints not done yet, new publish is comming\n");
m_OutMsgArray[cursor]->clear();
m_OutMsgArray[cursor]->resize(CIRCLE_ANGLE / m_iAngleSize * m_iLaserNum * m_iReturnBlockSize );
if(m_RedundantPointBuffer.size() > 0 && m_RedundantPointBuffer.size() < MAX_REDUNDANT_POINT_NUM){
// uint32_t startTick1 = GetTickCount();
moveTaskEndToStartAngle();
doTaskFlow(cursor);
m_OutMsgArray[cursor]->clear();
m_OutMsgArray[cursor]->resize(CIRCLE_ANGLE / m_iAngleSize * m_iLaserNum * m_iReturnBlockSize );
if(m_RedundantPointBuffer.size() > 0 && m_RedundantPointBuffer.size() < MAX_REDUNDANT_POINT_NUM){
for(int i = 0; i < m_RedundantPointBuffer.size(); i++){
m_OutMsgArray[cursor]->points[m_RedundantPointBuffer[i].index] = m_RedundantPointBuffer[i].point;
}
}
m_RedundantPointBuffer.clear();
// uint32_t endTick2 = GetTickCount();
// if(endTick2 - startTick2 > 2) {
// printf("m_OutMsgArray time:%d\n", endTick2 - startTick2);
// }
m_OutMsgArray[cursor]->header.frame_id = m_sFrameId;
m_OutMsgArray[cursor]->height = 1;
return false;
}
// uint32_t taskflow1 = GetTickCount();
// printf("if compare time: %d\n", ifTick - startTick);
doTaskFlow(cursor);
// uint32_t taskflow2 = GetTickCount();
// printf("taskflow time: %d\n", taskflow2 - taskflow1);
if (m_bPublishPointsFlag) {
pcl_conversions::toPCL(ros::Time(m_dTimestamp),
m_OutMsgArray[m_iPublishPointsIndex]->header.stamp);
pc = *m_OutMsgArray[m_iPublishPointsIndex];
return true;
}
}
m_RedundantPointBuffer.clear();
m_OutMsgArray[cursor]->header.frame_id = m_sFrameId;
m_OutMsgArray[cursor]->height = 1;
pcl_conversions::toPCL(ros::Time(m_dTimestamp),
m_OutMsgArray[m_iPublishPointsIndex]->header.stamp);
pc = *m_OutMsgArray[m_iPublishPointsIndex];
return true;
// checkClockwise();
// if(isNeedPublish()) { // Judging whether pass the start angle
// // uint32_t startTick1 = GetTickCount();
// moveTaskEndToStartAngle();
// doTaskFlow(cursor);
// // uint32_t startTick2 = GetTickCount();
// // printf("move and taskflow time:%d\n", startTick2 - startTick1);
// if(m_bPublishPointsFlag == false) {
// m_bPublishPointsFlag = true;
// m_iPublishPointsIndex = cursor;
// cursor = (cursor + 1) % 2;
//#ifdef PRINT_FLAG
// ROS_WARN("ts %lf cld size %u", m_dTimestamp, m_OutMsgArray[m_iPublishPointsIndex]->points.size());
//#endif
// }
// else
// ROS_WARN("publishPoints not done yet, new publish is comming\n");
// m_OutMsgArray[cursor]->clear();
// m_OutMsgArray[cursor]->resize(CIRCLE_ANGLE / m_iAngleSize * m_iLaserNum * m_iReturnBlockSize );
// if(m_RedundantPointBuffer.size() > 0 && m_RedundantPointBuffer.size() < MAX_REDUNDANT_POINT_NUM){
// for(int i = 0; i < m_RedundantPointBuffer.size(); i++){
// m_OutMsgArray[cursor]->points[m_RedundantPointBuffer[i].index] = m_RedundantPointBuffer[i].point;
// }
// }
// m_RedundantPointBuffer.clear();
// // uint32_t endTick2 = GetTickCount();
// // if(endTick2 - startTick2 > 2) {
// // printf("m_OutMsgArray time:%d\n", endTick2 - startTick2);
// // }
// m_OutMsgArray[cursor]->header.frame_id = m_sFrameId;
// m_OutMsgArray[cursor]->height = 1;
// return false;
// }
// // uint32_t taskflow1 = GetTickCount();
// // printf("if compare time: %d\n", ifTick - startTick);
// doTaskFlow(cursor);
// // uint32_t taskflow2 = GetTickCount();
// // printf("taskflow time: %d\n", taskflow2 - taskflow1);
// if (m_bPublishPointsFlag) {
// pcl_conversions::toPCL(ros::Time(m_dTimestamp),
// m_OutMsgArray[m_iPublishPointsIndex]->header.stamp);
// pc = *m_OutMsgArray[m_iPublishPointsIndex];
// return true;
// }
}
} // namespace pandar_pointcloud
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment