ETH Programming for Robotics--- Introduction to ROS作业及其初学ROS心得

ROS系统很强大，学习起来坑很多，时间并不是花在写代码分析问题上面，多数时候都在baidu bug, 并莫名一脸懵逼。 经常一个bug解决完一天就过去了。所以学习ROS还是适合找几个小伙伴一起研究分享效率更高。

ETH这5次作业，看起来似乎不难，但是。。。极其费时间。打个比方，前面我在python3.6下干了很长时间，各种不匹配的出错，baidu上一个一个找权衡的办法，后来火大了，干脆装anaconda用python2.7, 然后发现，原来前面几天都做了无用功，时间都白白打了水漂。
ETH的作业有个特点，就是不是光对照教材和视频就能完成的，需要去查资料找各种细节，很多地方我是找到了相关细节的例程才知道具体怎么用的。不过做完了看着自己的无人车自己开车撞电线杆子，还是蛮有成就感的：）

关于课程主页与题目本身，在这里http://www.rsl.ethz.ch/education-students/lectures/ros.html，relay zhang老师博客https://blog.csdn.net/ZhangRelay/article/details/79463689写的也很详细，懒得打字了。具体聊几个小细节。

1. 关于画图工具与利用共享资源的意识

一个是第四题的画图，ros本身的3d建模和视觉效果非常强，但是画曲线的功能却比较弱，自带的rqt几个功能连曲线的横坐标都没得选，只能是时间。所以ETH课程推荐了multiplot package，但是坑就坑在这个包我用不了，不管上直接装binary还是build 源码，结果都是段错误，核心已转存。搜索了很久都没找到能解决的办法，后来想算了不就是画图么，我自己移植其他的画图工具包不好么？有gnuplot，有opencv, 还有Python的matplotlib，都试了一遍，但是作为初学者，对ros之外的包的融合还是不能搞定，即便把cmakelist改了还是不能正常实现，都快要放弃了突然想起来，这么重要的功能肯定其他人做过了啊，于是去github还有ros.org下面搜，一下出来好多，挑了几个发现一个plotJuggler非常好用，于是终于在这里解决了。一个教训就是，有利用已有共享资源的意识，在ros中太重要了。

2. EKF与码盘的理解

