SVN常用命令

基本操作

  • 更新代码: svn up

  • 提交代码: 先执行add再commit,就是同git类似:svn add file, 之后svn commit -m "LogMessage" file

  • 删除文件:先delete再commit,svn delete test.php 然后再svn ci -m 'delete test file'。这样就解除了SVN对这个文件的管理,但是svn 删除某文件前,应当保证该文件的修改已经提交,否则会删除失败

  • 未提交代码时,取消对代码修改:如果在当前目录下,要撤销某文件的修改:svn revert file.cpp 如果撤销本目录所有文件修改:svn revert -R .,撤销成功会有提示,否则就是失败

  • 查看提交历史:svn log file.cpp,查看目录的历史也是这个,但版本可能是不连续的,因为某些版本只改一个文件,文件夹的版本没有改变

  • 查看工作目录的SVN信息:Win或Linux系统下,在工作目录下,打开命令行执行svn info,可用于relocate或update前后确认目录是否更新。

更改SVN默认编辑器

一般在Linux上,SVN用的是nano编辑器,非常不方便,修改使用命令svn update-alternatives --config editor

星号标出了当前使用的编辑器,选择4就改成了VIM

还原到某个版本

先用svn log查看文件的提交历史,找到需要的版本,比如我们打算把test.cpp从966版本退回到955

目前有三个版本:

1
2
3
4
5
6
7
8
9
10
11
12
13
 svn log test.cpp 
-------------------------------
r966 | user | 2019-03-25 09:31:29 +0800 (一, 2019-03-25) | 1

3
-------------------------------
r965 | user | 2019-03-25 09:31:16 +0800 (一, 2019-03-25) | 1

2
-------------------------------
r950 | user | 2019-03-22 11:19:08 +0800 (五, 2019-03-22) | 1

------------------------------------------------------------------------

先看一个常见的 错误用法

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
svn up test.cpp -r 950
#正在升级 'test.cpp':
U test.cpp
#更新到版本 950。

cat test.cpp // 950版本的内容
#111111111

// 这样返回到旧版本,但修改后再提交会失败
vim test.cpp // 做修改
svn ci test.cpp -m "4" //再提交
# ---------结果会报错---------
# 正在发送 test.cpp
# 传输文件数据 .svn: E155011: 提交失败(细节如下):
# svn: E155011: 文件“/home/user/work/workspace/src/test.cpp” 已经过时
# svn: E170004: File '/workspace/src/test.cpp' is out of date

正确做法

  1. 先更新到最新版本, 版本966
  2. 反向合并到旧版本

    现在内容是950,但版本是966

  3. 现在修改文件再提交,版本就是967了

同样地,反向合并目录下所有改动过的文件: svn merge -r 966:950 .

查看文件和目录的状态

svn status path? 不在svn的控制中;M 内容被修改;C 发生冲突;A 预定加入到版本库;K 被锁定

最好除了?外不要有其他标志

删除分支

右击已经存在的SVN项目->repo browser,弹出框的左边选择需要删除的分支右击->delete

锁定服务器文件

有的特殊文件不适合修改,所以锁定,防止各个客户端的提交修改,使用命令svn lock file

命令行实现relocate

命令:

1
svn sw --relocate Old_Path New_Path

会要求输入用户名和密码

参考:命令行重定位SVN地址

查看目录下所有状态异常的文件

在目录下使用svn status查看即可,如果只看冲突标志,使用 svn status | grep -P '^(?=.{0,6}C)'


使用chrono进行高精度计时

time_t在Linux中的类型其实是long int

1
2
3
struct timeval tv;
gettimeofday(&tv,nullptr);
time_t curTime = tv.tv_sec; // 获取当前UTC时间戳,单位为秒

1
2
3
4
5
6
7
8
9
10
#include <chrono>
using namespace chrono;

auto start = system_clock::now();
/* do something */
auto end = system_clock::now();
auto duration = duration_cast<microseconds>(end - start);
cout << "花费了"
<< double(duration.count()) * microseconds::period::num / microseconds::period::den
<< "秒" << endl;

有时候可能报错:error: ‘chrono’ in namespace ‘std’ does not name a type

解决方法: 改为using namespace std::chrono; in addition to #include <chrono>

或者用steady_clock

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
#include <chrono>

using std::chrono::steady_clock;
// 这个类型是 steady_clock::time_point
auto t1 = std::chrono::steady_clock::now();

// your code

auto t2 = std::chrono::steady_clock::now();

//秒
double dr_s=std::chrono::duration<double>(t2-t1).count();
//毫秒
double dr_ms=std::chrono::duration<double,std::milli>(t2-t1).count();
//微秒
double dr_us=std::chrono::duration<double,std::micro>(t2-t1).count();
//纳秒
double dr_us=std::chrono::duration<double,std::nano>(t2-t1).count();

ALOAM中的TicToc类其实就是std::chrono::steady_clock::now()

拥有从毫秒到小时的计时

1
2
3
4
std::chrono::milliseconds 
std::chrono::seconds
std::chrono::minutes
std::chrono::hours

可以用于休眠函数

1
2
std::this_thread::sleep_for(std::chrono::milliseconds(100));
std::this_thread::sleep_for(std::chrono::seconds(1));


完全退出ROS的方法

执行rosnode kill -a,这会杀死rosout之外的所有节点,但是这样还差很远。如果有的节点是以python文件启动,它对于的进程并没有杀死。另外rosout是可再生的,必须连roscore也杀掉.

