navigation stack的move_base包中一个插件。DWA的速度空间中如果没有可行的采样点，那么机器人get stuck，触发recovery行为。

recovery行为的实质是clear out space——试图搞清楚自己的处境：

1. 首先机器人会清扫地图——conservative reset
2. 然后原地旋转360度，刷新处境——clearing rotation
3. 如果还是导航失败，机器人会更加激进的清扫地图——aggressive reset
4. 然后原地旋转360度，刷新处境——clearing rotation
5. 如果仍旧失败——mission impossible

源代码在move_base.cpp里面，继承了nav_core的接口，设置在move_base_params.yaml配置文件中。

• nav_core的recovery_behavior.h封装了RecoveryBehavior类。
• move_base中创建了名为”clear_costmap_recovery/ClearCostmapRecovery”、”rotate_recovery/RotateRecovery”、”clear_costmap_recovery/ClearCostmapRecovery”的默认对象。
• move_base的主程序是一个状态机，case CLEARING就调用RecoveryBehavior的runBehavior()。

动态窗口：

窗口框的是速度空间的采样点$(v_t, w_t)$，一对$(v_t, w_t)$就代表一段轨迹，轨迹通过机器人底盘的运动学建模得到。

1
2
3
4
5
6
7
8
9
10

// base_local_planner的simple_trajectory_generator.cpp
// 直线模型
double dt = (current_time - last_time).toSec();
double delta_x = (vx * cos(theta) - vy * sin(theta)) * dt;
double delta_y = (vx * sin(theta) + vy * cos(theta)) * dt;
double delta_th = vth * dt;

x += delta_x;
y += delta_y;
theta += delta_th;

窗口的选择：

1. 速度限制

   $$(V, W) = \{v \in[v_{min}, v_{max}], w \in [w_{min}, w_{max}] \}$$

2. 加速度限制

   $$(V, W) = \left\{ \begin{array} & v \in[v_c - \dot{v}*\Delta t, v_c + \dot{v}*\Delta t], \\ w \in [w_c - \dot{w}*\Delta t, w_c + \dot{w}*\Delta t] \end{array} \right\}$$

3. 障碍物制动限制

   $$(V, W) = \left\{ v \leq \sqrt{2*dist(v,w)*\dot{v}}, w \leq \sqrt{2*dist(v,w)*\dot{w}} \right\}$$

   $dist(v,w)$表示采样点$(v, w)$对应轨迹上离障碍物最近的距离。

确定窗口后进行采样，可以得到一系列轨迹：

轨迹的选择：

原始论文中采用评价函数：

1. 方位角评价函数：采用当前采样点设定下，达到模拟轨迹末端时机器人的朝向角与目标点朝向角的差距。
2. 空隙评价：当前模拟轨迹上与最近障碍物之间的距离。
3. 速度评价：采样点速度与最大速度的差距。

上述评价函数要进行归一化。

算法流程：

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

BEGIN DWA(robotPose,robotGoal,robotModel)
    laserscan = readScanner()
    allowable_v = generateWindow(robotV, robotModel)
    allowable_w  = generateWindow(robotW, robotModel)
	for each v in allowable_v
        for each w in allowable_w
            dist = find_dist(v,w,laserscan,robotModel)
            breakDist = calculateBreakingDistance(v)
            if (dist > breakDist)  //can stop in time
                heading = hDiff(robotPose,goalPose, v,w) 
                clearance = (dist-breakDist)/(dmax - breakDist) 
                cost = costFunction(heading,clearance, abs(desired_v - v))
                if (cost > optimal)
                    best_v = v
                    best_w = w
                    optimal = cost
	set robot trajectory to best_v, best_w
END

有目的性地寻找最佳路径，首先定义一个损失函数，表示节点消耗：

$g$表示起点到当前节点的已知消耗

$h$表示对当前节点到终点消耗的猜测，估价函数有多种形式——启发式探索的核心

算法流程：

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

