motors.py
Go to the documentation of this file.
00001 # Software License Agreement (BSD License)
00002 #
00003 # Copyright (c) 2012, Willow Garage, Inc.
00004 # All rights reserved.
00005 #
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions
00008 # are met:
00009 #
00010 #  * Redistributions of source code must retain the above copyright
00011 #    notice, this list of conditions and the following disclaimer.
00012 #  * Redistributions in binary form must reproduce the above
00013 #    copyright notice, this list of conditions and the following
00014 #    disclaimer in the documentation and/or other materials provided
00015 #    with the distribution.
00016 #  * Neither the name of Willow Garage, Inc. nor the names of its
00017 #    contributors may be used to endorse or promote products derived
00018 #    from this software without specific prior written permission.
00019 #
00020 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 # POSSIBILITY OF SUCH DAMAGE.
00032 
00033 #
00034 # Ported from pr2_motors: Vincent Rabaud, Aldebaran Robotics, 2014
00035 #
00036 
00037 from python_qt_binding.QtGui import QMessageBox
00038 
00039 import actionlib
00040 import rospy
00041 from rqt_robot_dashboard.widgets import MenuDashWidget
00042 import std_srvs.srv
00043 
00044 from nao_msgs.msg import BodyPoseAction, BodyPoseGoal
00045 
00046 
00047 class Motors(MenuDashWidget):
00048     """
00049     Dashboard widget to display motor state and allow interaction.
00050     """
00051     def __init__(self, context):
00052         """
00053         :param context: the plugin context
00054         :type context: qt_gui.plugin.Plugin
00055         :param reset_callback: calback for the "reset" action
00056         :type reset_callback: function
00057         :param halt_callback: calback for the "reset" action
00058         :type halt_callback: function
00059         """
00060         ok_icon = ['bg-green.svg', 'ic-motors.svg']
00061         warn_icon = ['bg-yellow.svg', 'ic-motors.svg', 'ol-warn-badge.svg']
00062         err_icon = ['bg-red.svg', 'ic-motors.svg', 'ol-err-badge.svg']
00063         stale_icon = ['bg-grey.svg', 'ic-motors.svg', 'ol-stale-badge.svg']
00064 
00065         icons = [ok_icon, warn_icon, err_icon, stale_icon]
00066 
00067         super(Motors, self).__init__('Motors', icons)
00068         self.update_state(3)
00069 
00070         self.add_action('Init pose', self.on_init_pose)
00071         self.add_action('Sit down && remove stiffness', self.on_sit_down)
00072         self.add_action('Remove stiffness immediately', self.on_remove_stiffness)
00073 
00074         # clients for controlling the robot
00075         self.bodyPoseClient = actionlib.SimpleActionClient('body_pose', BodyPoseAction)
00076         self.stiffnessEnableClient = rospy.ServiceProxy("body_stiffness/enable", std_srvs.srv.Empty)
00077         self.stiffnessDisableClient = rospy.ServiceProxy("body_stiffness/disable", std_srvs.srv.Empty)
00078 
00079     def set_ok(self):
00080         self.update_state(0)
00081 
00082     def set_warn(self):
00083         self.update_state(1)
00084 
00085     def set_error(self):
00086         self.update_state(2)
00087 
00088     def set_stale(self):
00089         self.update_state(3)
00090       
00091     def on_init_pose(self):
00092         self.stiffnessEnableClient.call()
00093         self.bodyPoseClient.send_goal_and_wait(BodyPoseGoal(pose_name = 'init'))
00094   
00095     def on_sit_down(self):
00096         self.bodyPoseClient.send_goal_and_wait(BodyPoseGoal(pose_name = 'crouch'))
00097         state = self.bodyPoseClient.get_state()
00098         if state == actionlib.GoalStatus.SUCCEEDED:
00099             self.stiffnessDisableClient.call()
00100         else:
00101             QMessageBox(self, 'Error', 'crouch pose did not succeed: %s - cannot remove stiffness' % self.bodyPoseClient.get_goal_status_text())
00102             rospy.logerror("crouch pose did not succeed: %s", self.bodyPoseClient.get_goal_status_text())
00103 
00104     def on_remove_stiffness(self):
00105       reply = QMessageBox.question(self, 'Caution', 
00106                      'Robot may fall. Continue to remove stiffness?', QMessageBox.Yes, QMessageBox.No)
00107       if(reply == QMessageBox.Yes):
00108           self.stiffnessDisableClient.call()
00109 
00110     def on_halt_motors(self, evt):
00111         halt = rospy.ServiceProxy("pr2_etherCAT/halt_motors", std_srvs.srv.Empty)
00112         
00113         try:
00114             halt()
00115         except rospy.ServiceException, e:
00116             QMessageBox(self, 'Error',
00117                      'Failed to halt the motors: service call failed with error: %s'%(e))


nao_dashboard
Author(s): Stefan Osswald
autogenerated on Mon Oct 6 2014 02:41:12