现在执行ps aux | grep ros,会看到剩余的4个进程:

按照它们的pid, 使用kill pid的方式将它们杀死,这样才彻底退出所有ROS进程

但是注意如果是在远程机启动roscore,在本地机是不能终止它的。


OccupancyGrid地图详解

栅格地图是将环境栅格化, 然后根据测量值对每个栅格的占有率进行计算, 通过概率计算判断给各栅格可能被障碍物占的几率, 并赋值一个0到1之间的值。通过激光雷达获取的大量数据帮助对每个栅格点进行准确判断。栅格地图中栅格大小决定了地图的精度, 也将影响机器人的定位精度,因此追求更小更多的栅格,但是栅格变小后,其数据量将会显著增加, 因此栅格地图不适合用于大型地图的构建, 适用于室内定位这种小型环境地图构建

地图在ROS中是以Topic的形式维护和呈现的,这个Topic名称就叫做/map,它的消息类型是nav_msgs/OccupancyGrid。由于/map中实际上存储的是一张图片,为了减少不必要的开销,这个Topic往往采用锁存(latched)的方式来发布。

锁存器的作用就是,将发布者最后一次发布的消息保存下来,然后把它自动发送给后来的订阅者。这种方式非常适合变动较慢、相对固定的数据(例如地图),然后只发布一次,相比于同样的消息不定的发布,锁存的方式既可以减少通信中对带宽的占用,也可以减少消息资源维护的开销。

map话题的消息类型是OccupancyGrid,通过rosmsg show nav_msgs/OccupancyGrid来查看

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
std_msgs/Header header       #消息的报头
uint32 seq
time stamp
string frame_id #地图消息绑定在TF的哪个frame上,一般为map

nav_msgs/MapMetaData info #地图相关信息
time map_load_time #加载时间
float32 resolution #分辨率 单位:m/pixel
uint32 width #宽 单位:pixel
uint32 height #高 单位:pixel
geometry_msgs/Pose origin #原点
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
int8[] data #地图具体信息

info是地图的配置信息,它反映了地图的属性。设置地图原点
1
2
3
// 将栅格地图原点设置在整张图的(-20, -20)位置
grid_map.info.origin.x = -20;
grid_map.info.origin.y = -20;

整张地图的障碍物信息存放在data数据成员中,data是一个int8类型的vector,它存储的内容有width * height个int8型的数据,也就是这张地图上每个像素。map中data存储的格式如下

1
2
3
4
0      空白区域
100 障碍物
-1 未知
1-99 根据像素的灰度值转换后的值,代表有障碍物的概率、

左下角像素点在世界坐标系下的位置为(origin_x,origin_y), 单位米。那么世界坐标系下的点(x,y),转换到地图坐标系,那么该点对应的data中的索引index为:
1
index = (int)( (x - origin_x) / resolution) + ((int)((y - origin_y) / resolution)) * width;

该点在地图中的信息即为data[index]

一个宽和高的栅格地图,索引保存方式是这样的:

1
2
3
4
5
6
7
4        8        12     16

3 7 11 15

2 6 10 14

1 5 9 13

1相当于地图的原点,6就对应坐标(1, 1)。那么点坐标(2, 3),对应的栅格索引是x * map.info.width + y = 12

pgm和yaml

运行rosrun map_server map_saver -f fileName可以保存地图,保存成功后,会有文件名对应的pgm和yaml文件。pgm文件就是地图的文件,标准的地图是用inkscape打开时看到的模样。

地图就是一张普通的灰度图像,通常为pgm格式。在地图中,黑色的网格是障碍区域,白色的网格是无障碍区域,灰色的网格是未知区域。map_server从图片中读取信息然后将每个网格都赋值[-1, 100],灰色是-1,代表未知区域;白色是0,空闲区域;黑色是100,障碍区域。

yaml描述地图元数据,内容如下:

1
2
3
4
5
6
7
8
9
image: Software_Museum.pgm      #指定地图文件,可以是相对路径
resolution: 0.020000 #地图的分辨率 单位为 m/pixel
# 地图的原点,即地图左下像素的2D姿态(x,y,yaw),偏航为逆时针旋转,忽略偏航时为0
# 这个坐标体系用于让ros在图片里找到原点而已
origin: [-15.720000, -12.520000, 0.000000]
negate: 0 # 0代表白色空闲,黑色占据。一般不用修改
occupied_thresh: 0.65 # 当占据的概率大于0.65认为被占据
free_thresh: 0.196 # 当占据的概率小于0.196认为无障碍
# 介于 occupied_thresh 和 free_thresh 的栅格unknown

其中占据的概率occ = (255-color_avg) / 255.0, color_avg为RGB三个通道的平均值。

  • occupied_thresh:大于这个阀值的占用率的像素被认为occupied
  • free_thresh:小于这个阀值的占用率的像素被认为是free

如果occupied_thresh设置的比较大,有些障碍物会没有障碍层和膨胀层。
occupied_thresh=0.85.png
如果建图时,障碍的边缘不够明显,也可能出现此问题,此时可以降低occupied_thresh。 其实还可以降低参数cost_scaling_factor,不过对膨胀层的影响较大。

  • 栅格地图的分辨率不必太好,常用0.05
栅格地图的坐标系原点,是在栅格地图左下角,这里的origin是左下角在map坐标系的坐标,不是(0,0,0)

实际举例,先获得机器人的当前姿态:

然后在rviz上测量出机器人与左下角两个方向的距离

