teb_local_planner编译_integrated online trajectory planning and optimiza

项目场景：

ROS导航框架下的一个局部路径规划插件源码地址

---

相关文献：

C. Rösmann, F. Hoffmann and T. Bertram: Integrated online trajectory planning and optimization in distinctive topologies, Robotics and Autonomous Systems, Vol. 88, 2017, pp. 142–153. C. Rösmann, W. Feiten, T. Wösch, F. Hoffmann and T. Bertram: Trajectory modification considering dynamic constraints of autonomous robots. Proc. 7th German Conference on Robotics, Germany, Munich, May 2012, pp 74–79. C. Rösmann, W. Feiten, T. Wösch, F. Hoffmann and T. Bertram: Efficient trajectory optimization using a sparse model. Proc. IEEE European Conference on Mobile Robots, Spain, Barcelona, Sept. 2013, pp. 138–143. C. Rösmann, F. Hoffmann and T. Bertram: Planning of Multiple Robot Trajectories in Distinctive Topologies, Proc. IEEE European Conference on Mobile Robots, UK, Lincoln, Sept. 2015. C. Rösmann, F. Hoffmann and T. Bertram: Kinodynamic Trajectory Optimization and Control for Car-Like Robots, IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Vancouver, BC, Canada, Sept. 2017.

---

导航包安装：

首先用gmapping建立实验室地图 链接在此

创客智造上已经有的我就不重复了，我只写一下需要注意的事项。

全部流程走完，amcl导航应该可以用了，下一步就是加入teb插件。参考这篇博客

下载到工作空间之后

rosdep install teb_local_planner
1

报错:

ERROR: Rosdep cannot find all required resources to answer your query
Missing resource teb_local_planner
ROS path [0]=/opt/ros/indigo/share/ros
ROS path [1]=/home/turtlebot/turtlebot_ws/src
ROS path [2]=/home/turtlebot/catkin_ws/src
ROS path [3]=/opt/ros/indigo/share
ROS path [4]=/opt/ros/indigo/stacks
1
2
3
4
5
6
7

这一步我是肯定知道要报错的,source一下之后,再报错:

ERROR: the following packages/stacks could not have their rosdep keys resolved
to system dependencies:
teb_local_planner: Cannot locate rosdep definition for [visualization_msgs]
1
2
3

好怀念没重装系统之前,一下就编译过去了.

rosdep install --from-paths src --ignore-src --rosdistro=indigo -y
1

报错:

ERROR: the following packages/stacks could not have their rosdep keys resolved
to system dependencies:
teb_local_planner: Cannot locate rosdep definition for [costmap_converter]
1
2
3

再来

rosdep install --from-paths ~/turtlebot_ws/src/teb_local_planner-indigo-devel --ignore-src -r
1

也不行:

ERROR: the following packages/stacks could not have their rosdep keys resolved
to system dependencies:
teb_local_planner: Cannot locate rosdep definition for [costmap_converter]
Continuing to install resolvable dependencies...
#All required rosdeps installed successfully
1
2
3
4
5

上网一查发现有人遇到过这个问题

The problem is that costmap_converter is also not released into Hydro (see wiki/costmap_converter). You'll have to try and see whether you can build that from source on Hydro as well. Just place the package in the same workspace as where you have teb_local_planner. But again: no guarantees that this will work.
1

大意是说costmap_converter没有发行在Hydro上(当然也没有在indigo上),上wiki一查果然没有

sudo apt-get install ros-indigo-costmap-converter
1

再

rosdep install --from-paths ~/turtlebot_ws/src/teb_local_planner-indigo-devel --ignore-src -r

1
2

显示OK

#All required rosdeps installed successfully
1

单独编译teb:

catkin_make -DCATKIN_WHITELIST_PACKAGES=“~/turtlebot_ws/src/teb_local_planner”
1

查看是否编译完成:

rospack plugins --attrib=plugin nav_core
1

成功

