Created by [ Frank Yan], last modified on Dec 27, 2020
After the publisher node converts all the radar data to ROS messages, we then need to clean up all this data and make it more organized. The processor node is responsible for grouping the data and deleting any data with thresholds. How to launch this is documented inside the README.md file in the radar_driver repo This document is for the purpose of explaining the functionality. The processor node is composed of two layers:
Each layer has its own C++ file in the src directory of the radar_driver repo. The processing layer is called processPacket.cpp and the ROS publisher is called radarPublisher.cpp.
This layer is responsible for creating a publisher object to publish packets to the next node. Let's begin with the class declaration for this layer which can be found in radarPublisher.h.
Publisher Class
class RadarPublisher {
private:
ros::NodeHandle nh_; //Node Handle
// Publisher Objects
ros::Publisher packet_pub_;
ros::Publisher detection_pub_; //Not used now, kept if useful
//Publisher Functions
void publishRadarPacketMsg(radar_driver::RadarPacket& radar_packet_msg);
void publishRadarDetectionMsg(radar_driver::RadarDetection& radar_detection_msg);
public:
RadarPublisher(ros::NodeHandle new_nh_);
uint8_t setupProcessor(uint8_t scanMode);
uint8_t pubCallback(PacketGroup_t* Packets);
};
The detection_pub_ and publishRadarDetectionMsg are not used right now so just ignore them. The main functions that the publisher has is the pubCallback and publishRadarPacketMsg. These functions are responsible for sending the packets to the next node.
Set Up Processor
uint8_t RadarPublisher::setupProcessor(uint8_t scanMode)
{
uint8_t err = PacketProcessor::initializePacketProcessor(this, scanMode);
if (err) {
printf("Failed to Start Processor: %d\r\n", err);
return err; //Processor Failed to Init
}
return SUCCESS;
}
First thing to make sure publishing is good to go is to check if the processor initialized correctly. If the processor did not initialize properly then we return an error. If no issues are found then we return SUCCESS. The initializePacketProcessor function is discussed in the packet processor layer.
void RadarPublisher::publishRadarPacketMsg(radar_driver::RadarPacket& radar_packet_msg)
{
return packet_pub_.publish(radar_packet_msg);
}
uint8_t RadarPublisher::pubCallback(PacketGroup_t* Packets) { //call upon the appropriate publish function
for (uint8_t i = 0; i < Packets->numFarPackets; i++) {
this->publishRadarPacketMsg(Packets->farPackets[i]);
}
for (uint8_t i = 0; i < Packets->numNearPackets; i++) {
this->publishRadarPacketMsg(Packets->nearPackets[i]);
}
return SUCCESS;
}
The first function sends out an individual packet to the next node while the second function, pubCallback, loops through the entire group of packets for each near and far packets and uses the first function to send each packet in the group. This function will be called in the main function later on where the node is created.
In short, the packet processor is responsible for filtering and grouping the data. This layer is quite involved so beginning with the header file, processPacket.h, for this layer is a good start.
Packet Processor Return Values
//Return Values
#define SUCCESS 0
#define NO_DETECTIONS 1
#define PUBLISH_FAIL 2
#define CLEAR_FAIL 3
#define BAD_EVENT_ID 4
#define NO_PUBLISHER 5
#define INIT_FAIL 6
#define BAD_PORT 7
#define BAD_SERVICE_ID 8
#define NO_PUBLISHER 9
#define NO_PUB_CLR_FAIL 10
#define FALSE_DETECTION_1 11
#define FALSE_DETECTION_2 12
#define FALSE_DETECTION_3 13
#define TOO_MUCH_NOISE 14
#define SS_DEFECTIVE_HW 15
#define SS_BAD_VOLT 16
#define SS_BAD_TEMP 17
#define SS_GM_MISSING 18
#define SS_PWR_REDUCED 19
#define NO_PROCESS 20
There are a lot of return values for the radars. However, this is to help cover all possible cases. Each return value will give a status update on the radars and depending on what the status of the radars are, the return value will match what the radars are experiencing. We won't go over all 21 values as most of these cases are not used all that often so don't worry if this seems like a lot. SUCCESS and NO_DETECTIONS are the most common two return values for the radars.
// Logging Verbosity Levels
enum LoggingLevel ;
//Scan mode
enum ScanMode ; // 0=Near&Far, 1=NearOnly, 2=FarOnly
The next two lines of code are dedicated to defining enums. Logging levels are used for debugging purposes and raising alarms via rqt_console and terminal. To learn more about verbosity levels check out the ROS logging page. These logging levels will be used later on when processing the radar data.
Configuring the scan mode for a radar will determine its FOV. The default scan mode for the radars is near and far scan. Far scan will configure the radar's FOV to be +/- nine degrees with a range of up to 250 metres. Near scan will configure the radar's FOV to be +/- 75 degrees with a range of up to 70 metres. The other two modes are not frequently used but are still defined in the enum in case we need a radar to have only a far or near scan.
Filter Thresholds
//Filter Thresholds
#define SNR_THRESHOLD 3 //dBr
#define VELOCITY_LOWER_THRESHOLD 0.0 // m/s
#define RCS_THRESHOLD -25 //(dBm)^2
#define DISTANCE_MAX_THRESHOLD 20 //m
#define DISTANCE_MIN_THRESHOLD 0.0 //m
#define AZI_ANGLE_0_THRESHOLD -1.44862 //radians, 83 degrees Azi Angle 0 is the angle from the left center
#define AZI_ANGLE_1_THRESHOLD 1.44862 //Azi Angle 1 is the angle from the right center, positive according to Conti
These thresholds are used when deciding on what objects should be dropped. If an object does not meet any of these thresholds because it either falls short or is way out of bounds, then that object is simply not going to be detected. Check the radar signal definitions page to get a better understanding of what each threshold does.
PacketProcessor Class
class PacketProcessor {
private:
static class RadarPublisher* Publisher; //Publisher Object for Callback
static pthread_mutex_t Mutex; //Mutex for synchronization
static PacketGroup_t PacketsBuffer[2]; //Double buffer
static uint8_t curNearIdx, curFarIdx; //Double buffer indexes
static uint32_t curNearTimeStamp, curFarTimeStamp; //Time Stamps
static bool publish;
static uint8_t scanMode; // 0=Near&Far, 1=NearOnly, 2=FarOnly
static uint8_t clearPackets(uint8_t idx); //Don't expose these
static uint8_t clearAllPackets();
static uint8_t publishPackets(uint8_t idx);
public:
static uint8_t initializePacketProcessor(RadarPublisher* newPublisher, uint8_t newscanMode);
static uint8_t processRDIMsg(const radar_driver::RadarPacket::ConstPtr& packet);
static uint8_t processSSPacket(SSPacket_t* packet);
static void setPublisherCallback(RadarPublisher* newPublisher);
/* Print the index buffer chosen */
static void printPacketGroup(uint8_t idx);
There is a lot of stuff in this class but do not worry, it will all make more sense when these variables and functions are explained in the processPacket.cpp file.
Publisher Object
[]{.aui-icon .aui-icon-small .aui-iconfont-warning .confluence-information-macro-icon}
I'm not sure why we have a publisher callback. It is not even used in any file other than processPacket.cpp. Same for printing the packet group, that function is not used either.
Initializations of variables
pthread_mutex_t PacketProcessor::Mutex = PTHREAD_MUTEX_INITIALIZER; //Non recursive
PacketGroup_t PacketProcessor::PacketsBuffer[2]; //Init Double Buffer
RadarPublisher * PacketProcessor::Publisher = NULL; //Publisher object for callback
bool PacketProcessor::publish = false; //Publishing flag
static PacketGroup_t EmptyPackets; //Local static only, Cleanup struct
uint32_t PacketProcessor::curNearTimeStamp = 0, PacketProcessor::curFarTimeStamp = 0;
uint8_t PacketProcessor::curNearIdx = 0, PacketProcessor::curFarIdx = 0;
uint8_t PacketProcessor::scanMode = NEAR_FAR_SCAN; // 0=Near&Far, 1=NearOnly, 2=FarOnly
To begin the initializations, the mutex is set to the default values. In a nutshell, the mutex class is a class that can lock or unlock certain sections of code. The reason why you want this is to avoid collisions between other nodes as other nodes may be updating/using the same variables. For the radar-driver code, this is important for synchronizing the radar data when grouping everything together. Setting the mutex to default means that it is static and that its parameters will be set to zero including pointers. For more info about mutex click here.
The next line has a double buffer. A buffer is a section of computer memory that can hold data while a process is using/moving that data. A double buffer is composed of two buffers and the way it works is you have two pointers that are constantly swapping places between the two sections of memory. The reason why we have a double buffer is to organize the radar data. This helps organize the data because we have a way of determining of what data is old and new based on their timestamps of when they enter the double buffer. We do not have to wait for the old radar data to finish publishing before we can process the new radar data. It acts as somewhat of a queue for the packets. To explain more, when the buffer is first created a group of packets will come in and start filling up one buffer. When a new group of packets come in they go to the second buffer while the first buffer is finishing receiving the packets. Click on this link to get a visualization of our double buffer.
After the buffer line, we have a pointer that points to the publisher object. We set this pointer to null to prevent any wild pointers. Next there is a publishing flag that will turn on when the node wants to publish and turns off when the node is not publishing anything.
The EmptyPackets struct is the default structure for the PacketGroup_t structure. This structure is used to reset a packet to its default values.
Finally, everything else is set to zero. The curNearTimeStamp and curFarTimeStamp are used in grouping the data as the timestamp will determine what order the packets will be grouped in. The index serves the same purpose when ordering the data. By default the scan mode is set to near and far.
We can finally start looking at the functions after declaring everything. The first function is the initializePacketProcessor function that takes in the pointer for the publisher object and the scan mode.
uint8_t PacketProcessor::initializePacketProcessor(RadarPublisher* newPublisher, uint8_t newscanMode) {
pthread_mutex_lock(&Mutex);
if (clearAllPackets()) { //Wipe internal struct
pthread_mutex_unlock(&Mutex);
return INIT_FAIL;
}
scanMode = newscanMode;
Publisher = newPublisher;
//All packet structs should be 0'd by default
pthread_mutex_unlock(&Mutex);
return SUCCESS;
}
You may be thinking why are we not using a constructor? The methods are all static which means they belong to the class itself and not any objects created from the class. In other words, we can call these functions without having to create an object.
Anyways in this function we lock the mutex. This prevents any other process from changing the packets and publisher. The first conditional checks to see if clearing all the packets is successful. If not then it returns an INIT_FAIL. (Remember the SUCCESS return value has an integer value of 0). If clearing the packets is successful, then the parameters from the function are copied to the variables and the mutex is unlocked. Finally the function returns a SUCCESS which means the packet processor is initialized.
Now it is time to look at how grouping/organizing the data works.
Process RDI msg
uint8_t PacketProcessor::processRDIMsg(const radar_driver::RadarPacket::ConstPtr& packet) {
if ( packet->Detections.size() < 1 ){
return NO_DETECTIONS;
}
//Only mutex here b/c not using the Packets struct yet
pthread_mutex_lock(&Mutex);
The very first thing we check for each packet is the size of it. If it is less than one, then the method returns NO_DETECTIONS. The reason why it is less than one is because when the detection vector is created there is always at least one element regardless if the radars are detecting something or not. It will only be zero if it has not been created yet. If there is a detection then the mutex will be locked to prevent any changes to the packets.
FAR Event ID
if(packet->EventID == FAR1 || packet->EventID == FAR0) {
if (scanMode != NEAR_SCAN) {
if (curFarTimeStamp == 0){ //Init Case
curFarTimeStamp = packet->TimeStamp;
} else if (curFarTimeStamp != packet->TimeStamp) { //New timestamp
curFarIdx = (curFarIdx + 1) % 2;
if (scanMode == FAR_SCAN || curFarIdx == curNearIdx) { //Indexes are the same
// We just got a new TS for both near & far, so we should publish the old buffer
publish = true;
}
}
If the radars have an event ID of FAR, then that means it has been configured with a far scan. The first conditional checks the scan mode and if it is not near then proceed with the code. I'm going to assume that since the event ID is FAR, making the scan mode near wouldn't make sense so that's probably the reasoning behind this.
Continuing with the rest of conditional statements, the first nested one checks the time stamp of the packet. If the packet has a time stamp of zero then the curFarTimeStamp variable is updated to the packet's current timestamp. This is used whenever you first launch the radars to take care of the initialization. If the timestamp is different, then the curFarIdx is set to either zero or one. I'm assuming this is because of the double buffer where the indices should alternate between zero and one in order to keep the packets updated.
We also check for the condition where a new timestamp appears for near and far at the same time. In this case we publish the old buffer to empty one of the buffers. In this case we can get the next scan with newer timestamps.
PacketGroup_t * curGroup = &PacketsBuffer[curFarIdx]; //Tmp to make code easier to read
if (loadRDIMessageFromPacket(&curGroup->farPackets[curGroup->numFarPackets], packet)) {
curGroup->numFarPackets++;
}
if (publish) {
uint8_t err = publishPackets((curFarIdx+1)%2); //Publish the previous buffer idx
publish = false;
pthread_mutex_unlock(&Mutex);
return err;
}
}
We then get the address of the buffer's current index and save it in a pointer. As the comment states, this is just to make it easier to read. We will talk about the load function in the filtering section but just know that if the first conditional is true then the number of far packets increases by one.
If the publish boolean is true then we publish the previous' buffer's index. This conditional will publish the previous buffer and set the publish boolean to false and finally unlocks the mutex. The publish packets function will be discussed later on. Lastly, we return the int err which will be zero if everything goes smoothly. (SUCCESS is zero)
Near Event ID
else if (packet->EventID == NEAR0 || packet->EventID == NEAR1 || packet->EventID == NEAR2) {
if (scanMode != FAR_SCAN) {
Fortunately the NEAR event IDs have the exact same logic and code as the FAR event IDs. The only difference with the code are the variables where 'far' is now replaced with 'near'.
Bad Event ID
} else {
pthread_mutex_unlock(&Mutex);
return BAD_EVENT_ID;
}
Obviously, if the event ID is neither far nor near, then something is clearly wrong. The mutex is unlocked and we return a BAD_EVENT_ID.
pthread_mutex_unlock(&Mutex);
return SUCCESS;
Once everything has been resolved, the mutex is unlocked and we return a success.
TLDR: Add packets based on timestamp to our double buffer.
From the grouping section, the function loadRDIMessageFromPacket will update the number of packets in the buffer if the function is successful. Let's take a look at this function.
Load RDI Message Function
bool loadRDIMessageFromPacket(radar_driver::RadarPacket* newMsg, const radar_driver::RadarPacket::ConstPtr& oldMsg) {
newMsg->EventID = oldMsg->EventID;
newMsg->TimeStamp = oldMsg->TimeStamp;
newMsg->MeasurementCounter = oldMsg->MeasurementCounter;
newMsg->Vambig = oldMsg->Vambig;
newMsg->CenterFrequency = oldMsg->CenterFrequency;
newMsg->Detections.clear();
The oldMsg arguments are copied to the newMsg's arguments and the detections array gets cleared for the newMsg. We still want to keep the oldMsg's arguments except for the detections array. This is because the detections array will be modified in the next couple of lines.
Filtering
for(uint8_t i = 0; i < oldMsg->Detections.size(); i++) {
// TODO: Figure out an SNR threshold value that actually works here.
if (oldMsg->Detections[i].SNR < SNR_THRESHOLD) { // Too much noise; drop detection.
continue;
} else if (abs(oldMsg->Detections[i].VrelRad) < VELOCITY_LOWER_THRESHOLD) {
continue;
} else if (oldMsg->Detections[i].posX > DISTANCE_MAX_THRESHOLD) { // need to do trig
continue;
} else if (oldMsg->Detections[i].posX < DISTANCE_MIN_THRESHOLD) {
continue;
} else if (oldMsg->Detections[i].RCS0 > RCS_THRESHOLD) {
continue;
} else if (oldMsg->EventID == FAR1 || oldMsg->EventID == FAR0) {
//limit far scan to 9 degrees, according to conti
if (oldMsg->Detections[i].AzAng0 < -0.15708 || oldMsg->Detections[i].AzAng1 > 0.15708) {
continue;
}
} else if(oldMsg->Detections[i].AzAng0 < AZI_ANGLE_0_THRESHOLD || oldMsg->Detections[i].AzAng1 > AZI_ANGLE_1_THRESHOLD){
continue;
}
If the point in the array does not meet any of these thresholds then that point will be skipped and will not be included. Click here to learn more about these thresholds.
radar_driver::RadarDetection data;
// Copy all data from old message detection to new message
data.Pdh0 = oldMsg->Detections[i].Pdh0;
data.AzAng0 = oldMsg->Detections[i].AzAng0;
data.RCS0 = oldMsg->Detections[i].RCS0;
... copy all the other parameters
After we have the points we want, we can now copy the detections array to a new array.
newMsg->Detections.push_back(data);
}
// Return true if there is at least one detection in newMsg after filtering
return (newMsg->Detections.size() > 0);
}
At last, we can now push this to the newMsg packet. As a final check we will return true if there is at least one detection after filtering.
The last thing this layer takes care of is publishing the packets to the next node. Before we can look at the actual publishing function, we will check out the clearing functions.
Clear Packets
uint8_t PacketProcessor::clearPackets(uint8_t idx) {
if (idx < 2) {
PacketsBuffer[idx] = EmptyPackets; //Replace w/ empty struct
PacketsBuffer[idx].numFarPackets = 0;
PacketsBuffer[idx].numNearPackets = 0;
return SUCCESS;
}
return CLEAR_FAIL;
}
The index number must be less than two because our double buffer uses the indices zero and one. If the index is greater than two then a CLEAR_FAIL is returned. Otherwise the buffer is updated to the default packet structure and the number of far and near packets are set to zero.
Publish Packets
uint8_t PacketProcessor::publishPackets(uint8_t idx) {
if (Publisher == NULL) { //Publisher not set up
//Check if we can still clear the packets
if (clearPackets(idx)) { //Everything failed
return NO_PUB_CLR_FAIL;
}
return NO_PUBLISHER; //Only null publisher, cleared ok
} else if (Publisher->pubCallback(&PacketsBuffer[idx])) {
return PUBLISH_FAIL;
}
The very first thing we check for is to see if the publisher is set up or not. If it has not, then we can at least attempt to clear the packets. If that has failed then we return NO_PUB_CLR_FAIL. Otherwise we return NO_PUBLISHER, indicating that clearing the packets is successful with the publisher not being created yet.
If the publisher does exist but does not succeed with the pubCallback function then we return PUBLISH_FAIL.
if (clearPackets(idx)) {
return CLEAR_FAIL;
}
return SUCCESS;
The last thing we check for is to see if clearing the packets is successful. If not then we return a CLEAR_FAIL. If all these tests pass, we finish off this layer by returning a SUCCESS.
As mentioned above the processor node is composed of two layers. The processing layer is responsible for grouping the data and filtering the data while the radar publisher layer is responsible for sending out the filtered radar data. After having finished explaining these layers, let's take check how these layers are used in the main function. (Main function is found in the FilteredMain.cpp file)
Node Handle
ros::NodeHandle nh;
RadarPublisher rp(nh);
ros::Subscriber radarUnfilteredSub;
uint8_t scanMode = 0;
std::string unfiltered_topic = "unfiltered_radar_packet";
By default we create a node handle and pass it to the publisher. We also have a subscriber that takes the topic from the previous node, the 'publisher'. Also we set the scan mode to zero by default. Let's take a look at the publisher's constructor.
Publisher constructor
RadarPublisher::RadarPublisher (ros::NodeHandle nh_)
{
packet_pub_ = nh_.advertise<radar_driver::RadarPacket>(std::string("filtered_radar_packet"), 50);
}
From the radarPublisher.cpp file, the constructor will send its own topic, filtered_radar_packet, to the next node.
Radar Callback
void radarCallback(const radar_driver::RadarPacket::ConstPtr& msg) {
uint8_t ret_status = PacketProcessor::processRDIMsg(msg);
std::string status_info = "";
// Most of our return statuses are of type ERR so make this the default verbosity level
uint8_t log_verbosity = LOG_ERROR;
In the callback for the subscriber we get the return status from the processor. Depending on what status it is, the log messages will output something different indicating what problem/issue arises. The default verbosity level is LOG_ERROR.
Switch Statements
switch(ret_status) {
case NO_DETECTIONS:
status_info = "No Detections";
// No detections isn't really a failure, so just issue a warning so that it gets logged somewhere
log_verbosity = LOG_WARNING;
break;
case BAD_EVENT_ID:
status_info = "Bad Event ID";
break;
case NO_PUB_CLR_FAIL:
status_info = "No Publisher and Unable to Clear Packets";
break;
case NO_PUBLISHER:
status_info = "No Publisher";
break;
case PUBLISH_FAIL:
status_info = "Publishing failed";
break;
case CLEAR_FAIL:
status_info = "Failed to clear packets";
break;
default:
// Assume default ret_value is success in which case don't log anything
return;
This giant switch statement takes care of the return statuses from the processor. Most of them are rare except for the no detections status. The default case is a success where nothing needs to be logged.
Log Statements
if (log_verbosity == LOG_WARNING) {
ROS_WARN_STREAM("radarCallback warning, unable to execute processRDIMsg. Cause: " << status_info);
} else if (log_verbosity == LOG_ERROR) {
ROS_ERROR_STREAM("radarCallback ERROR, unable to execute processRDIMsg. Cause: " << status_info);
}
The last lines for the callback are the stream statements where the error/warning message will be printed to rqt_console. Everything is now done with the processor node and the next node will receive its topic, filtered_radar_packet.
If it seems like a lot of code was skipped it is because a lot of code was skipped. I skipped anything with SS messages as these functions are mainly used for debugging purposes. I also skipped any code that was not currently being used. Some smaller functions and lines were skipped as some of them are trivial as to what they do.
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