$search
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_apps 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 # Implementation of ROS node for script_server. 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 inspect 00062 00063 import roslib 00064 roslib.load_manifest('cob_script_server') 00065 import rospy 00066 import actionlib 00067 00068 from cob_script_server.msg import * 00069 from simple_script_server import * 00070 00071 sss = simple_script_server() 00072 00073 ## Script server class which inherits from script class. 00074 # 00075 # Implements actionlib interface for the script server. 00076 # 00077 class script_server(): 00078 ## Initializes the actionlib interface of the script server. 00079 # 00080 def __init__(self): 00081 self.ns_global_prefix = "/script_server" 00082 self.script_action_server = actionlib.SimpleActionServer(self.ns_global_prefix, ScriptAction, self.execute_cb, False) 00083 self.script_action_server.start() 00084 00085 #------------------- Actionlib section -------------------# 00086 ## Executes actionlib callbacks. 00087 # 00088 # \param server_goal ScriptActionGoal 00089 # 00090 def execute_cb(self, server_goal): 00091 server_result = ScriptActionResult().result 00092 if server_goal.function_name == None or server_goal.function_name.strip() == "": 00093 rospy.logerr("function name cannot be blank") 00094 return 00095 00096 if server_goal.function_name in dir(sss): 00097 func = getattr(sss, server_goal.function_name) 00098 argspec = inspect.getargspec(func) 00099 args = {} 00100 for arg in argspec.args: 00101 if arg in dir(server_goal): 00102 serverArg = getattr(server_goal, arg) 00103 if type(serverArg) == str: 00104 try: 00105 serverArg = eval(serverArg) 00106 except: 00107 pass 00108 args[arg] = serverArg 00109 00110 handle01 = func(*(), **args) 00111 else: 00112 rospy.logerr("function <<%s>> not supported", server_goal.function_name) 00113 self.script_action_server.set_aborted(server_result) 00114 return 00115 00116 if 'get_error_code' in dir(handle01): 00117 server_result.error_code = handle01.get_error_code() 00118 else: 00119 rospy.logwarn("unexpected action result type <<%s>> for function %s", type(handle01), server_goal.function_name) 00120 if server_result.error_code == 0: 00121 rospy.logdebug("action result success") 00122 self.script_action_server.set_succeeded(server_result) 00123 else: 00124 rospy.logerr("action result error, error_code: " + str(server_result.error_code)) 00125 self.script_action_server.set_aborted(server_result) 00126 00127 ## Main routine for running the script server 00128 # 00129 if __name__ == '__main__': 00130 rospy.init_node('script_server') 00131 script_server() 00132 rospy.loginfo("script_server is running") 00133 rospy.spin()