用机器人的坐标减去origin的坐标:

  • x = -2.394 - (-15.72) = 13.326
  • y = 2.37 - (-12.52) = 14.89

显然和测量结果大致相同,计算都是在map坐标系下。

map_server和map_saver

map_server是一个和地图相关的功能包,它可以将已知地图发布出来,供导航和其他功能使用,也可以保存SLAM建立的地图。它还提供了map_saver命令行实用程序,它允许动态生成的映射保存到文件。

要让map_server发布/map,需要yaml和pgm两个文件,然后可以通过指令来加载这张地图,map_server相关命令如下:

1
2
rosrun map_server map_server    test.yaml 	加载自定义的地图,通常使用roslaunch
rosrun map_server map_saver -f mymap 保存当前地图为mymap.pgm和mymap.yaml

或者使用roslaunch加载地图:
1
2
<arg name="map_file" default="/home/hlhp/robot_ws/workspace/maps/test.yaml"/>
<node name="map_server" type="map_server" args="$(arg map_file)" pkg="map_server"/>

map_server节点提供的服务:

  • static_map (nav_msgs/GetMap):提供检索地图服务

map_server节点读取pgm文件,通过服务static_map将地图数据提供给其他节点。比如move_base节点从中获取地图数据,用来进行路径规划;amcl节点用地图来进行定位,代码在AmclNode::requestMap()

map_server发布的话题包括:

1
2
/map_metadata    (nav_msgs/MapMetaData): 发布地图的描述信息,通过此锁存话题接收地图元数据。
/map (nav_msgs/OccupancyGrid): 通过此锁存话题接收地图

加载yaml文件后,出现下面的结果:

运行rostopic echo map_metadata的结果:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
map_load_time: 
secs: 1569549222
nsecs: 440982758
resolution: 0.019999999553
width: 1504
height: 2112
origin:
position:
x: -15.72
y: -12.52
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0

echo一下map话题,结果是一大堆-1和0,这就是上面所说的occupancy grid,-1代表未知,0代表无障碍,100代表障碍。

参数:

  • ~frame_id (string, default: “map”):地图坐标系的名称,要在已发布地图的标题中设置的框架, 绑定发布的地图与tf中的哪个frame,通常就是map

问题 1

有时运行rosrun map_server map_saver,结果显示 Wating for the map, 无法保存成功,进程阻塞了

问题 2

有几次,map_server打开pgm时一直阻塞,只能重启ROS

1
2
3
4
5
map_server::loadMapFromFile(&map_resp_,mapfname.c_str(),res,negate,occ_th,free_th, origin, mode);
ROS_INFO("map_server after loadMapFromFile");

// To make sure get a consistent time in simulation
ros::Time::waitForValid();

经过加日志,发现是 ros::Time::waitForValid();没有执行完,问题出在这里。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
bool Time::waitForValid(const ros::WallDuration& timeout)
{
ros::WallTime start = ros::WallTime::now();
while (!isValid() && !g_stopped)
{
ros::WallDuration(0.01).sleep();

if (timeout > ros::WallDuration(0, 0) && (ros::WallTime::now() - start > timeout))
{
return false;
}
}
if (g_stopped)
{
return false;
}
return true;
}


使launch文件开机自启动

robot_upstart

机器人开发中,一般都要求工控机开机后要启动某些节点或launch,ROS已经考虑到了这个需求。专门开发了一个package叫做robot_upstart,它提供脚本来安装和卸载随机启动的节点,但是经过很长时间的研究,发现这个包并不好用,需要额外很多设置。

安装: sudo apt-get install ros-kinetic-robot-upstart,现在有一个package,名称是nav,在它的launch文件夹里有个launch文件叫做all.launch,下面是第一个命令:

1
rosrun robot_upstart install nav/launch/all.launch

这实际是把nav做成了一个service,就跟MySQL服务器一样,然后按照系统自动提示执行:

1
sudo systemctl daemon-reload && sudo systemctl start foo

如果是普通节点,重新开机就能启动all.launch所有节点了,但是我的程序用到了很多自定义的环境变量,结果自启动总是失败。

生成的文件

按上面官方流程执行产生了几个文件,到/etc/ros/kinetic查看是否自启动的包有对应文件夹,里面是设定的launch文件,文件名就是启动名称

跟自启动服务相关的脚本是/usr/sbin/nav-start,停止脚本是同目录的nav-stop,内容比较复杂,也没有分析的必要了。

官方说自启动的日志文件在/var/log/upstart,但是我试了试发现从来都没有,后来看了看nav-start脚本,虽然没有完全看懂,但是在一定条件下会在/tmp下创建日志.不过我看了看日志,在自启动节点失败时,也没有什么有用的信息.

两个service文件其实完全一样,而且只修改一个就可以,/lib/systemd/system/nav.service的内容:

1
2
3
4
5
6
7
8
9
10
11
[Unit]
Description="bringup nav"
After=network.target

[Service]
Type=simple
EnvironmentFile=/etc/robotenv
ExecStart=/usr/sbin/nav-start

[Install]
WantedBy=multi-user.target

这里就是复杂的地方了,如果有自定义的环境变量,需要在执行rosrun robot_upstart install后加到这里,再执行后面的启动服务命令。

我新建了一个文件/etc/robotenv,里面定义环境变量:

1
2
MAP_FILE=/home/user/maps
BAG_FILE=/home/user/bags

