我看到很多篇论文使用了log格式的数据集进行建图算法的仿真，比如这里的，发现这输入carmen框架，已经比较古老

1. 进入informatik.uni-freiburg选择download log file下载数据集
2. 看到一个文本网页,另存下载或者新建一個文件如data.clf保存.注意是.clf格式
3. 这个clf文件显然不是我们想要的,可以转换为bag文件. 使用clf_to_bag.py

MIT data center的数据集页面
数据集2011-04-11-07-34-27.bag所用odom坐标系是odom_combined

俄勒冈大学的数据集

数据集hallway_slow_2011-03-04-21-41-33.bag所用的雷达话题是base_laser1_scan

参考：各类数据集

实现二进制栅格地图

The object keeps track of three reference frames: world, local, and, grid. 世界坐标系的原点由GridLocationInWorld决定, which defines the bottom-left corner of the map relative to the world frame. LocalOriginInWorld property specifies the location of the origin of the local frame relative to the world frame. The first grid location with index (1,1) begins in the top-left corner of the grid.

map = binaryOccupancyMap creates a 2-D binary occupancy grid with a width and height of 10m. The default grid resolution is one cell per meter.

1
2
3
4
5
6

image = imread('F:\map.pgm');
%imshow(image)

imageBW = image < 100;
map = binaryOccupancyMap(imageBW);
show(map)

读取建好的pgm地图,如果直接imshow，结果就是pgm文件的本来样子

加下面几句后，结果是这样的

实现栅格地图

occupancyMap对象support local coordinates, world coordinates, and grid indices. The first grid location with index (1,1) begins in the top-left corner of the grid.

Use the occupancyMap class to create 2-D maps of an environment with probability values representing different obstacles in your world. You can specify exact probability values of cells or include observations from sensors such as laser scanners.

Probability values are stored using a binary Bayes filter to estimate the occupancy of each grid cell. A log-odds representation is used, with values stored as int16 to reduce the map storage size and allow for real-time applications.

1
2
3
4
5

image = imread('F:\map.pgm');
imageNorm = double(image)/255;
imageOccupancy = 1 - imageNorm;
map = occupancyMap(imageOccupancy,20);
show(map)

pgm文件中的值是0~255的uint8类型，将其归一化:先转为double类型,再除以255. 图片中的障碍物对应值为0,应该用1减去它,这样1就代表障碍物了. 否则图片显示出来是一团黑.

使用occupancyMap函数创建栅格地图,分辨率为1米20个cell,所支持的分辨率极限是±0.001

读取雷达扫描结果

1
2
3
4
5
6
7
8
9
10
11
12
13
14

# 建一个空地图,宽和高依次为10m,分辨率20,也就是1米有20个cell
map = occupancyMap(10,10,20);
# 机器人位姿[x,y,theta]
pose = [5,5,pi/2];
# ones函数是生成100行1列的矩阵, 元素全是1, 又都乘以3
# ones(n)函数是生成nXn的矩阵
ranges = 3*ones(100,1);
angles = linspace(-pi/2,pi/2,100);
maxrange = 20;
scan = lidarScan(ranges,angles);
# 把laser scan的数据插入栅格地图
insertRay(map,pose,scan,maxrange);

show(map)

linspace(x,y,n)是Matlab中的一个指令，用于产生x,y之间n点行矢量。其中x是起始值、y是中止值，n表示元素个数，如果缺省，则默认n为100。

linspace(1,10,2)为1，10. linspace(1,10,4)为1，4，7，10

laserScan对象

使用laserScan对象作为一些机器人算法的输入,例如matchScans, controllerVFH, monteCarloLocalization.

1
2
3
4

// ranges and angles inputs are vectors of the same length
scan = lidarScan(ranges,angles)
scan = lidarScan(cart)
scan = lidarScan(scanMsg) # 从ROS message中创建

1
2
3
4
5

minRange = 0.1;
maxRange = 7;
scan2 = removeInvalidData(scan,'RangeLimits',[minRange maxRange]);
hold on
plot(scan2)

根据指定的范围,移除invalid数据

参考:Matlab occupancy map

概述

