1

private_nh.param("base_local_planner", local_planner, std::string("base_local_planner/TrajectoryPlannerROS"));

local planner

根据附近的障碍物进行躲避路线规划,只处理局部代价地图中的数据. 是利用base_local_planner包实现的。该包使用Trajectory Rollout和Dynamic Window approaches算法计算机器人每个周期内应该行驶的速度和角度。

对于全向机器人来说，也就是存在x方向的速度，y方向的速度，和角速度。DWAlocalplanner确实效率高一点。但是如果是非全向机器人，比如说只存在角速度和线速度，trajectorylocalplanner会更适用一点。

base_local_planner这个包通过地图数据，通过算法搜索到达目标的多条路经，利用一些评价标准（是否会撞击障碍物，所需要的时间等等）选取最优的路径，并且计算所需要的实时速度和角度。
其中，Trajectory Rollout 和Dynamic Window approaches算法的主要思路如下：

1. 采样机器人当前的状态（dx,dy,dtheta）；
2. 针对每个采样的速度，计算机器人以该速度行驶一段时间后的状态，得出一条行驶的路线。
3. 利用一些评价标准为多条路线打分。
4. 根据打分，选择最优路径。
5. 重复上面过程。

发送导航目标点后，首先在rviz上出现一个绿色全局路径，机器人开始行走，同时还有一个蓝色的本地路径，只是很小，不容易观察到，示意图如下：

现在机器人正在按照规划的全局路径行走,地图中突然增加了一个障碍物,它不会进入全局代价地图,但会在局部代价地图中创建.所以全局路径开始没有改变,当它快接近障碍物时,局部路径检测到了它,进而会重新规划一个全局路径,这样避开了障碍物. 局部规划器把全局路径分成几部分，再执行每一部分，生成控制命令。

话题

机器人行走时，在rviz上显示全局路径的相关话题是/move_base/DWAPlannerROS/global_plan，属于Global Map。局部规划器把全局路径的一部分发布到这个话题，代码是在DWAPlannerROS::computeVelocityCommands——DWAPlannerROS::publishGlobalPlan

rviz中显示局部路径的相关话题是/move_base/DWAPlannerROS/local_plan，属于Local Map。一旦局部路径计算出来，它就会发布到这个话题，代码是在DWAPlannerROS::computeVelocityCommands——DWAPlannerROS::publishLocalPlan

常用算法

ROS的局部规划器的接口plugin类为nav_core::BaseLocalPlanner，常用的包有4种：

• base_local_planner - 提供了 Dynamic Window Approach(DWA)和Trajectory Rollout方法来局部控制；
• dwa_local_planner - 模块化DWA方法应用，相比base_local_planner具有更多的恢复机制、更便于理解的接口和针对全向运动机器人更灵活的Y轴变量控制；
• teb_local_planner

参考：
dwa_local_planner

可以参考
如何在环境轮廓建图完成之后，继续完善地图细节
SLAMTEC激光建图传感器使用技巧

建地图时，最好雷达面对墙壁，小车逐渐倒退来完善地图，不要让小车正面对着墙壁和空旷环境前进，在某部分环境建地图后，可以让小车正面前进。

绘制的地图，黑线（墙）周围的黑点越少越好，毛刺越少越好，漏光线的环境会出现很长的白线，蓝色的块是避障保护区。

建图时出现重影

一般有两个方面的原因：

1. 里程计累计误差
2. 建图数据的时间戳可能没有对齐。

一般第一种比较常见，比如说编码器是否存在丢失脉冲的情况、换个比较坚硬的地面跑一跑看看建图效果。第二种比较难排查，最好能够录下数据查看。

删除文本第四行

1

sed '4d' input.txt > output.txt

实际上没有修改input.txt，而是修改文件流，然后生成到文件output.txt。前后两个文件名称不能相同，否则不能保存

使用sed处理当前目录(包括子目录)的所有cpp文件,将其中的aaa替换为bbb

1

sed -i "s/aaa/bbb/g" `grep aaa -rl . --include="*.cpp" `

grep:

• r 表示查找所有子目录
• l 表示仅列出符合条件的文件名，用来传给sed命令做操作
• --include="*.cpp" 表示仅查找cpp文件

