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() ...@@ -128,7 +128,9 @@ void AdjustPPK::LoadPPK()
// } // }
ieReader->Init(ieBaseDir_ + "/gnss.txt"); ieReader->Init(ieBaseDir_ + "/gnss.txt");
rawData_ = ieReader->GetData(); 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() 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) { ...@@ -1020,7 +1020,7 @@ void Convert::calcPointXYZIT(pandar_msgs::PandarPacket &packet, int cursor) {
} }
else if(azimuthIdx < 0) { else if(azimuthIdx < 0) {
azimuthIdx += CIRCLE; azimuthIdx += CIRCLE;
} }
point.x = xyDistance * m_fSinAllAngle[azimuthIdx]; point.x = xyDistance * m_fSinAllAngle[azimuthIdx];
point.y = xyDistance * m_fCosAllAngle[azimuthIdx]; point.y = xyDistance * m_fCosAllAngle[azimuthIdx];
point.z = distance * m_fSinAllAngle[pitchIdx]; point.z = distance * m_fSinAllAngle[pitchIdx];
...@@ -1441,52 +1441,75 @@ bool Convert::processPacket(PPointCloud& pc) ...@@ -1441,52 +1441,75 @@ bool Convert::processPacket(PPointCloud& pc)
pktCount = 0; pktCount = 0;
return false; 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(); // uint32_t startTick1 = GetTickCount();
m_OutMsgArray[cursor]->resize(CIRCLE_ANGLE / m_iAngleSize * m_iLaserNum * m_iReturnBlockSize ); moveTaskEndToStartAngle();
if(m_RedundantPointBuffer.size() > 0 && m_RedundantPointBuffer.size() < MAX_REDUNDANT_POINT_NUM){ 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++){ for(int i = 0; i < m_RedundantPointBuffer.size(); i++){
m_OutMsgArray[cursor]->points[m_RedundantPointBuffer[i].index] = m_RedundantPointBuffer[i].point; m_OutMsgArray[cursor]->points[m_RedundantPointBuffer[i].index] = m_RedundantPointBuffer[i].point;
} }
} }
m_RedundantPointBuffer.clear(); m_RedundantPointBuffer.clear();
// uint32_t endTick2 = GetTickCount();
// if(endTick2 - startTick2 > 2) { m_OutMsgArray[cursor]->header.frame_id = m_sFrameId;
// printf("m_OutMsgArray time:%d\n", endTick2 - startTick2); m_OutMsgArray[cursor]->height = 1;
// }
m_OutMsgArray[cursor]->header.frame_id = m_sFrameId; pcl_conversions::toPCL(ros::Time(m_dTimestamp),
m_OutMsgArray[cursor]->height = 1; m_OutMsgArray[m_iPublishPointsIndex]->header.stamp);
return false; pc = *m_OutMsgArray[m_iPublishPointsIndex];
} return true;
// uint32_t taskflow1 = GetTickCount();
// printf("if compare time: %d\n", ifTick - startTick); // checkClockwise();
doTaskFlow(cursor); // if(isNeedPublish()) { // Judging whether pass the start angle
// uint32_t taskflow2 = GetTickCount(); // // uint32_t startTick1 = GetTickCount();
// printf("taskflow time: %d\n", taskflow2 - taskflow1); // moveTaskEndToStartAngle();
// doTaskFlow(cursor);
if (m_bPublishPointsFlag) { // // uint32_t startTick2 = GetTickCount();
pcl_conversions::toPCL(ros::Time(m_dTimestamp), // // printf("move and taskflow time:%d\n", startTick2 - startTick1);
m_OutMsgArray[m_iPublishPointsIndex]->header.stamp); // if(m_bPublishPointsFlag == false) {
pc = *m_OutMsgArray[m_iPublishPointsIndex]; // m_bPublishPointsFlag = true;
return 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 } // 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