初始化openList
初始化closeList
将起点放入openList
while openList非空：
	找到开启列表上f最小的节点，记为q
    找到q周围的子节点p，记其父节点为q
    for 每一个子节点p：
    	if p是终点：
        	算法终止
            
        p.g = q.g + qp
        p.h = h(p, terminate)
        p.f = p.g + p.h
        if p已经在开启列表中且保存的f值小于当前计算值||p在关闭列表中：
        	跳过该节点
		else：
        	if p已经在开启列表中：
        		修改该节点的信息（父节点、fgh）
			else：
            	将该节点加入openList
	将q放入closeList

算法性能在细节上的优化：http://theory.stanford.edu/~amitp/GameProgramming/

序言：路径搜索算法的前世今生

Dijkstra算法：从初始节点开始向外扩展，直到到达目标节点。算法保证能找到从初始点到目标点的最短路径。

最佳优先搜索BFS算法：算法能够评估任意节点到目标点的代价，并优先选择离目标最近的结点。启发式算法，比Dijkstra算法运行快得多，但是不能保证路径最短。

如下面这种情况：

因为BFS是基于贪心策略的，它只关注到尽可能向着目标点移动，而不考虑已花费的代价。Dijkstra算法则正相反，它会确保每一步都是最优的，但是为此要遍历周围全部的节点。

A*算法：将两种路径搜索算法的思想结合起来，考虑两个极端及其中间的情况：

• 如果$h(n)$是0，只有$g(n)$起作用，那么算法演变为Dijkstra算法。

• 如果$h(n)$能够始终满足“比当前节点移动到目标节点的实际代价小”，那么算法保证能够找到最短路径。（$h(n)$越小，算法扩展的节点数就越多）

• 如果$h(n)$能够精确等于“当前节点移动到目标节点的实际代价”，那么算法将会仅仅扩展最优路径。而不扩展其他节点，算法运行非常快。
• 如果$h(n)$有时会“比当前节点移动到目标节点的实际代价大”，那么此时算法不能保证最短路径了。
• 如果$g(n)$比$h(n)$小的多，只有$h(n)$起作用，那么算法演变为BFS算法。

估价函数Heuristic function $h(a, b)$

估价函数的选择可以follow以下的instructions：

1. square grid that allows 4 directions：use Manhattan distance (L1)

   $$dx = abs(a.x - b.x)\\ dy = abs(a.y - b.y)\\ dis = D*(dx+dy)$$

2. square grid that allows 8 directions：use Diagonal distance (L∞)

   $$dx = abs(a.x - b.x)\\ dy = abs(a.y - b.y)\\ dis = D*(dx+dy) + (D2 - 2*D)*min(dx, dy)$$

   当$D = D2 =1$时，$dis = dx + dy -min(dx, dy) = max(dx, dy)$，这个距离称为切比雪夫距离。

   当$D = 1, D2 =\sqrt 2$时，这个距离称为octile distance。

3. square grid that allows any direcitons：use Euclidean distance (L2)

   $$dx = abs(a.x - b.x)\\ dy = abs(a.y - b.y)\\ dis = D*\sqrt{dx*dx + dy*dy}$$

   If A* is finding paths on the grid but you are allowing movement not on the grid, you may want to consider other representations of the map

   ​ 欧几里得距离并不适用于栅格地图，因为这会导致代价函数g和估价函数的不匹配（代价函数并不是连续的）。

   由于欧几里得距离引入了开根号计算，一些算法会直接用$dis = D(dxdx + dydy)$来代替，*不建议！，会引入尺度问题，$f = g + h$，其中代价函数会逐渐增长，估价函数则逐渐减小，平方会导致两个函数的变化速率不match，使得估价函数的权重过大，导致BFS。

code：

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

public function manhattanHeuristic(a:Object, b:Object):Number {
	return graph.distance(a, b) + simpleCost(a, b) - 1;
}

public function simpleCost(a:Object, b:Object):Number {
	var c:Number = costs[graph.nodeToString(b)];
	if (isNaN(c)) {
		return 1;
	} else {
    	return c;
	}
}
// simpleCost限定为小于等于1的数

此时$h(a,b) = dis(a,b)+c-1 \leq h^{*}(a,b)$，此时能够找到最优解。

一个栗子：整数规划问题欲求$max\ z = 5x_1 + 8x_2$

根据方程组可以绘制下图：