move_slow_and_clear /opt/ros/indigo/share/move_slow_and_clear/recovery_plugin.xml
base_local_planner /opt/ros/indigo/share/base_local_planner/blp_plugin.xml
clear_costmap_recovery /opt/ros/indigo/share/clear_costmap_recovery/ccr_plugin.xml
dwa_local_planner /opt/ros/indigo/share/dwa_local_planner/blp_plugin.xml
global_planner /opt/ros/indigo/share/global_planner/bgp_plugin.xml
navfn /opt/ros/indigo/share/navfn/bgp_plugin.xml
rotate_recovery /opt/ros/indigo/share/rotate_recovery/rotate_plugin.xml
teb_local_planner /home/turtlebot/turtlebot_ws/src/teb_local_planner/teb_local_planner_plugin.xml
carrot_planner /opt/ros/indigo/share/carrot_planner/bgp_plugin.xml

1
2
3
4
5
6
7
8
9
10

之后继续这篇博客

---

ROS与MATLAB通讯：

参考这篇博客
想把机器人运行时的速度和角速度画出来，科研的每一步都如同在黑暗中摸索前行，所以说难呀。

一、主从机连上同一局域网，直接用笔记本开热点：

二、修改ubuntu端的/etc/hosts文件，添加：

172.27.77.110 LAPTOP-62K6ND8C
1

三、matlab端输入：（设置ROS主机的IP地址为Ubuntu端电脑的IP）

setenv('ROS_MASTER_URI','http://192.168.137.125:11311')
启动ROS:rosinit
关闭ROS:rosshutdown
查看话题：rostopic list
1
2
3
4

成功：

四、接收信息并画图

laser = rossubscriber('/base_scan');
scandata = receive(laser,10);
plot(scandata)
1
2
3

添加地图转换：

就算科研的每一步都举步维艰，我们也要负重前行。

突然忘记怎么加插件的了，遇事不决上wiki

也可以参考教程里面的写法。

DWA导航实验：

运行DWA是为了和TEB作对比,再跑一遍是因为之前是用激光跑的,这次用相机的伪激光数据跑,不知道效果能不能好点:

roslaunch turtlebot_bringup minimal.launch
roslaunch turtlebot_rviz_launchers view_navigation.launch
roslaunch turtlebot_navigation amcl_demo.launch map_file:=/home/turtlebot/map/hokuyo_gmapping3.yaml
1
2
3

效果非常不好(我确实想让他差点但是这差的有点过头了),

没跑两步开始转圈(转好几次):

[ WARN] [1608269171.842333253]: DWA planner failed to produce path.
[ WARN] [1608269177.040261644]: Clearing costmap to unstuck robot (3.000000m).
[ WARN] [1608269182.240274629]: Rotate recovery behavior started.
[ WARN] [1608269187.640244875]: Clearing costmap to unstuck robot (1.840000m).
[ WARN] [1608269192.840292163]: Rotate recovery behavior started.
1
2
3
4
5

狭窄通道处:

[ WARN] [1608269342.893989734]: Clearing costmap to unstuck robot (1.840000m).
[ WARN] [1608269343.450923452]: Costmap2DROS transform timeout. Current time: 1608269343.4508, global_pose stamp: 1608269342.9126, tolerance: 0.5000
[ WARN] [1608269343.450990650]: Could not get robot pose, cancelling pose reconfiguration
[ WARN] [1608269343.496715714]: Unable to get starting pose of robot, unable to create global plan
[ WARN] [1608269343.693770662]: Unable to get starting pose of robot, unable to create global plan
[ WARN] [1608269343.893746296]: Unable to get starting pose of robot, unable to create global plan
[ WARN] [1608269344.093846252]: Unable to get starting pose of robot, unable to create global plan
[ WARN] [1608269344.293736232]: Unable to get starting pose of robot, unable to create global plan
1
2
3
4
5
6
7
8