不要加export,否则无法生效。然后用EnvironmentFile=/etc/robotenv调用,而且这句应当在ExecStart之前

但是在启动跟串口有关的节点时发现失败了,虽然我之前已经对串口进行了永久使能,但在开机过程中可能需要重新使能,问题在于串口使能前先启动了节点,结果失败。所以在rc.local中添加:

1
2
chmod 777 /dev/ttyS0
chmod 777 /dev/ttyS1

停止开机启动:rosrun robot_upstart uninstall nav,会删除除了/tmp里几个文件的其他所有文件
robot_upstart uninstall的注意使用包名

如果之前设置开机启动失败了,必须先用 rosrun robot_upstart uninstall 解除之前的设置,然后重新设置 ,如果是别人设置的开机启动,这个名称nav可以去到`/etc/ros/kinetic`里寻找 设置成功后,修改launch文件或更新节点文件不影响开机启动的设置

问题

在小强机器人上对startup.launch开机启动,在另一台电脑上执行rosnode listrostopic list都是正常的,但rostopic echo却没有结果,rosnode ping也不行。把所有ROS关闭,重启startup.launch,又正常了。

rc.local,只用于无界面节点

如果启动的节点比较简单,可以用rc.local的方式,在文件里添加roslaunch命令和环境变量即可,需要注意的是下面的失败的情况

环境变量

第一次开机启动后,节点都启动了,但发现一些节点不能正常运行,后来发现是一些节点中还使用了自定义的环境变量,比如某些目录的地址,所以把这些也要加上:

1
2
3
export CONFIG_PATH=/home/`hostname`/workspace/config
export MAP_FILES_PATH=/home/`hostname`/data
export LAUNCH_PATH=/home/`hostname`/workspace/src

mini-httpd用到的所有环境变量也要加入到rc.local当中,因为开机时加载的环境变量不是bashrc的,而是rc.local

脚本出错

编辑/etc/rc.local进行程序自启动时没有生效。可能是脚本出错,脚本第一行为#!/bin/sh -e,-e标志脚本如果有错误,就不再向下执行。加入下面内容,可以检查错误的原因:

1
2
3
exec 2> /tmp/rc.local.log    # send stderr from rc.local to a log file
exec 1>&2 # send stdout to the same log file
set -x # tell sh to display commands before execution

错误日志都在log文件里,第二行是把一般输出也放了进去,有时会很多,这行可以不加。
打开错误日志发现source not found,不识别source,这是shell的原因。把第一行改为#!/bin/bash,也就是换成bash,最终生效了。

权限和服务

有时按上面步骤还是失败,那么进行下面操作:

1
2
3
4
sudo chmod +x /etc/rc.local
# rc.local被认为是服务,默认是关闭的,需要打开
sudo systemctl enable rc-local.service
重启

完整脚本如下:

1
2
3
4
5
6
7
8
9
10
11
exec 2> /tmp/rc.local.log  # send stderr from rc.local to a log file
#exec 1>&2 # send stdout to the same log file
set -x

source /opt/ros/kinetic/setup.bash
source /home/user/name/workspace/devel/setup.bash
export ROS_IP=`hostname -I`
export ROS_HOSTNAME=`hostname -I`
export ROS_MASTER_URI=http://192.168.73.14:11311
roslaunch /home/user/name/workspace/src/package/launch/start.launch & mini_httpd -C /etc/mini_httpd.conf # 同一行
exit 0

只启动了一个launch文件

rc.local设置开机启动,最好只有一个launch,否则常常只启动一个,我们可以把所有launch放到一个里面.

启动roslaunch失败

rc.local中启动launch文件用的是roslaunch ~/Robot/workspace/package/bringup.launch,结果失败,查看日志发现实际路径是/root/Robot/workspace/package/bring.launch,可见开机时Linux不能把~识别为/home/user

gnome-session-properties,只用于有界面(带显示器)节点

以上方法用于普通ROS程序没问题,但带界面的程序就失灵了。这种情况下先新建一个脚本,比如test.sh:

1
2
3
4
5
6
7
8
9
10
#!/bin/bash
echo -----------准备开机自启动程序 robot-----------
sleep 10
source /opt/ros/kinetic/setup.sh
source /home/user/work/workspace/devel/setup.bash
export ROS_IP=`hostname -I`
export ROS_HOSTNAME=`hostname -I`
export ROS_MASTER_URI=http://192.168.31.14:11311

rosrun robot robot

注意文件要给权限 sleep 10是让脚本阻塞10秒钟,因为程序需要等待与roscore的通信,如果不阻塞,可能会先于roscore启动节点,导致失败而退出,其他就是加载需要的环境和执行程序。

然后在终端输入gnome-session-properties,打开应用程序首选项设置开机启动程序。

命令中填写gnome-terminal -x /path/test.sh。保存后重启,会出现一个终端执行脚本中的命令

这种方法只适用于有界面的情况,假如机器不带显示器,就无法启动成功。 最好每个节点就要对应一个sh文件,逐个添加。因为都放在一起会阻塞在第一个节点,退出以后会接着执行下一个,不能同时执行

参考:
ROS程序开机自启动的四种方法


Action通信机制(一) 概念和原理

尽管在ROS中已经提供了service机制来满足请求—响应式的使用场景, 但是假如某个请求执行时间很长,在此期间用户想查看执行的进度或者取消这个请求的话,service机制就不能满足了,但是action通信可满足用户这种需求。例如,控制机器人运动到地图中某一目标位置,这个过程可能复杂而漫长,执行过程中还可能强制中断或反馈信息,这时需要的就是 actionlib 机制