于是可以得到实数空间上的最优解：$x_1 = 2.25, x_2 = 3.75, z_0 = 41.25$。——松弛问题

由于存在整数限定条件：

1. 最优解$0 \leq z^{*} \leq 41$，且必为整数
2. x_2的最优解不在3和4之间，因为限定为整数

一、分支

于是问题可以拆分为：$max\ z = 5x_1 + 8x_2$

问题拆分的实质是将$x_2$在3和4之间的小数部分划掉了，将可行域拆分成$x_2 \leq 3$ 和$x_3 \geq 4$，但是没有排除任何整数可行解。——分支

二、定界

子问题$p1$的最优解为：$x_1 = 3, x_2=3, z^{*}=39$

子问题$p2$的最优解为：$x_1 = 1.8, x_2=4, z^{*}=41$

也就是说，子问题$p1$的整个参数空间上，能够取得的最优解为39，子问题$p2$上则为41，显然最优解应该位于子问题$p2$所在的参数空间中，且$39\leq z^{} \leq41$。——*定界

三、迭代

对$p2$参数空间再分支，参数$x_1$可以拆分为$x_1 \leq 1$和$x_1 \geq 2$：

四、总结

分支定界算法的总体流程如下：

1. 先求解相应的松弛问题，得到最优解，检查其是否符合原问题约束，若符合则为最优解，否则进行下一步。
2. 定界，取各分支中目标函数最大的作为上界$U_z$，取其余分支中目标函数中最大的作为下界$L_z$。$L_z \leq z^{*} \leq U_z$。
3. 分支，否则选择一个不符合原问题条件的变量，构建子问题。
4. 对各分支，有序地，进行步骤1。

在求解对应的松弛问题时，通常会有以下几种情况：

1. 松弛问题没有可行解，那么原问题也没有可行解。
2. 松弛问题的最优解也满足原问题约束，那么该解也是原问题的最优解，算法终止。
3. 松弛问题的最优解小于现有下界，那么应该对该子问题进行剪枝。

五、DFS

对一颗搜索树，不用计算每一层全部节点的score（BFS），我们会维护一个优先队列，其中按照score的大小存放节点，然后选择score最大的节点（the most promising child）进行分支。

系统总体的通信架构如下：

底盘驱动板Arduino负责接收上层的运动控制指令，并驱动电机，两块板子通过串口进行通信。

ROS提供了一个ros_arduino_bridge功能包集，它包括了Arduino库（ROSArduinoBridge）以及一系列用来控制Arduino-based robot的ROS功能包，这个功能包可以实现读取Twist控制信息，以及发布里程计信息等任务，封装了Raspberry Pi和Arduino之间的底层通信。

Arduino库（ROSArduinoBridge）位于ros_arduino_firmware/src/libraries/路径下，里面是一些arduino脚本和头文件，将这个文件夹复制到我们Arduino IDE的SKETCHBOOK_PATH下，然后在Arduino IDE中就可以直接打开这个sketch项目。

ROSArduinoBridge文件下是一些配置选项，另外commands.h文件中给出了一些可用的串口控制指令，如电机控制指令：

1

m 20 20   // move the robot forward at 20 encoder ticks per second

实验中使用了两种类型的底盘，基于差速驱动的2WD底盘和基于滑动转向的4WD底盘。两种驱动方式原理相似，也有其显著的区别。

相同点：

两种底盘都没有显示的转动机制，采用差速驱动的方式通过以不同的方向或速度驱动两边轮子来实现方向控制。

差速驱动的运动形式通常有一下几种类型：

• 第一种是原地旋转，左右轮的速度大小相等，方向相反，这样相当于绕着底盘的形心原地打转。
• 第二种是沿着某个方向直线行走，此时左右轮速度相同。
• 第三种是沿着某条曲线前行或后退，此时左右轮速度方向相同，大小不同。
• 第四种是旋转转弯，此时左右轮速度方向相反。

两种机构共同的优势是：没有显示的转向机构，极大地简化了运动学模型。

而两种机构共同的缺点是：由于两侧的轮子是由独立电机分别驱动的，直线运动要求两侧的轮子以相同速度转动，这将很难完成。

不同点：

