pattern_generator_widget.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import math
00004 
00005 import rospy
00006 import tf
00007 import std_msgs.msg
00008 
00009 from rqt_gui_py.plugin import Plugin
00010 from python_qt_binding.QtCore import Qt, Slot, QAbstractListModel
00011 from python_qt_binding.QtGui import QWidget, QHBoxLayout, QVBoxLayout, QCheckBox, QLabel, QListWidget, QPushButton, QDoubleSpinBox, QFrame
00012 
00013 from vigir_footstep_planning_msgs.msg import PatternGeneratorParameters
00014 from vigir_footstep_planning_lib.parameter_set_widget import *
00015 from vigir_footstep_planning_lib.qt_helper import *
00016 from vigir_footstep_planning_lib.logging import *
00017 
00018 
00019 class PatternGeneratorDialog(Plugin):
00020 
00021     def __init__(self, context):
00022         super(PatternGeneratorDialog, self).__init__(context)
00023         self.setObjectName('PatternGeneratorDialog')
00024 
00025         self._parent = QWidget()
00026         self._widget = PatternGeneratorWidget(self._parent)
00027 
00028         context.add_widget(self._parent)
00029 
00030     def shutdown_plugin(self):
00031         self._widget.shutdown_plugin()
00032 
00033 
00034 class PatternGeneratorWidget(QObject):
00035 
00036     enable_pattern_generator = False
00037 
00038     def __init__(self, context):
00039         super(PatternGeneratorWidget, self).__init__()
00040 
00041         # publisher
00042         self.pattern_generator_params_pub = rospy.Publisher('pattern_generator/set_params', PatternGeneratorParameters, queue_size = 1)
00043 
00044         # start widget
00045         widget = context
00046 
00047         # start upper part
00048         hbox = QHBoxLayout()
00049 
00050         # start left column
00051         left_vbox = QVBoxLayout()
00052 
00053         # start button
00054         start_command = QPushButton("Start")
00055         left_vbox.addWidget(start_command)
00056 
00057         # simulation checkbox
00058         self.simulation_mode_checkbox = QCheckBox()
00059         self.simulation_mode_checkbox.setText("Simulation Mode")
00060         self.simulation_mode_checkbox.setChecked(False)
00061         left_vbox.addWidget(self.simulation_mode_checkbox)
00062 
00063         # realtime checkbox
00064         self.realtime_mode_checkbox = QCheckBox()
00065         self.realtime_mode_checkbox.setText("Realtime Mode")
00066         self.realtime_mode_checkbox.setChecked(False)
00067         left_vbox.addWidget(self.realtime_mode_checkbox)
00068 
00069         # joystick checkbox
00070         self.joystick_mode_checkbox = QCheckBox()
00071         self.joystick_mode_checkbox.setText("Joystick Mode")
00072         self.joystick_mode_checkbox.setChecked(False)
00073         left_vbox.addWidget(self.joystick_mode_checkbox)
00074 
00075         # ignore invalid steps checkbox
00076         self.ignore_invalid_steps_checkbox = QCheckBox()
00077         self.ignore_invalid_steps_checkbox.setText("Ignore Invalid Steps")
00078         self.ignore_invalid_steps_checkbox.setChecked(False)
00079         left_vbox.addWidget(self.ignore_invalid_steps_checkbox)
00080 
00081         # foot seperation
00082         self.foot_seperation = generate_q_double_spin_box(0.2, 0.15, 0.3, 2, 0.01)
00083         self.foot_seperation.valueChanged.connect(self.callback_spin_box)
00084         add_widget_with_frame(left_vbox, self.foot_seperation, "Foot Seperation (m):")
00085 
00086         # delta x
00087         self.delta_x = generate_q_double_spin_box(0.0, -0.4, 0.4, 2, 0.01)
00088         self.delta_x.valueChanged.connect(self.callback_spin_box)
00089         add_widget_with_frame(left_vbox, self.delta_x, "dX (m):")
00090 
00091         # delta y
00092         self.delta_y = generate_q_double_spin_box(0.0, -2.2, 2.2, 2, 0.01)
00093         self.delta_y.valueChanged.connect(self.callback_spin_box)
00094         add_widget_with_frame(left_vbox, self.delta_y, "dY (m):")
00095 
00096         # delta yaw
00097         self.delta_yaw = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
00098         self.delta_yaw.valueChanged.connect(self.callback_spin_box)
00099         add_widget_with_frame(left_vbox, self.delta_yaw, "dYaw (deg):")
00100 
00101         # roll
00102         self.roll = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
00103         self.roll.valueChanged.connect(self.callback_spin_box)
00104         add_widget_with_frame(left_vbox, self.roll, "Roll (deg):")
00105 
00106         # pitch
00107         self.pitch = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
00108         self.pitch.valueChanged.connect(self.callback_spin_box)
00109         add_widget_with_frame(left_vbox, self.pitch, "Pitch (deg):")
00110 
00111         # end left column
00112         left_vbox.addStretch()
00113         hbox.addLayout(left_vbox, 1)
00114 
00115         # start right column
00116         right_vbox = QVBoxLayout()
00117 
00118         # stop button
00119         stop_command = QPushButton("Stop")
00120         right_vbox.addWidget(stop_command)
00121 
00122         # ignore collision
00123         self.collision_checkbox = QCheckBox()
00124         self.collision_checkbox.setText("Ignore Collision")
00125         self.collision_checkbox.setChecked(True)
00126         right_vbox.addWidget(self.collision_checkbox)
00127 
00128         # override 3D
00129         self.override_checkbox = QCheckBox()
00130         self.override_checkbox.setText("Override 3D")
00131         self.override_checkbox.setChecked(False)
00132         right_vbox.addWidget(self.override_checkbox)
00133 
00134         # end right coloumn
00135         right_vbox.addStretch()
00136         hbox.addLayout(right_vbox, 1)
00137 
00138         # add upper part
00139         hbox.setMargin(0)
00140         vbox = QVBoxLayout()
00141         vbox.addLayout(hbox)
00142 
00143         # parameter set selection
00144         self.parameter_set_widget = QParameterSetWidget()
00145         add_widget_with_frame(vbox, self.parameter_set_widget, "Parameter Set:")
00146 
00147         # end widget
00148         widget.setLayout(vbox)
00149         #context.add_widget(widget)
00150 
00151         # signal connections
00152         start_command.clicked.connect(self.start_command_callback)
00153         stop_command.clicked.connect(self.stop_command_callback)
00154         self.joystick_mode_checkbox.clicked.connect(self.joystick_mode_check_callback)
00155         self.ignore_invalid_steps_checkbox.clicked.connect(self._publish_parameters)
00156 
00157     def shutdown_plugin(self):
00158         print "Shutting down ..."
00159         self.pattern_generator_params_pub.unregister()
00160         print "Done!"
00161 
00162     # message publisher
00163     def _publish_parameters(self):
00164         params = PatternGeneratorParameters()
00165 
00166         params.enable = self.enable_pattern_generator
00167         params.simulation_mode = self.simulation_mode_checkbox.isChecked()
00168         params.joystick_mode = self.joystick_mode_checkbox.isChecked()
00169         params.ignore_invalid_steps = self.ignore_invalid_steps_checkbox.isChecked()
00170         params.d_step.position.x = self.delta_x.value()
00171         params.d_step.position.y = self.delta_y.value()
00172         params.d_step.position.z = 0
00173         q = tf.transformations.quaternion_from_euler(math.radians(self.roll.value()), math.radians(self.pitch.value()), math.radians(self.delta_yaw.value()))
00174         params.d_step.orientation.x = q[0]
00175         params.d_step.orientation.y = q[1]
00176         params.d_step.orientation.z = q[2]
00177         params.d_step.orientation.w = q[3]
00178         params.foot_seperation = self.foot_seperation.value()
00179         params.parameter_set_name.data = self.parameter_set_widget.current_parameter_set_name()
00180 
00181         print "Send stepping command = \n",params
00182         self.pattern_generator_params_pub.publish(params)
00183 
00184     # Define system command strings
00185     def start_command_callback(self):
00186         self.enable_pattern_generator = True
00187         self._publish_parameters()
00188 
00189     def stop_command_callback(self):
00190         self.enable_pattern_generator = False
00191         self._publish_parameters()
00192 
00193     def callback_spin_box(self, value_as_int):
00194         if self.realtime_mode_checkbox.isChecked():
00195             self._publish_parameters()
00196 
00197     def joystick_mode_check_callback(self):
00198         self.enable_pattern_generator = False
00199         self._publish_parameters()
00200 


vigir_footstep_planning_widgets
Author(s): Alexander Stumpf
autogenerated on Sat Jul 15 2017 02:48:11