使用chrono替换ros::Time::now()赋值给sensor_msgs::LaserScan消息中的header.stamp

使用chrono替换ros::Time::now()赋值给sensor_msgs::LaserScan消息中的header.stamp

使用std::chrono为sensor_msgs::LaserScan消息中的header.stamp字段赋值，可以考虑以下四种情况：
情况一：您的ROS版本支持直接使用std::chrono::time_point
尽管目前大多数ROS版本不直接支持将std::chrono::time_point赋值给header.stamp，但假设ROS版本或某个第三方库（如ros_chrono）提供了这样的支持，可以按照以下方式操作：

#include "sensor_msgs/LaserScan.h"
#include <chrono>

// 假设您已经创建了 LaserScan 消息实例：
sensor_msgs::LaserScan laser_scan_msg;

// 使用 std::chrono 获取当前时间点
//  std::chrono::steady_clock::now();
auto current_time = std::chrono::system_clock::now();

// 直接将当前时间点赋值给 LaserScan 消息的 header.stamp 字段
// 注意：此操作需要您的ROS版本支持将 std::chrono::time_point 赋值给 header.stamp
laser_scan_msg.header.stamp = current_time;

情况二：ROS版本不支持直接使用std::chrono::time_point
对于大多数现有的ROS版本，它们并不直接支持将std::chrono::time_point赋值给header.stamp。在这种情况下，需要通过某种方式将std::chrono::time_point转换为ROS可以接受的时间格式。虽然无法直接避免使用ros::Time，但可以尽量减少对其的依赖：

#include "sensor_msgs/LaserScan.h"
#include <chrono>

// 假设您已经创建了 LaserScan 消息实例：
sensor_msgs::LaserScan laser_scan_msg;

// 定义一个辅助函数，用于将 std::chrono::time_point 转换为 ros::Time
ros::Time to_ros_time(const std::chrono::system_clock::time_point& tp) {
    using namespace std::chrono;
    typedef duration<int64_t, std::ratio<1, 1000000000>> ns_type;
    return ros::Time{duration_cast<ns_type>(tp.time_since_epoch()).count()};
}

// 使用 std::chrono 获取当前时间点
//  std::chrono::steady_clock::now();
auto current_time = std::chrono::system_clock::now();

// 将当前时间点转换为 ROS 时间
ros::Time current_ros_time = to_ros_time(current_time);

// 赋值给 LaserScan 消息的 header.stamp 字段
laser_scan_msg.header.stamp = current_ros_time;

这里定义了一个辅助函数to_ros_time，它接受一个std::chrono::system_clock::time_point对象，并将其转换为ros::Time。这样，虽然在赋值给header.stamp时仍使用了ros::Time，但在其余代码中可以尽可能地使用std::chrono进行时间处理，仅在与ROS接口交互时进行必要的转换。
总结来说，如果确定您的ROS版本支持直接使用std::chrono::time_point，则可以直接赋值；否则，需要编写一个辅助函数将std::chrono::time_point转换为ros::Time后再进行赋值。在实际开发中，由于大多数ROS版本并不直接支持std::chrono::time_point，通常建议采用第二种方法。

情况三：使用std::chrono获取当前系统时间，并将其转换为sensor_msgs::Header::stamp结构所期望的秒（sec）和纳秒（nsec）形式。尽管这种方法没有直接使用ros::Time，但它实现了相同的功能，即将当前时间以ROS兼容的方式设置到sensor_msgs::LaserScan消息的header.stamp字段。

#include "sensor_msgs/LaserScan.h"
#include <chrono>

// 设置LaserScan消息
sensor_msgs::LaserScan output;

// 获取当前系统时间
//  std::chrono::steady_clock::now();
auto now = std::chrono::system_clock::now();

// 将当前系统时间转换为ROS时间戳
auto now_sec = std::chrono::time_point_cast<std::chrono::seconds>(now);
auto fraction = now - now_sec;
auto now_nsec = std::chrono::duration_cast<std::chrono::nanoseconds>(fraction);

