使用ROS的程序不易调试,可以有以下三种方法。

1. 在线播包调试

这是最简单的一种方式,因为一般情况下程序就是在线播包运行的,在线播包调试也就顺理成章。

但这种方法有严重的缺陷:调试过程很难与真实过程相同,且两次调试的运行过程可能完全不一样。这主要是因为当程序卡在一个断点处时,程序不再运行,但播包还在继续。手工暂停播包的方法也并不能精确控制时间,所以调试过程中获取的数据的时间间隔是比较随机的。

2. 离线读包调试

这种方式有两个优点,一是快速运行,程序以很快的速度到达调试点;二是问题完全复现。

但这种方法不适用于多线程调试。

3. 使用模拟器

设计一个模拟器,使用离线读包的方式获取数据,但在传递给程序时却要模拟在线的方式。也就是说虽然本质上是离线读包,但并不是每读到一个数据就直接传递给程序,而是等待一段时间再传递,等待的时间就是ROS时间间隔。

3.1. 模拟器的作用是提高开发效率

主要两个作用,分别是提高调试效率和评测效率,归根结底是提高开发效率。

3.1.1. 提高调试效率

开发工作的大部分时间都是调试,给开发人员提供可靠的单步调试方法将会大大提高调试效率。

3.1.2. 提高评测效率

评测模块希望能够使用大量数据进行运算,同时又不希望消耗过多的时间。把定位程序中的线程睡觉的时间节省下来即可。

3.2. 模拟器设计 

3.2.1. 设计原则

模拟器与中间件分离,更换中间件后不需要重写模拟器

模拟器与定位算法分离,尽量不影响定位算法。

3.2.2. 设计框图

修改ros2 传感器订阅qos_中间件

3.3. 实现 

#pragma once

#include <cstdlib>
#include <chrono>

class Simulator {
 public:
  using Ptr = std::shared_ptr<Simulator>;

  Simulator() = default;

  ~Simulator() = default;

  void AddImu(const Imu::Ptr &imu) {
  if (simulating_) {
    double time = imu->local_time;
    double delta_time = UpdateTime(time);
    if (delta_time > 0) {
      usleep(delta_time * 1e6);
    }
  }
  localization_manager_->AddImu(imu);
 }


 private:
  double UpdateTime(double time) {
  double delta_time(0); // The simulator need to wait for delta_time for simulating real situation
  std::chrono::time_point<std::chrono::system_clock> system_clock = std::chrono::system_clock::now();

  std::lock_guard<std::mutex> lock(mutex_);
  if (initialized_) {
    if (time > last_msg_time_) {
      std::chrono::duration<double> elapsed_seconds = system_clock - last_sys_clock_;
      double duration = elapsed_seconds.count(); // unit is second

      // time - last_msg_time_ is time difference of messages
      // However the Process has spent time duration, so we need to minus it
      delta_time = time - last_msg_time_ - duration;
      last_msg_time_ = time;
      last_sys_clock_ = system_clock;
    }
  } else {
    last_msg_time_ = time;
    last_sys_clock_ = system_clock;
    initialized_ = true;
  }

  return delta_time;
 }

 private:
  bool initialized_;
  const bool simulating_;

  std::mutex mutex_;
  double last_msg_time_;
  std::chrono::time_point<std::chrono::system_clock> last_sys_clock_;

}; // Simulator