script_server.py

Go to the documentation of this file.

#!/usr/bin/python
#################################################################
##\file
#
# \note
#   Copyright (c) 2010 \n
#   Fraunhofer Institute for Manufacturing Engineering
#   and Automation (IPA) \n\n
#
#################################################################
#
# \note
#   Project name: care-o-bot
# \note
#   ROS stack name: cob_apps
# \note
#   ROS package name: cob_script_server
#
# \author
#   Author: Florian Weisshardt, email:florian.weisshardt@ipa.fhg.de
# \author
#   Supervised by: Florian Weisshardt, email:florian.weisshardt@ipa.fhg.de
#
# \date Date of creation: Aug 2010
#
# \brief
#   Implementation of ROS node for script_server.
#
#################################################################
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
#     - Redistributions of source code must retain the above copyright
#       notice, this list of conditions and the following disclaimer. \n
#     - Redistributions in binary form must reproduce the above copyright
#       notice, this list of conditions and the following disclaimer in the
#       documentation and/or other materials provided with the distribution. \n
#     - Neither the name of the Fraunhofer Institute for Manufacturing
#       Engineering and Automation (IPA) nor the names of its
#       contributors may be used to endorse or promote products derived from
#       this software without specific prior written permission. \n
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU Lesser General Public License LGPL as 
# published by the Free Software Foundation, either version 3 of the 
# License, or (at your option) any later version.
# 
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU Lesser General Public License LGPL for more details.
# 
# You should have received a copy of the GNU Lesser General Public 
# License LGPL along with this program. 
# If not, see <http://www.gnu.org/licenses/>.
#
#################################################################

import time
import inspect

import roslib
roslib.load_manifest('cob_script_server')
import rospy
import actionlib

from cob_script_server.msg import *
from simple_script_server import *

sss = simple_script_server()

## Script server class which inherits from script class.
#
# Implements actionlib interface for the script server.
#
class script_server():
        ## Initializes the actionlib interface of the script server.
        #
        def __init__(self):
                self.ns_global_prefix = "/script_server"
                self.script_action_server = actionlib.SimpleActionServer(self.ns_global_prefix, ScriptAction, self.execute_cb, False)
                self.script_action_server.start()
        
#------------------- Actionlib section -------------------#
        ## Executes actionlib callbacks.
        #
        # \param server_goal ScriptActionGoal
        #
        def execute_cb(self, server_goal):
                server_result = ScriptActionResult().result
                if server_goal.function_name == None or server_goal.function_name.strip() == "":
                        rospy.logerr("function name cannot be blank")
                        return
                        
                if server_goal.function_name in dir(sss):
                        func = getattr(sss, server_goal.function_name)
                        argspec = inspect.getargspec(func)
                        args = {}
                        for arg in argspec.args:
                                if arg in dir(server_goal):
                                        serverArg = getattr(server_goal, arg)
                                        if type(serverArg) == str:
                                                try:
                                                        serverArg = eval(serverArg)
                                                except:
                                                        pass
                                        args[arg] = serverArg
                        
                        handle01 = func(*(), **args)
                else:
                        rospy.logerr("function <<%s>> not supported", server_goal.function_name)
                        self.script_action_server.set_aborted(server_result)
                        return
                
                if 'get_error_code' in dir(handle01):
                        server_result.error_code = handle01.get_error_code()
                else:
                        rospy.logwarn("unexpected action result type <<%s>> for function %s", type(handle01), server_goal.function_name)
                if server_result.error_code == 0:
                        rospy.logdebug("action result success")
                        self.script_action_server.set_succeeded(server_result)
                else:
                        rospy.logerr("action result error, error_code: " + str(server_result.error_code))
                        self.script_action_server.set_aborted(server_result)

## Main routine for running the script server
#
if __name__ == '__main__':
        rospy.init_node('script_server')
        script_server()
        rospy.loginfo("script_server is running")
        rospy.spin()