00001 # Copyright (c) 2009, Willow Garage, Inc. 00002 # All rights reserved. 00003 # 00004 # Redistribution and use in source and binary forms, with or without 00005 # modification, are permitted provided that the following conditions are met: 00006 # 00007 # * Redistributions of source code must retain the above copyright 00008 # notice, this list of conditions and the following disclaimer. 00009 # * Redistributions in binary form must reproduce the above copyright 00010 # notice, this list of conditions and the following disclaimer in the 00011 # documentation and/or other materials provided with the distribution. 00012 # * Neither the name of the Willow Garage, Inc. nor the names of its 00013 # contributors may be used to endorse or promote products derived from 00014 # this software without specific prior written permission. 00015 # 00016 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00017 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00018 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00019 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00020 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00021 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00022 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00023 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00024 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00025 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00026 # POSSIBILITY OF SUCH DAMAGE. 00027 00028 # Author: Alexander Sorokin. 00029 # Based on C++ goal_id_generator.h/cpp 00030 import rospy 00031 00032 import actionlib_msgs.msg 00033 from actionlib import goal_id_generator 00034 00035 00036 class StatusTracker: 00037 """ 00038 * @class StatusTracker 00039 * @brief A class for storing the status of each goal the action server 00040 * is currently working on 00041 """ 00042 00043 def __init__(self, goal_id=None, status=None, goal=None): 00044 """ 00045 @brief create status tracker. Either pass goal_id and status OR goal 00046 """ 00047 self.goal = None 00048 self.handle_tracker = None 00049 self.status = actionlib_msgs.msg.GoalStatus() 00050 00051 self.handle_destruction_time = rospy.Time() 00052 00053 self.id_generator = goal_id_generator.GoalIDGenerator() 00054 00055 if goal_id: 00056 # set the goal id and status appropriately 00057 self.status.goal_id = goal_id 00058 self.status.status = status 00059 else: 00060 self.goal = goal 00061 self.status.goal_id = goal.goal_id 00062 00063 # initialize the status of the goal to pending 00064 self.status.status = actionlib_msgs.msg.GoalStatus.PENDING 00065 00066 # if the goal id is zero, then we need to make up an id for the goal 00067 if self.status.goal_id.id == "": 00068 self.status.goal_id = self.id_generator.generate_ID() 00069 00070 # if the timestamp of the goal is zero, then we'll set it to now() 00071 if self.status.goal_id.stamp == rospy.Time(): 00072 self.status.goal_id.stamp = rospy.Time.now()