1. Mike Jacobs
  2. inmoov_ros

Source

inmoov_ros / inmoov / src / inmoov / command / command_processor.py

#! /usr/bin/env python

# Copyright (c) 2017 Mike Jacobs
# 
# Permission is hereby granted, free of charge, to any person obtaining 
# a copy of this software and associated documentation files 
# (the "Software"), to deal in the Software without restriction, 
# including without limitation the rights to use, copy, modify, merge, 
# publish, distribute, sublicense, and/or sell copies of the Software, 
# and to permit persons to whom the Software is furnished to do so, 
# subject to the following conditions:
# 
# The above copyright notice and this permission notice shall be included
# in all copies or substantial portions of the Software.
# 
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 
# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 
# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. 
# IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY 
# CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, 
# TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 
# SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
# 

import rospy
from inmoov.msg import Command
from command_constants import Action, Component, Context
from inmoov.srv import CommandService, CommandServiceResponse
from none_handler import NoneHandler
from network_handler import NetworkHandler
from conversation_handler import ConversationHandler

NODE_NAME = 'command_processor'
COMMAND_TOPIC = 'command' 
COMMAND_QUEUE_SIZE = 2
COMMAND_SERVICE_NAME = 'command'
        
class CommandProcessor(object):
        
    def __init__(self):
        self.__handlers = {}
        rospy.init_node(NODE_NAME)
        rospy.on_shutdown(self.__onShutdown)
        self.__commandSubscriber = rospy.Subscriber(COMMAND_TOPIC, Command, self.__handleCommand, queue_size = COMMAND_QUEUE_SIZE)
        self.__service = rospy.Service(COMMAND_SERVICE_NAME, CommandService, self.__process)
    
    def __process(self, request):
        command = request.command
        text = self.__handleCommand(command)
        response = CommandServiceResponse(text)
        return response
        
    def __handleCommand(self, command):
        component = command.component
        context = command.context
       # print("Handling command for {0}".format(component))
        handler = self.__defaultHandler
        if context in self.__handlers:
            handler = self.__handlers[context]
        elif component in self.__handlers:
            handler = self.__handlers[component]
            
        text = handler._handle(command)
        print(text)
        return text
          
    def register(self, aCommandHandler, default = False):
        key = aCommandHandler._getKey()
        self.__handlers[key] = aCommandHandler
        if default:
            self.__defaultHandler = aCommandHandler
        
    def __onShutdown(self):
        self.__commandSubscriber.unregister()
        self.__service.shutdown()

try:  
    processor = CommandProcessor()
    processor.register(NoneHandler(), True)
    processor.register(NetworkHandler())
    processor.register(ConversationHandler())
    rospy.spin()
except:
    import sys, traceback
    rospy.logwarn("Command Processor FAILED")
    exc_type, exc_value, exc_traceback = sys.exc_info()
    traceback.print_exception(exc_type, exc_value, exc_traceback, limit=5, file=sys.stdout)