随着答辩的结束，智慧物流这个项目就算是彻底结束了。不出意外的话，这应该是我大学生涯的参加的最后一个比赛了。看了看git的记录，提交的代码超过了1k行，同时也花了不少的时间在上面。做了这么多东西，不留下点什么总觉得不好，想来想去，还是把比赛做的东西做个小结吧，也算是纪念我这不算丰富的比赛时光吧（雾）
这里非常感谢我们队长，比赛里许多关键的问题是他解决的，他的动手能力和善于提出创新性想法的能力，都是值得我去学习的。也感谢另一位队友，在赛前紧张的准备时间里独自一人分担了制作PPT的工作，并且能做出如此精美的PPT。（大佬带带我 _(:з)∠)_ ）
感想也就写这么多吧，下面主要介绍我们比赛时所做的一些工作和创新。
#
程序的大体思路和流程
根据赛题要求，程序要实现下面几个功能：
- 导航至邮件分拣台
- 运用视觉算法识别邮件
- 机械臂定位抓取邮件
- 机器人运送邮件投入正确的邮箱
所以程序大致的步骤就是：建图->导航至分拣台->识别->抓取->导航至投递点->投递->导航至分拣台->……
这里我就按上面的要求一部分一部分的讲了。
#
建图
建图算法是Gmapping算法，这是一种基于2D激光雷达通过使用RBPF粒子滤波方法所完成的二维栅格地图构建SLAM算法，它也是移动机器人中适用最多的算法。它对激光雷达频率要求相对较低，稳定性较高，且能够结合里程计、陀螺仪等进行定位与地图构建，构建小场景地图时速度快、计算量小，缺点就是非常依赖里程计，所以无法适应不平坦的地面，无法使用回环检测，在大场景、粒子多的情况下，特别消耗资源。
当然我们并没有去仔细研究这个算法，建图的话按照官方提供的教程来一步步做就好了。不得不吐槽的是我们的小车一开始后轮是偏高的，导致雷达会打到地面，预选赛建图的时候一直建不好，最终只能靠侧边的雷达才把图建好，当时以为车就是这样的，直到正赛时才知道是车的问题。所以这种硬件问题一定要及时和厂商联系，不然全是泪啊😭。
#
导航
导航这里不是我们写，而是使用了官方提供的导航包。根据培训文档，这里主要采用了AMCL自适应蒙特卡罗算法，它是机器人在二维移动过程中的一种概率定位系统，通过它实现了自适应（或 kld 采样）的蒙特卡罗定位方法。由于采用粒子滤波器来跟踪已经知道的地图中机器人位姿，故对于大范围的局部定位问题工作良好。路径规划则是最短路径规划算法，例如采用Floyd算法、Dijkstra算法、Bellman-Ford算法等。
导航这里其实是有大坑的，我们最终比赛时就栽在这里了。我们发现小车运行时，激光雷达经常会发生错位，导致导航发生偏移。这种情况基本是出现在小车转向时，我们怀疑是轮子打滑，也调紧了后轮的弹簧，但也没有用。我们当时认为这就是小车本身的局限性，同时在小车运行时也会自动调整至正确位置，我们就没有特别重视这个问题。结果在正式比赛时导航就出现偏移，导致好几个包裹都错位抓取了，虽然最终都全部抓取上了，但浪费了很多时间，非常的可惜。所以有时间的话，一定要看看官方提供的导航定位算法，或者联系厂家看看雷达有无问题，不然程序没问题，却因为硬件问题而出错还是很难受的。( ‘-ωก̀ )
#
图像识别
图像识别这里我们是调用是百度的PaddleOCR模块，由于ROS里的Python是2版本的，我们采用的也是PaddleOCR里的develop分支而非最新版本。
我们一开始是打算自己训练PaddleOCR模型的，然而我尝试了一晚上，不是很清楚模型训练的方法，好不容易运行了一个公开的训练集，发现训练时间要一天多，同时还需要自己做训练集。考虑到时间的问题，最终还是选择采用官方的模型。当然，最主要的原因还是分布式图像识别提升了性能，这部分我在下面会提到，这里就先按下不表。
#
机械臂抓取及投递
根据机械臂的官方文档，这里涉及的坐标系主要是两种，一种是关节坐标系，一直是笛卡尔坐标系。关节坐标系比较难以理解，我们还是采用比较直观的笛卡尔坐标系。
同样，机械臂的抓取也有3种不同模式：MOVJ、MOVL、JUMP。MOVJ模式相较于MOVL模式，速度快；MOVJ模式相较于JUMP模式，稳定性高，不易越界。
综上，我们的机械臂控制都采用的是笛卡尔坐标系里的MOVEJ模式，这样能保证机械臂抓取时不会出现超出抓取范围的异常。
控制机械臂基本就只要用到下面这几个话题，还是比较容易的：
DobotServer/SetPTPCmd：移动机械臂到指定坐标DobotServer/SetEndEffectorSuctionCup：机械臂气泵控制DobotServer/ClearAllAlarmsState：清除机械臂警报
投递就没什么好说的了，根据文字识别的结果进行投递，只要能抓取到，基本上不会出现投递错位的情况。这里会出现的问题主要还是导航不准，容易偏或者撞。解决办法只能是不停的测试找准坐标以及将导航目的位置远离投递点。
#
一些创新点
#
键盘操控程序
在我们的程序中，主要是通过开启两个线程来实现程序的暂停和恢复，如果使用input函数进行键盘输入的接收，则会阻塞线程，同时在输入完成后还需要按下回车，不是很合适。官方是提供了一个键盘控制程序的样例，然而这个程序有个我认为很大的BUG，会导致程序的输出偏移。
作为一个对代码美观有洁癖的人，这怎么能忍，于是我开始了百度，下面是我找到的可以在Windows和Linux两种环境下实现类似C语言getchar()函数的代码。
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
|
def getChar():
# figure out which function to use once, and store it in _func
if"_func" not in getChar.__dict__:
try:
# for Windows-based systems
import msvcrt # If successful, we are on Windows
getChar._func=msvcrt.getch
except ImportError:
# for POSIX-based systems (with termios & tty support)
import tty, sys, termios # raises ImportError if unsupported
def _ttyRead():
fd = sys.stdin.fileno()
oldSettings = termios.tcgetattr(fd)
try:
tty.setcbreak(fd)
answer = sys.stdin.read(1)
finally:
termios.tcsetattr(fd, termios.TCSADRAIN, oldSettings)
return answer
getChar._func=_ttyRead
return getChar._func()
answer = getChar()
|
这里和官方提供的程序主要不同就在是tty.setcbreak(fd)，但我不知道原理，不过既然能用，就算了😇。
上面的代码还包括在Windows环境下的情况，我们不需要如此复杂，稍作修改将其整合进我们的程序，就构成了一个多线程程序的模板。
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
|
#!/usr/bin/env python
# coding=utf-8
import rospy
import threading
import sys
import termios
import tty
def main():
'''程序主要逻辑入口'''
pass
def getKey():
'''获取输入的字符'''
fd = sys.stdin.fileno()
oldSettings = termios.tcgetattr(fd)
try:
tty.setcbreak(fd)
key = sys.stdin.read(1)
finally:
termios.tcsetattr(fd, termios.TCSADRAIN, oldSettings)
return key
def initKeyListener():
'''输入监听程序'''
while(1):
key = getKey()
if key == 'k':
rospy.loginfo("退出程序")
os._exit(0)
elif key == 'a':
rospy.loginfo("恢复导航")
elif key == 's':
rospy.loginfo('取消导航')
elif key == 'q':
rospy.loginfo('开始执行程序')
if __name__ == "__main__":
# 给本程序初始化一个节点名称
rospy.init_node('match_topic')
# 开一个线程处理键盘监听
key_thread = threading.Thread(target=initKeyListener)
key_thread.start()
# 开一个线程处理任务
main_thread = threading.Thread(target=main)
main_thread.start()
|
#
摄像头分辨率提升及去畸变
由于ROS默认从摄像头中获取的图像是640*480，且去畸变也是根据这个分辨率进行设置的，这会导致文字识别很容易识别不出文字。我们将小车上的摄像头接入电脑，发现是720P的，既然你能清晰，那为什么不用呢？于是，我们修改了相关的lanuch文件，将摄像头的分辨率提升到了720P，并使用我们自己打印的标定板进行标定。
下面介绍一下修改的方法
在moveit_ws/src/moveit/probot_vision/launch目录下，我们可以看到usb_cam_in_hand.launch文件。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
|
<launch>
<node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
<param name="video_device" value="/dev/video0" />
<param name="image_width" value="1280" />
<param name="image_height" value="720" />
<param name="pixel_format" value="yuyv" />
<param name="camera_frame_id" value="cameral_base" />
<param name="io_method" value="mmap"/>
<param name="camera_info_url" type="string" value="file://$(find probot_vision)/ost-720p.yaml" />
<!--rosparam file="$(find probot_vision)/ost_4_new.yaml" command="load" /-->
</node>
</launch>
|
对其的image_width和image_height进行修改，改为1280和720即可实现摄像头画质提升至720P。
对于手动标定，我们需要准备标定板，可以自己打印也可以上淘宝购买，就是像下面这样的黑白方格。
然后在小车上启动标定程序（为什么不在自己电脑上启动呢，因为小车上已经帮你做好硬件端口绑定了，在自己电脑上还要再来一次）
1
2
3
4
|
# 启动摄像头
roslaunch usb_cam usb_cam-test.launch
# 启动标定程序
rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.024 image:=/usb_cam/image_raw camera:=/usb_cam
|
这里要注意修改size参数，改成你标定板的黑白格中间交点的个数。
square参数这里则是你标定板上方格的边长，单位是米。
启动完成以后就可以看到标定界面了，通过调整标定板的上下左右以及角度，让右边的进度条全变绿就算标定完成了。点击计算按钮得到数据，将保存的yml文件路径替换上面的camera_info_url。
仅仅是这样还不算是完成，我们还需要修改去畸变程序里的参数，将moveit_ws/src/moveit/probot_vision/src/image_correct.cpp里的相关参数按照yml文件的参数进行修改。（这里有疑似是自动读取的代码，但被注释了，我们也没测试能不能用）修改好后就可以执行编译了，我们在moveit_ws目录下执行catkin_make --pkg probot_vision，等待编译完成后这项工作就算完成了。
#
后退距离判断
由于小车的后面是没有雷达的，为了避免碰撞，我们编写了基于摄像头的后退距离判断。当小车到达分拣台前时，调用摄像头进行拍照，对其进行蓝色区域的识别，得到分拣台在图像中的像素位置，再利用公式进行转化，得到小车当前距离分拣台的距离，最后使用check话题控制小车后退至距离分拣台5cm左右的位置，既避免了碰撞，又最大化了抓取范围。
#
由消息驱动程序逻辑
在一开始，我们的程序是在函数里调用小车导航，并阻塞直到导航成功再继续执行。这样的程序有个问题，就是当小车出现错误时，无法立即停下小车并保存当前程序执行的进度。于是，我们便将程序修改为基于ROS消息来驱动。这样的好处是程序只有收到成功或者失败的消息后才会执行对应的逻辑，便于失败后状态的恢复。
想要实现相关的功能，首先是导航不能采用原来基于动作的模式了，而是改成基于消息的模式。我们在程序运行的一开始注册相关的话题发布者。
1
2
3
4
5
6
7
8
9
10
11
12
13
|
def initPublisher():
# 微调发布器
global checkPub
# 暂停导航发布器
global pauseNav
# 目标点发布器
global goalPub
goalPub = rospy.Publisher(
'move_base/goal', MoveBaseActionGoal, queue_size=1)
# 目标点里面的goal_id的id与上一次的id不能相同
checkPub = rospy.Publisher('check', String, queue_size=1)
pauseNav = rospy.Publisher(
'move_base/cancel', GoalID, queue_size=1)
|
同时，我们也需要注册相关话题的监听者以及对应的回调函数
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
|
def initListener():
# 订阅导航结果话题数据
rospy.Subscriber('move_base/result', MoveBaseActionResult,
navCallback)
# 订阅（微调）走固定距离结果话题数据
rospy.Subscriber('check_server/result', check_msgActionResult,
checkCallback)
rospy.loginfo("初始化订阅话题成功")
def checkCallback(data):
'''位置微移回调'''
if data.result.issuccess:
rospy.loginfo("位置微调完成")
pass
def navCallback(data):
'''导航消息回调'''
PROVINCES = {"四川", "安徽", "湖南", "广东", "浙江", "江苏", "福建", "河南"}
if data.status.status == 3:
rospy.loginfo("导航到达指定地点")
goal_name = data.status.goal_id.id.split('_')[0]
if goal_name[:9] == "分拣台":
# 到达分拣台前
pass
elif goal_name[:6] in PROVINCES:
# 到达投递点
pass
else:
rospy.logwarn("未知的节点:{}".format(data.status.goal_id.id))
elif data.status.status == 4:
rospy.logwarn("导航失败,当前id为:{}".format(data.status.goal_id.id))
else:
rospy.logwarn("导航被取消:{}".format(data.status.status))
|
在导航消息回调中，我们使用goal_id.id来进行目标地点的传递，其构造模式为targetName +'_' + str(time.time())[-9:-3]。这样既有目标地点信息，也避免了id重复。这里一个汉字占3位，所以看到上面的[:6]的写法也就代表前2个字。
#
分布式部署图像识别
对于图像识别，我们一开始是放在小车上进行的，结果发现在小车上识别速度并不是很快，平均每张图片需要5~6秒的时间才能完成。
检查小车的CPU发现，这性能还是不要指望其来跑图像识别算法了，还是采用在自己的电脑上跑图像识别，将结果返回给小车吧。
想要实现这样的功能，就需要在电脑里装好ROS环境，我们是直接使用虚拟机，里面已经配置好相关的环境了。
在~/.bashrc里增加export ROS_MASTER_URI=http://192.168.31.200:11311，让虚拟机使用小车作为服务端。之后就是编写程序注册服务，供主程序调用了，为了方便调用，我们自己编写了相应的数据结构。
我们注册了Province.msg，存放识别到的快递盒上所需的数据。name代表省份名称，x和y分别是相对偏移量。
1
2
3
|
string name
float32 x
float32 y
|
然后注册了ProvinceList.srv，作为我们服务的消息。
1
2
|
---
Province[] provinces
|
由于使用了自定义消息类型，要注意修改CMakeLists.txt，将相应的文件添加进去，并注意引入message_generation，否则不会生成供Python调用的消息类。
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
|
cmake_minimum_required(VERSION 3.0.2)
project(imgocr)
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
rospy
std_msgs
sensor_msgs
message_generation
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
## Generate messages in the 'msg' folder
add_message_files(
FILES
Province.msg
)
## Generate services in the 'srv' folder
add_service_files(
FILES
ProvinceList.srv
)
## Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
std_msgs
sensor_msgs
)
catkin_package(
CATKIN_DEPENDS rospy std_msgs message_runtime
)
include_directories(
# include
${catkin_INCLUDE_DIRS}
)
|
同样，在package.xml里也要添加message_generation等依赖
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
|
<?xml version="1.0"?>
<package format="2">
<name>imgocr</name>
<version>0.0.0</version>
<description>The imgocr package</description>
<maintainer email="eaibot@todo.todo">eaibot</maintainer>
<license>TODO</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>message_generation</build_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>message_runtime</exec_depend>
<export>
</export>
</package>
|
这样，就可以完成一个文字识别节点了。
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
|
#!/usr/bin/env python
# encoding: utf-8
from imp import reload
import sys
import rospy
import cv2
from imgocr.srv import ProvinceList, ProvinceListResponse
from imgocr.msg import Province
from sensor_msgs.msg import CompressedImage
from cv_bridge import CvBridge, CvBridgeError
import time
# 支持中文
reload(sys)
sys.setdefaultencoding("utf8")
def getImage():
'''获取摄像头的图片'''
data = rospy.wait_for_message(
'/usb_cam/image_correct/compressed', CompressedImage, timeout=15.0)
try:
cv_image = CvBridge().compressed_imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
rospy.logerr(e)
cv_image = cv2.rotate(cv_image, cv2.ROTATE_180)
cv_image = cv2.flip(cv_image, 1)
return cv_image
def doReq(req):
rospy.loginfo("开始执行文字识别")
start_time = time.time()
img = getImage()
pass
rospy.loginfo("识别用时：{}".format(time.time()-start_time))
resp = ProvinceListResponse()
for pro in ans:
a = Province()
a.name = pro["province"]
a.x = pro["x"]
a.y = pro["y"]
resp.provinces.append(a)
return resp
def main():
rospy.init_node("imgocr")
server = rospy.Service("imgocr/getProvince", ProvinceList, doReq)
rospy.loginfo("节点初始化完成")
rospy.spin()
if __name__ == "__main__":
main()
|
这里我们一开始获取摄像头的图片是采用/usb_cam/image_correct这个话题，但这个话题得到的图片是未压缩的，使用rostopic bw /usb_cam/image_correct可以看出大小差不多是好几M，这就会导致获取图像的时长远超文字识别时长，甚至是无法获取到图片。我们根据探索，发现/usb_cam/image_correct/compressed话题提供的是压缩后的图片，使用后获取图像的时长大大减小。
使用分布式图像识别后，识别时长基本控制在2S内。
由于我们是使用虚拟机，无法调用真机的显卡，如果有机会的话，可以尝试在真机上装相应的环境，使用GPU来进行图像识别，速度应该会更进一步。
#
机械臂异常自动检测和清除
在上面一小节中，我们获取到省份的坐标就可以送至机械臂进行抓取了。但是，机械臂的抓取范围是有限的，一旦超出抓取范围机械臂就会停止执行直至执行清除警报。一种思路是我们首先是在一个固定的定去识别邮件，首先记录这个点的高度，然后我们需要降到桌面去吸取邮件，这是第二个高度，我们只需要测定这两个高度Z的前提下，Y的移动范围就可以了。
这种思路确实可行，但如果后期更换了机械臂高度，又要手动测量，十分麻烦。于是，我想了另一个思路，通过查询API文档，我找到了警报检测的API，那就好说了。在执行可能越界的指令后接上异常检测，如果异常就自动清除，这样就能自动化的判断了。
话不多说，直接上代码：
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
|
#!/usr/bin/env python
# coding=utf-8
import rospy
from dobot.srv import GetAlarmsState, ClearAllAlarmsState
def getAlarmsState():
'''
查询机械臂报警状态，并自动进行清除
return 是否存在报警
'''
rospy.wait_for_service('DobotServer/GetAlarmsState')
rospy.wait_for_service('DobotServer/ClearAllAlarmsState')
try:
client = rospy.ServiceProxy(
'DobotServer/GetAlarmsState', GetAlarmsState)
client2 = rospy.ServiceProxy(
'DobotServer/ClearAllAlarmsState', ClearAllAlarmsState)
a = client()
isError = False
if a.alarmsState != "\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0":
rospy.loginfo("检测到警报，自动进行清除")
client2()
isError = True
return isError
except rospy.ServiceException as e:
rospy.logerr("查询机械臂报警状态失败: %s" % e)
|
#
ROS日志输出级别控制
在上面我提到过，我对代码美观有洁癖，那程序输出也自然不能放过。这个项目里所有的输出我都使用rospy.log来输出，这就带来一个问题，有些输出我将其设为DEBUG级别，而ROS默认的日志级别为INFO，导致这些信息根本看不到。每次想看时手动将代码换为INFO再换回去，虽然可行，但不优雅。于是我研究了一下ROS的日志方面的文档，发现是有方法来实现日志输出级别的控制的。
在终端运行rqt_logger_level，之后选择你的程序节点中的rosout，在后面修改就可以修改ROS日志输出的级别了。
时间有限，我就找到了这一个可行的方法，其余方法我测试后并没生效，如果有更好的方法欢迎在评论区留言。
#
一些小工具
在写程序时，为了避免重复工作，还是做了一些小工具，本着“不要重复造轮子”的精神，在此一并开源了，希望能帮助到有需要的同学。
#
随机快递箱顺序生成
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
|
#!/usr/bin/env python
# coding=utf-8
import random
provinces = []
for i in range(3):
provinces.append('四川')
for i in range(2):
provinces.append('安徽')
for i in range(2):
provinces.append('湖南')
for i in range(3):
provinces.append('广东')
for i in range(3):
provinces.append('浙江')
for i in range(2):
provinces.append('江苏')
for i in range(3):
provinces.append('福建')
for i in range(2):
provinces.append('河南')
random.shuffle(provinces)
for pro in provinces:
print(pro)
|
#
EAI资源点坐标格式转yaml格式
将官方QTEAIBotDemoPub(VER2)这个程序创建的导航目标点转成yaml格式，方便人修改及程序调用
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
|
#!/usr/bin/env python
# coding=utf-8
loacate = input("file loacate:")
file = open(loacate, "r")
file_data = file.readlines()
for row in file_data:
tmp_list = row.split(',')
print(tmp_list[0]+":")
positions = tmp_list[2].split('#')
print(" x: "+positions[0])
print(" y: "+positions[1])
orientations = tmp_list[3].split('#')
print(" z: "+orientations[2])
print(" w: "+orientations[3])
|
#
小结
这个项目到此也就完全结束了，回头看来，这个项目也是涉及了许多知识，诸如图像识别、自主定位等等，不过时间有限，基本都没怎么了解其中原理，只是调用了相关的接口，如果以后有空，可以继续研究研究😇。