ROS 的navigation官方功能包提供了三种全局路径规划器：carrot_planner、global_planner、navfn，默认使用的是navfn。

• carrot_planner检查需要到达的目标是不是一个障碍物，如果是一个障碍物，它就将目标点替换成一个附近可接近的点。因此，这个模块其实并没有做任何全局规划的工作。在复杂的室内环境中，这个模块并不实用。

• navfn使用Dijkstra算法找到最短路径。

• global planner是navfn的升级版。它支持A*算法； 可以切换二次近似； 切换网格路径；

目前常用的是global_planner，需要先设定move_base的参数： base_global_planner: "global_planner/GlobalPlanner"。global_planner根据给定的目标位置进行总体路径的规划，只处理全局代价地图中的数据。提供快速的、内插值的全局规划，目前已经取代navfn。遵循navcore::navcore 包中指定的BaseGlobalPlanner接口。它接受costmap生成的全局代价地图规划出从起始点到目标点的路径，为local_planner规划路径作出参考。

global_planner没有提供类似D*这样的动态方法，而是用了定时规划路径，ROS是启动了一个线程，在移动过程中对路径不断的重新规划。这个feature是可以去掉的，特别是当你的运算负载很高，处理器又有限的情况下。还有重新规划(当找不到路径，也就是走着走着新扫描到未知区域的障碍或者动态增加的障碍)两种办法。加上了定时规划和重新规划之后的A*和D*几乎是一模一样的。

配置

move_base是通过plugin调用全局规划器的，文件bgp_plugin.xml

1
2
3
4
5
6
7

<library path="lib/libglobal_planner">
  <class name="global_planner/GlobalPlanner" type="global_planner::GlobalPlanner" base_class_type="nav_core::BaseGlobalPlanner">
    <description>
      A implementation of a grid based planner using Dijkstras or A*
    </description>
  </class>
</library>

1
2
3

<export>
    <nav_core plugin="${prefix}/bgp_plugin.xml" />
</export>

参数

• allow_unknown: true    是否允许规划器规划在未知区域创建规划，只设置该参数为true还不行,还要在costmap_commons_params.yaml中设置 track_unknown_space 参数也为true才行，

• default_tolerance: 0.0    当设置的目的地被障碍物占据时，需要以该参数为半径寻找到最近的点作为新目的地点.

• visualize_potential: false    是否显示从PointCloud2计算得到的势区域. 这个参数可以让你看见potential array的图像，看计算出的cost是怎么样子(颜色深浅代表距离起始点的远近)

• use_dijkstra: true    设置为true,将使用dijkstra算法, 否则使用A*算法

• use_quadratic: true    设置为true,将使用二次函数近似函数计算potential，否则使用更加简单的计算方式，这样节省硬件资源

• use_grid_path: false    默认使用梯度下降法，路径更为光滑，从周围八个栅格中找到下降梯度最大的点。 如果为true，使用栅格路径，从终点开始找上下或左右4个中最小的栅格直到起点，会规划一条沿着网格边界的路径,偏向于直线穿越网格

• old_navfn_behavior: false    navfn是非常旧的ROS系统中使用的，现在已经都用global_planner代替navfn了，所以不建议设置为true.

• lethal_cost: 253    致命代价值,默认是设置为253,可以动态来配置该参数.

• neutral_cost: 50    中等代价值,默认设置是50,可以动态配置该参数.

• cost_factor: 3.0    代价地图与每个代价值相乘的因子.

• publish_potential: true    是否发布costmap的势函数.

• orientation_mode: 0    如何设置每个点的方向（None = 0,Forward = 1,Interpolate = 2,ForwardThenInterpolate = 3,Backward = 4,Leftward = 5,Rightward = 6）（可动态重新配置）

• orientation_window_size: 1    根据orientation_mode指定的位置积分来得到使用窗口的方向.默认值1,可以动态重新配置.

       A*和Dijkstra两种算法

两种算法的效果对比

A*比Dijkstra少计算很多，但可能不会产生相同路径。另外，在global_planner的A*里，the potentials are computed using 4-connected grid squares, while the path found by tracing the potential gradient from the goal back to the start uses the same grid in an 8-connected fashion. Thus, the actual path found may not be fully optimal in an 8-connected sense. (Also, no visited-state set is tracked while computing potentials, as in a more typical A* implementation, because such is unnecessary for 4-connected grids).

