Created by [ Frank Yan], last modified on Jan 05, 2021
The purpose of the publisher node is to convert the raw radar data to ROS messages. This allows us to manipulate the radar data via ROS messages. [How to launch this is documented inside the ][ file in the radar_driver/ directory. This document is for the purpose of explaining the functionality. The publisher node is composed of two layers:]
Each layer has its own C++ file in the radar_driver/directory named sniffer.cpp and parser.cpp accordingly.
The sniffer layer is responsible for collecting the data from the radars. It is also responsible for setting up the network information for the radars.
Sniffer Header File
int run(int port, int packets, char interface[], const char filter[], int capture_live, const char *capture_path);
Once the sniffer receives the raw data from the radars, the parser is responsible for getting the useful data from the raw data. Once it gets the data it will then put this data into packets. Packets are blocks of data that are transmitted over a network. We have defined our own packet called RadarPacket to group the useful radar data. To begin let's take a look at this layer's header file, parser.h.
Parser Header File
//Packet Cap Definitions: If we are using live capture or from pcap doc
#define OFFLINE 0
#define LIVE 1
//Service ID Definitions
#define RDI_PACKET_ID 220
#define SS_PACKET_ID 200
// Event ID Definitions
#define FAR0 1
#define FAR1 2
#define NEAR0 3
#define NEAR1 4
#define NEAR2 5
By default SS packets have a service ID of 200 and all radar packets will have a service ID of 220. A service ID is an ID assigned to a service/application and we use these to identify radar functions and packets. To differentiate between radar packets as they all have the same service ID, event IDs are defined depending on whether the radars are configured with only far or near scan, or far and near scan.
//General Definitions
#define SENSOR_SERIAL_NUM_LEN 26
#define RDI_ARRAY_LEN 38
#define RDI_ARRAY_LEN_NEAR_2 32
//Size Defs
#define NUM_FAR 12
#define NUM_NEAR 18
The general definitions define the length of three arrays which will be used later on. The size definitions will determine how many packets are configured for each far and near scans. In a detection with both near and far scan configured, we will have a total of 30 packets. 12 packets are reserved for far packets and 18 packets are reserved for near packets. The reasoning behind this is near packets will pick up more data than far packets as near packets cover more area than far packets.
Masks
#define SIDELOBE_PROB_MASK 0x04
#define INFERENCE_PROB_MASK 0x02
#define NEAR_PROB_MASK 0x01
// flag masks for SS Packet fields.
#define DEFECTIVE_HW 0x01
#define BAD_VOLTAGE 0x01
#define BAD_TEMP 0x01
#define GM_MISSING 0x01
#define POWER_REDUCED 0x01
Not sure why we have these "masks" if they are not being used for masking. See the note below.
Flag Masks Usage
[]{.aui-icon .aui-icon-small .aui-iconfont-warning .confluence-information-macro-icon}
I actually have not seen any section of code that use these flags for the purpose of masking... The only time we use them these masks is in conditionals like:
if (packet->Defective && DEFECTIVE_HW) {
return SS_DEFECTIVE_HW;
Even in this conditional this seems redundant because DEFECTIVE_HW is always going to be 1 or evaluate to true. Doing any 'and' operation with a true value is redundant as it will be the same as the statement itself.
Network Structures
//General Struct Defs for the Radar UDP Packets
typedef struct udphdr udphdr_t; //udp.h struct, typedef for clarity
//SOME/IP Header
typedef struct packetHeader
{
uint8_t service_ID;
uint8_t event_ID;
uint32_t length;
//request ID is split into client ID (16)and subscriber ID(16)
uint32_t request_ID;
uint8_t protocol_ver;
uint8_t interface_ver;
uint8_t message_Type;
uint8_t return_Code;
} packetHeader_t;
UDP is a network protocol that stands for User Datagram Protocol. UDP works by transferring data without having the receiver agree to receiving the data. When the car is moving and the radars are on, we want to get the real time data as quickly as possible. Click here to learn more about UDP.
For a more in depth document about SOME/IP click here. However, in a nutshell SOME/IP is a protocol used in embedded and automotive industries. How SOME/IP works is that it sends messages between devices/subscribers over IP and Ethernet.
Below are some custom structures we use to create our packets. The payloadHeader structure contains meta-information about the received package such as the timestamp, event ID, radar port, etc.
payloadHeader Structure
typedef struct payloadHeader
{
uint8_t EventID;
uint16_t CRC;
uint16_t Len;
uint8_t SQC;
uint8_t MessageCounter;
uint64_t UTCTimeStamp;
... //More meta-information
The packetRDIRawData and packetRDIData structures are also custom structures we define in the header file. These structures are responsible for packaging the information we want to parse from the radars' raw data such as the azimuth angles, range, etc. The difference between the two structures is the variables for packetRDIRawData are integers while packetRDIData variables are floats.
packetRDIRawData Structure
typedef struct packetRDIRawData {
uint16_t f_Range;
int16_t f_VrelRad;
int16_t f_AzAng0;
int16_t f_AzAng1;
int16_t f_ElAng;
... //Important metrics
Taking all this information is the RDIPacket structure which takes both types of packets and the payloadHeaderData, and puts it into one giant structure.
RDIPacket Structure
typedef struct RDIPacket{
payloadHeader_t payloadHeaderData;
packetRDIRawData_t listDataArray[RDI_ARRAY_LEN];
packetRDIData_t resListDataArray[RDI_ARRAY_LEN];
} RDIPacket_t;
Similarly we have a SSPacket structure. It is slightly different than the RDI packet but it has diagnostic and meta information about the sensors such as error checks for bad voltage and the serial number of the sensor.
Sensor Status Structure
//Sensor Status Data, slightly different structure than RDI packet
typedef struct SSPacket {
uint16_t CRC;
uint16_t len;
uint8_t SQC;
uint16_t CRC;
uint16_t len;
uint8_t SQC;
uint64_t PartNumber;
uint64_t AssemblyPartNumber;
uint64_t SWPartNumber;
uint8_t SerialNumber[SENSOR_SERIAL_NUM_LEN];
... //More metrics
uint8_t BadSupplyVolt;
... //Even more metrics
The final structure we have is the PacketGroup. This is used for our double buffer in the processPacket.cpp file. For more information check out the processor page to see in more detail on where this is used.
Packet Group Structure
//Static # of near or far packets, dynamic # of detections per packet
typedef struct PacketGroup {
radar_driver::RadarPacket nearPackets[NUM_NEAR];
radar_driver::RadarPacket farPackets[NUM_FAR];
uint8_t numFarPackets;
uint8_t numNearPackets;
} PacketGroup_t;
There are a lot of other functions defined in the header file but we will take the time to explain these functions in the upcoming section, where we will discuss the parser.cpp file.
Since we are using ROS, it is natural to have the node handle for the publisher of this layer. The function below starts the publisher which publishes the topic, unfiltered_radar_packet. There are also some initializations for the resolution multipliers. These are used later on to determine how fine/accurate the data is.
ROS Publisher and Initializations
//Resolution Multipliers, make static const global since never changing
static const resMultipliersRDI_t resRDIMults;
static const resMultipliersSS_t resSSMults;
//Unfiltered ROS publisher
static ros::Publisher unfilteredPublisher;
void initUnfilteredPublisher(ros::NodeHandle nh) {
unfilteredPublisher = nh.advertise<radar_driver::RadarPacket>("unfiltered_radar_packet", 50);
}
Now the main function that does the parsing is a big one that does a lot of bit shifting. It does a lot of bit shifting, so I will just go over the main points and won't copy and paste the entire thing since it's a lot. The basic idea behind this is that we create a custom header structure (the SOME/IP defined one) to get the event IDs, service IDs, etc from the packet pointer.
Parsing Function
uint8_t parse_packet(udphdr_t* udphdr, unsigned char* packetptr) {
packetHeader_t Header; //Create the header locally
//128 bit (16 Byte) Packet Header
//Were not really using this, decoding for completion's sake
Header.service_ID = (packetptr[8] << 8) | (packetptr[9]); //Big Endian
Header.event_ID = (packetptr[10] << 8) | packetptr[11];
... //More bit-shifting
Then we do even more bit shifting except we are now extracting information for the RDI packet structure.
RDI Packet Parsing
if (Header.service_ID == RDI_PACKET_ID) {
if (packetptr[DETECTIONS_IN_ELEMENT_BYTE_POS] == 0) {
return NO_DETECTIONS;
}
RDIPacket_t RDI_Packet; //Create RDI Packet struct
//256 Bit (32 Byte) Payload Header
RDI_Packet.payloadHeaderData.EventID = Header.event_ID; //Duplicated b/c useful later
RDI_Packet.payloadHeaderData.CRC = (packetptr[24] << 8) | (packetptr[25]);
RDI_Packet.payloadHeaderData.Len = (packetptr[26] << 8) | (packetptr[27]);
... //Keep on bit-shifting
Next we parse the actual data from the detections for the RDI packet. This will give info such as range, azimuth angles, RCS, etc. We also determine the resolution of the data received after parsing the data.
//7168 - 8512 Bit (896 - 1064 Byte) Radar Dection Payload. Size depends on eventID.
int listStartIndex = 56;
for (int i = 0; i < RDI_Packet.payloadHeaderData.DetectionsInPacket; i++) { //Only fill as many as we have valid packets, dont bother filling 0's
//Radar Detection Images are 224 Bits (28 Bytes) each
RDI_Packet.listDataArray[i].f_Range = (packetptr[listStartIndex++] << 8) | (packetptr[listStartIndex++]);
RDI_Packet.listDataArray[i].f_VrelRad = (packetptr[listStartIndex++] << 8) | (packetptr[listStartIndex++]);
... //Bit-shifting
...
... //Skip a couple of lines but still in the same loop
//Add in the proper resolution
RDI_Packet.resListDataArray[i].f_Range = RDI_Packet.listDataArray[i].f_Range * resRDIMults.f_RangeRes;
RDI_Packet.resListDataArray[i].f_VrelRad = RDI_Packet.listDataArray[i].f_VrelRad * resRDIMults.f_VrelRadRes;
... //More resolution stuff
Once all the parsing is done we finally publish the packet. The publishRDIPacket function will be discussed later. Also we create the sensor status packet. The rest of the code in this conditional block is doing the same thing as the RDI packet block except it will have different metrics and parameters that will need to be bit-shifted. However, we do not publish the sensor status packet. We are keeping them to help give diagnostic and meta information about the radars, just to make sure everything is working properly with the radars themselves.
//Send packet to get published
publishRDIPacket(&RDI_Packet);
} else if (Header.service_ID == SS_PACKET_ID) {
#ifdef PARSE_SS
SSPacket_t SS_Packet;
SS_Packet.CRC = (packetptr[24] << 8) | (packetptr[25]);
SS_Packet.len = (packetptr[26] << 8) | (packetptr[27]);
... //More bit-shifting again
Finally after the parsing is done we will return a SUCCESS status. If the service ID the packet header parsed is not an RDI Packet ID or sensor status ID, then we return a BAD_SERVICE_ID status. To learn more about what the statuses we have check out the processor page as that will go into more depth about what statuses we use.
} else {
return BAD_SERVICE_ID;
}
return SUCCESS
Within the parsing function we call the publishRDIPacket function once we are done parsing all the necessary information. We create a local variable called 'msg' which will be used later in the loadPacketMsg function. Once we do that we publish the message using the publisher we defined earlier.
Publish RDI Packet
void publishRDIPacket(RDIPacket_t * packet) {
radar_driver::RadarPacket msg;
loadPacketMsg(packet, &msg);
printf("Publishing new message: %d\n", packet->payloadHeaderData.DetectionsInPacket);
unfilteredPublisher.publish(msg);
}
The loadPacketMsg function will take in a pointer for the packet created in the parser function and a pointer for the 'msg' variable in the publish function. The importance of this function is to now load all the data from the packet in the parser function to the 'msg' structure. We are essentially saving the data once we have finished parsing.
loadPacketMsg
void loadPacketMsg(RDIPacket_t * packet, radar_driver::RadarPacket * msg) {
msg->EventID = packet->payloadHeaderData.EventID;
msg->TimeStamp = packet->payloadHeaderData.TimeStamp;
msg->MeasurementCounter = packet->payloadHeaderData.MeasurementCounter;
msg->Vambig = packet->payloadHeaderData.Vambig;
msg->CenterFrequency = packet->payloadHeaderData.CenterFrequency;
Next we extract the radar metrics from the parsed packet. We ignore error flags in the packet.
for(uint8_t i = 0; i < packet->payloadHeaderData.DetectionsInPacket; i++) {
radar_driver::RadarDetection data;
//Filter through error flags in packet
if (packet->resListDataArray[i].ui_Pdh0 && NEAR_PROB_MASK) {
continue;
} else if (packet->resListDataArray[i].ui_Pdh0 && INFERENCE_PROB_MASK) {
continue;
} else if (packet->resListDataArray[i].ui_Pdh0 && SIDELOBE_PROB_MASK) {
continue;
}
data.Pdh0 = packet->resListDataArray[i].ui_Pdh0;
data.AzAng0 = packet->resListDataArray[i].f_AzAng0;
data.RCS0 = packet->resListDataArray[i].f_RCS0;
... //More data extraction
Error Flag Usage
[]{.aui-icon .aui-icon-small .aui-iconfont-warning .confluence-information-macro-icon}
I'm not sure why we ignore error flags... I think it's because we want to publish the packet regardless of the error flags?
The only calculated metrics are the X and Y positions. Everything before that is a direct extraction. Once we are finished copying over the data we add the local data array to the detections array of the msg structure.
... //Continued data extraction
// Updated calculations based on email thread with Conti
// AzAng0 should always be the better of the two assupmtions
data.posX = packet->resListDataArray[i].f_Range * cos(data.AzAng0);
data.posY = packet->resListDataArray[i].f_Range * sin(data.AzAng0);
data.posZ = 0.0; // Z really should not be in your interface.
msg->Detections.push_back(data);
}
}
Everything is now done with the parser/publisher nodes. The next node that will receive the topics for data processing and filtering is the processor.
unfilteredMain.cpp file
[]{.aui-icon .aui-icon-small .aui-iconfont-warning .confluence-information-macro-icon}
There is a unfilteredMain.cpp file but since we are using roslaunch, the file isn't used as much. The reason why you would use it, is if you are only running this node through the command line without doing a roslaunch.
\
If it seems like a lot of code was skipped it is because a lot of code was skipped. This is mostly because most of it is rather straight-forward. This document is to help members get a better understanding of what is going on with the software stack. I also skipped any code that was not currently being used. I did make a lot of assumptions on why we have our code the way it is so again a fact-check is required for this document.
If there is anything on this page that needs to be rewritten due to it not being clear or having outdated information, please contact the radar-driver group in Discord!
\
Document generated by Confluence on Nov 28, 2021 22:40