srs_actions_server.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 #################################################################
00003 ##\file
00004 #
00005 # \note
00006 # Copyright (c) 2011 \n
00007 # Cardiff University \n\n
00008 #
00009 #################################################################
00010 #
00011 # \note
00012 # Project name: Multi-Role Shadow Robotic System for Independent Living
00013 # \note
00014 # ROS stack name: srs
00015 # \note
00016 # ROS package name: srs_decision_making
00017 #
00018 # \author
00019 # Author: Renxi Qiu, email: renxi.qiu@gmail.com
00020 #
00021 # \date Date of creation: Oct 2011
00022 #
00023 # \brief
00024 # Task coordination and interfacing for SRS decision making
00025 #
00026 #################################################################
00027 #
00028 # Redistribution and use in source and binary forms, with or without
00029 # modification, are permitted provided that the following conditions are met:
00030 #
00031 # - Redistributions of source code must retain the above copyright
00032 # notice, this list of conditions and the following disclaimer. \n
00033 #
00034 # - Redistributions in binary form must reproduce the above copyright
00035 # notice, this list of conditions and the following disclaimer in the
00036 # documentation and/or other materials provided with the distribution. \n
00037 #
00038 # This program is free software: you can redistribute it and/or modify
00039 # it under the terms of the GNU Lesser General Public License LGPL as
00040 # published by the Free Software Foundation, either version 3 of the
00041 # License, or (at your option) any later version.
00042 #
00043 # This program is distributed in the hope that it will be useful,
00044 # but WITHOUT ANY WARRANTY; without even the implied warranty of
00045 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
00046 # GNU Lesser General Public License LGPL for more details.
00047 #
00048 # You should have received a copy of the GNU Lesser General Public
00049 # License LGPL along with this program.
00050 # If not, see <http://www.gnu.org/licenses/>.
00051 #
00052 #################################################################
00053 
00054 import roslib; roslib.load_manifest('srs_decision_making')
00055 
00056 #roslib.load_manifest('knowledge_ros_service')
00057 
00058 import rospy
00059 import smach
00060 import smach_ros
00061 import sys, os, traceback, optparse, time, string, threading
00062 from actionlib import *
00063 from actionlib.msg import *
00064 from smach import Iterator, StateMachine, CBState
00065 from smach_ros import ConditionState, IntrospectionServer
00066 import unicodedata
00067 
00068 from srs_knowledge.srv import *
00069 from srs_knowledge.msg import *
00070 
00071 
00072 #from cob_tray_sensors.srv import *
00073 
00074 import util.json_parser as json_parser
00075 
00076 """
00077 smach introspection server not working in electric yet, modify the executive_smach/smach_msgs/msg/SmachContainerStatus.msg below can bypass error:
00078 
00079 # A pickled user data structure
00080 string local_data
00081 to:
00082 # A pickled user data structure
00083 uint8[] local_data
00084 
00085 and
00086 
00087 --- a/smach_ros/src/smach_ros/introspection.py  Tue Aug 02 10:31:19 2011 -0700
00088 +++ b/smach_ros/src/smach_ros/introspection.py  Thu Sep 29 22:31:48 2011 +0900
00089 @@ -219,7 +219,7 @@
00090                      path,
00091                      self._container.get_initial_states(),
00092                      self._container.get_active_states(),
00093 -                    pickle.dumps(self._container.userdata._data,2),
00094 +                    "",
00095                      info_str)
00096              # Publish message
00097              self._status_pub.publish(state_msg)
00098 """
00099 from srs_configuration_statemachines import *
00100             
00101 """
00102                 sub-statemachines in use:
00103                 ###########################################
00104                 sm_approach_pose_assisted()
00105                 #assisted navigation, operator could specify intermediate position for final goal
00106                
00107                 sm_detect_asisted_pose()
00108                 #operator or KB could adjust the scanning pose 
00109                 
00110                 sm_open_door()
00111                 #open door from IPA example
00112               
00113                 sm_pick_object_basic()
00114                 #basic routine for pick object from IPA example
00115 
00116                 sm_detect_asisted_region()
00117                 #operator or KB could specify region of interest
00118                 #require generic state detect_object()taking key_region as input_key
00119                 
00120                 sm_detect_asisted_pose_region()
00121                 #detect object, both base pose and region can be adjusted 
00122                 #require generic state detect_object()taking key_region as input_key
00123                 
00124                 sm_pick_object_asisted()
00125                 #pick object with user intervention for error handling
00126                 #require generic state grasp_general()taking grasp_conf as input_key
00127                 
00128                 sm_enviroment_object_update()
00129                 #updating the environment and find object
00130                 #require generic state verify_object() and update_env_model()
00131                 
00132                 ##########################################            
00133                 
00134                 states need to be developed within SRS
00135                 ##########################################
00136                 detect_object()
00137                 #taking key_region as input_key
00138                 #from srs_asisted_detect module
00139                 
00140                 grasp_general()
00141                 #taking grasp_conf as input_key
00142                 #from srs_asisted_grasp module
00143                 
00144                 verify_object() 
00145                 # model fitting from PRO
00146 
00147                 
00148                 update_env_model()
00149                 # update environment model from IPA
00150                 ##########################################                     
00151 """
00152 
00153 
00154 
00155 #Customised Action sever designed for overwriting SimpleActionServer pre-empty sequence
00156 class SRSActionServer(SimpleActionServer):
00157     def _init_(self, name, ActionSpec, execute_cb, auto_start):
00158         super(SRSActionServer, self).__init__(name, ActionSpec, execute_cb, auto_start)       
00159 
00160     def take_next_goal(self):
00161         #accept the next goal
00162         rospy.loginfo("Receiving a new goal")
00163         self.current_goal = self.next_goal
00164         self.new_goal = False
00165         #set preempt to request to equal the preempt state of the new goal
00166         rospy.loginfo("preempt_request %s", self.preempt_request)
00167         rospy.loginfo("new_goal preempt_request %s", self.new_goal_preempt_request)
00168         self.preempt_request = self.new_goal_preempt_request
00169         self.new_goal_preempt_request = False
00170         #set the status of the current goal to be active
00171         self.current_goal.set_accepted("This goal has been accepted by the simple action server");
00172         return self.current_goal.get_goal()
00173 
00174     
00175     def accept_new_goal(self):
00176         with self.lock:
00177             if not self.new_goal or not self.next_goal.get_goal():
00178                 rospy.loginfo("Attempting to accept the next goal when a new goal is not available");
00179                 return None;
00180  
00181              #check if we need to send a preempted message for the goal that we're currently pursuing
00182             if self.is_active() and self.current_goal.get_goal() and self.current_goal != self.next_goal:
00183                  #self.current_goal.set_canceled(None, "This goal was cancelled because another goal was received by the simple action server");
00184                  self.next_goal.set_rejected(None, "This goal was rejected because the server is busy with another task")
00185                  rospy.loginfo("Postpone a new goal")
00186             else:
00187                 #accept the next goal
00188                 rospy.loginfo("Receiving a new goal")
00189                 self.current_goal = self.next_goal
00190                 self.new_goal = False
00191                 #set preempt to request to equal the preempt state of the new goal
00192                 rospy.loginfo("preempt_request %s", self.preempt_request)
00193                 rospy.loginfo("new_goal preempt_request %s", self.new_goal_preempt_request)
00194                 
00195                 self.preempt_request = self.new_goal_preempt_request
00196                 self.new_goal_preempt_request = False
00197                 #set the status of the current goal to be active
00198                 self.current_goal.set_accepted("This goal has been accepted by the simple action server");
00199                 
00200  
00201             return self.current_goal.get_goal()
00202         
00203         
00204         """
00205         with self.lock:
00206             if not self.new_goal or not self.next_goal.get_goal():
00207                 rospy.loginfo("Attempting to accept the next goal when a new goal is not available");
00208             
00209             global current_task_info
00210             
00211             _result = xmsg.ExecutionResult()
00212             _result.return_value=3
00213             
00214             
00215             #check if we need to send a preempted message for the goal that we're currently pursuing
00216             if self.is_active() and self.current_goal.get_goal() and self.current_goal != self.next_goal:
00217                 #self.current_goal.set_canceled(None, "This goal was cancelled because another goal was received by the simple action server");
00218                 self.next_goal.set_rejected(None, "This goal was rejected because the server is busy with another task")
00219                 rospy.loginfo("Postpone a new goal")  
00220        
00221             elif self.next_goal.get_goal().action == 'pause':
00222                 current_task_info.set_pause_required(True) 
00223                 self.next_goal.set_accepted("")
00224                 self.next_goal.set_succeeded(_result, 'pause acknowledged')
00225 
00226             elif self.next_goal.get_goal().action == 'resume':
00227                 if current_task_info.get_pause_required() == True:   # already paused
00228                     current_task_info.set_pause_required(False)
00229                     self.next_goal.set_accepted("")
00230                     self.next_goal.set_succeeded(_result, 'resume acknowledged')
00231                 else:
00232                     _result.return_value = 2
00233                     self.next_goal.set_aborted(_result, 'not paused, no need for resuming')
00234                     pass 
00235 
00236             else:
00237                 if not self.current_goal.get_goal():
00238                     # current goal is null
00239                     return self.take_next_goal()
00240                 if self.next_goal.get_goal().action == 'stop':
00241                     current_task_info.set_stop_required(True) 
00242                     return self.take_next_goal()
00243                 if self.next_goal.get_goal().priority > self.current_goal.get_goal().priority:                             
00244                     current_task_info.set_customised_preempt_required(True)   
00245                     return self.take_next_goal()
00246             
00247             return None
00248         """
00249 
00250          
00251 class SRS_DM_ACTION(object):
00252     #action server for taking tasks 
00253     def __init__(self, name):
00254 
00255         self._action_name = name
00256         self._as = SRSActionServer(self._action_name, xmsg.ExecutionAction, self.execute_cb, auto_start=False) 
00257         self._feedback = xmsg.ExecutionFeedback()
00258         self._result = xmsg.ExecutionResult()
00259         self._task = ""
00260         self._parameter = ""
00261         self._as.start()
00262         self.robot_initialised= False
00263          
00264                 
00265         #self._as.register_goal_callback(self.goal_cb)
00266         self._as.register_preempt_callback(self.priority_cb)
00267         
00268         rospy.loginfo("Waiting for wake up the server ...")
00269         
00270     #processing manual command
00271     #move("torso",[[tilt1, pan, tilt2]])
00272     #as well as 
00273     #move("torso", "front")
00274     #move("head", "back")
00275     #move("tray", "home")
00276     def process_manual_command (self, the_task):
00277         
00278         #possible components and positions more details in cob_robots
00279         pre_positions = dict()
00280         pre_positions['torso'] = ['home','left','right','back','front','nod','bow','shake']
00281         pre_positions['head'] = ['front','back']
00282         pre_positions['tray'] = ['up','down']
00283         #default outcome
00284         outcome = 'task_failed'
00285               
00286         global sss
00287         # only processing manual command for known component
00288         if 'predefined_pose' in the_task['destination']:
00289             #move tor
00290             if the_task['component'] == 'torso' and the_task['destination']['predefined_pose'] in pre_positions['torso']:
00291                 handle = sss.move(the_task['component'], str(the_task['destination']['predefined_pose']), False)
00292                 handle.wait()
00293                 outcome = 'task_succeeded'
00294             #move head
00295             if the_task['component'] == 'head'and the_task['destination']['predefined_pose'] in pre_positions['head']:  
00296                 handle = sss.move(the_task['component'], str(the_task['destination']['predefined_pose']), False)
00297                 handle.wait()
00298                 outcome = 'task_succeeded'
00299             #move tray
00300             if the_task['component'] == 'tray' and the_task['destination']['predefined_pose'] in pre_positions['tray']:      
00301                 handle = sss.move(the_task['component'], str(the_task['destination']['predefined_pose']), False)
00302                 handle.wait()
00303                 outcome = 'task_succeeded'                
00304                         
00305         if the_task['component'] == 'torso' and 'torso_pose' in the_task['destination'] :
00306             if 'tilt1' in the_task['destination']['torso_pose'] and 'tilt2' in the_task['destination']['torso_pose'] and 'pan' in the_task['destination']['torso_pose']: 
00307                 target = list()             
00308                 target = [[the_task['destination']['torso_pose']['tilt1'], the_task['destination']['torso_pose']['pan'], the_task['destination']['torso_pose']['tilt2'] ]]            
00309                 handle = sss.move(the_task['component'], target, False)
00310                 handle.wait()  
00311                 outcome = 'task_succeeded' 
00312                                
00313         if outcome == "task_succeeded": 
00314             self._result.return_value=3
00315             self._as.set_succeeded(self._result)
00316         else :
00317             self._result.return_value=4
00318             self._as.set_aborted(self._result)                
00319                 
00320         return
00321         
00322     def robot_initialisation_process(self):
00323         if not self.robot_initialised :
00324             #initialisation of the robot
00325             # move to initial positions
00326             global sss
00327             #handle_torso = sss.move("torso", "home", False)
00328             #handle_tray = sss.move("tray", "down", False)
00329             #handle_arm = sss.move("arm", "folded", False)
00330             #handle_sdh = sss.move("sdh", "cylclosed", False)
00331             #handle_head = sss.move("head", "front", False)
00332     
00333         
00334             # wait for initial movements to finish
00335             #handle_torso.wait()
00336             #handle_tray.wait()
00337             #handle_arm.wait()
00338             #handle_sdh.wait()
00339             #handle_head.wait()
00340             self.robot_initialised = True
00341             
00342         
00343 
00344         
00345     def init_sm(self):
00346         self.temp = smach.StateMachine(outcomes=['task_succeeded','task_aborted', 'task_preempted'])
00347         
00348         #
00349         self.temp.userdata.target_base_pose=Pose2D()
00350         self.temp.userdata.target_object_name=''
00351         self.temp.userdata.target_object_pose=Pose()
00352         self.temp.userdata.the_target_object_found = ''
00353         self.temp.userdata.verified_target_object_pose=Pose()
00354         self.temp.userdata.surface_distance = -1000
00355         
00356         #session id for current task, on id per task.
00357         #session id can be shared by different clients
00358         self.session_id = 0
00359         
00360         #user intervention is possible or not.
00361         #False, DM has to make all the decision by it self
00362         #True, suitable client has been connected, can be relied on
00363         self.temp.userdata.semi_autonomous_mode=True
00364         
00365         
00366      # open the container
00367         with self.temp:
00368             # add states to the container
00369             smach.StateMachine.add('INITIALISE', initialise(),
00370                                    transitions={'succeeded':'SEMANTIC_DM', 'failed':'task_aborted'})
00371                         
00372             smach.StateMachine.add('SEMANTIC_DM', semantic_dm(),
00373                                    transitions={'succeeded':'task_succeeded',
00374                                                 'failed':'task_aborted',
00375                                                 'preempted':'task_preempted',
00376                                                 'navigation':'SM_NAVIGATION',
00377                                                 'detection':'SM_DETECTION',
00378                                                 'simple_grasp':'SM_OLD_GRASP',
00379                                                 'full_grasp':'SM_NEW_GRASP',
00380                                                 'put_on_tray':'SM_PUT_ON_TRAY',
00381                                                 'env_update':'SM_ENV_UPDATE',
00382                                                 'reset_robot_after_impossible_task':'RESET_ROBOT_AFTER_IMPOSSIBLE_TASK'},
00383                                    remapping={'target_base_pose':'target_base_pose',
00384                                                'target_object_name':'target_object_name',
00385                                                'target_object_pose':'target_object_pose',
00386                                                'target_object_id':'target_object_id',
00387                                                'target_object':'the_target_object_found',
00388                                                'target_workspace_name':'target_workspace_name',
00389                                                'semi_autonomous_mode':'semi_autonomous_mode',
00390                                                'grasp_categorisation':'grasp_categorisation',
00391                                                'scan_pose_list':'scan_pose_list'})
00392             
00393             smach.StateMachine.add('SM_NAVIGATION', srs_navigation_operation(),
00394                                    transitions={'succeeded':'SEMANTIC_DM', 'not_completed':'CHECKING_USER_INTERVENTION', 'failed':'SEMANTIC_DM','stopped':'task_preempted','preempted':'task_preempted'},
00395                                    remapping={'target_base_pose':'target_base_pose',
00396                                                'semi_autonomous_mode':'semi_autonomous_mode'})
00397 
00398             smach.StateMachine.add('SM_DETECTION', srs_detection_operation(),
00399                                    transitions={'succeeded':'SEMANTIC_DM', 'not_completed':'CHECKING_USER_INTERVENTION', 'failed':'SEMANTIC_DM','stopped':'task_preempted','preempted':'task_preempted'},
00400                                    remapping={'target_object_name':'target_object_name',
00401                                               'target_object_id':'target_object_id',
00402                                               'target_workspace_name':'target_workspace_name',
00403                                               'semi_autonomous_mode':'semi_autonomous_mode',
00404                                               'target_object':'the_target_object_found',
00405                                               'target_object_pose':'target_object_pose' })
00406        
00407             smach.StateMachine.add('SM_NEW_GRASP', srs_grasp_operation(),
00408                                    transitions={'succeeded':'SEMANTIC_DM', 'not_completed':'CHECKING_USER_INTERVENTION', 'failed':'SEMANTIC_DM','stopped':'task_preempted','preempted':'task_preempted'},
00409                                    remapping={'target_object_name':'target_object_name',
00410                                               'target_object_id':'target_object_id',
00411                                               'target_workspace_name':'target_workspace_name',
00412                                               'semi_autonomous_mode':'semi_autonomous_mode',
00413                                               'target_object':'the_target_object_found',
00414                                               'target_object_pose':'target_object_pose',
00415                                               'grasp_categorisation':'grasp_categorisation',
00416                                               'surface_distance':'surface_distance'})
00417             
00418             '''
00419             START
00420             #Old grasp added for backward compatible, should be removed after knowledge service updated completely
00421             '''
00422             smach.StateMachine.add('SM_OLD_GRASP', srs_old_grasp_operation(),
00423                                    transitions={'succeeded':'SEMANTIC_DM', 'not_completed':'CHECKING_USER_INTERVENTION', 'failed':'SEMANTIC_DM','stopped':'task_preempted','preempted':'task_preempted'},
00424                                    remapping={'target_object_name':'target_object_name',
00425                                               'semi_autonomous_mode':'semi_autonomous_mode',
00426                                               'target_object_id':'target_object_id',
00427                                               'target_object':'the_target_object_found',
00428                                               'grasp_categorisation':'grasp_categorisation',
00429                                               'surface_distance':'surface_distance'})
00430             '''
00431             #Old grasp added for backward compatible, should be removed after knowledge service updated completely
00432             END
00433             '''
00434             
00435             smach.StateMachine.add('SM_PUT_ON_TRAY', srs_put_on_tray_operation(),
00436                                    transitions={'succeeded':'SEMANTIC_DM', 'not_completed':'CHECKING_USER_INTERVENTION', 'failed':'SEMANTIC_DM','stopped':'task_preempted','preempted':'task_preempted'},
00437                                    remapping={'grasp_categorisation':'grasp_categorisation',
00438                                               'surface_distance':'surface_distance' })
00439 
00440             smach.StateMachine.add('SM_ENV_UPDATE', srs_enviroment_update_operation(),
00441                                    transitions={'succeeded':'SEMANTIC_DM', 'not_completed':'CHECKING_USER_INTERVENTION', 'failed':'SEMANTIC_DM','stopped':'task_preempted','preempted':'task_preempted'},
00442                                    remapping={'target_object_pose':'target_object_pose',
00443                                               'target_object_hh_id':'target_object_hh_id',
00444                                               'verified_target_object_pose':'verified_target_object_pose'})
00445             
00446             smach.StateMachine.add('RESET_ROBOT_AFTER_IMPOSSIBLE_TASK', reset_robot(),
00447                                    transitions={'completed':'task_aborted', 'failed':'task_aborted'},
00448                                    remapping={'grasp_categorisation':'grasp_categorisation' })
00449 
00450             smach.StateMachine.add('CHECKING_USER_INTERVENTION', remote_user_intervention(),
00451                                    transitions={'give_up':'SEMANTIC_DM', 'failed':'task_aborted', 'completed':'task_succeeded'},
00452                                    remapping={'semi_autonomous_mode':'semi_autonomous_mode' })
00453                         
00454                         
00455 
00456         return self.temp
00457                     
00458             
00459     
00460     def priority_cb(self):
00461         #rospy.loginfo("setting priority")
00462         # when this function is called self._as.preempt_request = True due to default policy of the simple action server
00463         # this function override the default policy by compare the priority 
00464         self._as.preempt_request = False # overwrite the default preempt policy of the simple action server
00465         
00466         global current_task_info
00467         result = xmsg.ExecutionResult()
00468         result.return_value=3
00469         #checking if there is a new goal and comparing the priority    
00470         if self._as.next_goal.get_goal() and self._as.new_goal:
00471             print self._as.next_goal.get_goal()
00472             print self._as.new_goal
00473             
00474             #new goal exist
00475             if self._as.next_goal.get_goal().action == 'stop':
00476                 current_task_info.set_stop_required(True)
00477             elif self._as.next_goal.get_goal().action == 'pause':
00478                 current_task_info.set_pause_required(True) 
00479                 self._as.next_goal.set_accepted("")
00480                 self._as.next_goal.set_succeeded(result, 'pause acknowledged')
00481                 self._as.new_goal=False
00482                 self._as.new_goal_preempt_request = False
00483             elif self._as.next_goal.get_goal().action == 'resume':
00484                 if current_task_info.get_pause_required() == True:   # already paused
00485                     current_task_info.set_pause_required(False)
00486                     self._as.next_goal.set_accepted("")
00487                     self._as.next_goal.set_succeeded(result, 'resume acknowledged')
00488                 else:
00489                     result.return_value=2
00490                     self._as.next_goal.set_accepted("")
00491                     self._as.next_goal.set_aborted(result, 'not paused, no need for resuming') 
00492                 self._as.new_goal=False
00493                 self._as.new_goal_preempt_request = False
00494             elif self._as.next_goal.get_goal().priority > self._as.current_goal.get_goal().priority:
00495                 current_task_info.set_customised_preempt_required(True)
00496         else:
00497             # pre-empty request is come from the same goal 
00498             self._sm_srs.request_preempt()             
00499               
00500     #def preempt_check(self):
00501     #    if self.get_customised_preempt_required()==True:
00502     #        self.set_customised_preempt_required(False)
00503     #        return True
00504     #    return False
00505    
00506         
00507     def execute_cb(self, gh):
00508 
00509         self._feedback.current_state = "initialisation"
00510         self._feedback.solution_required = False
00511         self._feedback.exceptional_case_id = 0
00512         self._as.publish_feedback(self._feedback)
00513         #goal=self._as.current_goal.get_goal()
00514         rospy.Subscriber("fb_executing_solution", Bool, self.callback_fb_solution_req)
00515         rospy.Subscriber("fb_executing_state", String, self.callback_fb_current_state)
00516         
00517         goal_handler = self._as.current_goal
00518         current_goal = goal_handler.get_goal()      
00519         
00520         #initialise task information for the state machine
00521         global current_task_info
00522         current_task_info.task_name = current_goal.action
00523         #if current_task_info.task_name=="":
00524         #    current_task_info.task_name="get"
00525         current_task_info.task_parameter = current_goal.parameter
00526 
00527         current_task_info.json_parameters = current_goal.json_parameters
00528         
00529         ## added by Ze
00530         # current_task_info.task_parameters = current_goal.parameters
00531         
00532         if current_task_info.task_name=='stop':
00533             current_task_info.set_stop_acknowledged(False)            #
00534             current_task_info.set_stop_required(False)
00535             
00536         print ("customised_preempt_acknowledged:",  current_task_info.get_customised_preempt_acknowledged())
00537         print ("customised_preempt_required:",  current_task_info.get_customised_preempt_required())
00538         print ("stop_acknowledged:",  current_task_info.get_stop_acknowledged())
00539         print ("stop_required:",  current_task_info.get_stop_required())
00540         print ("object_identification_state:",  current_task_info.get_object_identification_state())
00541         
00542       
00543         
00544         if not self.robot_initialised:
00545             self.robot_initialisation_process()
00546             
00547         current_task_info._srs_as = copy.copy(self)
00548         
00549         
00550         ##############################################
00551         # taskrequest From Knowledge_ros_service
00552         ##############################################
00553         print '#######################   Action Accept Command'
00554         print gh
00555         print 'Request new task'
00556         rospy.wait_for_service('task_request')
00557         try:
00558             requestNewTask = rospy.ServiceProxy('task_request', TaskRequest)
00559             #res = requestNewTask(current_task_info.task_name, current_task_info.task_parameter, None, None, None, None)
00560             req = TaskRequestRequest()
00561             req.task = current_task_info.task_name
00562             
00563             ## added by ze
00564             pars = current_task_info.task_parameter.split("%")    
00565             length = len(pars)
00566             if length == 1:
00567                 req.content = current_task_info.task_parameter
00568                 req.userPose = "order"
00569             elif length == 2:
00570                 req.content = pars[0]
00571                 req.userPose = pars[1]
00572             print req.content
00573             print req.userPose
00574             # req.parameters = current_task_info.parameters
00575             
00576            
00577             ### json parameters
00578             if not current_task_info.json_parameters == '':
00579                 print current_task_info.json_parameters
00580                 tasks = json_parser.Tasks(current_task_info.json_parameters)
00581                 if len(tasks.tasks_list) > 0:
00582                     
00583                     the_task = json.loads(tasks.tasks_list[0].task_json_string)                    
00584                    
00585                     print "current single task is"
00586                     print the_task
00587                     print "##############"
00588                     
00589                     # pre_processing for manual command
00590                     if 'mode' in the_task :
00591                         if the_task['mode'] == 'manual' :
00592                             self.process_manual_command (the_task)
00593                             #no more task completed
00594                             return
00595                     
00596                     #task_dict = tt.tasks[0]
00597                     #task_json = tt.tasks_json[0]
00598                     ## read parameter server
00599                     grasp_type = rospy.get_param("srs/grasping_type")
00600                     tasks.tasks_list[0].addItem('grasping_type', grasp_type)
00601                     
00602                     # if the task list contains multiple tasks,
00603                     # we can use the task id to specify them
00604                     # and the sequence of task execution can be controlled here  
00605                     req.json_parameters = tasks.tasks_list[0].task_json_string 
00606                     #req.json_parameters = tasks.tasks_list[1].task_json_string
00607                     
00608                     print "###req.json_parameters", req.json_parameters 
00609                     #print "###tasks.tasks_list[1]", tasks.tasks_list[1]
00610             res = requestNewTask(req)
00611             #res = requestNewTask(current_task_info.task_name, current_task_info.task_parameter, "order")
00612             print 'Task created with session id of: ' + str(res.sessionId)
00613             current_task_info.session_id = res.sessionId
00614             
00615             if not current_task_info.json_parameters == '':
00616                 #current_task_info.task_feedback = json_parser.Task_Feedback (gh.comm_state_machine.action_goal.goal_id.id , tasks.device_id, tasks.device_type, req.json_parameters)
00617                 current_task_info.task_feedback = json_parser.Task_Feedback (current_task_info.session_id , tasks.device_id, tasks.device_type, req.json_parameters)
00618 
00619             ####          
00620                         
00621             if res.result == 1:
00622                 self._as.set_aborted(self._result)
00623                 return
00624             #elif res.result == 0:
00625                 
00626         except rospy.ServiceException, e:
00627             print "Service call failed: %s"%e
00628         ##############################################
00629         # END OF taskrequest From Knowledge_ros_service
00630         ##############################################
00631 
00632 
00633         
00634         #initial internal state machine for task 
00635         # As action server, the SRSActionServer is also a state machine with basic priority handling ability by itself,
00636         # Therefore there is no need for a 'idle'/'wait for task' state in this state machine.
00637         self._sm_srs = self.init_sm()
00638         self.sis=smach_ros.IntrospectionServer('TASK_SM',self._sm_srs,'/SM_ROOT')
00639         #display smach state
00640         self.sis.start()           
00641         self._as.is_active()
00642         #run the state machine for the task
00643         outcome = self._sm_srs.execute()
00644         self.sis.stop()
00645         
00646         #Testing recorded task execution history
00647         if len (current_task_info.last_step_info) > 1 :
00648             last_step=current_task_info.last_step_info.pop()
00649             rospy.loginfo("sm last step name: %s", last_step.step_name)
00650             rospy.loginfo("sm last step session ID: %s", current_task_info.session_id)
00651         
00652         #set outcomes based on the execution result       
00653                 
00654         #if self.preempt_check()==True:
00655         #    self._result.return_value=2
00656         #    self._as.set_preempted(self._result)
00657         #    return
00658         
00659         
00660         current_task_info.set_customised_preempt_acknowledged(False)
00661         current_task_info.set_customised_preempt_required(False)
00662         current_task_info.set_stop_acknowledged(False)
00663         current_task_info.set_stop_required(False)        
00664         current_task_info.set_object_identification_state(False) 
00665         
00666         #I am not sure this is needed, to be discussed with UI developers
00667         current_task_info.set_pause_required(False)
00668         
00669         if outcome == "task_succeeded": 
00670             self._result.return_value=3
00671             self._as.set_succeeded(self._result)
00672             return
00673         if outcome == "task_preempted":
00674             self._result.return_value=2
00675             self._as.set_preempted(self._result, "stopped before complete or preempted by another task")
00676         #for all other cases outcome == "task_aborted": 
00677         self._result.return_value=4
00678         self._as.set_aborted(self._result)
00679 
00680             
00681     def callback_fb_solution_req(self, data):
00682         #rospy.loginfo("I heard %s",data.data)
00683         self._feedback.solution_required = data.data
00684         self._as.publish_feedback(self._feedback)
00685         
00686     def callback_fb_current_state(self, data):
00687         #rospy.loginfo("I heard %s",data.data)
00688         self._feedback.current_state = data.data
00689         self._as.publish_feedback(self._feedback)        
00690          
00691     def task_executor(self, gh):
00692         pass
00693 
00694 
00695 if __name__ == '__main__':
00696     rospy.init_node('srs_decision_making_actions')
00697     SRS_DM_ACTION(rospy.get_name())
00698     rospy.spin()
00699     #global listener    
00700     #listener = tf.TransformListener()
00701 


srs_decision_making
Author(s): Renxi Qiu
autogenerated on Sun Jan 5 2014 12:08:52