action_result_db.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 #
00003 # This script stores actionlib goals and results by MongoDB
00004 # And store the current robot joints when recieve result.
00005 #
00006 # table configuration is bottom of this script
00007 #
00008 
00009 # parameters:
00010 #    joint_tolerance: store the past joint_states (sec)
00011 
00012 import rospy
00013 import tf
00014 
00015 import std_msgs.msg
00016 import actionlib_msgs.msg
00017 import sensor_msgs.msg
00018 from sensor_msgs.msg import JointState
00019 
00020 from mongodb_store.message_store import MessageStoreProxy
00021 
00022 
00023 class ActionResultDB(object):
00024     loaded_types = []
00025     useless_types = ['std_msgs/Header'] # message types not but action (goal|result)
00026     subscribers = {} # topicname:subscriber
00027 
00028     def __init__(self): # TODO
00029         self.db_name = rospy.get_param('robot/database','jsk_robot_lifelog')
00030         try:
00031             self.col_name = rospy.get_param('robot/name')
00032         except KeyError as e:
00033             rospy.logerr("please specify param \"robot/name\" (e.g. pr1012, olive)")
00034             exit(1)
00035         self.update_cycle = rospy.get_param('~update_cycle', 1.0)
00036 
00037         self.joint_tolerance = 1.0
00038         self.joint_states = []
00039         self.joint_states_inserted = [] # list of time stamp
00040         self.joint_sub = rospy.Subscriber('/joint_states', JointState, self._joint_states_cb)
00041 
00042         self._load_params()
00043 
00044         self.msg_store = MessageStoreProxy(database=self.db_name, collection=self.col_name)
00045         rospy.loginfo("connected to %s.%s" % (self.db_name, self.col_name))
00046 
00047     def _load_params(self):
00048         try:
00049             self.action_name_white_list = rospy.get_param('~white_list')['name']
00050             rospy.loginfo("whitelist_name: %s", self.action_name_white_list)
00051         except:
00052             self.action_name_white_list = None
00053         try:
00054             self.action_name_black_list = rospy.get_param('~black_list')['name']
00055             rospy.loginfo("blacklist_name: %s", self.action_name_black_list)
00056         except:
00057             self.action_name_black_list = None
00058         try:
00059             self.action_type_white_list = rospy.get_param('~white_list')['type']
00060             rospy.loginfo("whitelist_type: %s", self.action_type_white_list)
00061         except:
00062             self.action_type_white_list = None
00063         try:
00064             self.action_type_black_list = rospy.get_param('~black_list')['type']
00065             rospy.loginfo("blacklist_name: %s", self.action_type_black_list)
00066         except:
00067             self.action_type_black_list = None
00068 
00069     # callback functions
00070     def _action_goal_cb(self, topic, type_name, msg):
00071         self._insert_action_goal(topic,type_name,msg)
00072 
00073     def _action_result_cb(self, topic, type_name, msg):
00074         self._insert_action_result(topic,type_name,msg)
00075         key_func = (lambda js: abs(js.header.stamp-msg.header.stamp))
00076         nearest_state = min(self.joint_states,key=key_func)
00077         self._insert_joint_states(nearest_state)
00078 
00079     def _joint_states_cb(self, msg):
00080         self.joint_states = [m for m in self.joint_states if (msg.header.stamp - m.header.stamp).to_sec() < self.joint_tolerance] + [msg]
00081         self.joint_states_inserted = [s for s in self.joint_states_inserted if (msg.header.stamp - s).to_sec() < self.joint_tolerance]
00082 
00083     # insert functions
00084     def _insert_action_goal(self,topic,type_name,msg):
00085         try:
00086             res = self.msg_store.insert(msg)
00087             rospy.loginfo("inserted action_goal message: %s (%s) -> %s", topic, type_name, res)
00088         except Exception as e:
00089             rospy.logerr("failed to insert action goal: %s (%s) -> %s", topic, type_name, e)
00090 
00091     def _insert_action_result(self,topic,type_name,msg):
00092         try:
00093             res = self.msg_store.insert(msg)
00094             rospy.loginfo("inserted action_result message: %s (%s) -> %s", topic, type_name, res)
00095         except Exception as e:
00096             rospy.logerr("failed to insert action result: %s (%s) -> %s", topic, type_name, e)
00097     
00098 
00099     def _insert_joint_states(self, msg):
00100         try:
00101             if msg.header.stamp in self.joint_states_inserted:
00102                 return
00103             res = self.msg_store.insert(msg)
00104             rospy.loginfo("inserted joint_states message: %s", res)
00105         except Exception as e:
00106             rospy.logerr("failed to insert joint states: %s", e)
00107 
00108     # if the message type is goal or result, return the callback
00109     def _message_callback_type(self,name,type_name,type_obj):
00110         if not hasattr(type_obj,'header'): return None # no header message
00111         if type(type_obj.header) != std_msgs.msg.Header: return None # custom header message
00112         if hasattr(type_obj,'goal_id') and hasattr(type_obj,'goal') and type(type_obj.goal_id) == actionlib_msgs.msg.GoalID:
00113             return (lambda msg: self._action_goal_cb(name,type_name,msg))
00114         if hasattr(type_obj,'status') and hasattr(type_obj,'result') and type(type_obj.status) == actionlib_msgs.msg.GoalStatus:
00115             return (lambda msg: self._action_result_cb(name,type_name,msg))
00116         else:
00117             return None
00118 
00119     def sleep_one_cycle(self):
00120         rospy.sleep(self.update_cycle)
00121 
00122     # subscriber updater
00123     def update_subscribers(self):
00124         # check new publishers
00125         topics = rospy.client.get_published_topics()
00126         if self.action_name_white_list:
00127             topics = [t for t in topics if t[0] in self.action_name_white_list]
00128         if self.action_type_white_list:
00129             topics = [t for t in topics if t[1].split('/')[1] in self.action_type_white_list]
00130         if self.action_name_black_list:
00131             topics = [t for t in topics if not t[0] in self.action_name_black_list]
00132         if self.action_type_black_list:
00133             topics = [t for t in topics if not t[1].split('/')[1] in self.action_type_black_list]
00134 
00135         for topic_name, topic_type in topics:
00136             (pkg_name, msg_type)  = topic_type.split('/')
00137             py_topic_class = '%s.msg.%s' % (pkg_name, msg_type)
00138             if py_topic_class in self.useless_types:
00139                 continue
00140             if topic_name in self.subscribers.keys():
00141                 continue
00142 
00143             # Import and Check
00144             try:
00145                 pypkg = __import__('%s.msg' % pkg_name)
00146                 rospy.loginfo('imported %s.msg', pkg_name)
00147             except Exception as e:
00148                 rospy.logerr('failed to import %s.msg: %s', pkg_name, e)
00149                 rospy.logerr('please catkin_make %s', pkg_name)
00150                 self.useless_types += [py_topic_class]
00151                 continue
00152 
00153             try:
00154                 type_class = getattr(getattr(pypkg, 'msg'), msg_type)
00155                 type_instance = type_class()
00156             except Exception as e:
00157                 rospy.logerr('failed to instantiate %s.msg.%s: %s', pkg_name, msg_type, e)
00158                 self.useless_types += [py_topic_class]
00159                 continue
00160 
00161             try:
00162                 cb_obj = self._message_callback_type(topic_name, msg_type, type_instance)
00163                 if cb_obj == None:
00164                     self.useless_types += [py_topic_class]
00165                     continue
00166                 
00167                 self.subscribers[topic_name] = rospy.Subscriber(topic_name, type_class, cb_obj)
00168                 rospy.loginfo("start subscribe (topic=%s type=%s)", topic_name, msg_type)
00169             except Exception as e:
00170                 self.useless_types += [py_topic_class]
00171                 rospy.logerr('error registering subscriber: %s', e)
00172                 continue
00173 
00174 
00175 if __name__ == "__main__":
00176     rospy.init_node('action_result_db')
00177     obj = ActionResultDB()
00178 
00179     while not rospy.is_shutdown():
00180         obj.update_subscribers()
00181         obj.sleep_one_cycle()


jsk_robot_startup
Author(s):
autogenerated on Sat Jul 1 2017 02:42:18