目录
一、./src/vloam_main_node.cpp代码学习
二、./launch/vloam_main.launch文件
三、./action/vloam_main.action文件
一、./src/vloam_main_node.cpp代码学习
头文件引用省略，结合README文档梳理主函数的功能。
1、参数创建与声明
// parameters from launch file
double rosbag_rate;
bool visualize_depth, publish_point_cloud, detach_VO_LO, save_traj;
int start_frame, end_frame; // inclusive interval首先从./launch/vloam_main.launch文件中读取参数，各个参数所代表的意义在后面解释。
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image,
sensor_msgs::CameraInfo,
sensor_msgs::PointCloud2>
MySyncPolicy;
std::shared_ptr<message_filters::Subscriber<sensor_msgs::Image>> sub_image00_ptr;
std::shared_ptr<message_filters::Subscriber<sensor_msgs::CameraInfo>> sub_camera00_ptr;
std::shared_ptr<message_filters::Subscriber<sensor_msgs::PointCloud2>> sub_velodyne_ptr; 这段代码定义了一种名为 MySyncPolicy 的同步策略，用于消息过滤器。它使用message_filters库中的sync_policies::ApproximateTime类，来大致同步类型为 sensor_msgs::Image、sensor_msgs::CameraInfo和sensor_msgs::PointCloud2的消息。接着声明了三个订阅者，分别用于接收sensor_msgs::Image、sensor_msgs::CameraInfo和sensor_msgs::PointCloud2类型的消息，并使用了std::shared_ptr来管理它们的生命周期。这些订阅者通常用于接收传感器数据，如图像、相机信息和点云数据。
Tips：typedef是C++中的一个关键字，用于给已存在的类型取一个别名。它的作用在于简化代码、增加可读性，并且可以使代码更易于维护。在这段代码中，使用了 typedef 来定义了一个同步策略MySyncPolicy，这样在代码中使用 MySyncPolicy 就等同于使用了message_filters::sync_policies::ApproximateTime<sensor_msgs::Image,sensor_msgs::CameraInfo,sensor_msgs::PointCloud2> 这个类型。这样做有助于代码的模块化和抽象化，使得代码更易于理解和扩展。
typedef actionlib::SimpleActionServer<vloam_main::vloam_mainAction> Server;
vloam_main::vloam_mainFeedback feedback;
vloam_main::vloam_mainResult result;
int count, i, j; // TODO: check if count will overflow 这段代码中，使用了typedef来创建了一个别名Server，它表示的是 actionlib::SimpleActionServer<vloam_main::vloam_mainAction> 这个类型。这个类型通常用于定义一个简单的动作服务器，而 vloam_main::vloam_mainAction 则是特定的动作类型。接着，代码声明了两个变量feedback和result，它们分别表示vloam_main::vloam_mainFeedback和vloam_main::vloam_mainResult这两个类型的变量。通常，这种命名方式是为了在程序中清晰地表示不同的数据或对象。最后，代码声明了三个整数变量 count、i 和 j，并添加了一个 TODO 注释，提醒开发者检查 count 是否会溢出。这个注释意味着需要考虑和处理可能的溢出问题，以避免潜在的错误。
Tips：actionlib::SimpleActionServer<vloam_main::vloam_mainAction> 这个语法是在 C++ 中使用 ROS 的 Action 功能时的一种语法，它的详细说明如下：
-
actionlib::SimpleActionServer：这是 ROS 中actionlib库提供的一个类，用于创建一个简单的动作服务器（Action Server）。动作服务器是用于执行动作目标（Action Goal）并向客户端提供结果（Action Result）和反馈（Action Feedback）的节点。SimpleActionServer是一个简化的版本，它提供了基本的功能来处理动作目标、发送结果和反馈。 -
<vloam_main::vloam_mainAction>：这部分是模板参数，用于指定所要处理的动作消息类型。在这里，vloam_main::vloam_mainAction是指定了动作消息的类型，其中vloam_main是 ROS 包的名称，vloam_mainAction是动作消息类型的名称。因此，这个语法表示创建一个处理vloam_mainAction类型的动作服务器。
std::shared_ptr<vloam::VisualOdometry> VO;
cv_bridge::CvImagePtr cv_ptr;
std::shared_ptr<vloam::LidarOdometryMapping> LOAM;
pcl::PointCloud<pcl::PointXYZ> point_cloud_pcl;
std::shared_ptr<vloam::VloamTF> vloam_tf;
std::string seq, cmd;
std::ostringstream ss;
int sys_ret;
// output for evaluation
std::string VOFileName;
std::string LOFileName;
std::string MOFileName;
FILE* LOFilePtr = NULL;
FILE* VOFilePtr = NULL;
FILE* MOFilePtr = NULL;这段代码声明了一些变量，然后初始化了一些文件指针，并为评估输出准备了一些文件名。逐步解释：
-
std::shared_ptr<vloam::VisualOdometry> VO声明了一个名为VO的指向vloam::VisualOdometry对象的共享指针。这表明VO可以指向一个vloam::VisualOdometry对象，并且通过使用共享指针，可以确保多个部分可以安全地共享对该对象的访问。 -
cv_bridge::CvImagePtr cv_ptr声明了一个名为cv_ptr的指向cv_bridge::CvImage对象的指针。通常，这个对象用于在OpenCV图像和ROS图像消息之间进行转换。 -
std::shared_ptr<vloam::LidarOdometryMapping> LOAM声明了一个名为LOAM的指向vloam::LidarOdometryMapping对象的共享指针。 -
pcl::PointCloud<pcl::PointXYZ> point_cloud_pcl声明了一个名为point_cloud_pcl的点云对象，类型为pcl::PointCloud<pcl::PointXYZ>。这是一个用于处理点云数据的对象，通常用于激光雷达数据的处理。 -
std::shared_ptr<vloam::VloamTF> vloam_tf声明了一个名为vloam_tf的指向vloam::VloamTF对象的共享指针。 -
std::string seq, cmd;声明了两个字符串变量seq和cmd，用于存储序列信息和命令信息。 -
std::ostringstream ss;声明了一个std::ostringstream对象ss，用于将数据转换为字符串。 -
int sys_ret;声明了一个整型变量sys_ret，用于存储系统调用的返回值。 -
std::string VOFileName;声明了一个字符串变量VOFileName，用于存储视觉里程计文件名。 -
std::string LOFileName;声明了一个字符串变量LOFileName，用于存储激光雷达里程计文件名。 -
std::string MOFileName;声明了一个字符串变量MOFileName，用于存储运动估计文件名。 -
FILE* LOFilePtr = NULL;声明了一个指向文件的指针LOFilePtr，并将其初始化为NULL。 -
FILE* VOFilePtr = NULL;声明了一个指向文件的指针VOFilePtr，并将其初始化为NULL。 -
FILE* MOFilePtr = NULL;声明了一个指向文件的指针MOFilePtr，并将其初始化为NULL。
这些变量和文件指针将在程序的后续部分中使用，用于存储和处理数据以及进行文件操作。
2、初始化函数
void init(const vloam_main::vloam_mainGoalConstPtr& goal)
{定义init函数，用于初始化程序的一些变量和文件指针，以及为新的估计准备数据。
第一部分：初始化文件指针
if (save_traj)
{
ss.clear();
ss.str("");
ss << std::setw(4) << std::setfill('0') << goal->seq;
seq = std::string(ss.str());
VOFileName = ros::package::getPath("vloam_main") + "/results/" + goal->date + +"_drive_" + seq;
LOFileName = ros::package::getPath("vloam_main") + "/results/" + goal->date + +"_drive_" + seq;
MOFileName = ros::package::getPath("vloam_main") + "/results/" + goal->date + +"_drive_" + seq;
cmd = "mkdir -p " + VOFileName;
sys_ret = system(cmd.c_str());
cmd = "mkdir -p " + LOFileName;
sys_ret = system(cmd.c_str());
cmd = "mkdir -p " + MOFileName;
sys_ret = system(cmd.c_str());
VOFileName += "/VO" + std::to_string(detach_VO_LO) + ".txt";
LOFileName += "/LO" + std::to_string(detach_VO_LO) + ".txt";
MOFileName += "/MO" + std::to_string(detach_VO_LO) + ".txt";
VOFilePtr = fopen(VOFileName.c_str(), "w");
LOFilePtr = fopen(LOFileName.c_str(), "w");
MOFilePtr = fopen(MOFileName.c_str(), "w");
if (VOFilePtr == NULL or LOFilePtr == NULL or MOFilePtr == NULL)
{
ROS_INFO("FilePtr == NULL");
ROS_BREAK();
}
ROS_INFO("\n LO file path; %s\n", LOFileName.c_str());
} 如果 save_traj 为True，则表示需要保存轨迹数据。这段代码首先根据 goal 的信息设置文件名，并创建文件夹来保存数据。（goal的定义位于action文件中）
- 生成序列号
seq的代码：
ss.clear(); 清除 stringstream ss 中的所有状态标志和数据。
ss.str(""); 将 stringstream ss 中的字符串内容清空，确保 ss 中不包含任何字符。
ss << std::setw(4) << std::setfill('0') << goal->seq; std::setw(4)：设置输出的最小宽度为4，即生成的字符串至少包含4个字符。std::setfill('0')：设置填充字符为 '0'，即在输出宽度不足时用 '0' 填充。goal->seq：获取seq的值，并将其写入stringstream ss中。其中goal是一个指向vloam_mainGoal类型对象的指针，而seq则是该对象的一个成员变量。通过goal->seq，可以访问到vloam_mainGoal对象中的seq成员变量的值。
seq = std::string(ss.str()); 将 stringstream ss 中的内容转换为字符串，并赋值给变量 seq。
- 创建目录的代码：
VOFileName = ros::package::getPath("vloam_main") + "/results/" + goal->date + +"_drive_" + seq;
LOFileName = ros::package::getPath("vloam_main") + "/results/" + goal->date + +"_drive_" + seq;
MOFileName = ros::package::getPath("vloam_main") + "/results/" + goal->date + +"_drive_" + seq;（这三行可能有错误，goal->date后面有两个+号，等之后运行代码的时候进一步验证）
ros::package::getPath("vloam_main"): 这个函数调用是用来获取名为 “vloam_main” 的 ROS package 的路径。
cmd = "mkdir -p " + VOFileName;
sys_ret = system(cmd.c_str());
cmd = "mkdir -p " + LOFileName;
sys_ret = system(cmd.c_str());
cmd = "mkdir -p " + MOFileName;
sys_ret = system(cmd.c_str()); 用linux命令，在main package中创建存放结果数据的目录。然后通过system(cmd.c_str())执行系统命令，其中cmd.c_str()将 cmd 转换成 C 风格的字符串，以符合 system() 函数的参数要求。执行完成后，将返回值（系统命令的执行状态码）赋给 sys_ret 变量，以便后续检查命令执行情况。
VOFileName += "/VO" + std::to_string(detach_VO_LO) + ".txt";
LOFileName += "/LO" + std::to_string(detach_VO_LO) + ".txt";
MOFileName += "/MO" + std::to_string(detach_VO_LO) + ".txt";
VOFilePtr = fopen(VOFileName.c_str(), "w");
LOFilePtr = fopen(LOFileName.c_str(), "w");
MOFilePtr = fopen(MOFileName.c_str(), "w"); 设置txt文件的名称以及路径，在原路径下新建文件夹/VO、/LO、/MO。然后，分别使用 fopen() 函数以写入模式(“w”)尝试打开这三个文件，以便后续写入数据。fopen() 函数接受的参数是一个 C 风格的字符串（字符数组），所以同样需要通过 c_str() 方法将 std::string 类型的文件路径转换为 C 风格的字符串。
在源工程中，存在作者已经生成的文件，代码效果如下：
第二部分：准备新的估计数据
count = 0;
vloam_tf = std::make_shared<vloam::VloamTF>();
VO = std::make_shared<vloam::VisualOdometry>();
LOAM = std::make_shared<vloam::LidarOdometryMapping>();
vloam_tf->init();
VO->init(vloam_tf);
LOAM->init(vloam_tf);- 这部分代码用于准备新的估计数据。首先将计数器
count初始化为零。 - 然后通过
std::make_shared创建了三个对象的共享指针：vloam::VloamTF类型的vloam_tf，vloam::VisualOdometry类型的VO，以及vloam::LidarOdometryMapping类型的LOAM。 - 分别调用了它们的
init函数来初始化这些对象。
3、回调函数
void callback(const sensor_msgs::Image::ConstPtr& image_msg, const sensor_msgs::CameraInfo::ConstPtr& camera_info_msg,
const sensor_msgs::PointCloud2::ConstPtr& point_cloud_msg)定义一个回调函数，用于处理来自传感器的图像、相机信息和点云数据。函数执行了一系列步骤，包括求解并发布里程计信息，启动各个传感器之间的坐标转换等。
static tf2_ros::StaticTransformBroadcaster static_broadcaster;
static tf2_ros::TransformBroadcaster dynamic_broadcaster; 在这段代码中，static_broadcaster 和 dynamic_broadcaster 是两个静态变量，它们分别属于 tf2_ros::StaticTransformBroadcaster 类和 tf2_ros::TransformBroadcaster 类的对象。这两个对象用于广播（broadcasting）转换（transform）信息。在ROS中，TF2（Transformation library for ROS）用于管理坐标系之间的关系，并通过广播器（broadcaster）将这些关系发布到ROS系统中，以供其他节点使用。
static_broadcaster用于发布静态的转换关系，这些关系在系统运行过程中保持不变。dynamic_broadcaster则用于发布动态的转换关系，这些关系可能随时间变化。
这段代码中的静态广播器和动态广播器在函数内部是静态变量，这意味着它们在函数调用结束后仍然存在，并且保留了它们的状态。这样做的目的可能是为了避免在每次函数调用时都重新创建这些广播器对象，以提高性能或确保转换关系的持续性。
i = count % 2;
VO->reset();
LOAM->reset(); 计算 count 除以 2 的余数，并将结果赋给变量 i。这个计算的目的可能是根据 count 是偶数还是奇数来交替执行两种状态或行为。之后调用 VO 和 LOAM 的 reset() 函数，通过重置它们的内部状态，为处理新的传感器数据准备 VO 和 LOAM 对象。这确保每次回调迭代都从一个干净的状态开始，以有效地处理新数据。
第一部分：相机信息预处理
// Section 1: Process Image // takes ~34ms
cv_ptr = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::MONO8);
VO->processImage(cv_ptr->image);
// Section 2: Process Static Transformations and Camera Intrinsics
if (count == 0)
{
vloam_tf->processStaticTransform();
VO->setUpPointCloud(camera_info_msg);
} 首先，使用 cv_bridge::toCvCopy() 函数将 ROS 图像消息 image_msg 转换为 OpenCV 的图像格式，并存储在 cv_ptr 变量中。这里使用了 MONO8 编码，表示单通道灰度图像。接着，调用 VO->processImage() 方法，将图像传递给 VO 对象进行处理。进行视觉里程计相关的图像处理，如特征提取、特征匹配等。这里 VO 是一个处理视觉里程计的对象。
其次，检查 count 变量是否为 0。如果是第一次接收到数据（即 count == 0），则执行以下步骤：
- 调用
vloam_tf->processStaticTransform()方法，处理静态转换。这可能涉及到传感器坐标系到世界坐标系的转换等静态参数的处理。 - 调用
VO->setUpPointCloud()方法，设置点云数据，其中可能包括相机的内参信息等。这个步骤通常在开始时执行，以初始化相机参数，为后续的视觉里程计计算做准备。
第二部分：点云预处理
// Section 3: Process Point Cloud // takes ~2.6ms
pcl::fromROSMsg(*point_cloud_msg, point_cloud_pcl); // optimization can be applied if pcl library is not necessarily
// ROS_INFO("point cloud width=%d, height=%d", point_cloud_pcl.width, point_cloud_pcl.height); // typical output
// "point cloud width=122270, height=1053676" // TODO: check why height is so large
VO->processPointCloud(point_cloud_msg, point_cloud_pcl, visualize_depth, publish_point_cloud); 首先，通过 pcl::fromROSMsg() 函数将 ROS 中的点云消息 point_cloud_msg 转换为 PCL（Point Cloud Library）格式的点云数据 point_cloud_pcl。这个步骤将 ROS 中的点云消息转换为 PCL 可以处理的点云格式，以便后续的处理。
然后，调用 VO->processPointCloud() 方法，将转换后的点云消息 point_cloud_msg 和 PCL 格式的点云数据 point_cloud_pcl 传递给 VO 对象进行处理。这个方法可能涉及到点云数据的滤波、配准、特征提取等操作，用于后续的视觉里程计计算。
第三部分：求解并发布里程计信息
// Section 4: Solve and Publish VO
if (count > 0)
{
VO->solveNlsAll(); // result is cam0_curr_T_cam0_last, f2f odometry
// VO->solveNls2dOnly();
// VO->solveRANSAC();
}
vloam_tf->VO2VeloAndBase(VO->cam0_curr_T_cam0_last); // transform f2f VO to world VO
vloam_tf->dynamic_broadcaster.sendTransform(vloam_tf->world_stamped_VOtf_base); // publish for visualization // can
// be commented out
VO->publish(); // publish nav_msgs::odometry
如果 count 大于 0，则执行以下步骤：
- 调用
VO->solveNlsAll()方法，进行非线性最小二乘求解（NLS），得到相邻帧间的里程计结果cam0_curr_T_cam0_last。这个结果通常表示当前相机位姿相对于上一帧相机位姿的转换，即帧间的相机运动。（根据需求选择不同的求解方法，如VO->solveNls2dOnly()或VO-> solveRANSAC()。） - 调用
vloam_tf->VO2VeloAndBase()方法，将帧间的视觉里程计结果转换为世界坐标系下的车体位姿。这个步骤将相机的相对运动转换为车体的位姿变化。 - 使用动态变换发布器
vloam_tf->dynamic_broadcaster发布从世界坐标系到车体坐标系的变换，以便进行可视化。这个步骤是可选的，可以根据需要注释掉。 - 最后，调用
VO->publish()方法，发布导航消息（nav_msgs::odometry），将视觉里程计的结果发布出去，供其他模块使用。
// Section 5: Solve and Publish LO MO
LOAM->scanRegistrationIO(point_cloud_pcl);
LOAM->laserOdometryIO();
LOAM->laserMappingIO();
// Section 6, save odoms
if (save_traj and count >= start_frame and count <= end_frame)
{
vloam_tf->VO2Cam0StartFrame(VOFilePtr, count - start_frame);
vloam_tf->LO2Cam0StartFrame(LOFilePtr, count - start_frame);
vloam_tf->MO2Cam0StartFrame(MOFilePtr, count - start_frame);
}
++count;
ROS_INFO("total count = %d", count);- 调用
LOAM->scanRegistrationIO()，对点云数据进行扫描注册，包括点云配准和特征提取等操作。 - 调用
LOAM->laserOdometryIO()，进行激光里程计的计算，得到激光雷达的运动估计。 - 最后，调用
LOAM->laserMappingIO()，进行激光雷达地图构建，将当前帧的点云数据融合到地图中。
如果 save_traj 为True且当前帧数在指定的起始帧和结束帧之间（count >= start_frame且count <= end_frame），则执行以下步骤：
- 调用
vloam_tf->VO2Cam0StartFrame()方法，将视觉里程计结果相对于起始帧的相机位姿保存到文件中。 - 调用
vloam_tf->LO2Cam0StartFrame()方法，将激光里程计结果相对于起始帧的激光雷达位姿保存到文件中。 - 调用
vloam_tf->MO2Cam0StartFrame()方法，将融合里程计结果相对于起始帧的位姿保存到文件中。
count 计数器递增，用于跟踪处理的总帧数。使用 ROS_INFO() 函数输出当前处理的总帧数，以便在控制台中进行实时监控和调试。（代码中%d是取整符）
Tips：ROS_INFO() 是 ROS (Robot Operating System) 中用于输出日志信息的函数之一，用于输出一般性的信息，通常用于指示程序的当前状态、执行进度或其他需要记录的信息。它可以输出变量的值、状态信息、调试消息等。
ROS_INFO(const char *format, ...)：这是ROS_INFO()函数的基本语法，类似于 C 语言中的printf()函数。它使用类似于格式化字符串的方式，可以输出字符串和变量的值。format参数是一个格式化字符串，可以包含占位符%，用于指定输出格式和位置。...表示可变参数列表，可以传入需要输出的变量或值。
4、执行函数
void execute(const vloam_main::vloam_mainGoalConstPtr& goal, Server* as)
{该函数是一个 ROS 动作服务器的执行函数
result.loading_finished = false; 初始化动作执行结果中的 loading_finished 标志为 false，表示加载未完成。
第一部分：初始化
// section1, generate new message filter and tf_buffer
message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(20), *sub_image00_ptr,
*sub_camera00_ptr,
*sub_velodyne_ptr);
这行代码是在ROS中使用的消息过滤器 message_filters::Synchronizer的实例化过程。实例化的对象名称是sync。
-
message_filters::Synchronizer<MySyncPolicy>：创建了一个同步器对象，用于同步多个话题的消息。这里使用了自定义的同步策略MySyncPolicy，它决定了同步的方式。 -
sync(MySyncPolicy(20), *sub_image00_ptr, *sub_camera00_ptr, *sub_velodyne_ptr)：这是对同步器对象的构造函数的调用。参数MySyncPolicy(20)指定了同步策略，并且20表示消息缓冲区的大小，用于在处理不同话题的消息时保持同步性。后面的*sub_image00_ptr、*sub_camera00_ptr和*sub_velodyne_ptr是用于订阅相应话题的指针，它们将作为同步器的输入。
sync.setInterMessageLowerBound(ros::Duration(0.09));
sync.registerCallback(boost::bind(&callback, _1, _2, _3)); 第一行代码设置了消息之间的最小间隔，即两个连续消息之间允许的最小时间间隔。ros::Duration(0.09)创建了一个持续时间对象，表示间隔为0.09秒。这意味着两个消息之间的时间间隔不能小于0.09秒，否则消息过滤器将等待直到满足这个条件后再继续处理消息。
第二行代码注册了一个回调函数，用于处理同步后的消息。
boost::bind(&callback, _1, _2, _3)创建了一个函数对象，将同步后的消息作为参数传递给回调函数 callback。_1、_2、_3 表示回调函数的参数，分别对应于同步器中订阅的三个话题的消息。
// section 2, initialize all pointers
init(goal); 所有指针被初始化。init(goal) 函数可能用于初始化所需的资源或数据结构，其中 goal 是传递给执行函数的参数之一。
第二部分：回放数据集
// section 3, play the bag
start_frame = goal->start_frame;
end_frame = goal->end_frame;
ss.clear();
ss.str("");
ss << std::setw(4) << std::setfill('0') << goal->seq;
seq = std::string(ss.str());
cmd = "rosbag play " + ros::package::getPath("vloam_main") + "/bags/kitti_" + goal->date + "_drive_" + seq +
"_synced.bag -d 2 -r " + std::to_string(rosbag_rate);
// TODO: add one more entry of goal for different dataset type:
// In kitti2bag, kitti_types = ["raw_synced", "odom_color", "odom_gray"]
// https://github.com/tomas789/kitti2bag/blob/bf0d46c49a77f5d5500621934ccd617d18cf776b/kitti2bag/kitti2bag.py#L264
ROS_INFO("The command is %s", cmd.c_str());
sys_ret = system(cmd.c_str());这一段和前面类似，不再赘述，主要功能是生成一个播放rosbag的linux命令：
rosbag play是一个ROS提供的命令行工具，用于播放ROS bag文件。ros::package::getPath("vloam_main")获取了ROS软件包vloam_main的路径。goal->date、seq是用于构建ROS bag文件名的一部分，可能表示日期和序列号。-d 2表示播放ROS bag文件时的延迟，默认值为0，这里设置为2秒。-r参数后面是一个浮点数，表示播放速率，根据变量rosbag_rate的值确定的。
第三部分：保存数据
// section 4, save files
if (save_traj)
{
fclose(LOFilePtr);
fclose(VOFilePtr);
fclose(MOFilePtr);
ROS_INFO("\n LO, VO, MO saved\n\n");
}
result.loading_finished = true;
as->setSucceeded(result); 检查变量 save_traj 是否为真（即非零）。如果条件成立，就执行紧跟在后面的代码块。花括号{}内的代码块：fclose(LOFilePtr);、fclose(VOFilePtr);、fclose(MOFilePtr);：这三行代码分别关闭了三个文件指针所指向的文件。这可能是在保存轨迹数据之后，关闭了与轨迹相关的文件。
ROS_INFO("\n LO, VO, MO saved\n\n");：这一行代码使用ROS的日志系统输出一条消息，表示轨迹数据已保存。\n是换行符，用于在输出中创建空行。result.loading_finished = true;：这一行代码将result对象中的loading_finished字段设置为true，表示加载已完成。as->setSucceeded(result);：这一行代码通过动作服务器（Action Server）将结果设置为成功。这可能表示在一个动作完成后，将结果标记为成功并发送给客户端。
5、main函数
int main(int argc, char** argv)
{
ros::init(argc, argv, "vloam_main_node");
ros::NodeHandle nh_private = ros::NodeHandle("~");Tips：argc 和 argv 是C++程序中的两个参数，它们通常用于处理命令行参数。在C++中，main() 函数可以接受两个参数：argc（argument count）和 argv（argument vector）。
-
argc是一个整数，表示命令行参数的数量，包括程序的名称。换句话说，argc统计了程序在运行时传递的命令行参数的个数，包括程序名称本身。例如，如果你在终端中执行./my_program arg1 arg2，那么argc的值将是 3，因为有三个参数：程序名称./my_program、arg1和arg2。 -
argv是一个指向字符指针数组的指针，它存储了每个命令行参数的字符串。argv[0]存储的是程序的名称，argv[1]存储的是第一个命令行参数，依此类推。因此，你可以通过遍历argv数组来访问所有的命令行参数。
在代码中，int main(int argc, char** argv) 表示 main() 函数接受两个参数：argc 表示命令行参数的数量，argv 是一个指向字符指针数组的指针，存储了每个命令行参数的字符串。这使得你的程序能够根据运行时传递的命令行参数执行不同的操作或者进行不同的配置。
当一个ROS节点初始化时，可以通过 ros::init(argc, argv, "node_name") 函数来指定节点的名称。节点名称是一个唯一标识符，用于在ROS系统中识别和区分不同的节点。在源代码中，节点名称被指定为 "vloam_main_node"。接下来，创建了一个私有的节点句柄 nh_private，通过 ros::NodeHandle("~") 来实现。在ROS中，节点句柄是与ROS系统通信的主要接口之一。通过节点句柄，可以发布和订阅话题、调用服务、执行动作以及管理参数等。
Tips：在ROS中，节点可以有一个全局的命名空间，也可以有一个私有的命名空间。私有命名空间是节点的一个特殊命名空间，用于存储节点特定的参数、话题和服务等。私有命名空间的优点在于它可以帮助避免参数名、话题名和服务名的冲突，特别是当节点被复用或者作为库被其他节点调用时。
在源代码中，私有节点句柄 nh_private 可以用于在私有命名空间中设置和获取参数。这样做的好处是，这些参数不会影响其他节点，也不会被其他节点修改。例如，可以通过以下方式设置一个私有命名空间下的参数：
int param_value;
nh_private.param("param_name", param_value, default_value);
这行代码会在私有命名空间下查找名为 “param_name” 的参数，如果找到则将其值赋给 param_value，否则使用 default_value 作为默认值。
除了参数配置，私有节点句柄还可以用于创建和管理私有命名空间下的话题、服务等。这可以通过在构造函数中传递参数来实现，例如：
ros::Publisher pub = nh_private.advertise<your_message_type>("topic_name", queue_size);
这行代码创建了一个发布者，将消息类型为 your_message_type 的消息发布到名为 “topic_name” 的话题上，队列大小为 queue_size。
nh_private.getParam("rosbag_rate", rosbag_rate);
nh_private.getParam("visualize_depth", visualize_depth);
nh_private.getParam("publish_point_cloud", publish_point_cloud);
nh_private.getParam("save_traj", save_traj);获取四个参数的数值，这四个参数在后面的launch文件中设置，代表的意义如下：
- rosbag_rate ：rosbag的播放倍率
- visualize_depth ：控制是否可视化深度信息
- publish_point_cloud ：控制是否发布点云数据
- save_traj ：控制是否保存数据
if (!ros::param::get("detach_VO_LO", detach_VO_LO))
ROS_BREAK();
ros::NodeHandle nh;这里标注一下，源代码if 后面没有{}，没有形成代码块导致后面的ROS_BREAK()函数必被触发，等之后进行验证。（检测这里的语法）
sub_image00_ptr =
std::make_shared<message_filters::Subscriber<sensor_msgs::Image>>(nh, "/kitti/camera_gray_left/image_raw", 10);
sub_camera00_ptr = std::make_shared<message_filters::Subscriber<sensor_msgs::CameraInfo>>(
nh, "/kitti/camera_gray_left/camera_info", 10);
sub_velodyne_ptr = std::make_shared<message_filters::Subscriber<sensor_msgs::PointCloud2>>(
nh, "/kitti/velo/pointcloud", 10); // TODO: change the buffer size to potentially reduce the data lost这段代码创建了三个消息订阅器的共享指针，用于订阅来自三个不同主题的消息。使用第一个举例，后面两个都是一样的。
前两行代码创建了一个共享指针sub_image00_ptr，指向一个message_filters::Subscriber对象，用于订阅 /kitti/camera_gray_left/image_raw 主题的 sensor_msgs::Image 类型的消息。10 是订阅器的队列大小，指定了可以在队列中缓冲的消息数量。这意味着如果消息处理不及时，最多可以缓冲10条消息。
Server server(nh, "load_small_dataset_action_server", boost::bind(&execute, _1, &server), false);
server.start(); 创建了一个名为 server 的服务器对象，使用了 Server 类型的构造函数。构造函数接受四个参数：
nh：ROS 节点句柄，用于与 ROS 系统通信。"load_small_dataset_action_server"：服务器的名称，这里是自定义的名称，用于标识该服务器。boost::bind(&execute, _1, &server)：这是一个函数对象，使用了boost::bind函数，将execute函数与服务器对象绑定。这表示当有动作请求时，会调用execute函数来处理请求。（execute函数就是上面的执行函数）false：这个参数表示服务器不自动启动，需要手动调用server.start()启动服务器。
Tips：在C++中，_1通常用作占位符。这是来自Boost库中的一种占位符，通常与boost::bind结合使用，用于创建函数对象。在源代码中，_1被用作boost::bind函数的参数，用于指示绑定的函数中的第一个参数应该由调用函数时提供的第一个参数来替换。
举个例子，如果有一个函数 void myFunction(int x, double y)，并且你想创建一个函数对象，但只绑定了第一个参数，那么你可以这样做：
boost::bind(myFunction, _1, 3.14);
这样创建的函数对象将接受一个int类型的参数作为第一个参数，而第二个参数将会是3.14。调用这个函数对象时，你需要提供一个int类型的参数作为第一个参数，而第二个参数将会是3.14。
ros::Rate loop_rate(100);
while (ros::ok())
{
ros::spinOnce();
loop_rate.sleep();
}
return 0;
} 这段代码是一个常见的 ROS 节点的主循环结构。第一行创建了一个ros::Rate对象 loop_rate，它会设置一个循环的频率，这里是每秒循环100次。这意味着循环体内的代码将以100Hz的频率执行。
主循环结构。ros::ok() 函数会在 ROS 节点没有收到关闭请求时返回 true，所以这个循环会一直运行直到收到关闭请求。在循环中，ros::spinOnce() 函数用于处理所有的回调函数。ROS 的消息订阅、发布和服务回调等都是通过 spinOnce() 函数来处理的。这确保了在主循环中可以及时处理来自 ROS 系统的所有事件。loop_rate.sleep() 函数会根据之前设置的频率来控制循环的速率，它会让循环暂停一段时间，以使得循环达到预设的频率。
终于把主函数部分代码撸完了，撒花~
二、./launch/vloam_main.launch文件
<launch>
<include file="$(find lidar_odometry_mapping)/launch/loam_velodyne_HDL_64_kitti.launch" />
<param name="detach_VO_LO" type="bool" value="true" />
<param name="reset_VO_to_identity" type="bool" value="false" />
<param name="remove_VO_outlier" type="int" value="100" />
<param name="keypoint_NMS" type="bool" value="false"/>
<param name="CLAHE" type="bool" value="false"/>
<param name="visualize_optical_flow" type="bool" value="true" />
<param name="optical_flow_match" type="bool" value="false" />
<node pkg="vloam_main" type="vloam_main_node" name="vloam_main_node" output="screen">
<param name="rosbag_rate" type="double" value="0.33" />
<param name="visualize_depth" type="bool" value="false" />
<param name="publish_point_cloud" type="bool" value="true" />
<param name="save_traj" type="bool" value="true" />
</node>
<node type="rviz" name="rviz" pkg="rviz" args="-d $(find vloam_main)/rviz/vloam.rviz" />
</launch>三、./action/vloam_main.action文件
在 ROS 中，可以通过创建 .action 文件来定义自定义的动作消息类型。通常情况下，这些 .action 文件会存放在ROS 包（package）的 action 目录中。例如，如果ROS 包名为 vloam_main，那么需要在 vloam_main 包的 action 目录下创建名为 vloam_main.action 的文件，其中定义了 vloam_mainAction 这个动作消息类型的结构。
# Define the goal
string date
string seq
int32 start_frame
int32 end_frame
---
# Define the result
bool loading_finished
---
# Define a feedback message
float32 loading_completion_percent 这段代码定义了一个名为 vloam_mainAction 的 ROS 动作消息类型。它包括了三个部分：
-
目标（Goal）：
string date：日期信息string seq：序列信息int32 start_frame：起始帧int32 end_frame：结束帧
-
结果（Result）：
bool loading_finished：加载是否完成的标志
-
反馈（Feedback）：
float32 loading_completion_percent：加载完成百分比
这些定义将用于在 ROS 中的动作服务器（Action Server）和客户端之间传递相关信息。
（持续更新中......）
扫一扫
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
