CancelGoalListen.py

最新推荐文章于 2024-08-12 15:48:42 发布

这个昵称已已已存在

最新推荐文章于 2024-08-12 15:48:42 发布

阅读量181

点赞数

分类专栏： rviz ros 文章标签：自动驾驶

本文链接：https://blog.youkuaiyun.com/qq_38212787/article/details/105688083

版权

ros 同时被 2 个专栏收录

17 篇文章

订阅专栏

rviz

4 篇文章

订阅专栏

#!/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 CancelGoalListen:
    
    def __init__(self):
        rospy.init_node('cancel_goal_listen', 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(60))
        rospy.loginfo('Connected to move base server')
        rospy.loginfo('Starting cancel_goal_listen')
        self.cancel_pub = rospy.Publisher('move_base/cancel', GoalID, queue_size = 10)
        self.r = redis.Redis(host = '127.0.0.1', port = 6379, db = 0)
        self.lastState = 'stopped'
        while not rospy.is_shutdown():
            mycurrentState = self.r.hget('GoalState', 'currentState')           //返回对应的状态
            if mycurrentState == 'stopped' and self.lastState == 'running':
                mycurrentGoal = self.r.hget('GoalState', 'currentGoal')         //当前的目标
                mygoalQueue = self.r.hget('GoalState', 'goalQueue')             //多少个目标
                mymode = self.r.hget('GoalState', 'mode')                       //模式
                if mymode == 'loop':
                    self.r.rpop(mygoalQueue)                                    //弹出队列中最后一个数据  redis语法
                self.r.lpush(mygoalQueue, mycurrentGoal)                        //mygoalQueue插入当前目标到当前队列的前面
                rospy.loginfo('cancel current goal.')
                goalId = GoalID()
                self.cancel_pub.publish(goalId)
            self.lastState = mycurrentState                                     //将当前的状态放在last
            rospy.sleep(1)

    
    def shutdown(self):
        rospy.loginfo('Stopping the robot...')
        self.move_base.cancel_goal()                                            //清除目标
        rospy.sleep(2)