话题

发布的话题是~<name>/plan(nav_msgs/Path)，即最新规划出的路径，每次规划出新路径就要发布一次，主要用于观测。

GlobalPlanner::initialize

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
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74

if (!initialized_)
{
    ros::NodeHandle private_nh("~/" + name);
    costmap_ = costmap;
    frame_id_ = frame_id;   //costmap_ros->getGlobalFrameID()

    unsigned int cx = costmap->getSizeInCellsX(), cy = costmap->getSizeInCellsY();
    // 参数赋值, 默认false
    private_nh.param("old_navfn_behavior", old_navfn_behavior_, false);
    if(!old_navfn_behavior_)
        convert_offset_ = 0.5;
    else
        convert_offset_ = 0.0;

    bool use_quadratic;   // 二次方的
    private_nh.param("use_quadratic", use_quadratic, true);

    // p_calc_声明为PotentialCalculator指针
    // QuadraticCalculator是它的派生类, 唯一区别是覆盖了calculatePotential函数
    if (use_quadratic)    // 默认使用二次
        p_calc_ = new QuadraticCalculator(cx, cy);
    else
        p_calc_ = new PotentialCalculator(cx, cy);

    bool use_dijkstra;
    private_nh.param("use_dijkstra", use_dijkstra, true);
    //DijkstraExpansion 和 AStarExpansion 都继承 Expander类
    if (use_dijkstra)
    {
        DijkstraExpansion* de = new DijkstraExpansion(p_calc_, cx, cy);
        if(!old_navfn_behavior_)
            de->setPreciseStart(true);
        planner_ = de;      // Expander* planner_;
    }
    else   // 如果不用dijkstra,就用A星算法.
        planner_ = new AStarExpansion(p_calc_, cx, cy);

    bool use_grid_path;
    private_nh.param("use_grid_path", use_grid_path, false);
    // GradientPath 和 GridPath都继承了Traceback
    if (use_grid_path)
        path_maker_ = new GridPath(p_calc_);
    else
        path_maker_ = new GradientPath(p_calc_);
        
    orientation_filter_ = new OrientationFilter();
    // NavfnROS::initialize中也注册此话题
    plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
    potential_pub_ = private_nh.advertise<nav_msgs::OccupancyGrid>("potential", 1);
    // 一系列参数赋值
    private_nh.param("allow_unknown", allow_unknown_, true);
    planner_->setHasUnknown(allow_unknown_);
    private_nh.param("planner_window_x", planner_window_x_, 0.0);
    private_nh.param("planner_window_y", planner_window_y_, 0.0);
    private_nh.param("default_tolerance", default_tolerance_, 0.0);
    private_nh.param("publish_scale", publish_scale_, 100);

    double costmap_pub_freq;
    private_nh.param("planner_costmap_publish_frequency", costmap_pub_freq, 0.0);

    //get the tf prefix
    ros::NodeHandle prefix_nh;
    tf_prefix_ = tf::getPrefixParam(prefix_nh);

    make_plan_srv_ = private_nh.advertiseService("make_plan", &GlobalPlanner::makePlanService, this);

    dsrv_ = new dynamic_reconfigure::Server<global_planner::GlobalPlannerConfig>(ros::NodeHandle("~/" + name));
    dynamic_reconfigure::Server<global_planner::GlobalPlannerConfig>::CallbackType cb = boost::bind(
            &GlobalPlanner::reconfigureCB, this, _1, _2);
    dsrv_->setCallback(cb);

    initialized_ = true;
} else
    ROS_WARN("This planner has already been initialized, you can't call it twice, doing nothing");

参考：
ROS- global_panner
global_planner历史背景和概述

以下面的加权图举例，说明算法的过程

加权图中不可有负权边

或者这样理解：

