Created by [ Rowan Dempster], last modified by [ Frank Yan] on Apr 28, 2020
Once all the radar data has been cleaned up and is ready to go, the last thing we need to do is visualize it. To do this, the visualizer node generates a point-cloud for the radar data so we can easily see what the radars are detecting. The visualizer is implemented using a ROS visualization tool called RViz. How to launch this is documented inside the README.md file in the radar_driver/ directory. This document is for the purpose of explaining the functionality. For RViz to visualize the point-cloud correctly it needs 2 things:
The visualizer will create both of these things for RViz.
All of the following code blocks can be found in the PointCloudGenerator.cpp file in the src directory of the radar_driver repo.
Visualizer global variables
std::string name = "";
std::string radarFrame = "radar_fixed";
pthread_mutex_t mutex; //Non recursive
tf::Transform fixed;
std::vector<radar_driver::RadarPacket> packets;
ros::Subscriber rawData;
ros::Publisher pcData;
ros::Publisher marker_pub;
visualization_msgs::Marker leftFOVLine, rightFOVline;
uint32_t curTimeStamp = 0;
This is where we create some global variables for our node. The mutex is initialized, but isn't used. The transform is to provide the fixed frame required. The vector is to hold the packet groups. The subscriber and publisher are discussed below. The markers are used to set up the FOV lines in RViz. The time stamp is used to keep track of time stamps for grouping packets.
Node Handle
ros::init(argc, argv, "radar_visualizer");
ros::NodeHandle n;
This is where we create the visualizer node in ROS. Standard method for creating it.
pcData = n.advertise<sensor_msgs::PointCloud2>(std::string("radar_pointcloud"), 100);
rawData = n.subscribe(packetTopic, 100, radarCallback);
marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 10);
The architecture for the visualizer is to receive packets in the RadarPacket message format, convert it to a PointCloud2 message and publish it. Here we create a subscriber for RadarPackets, a publisher for PointCloud2, and a publisher to display the FOV lines for the radars.
Transform
fixed.setOrigin( tf::Vector3(0.0, 0.0, 0.0) ); //Set transform to be 0 rot, 0 trans
fixed.setRotation( tf::Quaternion(0, 0, 0, 1) );
This is where we take the fixed transform we declared globally, and set it to 0 translation and 0 rotation, since we want the PointCloud to be at the origin.
packets.clear();
ros::spin();
We then clear the grouping vector and begin waiting for messages from the subscriber. When they come in they are sent to radarCallback.
Callback
void radarCallback(const radar_driver::RadarPacket::ConstPtr& msg) {
if (curTimeStamp == 0) { //Case 1
curTimeStamp = msg->TimeStamp;
packets.push_back(*msg);
} else if (curTimeStamp != msg->TimeStamp) { //Case 2
packetCloudGenerator(packets);
curTimeStamp = msg->TimeStamp;
packets.clear();
packets.push_back(*msg);
}
else { // Time Stamps are the same, Case 3
packets.push_back(*msg);
}
return;
}
Case 1: This is where the callback messages get automatically sent to. We first check if we are in the init case, where this is our first packet. We set the timestamp and add the packet to the group.
Case 2: Here is the case for a new timestamp. If so we update the
timestamp first.
[NOTE:] Maybe what
I'm doing next is wrong/not correct? If so feel free to change the code
and put up a merge request.
Here I am making a local copy of the packet group since the original group is a global. I clear the original group since we are starting a new group, and push our packet with a new timestamp. I then send this finished grouping to the point-cloud generator function.
Case 3: This is the simple case of the same timestamp. Just push and return out of the callback.
In RViz, configuring the FOV lines for the radars will help see what range the radars have.
FOV Function
void configureFOVlines(void) {
leftFOVLine.header.frame_id = rightFOVline.header.frame_id = radarFrame;
//Configure timestamp at each publish
leftFOVLine.ns = rightFOVline.ns = "points_and_lines";
leftFOVLine.action = rightFOVline.action = visualization_msgs::Marker::ADD;
leftFOVLine.pose.orientation.w = rightFOVline.pose.orientation.w = 1.0;
leftFOVLine.id = 1;
rightFOVline.id = 2;
leftFOVLine.type = rightFOVline.type = visualization_msgs::Marker::LINE_STRIP;
This sets up the FOV lines by creating two lines for the radars.
FOV Configuration
leftFOVLine.scale.x = rightFOVline.scale.x = 0.05;
leftFOVLine.color.b = rightFOVline.color.b = 1.0;
leftFOVLine.color.r = rightFOVline.color.r = 1.0;
leftFOVLine.color.g = rightFOVline.color.g = 1.0;
leftFOVLine.color.a = rightFOVline.color.a = 0.5;
This changes the colour and scale of the FOV lines. (ROS uses American spelling of colour)
geometry_msgs::Point p;
p.x = p.y = p.z = 0; //Create origin point
rightFOVline.points.push_back(p);
leftFOVLine.points.push_back(p);
p.x = MAX_DIST; //Create farthest point
p.y = p.x * tan(AZI_ANGLE_1_THRESHOLD);
rightFOVline.points.push_back(p);
p.y *= -1;
leftFOVLine.points.push_back(p);
Finally, we create the origin points of where the FOV lines go and what angles the FOV lines should be at.
Now we look at the PointCloudGenerator:
PC Generator
void packetCloudGenerator(std::vector<radar_driver::RadarPacket> group) {
printf("New Packet Group, num: %u\r\n", group.size());
if (group.size() == 0) {
printf("Skipping empty group\r\n");
return;
}
We skip any groups of empty packets in them first.
PC Message
sensor_msgs::PointCloud2 pc;
pc.header = group[0].header;
pc.header.frame_id = "radar_fixed";
pc.height = 1;
pc.width = 0;
Here we create our PointCloud2 message and copy the header over. To tell RViz this point is with respect to our frame, we set the fixed frame field to 'radar_fixed'. The height of our point-cloud is 1 and width is 0 (I'm not 100% sure what this means, but it works for our purposes).
PC Struct
//Set x field type
sensor_msgs::PointField pf;
pf.name='x';
pf.offset=0;
pf.datatype=7;
pf.count=1;
pc.fields.push_back(pf);
....y, z, intensity
Here we begin to fill the PointCloud2 struct. To understand this, it would be helpful to read on some documentation for this message. Essentially the PointFields describe one element of a point. For our cloud we use x, y, z, and intensity. Thus we have 4 PointField elements. We add this to the fields vector in our PointCloud2.
Parameters
pc.is_bigendian = false; //All computer tested on are little endian
pc.point_step = 16; //4 bytes for x, 4 bytes for y, 4 bytes for z, 4 bytes for intensity
pc.is_dense = true;
Here we set some parameters so RViz can correctly process our point-cloud data. Each parameter is a float32, which is 4 bytes. The computers I built and tested this on were all Little Endian, and in case of bad points I raise is is_dense flag.
uint8_t* tmp;
for (uint8_t i = 0; i < group.size(); i++) {
for (uint8_t j = 0; j < group[i].Detections.size(); j++) {
pc.width ++;
tmp = (uint8_t*)&(group[i].Detections[j].posX); //My computer is little endian (lsb @ lowest index)
for (uint8_t k = 0; k < 4; k++) {
pc.data.push_back(tmp[k]);
}
...y,z
All data for the point-cloud is stored in a uint8_t array. This means to input a float as a parameter we need to convert a float32 into an uint8. This can be done by getting the address of the float and setting that to a uint8_t pointer. Since the float will have its 4 bytes stored in contiguous memory, we can index these 4 bytes as an array using this method. We then loop through all packets, and per-packet loop through all detections. For each detection, we increment the width and encode the x, y, z, and intensity from floats to uint8_t's.
Intensity
//Map from -100/+100 to 0/+100, lower span & more colour changes
float intensity = (group[i].Detections[j].RCS0 / 2.0) + 50.0;
tmp = (uint8_t*) &(intensity); //Turn float32 into 4 bytes
for (uint8_t k = 0; k < 4; k++) {
pc.data.push_back(tmp[k]);
}
For intensity, we do something slightly different first. The intensity scale we get from the ARS430 is -100/+100. I map this to a 0/+100 scale for a couple of reasons, mainly to make the colour changes more drastic. The encoding process is the same from there.
pc.row_step = pc.point_step * pc.width;
leftFOVLine.header.stamp = rightFOVline.header.stamp = pc.header.stamp;
pcData.publish(pc);
marker_pub.publish(leftFOVLine);
marker_pub.publish(rightFOVline);
return;
Next we fill in the rest of the parameters in the point-cloud and publish it. After this, everything is done on our end and we return.
To configure RViz, a configuration file has been checked into source control. The file should be a YAML file with a .rviz extension. Loading this as
rosrun rviz rviz -d config_file.rviz
will apply all configurations in the file to RViz when it opens.
RViz Files
[]{.aui-icon .aui-icon-small .aui-iconfont-approve .confluence-information-macro-icon}
You can also change the RViz file in the all_radar.launch file to be a different RViz configuration file if you like. There are three RViz configurations in the "rviz" folder in the radar-driver repo.
\
\
Document generated by Confluence on Nov 28, 2021 22:40