Created by [ Frank Yan], last modified on Apr 28, 2020
The purpose of this node is to set up the multi-radar infrastructure. Most of the driver code is only applicable for a single radar so this means there is no way for two radars to communicate with each other. What the multi-radar node does is it can now identify what points the left radar and right radar are picking up. The multi-radar node is automatically launched whenever all_radar.launch is executed. Currently, there are no manual instructions for the multi-radar node in the README.md file. This is the newest node for the radar-driver group so a lot of this documentation is subject to change. However, the main purposes of what this node is supposed to do can be listed as such:
As of W20, the only file for the multi-radar node is radarDetection.cpp which can be found in the src directory of the radar_driver repo.
Multi-Radar Class
class MultiRadar {
public:
bool right_radar_detection = false;
bool left_radar_detection = false;
bool multi_radar_detection = false;
ros::Timer radar_timer; //Using a timer just because it has a callback
MultiRadar(ros::NodeHandle &nh); //Constructor
void detection(const ros::TimerEvent &event); //Main detection function
private:
ros::Subscriber right_radar_sub;
ros::Subscriber left_radar_sub;
ros::NodeHandle node_;
radar_driver::RadarPacket right_radar;
radar_driver::RadarPacket left_radar;
void rightDetection(const radar_driver::RadarPacket::ConstPtr &right_radar_msg);
void leftDetection(const radar_driver::RadarPacket::ConstPtr &left_radar_msg);
};
In the include file, multiRadar.h, the class declaration for the multi-radar is stated. There are booleans for the left and right radar detections which are used for the detection logic. The multi-radar detection boolean is not being used as of now. In the public scope of the class, there is a timer. A timer is being used because it has a callback and it periodically checks if there is a multi-radar detection or not. The functions will be discussed later in this document but for now we'll take a look at the declaration of the node.
Node Handle
ros::NodeHandle nh;
MultiRadar multiradar(nh);
In the main function the node is created and the constructor for the multi-radar class is called. Let's see what the constructor does.
Multi-Radar Constructor
MultiRadar::MultiRadar(ros::NodeHandle &nh){
std::string right_topic("/radar_right/filtered_radar_packet");
std::string left_topic("/radar_left/filtered_radar_packet");
int q = 10; //queue size
right_radar_sub = nh.subscribe(right_topic, q, &MultiRadar::rightDetection, this);
left_radar_sub = nh.subscribe(left_topic, q, &MultiRadar::leftDetection, this);
radar_timer = nh.createTimer(ros::Duration(1), &MultiRadar::detection, this);
}
Using the two subscribers declared in the private scope of the class, we now initialize and set the parameters for the left and right radar subscribers. The queue size will determine how many messages the subscriber will hold on to before deleting old messages. Also, the radar timer is created in this constructor where the callback function is called every second. For now we will go back to the main function to finish the node declaration. The callback functions will be discussed in the detection logic section.
Publishers
ros::Publisher overlap_right = nh.advertise<sensor_msgs::PointCloud2>("/radar_right/radar_pointcloud", 50);
ros::Publisher overlap_left = nh.advertise<sensor_msgs::PointCloud2>("/radar_left/radar_pointcloud", 50);
Finally, the multi_radar node will publish its messages to the point-clouds of each radar.
Publisher Destination
[]{.aui-icon .aui-icon-small .aui-iconfont-warning .confluence-information-macro-icon}
I'm not 100% sure if publishing to the point-clouds of each radar works but for now this is what I believe is correct. Once the code for dropping points works then we can see if these are the right topics.
Now to explain the multi-radar detections, we will first examine the callback of one radar.
Radar Detection
void MultiRadar::rightDetection(const radar_driver::RadarPacket::ConstPtr &right_radar_msg){
if(right_radar_msg->Detections.size() > 1){
right_radar_detection = true;
right_radar = *right_radar_msg;
ROS_INFO_STREAM("Right Radar Detection");
}
else{
right_radar_detection = false;
right_radar = *right_radar_msg;
}
}
If the detection vector is greater than one, then the right_radar_detection boolean will be true. The reason why it is not zero is because the detection vector will always exist which means that it has at least one element. The only time the detection vector has a size of zero is when you first start up the multi-radar set up and the radars have detected nothing, in other words the detection vector has not been created yet. After copying the message to its respective member variable of the class, rqt_console will print out "Right Radar Detection". When there is no detection, the boolean will be false and the message is still copied to update the variable indicating that nothing was picked up by the right radar.
RQT Console
[]{.aui-icon .aui-icon-small .aui-iconfont-approve .confluence-information-macro-icon}
To learn more about rqt_console, check out this page: http://wiki.ros.org/rqt_console. When doing any tests filter out any debug messages so they do not clog the console view.
This logic is the exact same for the left radar with the names of the variables being changed to suit the left radar.
To handle multi-radar detections the timer will call its callback function, detection. The callback function is called every second and will print messages to rqt_console to notify what point will be dropped.
Multi-Radar Detection
void MultiRadar::detection(const ros::TimerEvent &event){
if(right_radar_detection == true && left_radar_detection == true){
ROS_WARN_STREAM("Both radars detecting object...");
In order for the multi-radar detection to work, both the left and right radar detection booleans must be true. Right after the first conditional, a message will be printed to warn the user that there is a multi-radar detection.
/*If the right radar detects a point first, then the left
radar should drop the point.*/
if(right_radar.TimeStamp < left_radar.TimeStamp){
ROS_WARN_STREAM("Dropping left radar point...");
left_radar_detection = false;
//This is just a message, haven't done the actual dropping yet
}
else{
ROS_WARN_STREAM("Dropping right radar point...");
right_radar_detection = false;
}
}
}
Currently the multi-radar node will send a warning message based on the timestamp of the left and right radar messages. For example if the right radar detects something before the left radar, then the left radar will drop its point. In this case the left_radar_detection boolean is set to false and a message is printed stating that the left radar point is being dropped. The right radar will keep its point and vice versa with the left radar detecting something before the right radar.
The reason why I choose timestamp to be the deciding factor is because it is easy to use. However, sometime in the future there may be more sophisticated ways of deciding what point should be dropped. Nonetheless, the logic is still the same where one boolean would be switched off and one boolean is turned on.
The biggest thing we need from the multi-radar infrastructure is to delete duplicate points. As seen in the comments in the code, there has not been a way of dropping points yet. It will most likely be another function that takes care of deleting points. However, to get potentially started with dropping points here are some general steps/tips on what we could do:
This is a new node so active development and more experimentation is required. Feel free to ping the radar-driver group in Discord if you have any questions about this node.
[]{.aui-icon .aui-icon-small .aui-iconfont-warning .confluence-information-macro-icon}
This node is not finished yet. We still need to get duplicate points deleted and once that works, we can update this page.
Document generated by Confluence on Nov 28, 2021 22:40