1. 第一个核心步骤：找到当前未处理过的顶点中 $L_k$最小的点 V，（由于起点到起点的消耗为0，所以算法开始时 V 必定代表起点）；
2. 第二个核心步骤：若V有邻居，则计算经过 V 的情况下起点到达各邻居的消耗 $L$，并选择是否更新 V 邻居的$L_k$值。若没有邻居则对该点的处理结束
3. 重复以上两个核心步骤，直到满足算法终止的条件：有向图中所有的点都被处理过。

算法每处理完一个顶点后，该顶点对应的 $L_k$值就是起点到该点的最短路径长度，且$L_k$在这之后不会被更改。这就是最短路径的原因

Dijkstra的特点是从起点开始，由近及远，层层扩展。越靠前处理的点离起点越近，最后一个处理的点离起点最远。
即使我只想找两个点之间的最短路径，Dijkstra还是需要遍历所有节点，因此时间复杂度为$O(n^2)$，所以不适合复杂路径等大规模场景。

公式表示为：f(n)=g(n)+h(n)，其中f(n)是经过节点n从初始点到目标点的代价函数，g(n)表示从初始节点到节点n的代价，h(n)表示从节点n到目标点的启发式代价

• Dijkstra是无目的的扩展，A星是启发式，选择离目标点最近的方向扩展，但不一定是最短路径。
• Dijkstra算法的实质是广度优先搜索，是一种发散式的搜索，所以空间复杂度和时间复杂度都比较高。
• 对路径上的当前点，A*算法不但记录其到源点的代价，还计算当前点到目标点的期望代价，是一种启发式算法，也可以认为是一种深度优先的算法。

matlab执行dos命令

dos函数： dos('ping 192.168.0.109')

for循环

1
2
3
4
5
6
7

% ii  ---循环变量，也就是循环次数
clc;clear;

for ii = 1:10 
  fprintf('value of a: %d\n', ii);
end
  fprintf('跳出循环后，value of a: %d\n', ii);

控制表达式产生了一个1ⅹ10数组，所以语句1到n将会被重复执行10次。注意在循环体在最后一次执行后，循环系数将会一直为10。

randn 和 rand 函数

randn：产生正态分布的随机数或矩阵的函数

randn：产生均值为0，方差σ^2 = 1，标准差σ = 1的正态分布的随机数或矩阵的函数。

• Y = randn(n)：返回一个n*n的随机项的矩阵。如果n不是个数量，将返回错误信息。 n可以是0，此时为空矩阵

• Y = randn(m,n) 或 Y = randn([m n])： 返回一个m*n的随机项矩阵。

产生一个随机分布的指定均值和方差的矩阵：将randn产生的结果乘以标准差，然后加上期望均值即可。例如：产生均值为0.6，方差为0.1的一个5*5的随机数方式如下：

1

x = .6 + sqrt(0.1) * randn(5)

rand函数产生由在(0, 1)之间均匀分布的随机数组成的数组

• Y = rand(n): 返回一个n*n的随机矩阵如果n不是数量，则返回错误信息

• Y = rand(m,n) 或 Y = rand([m n]): 返回一个m x n的随机矩阵

scan_tools提供了一系列用于激光SLAM的工具,在github的分支只到indigo,所以无法从ros直接安装,但可以编译源码安装. 其中重要的有:

• laser_scan_matcher: an incremental laser scan matcher, using Andrea Censi’s Canonical Scan Matcher implementation. It downloads and installs Andrea Censi’s Canonical Scan Matcher [1] locally.

• scan_to_cloud_converter: converts LaserScan to PointCloud messages.

laser_filters 过滤雷达数据

过滤器的机制和代价地图是类似的，节点scan_to_scan_filter_chain相当于代价地图，每一个filter相当于地图的每一层，通过加载yaml而加载filter。

以range过滤器为例，修改range_filter.yaml如下：

1
2
3
4
5
6
7
8
9

scan_filter_chain:
- name: box_filter
  type: laser_filters/LaserScanRangeFilter
  params:
    # use_message_range_limits: false   # if not specified defaults to false
    lower_threshold: 0.18  # 默认0
    upper_threshold: 0.22  # 默认100000.0
    lower_replacement_value: 0    # 默认 NaN
    upper_replacement_value: 999  # 默认 NaN

结果查看scan_filtered话题，只显示出0.18~0.22距离的数据，太小的显示为0，太大的显示为999.