service是阻塞的,action是非阻塞的

通信机制

ROS的Action机制本质上仍然是一种Topic机制,只是通过actionlib进行了封装。ActionClientActionServer 通过ROS Action Protocol进行通信,本质上还是消息的方式,action protocol就是预定义的一组 ROS message。

在服务器和客户端之间通过goal, cancel, status, feedback, result五个主题实现Action机制。

  • goal: 代表一个任务,可以被ActionClient发送到ActionServer。比如在MoveBase中,它的类型是PoseStamped,包含了机器人运动目标位置的信息。

  • cancel: client发送取消请求到server

  • status: server通知client每个goal的当前状态

  • feedback: 向client发送周期性的goall执行过程中的辅助信息。在Move Base中,它表示机器人当前姿态。

  • result: 等goal执行结束,才向client发送一次性辅助信息

类似msg文件,在程序的action文件(文件名后缀为.action)中定义上述goal、result、feedback等消息,依次用---分隔

action通信时,如果双方约定的action名称为do_dishes,那么客户端会发布话题 /do_dishes/goal/do_dishes/cancel;服务端发布话题/do_dishes/feedback, /do_dishes/result, /do_dishes/status

client要么发送goal,要么cancel; result和feedback的取值由用户决定,status的取值是固定的:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
uint8 PENDING   = 0   # The goal has yet to be processed by the action server
uint8 ACTIVE = 1 # The goal is currently being processed by the action server

uint8 PREEMPTED = 2 # The goal received a cancel request after it started executing
# and has since completed its execution (Terminal State)
uint8 SUCCEEDED = 3 # The goal was achieved successfully by the action server (Terminal State)
uint8 ABORTED = 4 # The goal was aborted during execution by the action server due
# to some failure (Terminal State)
uint8 REJECTED = 5 # The goal was rejected by the action server without being processed,
# because the goal was unattainable or invalid (Terminal State)

uint8 PREEMPTING = 6 # The goal received a cancel request after it started executing
# and has not yet completed execution
uint8 RECALLING = 7 # The goal received a cancel request before it started executing,
# but the action server has not yet confirmed that the goal is canceled
uint8 RECALLED = 8 # The goal received a cancel request before it started executing
# and was successfully cancelled (Terminal State)
uint8 LOST = 9 # An action client can determine that a goal is LOST. This should not be
# sent over the wire by an action server

server状态机

Goals请求由ActionClient发出,ActionServer接收后会创建一个有限状态机来跟踪goal的状态,而不是server的状态,每个goal都有一个状态机。

server状态机的命令(actionlib::SimpleActionServer的函数)

  • setAccepted: 收到goal后,准备处理
  • setRejected: 收到goal后,拒绝处理
  • setSucceeded: 标志goal已经成功处理
  • setAborted: 处理goal的过程中发现错误,终止处理
  • setCanceled: server取消对goal的处理,与setRejected不同之处在于发出取消请求的是client

从客户端也可以异步地触发状态变换,比如cancelGoal函数

client状态机

server状态机用于追踪goal的状态,client状态机作为补充,追踪server的状态。一般根据goal的情况分成 PENDING,ACTIVE,DONE三部分。

由于可以有多个client连接到一个Action server,有可能第二个client会取消第一个client发的goal

Goal的状态转变

两张图片表示得太复杂了。一般这样记忆:对于MoveBase的client,最开始的状态是LOST。 当前sendGoal后,状态为ACTIVE. 调用cancelGoal后,变为 PREEMPTED,而不是Aborted。客户端实际的操作goal的函数只有sendcancel

车成功到达目标后,状态为SUCCEEDED。彻底失败,为Aborted

对于多个goal的处理策略

高层的client和server只考虑goal的处理情况,所以叫SimpleActionServerSimpleActionClient

Simple Action Client一次只追踪一个goal,它发出goal之后,会disables之前的goal的所有回调函数,也不再追踪之前goal的状态,但是并不cancel之前的 goal

SimpleActionServer在ActionServer类上实现了单一目标策略,就是在某一时刻只能有一个goal是处于active状态,并且新的goal可以抢占先前的goal:

  • 每次只能有一个goal处于active状态
  • 新的goal抢占以前的goal是根据GoalID中的时间戳
  • 新的goal抢占成功后,旧的goal状态会改变


如图,goal A正在被处理,后面等待的是B,此时client又发了C,那么SimpleActionServer会把C放到队列中,B进入RECALLING状态,然后server又调用setCanceled使B进入RECALLED状态,C成了PENDING

actionlib 为每个 goal 设置了一个状态跟踪器,每个目标有一个独有的id,这是状态发布与取消goal的基础。

actionlib 是通过多个话题的形式实现控制,由于它支持多个客户端同时对服务器发指令,内部必须维护一个list来控制多个目标点的实施。

工具

actionlib包自带了一个客户端的工具,可以用来测试与server端的通信:

1
rosrun actionlib axclient.py /do_dishes    // do_dishes是action名称

参考:

actionlibDetailedDescription
actionlib库的介绍
ROS actionlib学习(三)


move_base分析(二)导航过程
  1. 在我的程序里,从Service的客户端(比如网页端)选择一个目标点后,服务端获得目标的位姿,传递给需要的线程.
  1. 初始化一个Action的客户端SimpleActionClient的指针g_ac_ptr:
1
2
3
4
actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>* g_ac_ptr=NULL;
// 初始化move_base ActionClient
actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> ac("move_base",true); // action名称为move_base
g_ac_ptr = &ac;
  1. 发送目标:
1
2
3
4
5
6
7
move_base_msgs::MoveBaseGoal goal;

goal.target_pose.header.frame_id = "map";
goal.target_pose.header.stamp = ros::Time::now();
goal.target_pose.pose = g_request.pose; // 服务端获得目标的位姿

g_ac_ptr->sendGoal(goal);

sendGoal会将目标点发送到move_base节点进行处理,进而会发布话题move_base/goal,消息类型为move_base_msgs/MoveBaseActionGoal,用于存储导航的目标位置数据

我没有用到/move_base_simple/goal话题,其实平时应该用这个,比如:

1
2
rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped \
'{ header: { frame_id: "base_link" }, pose: { position: { x: 1.0, y: 0, z: 0 }, orientation: { x: 0, y: 0, z: 0, w: 1 } } }'

  1. Action的服务端在MoveBase的构造函数:
1
2
3
4
5
// 回调函数是 MoveBase::executeCb
as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);
......
//we're all set up now so we can start the action server
as_->start();

MoveBase::executeCb函数在move_base693行,过程太复杂,步骤大致如下:

  • 验证目标位姿的欧拉角是否合理
  • 目标位姿转换为全局frame中的位姿
1
2
3
4
5
6
//we have a goal so start the planner
boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
planner_goal_ = goal;
runPlanner_ = true;
planner_cond_.notify_one();
lock.unlock();
  • 发布话题move_base/current_goal,与/move_base/goal的target_pose部分只有seq的不同

调用executeCycle
进行local plan,发布cmd_vel的topic,根据小车处于的状态,进行相应的实现(会进行plan,control,clear obstacle)

1
2
3
4
5
6
7
8
9
10
11
12
enum MoveBaseState {
PLANNING,
CONTROLLING,
CLEARING
};

enum RecoveryTrigger
{
PLANNING_R,
CONTROLLING_R,
OSCILLATION_R
};

DWAPlannerROS和TrajectoryPlannerROS都继承了 nav_core::BaseLocalPlanner

1
2
3
4
5
6
DWAPlannerROS::computeVelocityCommands
dwaComputeVelocityCommands(current_pose_, cmd_vel);

DWAPlannerROS::publishGlobalPlan

base_local_planner::publishPlan(path, g_plan_pub_);

MoveBase::executeCycle调用了DWAPlannerROS::computeVelocityCommands


代价地图(二) 话题服务和部分源码

平常所说的costmap(代价地图)有下面三层,这三层组合成了master map(最终的costmap),供给路线规划模块使用。:

  • 静态层: 为了做全局规划,机器人需要一个超越其传感器的地图,以了解墙壁和其他静态障碍物的位置。 静态地图可以用SLAM算法生成,它表示了costmap中不会变化的大部分

  • 障碍层: 该层从诸如激光和RGB-D相机的高精度传感器收集数据,并将其放置在二维网格中,追踪障碍物。 ObstacleCostmapPlugin在二维层面标记和射线追踪障碍物,theVoxelCostmapPlugin是从三维追踪。

  • 膨胀层: 在每个致命障碍周围插入缓冲区,增加到costmap的值取决于离最近的障碍的距离(膨胀半径)。机器人肯定会发生碰撞的地点有致命的代价,而且周围的区域有一些小的非致命的成本。这些值确保机器人不会撞上致命的障碍物,也不愿太靠近

文件夹costmap_2d\plugins提供了这三层和voxel层的源文件,costmap_2d包中的costmap_plugins.xml定义了这几层和VoxelLayer

我自己定义的障碍物也可以是一层,假如我不想让机器人通过一个freespace就可以自己插入个障碍物,主要的接口是costmap_2d::Costmap2DROS,在每一层中使用pluginlib实例化Costmap2DROS并将每一层添加到LayeredCostmap,各个层可以被独立的编译。写法模仿obstacle_layer.cpp,在CMakeLists里添加后,一起编译成liblayers.so

rotate_recovery, move_base, move_slow_and_clear, global_planner, clear_costmap_recovery(这个库又在move_base中调用), dwa_local_planner, base_local_planner都使用了libcostmap_2d.so这个库

src文件夹中的costmap_2d_markers.cpp, costmap_2d_cloud.cpp, costmap_2d_node.cpp各自编译成一个可执行文件,其余文件联合编译成库文件libcostmap_2d.so

costmap相关的话题和服务

省略每一层和两个代价地图的parameter_descriptionsparameter_updates。以parameter_descriptionsparameter_updates结尾的话题用于dynamic_reconfigure机制,不必关注。echo话题会只获得一次消息,几乎不占用带宽,用rostopic bw没有结果。

1
2
3
4
5
6
7
8
/move_base/global_costmap/costmap
/move_base/global_costmap/costmap_updates
/move_base/global_costmap/footprint

# 局部代价地图的区别仅仅是没有静态层
/move_base/local_costmap/costmap
/move_base/local_costmap/costmap_updates
/move_base/local_costmap/footprint

/move_base/global_costmap/costmap 的类型是 nav_msgs/OccupancyGrid,所以不能直接订阅,需要转为代价地图.

参数~<name>/always_send_full_costmap,   (bool, default: false). 如果为true,那么每次更新,完整的costmap都会发布到话题~<name>/costmap. 如果为false, 只有部分 costmap 更新时,会发布到话题 ~<name>/costmap_updates。 设置always_send_full_costmap为false后,带宽大幅减少