差速驱动底盘通常是由一个两轮系统，每个轮子都带有独立的执行机构（直流电机），以及一个无驱动轮（可以是脚轮或者万向滚珠）组成，机器人的运动矢量是每个独立车轮运动的总和。

滑动转向底盘通常被用在履带车上，比如坦克和推土机，也被用于某些四轮六轮机构上，相比较于两轮差速底盘，滑动转向的主要区别在于：

• 优势：滑动转向使用了两个额外的驱动轮代替了差速驱动的脚轮，增大了牵引力。
• 劣势：引入了滑动，在对里程计要求高的场景中，滑动是一个致命的缺陷，因为这会对编码器造成负面影响，滑动的轮子不会跟踪机器人的确切运动。

运动学分析：

对于差速驱动机构，移动机器人航向角变化了多少角度，它就绕其运动轨迹的圆心旋转了多少角度。这句话很好验证，我们让机器人做圆周运动，从起点出发绕圆心一圈回到起点处，在这过程中机器人累计的航向角为360度，同时它也确实绕轨迹圆心运动了360度。

机器人的速度是指两个相邻的控制时刻之间的速度，因此小车的行驶轨迹可以分解为连续的圆弧片段，对于每一段圆弧，根据阿克曼转向几何原理，在小车转向时，为保证行驶稳定性，两侧轮胎都近似围绕一个中心点旋转。即整个小车底盘都围绕一个中心点旋转，已知小车中心的线速度（上层算法给定），此时小车底盘的运动学模型如下图：

参数说明：

$\alpha_1$是小车前左轮和后左轮的转角。

$\alpha_2$是小车前右轮和后右轮的转角。

$2L$是左右轮距离。

$2K$是前后轮距离。

$w$是小车转轴的角速度。

$v$是小车几何中心的线速度。

$v1, v2, v3, v4$是四个车轮的速度。

$i$是电机的减速比。

$r$是车轮半径。

首先可以得到各车轮速度和角速度的关系：

其中车轮沿着转动方向（$y$方向）的速度由电机提供，切向速度由地面摩擦提供，车轮沿着$y$方向的速度为：

那么电机的角速度为：

相应电机的转速（by rpm）为：

整理成矩阵表达式为：

该表达式反映了机器人关键点速度与主动轮转速之间的关系。给定小车底盘电机转速就可以求出机器人关键点的速度，并由此得到机器人上任意一点的速度（如激光雷达的安装位置的速度），上层算法给出的关键点速度控制信号也可以由此转化成电机的控制量。

里程计模型 ／ 机器人定位方法

坐标变换模型：

在一个较短的时间间隔$\Delta t$内，假定机器人左右轮的移动距离分别是$\Delta l$和$\Delta r$，那么在机器人坐标系下：机器人中心沿着机器人坐标系的$x$轴方向前进的距离为$\Delta u = (\Delta l + \Delta r)/2$，$y$轴方向前进的距离为$\Delta v = 0$，转过的角度为$\Delta \varphi = (\Delta l - \Delta r)/b$。机器人坐标系到世界坐标系的旋转变换矩阵为$R(\phi)$。

那么转换到世界坐标系下机器人的运动增量为：

世界坐标系下机器人位姿更新为：

beside from测量误差，利用坐标变换模型去推算里程计信息是引入了模型误差的——在时间间隔$\Delta t$内，为了简化计算，机器人坐标系相对世界坐标系的旋转变换矩阵被假定为起始值$R(\phi)$。在转向运动比较多的情况下，里程计信息会迅速恶化。

圆弧模型：

将极小时间间隔内小车运动的轨迹看作是一段圆弧，那么就可以确定该时刻的转动中心$C_{t-1}$，及内侧轮的转动半径为$R_{t-1}$，根据几何关系：

解得：

由三角相似得：

解得弦$D$的长度为：

弦$D$与世界坐标系$x$轴正向的夹角为$\theta = \phi - \Delta \varphi/2$，那么机器人在世界坐标系下的位姿增量为：

使用圆弧模型对里程计增量进行推算，完全依照几何关系来计算，计算过程中没有近似，能够有效控制误差累积。

概率模型：

