迷宫寻宝 | 木匠

ROS

发布日期: 2019-09-17

文章字数: 944

阅读时长: 4 分

阅读次数:

一、迷宫寻宝

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);       
}