从第三题开始，题目开始引导着构建一个闭合控制回路,说实在的，题目出的好，才是我花时间做题的动力。作为一个纯控制出身的，看着车子满屏幕的跑，比整天对着的动态方程和曲线，有着不一样的兴奋，但是，新鲜过去了，只有车子没有对比曲线显然完全不能给人安全感，我连好坏都无从对比啊。于是会画图了之后，终于找到了熟悉的感觉。在画出了和视频中一样的曲线后，就是下图右下角
我在想，这个图是ekf给出的轨迹，那准不准啊？没对比哪知道？所以有了上图其他几条曲线，也就是用码盘给出的odometry信息，和处理前的原始信息比终归没错。然而，大吃一惊的是，这差的也太远了。。。
然后我分析了一下，找到了ekf package的文档，然后先把配置文件localization.yaml拷贝到自己目录下，把里面初值改了，在最后加入了
initial_state: [28, 12, 0,
0, 0, 0,
0, 0, 0,
0, 0, 0,
0, 0, 0]
然后变成了这样
初值对上了，然而曲线还是不对啊。事实上，我的初值显然只给了位置的初始估计，姿态的初值依然是0，所以看到的轨迹连运动大方向都对不上了。当然，如果我连姿态初值也给的差不多，那么轨迹也就大体能对上了。然而我没有这么做，因为这tm是大名鼎鼎的ekf啊，一个初值的影响都搞不定（虽然根据lindquist的理论结果，kf的初值对系统影响确实比较大，但是在初始位置都给的差不多的情况下，说初值差的远已经说不过去了），肯定其他地方出问题了。然后我看到了配置文件的其他地方：
odom0: husky_velocity_controller/odom
#odom0_config: [false, false, false, # not estimate position, noly velocity, why? of course position estimation is needed
odom0_config: [false, false, false,
false, false, false,
true, true, true,
false, false, true,
false, false, false]
根据http://docs.ros.org/melodic/api/robot_localization/html/state_estimation_nodes.html#sensor-config，这是码盘的配置矩阵，5行分别是位置，姿态，线速度，角速度，线加速度。我注意到ekf并不使用码盘的位置信息做融合，三个值全都是false, 我想了一下我理解的ekf，从理论上我完全想不到有不用位置的理由，所以果断改掉，改成3个true ! 结果是显著的：
这下完全吻合了。好开心, 因为我似乎通过改配置文件，把轨迹的跟踪精度大大提高了。然后我就去relay zhang老师的博客下面显摆了一下。 然而，这并不是故事的结局。。。
在第五题的时候，显然我做了同样的事情，这次我嫌麻烦，连配置文件都懒得拷过来，直接去husky_control pkg里面用sudo强行改了原配置文件（幸亏我有备份原文件的好习惯），一劳永逸。改完之后，小车的轨线估计（只看top两幅图就好，中左的矩形波是手动service启动停止的结果）从
变成了
看似跟踪完美，然而，真相却是残酷的： 我的车什么时候跑到y轴负半平面去了？！！！ 我的车在pid 负反馈下一直从来都是向着电线杆子走的直线啊！！！电线杆子在第一象限啊！！！有gazebo图为证：
这货从来一直都在走直线啊！！！我差点没崩溃了！那我的ekf估计的是啥啊。。。
反省：
经过多次仿真观察，终于有了结果。
首先，事实一：gazebo的物理模型做的太好了，对真实世界的模型远远超出了几个微分方程的刻画。在汽车启停和转弯的时候，轮胎对地面的相对摩擦，是经常发生的。甚至车子有轮胎短暂腾空的时候，这些在gazebo里面也都会发生。这些时候，码盘的读数，并不是车体的真实位移。
事实二：我在设计控制器的时候，为了夸大视觉效果，加大了控制量，比例控制系数尽量给的比较大，以至于每次车体启动，车身都会倾斜并腾空。。。出现这种情况的原因可能还有自带的速度环husky_velocity_control做的不太好，我记得好像右转的时候限制了右前轮是速度为0，所以后轮加速的情况下容易侧翻。启动侧翻图这样：（截的效果不太好）
侧翻时车体倾斜，以至于激光雷达扫到地面，并把扫到的地面最近点当成目标的rviz图：（图中红线是激光在地面扫描点，绿柱子是最近的一个点，被误认为是柱子。
由于以上两点，在码盘经常与地面相对滑动时候，不要依赖码盘来读位移，才是ekf的正确使用方法！所以，码盘主要采用线速度，而陀螺仪主要采用角速度。所以husky control包既然被当成tutorial，这里的基本配置还是值得学习的。幸亏配置文件我备份了，赶紧改回去。
然后前面的正确的图我再贴一遍。。。也就是说，这个图
或者这个图

才是对的，ekf给出了精确的跟踪轨迹。
然后是时候回到第四题了，第四题是直接读bag file信息，没有gazebo的物理信息，所以本质上讲无法判断真实轨迹的样子，但是上面的教训告诉我，ekf给出的信息比码盘可靠，也就是原视频的轨线结果更接近真实值。为了在rviz中直观显示出来，我自己又单独写了一个节点，功能是把ekf和码盘的给定当前坐标，作为path topic发布出去，然后在rviz里面添加topic显示就可以了（这个节点以后在其他地方也可以用），效果如下：
绿色的是ekf给的路径，青色的是码盘路径，显而易见rviz采用的是ekf数据来显示车体的位置，这一点从rqt graph可见：

3. 关于倾车现象

最后稍微说一点车体倾斜和翻车现象。目前出现的情况有：加速前进，急刹车，急转弯，造成的倾斜，还有压过电线杆并翻车，还有两次一启动就翻车的情况，好像是因为比例控制参数调的太大了，并不是每次都能重现。倾斜的后果就激光雷达照到地面，以为目标在眼前或者身后，造成车头乱晃，所以我的曲线一般在启动阶段都会车头乱颤(在身后时扫描最大角度是2.35弧度，即正负135度，我设的比例控制系数是P=2.5，所以2.35*2.5=5.85，所以才有图上接近6的速度指令振荡），想消除减小控制量就好了，或者其实应该通过分析扫描到的直线判断这不是一个柱子，从而重新寻找远方的柱子。另外这台车的设计本身也由于车轮不能偏转，感觉容易倒。

终于告一个段落了，做完这几次作业，才有了ROS算基本入门的感觉。后续，有空读一下ekf的代码实现，这个代码的应用非常灵活，之前没想到有这么多自由度。另外看看机器人控制，还有导航有关的package。