不是单纯的基于里程计的估计，而是结合其他传感器的测量值对里程计进行矫正，详见滤波算法。

scan match模型：

同样也不是单纯的基于里程计的估计，详见karto scanMatch。与滤波的区别在于，返回的不是概率分布，而是一个代表最佳估计的值。

目前开源算法中采取的scanMatching方法主要是以下四种：

1. Gmapping：ICP（simple Gradient Descent）
2. Hector：Guass-Newton（multi-resolution map）
3. karto：Real-time CSM（multi-resolution + 三维窗口遍历寻优）
4. cartographer：Fast CSM（multi-resolution + branch and bound）

scanMatcher主要涉及两个评价函数，一个score用于优化调整粒子pose作为参考，一个likelihoodAndScore用于更新粒子权重：

粒子权重根据地图的匹配度更新：

part0

现实中想要移动平台移动到指定地点，机器人运动控制系统架构包括了如下几个层次：

最底层是机器人的底盘控制部分，驱动器接收的是机器人的期望速度（Twist），将速度解算为左右轮的期望速度，并根据期望速度对左右轮分别进行PID驱控速，输出电机的转速。

这部分ROS社区已经有针对Arduino封装好的Package——rosserial_arduino。

中间层是通信层，电脑端发布速度指令给平台，同时接收平台发布的当前速度，然后发布/odom topic，让其他节点订阅。

最上层是决策层，也就是导航规划层，goal、localization（matching&里程计）、path planner以及最终输出速度指令，这一部分都在navigation stack里面。

part1 packages

navigation stack是ROS提供的导航方案，内部集成了很多个package，模块之间完全解耦，可以个性化选择提供的方法。

官网上给出了导航栈宏观的结构描述：

move_base中主要包含三个部分，global_plan、local_plan以及recovery behavior。对应的插件有：

• global_plan：global_planner（实现了dijkstra和A*算法），carrot_planner，navfn
• local_plan：base_local_planner（实现了Trajectory Rollout和DWA算法），dwa_local_planner
• recovery：clear_costmap_recovery，move_slow_and_clear，rotate_recovery

nav_core是一个接口插件，包含了以上插件基类的头文件，move_base中的方法都是在其规则上扩展的。

两个灰色的插件map_server和amcl表示可选可不选：

• 可以使用meta package提供的map_server节点来进行代价地图管理，也可以使用其他节点（例如直接使用gmapping的输出）。
• 可以使用meta package提供的amcl节点来进行自定位，也可以使用其他算法包（例如ROS里面还有一个robot_pose_ekf节点）。

costmap_2d将不同传感器的输入处理成统一的栅格地图格式。以层的概念来组织图层，用户可以根据需要自己配置（通过Social Costmap Layer、Range Sensor Layer等开源插件），默认的层有：

• static_layer：静态地图层，（通过订阅map_server的/map主题）来生成。
• obstacle_layer：障碍地图层，根据动态的传感器信息来生成。
• inflation_layer：膨胀层，将前两个图层的信息综合进行缓冲区扩展。

voxel_grid是三维代价地图。

fake_localization用来做定位仿真，内含/base_pose_ground_truth话题。

part2 params

1. move_base_params.yaml：
   • planner_frequency：全局规划的执行频率，如果设置为0.0则全局规划器仅在接受到新目标点或者局部规划器报告路径堵塞时才会重新执行。
2. global_planner_params.yaml：
   • default_tolerance：当设置的目的地被占据时，以该参数为半径的范围内选取最近的点作为新目标点。
3. dwa_local_planner_params.yaml：
   • latch_xy_goal_tolerance：如果设置为true，达到xy_goal_tolerance以内机器人就会原地旋转，即使会转出容错圈外。
   • sim_granularity：间隔尺寸，轨迹上采样点步长。
   • scaling_speed：启动机器人底盘的速度。

4. global_costmap_params.yaml：

   • raytrace_range：实时清除代价地图上障碍物的最大范围，清除的是obstacle_layer的数据。

part3 topics