话题~<name>/costmap_updates, 类型map_msgs/OccupancyGridUpdate,the value of the updated area of the costmap

除了两个代价地图和每一层的set_parameters服务之外,服务就只有/move_base/clear_costmaps

服务set_parameters是由 Dynamic Parameters server创建,我在导航框架的源码里没找到这个关键词,怀疑它的发布在ROS源码里。

部分源码

move_base里初始化代价地图是使用的Costmap2DROS, move_base里没有直接使用过Costmap2D类的对象

1
2
3
4
5
6
7
8
9
10
planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
// 只有两行,类成员变量赋值: stop_updates_ = true; initialized_ = false;
planner_costmap_ros_->pause();

controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
controller_costmap_ros_->pause();
// 省略... 初始化global_planner local_planner
// 很简单的代码,对各层地图插件调用activate函数,激活各层地图
planner_costmap_ros_->start();
controller_costmap_ros_->start();


代价地图(一) 基础配置

地图

代价地图的参数定义在三个文件里,分别用于全局代价地图,局部代价地图,通用代价地图(用于两个代价地图的参数)

movebase建立了两个costmap,其中`planner_costmap_ros是用于全局导航的地图,controllercostmap_ros`是用于局部导航用的地图。

costmap原点在左下角,OpenCV图片的原点在左上

通用代价地图

参数文件common_costmap_parameters.yaml,这些参数会影响两个代价地图:

1
2
3
4
5
6
7
8
9
10
11
12
13
footprint: [[-0.24, -0.22], [-0.24, 0.22], [0.24, 0.22], [0.24, -0.22]]
inflation_radius: 0.05 #before: 1.8 0.3 1.2
obstacle_range: 2.5
raytrace_range: 3.0
cost_scaling_factor: 0.18
observation_sources: scan
scan:
data_type: LaserScan
topic: scan
marking: true
clearing: true

map_type: costmap

  • footprint: 机器人底盘的轮廓,由一系列二维数据表示,单位为米,每个表示机器人边界的点,逆时针或顺时针方向都可以,轮廓一般是个矩形,机器人中心相当于[0,0]。本参数其实用于表示内切圆和外接圆,它们以一定方式膨胀障碍物以适应机器人。一般为安全起见,本参数比真实轮廓稍微大一些。

  • robot_radius: 与footprint并列,用于圆形底盘,这个设置就简单多了,就是个半径值,单位米

  • min_obstacle_height: 传感器读数的最小高度(以米为单位)视为有效。通常设置为地面高度。

  • obstacle_range: 设置机器人检测障碍物的最大范围,意思是说超过该范围的障碍物,并不进行检测,只有靠近到该范围内才把该障碍物当作影响路径规划和移动的障碍物

  • raytrace_range: 在机器人移动过程中,实时清除代价地图上的障碍物的最大范围,更新可自由移动的空间数据。假如设置该值为3米,那么就意味着在3米内的障碍物,本来开始时是有的,但是本次检测却没有了,那么就需要在代价地图上来更新,将旧障碍物的空间标记为可以自由移动的空间

全局代价地图

全局地图是从静态地图创建的,使用了后者的数据,只是全局地图将静态地图中的障碍物膨胀化, 依据机器人的足迹半径.

千万注意 pgm 地图文件,边界必须是黑边,而且只给一圈黑色像素不够黑,至少4个,否则不能产生代价地图。

pgm文件.png
生成代价地图.png

相应的参数文件global_costmap_params.yaml

1
2
3
4
5
6
7
8
9
10
11
12
13
global_costmap:
global_frame: map
robot_base_frame: base_link # 或者 base_footprint
update_frequency: 2.0 #before: 5.0
publish_frequency: 2.0 #before 0.5
static_map: true
rolling_window: false # 与static_map相反
transform_tolerance: 0.5 # tf变换允许的最大时间

plugins:
- {name: static, type: "costmap_2d::StaticLayer"}
- {name: obstacles, type: "costmap_2d::VoxelLayer"}
- {name: inflation, type: "costmap_2d::InflationLayer"}

plugins表示你要在costmap加载哪几层,static,obstacles,inflation是不可缺少的。 这些层在common_costmap_parameters.yaml中定义,然后添加到全局和局部代价地图的yaml文件里。如果没有定义,就不能添加到后两者。

全局代价地图对应的话题是/move_base/global_costmap/costmap,注意类型是nav_msgs/OccupancyGrid
全局代价地图

加载代价地图后,可以在Rviz里看到基本参数信息

记录一个bug

全局代价地图如果正常生成,理应和地图的大小一样,是2000 x 2000

Costmap2D::worldToMap在刚启动时的打印结果
刚启动move_base

发导航命令后,代价地图的大小很奇怪地变为200, 原点变为(0,0)

1
2
3
4
5
6
7
8
9
10
11
Costmap2DR0S transform timeout. Current time: 1687663324.0445, global_pose stamp 321.6492, tolerance: 0.3000
[1687663324.0756730651]: Could not get robot pose, cancelling reconfiguration
[1687663324.6495552631]: MoveBase received move_base goal (48.000000, 58.000000), type: 3
[1687663324]: wx: 46.032079, wy: 46.095481, origin.x: 0.000000, origin_y_: 0.000000

[1687663324]: mx: 920, my: 921 , size_x_: 200, size_y_: 200, resolution.: 0.050000

