$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 # Live visualization of a scipt. 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 gtk 00061 import gtk.gdk 00062 import roslib; roslib.load_manifest('cob_script_server') 00063 import rospy 00064 import xdot 00065 from std_msgs.msg import String 00066 from cob_script_server.msg import * 00067 00068 import pygraphviz as pgv 00069 00070 gtk.gdk.threads_init() 00071 00072 00073 # check, if graph is available on parameter server 00074 if rospy.has_param('script_server/graph'): 00075 dotcode = rospy.get_param("script_server/graph") 00076 G=pgv.AGraph(dotcode) 00077 else: 00078 G=pgv.AGraph() 00079 G.add_node('no graph available') 00080 dotcode = G.string() 00081 00082 ## Graph callback. 00083 def graph_cb(msg): 00084 print "new graph received" 00085 global dotcode 00086 global G 00087 dotcode = msg.data 00088 print dotcode 00089 G=pgv.AGraph(dotcode) 00090 00091 # update vizualisation 00092 gtk.gdk.threads_enter() 00093 widget.set_dotcode(dotcode) 00094 widget.zoom_to_fit() 00095 gtk.gdk.threads_leave() 00096 00097 00098 ## State callback. 00099 def state_cb(msg): 00100 global widget 00101 00102 # modify active node 00103 active_node = msg.full_graph_name 00104 rospy.loginfo("Received state <<%s>> from node <<%s>>",str(msg.state),active_node) 00105 try: 00106 n=G.get_node(active_node) 00107 except: 00108 rospy.logwarn("Node <<%s>> not found in graph",active_node) 00109 return 00110 n.attr['style']='filled' 00111 if msg.state == ScriptState.UNKNOWN: 00112 n.attr['fillcolor']='white' 00113 elif msg.state == ScriptState.ACTIVE: 00114 n.attr['fillcolor']='yellow' 00115 elif msg.state == ScriptState.SUCCEEDED: 00116 n.attr['fillcolor']='green' 00117 elif msg.state == ScriptState.FAILED: 00118 n.attr['fillcolor']='red' 00119 elif msg.state == ScriptState.PAUSED: 00120 n.attr['fillcolor']='orange' 00121 else: 00122 n.attr['fillcolor']='blue' 00123 dotcode = G.string() 00124 00125 # update vizualisation 00126 gtk.gdk.threads_enter() 00127 widget.set_dotcode(dotcode) 00128 widget.zoom_to_fit() 00129 gtk.gdk.threads_leave() 00130 00131 00132 # create gtk window 00133 window = gtk.Window() 00134 window.set_title('script viewer') 00135 window.set_default_size(600, 800) 00136 vbox = gtk.VBox() 00137 window.add(vbox) 00138 00139 widget = xdot.DotWidget() 00140 widget.set_dotcode(dotcode) 00141 widget.zoom_to_fit() 00142 00143 00144 vbox.pack_start(widget) 00145 00146 window.show_all() 00147 00148 window.connect('destroy', gtk.main_quit) 00149 00150 rospy.init_node('script_viewer', anonymous=True) 00151 rospy.Subscriber("/script_server/graph", String, graph_cb) 00152 rospy.Subscriber("/script_server/state", ScriptState, state_cb) 00153 gtk.main()