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

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

1
2

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

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

1
2

chmod 777 /dev/ttyS0
chmod 777 /dev/ttyS1

停止开机启动：rosrun robot_upstart uninstall nav，会删除除了/tmp里几个文件的其他所有文件

问题

在小强机器人上对startup.launch开机启动，在另一台电脑上执行rosnode list和rostopic 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

脚本出错

编辑/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

权限和服务

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

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

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

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

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

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

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

service是阻塞的，action是非阻塞的

通信机制

ROS的Action机制本质上仍然是一种Topic机制，只是通过actionlib进行了封装。ActionClient 和 ActionServer 通过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的函数只有send和cancel。

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

对于多个goal的处理策略

高层的client和server只考虑goal的处理情况，所以叫SimpleActionServer和SimpleActionClient

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学习（三）

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_descriptions和parameter_updates。以parameter_descriptions 和 parameter_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

参数~<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();

AMCL定位偶尔会出现定位不准的情况，推测是由于工控机性能不足(粒子滤波很耗时)或地图上大特征较少/小特征较多(其总体结果有随机分布倾向)，造成AMCL无法及时定位

AMCL中，粒子很分散，定位置信度就很低。如果置信度太低，定位不准，可以停掉导航模块，先通知上层进行重定位。

amcl用到的话题和服务

话题

机器人稍微运动后，amcl节点开始读取scan,map,tf话题上的数据， 在重采样之后发布话题 amcl_pose， 平时小车不发布位姿坐标的，而重采样的触发条件是里程计显示小车参数update_min_d或者旋转参数update_min_a。我平时获取小车位姿用的是话题robot_pose，它来自robot_pose_publisher包，发布频率是10Hz

rviz中的particlecloud话题:

消息类型是geometry_msgs/PoseArray，由/amcl发布，被rviz订阅。作用是在rviz中显示机器人的可能位置，就是这样的一坨:

amcl还订阅了一个话题initialpose，但是我发现它没有发布者，看来是供用户发布的。

服务

服务global_localization，类型为std_srvs/Empty，初始化全局定位，使所有粒子在地图的自由空间上随机分散，也就是像这样：

call这个服务其实就相当于重新初始化了粒子位置，用于amcl的重新定位，但是我尝试移动机器人，定位经常是错误的。

服务static_map，类型nav_msgs/GetMap，它是由map_server发布的，用于获得地图数据进行定位。所以说amcl离不开map_server，其消息成员：

1
2

---
nav_msgs/OccupancyGrid map

request_nomotion_update服务，该服务是手动的来更新粒子并发布新的粒子，可以使用如下命令来调用服务执行更新粒子的操作，该服务一般需要多次调用才能逐渐看到粒子收敛的效果：

1

rosservice call request_nomotion_update "{}"

机器人准确定位

机器人定位需要的信息:

• 雷达扫描数据
• 里程计数据
• 扫描地图数据(不是静态地图)
• odom->base_link->laser的tf变换

初始点位姿参数有6个，均在handleMapMessage函数中使用，最终是在 pf_pdf_gaussian_sample函数

每次电脑重启后，在同一个点的坐标是不同的，每次都要手动定位。可以在amcl.launch中设置机器人初始化数据。

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

<!--初始位姿均值（x），用于初始化高斯分布滤波器。0.0 -->
<param name="initial_pose_x" value="-3.13841"/> 

<!--初始位姿均值（y），用于初始化高斯分布滤波器。 0.0-->
<param name="initial_pose_y" value="2.20967"/> 

<!--初始位姿均值（yaw），用于初始化高斯分布滤波器。 -->
<param name="initial_pose_a" value="2.611510"/> 

<!--初始位姿协方差（x*x），用于初始化高斯分布滤波器。 -->
<param name="initial_cov_xx" value="0.5*0.5"/> 

<!--初始位姿协方差（y*y），用于初始化高斯分布滤波器。 -->
<param name="initial_cov_yy" value="0.5*0.5"/> 

<!--初始位姿协方差（yaw*yaw），撒粒子的方差，使粒子均匀的围绕初始化点,用于初始化高斯分布滤波器  -->
<param name="initial_cov_aa" value="(π/12)*(π/12)"/> 

