multiGoalListen.py

#!/usr/bin/env python
# encoding: utf-8
# 如果觉得不错，可以推荐给你的朋友！http://tool.lu/pyc

import rospy
import actionlib
from actionlib_msgs.msg import *
from geometry_msgs.msg import Pose, Point, Quaternion, Twist
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from tf.transformations import quaternion_from_euler
from visualization_msgs.msg import Marker
from math import radians, pi
import redis
import datetime
import os
import time

class MultiGoalListen:                                                                  //多目标监听
    
    def __init__(self):
        rospy.init_node('MultiGoalListen', anonymous = False)
        rospy.on_shutdown(self.shutdown)
        self.move_base = actionlib.SimpleActionClient('move_base', MoveBaseAction)
        rospy.loginfo('Waiting for move_base action server...')
        self.move_base.wait_for_server(rospy.Duration(120))                 //延时等待120s
        rospy.loginfo('Connected to move base server')
        rospy.loginfo('Starting MultiGoalListen')
        rospy.Subscriber('/robot_pose', Pose, self.robot_pose_callback)     //接收到机器人位姿的信息，回调函数将机器人的位姿设置为 CurrentRobotPose表
        self.r = redis.Redis(host = '127.0.0.1', port = 6379, db = 0)
        attr_dict = {                                                       //初始化参数
            'mode': 'order',                                                //顺序
            'loopWay': 'auto',                                              //循环方式
            'isNavNext': 0,                                                 //是否导航到下一个
            'currentState': 'stopped',                                      //当前的状态
            'goalQueue': 'GoalQueueA',                                      //目标队列
            'priorGoalQueue': 'GoalQueueB',                                 //之前的目标队列  目标队列B
            'currentQueue': '',                                             //当前的队列
            'currentGoal': '',                                              //当前的目标
            'successNum': '0',                                              //成功的数目
            'failedNum': '0',                                               //失败的数目
            'intervalTime': '3' }                                           //间隔时间        
        self.r.hmset('GoalState', attr_dict)                                //hmset命令用于将 GoalState  设置为 attr_dict 表
        goal = MoveBaseGoal()
        quaternion = Quaternion()                                           //四元素
        while not rospy.is_shutdown():
            mymode = self.r.hget('GoalState', 'mode')                       //返回GoalState表中对应的模式
            priorgoalQueue = self.r.hget('GoalState', 'priorGoalQueue')
            goalQueue = self.r.hget('GoalState', 'goalQueue')
            loopWay = self.r.hget('GoalState', 'loopWay')
            isNavNext = self.r.hget('GoalState', 'isNavNext')
            if priorgoalQueue != None and self.r.llen(priorgoalQueue) > 0:  //之前的目标队列不为零
                mygoalQueue = priorgoalQueue                                            
            else:                                                           //之前的目标队列为零
                mygoalQueue = goalQueue
            mycurrentState = self.r.hget('GoalState', 'currentState')
            if mycurrentState != 'running':
                rospy.sleep(1)
                continue
            if loopWay == 'manual':
                if int(isNavNext) == 1:
                    self.r.hset('GoalState', 'isNavNext', 0)
                else:
                    rospy.sleep(1)
            self.r.hset('GoalState', 'currentQueue', mygoalQueue)
            mygoal = self.r.lpop(mygoalQueue)                               //弹出对列最前面的值
            if mygoalQueue == goalQueue and mymode == 'loop':
                self.r.rpush(mygoalQueue, mygoal)
                rospy.loginfo('rpush:' + mygoalQueue + ' ' + mygoal)
            if mygoal != None:
                self.r.hset('GoalState', 'currentGoal', mygoal)
                mygoalVal = self.r.hget('GoalAliaseSet', mygoal)
                if mygoalVal != None:
                    (x, y, z, qx, qy, qz, qw) = str.split(mygoalVal, '_')
                    goal.target_pose.header.frame_id = 'map'
                    goal.target_pose.header.stamp = rospy.Time.now()
                    quaternion.x = float(qx)
                    quaternion.y = float(qy)
                    quaternion.z = float(qz)
                    quaternion.w = float(qw)
                    goal.target_pose.pose.position.x = float(x)
                    goal.target_pose.pose.position.y = float(y)
                    goal.target_pose.pose.position.z = float(z)
                    goal.target_pose.pose.orientation = quaternion
                    self.move_base.send_goal(goal)
                    finished_within_time = self.move_base.wait_for_result(rospy.Duration(600))     //返回是否完成的结果，超时时间600
                    if not finished_within_time:                                    //没有完成
                        self.move_base.cancel_goal()                                //清除目标
                        rospy.logwarn('Timed out achieving goal')
                        self.r.hincrby('GoalState', 'failedNum')                    //将 failedNum ++    
                    else:                                                           //完成
                        state = self.move_base.get_state()                          //得到完成状态
                        if state == GoalStatus.SUCCEEDED:                           //状态成功
                            rospy.loginfo('Goal succeeded!')
                            self.r.hincrby('GoalState', 'successNum')               //successNum ++
                        else:
                            rospy.logwarn('Goal failed, state:' + str(state))       //失败
                            self.r.hincrby('GoalState', 'failedNum')
                
            else:                                                                   //无目标状态
                rospy.loginfo('all Goales have been finished')
                self.r.hset('GoalState', 'currentState', 'stopped')                 //将表GoalState中currentState 设置为 stopped
            rospy.sleep(int(self.r.hget('GoalState', 'intervalTime')))

    
    def robot_pose_callback(self, msg):
        x = msg.position.x                                                          //三维坐标
        y = msg.position.y
        z = msg.position.z
        qx = msg.orientation.x                                                      //四元素
        qy = msg.orientation.y
        qz = msg.orientation.z
        qw = msg.orientation.w
        attr_dict = {
            'x': '%f' % x,
            'y': '%f' % y,
            'z': '%f' % z,
            'qx': '%f' % qx,
            'qy': '%f' % qy,
            'qz': '%f' % qz,
            'qw': '%f' % qw }
        self.r.hmset('CurrentRobotPose', attr_dict)

    
    def shutdown(self):
        rospy.loginfo('Stopping the robot...')
        self.move_base.cancel_goal()
        rospy.sleep(2)