ROS理论与实践(以移动机器人为例)连载(十)【完结】 ——机器人综合应用—“迷宫寻宝”
这讲会提供示例代码！
Content
- 1. “迷宫寻宝” 之任务发布
- 2. “迷宫寻宝” 之任务分析
- 3. “迷宫寻宝” 之具体实现
- ①机器人建模
- ②图像处理
- ③视觉伺服
- ④导航&SLAM
1. “迷宫寻宝” 之任务发布
已知迷宫地图的覆盖范围（10*10m），其中某处藏有一处明显标记的宝藏（红色圆球），机器人在未知环境的情况下从起点出发，自主寻找宝藏，寻得宝藏之后需返回起点，任务完成。
- 地图预先使用Gazebo创建完成，并在Gazebo完成所有任务；
- 机器人在起点运动时开始计时，寻得宝藏回到起点后计时终止，须在5min内完成任务;
- 机器人搭载的传感器没有限制；
- 机器人接近宝藏1m范围内即认为获取宝藏；
- 机器人获取宝藏后，需通过语音播报状态信息；
- 宝藏的位置不固定，允许动态调整。
迷宫环境可以使用Building Editor建立，二维图中有长度信息。
2. “迷宫寻宝” 之任务分析
- 机器人建模仿真
- 图像识别
- 语音交互
- 科大讯飞 SDK
- 自主导航
- SLAM
- ROS通信机制
- 系统设计与集成
需要通过图像识别object_detect寻找到宝藏的位置，通过目标位置调整机器人的位置，使得机器人向它运动（视觉伺服），通过exploring_maze导航、通过gmapping建图。将导航目标点发送给move_base进行避障自主导航。tts_subscribe是之前提到过的科大讯飞开放平台提供的语音合成例程，这里用来发布寻找到目标物体的语音。
3. “迷宫寻宝” 之具体实现
①机器人建模
为了完成上述任务，给机器人搭建仿真环境时，添加camera、laser。
mbot_base_gazebo.xacro
<robot name="mbot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:property name="M_PI" value="3.1415926"/>
<xacro:property name="base_mass" value="1" />
<xacro:property name="base_radius" value="0.20"/>
<xacro:property name="base_length" value="0.16"/>
<xacro:property name="wheel_mass" value="0.2" />
<xacro:property name="wheel_radius" value="0.06"/>
<xacro:property name="wheel_length" value="0.025"/>
<xacro:property name="wheel_joint_y" value="0.19"/>
<xacro:property name="wheel_joint_z" value="0.05"/>
<xacro:property name="caster_mass" value="0.2" />
<xacro:property name="caster_radius" value="0.015"/>
<xacro:property name="caster_joint_x" value="0.18"/>
<material name="yellow">
<color rgba="1 0.4 0 1"/>
material>
<material name="black">
<color rgba="0 0 0 0.95"/>
material>
<material name="gray">
<color rgba="0.75 0.75 0.75 1"/>
material>
<xacro:macro name="sphere_inertial_matrix" params="m r">
<inertial>
<mass value="${m}" />
<inertia ixx="${2*m*r*r/5}" ixy="0" ixz="0"
iyy="${2*m*r*r/5}" iyz="0"
izz="${2*m*r*r/5}" />
inertial>
xacro:macro>
<xacro:macro name="cylinder_inertial_matrix" params="m r h">
<inertial>
<mass value="${m}" />
<inertia ixx="${m*(3*r*r+h*h)/12}" ixy="0" ixz="0"
iyy="${m*(3*r*r+h*h)/12}" iyz="0"
izz="${m*r*r/2}" />
inertial>
xacro:macro>
<xacro:macro name="wheel" params="prefix reflect">
<joint name="${prefix}_wheel_joint" type="continuous">
<origin xyz="0 ${reflect*wheel_joint_y} ${-wheel_joint_z}" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="${prefix}_wheel_link"/>
<axis xyz="0 1 0"/>
joint>
<link name="${prefix}_wheel_link">
<visual>
<origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
<geometry>
<cylinder radius="${wheel_radius}" length="${wheel_length}"/>
geometry>
<material name="gray" />
visual>
<collision>
<origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
<geometry>
<cylinder radius="${wheel_radius}" length="${wheel_length}"/>
geometry>
collision>
<cylinder_inertial_matrix m="${wheel_mass}" r="${wheel_radius}" h="${wheel_length}" />
link>
<gazebo reference="${prefix}_wheel_link">
<material>Gazebo/Graymaterial>
gazebo>
<transmission name="${prefix}_wheel_joint_trans">
<type>transmission_interface/SimpleTransmissiontype>
<joint name="${prefix}_wheel_joint" >
<hardwareInterface>hardware_interface/VelocityJointInterfacehardwareInterface>
joint>
<actuator name="${prefix}_wheel_joint_motor">
<hardwareInterface>hardware_interface/VelocityJointInterfacehardwareInterface>
<mechanicalReduction>1mechanicalReduction>
actuator>
transmission>
xacro:macro>
<xacro:macro name="caster" params="prefix reflect">
<joint name="${prefix}_caster_joint" type="continuous">
<origin xyz="${reflect*caster_joint_x} 0 ${-(base_length/2 + caster_radius)}" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="${prefix}_caster_link"/>
<axis xyz="0 1 0"/>
joint>
<link name="${prefix}_caster_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="${caster_radius}" />
geometry>
<material name="black" />
visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="${caster_radius}" />
geometry>
collision>
<sphere_inertial_matrix m="${caster_mass}" r="${caster_radius}" />
link>
<gazebo reference="${prefix}_caster_link">
<material>Gazebo/Blackmaterial>
gazebo>
xacro:macro>
<xacro:macro name="mbot_base_gazebo">
<link name="base_footprint">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.001 0.001 0.001" />
geometry>
visual>
link>
<gazebo reference="base_footprint">
<turnGravityOff>falseturnGravityOff>
gazebo>
<joint name="base_footprint_joint" type="fixed">
<origin xyz="0 0 ${base_length/2 + caster_radius*2}" rpy="0 0 0" />
<parent link="base_footprint"/>
<child link="base_link" />
joint>
<link name="base_link">
<visual>
<origin xyz=" 0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="${base_length}" radius="${base_radius}"/>
geometry>
<material name="yellow" />
visual>
<collision>
<origin xyz=" 0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="${base_length}" radius="${base_radius}"/>
geometry>
collision>
<cylinder_inertial_matrix m="${base_mass}" r="${base_radius}" h="${base_length}" />
link>
<gazebo reference="base_link">
<material>Gazebo/Bluematerial>
gazebo>
<wheel prefix="left" reflect="1"/>
<wheel prefix="right" reflect="-1"/>
<caster prefix="front" reflect="-1"/>
<caster prefix="back" reflect="1"/>
<gazebo>
<plugin name="differential_drive_controller"
filename="libgazebo_ros_diff_drive.so">
<rosDebugLevel>DebugrosDebugLevel>
<publishWheelTF>truepublishWheelTF>
<robotNamespace>/robotNamespace>
<publishTf>1publishTf>
<publishWheelJointState>truepublishWheelJointState>
<alwaysOn>truealwaysOn>
<updateRate>100.0updateRate>
<legacyMode>truelegacyMode>
<leftJoint>left_wheel_jointleftJoint>
<rightJoint>right_wheel_jointrightJoint>
<wheelSeparation>${wheel_joint_y*2}wheelSeparation>
<wheelDiameter>${2*wheel_radius}wheelDiameter>
<broadcastTF>1broadcastTF>
<wheelTorque>30wheelTorque>
<wheelAcceleration>1.8wheelAcceleration>
<commandTopic>cmd_velcommandTopic>
<odometryFrame>odomodometryFrame>
<odometryTopic>odomodometryTopic>
<robotBaseFrame>base_footprintrobotBaseFrame>
plugin>
gazebo>
xacro:macro>
robot>
mbot_with_camera_laser_gazebo.xacro
<robot name="arm" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find mbot_description)/urdf/mbot_base_gazebo.xacro" />
<xacro:include filename="$(find mbot_description)/urdf/sensors/camera_gazebo.xacro" />
<xacro:include filename="$(find mbot_description)/urdf/sensors/lidar_gazebo.xacro" />
<xacro:property name="camera_offset_x" value="0.17" />
<xacro:property name="camera_offset_y" value="0" />
<xacro:property name="camera_offset_z" value="0.10" />
<xacro:property name="pillar_length" value="0.1" />
<xacro:property name="pillar_radius" value="0.01" />
<xacro:property name="pillar_mass" value="0.1" />
<xacro:property name="lidar_offset_x" value="0" />
<xacro:property name="lidar_offset_y" value="0" />
<xacro:property name="lidar_offset_z" value="0.07" />
<joint name="camera_joint" type="fixed">
<origin xyz="${camera_offset_x} ${camera_offset_y} ${camera_offset_z}" rpy="0 0 0" />
<parent link="base_link"/>
<child link="camera_link"/>
joint>
<xacro:usb_camera prefix="camera"/>
<link name="pillar_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="${pillar_length}" radius="${pillar_radius}"/>
geometry>
<material name="gray" />
visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="${pillar_length}" radius="${pillar_radius}"/>
geometry>
collision>
<cylinder_inertial_matrix m="${pillar_mass}" r="${pillar_radius}" h="${pillar_length}" />
link>
<joint name="pillar_joint" type="fixed">
<origin xyz="0 0 0.10" rpy="0 0 0" />
<parent link="base_link"/>
<child link="pillar_link"/>
joint>
<joint name="lidar_joint" type="fixed">
<origin xyz="${lidar_offset_x} ${lidar_offset_y} ${lidar_offset_z}" rpy="0 0 0" />
<parent link="pillar_link"/>
<child link="laser_link"/>
joint>
<xacro:rplidar prefix="laser"/>
<mbot_base_gazebo/>
robot>
注意：camera_gazebo.xacro和lidar_gazebo.xacro和ROS理论与实践(以移动机器人为例)连载(五) ——构建机器人仿真平台中提到的相同，可以自行修改参数。
这里就不放.launch文件了，很简单，相信看到这里的童鞋都知道怎么写了。前面的课程 ROS理论与实践(以移动机器人为例)连载(五) ——构建机器人仿真平台 也有描述。
②图像处理
这里使用对颜色进行识别，因为目标物体和背景颜色差距很大。
object_detect.py
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import cv2
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
import numpy as np
from math import *
from geometry_msgs.msg import Pose
BLUE_LOW = 0
BLUE_HIGH = 20
GREEN_LOW = 20
GREEN_HIGH = 60
RED_LOW = 80
RED_HIGH = 150
class image_converter:
def __init__(self):
# 创建cv_bridge，声明图像的发布者和订阅者
self.bridge=CvBridge() #ROS图像和OpenCV图像信息的转换
self.image_sub=rospy.Subscriber("/camera/image_raw", Image, self.callback) #订阅Image，Camera的话题
self.image_pub=rospy.Publisher("object_detect_image", Image, queue_size=1) #发布识别结果
self.target_pub=rospy.Publisher("object_detect_target", Pose, queue_size=1) #发布target的Pose信息
def callback(self,data):
# 使用cv_bridge将ROS的图像数据转换成OpenCV的图像格式
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") #将ROS中拿到的数据转换成OpenCV能够使用的数据
except CvBridgeError as e:
print e
# define the list of boundaries in BGR
boundaries = [([BLUE_LOW, GREEN_LOW, RED_LOW], [BLUE_HIGH, GREEN_HIGH, RED_HIGH])] #识别颜色的范围值BGR
# loop over the boundaries
# print(boundaries)
for (lower, upper) in boundaries:
# create NumPy arrays from the boundaries
lower = np.array(lower, dtype = "uint8")
upper = np.array(upper, dtype = "uint8")
# find the colors within the specified boundaries and apply the mask
mask = cv2.inRange(cv_image, lower, upper)
output = cv2.bitwise_and(cv_image, cv_image, mask = mask)
cvImg = cv2.cvtColor(output, 6) #cv2.COLOR_BGR2GRAY
npImg = np.asarray( cvImg )
thresh = cv2.threshold(npImg, 1, 255, cv2.THRESH_BINARY)[1]
# find contours in the thresholded image
img, cnts, hierarchy = cv2.findContours(thresh, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
#cnts = cnts[0]
# loop over the contours
for c in cnts:
# compute the center of the contour
M = cv2.moments(c)
if int(M["m00"]) not in range(20000, 250000): #M["m00"]是面积，如果面积不达标就不认为是宝藏
continue
cX = int(M["m10"] / M["m00"])
cY = int(M["m01"] / M["m00"])
cv2.drawContours(cv_image, [c], -1, (0, 0, 255), 2)
cv2.circle(cv_image, (cX, cY), 1, (0, 0, 255), -1)
objPose = Pose()
objPose.position.x = cX;
objPose.position.y = cY;
objPose.position.z = M["m00"];
self.target_pub.publish(objPose)
# 显示Opencv格式的图像
# cv2.imshow("Image window", cv_image)
# cv2.waitKey(3)
# 再将opencv格式额数据转换成ros image格式的数据发布
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
except CvBridgeError as e:
print e
if __name__ == '__main__':
try:
# 初始化ros节点
rospy.init_node("object_detect")
rospy.loginfo("Starting detect object")
image_converter()
rospy.spin()
except KeyboardInterrupt:
print "Shutting down object_detect node."
cv2.destroyAllWindows()
那么有一个问题上面设置的检测颜色范围由于在Gazebo内光影的问题，实际每个人的是不相同的该如何去测算自己模型中的RGB区间呢？
我们可以把检测宝藏在Gazebo仿真环境中和机器人摄像头放在一起，保存下rqt_image_view中的显示图片。Gazebo仿真环境上面的模型构建中已经提到，有缺省的在之前的教程中也必然提到了。
这是保存好的图片↓
然后丢到这个程序里↓ hsv_test.py
# -*- coding:utf-8 -*-
import cv2
img = cv2.imread('image.png')
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
def mouse_click(event, x, y, flags, para):
if event == cv2.EVENT_LBUTTONDOWN: # 左边鼠标点击
#print('PIX:', x, y)
print("BGR:", img[y, x])
#print("GRAY:", gray[y, x])
#print("HSV:", hsv[y, x])
if __name__ == '__main__':
cv2.namedWindow("img")
cv2.setMouseCallback("img", mouse_click)
while True:
cv2.imshow('img', img)
if cv2.waitKey() == ord('q'):
break
cv2.destroyAllWindows()
检测结果↓
运行成功后可以使用rostopic echo /object_detect_target打印出发布的目标位置的圆心信息， ( x , y ) (x,y) (x,y)当然是圆心坐标了， z z z是这个目标在Camera二维图像中的面积。
③视觉伺服
示例代码 move_to_target.cpp↓
代码中使用宏定义，定义了机器人的三种状态以及因为 z z z是二维图像中目标的面积，那么定义了GET_TARGET_SIZE用来确定是否到达限定距离。
同时，在机器人获取宝藏后，发布话题/voiceWords通过tts_subscribe.cpp也就是之前在机器语音那节使用过的由科大讯飞开放平台提供的语音合成代码。用来发布当前状态的语音信息。
#include上述代码中回调函数poseCallback内的条件语句属于之后集成系统的部分，可以先忽略，只关注最后一段else if即可。
④导航&SLAM
联系上一个代码，如果接收到/object_detect_pose即检测到目标宝藏时，会调回回调函数，回调函数中初始化了status_flag这个变量为STATUS_CLOSE_TARGET，那么跳入视觉伺服，进行机器人检测到宝藏后的最后移动；倘若既满足STATUS_CLOSE_TARGET，又满足面积比预设的面积阈值大，也就是到达了能够抓取宝藏的距离，status_flag这个变量被赋值为STATUS_GO_HOME，也就是fetch the treasure，那么在下面↓这个代码中就可以看到设定的MoveBaseGoal是初始原点；另一种status_flag变量形式是STATUS_EXPLORING，就是未找到目标物体，但是仍在建图导航+寻找中，那么随机导航目标点，同时进行物体的检测。当检测到物体后就跳转到上面说过的，取消self.move_base.cancel_goal()随机导航，转向视觉伺服。
下面是导航的示例代码↓ exploring_maze.py
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import roslib;
import rospy
import actionlib
import random
from actionlib_msgs.msg import *
from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from std_msgs.msg import Int8
STATUS_EXPLORING = 0
STATUS_CLOSE_TARGET = 1
STATUS_GO_HOME = 2
class ExploringMaze():
def __init__(self):
rospy.init_node('exploring_maze', anonymous=True)
rospy.on_shutdown(self.shutdown) # shutdown是用来在CTRL+C取消后，给机器人发布一个0速度的指令
# 在每个目标位置暂停的时间 (单位：s)
self.rest_time = rospy.get_param("~rest_time", 0.5)
# 发布控制机器人的消息
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
# 创建一个Subscriber，订阅/exploring_cmd
rospy.Subscriber("/exploring_cmd", Int8, self.cmdCallback)
# 订阅move_base服务器的消息
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) # 订阅move_base服务器，相当于这是个客户端用来给导航发布Goal指令的
rospy.loginfo("Waiting for move_base action server...")
# 60s等待时间限制
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
# 保存运行时间的变量
start_time = rospy.Time.now()
running_time = 0
rospy.loginfo("Starting exploring maze")
# 初始位置
start_location = Pose(Point(0, 0, 0), Quaternion(0.000, 0.000, 0.709016873598, 0.705191515089)) # 设定初始位置实际上就是零位置HOME
# 命令初值
self.exploring_cmd = 0
# 开始主循环，随机导航
while not rospy.is_shutdown():
# 设定下一个随机目标点
self.goal = MoveBaseGoal()
self.goal.target_pose.pose = start_location # 先回到初始点
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now() # 把当前时间发布
if self.exploring_cmd is STATUS_EXPLORING: # 探索，随机点导航
self.goal.target_pose.pose.position.x = random.randint(0,8)
self.goal.target_pose.pose.position.y = random.randint(0,9)
elif self.exploring_cmd is STATUS_CLOSE_TARGET: # 视觉伺服
rospy.sleep(0.1)
elif self.exploring_cmd is STATUS_GO_HOME: # 返航
self.goal.target_pose.pose.position.x = 0
self.goal.target_pose.pose.position.y = 0
# 让用户知道下一个位置
rospy.loginfo("Going to:" + str(self.goal.target_pose.pose))
# 向下一个位置进发
self.move_base.send_goal(self.goal)
# 五分钟时间限制
finished_within_time = self.move_base.wait_for_result(rospy.Duration(300))
# 查看是否成功到达
if not finished_within_time:
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal succeeded!")
else:
rospy.loginfo("Goal failed!")
# 运行所用时间
running_time = rospy.Time.now() - start_time
running_time = running_time.secs / 60.0
# 输出本次导航的所有信息
rospy.loginfo("current time:" + str(trunc(running_time,1))+"min")
self.shutdown()
def cmdCallback(self, msg):
rospy.loginfo("Receive exploring cmd : %d", msg.data)
self.exploring_cmd = msg.data
if self.exploring_cmd is STATUS_CLOSE_TARGET:
rospy.loginfo("Stopping the robot...")
self.move_base.cancel_goal()
def shutdown(self):
rospy.loginfo("Stopping the robot...")
self.move_base.cancel_goal()
rospy.sleep(2)
self.cmd_vel_pub.publish(Twist())
rospy.sleep(1)
def trunc(f, n):
slen = len('%.*f' % (n, f))
return float(str(f)[:slen])
if __name__ == '__main__':
try:
ExploringMaze()
rospy.spin()
# spin一直在订阅exploring_cmd如果flag有变化，那么机器人也在导航和视觉伺服直接跳换
except rospy.ROSInterruptException:
rospy.loginfo("Exploring maze finished.")
大体上的介绍和代码就是这样的，三个.launch文件如下：
- mbot_maze_gazebo.launch 也就是仿真模型
<launch>
<arg name="world_name" value="$(find mbot_gazebo)/worlds/maze.world"/>
<arg name="paused" default="false"/>
<arg name="use_sim_time" default="true"/>
<arg name="gui" default="true"/>
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(arg world_name)" />
<arg name="debug" value="$(arg debug)" />
<arg name="gui" value="$(arg gui)" />
<arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="headless" value="$(arg headless)"/>
include>
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find mbot_description)/urdf/mbot_with_camera_laser_gazebo.xacro'" />
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" >node>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen" >
<param name="publish_frequency" type="double" value="50.0" />
node>
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -model mbot -param robot_description"/>
launch>
- exploring_slam_demo.launch 导航和建图，并根据exploring_cmd进行系统上与视觉伺服控制的替换
<launch>
<include file="$(find mbot_navigation)/launch/gmapping.launch"/>
<include file="$(find mbot_navigation)/launch/move_base.launch" />
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find mbot_navigation)/rviz/nav.rviz"/>
launch>
这个包含了 gmapping.launch 和 move_base.launch 上一讲我们都提到过，可以查看ROS理论与实践(以移动机器人为例)连载(九) ——机器人自主导航。
- find_target.launch 寻找宝藏
<launch>
<node name="exploring_maze" type="exploring_maze.py" pkg="mbot_navigation" output="screen" />
<node name="object_detect" type="object_detect.py" pkg="mbot_vision" output="screen"/>
<node name="move_to_target" type="move_to_target" pkg="mbot_vision" output="screen" />
<node name="tts_subscribe" type="tts_subscribe" pkg="mbot_voice" output="screen" />
launch>
大致的就这样结束了，当然在已知map的情况下不使用随机目标点导航，而是有目的的确定几个空间下的关键点，对整个任务完成效率更高。
想要如何获得地图中某个点的坐标和四元数，可以使用rostopic echo /move_base/goal来打印出目标点的坐标，使用2D Navigation Go的GUI进行设定目标点。