cob_script_server: cob_script_server: simple_script

00001 #!/usr/bin/python
00002 #################################################################
00003 ##\file
00004 #
00005 # \note
00006 #   Copyright (c) 2010 \n
00007 #   Fraunhofer Institute for Manufacturing Engineering
00008 #   and Automation (IPA) \n\n
00009 #
00010 #################################################################
00011 #
00012 # \note
00013 #   Project name: care-o-bot
00014 # \note
00015 #   ROS stack name: cob_command_tools
00016 # \note
00017 #   ROS package name: cob_script_server
00018 #
00019 # \author
00020 #   Author: Florian Weisshardt, email:florian.weisshardt@ipa.fhg.de
00021 # \author
00022 #   Supervised by: Florian Weisshardt, email:florian.weisshardt@ipa.fhg.de
00023 #
00024 # \date Date of creation: Aug 2010
00025 #
00026 # \brief
00027 #   Implements script server functionalities.
00028 #
00029 #################################################################
00030 #
00031 # Redistribution and use in source and binary forms, with or without
00032 # modification, are permitted provided that the following conditions are met:
00033 #
00034 #     - Redistributions of source code must retain the above copyright
00035 #       notice, this list of conditions and the following disclaimer. \n
00036 #     - Redistributions in binary form must reproduce the above copyright
00037 #       notice, this list of conditions and the following disclaimer in the
00038 #       documentation and/or other materials provided with the distribution. \n
00039 #     - Neither the name of the Fraunhofer Institute for Manufacturing
00040 #       Engineering and Automation (IPA) nor the names of its
00041 #       contributors may be used to endorse or promote products derived from
00042 #       this software without specific prior written permission. \n
00043 #
00044 # This program is free software: you can redistribute it and/or modify
00045 # it under the terms of the GNU Lesser General Public License LGPL as 
00046 # published by the Free Software Foundation, either version 3 of the 
00047 # License, or (at your option) any later version.
00048 # 
00049 # This program is distributed in the hope that it will be useful,
00050 # but WITHOUT ANY WARRANTY; without even the implied warranty of
00051 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
00052 # GNU Lesser General Public License LGPL for more details.
00053 # 
00054 # You should have received a copy of the GNU Lesser General Public 
00055 # License LGPL along with this program. 
00056 # If not, see <http://www.gnu.org/licenses/>.
00057 #
00058 #################################################################
00059 
00060 import time
00061 import os
00062 import sys
00063 import types
00064 import thread
00065 import commands
00066 import math
00067 
00068 # ROS imports
00069 import roslib
00070 roslib.load_manifest('cob_script_server')
00071 import rospy
00072 import actionlib
00073 
00074 # msg imports
00075 from trajectory_msgs.msg import *
00076 from geometry_msgs.msg import *
00077 #from pr2_controllers_msgs.msg import * #replaced by control_msgs.msg (FollowJointTrajectoryAction)
00078 from move_base_msgs.msg import *
00079 from arm_navigation_msgs.msg import *
00080 from arm_navigation_msgs.srv import *
00081 from tf.transformations import *
00082 from std_msgs.msg import String,ColorRGBA
00083 from kinematics_msgs.srv import *
00084 from control_msgs.msg import *
00085 
00086 # care-o-bot includes
00087 from cob_sound.msg import *
00088 from cob_script_server.msg import *
00089 from cob_srvs.srv import *
00090 
00091 # graph includes
00092 import pygraphviz as pgv
00093 
00094 graph=""
00095 graph_wait_list=[]
00096 function_counter = 0
00097 ah_counter = 0
00098 graph = pgv.AGraph()
00099 graph.node_attr['shape']='box'
00100 last_node = "Start"
00101 
00102 ## Script class from which all script inherit.
00103 #
00104 # Implements basic functionalities for all scripts.
00105 class script():
00106         def __init__(self):
00107                 # use filename as nodename
00108                 filename = os.path.basename(sys.argv[0])
00109                 self.basename, extension = os.path.splitext(filename)
00110                 rospy.init_node(self.basename)
00111                 self.graph_pub = rospy.Publisher("/script_server/graph", String)
00112 
00113         ## Dummy function for initialization
00114         def Initialize(self):
00115                 pass
00116 
00117         ## Dummy function for main run function
00118         def Run(self):
00119                 pass
00120 
00121         ## Function to start the script
00122         #
00123         # First does a simulated turn and then calls Initialize() and Run().
00124         def Start(self):
00125                 self.Parse()
00126                 global ah_counter
00127                 ah_counter = 0
00128                 self.sss = simple_script_server()
00129                 rospy.loginfo("Starting <<%s>> script...",self.basename)
00130                 self.Initialize()
00131                 self.Run()
00132                 # wait until last threaded action finishes
00133                 rospy.loginfo("Wait for script to finish...")
00134                 while ah_counter != 0:
00135                         rospy.sleep(1)
00136                 rospy.loginfo("...script finished.")
00137         
00138         ## Function to generate graph view of script.
00139         #
00140         # Starts the script in simulation mode and calls Initialize() and Run().
00141         def Parse(self):
00142                 rospy.loginfo("Start parsing...")
00143                 global graph
00144                 global function_counter
00145                 function_counter = 0
00146                 # run script in simulation mode
00147                 self.sss = simple_script_server(parse=True)
00148                 self.Initialize()
00149                 self.Run()
00150                 
00151                 # save graph on parameter server for further processing
00152 #               self.graph = graph
00153                 rospy.set_param("/script_server/graph", graph.string())
00154                 self.graph_pub.publish(graph.string())
00155                 rospy.loginfo("...parsing finished")
00156                 function_counter = 0
00157                 return graph.string()
00158 
00159 ## Simple script server class.
00160 #
00161 # Implements the python interface for the script server.
00162 class simple_script_server:
00163         ## Initializes simple_script_server class.
00164         #
00165         # \param parse Defines wether to run script in simulation for graph generation or not
00166         def __init__(self, parse=False):
00167                 global graph
00168                 self.ns_global_prefix = "/script_server"
00169                 self.wav_path = ""
00170                 self.parse = parse
00171                 
00172                 # init publishers
00173                 self.pub_light = rospy.Publisher('/light_controller/command', ColorRGBA)
00174                 
00175                 rospy.sleep(1) # we have to wait here until publishers are ready, don't ask why
00176 
00177     #------------------- Init section -------------------#
00178         ## Initializes different components.
00179         #
00180         # Based on the component, the corresponding init service will be called.
00181         #
00182         # \param component_name Name of the component.
00183         def init(self,component_name,blocking=True):
00184                 return self.trigger(component_name,"init",blocking=blocking)
00185 
00186         ## Stops different components.
00187         #
00188         # Based on the component, the corresponding stop service will be called.
00189         #
00190         # \param component_name Name of the component.
00191         def stop(self,component_name,mode="omni",blocking=True):
00192                 #return self.trigger(component_name,"stop",blocking=blocking)
00193                 if component_name == "base":
00194                         ah = action_handle("stop", component_name, "", False, self.parse)
00195                         if(self.parse):
00196                                 return ah
00197                         else:
00198                                 ah.set_active(mode="service")
00199         
00200                         rospy.loginfo("<<stop>> <<%s>>", component_name)
00201         
00202                         # call action server
00203                         if(mode == None or mode == ""):
00204                                 action_server_name = "/move_base"
00205                         elif(mode == "omni"):
00206                                 action_server_name = "/move_base"
00207                         elif(mode == "diff"):
00208                                 action_server_name = "/move_base_diff"
00209                         elif(mode == "linear"):
00210                                 action_server_name = "/move_base_linear"
00211                         else:
00212                                 rospy.logerr("no valid navigation mode given for %s, aborting...",component_name)
00213                                 print "navigation mode is:",mode
00214                                 ah.set_failed(33)
00215                                 return ah
00216 
00217                         rospy.logdebug("calling %s action server",action_server_name)
00218                         client = actionlib.SimpleActionClient(action_server_name, MoveBaseAction)
00219                         
00220                         if blocking:
00221                                 # trying to connect to server
00222                                 rospy.logdebug("waiting for %s action server to start",action_server_name)
00223                                 if not client.wait_for_server(rospy.Duration(5)):
00224                                         # error: server did not respond
00225                                         rospy.logerr("%s action server not ready within timeout, aborting...", action_server_name)
00226                                         ah.set_failed(4)
00227                                         return ah
00228                                 else:
00229                                         rospy.logdebug("%s action server ready",action_server_name)
00230 
00231                         # cancel all goals
00232                         client.cancel_all_goals()
00233                         
00234                         ah.set_succeeded() # full success
00235                         return ah       
00236                 else:
00237                         return self.trigger(component_name,"stop",blocking=blocking)
00238 
00239         ## Recovers different components.
00240         #
00241         # Based on the component, the corresponding recover service will be called.
00242         #
00243         # \param component_name Name of the component.
00244         def recover(self,component_name,blocking=True):
00245                 return self.trigger(component_name,"recover",blocking=blocking)
00246 
00247         ## Deals with all kind of trigger services for different components.
00248         #
00249         # Based on the component and service name, the corresponding trigger service will be called.
00250         #
00251         # \param component_name Name of the component.
00252         # \param service_name Name of the trigger service.
00253         # \param blocking Service calls are always blocking. The parameter is only provided for compatibility with other functions.
00254         def trigger(self,component_name,service_name,blocking=True, planning=False):
00255                 ah = action_handle(service_name, component_name, "", blocking, self.parse)
00256                 if(self.parse):
00257                         return ah
00258                 else:
00259                         ah.set_active(mode="service")
00260 
00261                 rospy.loginfo("<<%s>> <<%s>>", service_name, component_name)
00262                 service_full_name = "/" + component_name + "_controller/" + service_name
00263                 
00264                 if blocking:
00265                         # check if service is available
00266                         try:
00267                                 rospy.wait_for_service(service_full_name,rospy.get_param('server_timeout',3))
00268                         except rospy.ROSException, e:
00269                                 error_message = "%s"%e
00270                                 rospy.logerr("...<<%s>> service of <<%s>> not available, error: %s",service_name, component_name, error_message)
00271                                 ah.set_failed(4)
00272                                 return ah
00273 
00274                 # check if service is callable
00275                 try:
00276                         init = rospy.ServiceProxy(service_full_name,Trigger)
00277                         #print init()
00278                         #resp = init()
00279                         if blocking:
00280                                 rospy.loginfo("Wait for <<%s>> to <<%s>>...", component_name, service_name)
00281                                 resp = init()
00282                         else:
00283                                 thread.start_new_thread(init,())
00284                 except rospy.ServiceException, e:
00285                         error_message = "%s"%e
00286                         rospy.logerr("...calling <<%s>> service of <<%s>> not successfull, error: %s",service_name, component_name, error_message)
00287                         ah.set_failed(10)
00288                         return ah
00289 
00290                 if blocking:
00291                         # evaluate sevice response
00292                         if not resp.success.data:
00293                                 rospy.logerr("...<<%s>> <<%s>> not successfull, error: %s",service_name, component_name, resp.error_message.data) 
00294                                 ah.set_failed(10)
00295                                 return ah
00296                 
00297                         # full success
00298                         rospy.loginfo("...<<%s>> is <<%s>>", component_name, service_name)
00299                         ah.set_succeeded() # full success
00300                 return ah
00301 
00302 #------------------- Move section -------------------#
00303         ## Deals with all kind of movements for different components.
00304         #
00305         # Based on the component, the corresponding move functions will be called.
00306         #
00307         # \param component_name Name of the component.
00308         # \param parameter_name Name of the parameter on the ROS parameter server.
00309         # \param blocking Bool value to specify blocking behaviour.
00310         def move(self,component_name,parameter_name,blocking=True, mode=None):
00311                 if component_name == "base":
00312                         return self.move_base(component_name,parameter_name,blocking, mode)
00313                 elif component_name == "arm" and mode=="planned":
00314                         return self.move_planned(component_name,parameter_name,blocking)
00315                 else:
00316                         return self.move_traj(component_name,parameter_name,blocking)
00317 
00318         ## Deals with movements of the base.
00319         #
00320         # A target will be sent to the actionlib interface of the move_base node.
00321         #
00322         # \param component_name Name of the component.
00323         # \param parameter_name Name of the parameter on the ROS parameter server.
00324         # \param blocking Bool value to specify blocking behaviour.
00325         def move_base(self,component_name,parameter_name,blocking, mode):
00326                 ah = action_handle("move", component_name, parameter_name, blocking, self.parse)
00327                 if(self.parse):
00328                         return ah
00329                 else:
00330                         ah.set_active()
00331                 
00332                 if(mode == None or mode == ""):
00333                         rospy.loginfo("Move <<%s>> to <<%s>>",component_name,parameter_name)
00334                 else:
00335                         rospy.loginfo("Move <<%s>> to <<%s>> using <<%s>> mode",component_name,parameter_name,mode)
00336                 
00337                 # get joint values from parameter server
00338                 if type(parameter_name) is str:
00339                         if not rospy.has_param(self.ns_global_prefix + "/" + component_name + "/" + parameter_name):
00340                                 rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",self.ns_global_prefix + "/" + component_name + "/" + parameter_name)
00341                                 ah.set_failed(2)
00342                                 return ah
00343                         param = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/" + parameter_name)
00344                 else:
00345                         param = parameter_name
00346                 
00347                 # check trajectory parameters
00348                 if not type(param) is list: # check outer list
00349                                 rospy.logerr("no valid parameter for %s: not a list, aborting...",component_name)
00350                                 print "parameter is:",param
00351                                 ah.set_failed(3)
00352                                 return ah
00353                 else:
00354                         #print i,"type1 = ", type(i)
00355                         DOF = 3
00356                         if not len(param) == DOF: # check dimension
00357                                 rospy.logerr("no valid parameter for %s: dimension should be %d and is %d, aborting...",component_name,DOF,len(param))
00358                                 print "parameter is:",param
00359                                 ah.set_failed(3)
00360                                 return ah
00361                         else:
00362                                 for i in param:
00363                                         #print i,"type2 = ", type(i)
00364                                         if not ((type(i) is float) or (type(i) is int)): # check type
00365                                                 #print type(i)
00366                                                 rospy.logerr("no valid parameter for %s: not a list of float or int, aborting...",component_name)
00367                                                 print "parameter is:",param
00368                                                 ah.set_failed(3)
00369                                                 return ah
00370                                         else:
00371                                                 rospy.logdebug("accepted parameter %f for %s",i,component_name)
00372 
00373                 # convert to pose message
00374                 pose = PoseStamped()
00375                 pose.header.stamp = rospy.Time.now()
00376                 pose.header.frame_id = "/map"
00377                 pose.pose.position.x = param[0]
00378                 pose.pose.position.y = param[1]
00379                 pose.pose.position.z = 0.0
00380                 q = quaternion_from_euler(0, 0, param[2])
00381                 pose.pose.orientation.x = q[0]
00382                 pose.pose.orientation.y = q[1]
00383                 pose.pose.orientation.z = q[2]
00384                 pose.pose.orientation.w = q[3]
00385                 
00386                 # call action server
00387                 if(mode == None or mode == ""):
00388                         action_server_name = "/move_base"
00389                 elif(mode == "omni"):
00390                         action_server_name = "/move_base"
00391                 elif(mode == "diff"):
00392                         action_server_name = "/move_base_diff"
00393                 elif(mode == "linear"):
00394                         action_server_name = "/move_base_linear"
00395                 else:
00396                         rospy.logerr("no valid navigation mode given for %s, aborting...",component_name)
00397                         print "navigation mode is:",mode
00398                         ah.set_failed(33)
00399                         return ah
00400                 
00401                 rospy.logdebug("calling %s action server",action_server_name)
00402                 client = actionlib.SimpleActionClient(action_server_name, MoveBaseAction)
00403                 # trying to connect to server
00404                 rospy.logdebug("waiting for %s action server to start",action_server_name)
00405                 if not client.wait_for_server(rospy.Duration(5)):
00406                         # error: server did not respond
00407                         rospy.logerr("%s action server not ready within timeout, aborting...", action_server_name)
00408                         ah.set_failed(4)
00409                         return ah
00410                 else:
00411                         rospy.logdebug("%s action server ready",action_server_name)
00412 
00413                 # sending goal
00414                 client_goal = MoveBaseGoal()
00415                 client_goal.target_pose = pose
00416                 #print client_goal
00417                 client.send_goal(client_goal)
00418                 ah.set_client(client)
00419 
00420                 ah.wait_inside()
00421 
00422                 return ah
00423 
00424         ## Deals with all kind of trajectory movements for different components.
00425         #
00426         # A trajectory will be sent to the actionlib interface of the corresponding component.
00427         #
00428         # \param component_name Name of the component.
00429         # \param parameter_name Name of the parameter on the ROS parameter server.
00430         # \param blocking Bool value to specify blocking behaviour.
00431         def move_traj(self,component_name,parameter_name,blocking):
00432                 ah = action_handle("move", component_name, parameter_name, blocking, self.parse)
00433                 if(self.parse):
00434                         return ah
00435                 else:
00436                         ah.set_active()
00437                 
00438                 rospy.loginfo("Move <<%s>> to <<%s>>",component_name,parameter_name)
00439                 
00440                 # get joint_names from parameter server
00441                 param_string = self.ns_global_prefix + "/" + component_name + "/joint_names"
00442                 if not rospy.has_param(param_string):
00443                                 rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",param_string)
00444                                 ah.set_failed(2)
00445                                 return ah
00446                 joint_names = rospy.get_param(param_string)
00447                 
00448                 # check joint_names parameter
00449                 if not type(joint_names) is list: # check list
00450                                 rospy.logerr("no valid joint_names for %s: not a list, aborting...",component_name)
00451                                 print "joint_names are:",joint_names
00452                                 ah.set_failed(3)
00453                                 return ah
00454                 else:
00455                         for i in joint_names:
00456                                 #print i,"type1 = ", type(i)
00457                                 if not type(i) is str: # check string
00458                                         rospy.logerr("no valid joint_names for %s: not a list of strings, aborting...",component_name)
00459                                         print "joint_names are:",param
00460                                         ah.set_failed(3)
00461                                         return ah
00462                                 else:
00463                                         rospy.logdebug("accepted joint_names for component %s",component_name)
00464                 
00465                 # get joint values from parameter server
00466                 if type(parameter_name) is str:
00467                         if not rospy.has_param(self.ns_global_prefix + "/" + component_name + "/" + parameter_name):
00468                                 rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",self.ns_global_prefix + "/" + component_name + "/" + parameter_name)
00469                                 ah.set_failed(2)
00470                                 return ah
00471                         param = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/" + parameter_name)
00472                 else:
00473                         param = parameter_name
00474 
00475                 # check trajectory parameters
00476                 if not type(param) is list: # check outer list
00477                                 rospy.logerr("no valid parameter for %s: not a list, aborting...",component_name)
00478                                 print "parameter is:",param
00479                                 ah.set_failed(3)
00480                                 return ah
00481 
00482                 traj = []
00483 
00484                 for point in param:
00485                         #print point,"type1 = ", type(point)
00486                         if type(point) is str:
00487                                 if not rospy.has_param(self.ns_global_prefix + "/" + component_name + "/" + point):
00488                                         rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",self.ns_global_prefix + "/" + component_name + "/" + point)
00489                                         ah.set_failed(2)
00490                                         return ah
00491                                 point = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/" + point)
00492                                 point = point[0] # \todo TODO: hack because only first point is used, no support for trajectories inside trajectories
00493                                 #print point
00494                         elif type(point) is list:
00495                                 rospy.logdebug("point is a list")
00496                         else:
00497                                 rospy.logerr("no valid parameter for %s: not a list of lists or strings, aborting...",component_name)
00498                                 print "parameter is:",param
00499                                 ah.set_failed(3)
00500                                 return ah
00501 
00502                         # here: point should be list of floats/ints
00503                         #print point
00504                         if not len(point) == len(joint_names): # check dimension
00505                                 rospy.logerr("no valid parameter for %s: dimension should be %d and is %d, aborting...",component_name,len(joint_names),len(point))
00506                                 print "parameter is:",param
00507                                 ah.set_failed(3)
00508                                 return ah
00509 
00510                         for value in point:
00511                                 #print value,"type2 = ", type(value)
00512                                 if not ((type(value) is float) or (type(value) is int)): # check type
00513                                         #print type(value)
00514                                         rospy.logerr("no valid parameter for %s: not a list of float or int, aborting...",component_name)
00515                                         print "parameter is:",param
00516                                         ah.set_failed(3)
00517                                         return ah
00518                         
00519                                 rospy.logdebug("accepted value %f for %s",value,component_name)
00520                         traj.append(point)
00521 
00522                 rospy.logdebug("accepted trajectory for %s",component_name)
00523                 
00524                 # convert to ROS trajectory message
00525                 traj_msg = JointTrajectory()
00526                 traj_msg.header.stamp = rospy.Time.now()+rospy.Duration(0.5)
00527                 traj_msg.joint_names = joint_names
00528                 point_nr = 0
00529                 for point in traj:
00530                         point_nr = point_nr + 1
00531                         point_msg = JointTrajectoryPoint()
00532                         point_msg.positions = point
00533                         point_msg.velocities = [0]*len(joint_names)
00534                         point_msg.time_from_start=rospy.Duration(3*point_nr) # this value is set to 3 sec per point. \todo TODO: read from parameter
00535                         traj_msg.points.append(point_msg)
00536 
00537                 # call action server
00538                 action_server_name = "/" + component_name + '_controller/follow_joint_trajectory'
00539                 rospy.logdebug("calling %s action server",action_server_name)
00540                 client = actionlib.SimpleActionClient(action_server_name, FollowJointTrajectoryAction)
00541                 # trying to connect to server
00542                 rospy.logdebug("waiting for %s action server to start",action_server_name)
00543                 if not client.wait_for_server(rospy.Duration(5)):
00544                         # error: server did not respond
00545                         rospy.logerr("%s action server not ready within timeout, aborting...", action_server_name)
00546                         ah.set_failed(4)
00547                         return ah
00548                 else:
00549                         rospy.logdebug("%s action server ready",action_server_name)
00550                 
00551                 # set operation mode to position
00552                 #if not component_name == "arm":
00553                 #       self.set_operation_mode(component_name,"position")
00554                 #self.set_operation_mode(component_name,"position")             
00555 
00556                 # sending goal
00557                 client_goal = FollowJointTrajectoryGoal()
00558                 client_goal.trajectory = traj_msg
00559                 #print client_goal
00560                 client.send_goal(client_goal)
00561                 ah.set_client(client)
00562 
00563                 ah.wait_inside()
00564                 return ah
00565                 
00566         def move_planned(self, component_name, parameter_name, blocking=True): # for backward compatibility
00567                 return self.move_joint_goal_planned(component_name, parameter_name, blocking)
00568         
00569         def move_joint_goal_planned(self, component_name, parameter_name, blocking=True):
00570                 ah = action_handle("move_joint_goal_planned", component_name, parameter_name, blocking, self.parse)
00571                 if(self.parse):
00572                         return ah
00573                 else:
00574                         ah.set_active()
00575                 
00576                 if component_name != "arm":
00577                         rospy.logerr("Only arm component is supported in move_joint_goal_planned.")
00578                         ah.set_failed(4)
00579                         return ah
00580                         
00581                 rospy.loginfo("Move planned <<%s>> to <<%s>>",component_name,parameter_name)
00582                 
00583                 # get joint_names from parameter server
00584                 param_string = self.ns_global_prefix + "/" + component_name + "/joint_names"
00585                 if not rospy.has_param(param_string):
00586                                 rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",param_string)
00587                                 ah.set_failed(2)
00588                                 return ah
00589                 joint_names = rospy.get_param(param_string)
00590                 
00591                 # check joint_names parameter
00592                 if not type(joint_names) is list: # check list
00593                                 rospy.logerr("no valid joint_names for %s: not a list, aborting...",component_name)
00594                                 print "joint_names are:",joint_names
00595                                 ah.set_failed(3)
00596                                 return ah
00597                 else:
00598                         for i in joint_names:
00599                                 #print i,"type1 = ", type(i)
00600                                 if not type(i) is str: # check string
00601                                         rospy.logerr("no valid joint_names for %s: not a list of strings, aborting...",component_name)
00602                                         print "joint_names are:",param
00603                                         ah.set_failed(3)
00604                                         return ah
00605                                 else:
00606                                         rospy.logdebug("accepted joint_names for component %s",component_name)
00607                 
00608                 # get joint values from parameter server
00609                 if type(parameter_name) is str:
00610                         if not rospy.has_param(self.ns_global_prefix + "/" + component_name + "/" + parameter_name):
00611                                 rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",self.ns_global_prefix + "/" + component_name + "/" + parameter_name)
00612                                 ah.set_failed(2)
00613                                 return ah
00614                         param = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/" + parameter_name)
00615                 else:
00616                         param = parameter_name
00617 
00618                 # check trajectory parameters
00619                 if not type(param) is list: # check outer list
00620                                 rospy.logerr("no valid parameter for %s: not a list, aborting...",component_name)
00621                                 print "parameter is:",param
00622                                 ah.set_failed(3)
00623                                 return ah
00624 
00625                 traj = []
00626 
00627                 for point in param:
00628                         #print point,"type1 = ", type(point)
00629                         if type(point) is str:
00630                                 if not rospy.has_param(self.ns_global_prefix + "/" + component_name + "/" + point):
00631                                         rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",self.ns_global_prefix + "/" + component_name + "/" + point)
00632                                         ah.set_failed(2)
00633                                         return ah
00634                                 point = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/" + point)
00635                                 point = point[0] # \todo TODO: hack because only first point is used, no support for trajectories inside trajectories
00636                                 #print point
00637                         elif type(point) is list:
00638                                 rospy.logdebug("point is a list")
00639                         else:
00640                                 rospy.logerr("no valid parameter for %s: not a list of lists or strings, aborting...",component_name)
00641                                 print "parameter is:",param
00642                                 ah.set_failed(3)
00643                                 return ah
00644 
00645                         # here: point should be list of floats/ints
00646                         #print point
00647                         if not len(point) == len(joint_names): # check dimension
00648                                 rospy.logerr("no valid parameter for %s: dimension should be %d and is %d, aborting...",component_name,len(joint_names),len(point))
00649                                 print "parameter is:",param
00650                                 ah.set_failed(3)
00651                                 return ah
00652 
00653                         for value in point:
00654                                 #print value,"type2 = ", type(value)
00655                                 if not ((type(value) is float) or (type(value) is int)): # check type
00656                                         #print type(value)
00657                                         rospy.logerr("no valid parameter for %s: not a list of float or int, aborting...",component_name)
00658                                         print "parameter is:",param
00659                                         ah.set_failed(3)
00660                                         return ah
00661                         
00662                                 rospy.logdebug("accepted value %f for %s",value,component_name)
00663                         traj.append(point)
00664 
00665                 rospy.logdebug("accepted trajectory for %s",component_name)
00666 
00667                 goal_constraints = Constraints() #arm_navigation_msgs/Constraints
00668                 goal_constraints.joint_constraints=[]
00669                 
00670                 for i in range(len(joint_names)):
00671                         new_constraint = JointConstraint()
00672                         new_constraint.joint_name = joint_names[i]
00673                         new_constraint.position = 0.0
00674                         new_constraint.tolerance_below = 0.4
00675                         new_constraint.tolerance_above = 0.4
00676                         goal_constraints.joint_constraints.append(new_constraint)
00677                 #no need for trajectories anymore, since planning (will) guarantee collision-free motion!
00678                 traj_endpoint = traj[len(traj)-1]
00679                 for k in range(len(traj_endpoint)):
00680                         #print "traj_endpoint[%d]: %f", k, traj_endpoint[k]
00681                         goal_constraints.joint_constraints[k].position = traj_endpoint[k]
00682 
00683                 return self.move_constrained_planned(component_name, goal_constraints, blocking, ah)
00684 
00685 
00686         def move_constrained_planned(self, component_name, parameter_name, blocking=True, ah=None):
00687                 if ah is None:
00688                         ah = action_handle("move_constrained_planned", component_name, "constraint_goal", blocking, self.parse)
00689                         if(self.parse):
00690                                 return ah
00691                         else:
00692                                 ah.set_active()
00693                         
00694                 if component_name != "arm":
00695                         rospy.logerr("Only arm component is supported in move_constrained_planned.")
00696                         ah.set_failed(4)
00697                         return ah
00698                 
00699                 # convert to ROS Move arm message
00700                 motion_plan = MotionPlanRequest()
00701                 motion_plan.group_name = component_name
00702                 motion_plan.num_planning_attempts = 1
00703                 motion_plan.allowed_planning_time = rospy.Duration(50.0)
00704 
00705                 motion_plan.planner_id= ""
00706                 motion_plan.goal_constraints = parameter_name
00707 
00708                 # call action server
00709                 action_server_name = "/move_"+component_name
00710                 rospy.logdebug("calling %s action server",action_server_name)
00711                 client = actionlib.SimpleActionClient(action_server_name, MoveArmAction)
00712                 # trying to connect to server
00713                 rospy.logdebug("waiting for %s action server to start",action_server_name)
00714                 if not client.wait_for_server(rospy.Duration(5)):
00715                         # error: server did not respond
00716                         rospy.logerr("%s action server not ready within timeout, aborting...", action_server_name)
00717                         ah.set_failed(4)
00718                         return ah
00719                 else:
00720                         rospy.logdebug("%s action server ready",action_server_name)
00721                 
00722                 # set operation mode to position
00723                 #self.set_operation_mode(component_name,"position")
00724                 
00725                 # sending goal
00726                 client_goal = MoveArmGoal()
00727                 client_goal.planner_service_name = "ompl_planning/plan_kinematic_path"          #choose planner
00728                 #client_goal.planner_service_name = "cob_prmce_planner/plan_kinematic_path"
00729                 client_goal.motion_plan_request = motion_plan
00730                 #print client_goal
00731                 client.send_goal(client_goal)
00732                 ah.set_client(client)
00733 
00734                 ah.wait_inside()
00735                 return ah
00736         ## Relative movement of the base
00737         #
00738         # \param component_name Name of component; here always "base".
00739         # \param parameter_name List of length 3: (item 1 & 2) relative x and y translation [m]; (item 3) relative rotation about z axis [rad].
00740         # \param blocking Bool value to specify blocking behaviour.
00741         # 
00742         # # throws error code 3 in case of invalid parameter_name vector 
00743         def move_base_rel(self, component_name, parameter_name=[0,0,0], blocking=True): 
00744                 ah = action_handle("move_base_rel", component_name, parameter_name, blocking, self.parse)
00745                 if(self.parse):
00746                         return ah
00747                 else:
00748                         ah.set_active(mode="topic")
00749 
00750                 rospy.loginfo("Move base relatively by <<%s>>", parameter_name)
00751 
00752                 # step 0: check validity of parameters:
00753                 if not len(parameter_name) == 3 or not isinstance(parameter_name[0], (int, float)) or not isinstance(parameter_name[1], (int, float)) or not isinstance(parameter_name[2], (int, float)):
00754                         rospy.logerr("Non-numeric parameter_name list, aborting move_base_rel")
00755                         print("parameter_name must be numeric list of length 3; (relative x and y transl [m], relative rotation [rad])")
00756                         ah.set_failed(3)
00757                         return ah
00758                 if math.sqrt(parameter_name[0]**2 + parameter_name[1]**2) >= 0.15:
00759                         rospy.logerr("Maximal relative translation step exceeded, aborting move_base_rel")
00760                         print("Maximal relative translation step is 0.1 m")
00761                         ah.set_failed(3)
00762                         return(ah)
00763                 if abs(parameter_name[2]) >= math.pi/2:
00764                         rospy.logerr("Maximal relative rotation step exceeded, aborting move_base_rel")
00765                         print("Maximal relative rotation step is pi/2")
00766                         ah.set_failed(3)
00767                         return(ah)
00768 
00769                 # step 1: determine duration of motion so that upper thresholds for both translational as well as rotational velocity are not exceeded
00770                 max_trans_vel = 0.05 # [m/s]
00771                 max_rot_vel = 0.2 # [rad/s]
00772                 duration_trans_sec = math.sqrt(parameter_name[0]**2 + parameter_name[1]**2) / max_trans_vel
00773                 duration_rot_sec = abs(parameter_name[2] / max_rot_vel)
00774                 duration_sec = max(duration_trans_sec, duration_rot_sec)
00775                 duration_ros = rospy.Duration.from_sec(duration_sec) # duration of motion in ROS time
00776 
00777                 # step 2: determine actual velocities based on calculated duration
00778                 x_vel = parameter_name[0] / duration_sec
00779                 y_vel = parameter_name[1] / duration_sec
00780                 rot_vel = parameter_name[2] / duration_sec
00781 
00782                 # step 3: send constant velocity command to base_controller for the calculated duration of motion
00783                 pub = rospy.Publisher('/base_controller/command_safe', Twist)  # todo: use Matthias G.'s safe_command
00784                 twist = Twist()
00785                 twist.linear.x = x_vel
00786                 twist.linear.y = y_vel
00787                 twist.angular.z = rot_vel
00788                 r = rospy.Rate(10) # send velocity commands at 10 Hz
00789                 end_time = rospy.Time.now() + duration_ros
00790                 while not rospy.is_shutdown() and rospy.Time.now() < end_time:
00791                         pub.publish(twist)
00792                         r.sleep()
00793 
00794                 ah.set_succeeded()
00795                 return ah
00796 
00797         
00798         ## Set the operation mode for different components.
00799         #
00800         # Based on the component, the corresponding set_operation_mode service will be called.
00801         #
00802         # \param component_name Name of the component.
00803         # \param mode Name of the operation mode to set.
00804         # \param blocking Service calls are always blocking. The parameter is only provided for compatibility with other functions.
00805         def set_operation_mode(self,component_name,mode,blocking=True, planning=False):
00806                 #rospy.loginfo("setting <<%s>> to operation mode <<%s>>",component_name, mode)
00807                 rospy.set_param("/" + component_name + "_controller/OperationMode",mode) # \todo TODO: remove and only use service call
00808                 #rospy.wait_for_service("/" + component_name + "_controller/set_operation_mode")
00809                 try:
00810                         set_operation_mode = rospy.ServiceProxy("/" + component_name + "_controller/set_operation_mode", SetOperationMode)
00811                         req = SetOperationModeRequest()
00812                         req.operation_mode.data = mode
00813                         #print req
00814                         resp = set_operation_mode(req)
00815                         #print resp
00816                 except rospy.ServiceException, e:
00817                         print "Service call failed: %s"%e
00818                 
00819 #------------------- LED section -------------------#
00820         ## Set the color of the cob_light component.
00821         #
00822         # The color is given by a parameter on the parameter server.
00823         #
00824         # \param parameter_name Name of the parameter on the parameter server which holds the rgb values.
00825         def set_light(self,parameter_name,blocking=False):
00826                 ah = action_handle("set", "light", parameter_name, blocking, self.parse)
00827                 if(self.parse):
00828                         return ah
00829                 else:
00830                         ah.set_active(mode="topic")
00831 
00832                 rospy.loginfo("Set light to <<%s>>",parameter_name)
00833                 
00834                 # get joint values from parameter server
00835                 if type(parameter_name) is str:
00836                         if not rospy.has_param(self.ns_global_prefix + "/light/" + parameter_name):
00837                                 rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",self.ns_global_prefix + "/light/" + parameter_name)
00838                                 return 2
00839                         param = rospy.get_param(self.ns_global_prefix + "/light/" + parameter_name)
00840                 else:
00841                         param = parameter_name
00842                         
00843                 # check color parameters
00844                 if not type(param) is list: # check outer list
00845                         rospy.logerr("no valid parameter for light: not a list, aborting...")
00846                         print "parameter is:",param
00847                         ah.error_code = 3
00848                         return ah
00849                 else:
00850                         if not len(param) == 3: # check dimension
00851                                 rospy.logerr("no valid parameter for light: dimension should be 3 (r,g,b) and is %d, aborting...",len(param))
00852                                 print "parameter is:",param
00853                                 ah.error_code = 3
00854                                 return ah
00855                         else:
00856                                 for i in param:
00857                                         #print i,"type1 = ", type(i)
00858                                         if not ((type(i) is float) or (type(i) is int)): # check type
00859                                                 #print type(i)
00860                                                 rospy.logerr("no valid parameter for light: not a list of float or int, aborting...")
00861                                                 print "parameter is:",param
00862                                                 ah.error_code = 3
00863                                                 return ah
00864                                         else:
00865                                                 rospy.logdebug("accepted parameter %f for light",i)
00866                 
00867                 # convert to ColorRGBA message
00868                 color = ColorRGBA()
00869                 color.r = param[0]
00870                 color.g = param[1]
00871                 color.b = param[2]
00872                 color.a = 1 # Transparency
00873 
00874                 # publish color         
00875                 self.pub_light.publish(color)
00876                 
00877                 ah.set_succeeded()
00878                 ah.error_code = 0
00879                 return ah
00880 
00881 #-------------------- Sound section --------------------#
00882         ## Say some text.
00883         #
00884         # The text to say may be given by a list of strings or a single string which points to a parameter on the ROS parameter server.
00885         #
00886         # \param parameter_name Name of the parameter
00887         # \param language Language to use for the TTS system
00888         def say(self,parameter_name,blocking=True):
00889                 component_name = "sound"
00890                 ah = action_handle("say", component_name, parameter_name, blocking, self.parse)
00891                 if(self.parse):
00892                         return ah
00893                 else:
00894                         ah.set_active()
00895                         
00896                 text = ""
00897                 
00898                 # get values from parameter server
00899                 language = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/language","en")
00900                 if type(parameter_name) is str:
00901                         if not rospy.has_param(self.ns_global_prefix + "/" + component_name + "/" + language + "/" + parameter_name):
00902                                 rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",self.ns_global_prefix + "/" + component_name + "/" + language + "/" + parameter_name)
00903                                 ah.set_failed(2)
00904                                 return ah
00905                         param = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/" + language + "/" + parameter_name)
00906                 else:
00907                         param = parameter_name
00908                 
00909                 # check parameters
00910                 if not type(param) is list: # check list
00911                                 rospy.logerr("no valid parameter for %s: not a list, aborting...",component_name)
00912                                 print "parameter is:",param
00913                                 ah.set_failed(3)
00914                                 return ah
00915                 else:
00916                         for i in param:
00917                                 #print i,"type1 = ", type(i)
00918                                 if not type(i) is str:
00919                                         rospy.logerr("no valid parameter for %s: not a list of strings, aborting...",component_name)
00920                                         print "parameter is:",param
00921                                         ah.set_failed(3)
00922                                         return ah
00923                                 else:
00924                                         text = text + i + " "
00925                                         rospy.logdebug("accepted parameter <<%s>> for <<%s>>",i,component_name)
00926 
00927                 rospy.loginfo("Saying <<%s>>",text)
00928                 
00929                 # call action server
00930                 action_server_name = "/sound_controller/say"
00931                 rospy.logdebug("calling %s action server",action_server_name)
00932                 client = actionlib.SimpleActionClient(action_server_name, SayAction)
00933                 # trying to connect to server
00934                 rospy.logdebug("waiting for %s action server to start",action_server_name)
00935                 if not client.wait_for_server(rospy.Duration(5)):
00936                         # error: server did not respond
00937                         rospy.logerr("%s action server not ready within timeout, aborting...", action_server_name)
00938                         ah.set_failed(4)
00939                         return ah
00940                 else:
00941                         rospy.logdebug("%s action server ready",action_server_name)
00942 
00943                 # sending goal
00944                 client_goal = SayGoal()
00945                 client_goal.text.data = text
00946                 #print client_goal
00947                 client.send_goal(client_goal)
00948                 ah.set_client(client)
00949 
00950                 ah.wait_inside()
00951                 return ah
00952 
00953         ## Play a sound file.
00954         #
00955         # \param parameter_name Name of the parameter
00956         # \param language Language to use
00957         def play(self,parameter_name,blocking=True):
00958                 component_name = "sound"
00959                 ah = action_handle("play", component_name, parameter_name, False, self.parse)
00960                 if(self.parse):
00961                         return ah
00962                 else:
00963                         ah.set_active(mode="system")
00964                 
00965                 language = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/language","en")
00966                 if self.wav_path == "":
00967                         wav_path = commands.getoutput("rospack find cob_script_server")
00968                 else:
00969                         wav_path = self.wav_path
00970                 filename = wav_path + "/common/files/" + language + "/" + parameter_name + ".wav"
00971                 
00972                 rospy.loginfo("Playing <<%s>>",filename)
00973                 #self.soundhandle.playWave(filename)
00974                 
00975                 #\todo TODO: check if file exists
00976                 # if filename exists:
00977                 #       do ...
00978                 # else 
00979                 #       ah.set_fail(3)
00980                 #       return ah
00981                 
00982                 if blocking:
00983                         ret = os.system("aplay -q " + filename)
00984                         if ret != 0:
00985                                 ah.set_failed(99)
00986                                 return ah
00987                 else:
00988                         os.system("aplay -q " + filename + "&") # TODO how to check if execution failed (e.g. file could be found)?
00989                 ah.set_succeeded()
00990                 return ah
00991                 
00992         def set_wav_path(self,parameter_name,blocking=True):
00993                 if type(parameter_name) is str:
00994                         self.wav_path = parameter_name
00995                 else:
00996                         rospy.logerr("invalid wav_path parameter specified, aborting...")
00997                         print "parameter is:", parameter_name
00998                         ah.set_failed(2)
00999                         return ah               
01000 
01001 #------------------- General section -------------------#
01002         ## Sleep for a certain time.
01003         #
01004         # \param duration Duration in seconds to sleep.
01005         #
01006         def sleep(self,duration):
01007                 ah = action_handle("sleep", "", str(duration), True, self.parse)
01008                 if(self.parse):
01009                         return ah
01010                 else:
01011                         ah.set_active()
01012                 rospy.loginfo("Wait for %f sec",duration)
01013                 rospy.sleep(duration)
01014                 
01015                 ah.set_succeeded()
01016                 return ah
01017 
01018         ## Waits for user input.
01019         #
01020         # Waits either for a user input or until timeout is reached.
01021         #
01022         # \param duration Duration in seconds for timeout.
01023         # 
01024         # \todo TODO: implement waiting for timeout
01025         def wait_for_input(self,duration=0):
01026                 ah = action_handle("wait", "input", str(duration), True, self.parse)
01027                 if(self.parse):
01028                         return ah
01029                 else:
01030                         ah.set_active()
01031                 
01032                 if (duration != 0):
01033                         rospy.logerr("Wait with duration not implemented yet") # \todo TODO: implement waiting with duration
01034                 
01035                 rospy.loginfo("Wait for user input...")
01036                 retVal = raw_input()
01037                 rospy.loginfo("...got string <<%s>>",retVal)
01038                 ah.set_succeeded()
01039                 return retVal
01040 
01041 #------------------- action_handle section -------------------# 
01042 ## Action handle class.
01043 #
01044 # The action handle is used to implement asynchronous behaviour within the script.
01045 class action_handle:
01046         ## Initializes the action handle.
01047         def __init__(self, function_name, component_name, parameter_name, blocking, parse):
01048                 global graph
01049                 global function_counter
01050                 self.parent_node = ""
01051                 self.error_code = -1
01052                 self.wait_log = False
01053                 self.function_counter = function_counter
01054                 self.function_name = function_name
01055                 self.component_name = component_name
01056                 self.parameter_name = parameter_name
01057                 self.state = ScriptState.UNKNOWN
01058                 self.blocking = blocking
01059                 self.parse = parse
01060                 self.level = int(rospy.get_param("/script_server/level",100))
01061                 self.state_pub = rospy.Publisher("/script_server/state", ScriptState)
01062                 self.AppendNode(blocking)
01063                 self.client = actionlib.SimpleActionClient("dummy",ScriptAction)
01064                 self.client_state = 9
01065                 self.client_mode = ""
01066 
01067         ## Sets the actionlib client.
01068         def set_client(self,client):
01069                 self.client = client
01070 
01071         ## Sets the execution state to active, if not paused
01072         def set_active(self,mode=""):
01073                 if mode != "": # not processing an actionlib client
01074                         self.client_mode = mode
01075                         self.client_state = 1
01076                 self.check_pause()
01077                 self.state = ScriptState.ACTIVE
01078                 self.error_code = -1
01079                 self.PublishState()
01080                 
01081                 global ah_counter
01082                 ah_counter += 1
01083                 
01084         ## Checks for pause
01085         def check_pause(self):
01086                 param_string = "/script_server/pause"
01087                 while bool(rospy.get_param(param_string,False)):
01088                         rospy.logwarn("Script is paused...")
01089                         self.state = ScriptState.PAUSED
01090                         self.PublishState()
01091                         rospy.sleep(1)
01092                 if self.state == ScriptState.PAUSED:
01093                         rospy.loginfo("...continuing script")
01094                 
01095         ## Sets the execution state to succeeded.
01096         def set_succeeded(self):
01097                 if self.client_mode != "": # not processing an actionlib client
01098                         self.client_state = 3
01099                 self.state = ScriptState.SUCCEEDED
01100                 self.error_code = 0
01101                 self.PublishState()
01102                 
01103                 global ah_counter
01104                 ah_counter -= 1
01105                 
01106         ## Sets the execution state to failed.
01107         def set_failed(self,error_code):
01108                 if self.client_mode != "": # not processing an actionlib client
01109                         self.client_state = 4
01110                 self.state = ScriptState.FAILED
01111                 self.error_code = error_code
01112                 self.PublishState()
01113 
01114                 global ah_counter
01115                 ah_counter -= 1
01116                 
01117         ## Gets the state of an action handle.
01118         def get_state(self):
01119                 if self.client_mode != "": # not processing an actionlib client
01120                         return self.client_state
01121                 else:
01122                         return self.client.get_state()
01123 
01124         ## Gets the error code of an action handle.
01125         def get_error_code(self):
01126                 return self.error_code
01127         
01128         ## Returns the graphstring.
01129         def GetGraphstring(self):
01130                 if type(self.parameter_name) is types.StringType:
01131                         graphstring = str(self.function_counter)+"_"+self.function_name+"_"+self.component_name+"_"+self.parameter_name
01132                 else:
01133                         graphstring = str(self.function_counter)+"_"+self.function_name+"_"+self.component_name
01134                 return graphstring
01135 
01136         ## Gets level of function name.
01137         def GetLevel(self,function_name):
01138                 if (function_name == "move"):
01139                         level = 0
01140                 elif (function_name == "init"):
01141                         level = 1
01142                 elif (function_name == "stop"):
01143                         level = 1
01144                 elif (function_name == "sleep"):
01145                         level = 2
01146                 else:
01147                         level = 100
01148                 return level
01149                 
01150         ## Appends a registered function to the graph.
01151         def AppendNode(self, blocking=True):
01152                 global graph
01153                 global graph_wait_list
01154                 global function_counter
01155                 global last_node
01156                 graphstring = self.GetGraphstring()
01157                 if self.parse:
01158                         if ( self.level >= self.GetLevel(self.function_name)):
01159                                 #print "adding " + graphstring + " to graph"
01160                                 graph.add_edge(last_node, graphstring)
01161                                 for waiter in graph_wait_list:
01162                                         graph.add_edge(waiter, graphstring)
01163                                 graph_wait_list=[]
01164                                 if blocking:
01165                                         last_node = graphstring
01166                                 else:
01167                                         self.parent_node = graphstring
01168                         #else:
01169                                 #print "not adding " + graphstring + " to graph"
01170                 #else:
01171                         #self.PublishState()
01172                 function_counter += 1
01173                 
01174         ## Publishs the state of the action handle
01175         def PublishState(self):
01176                 script_state = ScriptState()
01177                 script_state.header.stamp = rospy.Time.now()
01178                 script_state.number = self.function_counter
01179                 script_state.function_name = self.function_name
01180                 script_state.component_name = self.component_name
01181                 script_state.full_graph_name = self.GetGraphstring()
01182                 if ( type(self.parameter_name) is str ):
01183                         script_state.parameter_name = self.parameter_name
01184                 else:
01185                         script_state.parameter_name = ""
01186                 script_state.state = self.state
01187                 script_state.error_code = self.error_code
01188                 self.state_pub.publish(script_state)
01189                 
01190         ## Handles wait.
01191         #
01192         # This function is meant to be uses directly in the script.
01193         #
01194         # \param duration Duration for timeout.
01195         def wait(self, duration=None):
01196                 global ah_counter
01197                 ah_counter += 1
01198                 self.blocking = True
01199                 self.wait_for_finished(duration,True)
01200 
01201         ## Handles inside wait.
01202         #
01203         # This function is meant to be uses inside the simple_script_server.
01204         #
01205         # \param duration Duration for timeout.
01206         def wait_inside(self, duration=None):
01207                 if self.blocking:
01208                         self.wait_for_finished(duration,True)
01209                 else:
01210                         thread.start_new_thread(self.wait_for_finished,(duration,False,))
01211                 return self.error_code
01212         
01213         ## Waits for the action to be finished.
01214         #
01215         # If duration is specified, waits until action is finished or timeout is reached.
01216         #
01217         # \param duration Duration for timeout.
01218         # \param logging Enables or disables logging for this wait.
01219         def wait_for_finished(self, duration, logging):
01220                 global graph_wait_list
01221                 if(self.parse):
01222                         if(self.parent_node != ""):
01223                                 graph_wait_list.append(self.parent_node)
01224                         return
01225 
01226                 if self.error_code <= 0:                        
01227                         if duration is None:
01228                                 if logging:
01229                                         rospy.loginfo("Wait for <<%s>> reaching <<%s>>...",self.component_name, self.parameter_name)
01230                                 self.client.wait_for_result()
01231                         else:
01232                                 if logging:
01233                                         rospy.loginfo("Wait for <<%s>> reached <<%s>> (max %f secs)...",self.component_name, self.parameter_name,duration)
01234                                 if not self.client.wait_for_result(rospy.Duration(duration)):
01235                                         if logging:
01236                                                 rospy.logerr("Timeout while waiting for <<%s>> to reach <<%s>>. Continuing...",self.component_name, self.parameter_name)
01237                                         self.set_failed(10)
01238                                         return
01239                         # check state of action server
01240                         #print self.client.get_state()
01241                         if self.client.get_state() != 3:
01242                                 if logging:
01243                                         rospy.logerr("...<<%s>> could not reach <<%s>>, aborting...",self.component_name, self.parameter_name)
01244                                 self.set_failed(11)
01245                                 return
01246 
01247                         if logging:
01248                                 rospy.loginfo("...<<%s>> reached <<%s>>",self.component_name, self.parameter_name)
01249                 else:
01250                         rospy.logwarn("Execution of <<%s>> to <<%s>> was aborted, wait not possible. Continuing...",self.component_name, self.parameter_name)
01251                         self.set_failed(self.error_code)
01252                         return
01253                         
01254                 self.set_succeeded() # full success
01255         
01256         ## Cancel action
01257         #
01258         # Cancels action goal(s).
01259         def cancel(self):
01260                 self.client.cancel_all_goals()
simple_script_server.py
