8.在STDR中进行定点巡航仿真
corvin_zhang
0x00 为何需要定点巡航
现在我们已经可以在已知地图中进行自动导航了，那就可以利用这个功能来解决一些实际生活中的需求了。下面就是我思考的几个实际场景中可能用到定点巡航的功能：在工厂中经常会遇到需要运送物料的要求，一般都是从固定地点将东西运送到固定的指定地点，而且可能会在多点之间不断的运送。如果使用人工的话，效率太低而且人也会累。此时，我们就可以使用现在这个定点巡航功能，可以让移动机器人来充当一个工厂车间内的“公交车”。这样就只需将运送的物料放在机器人上，就可以在多点之间不断运送，大大减少了人工成本。
还有就是现在的零食饮料贩卖机都是固定在一个地点，顾客如果想要买饮料的话就需要找到饮料贩卖机。但是现在我们就可以利用机器人的定点导航功能来实现，让贩卖机来不断的巡航。就跟以前的那些货郎一样，沿街叫卖。这样就可以节省顾客的时间，就跟我们在火车上遇到的贩卖车一样。不过火车上那个是列车员推着小车走，我们可以做成小车自动走，有人买东西只要打声招呼（结合语音系统的唤醒词来实现），小车就可以停下来。等顾客扫码购买完东西后，小车仍然按照既定的目标点来不断巡航。
还有就是安防需求了，在一些敏感场所中，我们经常需要派人不断的巡逻。防止有坏人来破坏，如果派人来不断巡逻的话，人总会有需要休息的时间，这样就会给坏人以可乘之际。利用休息时间或者巡逻人员的换班时间进入场所中实施破坏或偷盗。如果我们派机器人来巡逻的话就可以节省很多人员精力了，机器人可以24小时不间断巡逻。利用图像处理功能来识别陌生人或麦克风阵列来识别异常声音，从而可以进行报警提醒安保人员。
0x01 创建软件包
我们可以直接在catkin_ws工作空间的src目录下创建stdr_navigation测试软件包，具体操作如下图所示：
在launch文件夹中创建patrol_nav.launch启动文件，该文件的具体代码如下：
<!-- Copyright: 2016-2018: www.corvin.cn Author: corvin Description: 启动move_base和amcl来进行自动导航,然后启动patrol_nav_node. History: 20180806: initial this file. 20180807: add patrol_time and random_patrol parameters. --> <launch> <!-- startup move_base node --> <include file="$(find stdr_move_base)/launch/stdr_move_base.launch" /> <!-- startup amcl node --> <include file="$(find stdr_amcl)/launch/stdr_amcl.launch" /> <!-- startup patrol navigation node --> <node pkg="stdr_navigation" type="patrol_nav.py" name="patrol_nav_node" output="screen" > <param name="rest_time" value="5" /> <param name="keep_patrol" value="true" /> <param name="random_patrol" value="false" /> <param name="patrol_type" value="0" /> <param name="patrol_loop" value="1" /> <param name="patrol_time" value="3" /> </node> </launch>
下面开始对该launch文件进行简要说明：
- <include file="$(find stdr_move_base)/launch/stdr_move_base.launch" />
这里我们直接调用的是上面的教程中配置的move_base启动文件，这里我们就不再重新配置该launch文件，而是直接调用以前有的。所以建议大家可以先将前面的教程练习一遍再来学习这篇教程。
- <include file="$(find stdr_amcl)/launch/stdr_amcl.launch" />
这里我们直接调用启动amcl的launch文件，我们这里的重点是发布巡航目标点的python节点，因此这里也不再配置amcl，而是直接调用以前有的。
- <node pkg="stdr_navigation" type="patrol_nav.py" name="patrol_nav_node" output="screen" >
这将要启动patrol_nav_node，该节点代码是由python来编写实现的。如果没有该节点的话，只有上面两行代码。那就需要我们自己在RViz中使用"2D Nav Goal“来指定导航的目标点了，该节点的主要作用就是向move_base中发布我们事先在地图上规定好的坐标点，它可以配置若干个参数，下面来详细介绍各参数意义：
(1)rest_time:到达一个目标点后,需要等待的秒数,然后再前往下一个目标点。
(2)keep_patrol:是否巡逻完一圈后不断的来巡逻,一直不停止.当设置了此参数为true后,那么下面的巡逻类型参数就会无效,因为将不断的巡逻,没有圈数和时间的限制了.而只能设置random_patrol,是否启用随机目标点巡逻。
(3)random_patrol:是否启用随机目标点巡逻,而不是按照顺序点来巡逻.该参数只有在keep_patrol无限巡逻设置为true后设置该参数才有效.如果在patrol_loop和patrol_time时只能按照顺序点来导航。
(4)patrol_type:巡逻的类型,当不是不断的巡逻的话,就需要设置使用哪种巡逻方式了.这些参数具备互斥性,设置为0后,那么设置的patrol_time参数就会无效,如果设置为1,那么设置的patrol_loop参数就会无效:
0:使用巡逻圈数patrol_loop,当巡逻指定的圈数后停止;
1:使用巡逻时间patrol_time,当巡逻指定的时间后返回起始点停止;
(5)patrol_loop:当keep_patrol设置为false后,可以来设置该参数,设置巡航的圈数.即按照顺序将所有目标点巡逻一遍就是走了一圈。
(6)patrol_time:配置巡逻的时间,当检测到巡逻时间到达指定时间后,就返回到起始点.设置的巡逻时间单位是分钟。
0x02 编写发送导航目标点的代码
接下来开始介绍patrol_nav.py的代码，该源码需要放到scripts文件夹中，它是完成定点巡航非常重要的源码，具体代码如下：
#!/usr/bin/env python
# -*- coding: UTF-8 -*-
################################################
# Copyright(c): 2016-2018 www.corvin.cn
################################################
# Author: corvin
################################################
# Description:
# 在地图上指定6个坐标点作为巡逻点,可以在这些点
# 之间进行不断的巡逻,也可以指定巡逻的圈数,当到
# 达指定的圈数后就会停止运行.该patrol_nav_node,
# 是通过向MoveBaseGoal的target_pose中发布目标
# 位姿来达到巡航的目的,根据move_base的action状态
# 来判断机器人是否到达了目标位置.当到达了目标位置
# 后取出下一个目标位姿进行导航.直到字典存储的所有
# 目标位姿都到达后,就认为巡航一圈结束了.
################################################
# History:
# 20180806: initial this file.
################################################
import rospy
import random
import actionlib
from actionlib_msgs.msg import *
from geometry_msgs.msg import Pose, Point, Quaternion
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
class PatrolNav():
def __init__(self):
rospy.init_node('patrol_nav_node', anonymous=False)
rospy.on_shutdown(self.shutdown)
# From launch file get parameters
self.rest_time = rospy.get_param("~rest_time", 5)
self.keep_patrol = rospy.get_param("~keep_patrol", False)
self.random_patrol = rospy.get_param("~random_patrol", False)
self.patrol_type = rospy.get_param("~patrol_type", 0)
self.patrol_loop = rospy.get_param("~patrol_loop", 2)
self.patrol_time = rospy.get_param("~patrol_time", 5)
#set all navigation target pose
self.locations = dict()
self.locations['one'] = Pose(Point(13.2, 1.590, 0.000), Quaternion(0.000, 0.000, 0.006, 0.999))
self.locations['two'] = Pose(Point(13.8, 12.33, 0.000), Quaternion(0.000, 0.000, 0.99, 0.038))
self.locations['three'] = Pose(Point(5.53, 12.48, 0.000), Quaternion(0.000, 0.000, 0.704, 0.7097))
self.locations['four'] = Pose(Point(2.559, 9.689, 0.000), Quaternion(0.000, 0.000, 0.999, 0.012))
self.locations['five'] = Pose(Point(6.886, 1.490, 0.000), Quaternion(0.000, 0.000, 0.823, 0.567))
self.locations['six'] = Pose(Point(1.765, 2.084, 0.000), Quaternion(0.000, 0.000, -0.005, 0.999))
# Goal state return values
goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED',
'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST']
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
self.move_base.wait_for_server(rospy.Duration(30))
rospy.loginfo("Connected to move base server")
# Variables to keep track of success rate, running time etc.
loop_cnt = 0
n_goals = 0
n_successes = 0
target_num = 0
running_time = 0
location = ""
start_time = rospy.Time.now()
locations_cnt = len(self.locations)
sequeue = ['one', 'two', 'three', 'four', 'five', 'six']
rospy.loginfo("Starting position navigation ")
# Begin the main loop and run through a sequence of locations
while not rospy.is_shutdown():
#judge if set keep_patrol is true
if self.keep_patrol == False:
if self.patrol_type == 0: #use patrol_loop
if target_num == locations_cnt :
if loop_cnt < self.patrol_loop-1:
target_num = 0
loop_cnt += 1
rospy.logwarn("Left patrol loop cnt: %d", self.patrol_loop-loop_cnt)
else:
rospy.logwarn("Now patrol loop over, exit...")
rospy.signal_shutdown('Quit')
break
else:
if self.random_patrol == False:
if target_num == locations_cnt:
target_num = 0
else:
target_num = random.randint(0, locations_cnt-1)
# Get the next location in the current sequence
location = sequeue[target_num]
rospy.loginfo("Going to: " + str(location))
self.send_goal(location)
# Increment the counters
target_num += 1
n_goals += 1
# Allow 5 minutes to get there
finished_within_time = self.move_base.wait_for_result(rospy.Duration(300))
# Check for success or failure
if not finished_within_time:
self.move_base.cancel_goal()
rospy.logerr("ERROR:Timed out achieving goal")
else:
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
n_successes += 1
rospy.loginfo("Goal succeeded!")
else:
rospy.logerr("Goal failed with error code:"+str(goal_states[state]))
# How long have we been running?
running_time = rospy.Time.now() - start_time
running_time = running_time.secs/60.0
# Print a summary success/failure and time elapsed
rospy.loginfo("Success so far: " + str(n_successes) + "/" +
str(n_goals) + " = " +
str(100 * n_successes/n_goals) + "%")
rospy.loginfo("Running time: " + str(self.trunc(running_time, 1)) + " min")
rospy.sleep(self.rest_time)
if self.keep_patrol == False and self.patrol_type == 1: #use patrol_time
if running_time >= self.patrol_time:
rospy.logwarn("Now reach patrol_time, back to original position...")
self.send_goal('six')
rospy.signal_shutdown('Quit')
def send_goal(self, locate):
# Set up the next goal location
self.goal = MoveBaseGoal()
self.goal.target_pose.pose = self.locations[locate]
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now()
self.move_base.send_goal(self.goal) #send goal to move_base
def trunc(self, f, n):
# Truncates/pads a float f to n decimal places without rounding
slen = len('%.*f' % (n, f))
return float(str(f)[:slen])
def shutdown(self):
rospy.logwarn("Stopping the patrol...")
if __name__ == '__main__':
try:
PatrolNav()
rospy.spin()
except rospy.ROSInterruptException:
rospy.logwarn("patrol navigation exception finished.")
下面来对该代码进行简要解析，首先来看一下该代码的整体结构，如下图所示：
下面来依次介绍类中各函数的代码，首先来看__init__函数：
- 第1部分代码的第一行就是为了初始化该节点，并将该节点命名为patrol_nav_node。第二行是设置节点在退出时调用的函数，这里我们设置的函数名为shutdown。
- 第2部分的代码是为了从launch文件中获取到配置的参数值，在rospy.get_param("~rest_time", 5)中的5是该参数的默认值，意思是说如果在launch文件中若没有配置rest_time这个参数，那么self.rest_time将使用默认值5。
既然这里说到了shutdown函数，那就顺便看一下该函数：
def shutdown(self):
rospy.logwarn("Stopping the patrol...")
可以看到该函数只有一行调试打印信息，其实在该函数中可以增加很多功能，例如一些节点退出运行前的一些数据保存工作等。我这里没有那么复杂的功能，因此就懒着写那么多代码了。
- 第1部分代码就是我们想要在地图上巡逻的目标点，其中还包括了最终的朝向。这6个坐标点在地图上的位置如下，这是我自己随便定义的点，大家可以根据需要来自己修改：
对于这些坐标点的获取，我们在以前的教程中介绍过。我们在启动了move_base节点后就可以使用Rviz中的"2D Nav Goal"功能来获取到地图上坐标点的坐标和朝向，具体的命令如下：
rostopic echo /move_base_simple/goal
使用该命令我们就可以得到各坐标点的位姿信息了，如下图所示：
上图中黄色框中的就是我们最终需要的各目标点信息，其中position的x，y，z就是代码中Point（x,y,z）三个参数。orientation的x,y,z,w就是代码中Quaternion(x,yz,z,w)的四个参数。
- 第2部分的goal_states中存储的就是move_base在导航过程中反馈的各种状态，我们就是根据这些反馈来获取机器人向目标点导航时的状态。只有获取到SUCCEEDED状态才算正确到达了一个指定的目标点。
- 第1部分代码中，我们初始化了一个SimpleActionClient。这样我们就可以获取到move_base在导航过程中的状态反馈，根据反馈的状态我们来决定何时发送下一个目标点的坐标。
- 第2部分代码就是初始化一些我们代码中用到的一些变量。
在上图中向move_base发送导航坐标点的函数为self.send_goal，那么下面就来看下该函数是如何实现的：
在该函数中，我们初始化了一个MoveBaseGoal()对象，然后为该对象的成员函数target_pose各参数赋值，pose是导航的坐标点。header中设置了全局坐标系名称和当前时间戳。最后，使用move_base的send_goal函数将该对象中设置的各参数发送给global_planner开始进行路径规划。
- 第1部分代码是判断若到达目标点时超时了，那么就取消这个目标点的导航任务，准备开始前往下一个目标点。
- 第2部分代码是获取move_base导航时的任务状态，如果反馈的状态是SUCCEEDED，那么就认为本次导航任务成功，成功导航次数计数n_successes加1。若状态反馈不是SUCCEEDED那么就打印出反馈的状态码。
- 第1部分代码是打印一些导航的任务信息，包括：导航成功次数/总导航次数 = 导航成功率%，巡航任务执行的总时间。这里使用了trunc函数，为了就是截取小数点后面一位小数。最后的rospy.sleep()就是为了在到达一个目标点后休息几秒钟。
- 第2部分代码是判断是否在使用巡航时间模式，直接判断running_time运行时间是否超过了指定的时间限制。如果达到了巡航的时间，那么就直接向move_base发送初始位置的坐标点。使机器人回到初始位置后，就退出停止节点。
最后就来说一个这个trunc函数，其实就是为了截短一下字符串的长度，代码如下：
0x03 自动巡航效果演示
在进行演示自动巡航前，我们需要先明白在自动巡航是设置的参数的逻辑图：
由上图可知，我们其实最终可以仿真的巡航模式总共有4种。分别如下：
- 随机坐标点的无限巡航模式:随机坐标点意思是我们不是按照我们规定的one->two->three->four->five->six这样顺序的方式来依次导航的，而是随机从6个坐标点中取一个位置来导航。
- 顺序坐标点的无限巡航模式:对于顺序坐标点的理解我们已经知道了，这个无限巡航就是说我们巡航任务不会停止。会一直在各坐标点间不断的导航，除非patrol_nav_node节点退出。
- 巡航圈数模式:我们从one->two->three->four->five->six走完一遍所有的坐标点就认为巡航完一圈了，到达我们指定的圈数后就停止巡航任务。
- 巡航时间模式:在从one->two->three->four->five->six巡航过程中，我们一直都在计算当前导航任务的执行时间，当到达一个坐标点后就开始判断现在的任务执行时间是否已经到达或超过了指定的巡航时间，如果没有那就继续前往下一个坐标点。如果已经达到了，那就退出巡航模型。
接下来就演示一下顺序坐标点的无限巡航模式的效果，就是使用默认的launch参数，启动命令如下：
roslaunch stdr_navigation patrol_nav.launch
具体的演示效果，如下动图所示：
在自动巡航过程中，当到达一个目标点后。我们就可以在终端中看到如下统计信息：
下面再可以来看看随机坐标点的无限巡航模式，此时就需要修改patrol_nav.launch文件中的random_patrol参数为true才行，效果如下动图所示：
0x04 Run-In Test
当定点巡航功能可以正常运行起来后，我们就可以来进行运转测试了。也就是简单测试一下，该定点巡航系统是否可以长时间的正常稳定工作。我让整个定点巡航程序运行了两个半小时，运行日志如下截图所示：
根据上图可知，运行定点导航任务的总时间是152分钟。我们总共发送了166个导航目标点，成功移动到目标点的次数为116次。最终计算出定点导航的成功率大概为69%，由此可见，该定点巡航系统还不稳定。
经过我的观察，发现大部分的导航任务的失败都是因为在运行过程中，机器人的定位系统突然出现了偏差。就是本来机器人的定位是准的，但是不知道什么原因，就突然把它定位到了其他位置上。这样就会在错误的定位上重新开始路径规划，这样就无法正确的控制机器人移动到目标点去了。所以，如果要优化该定点巡航功能的话，我们首先需要优化的地方就是AMCL。
对于如何来优化AMCL，我们目前简单的做法就是调整AMCL节点的参数。复杂的做法就是我们需要自己来修改AMCL的源码了，或者实现自己的定位算法。在真实的机器人开发中，我们除了使用AMCL来进行定位外。还可以使用UWB定位技术。UWB表示Ultra-WideBand超宽带，超宽带技术是一种全新的、与传统通信技术有极大差异的通信新技术。它不需要使用传统通信体制中的载波，而是通过发送和接收具有纳秒或纳秒级以下的极窄脉冲来传输数据，从而具有GHz量级的带宽。
- UWB的测距原理：双向飞行时间法（TW-TOF,two way-time of flight）每个模块从启动开始即会生成一条独立的时间戳 。模块A的发射机在其时间戳上的Ta1发射请求性质的脉冲信号，模块B在Tb2时刻发射一个响应性质的信号，被模块A在自己的时间戳Ta2时刻接收。有次可以计算出脉冲信号在两个模块之间的飞行时间，从而确定飞行距离S。
- UWB的定位原理：UWB的室内定位功能和卫星原理很相似，就是通过室内布置4个已知坐标的定位基站。需要定位的设备携带定位标签，标签按照一定的频率发送脉冲。它不断的和4个已知位置的基站进行测距，通过一定的精确算法定出标签的位置
关于UWB的详细知识，请大家参考更为专业的书籍。我这里不做更多介绍，只是让大家对机器人定位有个更开阔的认识。除了使用概率法定位的AMCL，这种高精度的UWB定位也是非常不错的。
0x04 Bibliography & Reference
[1]. ros小生. ROSByExampleⅡ—第三章 使用ROS任务执行 (二).
https://blog.csdn.net/u011118482/article/details/73251360
[2]. 古月. ROS探索总结（十四）——move_base(路径规划). http://www.guyuehome.com/270
[3]. yuyangyg. UWB定位技术.
https://blog.csdn.net/yuyangyg/article/details/77043253
[4]. UWB百度百科. https://baike.baidu.com/item/UWB/184309?fr=aladdin
0x05 Feedback
大家在按照教程操作过程中有任何问题，可以关注ROS小课堂的官方微信公众号，在公众号中给我发消息反馈问题即可，我基本上每天都会处理公众号中的留言！当然如果你要是顺便给ROS小课堂打个赏，我也会感激不尽的，打赏30块还会邀请进ROS小课堂的微信群与更多志同道合的小伙伴一起学习和交流！
