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(
m_OutMsgArray[0]->clear();
m_OutMsgArray[0]->resize(CIRCLE_ANGLE / m_iAngleSize * m_iLaserNum * m_iReturnBlockSize );
for (size_t next = 0; next < scanMsg->packets.size(); ++next) {
if(!inited){
init(scanMsg->packets[next]);
inited = true;
}
calcPointXYZIT(scanMsg->packets[next], 0);
}
m_OutMsgArray[0]->header.frame_id = m_sFrameId;
......@@ -599,16 +603,12 @@ void Convert::moveTaskEndToStartAngle() {
}
void Convert::init() {
static bool inited = false;
if(inited){
return;
}
while (1) {
if(!m_PacketsBuffer.hasEnoughPackets()) {
usleep(1000);
continue;
}
uint16_t lidarmotorspeed = 0;
}
uint16_t lidarmotorspeed = 0;
m_u8UdpVersionMajor = (m_PacketsBuffer.getTaskEnd() - 1)->data[2];
m_u8UdpVersionMinor = (m_PacketsBuffer.getTaskEnd() - 1)->data[3];
ROS_WARN("UDP Version is:%d.%d", m_u8UdpVersionMajor, m_u8UdpVersionMinor);
......@@ -701,7 +701,6 @@ void Convert::init() {
m_OutMsgArray[1] = outMag1;
break;
}
inited = true;
}
void Convert::publishPoints() {
......@@ -721,6 +720,101 @@ void Convert::publishPoints() {
// 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() {
sched_param param;
param.sched_priority = 90;
......
libs/locate_system/adjust_ppk_by_locate/convert.h
View file @ 5323041e
......@@ -396,6 +396,7 @@ class Convert {
void publishPoints();
private:
void init(pandar_msgs::PandarPacket packet);
void processGps(const pandar_msgs::PandarGps::ConstPtr &gpsMsg);
int parseData(Pandar128PacketVersion13 &pkt, const uint8_t *buf, const int len);
......@@ -482,7 +483,7 @@ class Convert {
bool m_bCoordinateCorrectionFlag;
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