机器人综合应用——“迷宫寻宝”
一、迷宫寻宝
1.1 游戏规则
已知一个红色小球藏在地图的某处,机器人在环境未知的情况下从起点出发,自主寻找宝藏,寻找宝藏后回到起点。
1.2 任务实现流程
- 地图预先导入Gazebo
- 机器人搭载激光雷达和相机
- 机器人通过slam进行未知环境探索
- 机器人寻找到宝藏后语音播报状态
- 宝藏获得后,在已知的部分地图上导航回出发点
1.3 实现框架
二、功能模块
2.1 导航&SLAM
导航&SLAM模块是exploring_maze节点,通过随机在地图上设定导航点,然后由机器人进行自主导航,并通过视觉寻找小圆球,最后再输出寻宝导航过程的时间。
# 开始主循环,随机导航
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)
continue
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")
if self.exploring_cmd is STATUS_GO_HOME:
break
else:
rospy.sleep(self.rest_time)
self.shutdown()
2.2 目标识别
目标识别模块是object_detect节点,通过RGB颜色识别小圆球,一旦识别到小圆球后就靠近小圆球。
# 定义目标物体的颜色范围边界
boundaries = [([BLUE_LOW, GREEN_LOW, RED_LOW], [BLUE_HIGH, GREEN_HIGH, RED_HIGH])]
for (lower, upper) in boundaries:
# 从边界创建NumPy数组
lower = np.array(lower, dtype = "uint8")
upper = np.array(upper, dtype = "uint8")
# 找到指定边界内的颜色并使用掩码
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]
# 找到阈值图像中的轮廓
img, cnts, hierarchy = cv2.findContours(thresh, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
# 循环找到的轮廓
for c in cnts:
# 计算轮廓的中心
M = cv2.moments(c)
if int(M["m00"]) not in range(20000, 250000):
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) #不断靠近小圆球
2.3 视觉伺服
视觉伺服木块是move_to_target节点,订阅图像识别的结果,根据目标像素位置调整速度指令。
// 将接收到的消息打印出来
ROS_INFO("Target pose: x:%0.6f, y:%0.6f, z:%0.6f", msg->position.x, msg->position.y, msg->position.z);
// 停止机器人导航
if(status_flag == STATUS_EXPLORING)
{
status_flag = STATUS_CLOSE_TARGET;
std_msgs::Int8 cmd;
cmd.data = STATUS_CLOSE_TARGET;
cmd_pub.publish(cmd);
std_msgs::String msg;
msg.data = "发现宝藏,向宝藏进发";
voice_pub.publish(msg);
}
else if(status_flag == STATUS_CLOSE_TARGET && msg->position.z > GET_TARGET_SIZE)
{
status_flag = STATUS_GO_HOME;
std_msgs::Int8 cmd;
cmd.data = STATUS_GO_HOME;
cmd_pub.publish(cmd);
std_msgs::String msg;
msg.data = "拿到宝藏,撤退";
voice_pub.publish(msg);
}
else if(status_flag == STATUS_CLOSE_TARGET)
{
// 初始化geometry_msgs::Twist类型的消息
geometry_msgs::Twist vel_msg;
vel_msg.linear.x = (150000 - msg->position.z) / 150000 * 0.3;
vel_msg.angular.z = (640 - msg->position.x) / 640 * 0.3;
// 发布消息
vel_pub.publish(vel_msg);
ROS_INFO("Publsh velocity command[%0.2f m/s, %0.2f rad/s]", vel_msg.linear.x, vel_msg.angular.z);
}
三、运行结果
该机器人寻宝使用的技术点有机器人建模仿真,图像识别,语音交互,自主导航,SLAM,ROS通信机制,系统设计与集成。