整个grep命令用`号括起来,表示结果是一系列文件名

sed -i后面是要处理的文件,s/aaa/bbb/g跟vim下的替换命令一样,表示把aaa全替换为bbb,加g表示一行有多个aaa时,要全部替换,

删除空白行: sed ‘/^$/d’ -i delete.txt

对于当前目录(包括子目录)的所有txt文件,删除含有以ros开头字符串的所有行

1

sed -i '/ros/d' `grep -rl "^ros" . --include="*.txt" `

删除前5个字符

1

sed 's/.\{5\}//' test.log > a.log

slam_gmapping节点在gmapping包里面，它的节点信息如下：

它获取sensor_msgs/LaserScan的消息,然后用来建图。/slam_gmapping一边建图一边将地图数据发布到map话题，如果用rviz订阅map话题，我们就能看到建图的过程了.

话题和服务

entropy话题实际是simple_gmapping/entropy，发布机器人姿态分布熵的估计

服务dynamic_map(nav_msgs/GetMap)，调用该服务可以获取地图数据，可以用rosservice call试验

需要的tf变换:

• laser → base_link, 一般是固定的变换,使用static_transform_publisher节点实现
• base_link → odom, 由里程计系统提供

运行导航程序后，获得tf变换关系：

slam_gmapping的参数

gmapping包生成三个节点,分别为slam_gmapping和slam_gmapping_replay及一个测试用的节点。我们主要看slam_gmapping节点,它用到了很多参数，蓝色的是影响CPU性能的

gmapping wrapper参数

• throttle_scans : [int,默认1] 当接收到n个(默认1个)激光扫描数据，就进行一次更新
• base_frame: [string, default:”base_link”] 机器人上用的坐标系frame_id
• map_frame: [string,default:”map”] 机器人地图所用的frame_id
• odom_frame: [string,default:”odom”] 里程计坐标系的frame_id
• map_update_interval : [float,default:5.0] 两次更新地图的间隔,降低参数会更新occupancy grid更频繁,但增大CPU占用

Laser Parameters

• maxRange [double] 雷达扫描的最大有效距离,超过这个数值的扫描会忽略。

• maxUrange [double,default: maxRange] 用于建图的最大可用扫描距离，因为激光
  雷达的测量距离越远，测量数据的误差越大， 考虑扫地机器人为室内机器人， 因
  maxUrange尽量设置小一些。

• sigma [float, default: 0.05] The sigma used by the greedy endpoint matching。endpoint匹配标准差,计算score时的方差

• kernelSize [int,default: 1] The kernel in which to look for a correspondence, search window for the scan matching process. 用于查找对应的kernel size, 主要用在计算score时搜索框的大小

• lstep [float,default: 0.05] initial search step for scan matching (linear).平移优化步长

• astep [float,default: 0.05] initial search step for scan matching (angular).旋转优化步长

• iterations [int, default: 5] 扫描匹配的迭代步长的细分数量，最终距离和角度的匹配精度分别为lstep * 2 ^ (-iterations) 和 astep * 2 ^ (-iterations)，这个不是ICP迭代次数，而是用在ScanMatcher::optimize里的搜索步长范围

• lsigma [float, default: 0.075] The sigma of a beam used for likelihood computation. 用于扫描匹配概率的激光标准差

• ogain [float, default: 3.0] Gain to be used while evaluating the likelihood, for smoothing the resampling effects. 似然估计为平滑重采样影响使用的gain. 对应函数normalize()中的m_obsSigmaGain，用于粒子权重归一化

• lskip [int, default: 0]计算似然时，跳过的激光束。0表示所有的激光都处理，如果计算压力过大，可以改成1。源码中的逻辑相当于这样：

1
2
3
4
5
6
7
8
9
10

// 令 lskip 为5， 50为激光束的个数
unsigned int skip=0;
for (unsigned int i=0; i<50; i++)
{
    skip++;
    skip=skip > 5 ? 0 : skip;
    if (skip)
        continue;
    cout << i << "  ";
}

结果为 5 11 17 23 29 35 41 47

• minimumScore [float, 默认 0.0] 最小匹配得分，一定要自定义，它决定了你对激光的一个置信度，越高说明你对激光匹配算法的要求越高，激光的匹配也越容易失败而转去使用里程计数据，而设的太低又会使地图中出现大量噪声。为获得好的扫描匹配输出结果，用于避免在大空间范围使用有限距离的激光扫描仪（如5m）出现的jumping pose estimates问题。当Scores高达600+，如果出现了该问题可以考虑设定值50。

Others:

• linearUpdate: [float,默认1] 机器人走过多少米,gmapping才处理雷达scan数据

• angularUpdate: [float,默认0.5] 机器人转过多少弧度,gmapping才处理雷达scan数据

• temporalUpdate: [float,默认-1] 两次雷达scan数据之前的等待时间,小于0则不启用这个功能

• resampleThreshold [float,default: 0.5] 粒子重新采样的门槛值,越大表示重新采样越频繁

• particles : [int,默认30] 决定gmapping算法中的粒子数，每个粒子代表了机器人走过的可能的trajectory，因为gmapping使用的是粒子滤波算法，粒子在不断地迭代更新，所以选取一个合适的粒子数可以让算法在保证比较准确的同时有较高的速度。 看gmapping论文，一般设置为30，高难度地图用80，我一般用50个，大概对应CPU占比25%

运动模型参数 (高斯噪声模型的所有标准偏差)

• srr [float, default: 0.1] linear noise component (x and y),Odometry error in translation as a function of translation (rho/rho) 线性运动造成的线性误差的方差

• srt [float, default: 0.2] linear -> angular noise component,Odometry error in translation as a function of rotation (rho/theta). 线性运动造成的角度误差的方差 (rho/theta)

• str [float, default: 0.1] angular -> linear noise component,Odometry error in rotation as a function of translation (theta/rho). 旋转运动造成的线性误差的方差 (theta/rho)

• stt [float, default: 0.2] angular noise component (theta),Odometry error in rotation as a function of rotation (theta/theta). 旋转运动造成的角度误差的方差 (theta/theta)

Likelihood sampling (used in scan matching)

• llsamplerange [float, default: 0.01] Translational sampling range for the likelihood. 用于似然计算的平移采样距离
• llsamplestep [float, default: 0.005] Translational sampling step for the likelihood. 用于似然计算的平移采样步长
• lasamplerange [float, default: 0.01] Angular sampling range for the likelihood. 用于似然计算的角度采样距离
• lasamplestep [float, default: 0.005] Angular sampling step for the likelihood. 用于似然计算的角度采样步长

地图初始维度和分辨率

• xmin [double] minimum x position in the map [m]
• ymin [double] minimum y position in the map [m]
• xmax [double] maximum x position in the map [m]
• ymax [double] maximum y position in the map [m]
• delta [double] 地图分辨率,就是话题map_metadata中的resolution
• occ_thresh (float, default: 0.25)，栅格地图栅格值 (i.e., set to 100 in the resultingsensor_msgs/LaserScan).

参考: ROS Wiki-gmapping

vector的底层（存储）机制

vector就是一个动态数组，底层有一个指针指向一片连续的内存空间，当空间不够装下数据时，会自动申请另一片更大的空间（一般是当前容量的2倍），然后把原来的数据拷贝过去，接着释放原来的那片空间；如果元素数量比较大(比如上百个)，重新分配内存的耗费会非常大，可能会出错。

当删除里面的数据时，其存储空间不释放，仅仅是清空了里面的数据。

vector中begin和end函数返回的是什么？

begin返回的是第一个元素的迭代器，end返回的是最后一个元素后面位置的迭代器。

为什么vector的插入操作可能会导致迭代器失效？

vector动态增加大小时，并不是在原空间后增加新的空间，而是以原大小的两倍在另外配置一片较大的新空间，然后将内容拷贝过来，并释放原来的空间。由于操作改变了空间，所以迭代器失效。

vector怎么实现动态空间分布?

vector容器基于数组实现，其元素在内存中连续存放，vector容器除了容器尾部之外，在其他任意位置插入或删除元素时，都需要移动该元素后面的所有元素

capacity和size

capacity是容器不用重新分配内存就可容纳的元素个数，容器的大小一旦超过capacity的大小，vector会重新配置内部的存储器，导致和vector元素相关的所有reference、pointers、iterator都会失效。内存的重新配置会很耗时间，
size()是容器中现有的元素个数，max_size()返回Vector所能容纳元素数量的最大值(包括可重新分配内存)，一般是一个很大的数

reserve

vector频繁的销毁新建内存会导致效率很低，所以正确的做法是新建vector的时候初始化一个合适的大小，就像使用普通数组一样，这就用到了reserve。 如果reserve的值大于当前的capacity，那么会重新分配内存

reserve是容器预留空间，但并不真正创建元素对象，在创建对象之前，不能引用容器内的元素。否则会报错terminate called after throwing an instance of 'std::out_of_range'

保证vector的capacity最少达到它的参数所指定的大小n

resize

resize跟reserve不同了，它是改变容器的大小，并且可以创建对象，调用这个函数之后，就可以引用容器内的对象了.

1
2

void resize(size_type _Newsize)
void resize(size_type _Newsize, _Ty _Val)

对于第1个版本：

• 若_Newsize小于oldsize，则剩余元素值不变
• 若_Newsize大于oldsize，则新添加的元素值用元素的默认构造函数初始化（特别的，int型的将被初始化为0）

对于第2个版本：

• 若_Newsize小于oldsize，则剩余元素值不变,全部调用erase(begin() + _Newsize,end())擦除掉多余元素
• 若_Newsize大于oldsize，则新添加的元素值用提供的第2个参数初始化。

1
2
3
4
5
6
7
8
9
10

vector<int> vec;
vec.push_back(1);
vec.push_back(2);
vec.push_back(3);
vec.push_back(4);
vec.push_back(5);

cout<< vec.size() <<"  "<<vec.capacity()<<"  "<<endl;   // 5   8
vec.resize(10);     // 改变大小
cout<< vec.size() <<"  "<<vec.capacity()<<"  "<<endl;   // 10   10

如果_Newsize小于之前的capacity，那么capacity不变。如果大于，那么capacity也要改变.也就是说 resize也涉及了内存重新分配，同时改变了capacity和size

data

返回指向vector中第一个数据的指针或空vector之后的位置

1
2

vector<int> v{1,2,3,4,5};
qDebug()<< *(v.data() ) ;  // 1

assign

1
2
3
4

// 先清除vector之前的内容,再将区间[first, last)的元素赋到当前vector
void assign(const_iterator first, const_iterator last);
// 先清除vector之前的内容,再将Count个Val插入到vector
void assign( size_type _Count, const Type& _Val );

使用assign赋值给容器:

1
2
3
4

vector<int> vec;
vec.assign(5,9);

cout<< vec.size() <<"  "<<vec.capacity()<<"  "<<endl;

push_back

但是如果使用push_back:

1
2
3
4
5
6
7

vector<int> vec;
vec.push_back(1);
vec.push_back(2);
vec.push_back(3);
vec.push_back(4);
vec.push_back(5);
cout<< vec.size() <<"  "<<vec.capacity()<<"  "<<endl;

每次执行push_back操作,相当于底层的数组实现要重新分配大小,如果capacity不够插入新元素，需要开辟新的内存空间，把原来的元素复制到新内存，释放原来内存。

频繁的vector扩容会使效率很低，所以 不要使用for循环和push_back给vector赋值，这样会多次重新分配内存；如果先reserve分配需要的内存空间，再push_back会显著提高效率。

vector的几种清空容器办法

1
2
3
4

iterator erase(iterator it); // 删除指定元素，并返回删除元素后一个元素的位置（如果无元素，返回end()）
iterator erase(iterator first, iterator last); // 注意：删除元素后，删除点之后的元素对应的迭代器不再有效。

void clear() const; // 函数删除当前vector中的所有元素，相当于调用erase( begin(), end())

释放内存

vector的内存空间是只增加不减少的，erase和clear只删除元素，不能释放内存。如果是C++11环境，直接使用std::vector::shrink_to_fit()，它会将capacity缩减到size大小，所以如果要释放内存，可以先clear，再shrink_to_fit，这跟Qt里的qDeleteAll非常相似

元素是指针时，如何释放内存

如果vector中存放的是指针，而这些指针指向的对象没有销毁，那么当vector销毁时，内存不会被释放。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25

int v[6] = {1,2,3,4,5,6};
std::vector<int*> ps;
int* p1 = v;
int* p2 = v+1;
int* p3 = v+2;
int* p4 = v+3;

ps.push_back( p1 );
ps.push_back( p2 );
ps.push_back( p3 );
ps.push_back( p4 );

// delete ps.back(), ps.pop_back();

for (auto it = ps.begin(); it != ps.end(); it++)
{
  if (*it != NULL)
  {
    delete *it;
    *it = NULL;
  }
}

ps.clear();
ps.shrink_to_fit();

vector赋值速度

vector所有赋值方式的速度排名：

1. assign
2. resize以后用copy算法赋值
3. 赋值操作符，也就是 destinationVector = sourceVector;
4. 采用插入迭代器再用copy的方式
5. insert
6. push_back

记住最快的是assign，最慢的是push_back

vector遍历元素的速度

一般的速度排名： iterator > [] > at

但是根据编译器和平台的不同，iterator和[]速度有快有慢，但基本上at都是最慢的

尽量避免在vector前部插入元素

一般也没人这么干，任何在vetor前部的插入操作其复杂度都是O(n)的，所以十分低效，如果实在需要在前部插入元素，可以用list，它的效率远远超过vector

这样的代码是valid

1
2
3

using Type = std::vector<int>;
int f = 12;
Type vec={f};

参考：
vector/list的几种赋值方法的速度比较

svn的用户名，密码都是明文的,都在~/.subversion/auth/svn.simple下面的文件里.

SVN 无法登入

经查，原因是没有获得权限

某文件处于冲突状态

这说明文件没有处于某个版本，导致其他文件无法成功提交。所以先解决冲突:
svn revert indoor.yaml或者是svn resolve --accept working indoor.yaml，一般用前者

节点冲突

节点冲突和文件冲突不同，这次应该到上级目录执行svn resolve .，然后就可以升级和提交了

找不到节点

原因是这个文件是新创建的，但没有add和commit，所以找不到节点

基本操作

• 更新代码： 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)'