command_gui_buttons.py

Go to the documentation of this file.

#!/usr/bin/env python
#
# Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#   http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.


import rospy
from simple_script_server import *

## Implements configurable buttons
class command_gui_buttons:
        def __init__(self):
                self.sss = simple_script_server()
                self.panels = []
                self.stop_buttons = []
                self.init_buttons = []
                self.recover_buttons = []
                self.halt_buttons = []
                self.CreateControlPanel()

        ## Creates the control panel out of configuration from ROS parameter server
        def CreateControlPanel(self):
                param_prefix = "~control_buttons"
                if not rospy.has_param(param_prefix):
                        rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",param_prefix)
                        return False
                group_param = rospy.get_param(param_prefix)
                #print group_param
                group_param = self.SortDict(group_param)
                #print group_param

                for group in group_param:
                        group_name = group[1]["group_name"]
                        component_name = group[1]["component_name"] # \todo check component name with robot_components.yaml files
                        button_list = group[1]["buttons"]
                        buttons = []
                        for button in button_list:
                                #print "button = ",button
                                if button[1] == "move":
                                        buttons.append(self.CreateButton(button[0],self.sss.move,component_name,button[2]))
                                elif button[1] == "move_base_rel":
                                        buttons.append(self.CreateButton(button[0],self.sss.move_base_rel,component_name,button[2]))
                                elif button[1] == "trigger_action":
                                        buttons.append(self.CreateButton(button[0],self.sss.trigger_action,component_name,button[2],True))
                                elif button[1] == "string_action":
                                        buttons.append(self.CreateButton(button[0],self.sss.string_action,component_name,button[2],True))
                                elif button[1] == "trigger":
                                        buttons.append(self.CreateButton(button[0],self.sss.trigger,component_name,button[2]))
                                        if button[2] == "stop":
                                                self.stop_buttons.append(component_name)
                                        if button[2] == "init":
                                                self.init_buttons.append(component_name)
                                        if button[2] == "recover":
                                                self.recover_buttons.append(component_name)
                                        if button[2] == "halt":
                                                self.halt_buttons.append(component_name)
                                elif button[1] == "stop":
                                        buttons.append(self.CreateButton(button[0],self.sss.stop,component_name))
                                        self.stop_buttons.append(component_name)
                                elif button[1] == "init":
                                        buttons.append(self.CreateButton(button[0],self.sss.init,component_name))
                                        self.init_buttons.append(component_name)
                                elif button[1] == "recover":
                                        buttons.append(self.CreateButton(button[0],self.sss.recover,component_name))
                                        self.recover_buttons.append(component_name)
                                elif button[1] == "halt":
                                        buttons.append(self.CreateButton(button[0],self.sss.halt,component_name))
                                        self.halt_buttons.append(component_name)
                                else:
                                        rospy.logerr("Function <<%s>> not known to command gui",button[1])
                                        return False
                        group = (group_name,buttons)

                        # add nav buttons (optional)
                        if component_name == "base": # \todo get base name from robot_components.yaml
                                param_prefix = "~nav_buttons"
                                if rospy.has_param(param_prefix):
                                        nav_buttons_param = rospy.get_param(param_prefix)
                                        nav_button_list = nav_buttons_param["buttons"]
                                        #print nav_button_list
                                        for button in nav_button_list:
                                                #print "button = ",button
                                                buttons.append(self.CreateButton(button[0],self.sss.move,component_name,button[2]))
                                else:
                                        rospy.logwarn("parameter %s does not exist on ROS Parameter Server, no nav buttons will be available.",param_prefix)
                        self.panels.append(group)

                # uniqify lists to not have double entries
                self.stop_buttons = self.uniqify_list(self.stop_buttons)
                self.init_buttons = self.uniqify_list(self.init_buttons)
                self.recover_buttons = self.uniqify_list(self.recover_buttons)
                self.halt_buttons = self.uniqify_list(self.halt_buttons)


        ## Creates one button with functionality
        def CreateButton(self,button_name,function,component_name,parameter_name=None,blocking=False):
                if parameter_name == None:
                        button = (button_name,function,(component_name,blocking))
                else:
                        button = (button_name,function,(component_name,parameter_name,blocking))
                return button

        ## Sorts a dictionary alphabetically
        def SortDict(self,dictionary):
                keys = sorted(dictionary.keys())
                k=[]
                #print "keys = ", keys
                #for key in keys:
                #       print "values = ", dictionary[key]
                return [[key,dictionary[key]] for key in keys]

        def idfun(self,x):
                return x

        ## Uniqifies a list to not have double entries
        def uniqify_list(self,seq, idfun=None):
                # order preserving
                if idfun is None:
                        idfun=self.idfun
                seen = {}
                result = []
                for item in seq:
                        marker = idfun(item)
                        # in old Python versions:
                        # if seen.has_key(marker)
                        # but in new ones:
                        if marker in seen: continue
                        seen[marker] = 1
                        result.append(item)
                return result