又比如使用LaserScanAngularBoundsFilter，只取-45°~45°内的scan，结果如下，要把laser坐标系的x轴放到水平向右的方向观察 ：

参考：
scan_filtered的使用
laser_filters

读文件

txt文件里是这样的：

1
2
3
4

-3.35 2.332 
-3.18 1.876 
-3.10 2.577 
-3.51 2.991 

可以这样画散点图，最后设置了坐标轴的范围：

1
2
3
4
5
6

clear all
close all
data=load('E:\data.txt');
scatter(data(:,1),data(:,2))
xlim([-6 4])
ylim([-4 10])

读点云文件画图：

1
2
3
4
5
6
7
8
9
10
11

A = load('cloud3d.dat'); %读入数据
%x,y,z轴坐标
x=A(:,1);
y=A(:,2);
z=A(:,3);
scatter3(x,y,z);    %散点图
[X,Y,Z]=griddata(x,y,z,linspace(min(x),max(x))',linspace(min(y),max(y)),'v4'); %构造坐标点
pcolor(X,Y,Z);
shading interp;        %伪彩色图
fcontourf(X,Y,Z);     %等高线图
figure,surf(X,Y,Z);    %三维曲面

写文件

1
2
3
4
5
6
7
8

fileID = fopen('F:\test.ply', 'w');
fprintf(fileID, 'ply\n');
fprintf(fileID, 'format ascii 1.0\n');
fprintf(fileID, 'property int path_id\n');
fprintf(fileID, 'property int group_id\n');
fprintf(fileID, 'end_header\n');
fprintf(fileID, '%f %f %f %d %d\n', [1,2,3,4,5]);
fclose(fileID);

打开文件是：

1
2
3
4
5
6

ply
format ascii 1.0
property int path_id
property int group_id
end_header
1.000000 2.000000 3.000000 4 5

函数名就是个指针,它指向函数代码在内存中的首地址.

1
2
3
4
5
6
7
8
9

typedef double (*FuncType) (int data);
double func(int num)
{
    return num;
}

// 调用
FuncType ptr = func;
cout << ptr(11) <<endl;

第一行定义了一种函数指针,类型为FuncType,它指向的函数返回为 double, 形参为一个int. 然后定义了一个名为func的函数.

FuncType ptr = func;是声明一个指针, 类型为 FuncType, 指向func函数. 接下来就可以拿ptr当函数用了

1
2
3
4
5
6
7
8

void test(int id, FuncType foo)
{
    cout << foo(110) << endl;
    cout << "id: "<<id<<endl;
}

//调用
test(1, static_cast<FuncType>(func) );

定义一个函数test, 它第二个形参是类型为FuncType的函数指针. 调用时, 最好对第二个形参做转换, 这里用static_cast再合适不过.

异常是运行期出现的情况，编译不会报错。如果出现异常，它后面的代码不会执行，一般会显示 The program has unexpectedly finished. </font>。如果能处理好异常，就可以让后面的代码继续运行

throw就是抛出异常，后面可以接任何语句表示异常。比如throw 123;, throw "exception";

try里面的第一个语句必须包含throw，可以是个函数。之后的语句不再进行，直接进catch了

catch的参数是和throw一致的，比如下面的const char*，如果要catch任何类型，小括号内换成...

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

double division(int a, int b)
{
   if( b == 0 )
      throw "Division by zero condition!";
   return (a/b);
}


try {
    cout << division(1,0) <<endl;
    cout << "本句不执行" <<endl;
}
catch(const char* msg)
{
  cerr << msg << endl;	// 输出错误用cerr
}

C++ 提供了一系列标准的异常，定义在，我们可以在程序中使用这些标准的异常。它们是以父子类层次结构组织起来的。可以使用catch(std::exception e)

一些第三方库也提供了异常，使用时要注意。比如log4cpp:

1
2
3
4
5
6
7
8

try
{
    log4cpp::PropertyConfigurator::configure(config_base_path+"setting.conf");
}
catch (log4cpp::ConfigureFailure& f)
{
    std::cout << "Configure Problem: " << f.what() << std::endl;
}