直接过不了，难道说换了相机之后效果变差了？
经过无数次实验失败，怎么调参都不行那种，我终于可以断定用深度相机做导航确实很垃圾，深度数据转换后激光数据的精度和真正的激光数据完全不在一个档次上，机器人走不了两步就开始转圈（我本来指望相机能把初始位姿这个问题搞定），还是老老实实用激光吧。
之前改的程序因为师弟重装系统无了，我对怎么改毫无头绪，翻了好久才把参考的文章找到。https://blog.csdn.net/kuizhao8951/article/details/103889759注意xml文件不能复制粘贴。
想起来可以使用惯导显示速度和加速度，参考链接。

前几天想用imu显示速度曲线结果一直没成功，然后一直死磕，最后以系统崩坏收场，我前期那么多东西全部付诸东流，现在就是非常后悔。现在仔细想想也就是gcc版本问题，但是这个东西涉及到系统内核，实在非常危险，那天查了无数资料，又是建软链接（软链接也不是什么好东西，不要轻易碰）又是下东西，后来软链接层数加多了，系统直接找不到文件了，然后又开始疯狂删，一不小心把不该删的东西给删掉了，导致apt-get失效，万事休矣。

今天找到一个不用正则表达式的方式：

file_path="http://1.1.1.1:9001/group1/M00/00/37/wKgfdVoNSTKAKXAzAAD25Sg6ZTE5747.gz";（这里写你自己的路径）
file_path.substr(file_path.rfind("/")+1);  （可惜还是不好用）
1
2

gcc版本说到底也就是c++版本的问题，因为c++11增加了一些新特性，所以普通gcc编译不过去肯定报错。版本问题确实麻烦一会这个不好使一会那个编译不过，什么时候能出一个兼容一切的版本就好了。

TEB仿真实验：

内容不够，百般无奈，加点仿真好了。照着Tutorials上面做吧。

roslaunch teb_local_planner test_optim_node.launch
1

报错：

ERROR: cannot launch node of type [teb_local_planner/test_optim_node]: can't locate node [test_optim_node] in package [teb_local_planner]
1

检查odom：

百度了一下，似乎是没有生成可执行文件，可以使用手动make来解决，在对应的项目文件中新建build文件夹，确保build文件夹上一级含有cmakeLists文件，然后使用下面的命令进行构建，注意每次更改程序之后都需要手动重新进行make。

cmake ..
make
1
2

看样子在工作空间根目录下直接catkin_make不好使,成功

速度可视化：

rosrun teb_local_planner_tutorials visualize_velocity_profile.py
1

报错：

Traceback (most recent call last):
  File "/home/cjt/turtlebot_ws/src/teb_local_planner_tutorials/scripts/visualize_velocity_profile.py", line 9, in <module>
    from teb_local_planner.msg import FeedbackMsg, TrajectoryMsg, TrajectoryPointMsg
ImportError: No module named teb_local_planner.msg
1
2
3
4

因为脚本是用python编写的，涉及到环境变量的问题，经过无数次失败，我终于有点眉目了。

设置python环境参考：

cjt@cjt-ubuntu:~$ python
Python 2.7.6 (default, Nov 13 2018, 12:45:42) 
[GCC 4.8.4] on linux2
Type "help", "copyright", "credits" or "license" for more information.
>>> import sys
>>> sys.path
['', '/home/cjt/turtlebot_ws/devel/lib/python2.7/dist-packages', '/opt/ros/indigo/lib/python2.7/dist-packages', '/usr/lib/python2.7', '/usr/lib/python2.7/plat-x86_64-linux-gnu', '/usr/lib/python2.7/lib-tk', '/usr/lib/python2.7/lib-old', '/usr/lib/python2.7/lib-dynload', '/usr/local/lib/python2.7/dist-packages', '/usr/lib/python2.7/dist-packages', '/usr/lib/python2.7/dist-packages/PILcompat', '/usr/lib/python2.7/dist-packages/gtk-2.0', '/usr/lib/pymodules/python2.7', '/usr/lib/python2.7/dist-packages/ubuntu-sso-client', '/usr/lib/python2.7/dist-packages/wx-2.8-gtk2-unicode']
1
2
3
4
5
6
7

