使用ROS的程序不易调试,可以有以下三种方法。
1. 在线播包调试
这是最简单的一种方式,因为一般情况下程序就是在线播包运行的,在线播包调试也就顺理成章。
但这种方法有严重的缺陷:调试过程很难与真实过程相同,且两次调试的运行过程可能完全不一样。这主要是因为当程序卡在一个断点处时,程序不再运行,但播包还在继续。手工暂停播包的方法也并不能精确控制时间,所以调试过程中获取的数据的时间间隔是比较随机的。
2. 离线读包调试
这种方式有两个优点,一是快速运行,程序以很快的速度到达调试点;二是问题完全复现。
但这种方法不适用于多线程调试。
3. 使用模拟器
设计一个模拟器,使用离线读包的方式获取数据,但在传递给程序时却要模拟在线的方式。也就是说虽然本质上是离线读包,但并不是每读到一个数据就直接传递给程序,而是等待一段时间再传递,等待的时间就是ROS时间间隔。
3.1. 模拟器的作用是提高开发效率
主要两个作用,分别是提高调试效率和评测效率,归根结底是提高开发效率。
3.1.1. 提高调试效率
开发工作的大部分时间都是调试,给开发人员提供可靠的单步调试方法将会大大提高调试效率。
3.1.2. 提高评测效率
评测模块希望能够使用大量数据进行运算,同时又不希望消耗过多的时间。把定位程序中的线程睡觉的时间节省下来即可。
3.2. 模拟器设计
3.2.1. 设计原则
模拟器与中间件分离,更换中间件后不需要重写模拟器
模拟器与定位算法分离,尽量不影响定位算法。
3.2.2. 设计框图
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