output.header.stamp.sec = now_sec.time_since_epoch().count();
output.header.stamp.nsec = now_nsec.count();

这段代码可以正常工作，因为它：
使用std::chrono::system_clock::now()获取当前系统时间。
将当前时间向下舍入到最近的秒，存储在now_sec中。
计算now与now_sec之间的差值（即剩余的亚秒部分），存储在fraction中。
将fraction转换为纳秒精度的duration，存储在now_nsec中。
将now_sec和now_nsec分别转换为其对应的计数值（以秒和纳秒为单位），并赋值给output.header.stamp.sec和output.header.stamp.nsec。
这样，output.header.stamp就包含了与当前系统时间相对应的ROS时间戳，可以被正确地用于ROS消息发布。
虽然这种方法没有直接使用ros::Time类型，但它通过手动计算实现了类似的效果。如果您确实希望避免直接使用ros::Time，这种方法是可以接受的。不过，需要注意的是，这种方法相对于直接使用ros::Time可能会显得较为繁琐，特别是当您在多个地方需要进行类似的转换时。在大多数情况下，直接使用ros::Time（如前文所示）可能更具可读性和简洁性。

情况四：要使用C++11中的chrono库替换ros::Time::now()，您可以使用std::chrono::system_clock::now()来获取当前系统时间。然后，您需要将这个时间转换为ROS时间戳。

以下是如何修改代码以使用chrono替换ros::Time::now()：

#include <chrono>
#include <sensor_msgs/LaserScan.h>
void pointCloudCallback(const sensor_msgs::PointCloud2& cloud_msg) {
    // 设置LaserScan消息
    sensor_msgs::LaserScan output;
    
    // 获取当前系统时间
//  std::chrono::steady_clock::now();
    auto now = std::chrono::system_clock::now();

    // 将当前系统时间转换为ROS时间戳
    auto now_sec = std::chrono::time_point_cast<std::chrono::seconds>(now);
    auto fraction = now - now_sec;
    auto now_nsec = std::chrono::duration_cast<std::chrono::nanoseconds>(fraction);

    output.header.stamp.sec = now_sec.time_since_epoch().count();
    output.header.stamp.nsec = now_nsec.count();

    // 其他代码保持不变
    output.header = cloud_msg.header; // 保留原始header信息
    output.header.frame_id=scan_frame_id_;
    output.angle_min = angle_min_; // -90度
    output.angle_max = angle_max_; // 90度
    output.angle_increment = angle_increment_; // 1度的分辨率
    output.time_increment = 0;
    output.scan_time = scan_time_; // 扫描时间假设为0.1秒
    output.range_min = range_min_; // 最小距离
    output.range_max = range_max_; // 最大距离

    // 初始化距离数组
    int ranges_size = std::ceil((output.angle_max - output.angle_min) / output.angle_increment);
    output.ranges.assign(ranges_size, std::numeric_limits<float>::infinity());

    // 迭代点云
    sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud_msg, "x"), iter_y(cloud_msg, "y");
    for (; iter_x != iter_x.end(); ++iter_x, ++iter_y) {
        float x = *iter_x;
        float y = *iter_y;
        float angle = std::atan2(y, x);
        float range = std::hypot(x, y);

        if (angle < output.angle_min || angle > output.angle_max)
            continue;

        int index = (angle - output.angle_min) / output.angle_increment;
        if (range < output.ranges[index])
            output.ranges[index] = range; // 只保留最近的距离
    }

    // 发布LaserScan消息
    scan_pub_.publish(output);
}

这样，就可以使用chrono库来替换ros::Time::now()，并将当前系统时间转换为ROS时间戳。