covariance是一个6x6的协方差矩阵，6个状态量是x,y,z,绕x轴的旋转(roll),绕y轴的旋转(pitch),绕z轴的旋转(yaw)。矩阵对角线的6个元素就是6个状态量的方差，由于z,pitch,roll都是0，所以只看第1,2和最后的元素，也就是矩阵的第1,8和最后的元素，分别是x,y,yaw的方差

在pf_pdf_gaussian_alloc函数中，对协方差矩阵进行了特征值分解。也就是协方差矩阵cx=cr cd cr^T ，cd是对角矩阵，取cd的三个对角元素开平方后，传入pf_ran_gaussian函数，作为set_a粒子集的初始化的标准差。yaw的方差在对角矩阵cd里不变，cd的最后一个元素仍然是initial_cov_aa

主要是前三项参数，在建好地图后，选择一个启动点的坐标，将朝向转换获得yaw，以后每次启动机器人时，都要让机器人在这个位姿。因为每次启动move_base.launch后，只调整机器人位姿无法定位准确，还需要rviz手动定位，有了这个初始位姿之后，不必rviz手动定位了，只调整机器人位姿就可以了，调整方式是先让机器人到某个点。

在机器人移动时，才会发布/amcl_pose的消息，类型为geometry_msgs/PoseWithCovarianceStamped，也就是带协方差的消息。robot_pose_publisher发布了/base_link坐标系和/map坐标系的转换， 一般放在机器人开机启动的节点里，发布话题robot_pose(geometry_msgs/Pose)。常用这两个话题获取机器人当前位置

在远离初始点的位置,执行rosparam get amcl/initial_pose_x，发现它的值实际是当前的位姿x坐标， 而不是amcl.launch中的参数了，显然是参数服务器在更新这个参数

amcl参数

蓝色的是需要自己修改的

滤波器参数

handleMapMessage函数中的pf_alloc所用：

• min_particles和max_particles 是允许的粒子数量的最小值和最大值，也就是在rviz里看到的箭头数量。分别对应程序里的变量min_samples和max_samples。前者默认100，后者默认5000。原则上越大，定位精度越好，但会影响占用的CPU

AMCL收敛速度不够快，在初值比较好的情况下，如果设置min_particles为300，max_particles为5000，重采样需要14~20次才能收敛到300个粒子。如果设置得更小，收敛速度就更慢。 重采样几次时，有的粒子权重还是另一些粒子权重的三四倍。

min_particles不能太大，应该比pf_resample_limit函数返回值小，否则set_b偏大，位姿不够精确。 我见过pf_resample_limit最小返回值是66，一般都大于100，所以min_particles取100即可。

max_particles不能太小，一方面难以筛选权重大的粒子，另一方面在初值不太准确时，粒子集不能有效覆盖真实初值的的周边。但也不能太大，否则占用CPU上升。经过反复试验，max_particles取3000，此时AMCL占用CPU 2%左右

• kld_err: 初始粒子真实分布和估计分布之间的最大误差，默认0.01

• kld_z: 上标准分位数（1-p），其中p是估计分布上误差小于kld_err的概率，默认0.99，但实际在pf_alloc为3

• recovery_alpha_slow: 慢速的平均权重滤波的指数衰减频率，用作决定什么时候通过增加随机位姿来recover，默认0（disable），可能0.001是一个不错的值

• recovery_alpha_fast: 快速的平均权重滤波的指数衰减频率，用作决定什么时候通过增加随机位姿来recover，默认0（disable），可能0.1是个不错的值

这两个参数在AMCL失效恢复中，用于短期和长期的似然平均，要求 0 < alpha_slow << alpha_fast，用于计算w_slow和w_fast，最终判断是否触发recover机制，增加随机位姿， 用以削弱粒子滤波器的粒子匮乏问题的影响。

运动模型准备工作中所用：

• update_min_d : 在执行滤波更新前平移运动的最小距离，默认0.2m，可改为0.05
• update_min_a: 执行滤波更新前旋转的最小角度，单位弧度，默认 pi/6
  
  

重采样所用：

• resample_interval : 在重采样前需要等待的滤波次数，只能是大于1的整数，默认2。越大，重采样频率越低

源码里对应：

1
2
3
4
5

if(  !(++resample_count_ % resample_interval_)  )
{
  pf_update_resample(pf_);
  resampled = true;
}

如何接受地图数据：

