diff --git a/README.md b/README.md index 85ceb25..573618c 100644 --- a/README.md +++ b/README.md @@ -9,7 +9,7 @@ Developed based on [HesaiLidar_Swift_SDK](https://github.com/HesaiTechnology/Hes ``` * master: Pandar LiDAR ROS driver for Ubuntu 18.04 and Ubuntu20.04 supports the latest UDP protocol v1.4 v1.3 and v3.2 * UDP1.4_ubuntu16.04: Pandar LiDAR ROS driver for Ubuntu 16.04 supports the latest UDP protocol v1.4 -* UDP1.3: Pandar LiDAR ROS driver for ubuntu16.04,ubuntu 18.04 and Ubuntu 20.04 supports UDP protocol v1.3 +* UDP4.3: Pandar LiDAR ROS driver for Ubuntu 16.04/Ubuntu 18.04/Ubuntu 20.04 supports the latest UDP protocol v4.3 To get the UDP protocol version number of your device, check the UDP package header field. ``` diff --git a/change notes.md b/change notes.md index d750b5a..d4cee45 100644 --- a/change notes.md +++ b/change notes.md @@ -146,5 +146,12 @@ PandarSwiftROS_1.0.36 ##modify 1. Support multicast +Wednesday December 14th, 2022 17:30 +##version +PandarSwiftROS_1.0.37 + +##modify +1. Support save pcd + diff --git a/pandar_pointcloud/src/conversions/convert.cc b/pandar_pointcloud/src/conversions/convert.cc index 17a25a3..a2c5fc3 100644 --- a/pandar_pointcloud/src/conversions/convert.cc +++ b/pandar_pointcloud/src/conversions/convert.cc @@ -86,7 +86,7 @@ Convert::Convert(ros::NodeHandle node, ros::NodeHandle private_nh, : data_(new pandar_rawdata::RawData()), drv(node, private_nh, node_type, this) { - m_sRosVersion = "PandarSwiftROS_1.0.36"; + m_sRosVersion = "PandarSwiftROS_1.0.37"; ROS_WARN("--------PandarSwift ROS version: %s--------\n\n",m_sRosVersion.c_str()); publishmodel = ""; @@ -344,7 +344,7 @@ int Convert::loadChannelConfigFile(std::string channel_config_content){ m_PandarQTChannelConfig.m_u8MinVersion = std::stoi(versionLine[2].c_str()); } else{ - std::cout << "channel config file delimiter is wrong" << versionLine[0]; + std::cout << "channel config file delimiter is wrong" << versionLine[0] << std::endl; return -1; } std::getline(ifs, line); @@ -366,13 +366,17 @@ int Convert::loadChannelConfigFile(std::string channel_config_content){ std::getline(ifs, line); std::vector ChannelLine; boost::split(ChannelLine, line, boost::is_any_of(",")); + if (ChannelLine.size() != loop_num) { + std::cout << "channel config file format is wrong" << std::endl; + return -1; + } for(int j = 0; j < loop_num; j++){ if(ChannelLine.size() == loop_num){ m_PandarQTChannelConfig.m_vChannelConfigTable[j][i] = std::stoi(ChannelLine[j].c_str()); // printf("%d %d \n",i, m_PandarQTChannelConfig.m_vChannelConfigTable[j][i]); } else{ - std::cout << "loop num is not equal to the first channel line"; + std::cout << "loop num is not equal to the first channel line" << std::endl; return -1; } @@ -772,7 +776,7 @@ void Convert::init() { PANDAR128_AZIMUTH_SIZE * header->u8BlockNum + PANDAR128_CRC_SIZE + (header->hasFunctionSafety()? PANDAR128_FUNCTION_SAFETY_SIZE : 0)); - m_iWorkMode = tail->nShutdownFlag & 0x03; + m_iWorkMode = tail->getOperationMode(); m_iReturnMode = tail->nReturnMode; drv.setUdpVersion(m_u8UdpVersionMajor, m_u8UdpVersionMinor); lidarmotorspeed = tail->nMotorSpeed; @@ -953,7 +957,7 @@ int Convert::checkLiadaMode() { PANDAR128_AZIMUTH_SIZE * header->u8BlockNum + PANDAR128_CRC_SIZE + (header->hasFunctionSafety()? PANDAR128_FUNCTION_SAFETY_SIZE : 0)); - lidarworkmode = tail->nShutdownFlag & 0x03; + lidarworkmode = tail->getOperationMode(); lidarreturnmode = tail->nReturnMode; lidarmotorspeed = tail->nMotorSpeed; laserNum = header->u8LaserNum; @@ -1221,9 +1225,8 @@ void Convert::calcPointXYZIT(pandar_msgs::PandarPacket &packet, int cursor) { // ROS_WARN("#####block.fAzimuth[%u]",u16Azimuth); index += PANDAR128_AZIMUTH_SIZE; - int mode = tail->nShutdownFlag & 0x03; - int state = (tail->nShutdownFlag & 0xF0) >> 4; - + int mode = tail->getOperationMode(); + int state = tail->getAngleState(blockid); for (int i = 0; i < header->u8LaserNum; i++) { /* for all the units in a block */ uint16_t u16Distance = *(uint16_t*)(&packet.data[0] + index); @@ -1343,7 +1346,7 @@ void Convert::calcQT128PointXYZIT(pandar_msgs::PandarPacket &packet, int cursor) int index = 0; index += PANDAR128_HEAD_SIZE; for (int blockid = 0; blockid < header->u8BlockNum; blockid++) { - int loopIndex = blockid; + int loopIndex = (tail->nModeFlag + (blockid / ((tail->nReturnMode < 0x39) ? 1 : 2)) + 1) % header->u8BlockNum; if((isSelfDefine && m_PandarQTChannelConfig.m_bIsChannelConfigObtained)){ loopIndex = (tail->nModeFlag + (blockid / ((tail->nReturnMode < 0x39) ? 1 : 2)) + 1) % m_PandarQTChannelConfig.m_vChannelConfigTable.size(); } diff --git a/pandar_pointcloud/src/conversions/convert.h b/pandar_pointcloud/src/conversions/convert.h index 9b3359b..a3769be 100644 --- a/pandar_pointcloud/src/conversions/convert.h +++ b/pandar_pointcloud/src/conversions/convert.h @@ -235,6 +235,10 @@ typedef struct __attribute__((__packed__)) Pandar128TailVersion14_s { uint32_t nTimestamp; uint8_t nFactoryInfo; uint32_t nSeqNum; + inline uint8_t getAngleState(int blockIndex) const { + return (nAzimuthFlag >> (2 * (7 - blockIndex))) & 0x03; + } + inline uint8_t getOperationMode() const { return nShutdownFlag & 0x0f; } } Pandar128TailVersion14; typedef struct __attribute__((__packed__)) PandarQT128Tail_s { @@ -250,6 +254,10 @@ typedef struct __attribute__((__packed__)) PandarQT128Tail_s { uint32_t nTimestamp; uint8_t nFactoryInfo; uint32_t nSeqNum; + inline uint8_t getAngleState(int blockIndex) const { + return (nAzimuthFlag >> (2 * (7 - blockIndex))) & 0x03; + } + inline uint8_t getOperationMode() const { return nShutdownFlag & 0x0f; } } PandarQT128Tail; typedef struct __attribute__((__packed__)) Pandar128PacketVersion13_t { @@ -323,6 +331,13 @@ typedef struct PacketsBuffer_s { lastOverflowed = false; ROS_WARN("buffer recovered"); } + if(((m_iterPush > m_iterTaskEnd) && (m_iterPush - m_iterTaskEnd) > 4 * m_stepSize) || + ((m_iterPush < m_iterTaskBegin) && (m_iterTaskBegin - m_iterPush) < CIRCLE - 4 * m_stepSize)){ + + while((((m_iterPush > m_iterTaskEnd) && (m_iterPush - m_iterTaskEnd) > 4 * m_stepSize) || + ((m_iterPush < m_iterTaskBegin) && (m_iterTaskBegin - m_iterPush) < CIRCLE - 4 * m_stepSize))) + usleep(1000); + } *(m_iterPush++) = pkt; return 1; }