三.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---1.项目文件介绍（除主要源码部分）
作者：互联网
整个项目是用ROS环境下的catkin make进行编译的，初学者主要关注include、launch、rviz_cfg、src文件夹和README、CMakeLists、package文件。
CMakeLists文件：
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
nav_msgs
sensor_msgs
roscpp
rospy
rosbag
std_msgs
image_transport
cv_bridge
tf
)该框架使用了ROS自带的package。
find_package(PCL REQUIRED)
find_package(OpenCV REQUIRED)
find_package(Ceres REQUIRED)
include_directories(
include
${catkin_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
${CERES_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS})
add_executable(ascanRegistration src/scanRegistration.cpp)
target_link_libraries(ascanRegistration ${catkin_LIBRARIES} ${PCL_LIBRARIES})
add_executable(alaserOdometry src/laserOdometry.cpp)
target_link_libraries(alaserOdometry ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES})
add_executable(alaserMapping src/laserMapping.cpp)
target_link_libraries(alaserMapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES})
add_executable(kittiHelper src/kittiHelper.cpp)
target_link_libraries(kittiHelper ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS})包含这些第三方库的头文件，将第三方库文件跟源文件进行关联。
catkin_package(
CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs
DEPENDS EIGEN3 PCL
INCLUDE_DIRS include
)DEPENDS 和 CATKIN_DEPENDS 用来告诉 catkin ，我们建立的程序包有哪些依赖项。
README文件：
介绍了A-LOAM如何部署，还有一些数据集如何获取，作者的信息和Docker的配置。
Package.xml文件：
这是A-LOAM包信息，包含作者邮箱、包构建和运行的依赖项。
Include文件夹：
Common.h：
inline double rad2deg(double radians)
{
return radians * 180.0 / M_PI;
}
inline double deg2rad(double degrees)
{
return degrees * M_PI / 180.0;
}
该头文件定义了两个函数，一个是弧度转角度，一个是角度转弧度。
Tic_toc.h ：
class TicToc
{
public:
TicToc()
{
tic();
}
void tic()
{
start = std::chrono::system_clock::now();
}
double toc()
{
end = std::chrono::system_clock::now();
std::chrono::duration<double> elapsed_seconds = end - start;
return elapsed_seconds.count() * 1000;
}
private:
std::chrono::time_point<std::chrono::system_clock> start, end;
};
该头文件定义了一个类，是作者自己写的用于计时的类。用了C++的chrono时间库，调用system_clock类里面的now方法获取当前系统时间，在一个代码块的开头调用tic()方法（构造函数只需要实例化对象就可调用），结尾调用toc()方法，传回的参数就是代码块的执行时间，单位为ms。
Launch文件夹：
<launch>
<param name="scan_line" type="int" value="16" />
<!-- if 1, do mapping 10 Hz, if 2, do mapping 5 Hz. Suggest to use 1, it will adjust frequence automaticlly -->
<param name="mapping_skip_frame" type="int" value="1" />
<!-- remove too closed points -->
<param name="minimum_range" type="double" value="0.3"/>
<param name="mapping_line_resolution" type="double" value="0.2"/>
<param name="mapping_plane_resolution" type="double" value="0.4"/>
<node pkg="aloam_velodyne" type="ascanRegistration" name="ascanRegistration" output="screen" />
<node pkg="aloam_velodyne" type="alaserOdometry" name="alaserOdometry" output="screen" />
<node pkg="aloam_velodyne" type="alaserMapping" name="alaserMapping" output="screen" />
<arg name="rviz" default="true" />
<group if="$(arg rviz)">
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find aloam_velodyne)/rviz_cfg/aloam_velodyne.rviz" />
</group>
</launch>以aloam_velodyne_VLP_16.launch为例，launch文件中通过node符号可以开启多个节点，且rosmaster也同时开启了；同时还定义了包名，这在运行launch文件中会用到。通过param符号可作为参数服务器，以便在程序中读取。同时也可设置ROS自带显示软件RVIZ是否开启和开启的RVIZ配置路径。
rviz_cfg文件夹：
在这个文件夹存放rviz的配置文件，可在rviz软件中修改参数后进行保存。
src文件夹：
kittiHelper.cpp：
这个源文件的作用是将kitti数据转换成ros下的话题数据，并可选择保存为bag文件。首先我们先看主函数的代码注释。
int main(int argc, char** argv)
{
ros::init(argc, argv, "kitti_helper"); //该节点名称，且初始化
ros::NodeHandle n("~");
std::string dataset_folder, sequence_number, output_bag_file;
n.getParam("dataset_folder", dataset_folder);
//从参数服务器获取数据集文件夹和对应号码，可在launch文件中根据数据存放地址来修改
n.getParam("sequence_number", sequence_number);
std::cout << "Reading sequence " << sequence_number << " from " << dataset_folder << '\n';
bool to_bag;
n.getParam("to_bag", to_bag); //读取是否转换成bag标志
if (to_bag)
n.getParam("output_bag_file", output_bag_file); //同上，获取bag输出文件夹
int publish_delay;
n.getParam("publish_delay", publish_delay); //同上，获取发布延迟时间
publish_delay = publish_delay <= 0 ? 1 : publish_delay; //三目运算符，延迟时间<=0 则设为1
//初始化发布雷达话题数据
ros::Publisher pub_laser_cloud = n.advertise<sensor_msgs::PointCloud2>("/velodyne_points", 2);
//用image_transport初始化发布左右相机数据，对ImageTransport类进行实例化
image_transport::ImageTransport it(n);
image_transport::Publisher pub_image_left = it.advertise("/image_left", 2);
image_transport::Publisher pub_image_right = it.advertise("/image_right", 2);
//初始化里程计发布，ground_truth包括旋转的四元数和位置坐标向量，这里没有用上
ros::Publisher pubOdomGT = n.advertise<nav_msgs::Odometry> ("/odometry_gt", 5);
nav_msgs::Odometry odomGT;
odomGT.header.frame_id = "/camera_init";
odomGT.child_frame_id = "/ground_truth";
ros::Publisher pubPathGT = n.advertise<nav_msgs::Path> ("/path_gt", 5);
nav_msgs::Path pathGT;
pathGT.header.frame_id = "/camera_init";
//获取时间戳地址
std::string timestamp_path = "sequences/" + sequence_number + "/times.txt";
std::ifstream timestamp_file(dataset_folder + timestamp_path, std::ifstream::in);
std::string ground_truth_path = "results/" + sequence_number + ".txt";
std::ifstream ground_truth_file(dataset_folder + ground_truth_path, std::ifstream::in);
rosbag::Bag bag_out;
if (to_bag)
bag_out.open(output_bag_file, rosbag::bagmode::Write); //新建并打开一个bag文件
Eigen::Matrix3d R_transform; //用在ground_truth数据中，这里没有用上
R_transform << 0, 0, 1, -1, 0, 0, 0, -1, 0;
Eigen::Quaterniond q_transform(R_transform);
std::string line;
std::size_t line_num = 0;
ros::Rate r(10.0 / publish_delay); //ros循环频率
// 遍历时间戳这个文本文件，得到时间戳信息
std::cout << "timestamp_file " << std::string(dataset_folder + timestamp_path) << std::endl;
while (std::getline(timestamp_file, line) && ros::ok())
{
// 把string转成浮点型float
float timestamp = stof(line);
std::stringstream left_image_path, right_image_path;
// 找到对应的左右目的图片位置，并用opencv接口读取
left_image_path << dataset_folder << "sequences/" + sequence_number + "/image_0/" << std::setfill('0') << std::setw(6) << line_num << ".png";
cv::Mat left_image = cv::imread(left_image_path.str(), CV_LOAD_IMAGE_GRAYSCALE);
right_image_path << dataset_folder << "sequences/" + sequence_number + "/image_1/" << std::setfill('0') << std::setw(6) << line_num << ".png";
cv::Mat right_image = cv::imread(left_image_path.str(), CV_LOAD_IMAGE_GRAYSCALE);
std::stringstream lidar_data_path;
// 获取lidar数据的文件名
lidar_data_path << dataset_folder << "velodyne/sequences/" + sequence_number + "/velodyne/"
<< std::setfill('0') << std::setw(6) << line_num << ".bin";
//这里调用读雷达数据的函数
std::vector<float> lidar_data = read_lidar_data(lidar_data_path.str());
std::cout << "totally " << lidar_data.size() / 4.0 << " points in this lidar frame \n";
std::vector<Eigen::Vector3d> lidar_points;
std::vector<float> lidar_intensities;
pcl::PointCloud<pcl::PointXYZI> laser_cloud;
// 每个点数据占四个float数据，分别是xyz，intensity，存到laser_cloud容器中
for (std::size_t i = 0; i < lidar_data.size(); i += 4)
{
lidar_points.emplace_back(lidar_data[i], lidar_data[i+1], lidar_data[i+2]);
lidar_intensities.push_back(lidar_data[i+3]);
// 构建pcl的点云格式
pcl::PointXYZI point;
point.x = lidar_data[i];
point.y = lidar_data[i + 1];
point.z = lidar_data[i + 2];
point.intensity = lidar_data[i + 3];
laser_cloud.push_back(point);
}
// 转成ros的消息格式
sensor_msgs::PointCloud2 laser_cloud_msg;
pcl::toROSMsg(laser_cloud, laser_cloud_msg);
laser_cloud_msg.header.stamp = ros::Time().fromSec(timestamp); //设定雷达时间戳
laser_cloud_msg.header.frame_id = "/camera_init"; //设定在相机坐标系下
// 发布点云数据
pub_laser_cloud.publish(laser_cloud_msg);
// 图片也转成ros的消息发布出去
sensor_msgs::ImagePtr image_left_msg = cv_bridge::CvImage(laser_cloud_msg.header, "mono8", left_image).toImageMsg();
sensor_msgs::ImagePtr image_right_msg = cv_bridge::CvImage(laser_cloud_msg.header, "mono8", right_image).toImageMsg();
pub_image_left.publish(image_left_msg);
pub_image_right.publish(image_right_msg);
// 也可以写到rosbag包中去
if (to_bag)
{
bag_out.write("/image_left", ros::Time::now(), image_left_msg);
bag_out.write("/image_right", ros::Time::now(), image_right_msg);
bag_out.write("/velodyne_points", ros::Time::now(), laser_cloud_msg);
bag_out.write("/path_gt", ros::Time::now(), pathGT); //下面两个实际没有数据
bag_out.write("/odometry_gt", ros::Time::now(), odomGT);
}
line_num ++;
r.sleep();
}
bag_out.close();
std::cout << "Done \n";
return 0;
}
我们再看一下read_lidar_data函数，功能是将雷达每一帧的数据读取到一个float类型的vector容器中。
std::vector<float> read_lidar_data(const std::string lidar_data_path)
{
std::ifstream lidar_data_file(lidar_data_path, std::ifstream::in | std::ifstream::binary);
lidar_data_file.seekg(0, std::ios::end); // 文件指针指向文件末尾
const size_t num_elements = lidar_data_file.tellg() / sizeof(float); // 统计一下文件有多少float数据
lidar_data_file.seekg(0, std::ios::beg); // 再把指针指向文件开始
std::vector<float> lidar_data_buffer(num_elements);
// 读取所有的数据
lidar_data_file.read(reinterpret_cast<char*>(&lidar_data_buffer[0]), num_elements*sizeof(float));
return lidar_data_buffer;
}
下一节讲解scanRegistration.cpp。
标签：std,框架,image,lidar,---,bag,源码,msg,data 来源： https://blog.csdn.net/weixin_36773706/article/details/89409490