1. move_base & move_base_simple：

   1 2 3 4 5 6 7 8

   ros::NodeHandle action_nh("move_base"); action_goal_pub_ = action_nh.advertise<move_base_msgs::MoveBaseActionGoal>("goal", 1); //we'll provide a mechanism for some people to send goals as PoseStamped messages over a topic //they won't get any useful information back about its status, but this is useful for tools //like nav_view and rviz ros::NodeHandle simple_nh("move_base_simple"); goal_sub_ = simple_nh.subscribe<geometry_msgs::PoseStamped>("goal", 1, boost::bind(&MoveBase::goalCB, this, _1));

   之前查看节点图的时候发现这两个节点都提供goal，一直没想通两者的关系，发现代码注释里面有，move_base继承了actionlib，有状态反馈（详见wiki 1.1.2 ActionAPI），move_base_simple就是一个publisher（topic可以来自rviz／cmd line）。

   

   /result 记录了Goal reached

   /feedback 记录了每个时刻机器人的位姿

   /status 记录了任务进程（goal accepted、failed、aborting）

   /cancel 没echo出信息，应该与上层对接

   【定点巡航】另外，定点巡航的时候将global_path的buffer设置为n就可以显示多条路径了。

2. DWAPlanner的global_plan & local_plan：

   local_plan就是DWA算法每个时刻计算的最优预期路径。global_plan是整个局部代价地图上的路径，它是全局路径的crop，因为局部动态环境不会影响全局路径，我们只研究落在localmap以内这一段路径是否需要矫正。

   

今天发现了一个支持二次开发的开源方案，说白了就是把karto、acml、navigation stack等几个ROS开源包整合的比较漂亮，代码结构值得借鉴。

1. 执行keyboard_control之前要先执行脚本，添加键盘。

   老AS包里面没有keyboard control，可以执行teleop_twist_keyboard包。autolabor_fake是虚拟小车的driver，建模了电机、odom相关信息，订阅cmd_vel信息。控制真实小车时这个节点要替换。

2. base是对机器人底盘的仿真，launch文件的默认Fixed Frame是base_link，想要控制小车运动可以将Frame切成real_map或odom。

   todolist:

   命令行里面控制量的显示不太好看，可以尝试在源文件里面优化，添加交互提示。

   

3. stage是对场景的仿真，场景由map_server读取，launch以后就能查看当前场景地图。

   rostopic里面有一个initialpose信息，由rviz发布。rostopic list里面好多topic都是rviz发布的，在displays栏目里面取消勾选就不会发布了。

   

4. object是对障碍物的仿真，调用stage，添加interactivemarker，然后选择Interact工具，理论上地图上应该出现障碍物，但是我没找到。。。状态显示waiting for tf info。

   

   

   修正：添加的marker要在topic里面选择，不要在type栏下。然后地图上放好障碍物以后要右键apply。

   

5. lidar是对雷达点云的仿真，launch中给了一个lidar和map的静态tf，实际使用中应该给lidar和base_link的。

   todolist:

   map和real_map给的有点混乱，明天会统一一下。

6. 老AS包create_map仿真过程中，由于场景提供mapserver和slam算法同时publish了/map这个topic，要进行区分，在launch文件里面对其中一个进行remap：

   1 2 3

   <node pkg="map_server" type="map_server" name="map_server" args="$(find simulation_launch)/map/MG_map.yaml"> <remap from="/map" to="real_map" /> </node>

   

   这时rostopic里面就会出现real_map这个话题，两个地图能够同时显示。

   

代码解析

首先是simulation包：

1. autolabor_description没啥好说的，urdf文件里面定义了一个robot，整个机器人被渲染成了一个base_link，没有子节点，懒。

2. autolabor_fake包是底盘驱动，提供了一个autolabor_fake_node节点，其订阅类型为geometry_msgs/Twist的话题cmd_vel，信息来源可以是joystick／keyboard（tele_op_xxx）／cmd line（rostopic pub xxx）。其发布类型为nav_msgs/Odometry的话题odom。同时该节点还会将odom frame到base_link frame的transform信息提供给tf node，用来tf_broadcast。

   

