actuator_array_gui: actuator_array_gui

Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 
00003 #***********************************************************
00004 #* Software License Agreement (BSD License)
00005 #*
00006 #* Copyright (c) 2011, A.M.Howard, S.Williams
00007 #* All rights reserved.
00008 #* 
00009 #* Redistribution and use in source and binary forms, with or without
00010 #* modification, are permitted provided that the following conditions are met:
00011 #*     * Redistributions of source code must retain the above copyright
00012 #*       notice, this list of conditions and the following disclaimer.
00013 #*     * Redistributions in binary form must reproduce the above copyright
00014 #*       notice, this list of conditions and the following disclaimer in the
00015 #*       documentation and/or other materials provided with the distribution.
00016 #*     * Neither the name of the <organization> nor the
00017 #*       names of its contributors may be used to endorse or promote products
00018 #*       derived from this software without specific prior written permission.
00019 #*  
00020 #* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00021 #* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00022 #* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00023 #* DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
00024 #* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00025 #* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00026 #* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00027 #* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00028 #* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00029 #* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00030 #***********************************************************
00031 
00032 import roslib;
00033 roslib.load_manifest('actuator_array_gui')
00034 
00035 import rospy
00036 from std_srvs.srv import *
00037 from sensor_msgs.msg import JointState
00038 from actuator_array_gui.joint_panel import JointPanel
00039 
00040 from xml.dom.minidom import parse, parseString
00041 
00042 import wx
00043 import wx.lib.scrolledpanel
00044 
00045 
00046 class ActuatorArrayGuiFrame(wx.Frame):
00047 
00048         class ActuatorData:
00049                 name = None
00050                 min_position = -3.14159
00051                 max_position = +3.14159
00052                 max_velocity = 100.0
00053                 max_effort = 100.0
00054 
00055 
00056         def __init__(self):
00057     
00058                 # Create a map to hold the actuators to control
00059                 self.actuators = []
00060 
00061                 # Initialize ROS Node
00062                 rospy.init_node('actuator_array_gui')
00063 
00064                 # Get actual remapped node name
00065                 node_name = rospy.get_name()
00066 
00067                 # Call base constructor
00068                 wx.Frame.__init__(self, None, wx.ID_ANY, title=node_name, size=(550, 400))
00069 
00070                 # Read list of joints to control from Parameter Server
00071                 # The list may be a complex list of structs, or a simple list of joint names
00072                 joints_array = rospy.get_param("~joints")
00073                 for joint in joints_array:
00074                         # Create an ActuatorData structure to hold information about each joint
00075                         actuator = self.ActuatorData()
00076 
00077                         # Get the name of the actuator from the "joints" parameter
00078                         if isinstance(joint, str):
00079                                 actuator.name = joint
00080                         else:
00081                                 actuator.name = joint["name"]
00082 
00083                         # Add joint to the list of actuators to control
00084                         self.actuators.append(actuator)
00085         
00086                 # Extract joint properties from the robot_description URDF
00087                 robot_description_parameter = rospy.get_param("~robot_description_parameter", "robot_description")
00088                 if len(robot_description_parameter) > 0:
00089                         robot_description_parameter = rospy.resolve_name(robot_description_parameter)
00090                         urdf_string = rospy.get_param(robot_description_parameter)
00091                         urdf_doc = parseString(urdf_string)
00092                         joints = urdf_doc.getElementsByTagName("joint")
00093                         for joint in joints:
00094                                 name = joint.getAttribute("name")
00095                                 indices = [index for index,actuator in enumerate(self.actuators) if actuator.name == name]
00096                                 if indices:
00097                                         index = indices[0]
00098                                         limits = joint.getElementsByTagName("limit")
00099                                         for limit in limits:
00100                                                 if limit.hasAttribute("lower"):
00101                                                         self.actuators[index].min_position = float(limit.getAttribute("lower"))
00102                                                 if limit.hasAttribute("upper"):
00103                                                         self.actuators[index].max_position = float(limit.getAttribute("upper"))
00104                                                 if limit.hasAttribute("velocity"):
00105                                                         self.actuators[index].max_velocity = float(limit.getAttribute("velocity"))
00106                                                 if limit.hasAttribute("effort"):
00107                                                         self.actuators[index].max_effort = float(limit.getAttribute("effort"))
00108         
00109                 # Advertise Commands
00110                 self.command_pub = rospy.Publisher("command", JointState)
00111                 self.command_msg = JointState()
00112                 for actuator in self.actuators:
00113                         self.command_msg.name.append(actuator.name)
00114                 self.command_msg.position = [0] * len(self.actuators)
00115                 self.command_msg.velocity = [0] * len(self.actuators)
00116                 self.command_msg.effort = [0] * len(self.actuators)
00117 
00118                 # Subscribe to JointStates
00119                 self.joint_state_sub = rospy.Subscriber("joint_states", JointState, self.joint_states_callback, None, 1)
00120                 self.joint_state_msg = JointState()
00121                 for actuator in self.actuators:
00122                         self.joint_state_msg.name.append(actuator.name)
00123                 self.joint_state_msg.position = [None] * len(self.actuators)
00124                 self.joint_state_msg.velocity = [None] * len(self.actuators)
00125                 self.joint_state_msg.effort = [None] * len(self.actuators)
00126 
00127                 # Create Service Call Proxies
00128                 self.srv_home = rospy.ServiceProxy('home', Empty)
00129                 self.srv_stop = rospy.ServiceProxy('stop', Empty)
00130 
00131                 # Update the display every 10th of a second
00132                 self.update_timer = wx.Timer(self, wx.ID_ANY)
00133                 self.Bind(wx.EVT_TIMER, self._on_update_timer, self.update_timer)
00134                 self.update_timer.Start(100, False)
00135 
00136 
00137                 # Create menu
00138                 self.menubar = wx.MenuBar()
00139                 self.filemenu = wx.Menu()
00140                 self.filemenu.Append(wx.ID_EXIT, 'E&xit', 'Exit the program')
00141                 wx.EVT_MENU(self, wx.ID_EXIT, self.on_exit)
00142                 self.menubar.Append(self.filemenu, '&File')
00143                 self.SetMenuBar(self.menubar)
00144 
00145                 # Create main panel
00146                 self.main_panel = wx.Panel(self, wx.ID_ANY)
00147 
00148 
00149                 # Create panel to hold joint controls
00150                 self.joint_panel = wx.lib.scrolledpanel.ScrolledPanel(self.main_panel, wx.ID_ANY)
00151 
00152 
00153                 joint_panel_sizer = wx.BoxSizer(wx.VERTICAL)
00154                 title_font = wx.Font(pointSize=12, family=wx.DEFAULT, style=wx.NORMAL, weight=wx.BOLD)
00155                 
00156                 # Create Joint Controls
00157                 self.joint_command_panels = []
00158                 self.joint_status_panels = []
00159                 for actuator in self.actuators:
00160                         # Create label and controls for each joint
00161                         joint_label = wx.StaticText(self.joint_panel, wx.ID_ANY, actuator.name)
00162                         joint_label.SetFont(title_font)
00163                         joint_command_label = wx.StaticText(self.joint_panel, wx.ID_ANY, 'Command')
00164                         joint_command_panel = JointPanel(self.joint_panel, joint_name=actuator.name, min_position=actuator.min_position, max_position=actuator.max_position, max_velocity=actuator.max_velocity, max_effort=actuator.max_effort, input_mode=True)
00165                         joint_status_label = wx.StaticText(self.joint_panel, wx.ID_ANY, 'Status')
00166                         joint_status_panel = JointPanel(self.joint_panel, joint_name=actuator.name, min_position=actuator.min_position, max_position=actuator.max_position, max_velocity=actuator.max_velocity, max_effort=actuator.max_effort, input_mode=False)
00167                         sizer_joint = wx.FlexGridSizer(rows=2, cols=2, vgap=2, hgap=0)
00168                         sizer_joint.SetFlexibleDirection(wx.BOTH)
00169                         sizer_joint.AddGrowableCol(1, 1)
00170                         sizer_joint.Add(joint_command_label, 0, wx.ALIGN_CENTER_VERTICAL | wx.ALL, 5)
00171                         sizer_joint.Add(joint_command_panel, 1, wx.EXPAND)
00172                         sizer_joint.Add(joint_status_label, 0, wx.ALIGN_CENTER_VERTICAL | wx.ALL, 5)
00173                         sizer_joint.Add(joint_status_panel, 1, wx.EXPAND)
00174 
00175                         joint_panel_sizer.Add((1,1), 1, wx.CENTER)
00176                         joint_panel_sizer.Add(joint_label, 0, wx.CENTER | wx.ALL, 5)
00177                         joint_panel_sizer.Add(sizer_joint, 0, wx.EXPAND | wx.LEFT | wx.RIGHT, 5)
00178 
00179                         self.joint_command_panels.append(joint_command_panel)
00180                         self.joint_status_panels.append(joint_status_panel)
00181 
00182                 self.joint_panel.SetSizer(joint_panel_sizer)
00183                 self.joint_panel.SetupScrolling()
00184 
00185 
00186                 # Create panel to hold service buttons, etc
00187                 self.button_panel = wx.Panel(self.main_panel, wx.ID_ANY)
00188 
00189                 # Add buttons
00190                 self.home_button = wx.Button(self.button_panel, wx.ID_ANY, 'Home')
00191                 self.stop_button = wx.Button(self.button_panel, wx.ID_ANY, 'Stop')
00192                 self.send_button = wx.Button(self.button_panel, wx.ID_ANY, 'Send')
00193                 self.auto_button = wx.ToggleButton(self.button_panel, wx.ID_ANY, 'Auto Send')
00194 
00195                 self.home_button.Bind(wx.EVT_BUTTON, self._on_home_button)
00196                 self.stop_button.Bind(wx.EVT_BUTTON, self._on_stop_button)
00197                 self.send_button.Bind(wx.EVT_BUTTON, self._on_send_button)
00198                 self.auto_button.Bind(wx.EVT_TOGGLEBUTTON, self._on_auto_button)
00199 
00200                 # Add to button panel sizer
00201                 button_panel_sizer = wx.BoxSizer(wx.HORIZONTAL)
00202                 button_panel_sizer.Add(self.home_button, 0, wx.CENTER | wx.ALL, 10)
00203                 button_panel_sizer.Add((1,1), 1, wx.CENTER | wx.ALL, 10)
00204                 button_panel_sizer.Add(self.stop_button, 0, wx.CENTER | wx.ALL, 10)
00205                 button_panel_sizer.Add((1,1), 1, wx.CENTER | wx.ALL, 10)
00206                 button_panel_sizer.Add(self.send_button, 0, wx.CENTER | wx.ALL, 10)
00207                 button_panel_sizer.Add((1,1), 1, wx.CENTER | wx.ALL, 10)
00208                 button_panel_sizer.Add(self.auto_button, 0, wx.CENTER | wx.ALL, 10)
00209 
00210                 self.button_panel.SetSizer(button_panel_sizer)
00211 
00212                 # add the two main panels to a sizer
00213                 main_panel_sizer = wx.BoxSizer(wx.VERTICAL)
00214                 main_panel_sizer.Add(self.joint_panel, 1, wx.EXPAND)
00215                 main_panel_sizer.Add(self.button_panel, 0, wx.EXPAND)
00216                 self.main_panel.SetSizer(main_panel_sizer)
00217 
00218         def on_exit(self, e):
00219                 self.Close(True)
00220                 self.Refresh()
00221 
00222         def _on_home_button(self, event):
00223                 try:
00224                         # Turn off auto-send. Would cause robot to immediately start back
00225                         self.auto_button.SetValue(False)
00226                         self.send_button.Enable(True)
00227                         # Send a 'home" service request
00228                         resp = self.srv_home()
00229                 except rospy.ServiceException, e:
00230                         print "Service call failed: %s"%e
00231 
00232         def _on_stop_button(self, event):
00233                 try:
00234                         # Turn off auto-send. Would cause robot to immediately start moving again
00235                         self.auto_button.SetValue(False)
00236                         self.send_button.Enable(True)
00237                         # Send a 'stop" service request
00238                         resp = self.srv_stop()
00239                 except rospy.ServiceException, e:
00240                         print "Service call failed: %s"%e
00241 
00242         def _on_send_button(self, event):
00243                 self.command_publish()
00244 
00245         def _on_auto_button(self, event):
00246                 self.send_button.Enable(not self.auto_button.GetValue())
00247 
00248         def _on_update_timer(self, event):
00249                 # Loop over each status panel, update the properties from the message
00250                 for i in range(len(self.actuators)):
00251                         self.joint_status_panels[i].position = self.joint_state_msg.position[i]
00252                         self.joint_status_panels[i].velocity = self.joint_state_msg.velocity[i]
00253                         self.joint_status_panels[i].effort = self.joint_state_msg.effort[i]
00254                         self.joint_status_panels[i].update_panel()
00255 
00256                 # If the auto-send flag is set, send out a command as well
00257                 if self.auto_button.GetValue() == True:
00258                         self.command_publish()
00259 
00260         def joint_states_callback(self, msg):
00261                 # Copy data about matching joint names into the local joint_state message
00262                 for i in range(len(msg.name)):
00263                         indices = [index for index,actuator in enumerate(self.actuators) if actuator.name == msg.name[i]]
00264                         if indices:
00265                                 j = indices[0]  
00266                                 self.joint_state_msg.position[j] = msg.position[i]
00267                                 self.joint_state_msg.velocity[j] = msg.velocity[i]
00268                                 self.joint_state_msg.effort[j] = msg.effort[i]
00269 
00270         def command_publish(self):
00271                 # Get current values from the joint command panel and save in command message
00272                 for i in range(len(self.actuators)):
00273                         self.command_msg.position[i] = self.joint_command_panels[i].position
00274                         self.command_msg.velocity[i] = self.joint_command_panels[i].velocity
00275                         self.command_msg.effort[i] = self.joint_command_panels[i].effort
00276                 self.command_msg.header.stamp = rospy.Time.now()
00277                 self.command_pub.publish(self.command_msg)