docker中的c++ ROS节点中使用Matplotlib-cpp

背景

有时debug算法问题，想把算法，代码的中间量快速可视化出来，目前采用rviz或者qt_ros可以实现，但都不是很方便，代码开发量较大，常常会想要是能在ros c++中也能像Matlab一样直接plot绘图就好了，正好Matplotlib-cpp库就可以解决该问题．

环境：Ubuntu18.04,c++,docker,ROS，docker内python2.7,python3.6

步骤１ 拉取Matplotlib-cpp库

git clone https://github.com/lava/matplotlib-cpp.git

可以先拉取到本地，然后复制到要显示的ROS工程里一起挂载到容器．

步骤2 启动可以显示图像的docker容器

xhost +   #先执行这步，进入docker才可以显示图像
docker run -it --net=host --name <容器名> -v /tmp/.X11-unix:/tmp/.X11 -v <你要挂载的本地路径>:<容器内路径> -e DISPLAY=$DISPLAY --privileged <镜像名>:<tag名> bash

必须要这样启动容器尤其是挂载.X11，-e操作等，进入容器后可以编译，可以先跑一下matplotlib-cpp的example看看能否在docker绘图，再进行下一步

步骤3 容器内编译安装Matplotlib-cpp

进入到容器后，可以先拉取matplotlib-cpp，或者是将本地的matplotlib-cpp挂载到容器，因为我时用到ROS工程里，可以放入ROS工程的build/ devel/这一级，以免被catkin_make编译，这里自己cmake编译．
进入到容器内ros工程

cd /matplotlib-cpp
mkdir build
cd build
cmake ..
make
make install

步骤4 容器使用环境准备

要确保你的容器里是有python2,python3的,容器内执行

apt-get install python-matplotlib
apt-get install python3-matplotlib

安装完后，可以先跑以下matplotlib-cpp的example试试

./matplotlib-cpp/build/bin/bar

如果执行完后能出现该图，说明调用matplotlib-cpp安装好了，而且docker内显示图像也ok

步骤５修改CMakeList

修改你要画图的那个ros节点的cmakelist里
这一块自己做了很多摸索，可能有些设置是不需要的，但都加上可以跑起来．

find_package(PythonLibs 2.7)  #我这里python是2.7版本，最好保持一致

include_directories(
/usr/local/include/
/usr/include/python2.7/
/usr/include/python3.6m/
)
# add for matplot
include_directories(${PYTHON_INCLUDE_DIRS})

target_link_libraries(${NODE_NAME}
#加上下面这行依赖库
${PYTHON_LIBRARIES} # add for matplot
}

步骤６一个简单的动态绘图Demo

需要在ROS工程中使用matplotlib-cpp的话，先将　拉取的matplotlib-cpp/matplotlibcpp.h拷贝到你ROS工程里的头文件路径，然后需要画图的.cc/.cpp里＃include “xxx/xxx/matplotlibcpp.h”，xxx代表你的相对路径
１．包含头文件

#include "matplotlibcpp.h"
#include <thread>   //多线程来更新图片

２．导入matplotlibcpp的命名空间

namespace plt = matplotlibcpp;

３．要plot的变量都定义成vector，这里是想画横轴为时间，纵轴分别为车速，参考加速度，实际加速度曲线,定义了一个当前时间变量

std::vector<double> time_plot, v_plot, acc_ref_plot, acc_real_plot;
double time_plot_now = 0.0;

４．在订阅车辆底盘topic的回调函数里，把各个变量最新的值往vector里塞，时间自己按照轮循的频率递增即可．放在一起塞保证各vector数量一致

time_plot.push_back(time_plot_now);
v_plot.push_back(msg->speed * 3.6);
acc_real_plot.push_back(msg->acceleration);
acc_ref_plot.push_back(g_lonctrdebug.preview_acceleration_reference);
time_plot_now = time_plot_now + 0.01;

５．定义了一个线程单独执行的画图函数

void PlotSpd() {
plt::ion();  //加这句为了动态画图
plt::figure_size(1200, 780);  //定义画图框大小
while (true) { //该线程函数里写了个死循环一直更新图像
if (time_plot.size() > 0) {  //有数据的时候才画
plt::subplot(2, 1, 1); //211含义分别为将上面figure分２行，１列，这个子图的index为１
plt::plot(time_plot, v_plot, "r*-");//该子图里画t-v曲线，r代表red,*-代表matlab里*连接每个散点
plt::title("SpeedKph");//该子图的标题
plt::subplot(2, 1, 2);//212含义分别为将上面figure分２行，１列，这个子图的index为2
plt::named_plot("acc_ref", time_plot, acc_ref_plot, "b*-");//named_plot在子图２带legend的plot
plt::named_plot("acc_real", time_plot, acc_real_plot, "r*-");
plt::legend();//将上面named_plot带的legend显示在该子图上
plt::title("acc");//该子图的标题
plt::pause(0.001);//这句非常重要，加了这句就会实时更新图像
}
}
}

６．在主线程里开辟一个线程专门调用画图函数，多线程执行减少图像显示的延迟

std::thread plot_thread(PlotSpd);