[1687663324]: wx: 46.032079, wy: 46.095481, start_x_i: 920, start_y_i: 921
[1687663324]: The robot's start position is off the global costmap. Planning will always fail, you sure the robot has been properly localized ?
[1687663324.]: move_base failed to find a plan to point (48.00, 58.00) !
[1687663324.]: wx: 46.032090, wy: 46.095525, start_x_i: 920, start_y_i: 921

局部代价地图

局部规划器用局部代价地图计算出局部路径,局部代价地图是依据机器人的传感器数据创建的,与静态地图无关,它由用户给出宽度和高度。不管机器人怎么运动,它总在局部代价地图的中心,不再这个动态窗口内的障碍信息会drop

相应的参数文件local_costmap_params.yaml

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
local_costmap:
global_frame: odom # 局部代价地图运行的坐标系,也可以是map
robot_base_frame: base_link # 也可以是map
update_frequency: 5.0 # 局部代价地图的更新频率,太大会影响消耗CPU
publish_frequency: 5.0 # 局部代价地图更新的发布频率,如果机器人运动很快,需要增大
static_map: false # 只能是false
rolling_window: true # 只能是true,代价地图在一个rolling window内更新
width: 4 # rolling window的宽度,在rviz显示时会四舍五入
height: 4 # rolling window的高度,在rviz显示时会四舍五入
resolution: 0.02 # rolling window的分辨率
transform_tolerance: 0.5 # tf变换的最大延迟

plugins:
- {name: obstacles_layer, type: "costmap_2d::ObstacleLayer"}
- {name: obstacles, type: "costmap_2d::ObstacleLayer"}
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}

加载局部代价地图成功后

局部代价地图的origin_x_origin_y_(在costmap_2d.cpp中的成员变量)如图解释,显然随着机器人的运动会变化。

局部代价地图有一个更新的过程:

  1. 订阅传感器相关主题,根据接受的传感器数据更新代价地图
  2. 执行标记和清除动作,前者是向代价地图insert障碍信息,后者是从代价地图remove障碍信息。两种动作在障碍层中定义
  3. 计算每个网格的代价值
  4. 对每个包含障碍的cell进行障碍膨胀,从每个占据的cell按一定的膨胀半径向外扩张代价值

两个代价地图的设置中,膨胀层要放到所有层的最后,否则全局路径不是贴着膨胀层,而是障碍层。

Rviz的map包含3个Color Scheme,costmap, map, raw。若要增加主题,只能写一个Rviz插件或者修改Rviz的源码

参考:代价地图全面分析总结


cmake教程(四)ROS中的catkin_make

使用catkin_make编译时,有时编译内容太长,而错误在终端的上面,使用滑条仍有可能看不到。这时可以使用catkin_make > path/file.log将编译内容输出到某个文件,这样就容易查看了。 catkin本身不会存储编译过程的日志

catkin_package()

这个函数十分重要,观察CMakeLists就可以发现,它是唯一的非cmake函数,显然ROS的catkin_make就是靠它了,它的作用是指定功能包构建信息导出,在使用add_library()或者add_executable() 之前,必须调用此函数。

该函数有5个可选参数:

1
2
3
4
5
6
7
8
9
INCLUDE_DIRS  导出包含功能包的路径(即cflags)

LIBRARIES 项目中导出的库

CATKIN_DEPENDS 该项目所依赖的其他catkin项目

DEPENDS 此项目依赖的非catkin CMake项目

CFG_EXTRAS 其他配置选项

比如:
1
2
3
4
5
catkin_package(
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME}
CATKIN_DEPENDS roscpp nodelet
DEPENDS eigen opencv)

这表示功能包文件夹中的“include”文件夹是导出头文件所在的位置。roscpp+nodelet是构建/运行此功能包需要存在的功能包,eigen+opencv是构建/运行此功能包时需要存在的系统依赖性。

catkin_package在/opt/ros/kinetic/share/catkin/cmake/catkin_package.cmake文件中定义。develbuild文件夹内的内容基本都是由其生成的。

这个函数解析package.xml文件,提取出里面的参数,执行catkin_make时,会在workspace/devel/share里生成pkgConfig.cmake文件的,在cmake文件中find_package就会用到这些文件,这样其他的对这个软件包有依赖的程序就可以方便的使用了。

比如现在有sub这个包,编译之后会在devel/share/sub/cmake生成两个文件:pubConfig.cmakepubConfig-version.cmake,然后到另一个包pub的CMakeLists里调用sub:find_package(sub),这样编译pub就通过了.假如把那两个文件删掉,然后使用catkin_make --pkg pub编译,就会报错:

对于build文件夹,生成是一些编译中的中间文件,比如用来存储一些环境变量之类的文件。这个文件夹意义不大。

在工程中显示其他源文件

新建CMake工程后,只有main.cpp,向目录添加文件foo.h和foo.cpp,但是它们不会在工程目录里显示,编辑CMakeLists如下

1
2
3
4
5
6
7
8
cmake_minimum_required(VERSION 2.8)

project(testStream)

include_directories(.)

# 或者add_library(Foo "foo.cpp")
add_executable(${PROJECT_NAME} "main.cpp" "foo.cpp")

然后执行cmake后就会显示

报错Policy CMP0046 is not set

Policy CMP0046 is not set
可以不用管,也可以按提示修改,最好先安装:sudo apt-get install libproj-dev

参考:Policy CMP0046问题