输出耗时时间：

       // 匹配开始时间
       auto start2 = std::chrono::high_resolution_clock::now();

       // 匹配完成时间
       auto end2 = std::chrono::high_resolution_clock::now();
       // 计算并转换为毫秒
       auto duration2 = std::chrono::duration_cast<std::chrono::milliseconds>(end2 - start2);

      // 输出执行时间
      std::cout <<  执行时间：" << duration2.count() "<< " 秒" std::endl;

以下是一个简单的C++代码示例，使用std::chrono::high_resolution_clock来测量代码段的执行时间，并将结果转换为秒（double类型）表示：

#include <iostream>
#include <chrono>

// 待测代码段
void functionToMeasureTime() {
    // 在这里放置你想测量执行时间的代码
    for(int i = 0; i < 1000000; ++i) {
        // 示例操作
    }
}

int main() {
    // 记录开始时间
    auto startTime = std::chrono::high_resolution_clock::now();

    // 执行待测代码段
    functionToMeasureTime();

    // 记录结束时间
    auto endTime = std::chrono::high_resolution_clock::now();

    // 计算并转换执行时间到秒（double类型）
    std::chrono::duration<double> duration = endTime - startTime;
    double executionTimeInSeconds = duration.count();

    // 输出结果
    std::cout << "代码执行耗时: " << executionTimeInSeconds << " 秒" << std::endl;

    return 0;
}

#include <iostream>
#include <chrono>

// 待测代码段
void functionToMeasureTime() {
    // 在这里放置你想测量执行时间的代码
    for(int i = 0; i < 1000000; ++i) {
        // 示例操作
    }
}

int main() {
    // 明确指定变量类型
    std::chrono::time_point<std::chrono::high_resolution_clock> startTime, endTime;

    // 记录开始时间
    startTime = std::chrono::high_resolution_clock::now();

    // 执行待测代码段
    functionToMeasureTime();

    // 记录结束时间
    endTime = std::chrono::high_resolution_clock::now();

    // 计算并转换执行时间到秒（double类型）
    std::chrono::duration<double> duration = endTime - startTime;
    double executionTimeInSeconds = duration.count();

    // 输出结果
    std::cout << "代码执行耗时: " << executionTimeInSeconds << " 秒" << std::endl;

    return 0;
}

std::chrono::high_resolution_clock, std::chrono::system_clock, 以及 std::chrono::steady_clock 都属于 <chrono> 库中的时钟类型，各自有其特点和适用场景。下面是它们的主要区别：
std::chrono::high_resolution_clock:
特性: 提供可能的最高分辨率计时，适用于需要精密时间间隔测量的场景，如性能测试。
精度与分辨率: 非常高，但具体取决于硬件和操作系统。
单调性: 不保证单调递增，跨进程使用可能不一致。
时间起点: 未指定，不适合表示具体日期时间。
std::chrono::system_clock:
特性: 与系统的真实时间（如UTC或本地时间）同步，可用于获取或设置日期和时间。
精度与分辨率: 较高，但相比high_resolution_clock可能较低，足以满足日常时间记录需求。
单调性: 保证单调递增，时间点具有绝对意义。
时间起点: 通常为Unix纪元（1970年1月1日 00:00:00 UTC），可转换为人类可读的时间格式。
std::chrono::steady_clock:
特性: 专为测量时间间隔设计，保证单调递增，不受系统时间调整影响，适合计时器和延迟操作。
精度与分辨率: 高，适合需要稳定、不受系统时间改变影响的计时场景。
单调性: 始终单调递增，即使用户修改了系统时间。
时间起点: 未定义，仅用于计算时间差，不能直接关联到真实日期时间。
总结:
如果你需要测量代码段的执行时间或进行性能测试，high_resolution_clock 或 steady_clock 是更好的选择，其中 steady_clock 更适合需要稳定不变时间基准的场景。
当你需要获取或设置与现实世界对应的时间信息时，应使用 system_clock。
若要避免系统时间调整的影响，确保时间间隔测量的准确性，steady_clock 是最合适的选项。