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)