标签：std 框架 image lidar --- bag 源码 msg data
整个项目是用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
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。