Table of contents
Method 2. Use kitti_helper.launch
Fault: There is no image on the rviz interface
View the topic released by rosbag
The topic received in the a-loam code
Method 2. Use ros::Subscriber and callback functions
Method 3: Create a new node to save the track
Run the kitti dataset with A-loam.
Run the kitti dataset
Method 1: Use rosbag to play
Use the code:
roslaunch aloam_velodyne xxx.launch
rosbag play /.../xxx.bag
Among them, when running the kitti dataset, xxx.launch is changed to aloam_velodyne_HDL_64.launch, /.../xxx.bag is changed to the running rosbag file, and /... is the path of the rosbag.
The specific operation is as follows:
Open a terminal and run
roslaunch aloam_velodyne aloam_velodyne_HDL_64.launchOpen another terminal and run
rosbag play /.../xxx.bagAt this time, if there is no image on the rviz interface when running, it may be that the topic released by rosbag does not correspond to the topic received in the aloam code. See below for the modification method Fault: There is no image part on the rviz interface .
Method 2. Use kitti_helper.launch
As stated in A-loam's README.md:
## 4. KITTI Example (Velodyne HDL-64)
Download [KITTI Odometry dataset](http://www.cvlibs.net/datasets/kitti/eval_odometry.php) to YOUR_DATASET_FOLDER and set the `dataset_folder` and `sequence_number` parameters in `kitti_helper.launch` file. Note you also convert KITTI dataset to bag file for easy use by setting proper parameters in `kitti_helper.launch`.```
roslaunch aloam_velodyne aloam_velodyne_HDL_64.launch
roslaunch aloam_velodyne kitti_helper.launch
```
Run in two terminals separately
roslaunch aloam_velodyne aloam_velodyne_HDL_64.launch
roslaunch aloam_velodyne kitti_helper.launch
Among them, the parameters in the kitti_helper.launch file need to be configured in advance.
This method works well for downloaded 00-10 sequences.
Fault: There is no image on the rviz interface
There is no image on the rviz interface during runtime. It may be that the topic released by rosbag does not correspond to the topic received in the aloam code.
View the topic released by rosbag
Open a terminal, run
rosbag info /.../xxx.bagAmong them, /.../xxx.bag is the path and file name of the rosbag to be viewed.
Taking my rosbag as an example, a section of the printed output information is intercepted as follows:
Among them, topics is the topic topic released by rosbag in play.
In topics, the topic name of the laser point cloud is /kitti/velo/pointcloud.
The topic received in the a-loam code
In the aloam source code, the scanRegistration.cpp file contains the topic received by a-loam, and the modification corresponds to the topic name of the laser point cloud released by rosbag.
The path of scanRegistration.cpp: /catkin_ws/src/A-LOAM-devel/src/scanRegistration.cpp
original:
ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_points", 100, laserCloudHandler);
Change the original /velodyne_points to /kitti/velo/pointcloud, after modification:
ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/kitti/velo/pointcloud", 100, laserCloudHandler);
compile again
cd ~/catkin_ws
catkin_make
source ~/catkin_ws/devel/setup.bashAfter the compilation is successful, run the kitti dataset .
Example of running results:
In the figure, the colored dots are being processed, the white dots are processed, and the horizontal line in the middle is the motion track.
rqt
start rqt
rqt is a graphics development platform for ROS and a Qt-based framework. Use the "rqt_graph" command to display the current system operation.
If you have installed rqt,
In a new terminal run
rosrun rqt_graph rqt_graphor
rqt_graphThe rqt interface will pop up.
If the rqt interface does not pop up, you may need to install rqt. The installation instructions are similar to the following:
sudo apt-get install ros-indigo-rqt
sudo apt-get install ros-indigo-rqt-common-pluginsThe instructions are modified according to the actual ROS version.
View the rqt interface
In the normally pop-up rqt interface, real-time node nodes and topic topic information are drawn.
You can click the "Refresh ROS graph" button in the upper right corner to refresh the interface.
The image when running a-loam in this article is as follows:
Only run the roslauch command for a-loam:
When running kitti's rosbag:
There are more nodes and topics of the rosbag on the left, and the topic topic of the laser point cloud of the rosbag is received by the scanRegistration node.
There is also a topic/tf in the lower right corner. At this time, the program is running normally, and there are positioning and map results in rviz.
After rosbag playback is complete:
The nodes and topics related to rosbag disappear, and the program ends.
You can view the running status of rosbag and aloam through rqt, which is easy to understand.
save track
The original aloam does not have a program to save the track, you can add it manually.
The core idea is to save the data in the /aft_mapped_to_init topic to txt in a certain format.
Method one, simple
Modify the code in the aloam source code laserMapping.cpp, and add the save trajectory code in the laserOdometryHandler function.
The location of laserMapping.cpp: /catkin_ws/src/A-LOAM-devel/src/laserMapping.cpp
In the file, find the implementation of the laserOdometryHandler function:
void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr &laserOdometry)
{
Append at the end of the laserOdometryHandler function
std::ofstream pose1("/your_path/your_file_name.txt", std::ios::app);
pose1.setf(std::ios::scientific, std::ios::floatfield);
//保存结果的精度，可调
pose1.precision(6);
pose1 << odomAftMapped.header.stamp << " "
<< odomAftMapped.pose.pose.position.x << " "
<< odomAftMapped.pose.pose.position.y << " "
<< odomAftMapped.pose.pose.position.z << " "
<< odomAftMapped.pose.pose.orientation.x << " "
<< odomAftMapped.pose.pose.orientation.y << " "
<< odomAftMapped.pose.pose.orientation.z << " "
<< odomAftMapped.pose.pose.orientation.w << std::endl;
pose1.close();The odometer after odomAftMapped drawing, take the time, position xyz and quaternion, save it to /your_path/your_file_name.txt, your_path and your_file_name can be set according to your preferences.
Advantages: Simple modification and easy operation.
Method 2. Use ros::Subscriber and callback functions
Modify the code in laserMapping.cpp in the aloam source code. Create a new ros::Subscriber in the main function to execute the callback function when the required topic is received, and write the callback function before the main function to save the track.
in the main function
int main(int argc, char **argv)
{
Many subscribers ros::Subscriber are defined, including subLaserOdometry, subLaserCloudFullRes, etc. It is defined as follows:
ros::Subscriber subLaserOdometry = nh.subscribe<nav_msgs::Odometry>("/laser_odom_to_init", 100, laserOdometryHandler);
ros::Subscriber subLaserCloudFullRes = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_cloud_3", 100, laserCloudFullResHandler);You can then define your own ros::Subscriber and name it save_path.
ros::Subscriber save_path= nh.subscribe<nav_msgs::Odometry>("/aft_mapped_to_init", 100, path_save); ros::Subscriber save_path When there is a /aft_mapped_to_init topic, execute the callback function path_save.
Implementation of path_save:
void path_save(nav_msgs::Odometry odomAftMapped ){
//保存轨迹，path_save是文件目录,txt文件提前建好,/home/xxx/xxx.txt,
std::ofstream pose1("/path_save/xxx.txt", std::ios::app);
pose1.setf(std::ios::scientific, std::ios::floatfield);
pose1.precision(9); //精度可调，根据需要调整
static double timeStart = odomAftMapped.header.stamp.toSec();
auto T1 =ros::Time().fromSec(timeStart) ;
pose1<< odomAftMapped.header.stamp -T1<< " "
<<-odomAftMapped.pose.pose.position.y << " "
<< odomAftMapped.pose.pose.position.z << " "
<< odomAftMapped.pose.pose.position.x << " "
<< odomAftMapped.pose.pose.orientation.x << " "
<< odomAftMapped.pose.pose.orientation.y << " "
<< odomAftMapped.pose.pose.orientation.z << " "
<< odomAftMapped.pose.pose.orientation.w << std::endl;
pose1.close();
} The implementation of this path_save is written before the main function.
Method 3: Create a new node to save the track
Create a new node path_save. Create a new cpp file, modify CMakeLists.txt, and modify aloam_velodyne_VLP_16.launch.
1. Create a new cpp file
Create a new cpp file in the aloam source code, such as savePath.cpp.
aloam source code path: /catkin_ws/src/A-LOAM-devel/src/savePath.cpp
write in the file
void path_save(nav_msgs::Odometry odomAftMapped ){
//保存轨迹，path_save是文件目录,txt文件提前建好,/home/xxx/xxx.txt,
std::ofstream pose1(“path_save”, std::ios::app);
pose1.setf(std::ios::scientific, std::ios::floatfield);
pose1.precision(9);
static double timeStart = odomAftMapped.header.stamp.toSec();
auto T1 =ros::Time().fromSec(timeStart) ;
pose1<< odomAftMapped.header.stamp -T1<< " "
<< -odomAftMapped.pose.pose.position.y << " "
<< odomAftMapped.pose.pose.position.z << " "
<< odomAftMapped.pose.pose.position.x << " "
<< odomAftMapped.pose.pose.orientation.x << " "
<< odomAftMapped.pose.pose.orientation.y << " "
<< odomAftMapped.pose.pose.orientation.z << " "
<< odomAftMapped.pose.pose.orientation.w << std::endl;
pose1.close();
}
int main(int argc, char **argv){
ros::init(argc, argv, "path_save");
ros::NodeHandle nh;
ros::Subscriber save_path = nh.subscribe<nav_msgs::Odometry>("/aft_mapped_to_init", 100, path_save); //保存轨迹，a_loam直接订阅话题/aft_mapped_to_init。
ros::spin();
}Among them, the header file can be added according to other source files, such as the laserMapping.cpp file.
2. Modify CMakeLists.txt
At the same time, CMakeLists.txt should also be modified.
Modeled after alaserMapping
add_executable(kittiHelper src/kittiHelper.cpp)
target_link_libraries(kittiHelper ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS})Append at the end of CMakeLists.txt, write savePath.cpp:
add_executable(apath_save src/savePath.cpp)
target_link_libraries(apath_save ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS})
Create the executable file apath_save with savePath.cpp and link the corresponding library
3. Modify aloam_velodyne_VLP_16.launch
aloam_velodyne_VLP_16.launch To release the newly created node, add after the code of the original release node:
<node pkg="aloam_velodyne" type="apath_save" name="b_path_save" output="screen" />
Among them, apath_save corresponds to the executable file name apath_save generated by add_executable in CMakeLists.txt;
The node name b_path_save is arbitrary, and a new node b_path_save will be generated, and rqt can view the generation of new nodes.
This method is more complicated, and the savePath.cpp file creates a new node path_save.
Advantages: When you don't need to save the trajectory, you only need to delete or comment out the code that publishes the new node, and you don't need to recompile.
<!-- xxxxxxx -->
summary
Save the trajectory in TUM format, and use tools such as evo to draw images.
Summarize
1. Run the kitti dataset
roslaunch aloam_velodyne xxx.launch
rosbag play /.../xxx.bag
2. Fault: rviz has no image
Match the topic received in the aloam code with the topic published by rosbag.
Don't forget to run other datasets to modify back later.
3.rqt view topic situation
rqt_graph
4. Save track
Modify laserMapping.cpp, and add the code to save the trajectory in the laserOdometryHandler function.
In the article, "aloam", "a-loam", "A-loam", "A-Loam", and "A-LOAM" all refer to the A-LOAM of HKUST-Aerial-Robotics. Link:
GitHub - HKUST-Aerial-Robotics/A-LOAM: Advanced implementation of LOAM