Skip to content

L
localize_for_ppk
Commit 5323041e authored by wangdawei's avatar wangdawei
Browse files
Options

test

parent c0cf4430
Hide whitespace changes
Inline Side-by-side
Showing with 103 additions and 8 deletions
libs/locate_system/adjust_ppk_by_locate/convert.cc
View file @ 5323041e
...@@ -210,6 +210,10 @@ bool Convert::processScan( ...@@ -210,6 +210,10 @@ bool Convert::processScan(
m_OutMsgArray[0]->clear(); m_OutMsgArray[0]->clear();
m_OutMsgArray[0]->resize(CIRCLE_ANGLE / m_iAngleSize * m_iLaserNum * m_iReturnBlockSize ); m_OutMsgArray[0]->resize(CIRCLE_ANGLE / m_iAngleSize * m_iLaserNum * m_iReturnBlockSize );
for (size_t next = 0; next < scanMsg->packets.size(); ++next) { for (size_t next = 0; next < scanMsg->packets.size(); ++next) {
if(!inited){
init(scanMsg->packets[next]);
inited = true;
}
calcPointXYZIT(scanMsg->packets[next], 0); calcPointXYZIT(scanMsg->packets[next], 0);
} }
m_OutMsgArray[0]->header.frame_id = m_sFrameId; m_OutMsgArray[0]->header.frame_id = m_sFrameId;
...@@ -599,16 +603,12 @@ void Convert::moveTaskEndToStartAngle() { ...@@ -599,16 +603,12 @@ void Convert::moveTaskEndToStartAngle() {
} }
void Convert::init() { void Convert::init() {
static bool inited = false;
if(inited){
return;
}
while (1) { while (1) {
if(!m_PacketsBuffer.hasEnoughPackets()) { if(!m_PacketsBuffer.hasEnoughPackets()) {
usleep(1000); usleep(1000);
continue; continue;
} }
uint16_t lidarmotorspeed = 0; uint16_t lidarmotorspeed = 0;
m_u8UdpVersionMajor = (m_PacketsBuffer.getTaskEnd() - 1)->data[2]; m_u8UdpVersionMajor = (m_PacketsBuffer.getTaskEnd() - 1)->data[2];
m_u8UdpVersionMinor = (m_PacketsBuffer.getTaskEnd() - 1)->data[3]; m_u8UdpVersionMinor = (m_PacketsBuffer.getTaskEnd() - 1)->data[3];
ROS_WARN("UDP Version is:%d.%d", m_u8UdpVersionMajor, m_u8UdpVersionMinor); ROS_WARN("UDP Version is:%d.%d", m_u8UdpVersionMajor, m_u8UdpVersionMinor);
...@@ -701,7 +701,6 @@ void Convert::init() { ...@@ -701,7 +701,6 @@ void Convert::init() {
m_OutMsgArray[1] = outMag1; m_OutMsgArray[1] = outMag1;
break; break;
} }
inited = true;
} }
void Convert::publishPoints() { void Convert::publishPoints() {
...@@ -721,6 +720,101 @@ void Convert::publishPoints() { ...@@ -721,6 +720,101 @@ void Convert::publishPoints() {
// if (end - start > 150) ROS_WARN("publishPoints time:%d", end - start); // if (end - start > 150) ROS_WARN("publishPoints time:%d", end - start);
} }
void Convert::init(pandar_msgs::PandarPacket packet)
{
m_u8UdpVersionMajor = packet.data[2];
m_u8UdpVersionMinor = packet.data[3];
uint16_t lidarmotorspeed = 0;
ROS_WARN("UDP Version is:%d.%d", m_u8UdpVersionMajor, m_u8UdpVersionMinor);
switch (m_u8UdpVersionMajor){
case 1:
switch (m_u8UdpVersionMinor)
{
case 3:
{
Pandar128PacketVersion13 packet;
memcpy(&packet, &(packet.data[0]), sizeof(Pandar128PacketVersion13));
m_iWorkMode = packet.tail.nShutdownFlag & 0x03;
m_iReturnMode = packet.tail.nReturnMode;
lidarmotorspeed = packet.tail.nMotorSpeed;
m_iLaserNum = packet.head.u8LaserNum;
m_iFirstAzimuthIndex = PANDAR128_HEAD_SIZE;
m_iLastAzimuthIndex = PANDAR128_HEAD_SIZE + PANDAR128_BLOCK_SIZE;
}
break;
case 4:
{
auto header = (Pandar128HeadVersion14*)(&(packet.data[0]));
auto tail = (Pandar128TailVersion14*)(&(packet.data[0]) + PANDAR128_HEAD_SIZE +
header->unitSize() * header->u8LaserNum * header->u8BlockNum +
PANDAR128_AZIMUTH_SIZE * header->u8BlockNum +
PANDAR128_CRC_SIZE +
(header->hasFunctionSafety()? PANDAR128_FUNCTION_SAFETY_SIZE : 0));
m_iWorkMode = tail->getOperationMode();
m_iReturnMode = tail->nReturnMode;
lidarmotorspeed = tail->nMotorSpeed;
m_iLaserNum = header->u8LaserNum;
m_iFirstAzimuthIndex = PANDAR128_HEAD_SIZE;
m_iLastAzimuthIndex = PANDAR128_HEAD_SIZE +
(header->hasConfidence() ? PANDAR128_UNIT_WITH_CONFIDENCE_SIZE * header->u8LaserNum * (header->u8BlockNum - 1) : PANDAR128_UNIT_WITHOUT_CONFIDENCE_SIZE * header->u8LaserNum * (header->u8BlockNum - 1)) +
PANDAR128_AZIMUTH_SIZE * (header->u8BlockNum - 1);
}
break;
default:
break;
}
case 3:
switch (m_u8UdpVersionMinor)
{
case 2:
{
auto header = (PandarQT128Head*)(&(packet.data[0]));
auto tail = (PandarQT128Tail*)(&(packet.data[0]) + PANDAR128_HEAD_SIZE +
(header->hasConfidence() ? PANDAR128_UNIT_WITH_CONFIDENCE_SIZE * header->u8LaserNum * header->u8BlockNum : PANDAR128_UNIT_WITHOUT_CONFIDENCE_SIZE * header->u8LaserNum * header->u8BlockNum) +
PANDAR128_AZIMUTH_SIZE * header->u8BlockNum +
PANDAR128_CRC_SIZE +
(header->hasFunctionSafety()? PANDAR128_FUNCTION_SAFETY_SIZE : 0));
m_iWorkMode = tail->nShutdownFlag & 0x03;
m_iReturnMode = tail->nReturnMode;
lidarmotorspeed = tail->nMotorSpeed;
m_iLaserNum = header->u8LaserNum;
m_iFirstAzimuthIndex = PANDAR128_HEAD_SIZE;
m_iLastAzimuthIndex = PANDAR128_HEAD_SIZE +
(header->hasConfidence() ? PANDAR128_UNIT_WITH_CONFIDENCE_SIZE * header->u8LaserNum * (header->u8BlockNum - 1) : PANDAR128_UNIT_WITHOUT_CONFIDENCE_SIZE * header->u8LaserNum * (header->u8BlockNum - 1)) +
PANDAR128_AZIMUTH_SIZE * (header->u8BlockNum - 1);
m_PacketsBuffer.m_stepSize = PANDARQT128_TASKFLOW_STEP_SIZE;
if(header->isSelfDefine()){
loadChannelConfigFile();
}
loadFireTimeFile();
}
break;
default:
break;
}
default:
break;
}
if(abs(lidarmotorspeed - MOTOR_SPEED_600) < 100) { //ignore the speed gap of 6000 rpm
lidarmotorspeed = MOTOR_SPEED_600;
}
else if(abs(lidarmotorspeed - MOTOR_SPEED_1200) < 100) { //ignore the speed gap of 1200 rpm
lidarmotorspeed = MOTOR_SPEED_1200;
}
else {
lidarmotorspeed = MOTOR_SPEED_600; //changing the speed,give enough size
}
m_iMotorSpeed = lidarmotorspeed;
printf("init mode: workermode: %x,return mode: %x,speed: %d\n",m_iWorkMode, m_iReturnMode, m_iMotorSpeed);
changeAngleSize();
changeReturnBlockSize();
checkClockwise();
boost::shared_ptr<PPointCloud> outMag0(new PPointCloud(CIRCLE_ANGLE / m_iAngleSize * m_iLaserNum * m_iReturnBlockSize, 1));
boost::shared_ptr<PPointCloud> outMag1(new PPointCloud(CIRCLE_ANGLE / m_iAngleSize * m_iLaserNum * m_iReturnBlockSize, 1));
m_OutMsgArray[0] = outMag0;
m_OutMsgArray[1] = outMag1;
}
void Convert::publishPointsThread() { void Convert::publishPointsThread() {
sched_param param; sched_param param;
param.sched_priority = 90; param.sched_priority = 90;
......
libs/locate_system/adjust_ppk_by_locate/convert.h
View file @ 5323041e
...@@ -396,6 +396,7 @@ class Convert { ...@@ -396,6 +396,7 @@ class Convert {
void publishPoints(); void publishPoints();
private: private:
void init(pandar_msgs::PandarPacket packet);
void processGps(const pandar_msgs::PandarGps::ConstPtr &gpsMsg); void processGps(const pandar_msgs::PandarGps::ConstPtr &gpsMsg);
int parseData(Pandar128PacketVersion13 &pkt, const uint8_t *buf, const int len); int parseData(Pandar128PacketVersion13 &pkt, const uint8_t *buf, const int len);
...@@ -482,7 +483,7 @@ class Convert { ...@@ -482,7 +483,7 @@ class Convert {
bool m_bCoordinateCorrectionFlag; bool m_bCoordinateCorrectionFlag;
PandarQTChannelConfig m_PandarQTChannelConfig; PandarQTChannelConfig m_PandarQTChannelConfig;
boost::function<void(const PPointCloud&)> cb_; bool inited = false;
}; };
......
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