In the last article, we know various properties of radar data through experiments, but how does radar data reflect in the code?
In this article, you’ll learn how to traverse the radar data and how to process it by creating a new ROS package.
First, I’ll explain the code environment I’m using:
Ubuntu version: 16.04.01 ROS: Kinetic version
Programming language: C++ IDE recommendation: I am currently using VS code. How to configure it will be explained in a later article
1 Message format of lidar data in ROS
Print the message format of the lidar data in the ROS by entering the following command in the terminal
rosmsg show sensor_msgs/LaserScan
Copy the codeThe results are shown below
std_msgs/Header header // The header of the data
uint32 seq // The number of the data
time stamp // The timestamp of the data
string frame_id // The data coordinate system
float32 angle_min // The starting Angle of the radar data (minimum Angle)
float32 angle_max // End Angle of radar data (maximum Angle)
float32 angle_increment // The angular resolution of the radar data (angular increment)
float32 time_increment // Time interval for each data point of radar data
float32 scan_time // The interval between the current frame data and the next frame data
float32 range_min // The minimum value of radar data
float32 range_max // The maximum value of radar data
float32[] ranges // The range value in polar coordinates for each point in the radar data
float32[] intensities // The intensity value for each point in the radar data
Copy the code2 Create a ROS package
Knowing the data structure of radar data, next we will have a deeper understanding of radar data through code.
First, create a new workspace and a new package with the following command
mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src/
catkin_create_pkg lesson1 roscpp sensor_msgs
Copy the codeSince we are writing in c++, the first dependency is roscpp. Since the data type of the radar data is sensor_MSGS /LaserScan, the second dependency is sensor_msgs.
2.1 lesson1 / SRC/laser_scan_node. Cc
First, create a new file under lesson1/ SRC and name it laser_scan_node.cc. Copy the following code in. The meaning of the code is clearly explained in the comments.
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
// Declare a class
class LaserScan
{
private:
ros::NodeHandle node_handle_; // Handle in ros
ros::NodeHandle private_node_; // Private handle in ros
ros::Subscriber laser_scan_subscriber_; // Declare a Subscriber
public:
LaserScan(a); ~LaserScan(a);void ScanCallback(const sensor_msgs::LaserScan::ConstPtr &scan_msg);
};
// The constructor
LaserScan::LaserScan() : private_node_("~")
{
ROS_INFO_STREAM("LaserScan initial.");
// Bind the radar callback function to the subscribed topic
laser_scan_subscriber_ = node_handle_.subscribe("laser_scan".1, &LaserScan::ScanCallback, this);
}
LaserScan::~LaserScan() {}// The callback function
void LaserScan::ScanCallback(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{
ROS_INFO_STREAM(
"seqence: " << scan_msg->header.seq <<
", time stamp: " << scan_msg->header.stamp <<
", frame_id: " << scan_msg->header.frame_id <<
", angle_min: " << scan_msg->angle_min <<
", angle_max: " << scan_msg->angle_max <<
", angle_increment: " << scan_msg->angle_increment <<
", time_increment: " << scan_msg->time_increment <<
", scan_time: " << scan_msg->scan_time <<
", range_min: " << scan_msg->range_min <<
", range_max: " << scan_msg->range_max <<
", range size: " << scan_msg->ranges.size() < <", intensities size: " << scan_msg->intensities.size());
// The Euclidean coordinates of the fifth point are
double range = scan_msg->ranges[4];
double angle = scan_msg->angle_min + scan_msg->angle_increment * 4;
double x = range * cos(angle);
double y = range * sin(angle);
ROS_INFO_STREAM(
// The polar coordinates of the fifth data point are:
"range = " << range << ", angle = " << angle <<
// The euclidian coordinates of the fifth data point are:
", x = " << x << ", y = " << y
);
// Traverse the radar data by the number of data in ranges
// for (int i = 0; i < scan_msg->ranges.size(); i++)
/ / {
// }
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "lesson1_laser_scan_node"); // Name of the node
LaserScan laser_scan;
ros::spin(a);// At this point, the program starts to wait and executes ScanCallback() each time a subscribed message arrives.
return 0;
}
Copy the code2.2 CMakeLists. TXT
Since we have a new.cc file, we will add the compile option to this file in cMakelists.txt, and add the following at the bottom of the file:
# Generate executable files for the specified file
add_executable(${PROJECT_NAME}_laser_scan_node src/laser_scan_node.cc)
# Add dependencies to the generated executable
add_dependencies(${PROJECT_NAME}_laser_scan_node The ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
Add links to libraries for generated executables
target_link_libraries(${PROJECT_NAME}_laser_scan_node
${catkin_LIBRARIES}
)
Copy the code2.3 package. The XML
Since we used the catkin_create_pkg command to generate the package, it already configured package. XML when the package was generated, so we don’t need to change the file here, just list the contents of the generated file.
<package format="2">
<name>lesson1</name>
<version>0.0.0</version>
<description>The lesson1 package</description>
<maintainer email="[email protected]">lx</maintainer>
<license>TODO</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<export>
</export>
</package>
Copy the code2.4 build
After the cmakelists. TXT file is modified, the code can be compiled. The specific instructions are as follows:
cd ~/catkin_ws
catkin_make
Copy the code2.5 run
If nothing else, there should be no compilation error.
When compiled, an executable named Lesson1_laser_scan_node is generated in the ~/catkin_ws/devel/lib/lesson1 folder.
You can run the following command to execute the command
First, open a terminal and enter roscore in the terminal. Then create a new terminal and enter roscore in the terminalcd ~/catkin_ws/devel/lib/lesson1
./lesson1_laser_scan_node
Copy the codeThe following log will be printed,
[INFO] [1606545572.752075473]: LaserScan initial.Copy the codeAt this time, there is no other message printed out, because our code did not receive the lidar data message, so we use the following command to play the bag, after the bag data is downloaded, right-click to decompress, and put it in the ~/bagfiles folder.
(The corresponding bag data of this article can be downloaded from my official account: Lesson1 in Building SLAM from scratch.)
Open a new terminal again and enter the following commandcd ~/bagfiles
rosbag play lesson1.bag
Copy the codeAt this point, the terminal window executing./lesson1_laser_scan_node should print the following message repeatedly
[INFO] [1606545575.110606737]: Frame_id: front_laser_link, angle_min: -3.14159, ANGLE_max: 3.14159, ANGLE_INCREMENT: 0.00436332, time_increment: 7.15627e-05, scan_time: 0.102979, range_min: 0.01, range_max: 25, range size: 1440, intensities size: 1440 [INFO] [1606545575.110772238]: Range = 2.6, Angle = -3.12414, x = -2.5996, y = -0.0453758Copy the code2.6 Result Analysis
From this information we can see:
-
The coordinate system for the radar is front_laser_link
-
The minimum Angle and maximum Angle of the radar data are -3.14159 and 3.14159 respectively. It can be seen that this is a radar with a horizontal view of 360 degrees
-
The nearest and furthest distances of radar data are 0.01m and 25m respectively, so it can be seen that the blind area of this radar is 1cm
-
A week of radar scanning will return 1,440 data points.
2.7 illustrates
2.7.1
We can get the most basic information of radar data from radar data, but these information may not be true and reliable, because these data are written in the driver package of radar. Some radar manufacturers’ codes are not standardized, and the information filled in when sending radar data may be wrong. For example, the radar blind area can rarely be as small as 1cm, generally more than 10cm.
2.7.2 Coordinate conversion
In addition, the ranges field in the radar data only stores the distance value in the polar coordinate system. If we want to know the Euclidean coordinates corresponding to each data point, we also need to transform the polar coordinates.
The method of transformation is to get the value of the ranges through the index, and then calculate the Angle corresponding to the value through the index
Angle = ANGle_min + ANGLE_increment * I so the x-coordinate corresponding to this point is ranges[I] * cos(Angle) So y = ranges[I] * sin(Angle)
2.7.3 Traversal of radar data
The traversal of 2d lidar data only carries out a for loop through the number of ranges fields, and obtains the distance value through the index.
2.8 launch file
2.8.1 launch file
Currently, we need to open three terminals to start this node. Is there a more convenient way to start this node?
Of course there is, and ROS uses the launch file to do this.
First, close all open terminals. In Ubuntu, the command to terminate what is being executed is Ctrl+C.
Create a launch folder
mkdir -p ~/catkin_ws/src/lesson1/launch
Copy the codeCreate a new file called demo.launch in the new Launch folder and fill it with the following.
<launch>
<! -- the address and name of bag need to be changed to the address corresponding to your own computer -->
<arg name="bag_filename" default="/home/lx/bagfiles/lesson1.bag"/>
<! -- use the time stamp of bag -->
<param name="use_sim_time" value="true" />
<! -- Start node -->
<node name="lesson1_laser_scan_node" pkg="lesson1" type="lesson1_laser_scan_node" output="screen" />
<! -- play bagfile -->
<node name="playbag" pkg="rosbag" type="play"
args="--clock $(arg bag_filename)" />
</launch>
Copy the codeThen just launch the launch file, and it will help us launch the roScore, the desired node, and the bag.
2.8.2 Setting the Environment
Since we are a new workspace, we first need to execute the following command to register the index of our new package in the terminal so that the terminal can find our package.
rospack profile
source ~/catkin_ws/devel/setup.bash
Copy the codeThe source command is executed again every time a new terminal is opened. You can also run the following command to write the source command to.bashrc. In this way, you do not need to execute the source command every time a new terminal is opened.
echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
Copy the codeWith the environment set, we will launch the launch file with roslaunch
roslaunch lesson1 demo.launch
Copy the codeIf all goes well we can get the same result as above.
3 summary
In this article, we create a new package in ROS from zero using the example of getting lidar data.
We showed how to create packages by command and how to add dependencies when creating packages.
After that, we created a new.cc file, and the contents in the file were written in accordance with the standard ROS format. In the future, our programs will follow this pattern for code writing. In the program, we know the message content of radar data, how to transform polar coordinates and European coordinates, and how to traverse the radar data.
After that, we modified the cMakelists.txt file and successfully compiled and ran it.
Finally, we covered the launch file and how to set up the code’s environment.
4 Next
In the next article we will not go into such a detailed explanation of the environment, we will pay more attention to the implementation of the functionality and code implementation.
In the next article, we will introduce how to process and filter radar data, and how to identify and filter out human calves in radar data.
The article will be updated in the public number: from scratch with SLAM, welcome everyone to follow, in order to update the article in the first time to inform you.
At the same time, I also hope that you can recommend this official account to the people around you who do laser SLAM, so that we can make progress together.
Reply to lesson1 on the official account to get the download link of the bag data in this article. Reply to open source address on the official account to get the Github address of the code in this article
