$search
00001 #!/usr/bin/python 00002 # -*- coding: utf-8 -*- 00003 ################################################################# 00004 ##\file 00005 # 00006 # \note 00007 # Copyright (c) 2011 \n 00008 # Cardiff University \n\n 00009 # 00010 ################################################################# 00011 # 00012 # \note 00013 # Project name: Multi-Role Shadow Robotic System for Independent Living 00014 # \note 00015 # ROS stack name: srs 00016 # \note 00017 # ROS package name: srs_states 00018 # 00019 # \author 00020 # Author: Renxi Qiu, email: renxi.qiu@gmail.com 00021 # 00022 # \date Date of creation: Oct 2011 00023 # 00024 # \brief 00025 # information shared by the SRS components 00026 # 00027 ################################################################# 00028 # 00029 # Redistribution and use in source and binary forms, with or without 00030 # modification, are permitted provided that the following conditions are met: 00031 # 00032 # - Redistributions of source code must retain the above copyright 00033 # notice, this list of conditions and the following disclaimer. \n 00034 # 00035 # - Redistributions in binary form must reproduce the above copyright 00036 # notice, this list of conditions and the following disclaimer in the 00037 # documentation and/or other materials provided with the distribution. \n 00038 # 00039 # This program is free software: you can redistribute it and/or modify 00040 # it under the terms of the GNU Lesser General Public License LGPL as 00041 # published by the Free Software Foundation, either version 3 of the 00042 # License, or (at your option) any later version. 00043 # 00044 # This program is distributed in the hope that it will be useful, 00045 # but WITHOUT ANY WARRANTY; without even the implied warranty of 00046 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00047 # GNU Lesser General Public License LGPL for more details. 00048 # 00049 # You should have received a copy of the GNU Lesser General Public 00050 # License LGPL along with this program. 00051 # If not, see <http://www.gnu.org/licenses/>. 00052 # 00053 ################################################################# 00054 # ROS imports 00055 import os, sys 00056 00057 import roslib 00058 roslib.load_manifest('srs_states') 00059 import rospy 00060 import smach 00061 import threading 00062 import tf 00063 from std_msgs.msg import String, Bool, Int32 00064 from geometry_msgs.msg import * 00065 00066 00067 """ 00068 current_task_info is a global shared memory for exchanging task information among statuses 00069 00070 smach is slow on passing large amount of userdata. Hence they are stored under goal_structure as global variable 00071 00072 srs_dm_action perform one task at a time and maintain a unique session id. 00073 """ 00074 00075 listener = tf.TransformListener() 00076 00077 class goal_structure(): 00078 00079 def __init__(self): 00080 00081 #goal of the high level task, replaced by self.task_feedback 00082 self.task_name ="" 00083 00084 #task parameter, replaced by self.task_feedback 00085 self.task_parameter="" 00086 00087 00088 # contain task_id, task_initializer, task_initializer_type, task_name, task_parameter, task_schedule, action_object, action_object_parent 00089 # defined in srs_decision_making json_parser.py 00090 self.task_feedback="" 00091 00092 00093 ## by Ze 00094 #self.parameters = list() 00095 #complete json parameters come from user 00096 self.json_parameters = '' 00097 00098 #Information about last step, use Last_step_info_msg 00099 self.last_step_info = list() 00100 00101 #customised pre-empty signal received or not 00102 self.preemptied = False 00103 00104 #reference to the action server 00105 self._srs_as="" 00106 00107 ## backward compatible need to be revised after the integration meeting 00108 #feedback publisher, intervention required 00109 self.pub_fb = rospy.Publisher('fb_executing_solution', Bool) 00110 #feedback publisher, operational state 00111 self.pub_fb2 = rospy.Publisher('fb_executing_state', String) 00112 ## backward compatible need to be revised after the integration meeting 00113 00114 self.object_in_hand = False 00115 00116 self.object_identified = False 00117 00118 #True already adjusted, False not adjusted yet 00119 self.post_grasp_adjusted = False 00120 00121 00122 # this can be replaced by the tray service with real robot 00123 self.object_on_tray = False 00124 00125 self.arm_folded_ready_for_transfer = False #arm in hold or folded position 00126 00127 self.lock = threading.Lock() 00128 00129 self.customised_preempt_required = False 00130 00131 self.customised_preempt_acknowledged = False 00132 00133 self.pause_required = False 00134 00135 self.stop_required = False 00136 00137 self.stop_acknowledged = False 00138 00139 self.language_set = '' 00140 00141 try: 00142 self.language_set = rospy.get_param("/srs/language") 00143 except Exception, e: 00144 self.language_set = 'English' 00145 00146 #try reading the parameter server 00147 00148 self.speaking_language = dict() 00149 self.feedback_messages = dict() 00150 00151 if self.language_set.lower() == 'italian': 00152 self.speaking_language['Grasp']= "Ora sto afferrare il " 00153 self.speaking_language['Search']= "Ora sto cercando il " 00154 self.speaking_language['Navigation']= "Ora mi sto muovendo" 00155 self.speaking_language['Stop']= "Ora sto fermare l attività corrente " 00156 self.speaking_language['Pause']= "Ora sto pausa l attività corrente" 00157 self.speaking_language['Resume']= "Ora sto riprendendo l'attività corrente" 00158 self.speaking_language['Preempted']= "Ora vengo fermato da un altro task con priorità maggiore" 00159 00160 self.feedback_messages['sm_srs_grasp']= "Greifen Sie das Objekt" 00161 self.feedback_messages['sm_put_object_on_tray']= "Legen Sie das Objekt auf dem Tablett" 00162 self.feedback_messages['sm_enviroment_update']= "Umgebung zu aktualisieren" 00163 self.feedback_messages['sm_srs_detection']= "Auf der Suche nach Objekt" 00164 self.feedback_messages['sm_srs_navigation']= "Umzug nach Ziel" 00165 self.feedback_messages['initialise']='Initialisierung Roboter' 00166 00167 elif self.language_set.lower()== 'german': 00168 self.speaking_language['Grasp']= "Jetzt bin ich Ergreifen der " 00169 self.speaking_language['Search']= "Jetzt suche ich die " 00170 self.speaking_language['Navigation']= "Jetzt bin ich bewegt " 00171 self.speaking_language['Stop']= "Jetzt bin ich Anhalten der aktuellen Aufgabe " 00172 self.speaking_language['Pause']= "Jetzt bin ich Anhalten der aktuellen Aufgabe" 00173 self.speaking_language['Resume']= "Jetzt bin ich die Wiederaufnahme der aktuellen Aufgabe" 00174 self.speaking_language['Preempted']= "Jetzt bin ich von einer anderen Task mit höherer Priorität unterbrochen" 00175 00176 self.feedback_messages['sm_srs_grasp']= "Cogliere l'oggetto" 00177 self.feedback_messages['sm_put_object_on_tray']= "Mettere l'oggetto sul vassoio" 00178 self.feedback_messages['sm_enviroment_update']= "Ambiente aggiornamento" 00179 self.feedback_messages['sm_srs_detection']= "Ricerca di oggetti" 00180 self.feedback_messages['sm_srs_navigation']= "Spostamento di destinazione" 00181 self.feedback_messages['initialise']='inizializzazione robot' 00182 00183 else: 00184 #default language is English 00185 self.speaking_language['Grasp']= "Now I am grasping the " 00186 self.speaking_language['Search']= "Now I am searching the " 00187 self.speaking_language['Navigation']= "Now I am moving " 00188 self.speaking_language['Stop']= "Now I am stopping the current task " 00189 self.speaking_language['Pause']= "Now I am pausing the current task " 00190 self.speaking_language['Resume']= "Now I am resuming the current task " 00191 self.speaking_language['Preempt']= "Now I am stopped by another task with higher priority" 00192 00193 self.feedback_messages['sm_srs_grasp']= "Grasping the object" 00194 self.feedback_messages['sm_put_object_on_tray']= "Put the object on the tray" 00195 self.feedback_messages['sm_enviroment_update']= "Environment update" 00196 self.feedback_messages['sm_srs_detection']= "Searching for object" 00197 self.feedback_messages['sm_srs_navigation']= "Moving to target" 00198 self.feedback_messages['initialise']='initialising robot' 00199 00200 00201 00202 def get_post_grasp_adjustment_state(self): 00203 self.lock.acquire() 00204 #True already adjusted, False not adjusted yet 00205 value = self.post_grasp_adjusted 00206 self.lock.release() 00207 return value 00208 00209 def set_post_grasp_adjustment_state(self, value): 00210 self.lock.acquire() 00211 #True already adjusted, False not adjusted yet 00212 self.post_grasp_adjusted = value 00213 self.lock.release() 00214 00215 def get_object_identification_state(self): 00216 self.lock.acquire() 00217 value = self.object_identified 00218 self.lock.release() 00219 return value 00220 00221 def set_object_identification_state(self, value): 00222 self.lock.acquire() 00223 self.object_identified = value 00224 self.lock.release() 00225 00226 00227 def get_pause_required(self): 00228 self.lock.acquire() 00229 value = self.pause_required 00230 self.lock.release() 00231 return value 00232 00233 def get_customised_preempt_required(self): 00234 self.lock.acquire() 00235 value = self.customised_preempt_required 00236 self.lock.release() 00237 return value 00238 00239 def get_customised_preempt_acknowledged(self): 00240 self.lock.acquire() 00241 value = self.customised_preempt_acknowledged 00242 self.lock.release() 00243 return value 00244 00245 def get_stop_required(self): 00246 self.lock.acquire() 00247 value = self.stop_required 00248 self.lock.release() 00249 return value 00250 00251 def get_stop_acknowledged(self): 00252 self.lock.acquire() 00253 value = self.stop_acknowledged 00254 self.lock.release() 00255 return value 00256 00257 def set_pause_required(self,value): 00258 self.lock.acquire() 00259 self.pause_required = value 00260 self.lock.release() 00261 00262 def set_customised_preempt_required(self,value): 00263 self.lock.acquire() 00264 self.customised_preempt_required = value 00265 self.lock.release() 00266 00267 00268 def set_customised_preempt_acknowledged(self, value): 00269 self.lock.acquire() 00270 self.customised_preempt_acknowledged = value 00271 self.lock.release() 00272 00273 00274 def set_stop_required(self, value): 00275 self.lock.acquire() 00276 self.stop_required = value 00277 self.lock.release() 00278 00279 def set_stop_acknowledged(self, value): 00280 self.lock.acquire() 00281 self.acknowledged = value 00282 self.lock.release() 00283 00284 #checking if the current atomic operation can be stopped or not 00285 #true can be stopped in the middle 00286 def stopable(self): 00287 #List of conditions which robot should not be stopped in the middle 00288 self.lock.acquire() 00289 #condition 1: 00290 #If a object has been grasped, operation should not be stopped until the object is released or arm is folded ready for transfer 00291 if self.object_in_hand and not self.arm_folded_ready_for_transfer: 00292 outcome = False 00293 else: 00294 outcome = True 00295 self.lock.release() 00296 00297 return outcome 00298 00299 00300 def reset(self): 00301 00302 self.__init__() 00303 gc.collect() 00304 00305 def get_robot_pos(self): 00306 try: 00307 #listener.waitForTransform("/map", "/base_link", rospy.Time(0), rospy.Duration(4.0)) 00308 (trans,rot) = listener.lookupTransform("/map", "/base_link", rospy.Time(0)) 00309 except rospy.ROSException, e: 00310 print "Transformation not possible: %s"%e 00311 return None 00312 00313 rb_pose = Pose2D() 00314 rb_pose.x = trans[0] 00315 rb_pose.y = trans[1] 00316 rb_pose_rpy = tf.transformations.euler_from_quaternion(rot) 00317 rb_pose.theta = rb_pose_rpy[2] 00318 00319 return rb_pose 00320 00321 00322 00323 current_task_info = goal_structure()