• use_map_topic: 导航时设置为false。 当设置为true时，AMCL将会订阅map话题，而不是调用服务返回地图。也就是说，当设置为true时，有另外一个节点实时的发布map话题，也就是机器人在实时的进行地图构建，并供给amcl话题使用；当设置为false时，通过map server，也就是调用已经构建完成的地图

• first_map_only: 用于mapReceived函数，可以不看。当设置为true时，AMCL将仅仅使用订阅的第一个地图，而不是每次接收到新的时更新为一个新的地图，导航时设置为false

• transform_tolerance: tf变换发布推迟的时间，为了说明tf变换在未来时间内是可用的，默认0.1

• gui_publish_rate: 扫描和路径发布到可视化软件的最大频率，默认参数-1.0表示disable，可以设置为10
• save_pose_rate: 存储上一次估计的位姿和协方差到参数服务器的最大速率。被保存的位姿将会用在连续的运动上来初始化滤波器。默认参数-1.0表示disable，可以用0.5

激光模型参数，都在测量模型中使用

• laser_model_type: 模型使用，可以是beam, likehood_field, likehood_field_prob（和likehood_field一样但是融合了beamskip特征），默认是 likehood_field

• laser_min_range: 被考虑的最小扫描范围；默认参数-1.0，将会使用激光上报的最小扫描范围

• laser_max_range: 被考虑的最大扫描范围；默认参数-1.0时，将会使用激光上报的最大扫描范围

• laser_max_beams: 更新滤波器时，每次扫描中多少个等间距的光束被使用，默认30

• laser_z_hit: 似然域模型的高斯噪声的最大权值，默认0.95

• laser_z_rand: 似然域模型的随机噪声的最大权值，默认0.05
• laser_sigma_hit: 高斯噪声模型的标准差，默认0.2m

以下几个参数不在似然域模型中使用：

• laser_z_short: 模型的z_short部分的最大权值，默认0.1
• laser_z_max: 模型的z_max部分的最大权值，默认0.05
• laser_lambda_short: 模型z_short部分的指数衰减参数，默认0.1
• laser_likehood_max_dist: 地图上做障碍物膨胀的最大距离，用作likehood_field模型，默认2m

里程计模型参数，都在运动模型部分使用

• odom_model_type: 模型使用，默认diff，还可以是omni(全向轮), diff-corrected, omni-corrected，后面两个是对老版本里程计模型的矫正，相应的里程计参数需要做一定的减小。一般用diff
  
  
• odom_alpha1 : 指定由机器人运动部分的旋转分量估计的里程计旋转的期望噪声，默认0.2。增大该值，机器人发生有旋转运动时，就会出现扇形噪声粒子云
  
• odom_alpha2: 指定由机器人运动部分的平移分量估计的里程计旋转的期望噪声，默认0.2
  
• odom_alpha3: 指定由机器人运动部分的平移分量估计的里程计平移的期望噪声，默认0.2。纵向分量运动噪声，沿着机器人前进方向分布
  
• odom_alpha4: 指定由机器人运动部分的旋转分量估计的里程计平移的期望噪声，默认0.2。斜角方向上的运动噪声
  

• odom_alpha5: 平移相关的噪声参数（仅用于模型是omni的情况），默认0.1

• odom_frame_id: 里程计默认使用的坐标系，默认odom

• base_frame_id: 机器人的基坐标系，默认base_link
• global_frame_id: 由定位系统发布的坐标系名称，默认map
• tf_broadcast: 设置为false会阻止amcl发布全局坐标系和里程计坐标系之间的tf变换，一般为true

参数：geometry_msgs/PoseWithCovariance

地图

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

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

通用代价地图

参数文件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米内的障碍物，本来开始时是有的，但是本次检测却没有了，那么就需要在代价地图上来更新，将旧障碍物的空间标记为可以自由移动的空间

全局代价地图

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

相应的参数文件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在刚启动时的打印结果

发导航命令后，代价地图的大小很奇怪地变为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的源码

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

使用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)

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

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

比如现在有sub这个包，编译之后会在devel/share/sub/cmake生成两个文件：pubConfig.cmake和pubConfig-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")

报错Policy CMP0046 is not set

可以不用管，也可以按提示修改，最好先安装：sudo apt-get install libproj-dev

参考：Policy CMP0046问题

Memcheck

默认的工具，用来检测程序中出现的内存问题，所有对内存的读写都会被检测到，一切对malloc、free、new、delete的调用都会被捕获。所以，它能检测以下问题：

1. 对未初始化内存的使用；

