00001 try: 00002 from hrpsys_ros_bridge.hrpsys_dashboard import HrpsysDashboard 00003 except: 00004 import roslib; roslib.load_manifest("hrpsys_gazebo_atlas") 00005 from hrpsys_ros_bridge.hrpsys_dashboard import HrpsysDashboard 00006 00007 from rqt_robot_dashboard.widgets import MenuDashWidget 00008 from python_qt_binding.QtCore import QSize 00009 from python_qt_binding.QtGui import QMessageBox 00010 import random 00011 import rospy 00012 00013 from atlas_msgs.msg import AtlasSimInterfaceCommand 00014 from std_msgs.msg import String 00015 from sandia_hand_msgs.msg import SimpleGrasp 00016 00017 class SimCommandMenu(MenuDashWidget): 00018 def __init__(self): 00019 icons = [['bg-green.svg', 'ic-runstop-off.svg'],] 00020 super(SimCommandMenu, self).__init__('SimCommand', icons) 00021 self.update_state(0) 00022 self.add_action('UserMode', self.send_user_command) 00023 self.add_action('StandMode', self.send_stand_command) 00024 self.setFixedSize(self._icons[0].actualSize(QSize(50, 30))) 00025 self.sim_command_pub = rospy.Publisher('/atlas/atlas_sim_interface_command', AtlasSimInterfaceCommand, queue_size=10) 00026 def send_user_command(self): 00027 rospy.loginfo("set user mode") 00028 _sim_msg = AtlasSimInterfaceCommand(behavior = 1, k_effort = [255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]) 00029 _sim_msg.header.stamp = rospy.Time.now() 00030 self.sim_command_pub.publish(_sim_msg) 00031 def send_stand_command(self): 00032 rospy.loginfo("set stand mode") 00033 _sim_msg = AtlasSimInterfaceCommand(behavior = 0, k_effort = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) 00034 _sim_msg.header.stamp = rospy.Time.now() 00035 self.sim_command_pub.publish(_sim_msg) 00036 00037 class SimModeMenu(MenuDashWidget): 00038 def __init__(self): 00039 icons = [['bg-green.svg', 'ic-steering-wheel.svg'],] 00040 super(SimModeMenu, self).__init__('SimMode', icons) 00041 self.update_state(0) 00042 self.add_action('Pinned', self.set_pinned_mode) 00043 self.add_action('Unpinned', self.set_unpinned_mode) 00044 self.setFixedSize(self._icons[0].actualSize(QSize(50, 30))) 00045 self.sim_command_pub = rospy.Publisher('/atlas/mode', String, queue_size=10) 00046 def set_pinned_mode(self): 00047 rospy.loginfo("set pinned mode") 00048 self.sim_command_pub.publish("pinned") 00049 def set_unpinned_mode(self): 00050 rospy.loginfo("set unpinned mode") 00051 self.sim_command_pub.publish("nominal") 00052 00053 class HandCommandMenu(MenuDashWidget): 00054 def __init__(self): 00055 icons = [['bg-green.svg', 'ic-wrench.svg'], 00056 ['bg-red.svg', 'ic-wrench.svg'],] 00057 super(HandCommandMenu, self).__init__('HandCommand', icons) 00058 self.update_state(0) 00059 self.add_action('GraspHand', self.grasp_hand) 00060 self.add_action('OpenHand', self.open_hand) 00061 self.setFixedSize(self._icons[0].actualSize(QSize(50, 30))) 00062 self.left_hand_command_pub = rospy.Publisher('/sandia_hands/l_hand/simple_grasp', SimpleGrasp, queue_size=10) 00063 self.right_hand_command_pub = rospy.Publisher('/sandia_hands/r_hand/simple_grasp', SimpleGrasp, queue_size=10) 00064 def grasp_hand(self): 00065 rospy.loginfo("grasp_hand") 00066 _hand_msg = SimpleGrasp(name="cylindrical", closed_amount=1) 00067 self.left_hand_command_pub.publish(_hand_msg) 00068 self.right_hand_command_pub.publish(_hand_msg) 00069 self.update_state(1) 00070 def open_hand(self): 00071 rospy.loginfo("open hand") 00072 _hand_msg = SimpleGrasp(name="cylindrical", closed_amount=0) 00073 self.left_hand_command_pub.publish(_hand_msg) 00074 self.right_hand_command_pub.publish(_hand_msg) 00075 self.update_state(0) 00076 00077 00078 class AtlasHrpsysDashboard(HrpsysDashboard): 00079 def setup(self, context): 00080 super(AtlasHrpsysDashboard, self).setup(context) 00081 self.name = "atlas_hrpsys dashboard" 00082 self._sim_command_button = SimCommandMenu() 00083 self._sim_mode_button = SimModeMenu() 00084 self._hand_command_button = HandCommandMenu() 00085 def get_widgets(self): 00086 widgets = [] 00087 atlas_widgets = [] 00088 if self._rtcd: 00089 widgets.append(self._rtcd) 00090 if self._ros_bridge: 00091 widgets.append(self._ros_bridge) 00092 if self._power: 00093 widgets.append(self._power) 00094 if self._servo: 00095 widgets.append(self._servo) 00096 if self._sim_command_button: 00097 atlas_widgets.append(self._sim_command_button) 00098 if self._sim_mode_button: 00099 atlas_widgets.append(self._sim_mode_button) 00100 if self._hand_command_button: 00101 atlas_widgets.append(self._hand_command_button) 00102 return [[self._monitor, self._console, self._log], widgets, atlas_widgets] 00103 def dashboard_callback(self, msg): 00104 super(AtlasHrpsysDashboard, self).dashboard_callback(msg) 00105