sys.path包含了一个Python解释器自动查找所需模块的路径的列表。注意列表中的第一个字符串是空的，这说明当前目录也是sys.path中的一部份，环境变量PYTHONPATH也一样。这说明你可以在程序中引入当前目录中的模块。

可以看到我们确实没有把需要的路径添加进去。

通过建立.pth文件方式，在python2.7的site-packages文件夹下中创建 .pth文件，然后将需要添加的路径写到.pth文件中去

cd /usr/local/lib/python2.7/dist-packages
sudo nano pythonpathdelf.pth（^按ctrl）
写入（我说之前为啥一直写不进去，原来是没找对地方，写入的必须是python文件才行）：/home/cjt/turtlebot_ws/src/teb_local_planner/build/devel/lib/python2.7/dist-packages/teb_local_planner/msg
1
2
3

再查OK：

cjt@cjt-ubuntu:~$ python
Python 2.7.6 (default, Nov 13 2018, 12:45:42) 
[GCC 4.8.4] on linux2
Type "help", "copyright", "credits" or "license" for more information.
>>> import sys
>>> sys.path
['', '/home/cjt/turtlebot_ws/devel/lib/python2.7/dist-packages', '/opt/ros/indigo/lib/python2.7/dist-packages', '/usr/lib/python2.7', '/usr/lib/python2.7/plat-x86_64-linux-gnu', '/usr/lib/python2.7/lib-tk', '/usr/lib/python2.7/lib-old', '/usr/lib/python2.7/lib-dynload', '/usr/local/lib/python2.7/dist-packages', '/home/cjt/turtlebot_ws/src/teb_local_planner/build/devel/lib/python2.7/dist-packages/teb_local_planner/msg', '/usr/local/lib/python2.7/dist-packages', '/usr/lib/python2.7/dist-packages', '/usr/lib/python2.7/dist-packages/PILcompat', '/usr/lib/python2.7/dist-packages/gtk-2.0', '/usr/lib/pymodules/python2.7', '/usr/lib/python2.7/dist-packages/ubuntu-sso-client', '/usr/lib/python2.7/dist-packages/wx-2.8-gtk2-unicode']
1
2
3
4
5
6
7

然而再运行还是不行，搞不懂。

百思不得其解，我又make了几次teb_local_planner那个包，然后那个包变得非常乱，我直接删了重装好了，反正再遇到什么问题都有记录的。

sudo apt-get install ros-indigo-teb-local-planner
rosdep install --from-paths src --ignore-src --rosdistro=indigo -y
1
2

再运行就可以了，实在是莫名其妙。

rosparam set /test_optim_node/publish_feedback true
roslaunch teb_local_planner test_optim_node.launch
cjt@cjt-ubuntu:~$ rosrun teb_local_planner_tutorials visualize_velocity_profile.py
[INFO] [WallTime: 1608534468.714216] Visualizing velocity profile published on '/test_optim_node/teb_feedback'.
[INFO] [WallTime: 1608534468.714789] Make sure to enable rosparam 'publish_feedback' in the teb_local_planner.
1
2
3
4
5

一些有用的链接：

添加障碍物：

chmod +x publish_test_obstacles.py
rosrun teb_local_planner_tutorials publish_test_obstacles.py
1
2

跟着教程一套走下来收获了不少仿真的内容，勉强让内容充实了一点点，然而不过是垂死挣扎。

地图转换可视化：https://answers.ros.org/question/234139/costmap-converter-visualization-teb_local_planner/

如果仅仅想运行脚本用二进制安装：

sudo apt-get install ros-indigo-teb-local-planner-tutorials
1

二进制安装就直接编译好了，相当于黑箱安装省事很多，但是如果想进行修改还是得源码安装。

调试参数的链接

vertices:[[-0.08,0.14],[0.08,0.14],[0.16,0],[0.08,-0.14],[-0.08,-0.14],[-0.16,0]]
1

能显示路径信息的插件：https://github.com/coins-lab/relaxed_astar
https://web.archive.org/web/20160913102243/http://www.iroboapp.org/index.php?title=Adding_Relaxed_Astar_Global_Path_Planner_As_Plugin_in_ROS