2. 读/写释放后的内存块；

3. 读/写超出malloc分配的内存块；

4. 读/写不适当的栈中内存块；

5. 内存泄漏，指向一块内存的指针永远丢失；

6. 不正确的malloc/free或new/delete匹配；

7. memcpy()相关函数中的dst和src指针重叠

valgrind只报告内存泄漏，但没有显示具体代码中泄漏的地方。因此需要使用--leak-check=full选项启动valgrind。你的程序将比正常的运行慢得多（如20〜30倍），并使用更多的内存。Memcheck会报告关于内存错误，和检测到的泄漏的消息。

Memcheck也报告未初始化值的使用，最常见的消息是Conditional jump or move depends on uninitialised value(s)。可以难以确定这些错误的根源。尝试使用--track-origins=yes来获得额外的信息。这使得Memcheck运行得更慢，

Invalid write of size 1 //堆内存越界被查出来

Invalid free() / delete / delete[] //重复释放

Process terminating with default action of signal 11 (SIGSEGV): dumping core //非法指针，导致coredump

massif

堆栈分析器，指示程序中使用了多少堆内存等信息。它能测量程序在堆栈中使用了多少内存，告诉我们堆块，堆管理块和栈的大小。Massif能帮助我们减少内存的使用，在带有虚拟内存的现代系统中，它还能够加速我们程序的运行，减少程序停留在交换区中的几率。

Massif对内存的分配和释放做profile。程序开发者通过它可以深入了解程序的内存使用行为，从而对内存使用进行优化。这个功能对C++尤其有用，因为C++有很多隐藏的内存分配和释放

memcheck的常用选项

—tool= [default: memcheck]

最常用的选项。运行valgrind中名为toolname的工具。如果省略工具名，默认运行memcheck。

—leak-check= [default: summary]

用于控制内存泄漏检测力度。

• no，不检测内存泄漏；

• summary，仅报告总共泄漏的数量，不报告具体泄漏位置；

• yes/full，报告泄漏总数、泄漏的具体位置

—show-reachable= [default: no]

用于控制是否检测控制范围之外的泄漏，比如全局指针、static指针等。

—undef-value-errors= [default: yes]

用于控制是否检测代码中使用未初始化变量的情况。

—log-file=filename # 将结果输出到文件，支持相对路径

—track-fds= [default: no] # 跟踪打开的文件描述符

五种泄露内存的类型

• definitely lost：确认丢失。程序中存在内存泄露，应尽快修复。当程序结束时如果一块动态分配的内存没有被释放且通过程序内的指针变量均无法访问这块内存则会报这个错误。

• indirectly lost：间接丢失。当使用了含有指针成员的类或结构时可能会报这个错误。这类错误无需直接修复，他们总是与”definitely lost”一起出现，只要修复”definitely lost”即可。例子可参考我的例程。

• possibly lost：可能丢失。大多数情况下应视为与”definitely lost”一样需要尽快修复，除非你的程序让一个指针指向一块动态分配的内存（但不是这块内存起始地址），然后通过运算得到这块内存起始地址，再释放它。例子可参考我的例程。当程序结束时如果一块动态分配的内存没有被释放且通过程序内的指针变量均无法访问这块内存的起始地址，但可以访问其中的某一部分数据，则会报这个错误。

• still reachable：可以访问，未丢失但也未释放。如果程序是正常结束的，那么它可能不会造成程序崩溃，但长时间运行有可能耗尽系统资源，因此笔者建议修复它。如果程序是崩溃（如访问非法的地址而崩溃）而非正常结束的，则应当暂时忽略它，先修复导致程序崩溃的错误，然后重新检测。

• suppressed：已被解决。出现了内存泄露但系统自动处理了。可以无视这类错误。这类错误我没能用例程触发，看官方的解释也不太清楚是操作系统处理的还是valgrind，也没有遇到过。

操作MySQL结果集后释放内存

代码中获取MySQL结果常用由mysql_store_result，结果用valgrind检查时发现了内存泄露问题：

由mysql_store_result()、mysql_use_result()、mysql_list_dbs()获得结果集，在完成对结果集的操作后，必须调用mysql_free_result()释放结果集使用的内存。每次查询返回的结果的地址是不一样的，所以每次都要释放，否则会造成内存泄露。

1
2
3

mysql_free_result(result);
mysql_close(conn);
mysql_library_end();