3. lidar_simulation包自身提供了两个节点lidar_simulation和obstacle_simulation。

   3.1 LidarSimulation::getPose这个函数中有一段代码开始比较困惑：

   1 2 3 4 5 6 7

   // ROS_INFO("roll and pitch and yaw and esp :%lf %lf %lf %lf", roll, pitch, yaw, esp); //esp=0.000001,r&p=0.000000 if (pow(roll,2) + pow(pitch,2) > esp){ start_angle = yaw + max_angle_; reverse = -1.0; }else{ // default situation: start_angle = yaw + min_angle_; reverse = 1.0;

   通常情况下，我们都使用右手坐标系，二维平面下，global_frame_到lidar_frame_的坐标变换transform欧拉角形式下的r和p角应该始终是$0.0$，yaw代表了激光雷达$x$轴的变换，加上min_angle_就切换成了激光光束的初始发射角度start_angle。如果坐标系定义反了，r&p就应该有值，这时因为坐标轴定义反过来了，激光光束的初始发射角度就变成了从正方向上的max_angle_开始的。

   3.2 LidarSimulation::updateMap这个函数值得注意，这是一个service client，用来调用地图更新，在当前功能包的默认launch文件中，只加载了一次地图，没有体现出它的作用。当执行建图任务时，因为map frame和odom frame会不断进行矫正，建图包就会call这个request来实时更新地图。

   

   3.3 该功能包下还自定义了一个obstacle service，提供obstacle_simulation节点来更新障碍物信息。这里的障碍物是指手动添加的障碍物（interactiveMarker），launch文件中可以定义其形状顶点。

   ​ 其中的ObstacleSimulation::pnpoly函数用来判断某点是否落在多边形内，之前刷算法时有考虑过这个问题，这里给出的解法不知道是不是最优的，just for record：

   1 2 3 4 5 6 7 8 9 10 11

   bool ObstacleSimulation::pnpoly(geometry_msgs::Polygon& footprint, float& x, float& y){ int i,j; bool c = false; for (i=0, j=footprint.points.size()-1; i<footprint.points.size(); j = i++){ if ( ( (footprint.points.at(i).y > y) != (footprint.points.at(j).y > y) ) && (x < (footprint.points.at(j).x-footprint.points.at(i).x) * (y-footprint.points.at(i).y) / (footprint.points.at(j).y - footprint.points.at(i).y) + footprint.points.at(i).x) ){ c = !c; } } return c; }

   这里面的for，循环条件遍历的是多边形的每一条边，如$(3,0), (0,1), (1,2), (2, 3)$这样。判定的是给定点是否落在给定边的内侧，这里所谓的内侧是以给定边的起始节点为原点，给定线段的顺时针方向。

   3.4 obstacle的具体操作定义在static_map中，这里面出现了世界坐标系World，值得注意的是，栅格地图的原点在地图的一角，栅格的位置用整型来表示，而世界坐标系中栅格的位置由其中心来表示，两者相差$0.5$个resolution。lidar_simulation里面创建了一个static_map对象map_，以及回调函数LidarSimulation::obstacleHandleServer。

   3.5 lidar_simulation功能包中的这两个节点：lidar_simulation是map级的，obstacle_simulation是obstacle级的。

接下来看simulation_launch包：

这个包里面没有源代码，只提供了几个launch文件，用来仿真几种不同的情况：

• sim_move_simulation.launch就是简单的底盘控制，控制小车在给定地图上运动，同时可视化雷达点云信息。
• create_map_simulation.launch用来建图，在底盘控制的基础上，启动了建图功能包。发布默认名字为/map的topic，命令行执行map_saver保存。
• move_base_simulation.launch用来导航，在底盘控制的基础上，启动了导航套件acml&move_base，这时小车的底盘控制节点autolabor_fake_node订阅的cmd_vel信息不再来自teleop_keyboard，而是来自move_base的规划结果。

最后是move_base_sim这个功能包：

是用作真实底盘控制的（目测就是对ROS开源的move_base包的二次封装，貌似删了一些不用的插件），先skip，接下来我会直接解析ROS导航套件。

Package Control的配置文件中添加：

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

"downloader_precedence":
{
	"linux":
	[
		"curl",
		"urllib",
		"wget"
	],
	"osx":
	[
		"curl",
		"urllib"
	],
	"windows":
	[
		"wininet"
	]
},

OSX和ubuntu下亲测均有效。