vigir_footstep_planning_widgets: step_interface

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import math
00004 
00005 import rospy
00006 import actionlib
00007 import std_msgs.msg
00008 import vigir_footstep_planning_msgs.msg
00009 
00010 from rqt_gui_py.plugin import Plugin
00011 from python_qt_binding.QtCore import Qt, QObject, Slot, QSignalMapper
00012 from python_qt_binding.QtGui import QHBoxLayout, QVBoxLayout, QCheckBox, QLineEdit, QPushButton, QDoubleSpinBox
00013 
00014 from vigir_footstep_planning_msgs.msg import StepPlanRequest, StepPlanRequestAction, StepPlanRequestGoal, StepPlanRequestResult, PatternParameters, Feet
00015 from vigir_footstep_planning_lib.execute_step_plan_widget import *
00016 from vigir_footstep_planning_lib.error_status_widget import *
00017 from vigir_footstep_planning_lib.parameter_set_widget import *
00018 from vigir_footstep_planning_lib.qt_helper import *
00019 
00020 
00021 class StepInterfaceDialog(Plugin):
00022 
00023     def __init__(self, context):
00024         super(StepInterfaceDialog, self).__init__(context)
00025         self.setObjectName('StepInterfaceDialog')
00026 
00027         self._parent = QWidget()
00028         self._widget = StepInterfaceWidget(self._parent)
00029 
00030         context.add_widget(self._parent)
00031 
00032     def shutdown_plugin(self):
00033         self._widget.shutdown_plugin()
00034 
00035 
00036 class StepInterfaceWidget(QObject):
00037 
00038     command_buttons = []
00039     start_feet = Feet()
00040 
00041     def __init__(self, context, add_execute_widget=True):
00042         super(StepInterfaceWidget, self).__init__()
00043 
00044         # init signal mapper
00045         self.command_mapper = QSignalMapper(self)
00046         self.command_mapper.mapped.connect(self._publish_step_plan_request)
00047 
00048         # start widget
00049         widget = context
00050         error_status_widget = QErrorStatusWidget()
00051         self.logger = Logger(error_status_widget)
00052         vbox = QVBoxLayout()
00053 
00054         # start control box
00055         controls_hbox = QHBoxLayout()
00056 
00057         # left coloumn
00058         left_controls_vbox = QVBoxLayout()
00059         left_controls_vbox.setMargin(0)
00060 
00061         self.add_command_button(left_controls_vbox, "Rotate Left", PatternParameters.ROTATE_LEFT)
00062         self.add_command_button(left_controls_vbox, "Strafe Left", PatternParameters.STRAFE_LEFT)
00063         self.add_command_button(left_controls_vbox, "Step Up", PatternParameters.STEP_UP)
00064         self.add_command_button(left_controls_vbox, "Center on Left", PatternParameters.FEET_REALIGN_ON_LEFT)
00065 
00066         left_controls_vbox.addStretch()
00067         controls_hbox.addLayout(left_controls_vbox, 1)
00068 
00069         # center coloumn
00070         center_controls_vbox = QVBoxLayout()
00071         center_controls_vbox.setMargin(0)
00072 
00073         self.add_command_button(center_controls_vbox, "Forward", PatternParameters.FORWARD)
00074         self.add_command_button(center_controls_vbox, "Backward", PatternParameters.BACKWARD)
00075         self.add_command_button(center_controls_vbox, "Step Over", PatternParameters.STEP_OVER)
00076         self.add_command_button(center_controls_vbox, "Center Feet", PatternParameters.FEET_REALIGN_ON_CENTER)
00077         self.add_command_button(center_controls_vbox, "Wide Stance", PatternParameters.WIDE_STANCE)
00078 
00079         center_controls_vbox.addStretch()
00080         controls_hbox.addLayout(center_controls_vbox, 1)
00081 
00082         # right coloumn
00083         right_controls_vbox = QVBoxLayout()
00084         right_controls_vbox.setMargin(0)
00085 
00086         self.add_command_button(right_controls_vbox, "Rotate Right", PatternParameters.ROTATE_RIGHT)
00087         self.add_command_button(right_controls_vbox, "Strafe Right", PatternParameters.STRAFE_RIGHT)
00088         self.add_command_button(right_controls_vbox, "Step Down", PatternParameters.STEP_DOWN)
00089         self.add_command_button(right_controls_vbox, "Center on Right", PatternParameters.FEET_REALIGN_ON_RIGHT)
00090 
00091         right_controls_vbox.addStretch()
00092         controls_hbox.addLayout(right_controls_vbox, 1)
00093 
00094         # end control box
00095         add_layout_with_frame(vbox, controls_hbox, "Commands:")
00096 
00097         # start settings
00098         settings_hbox = QHBoxLayout()
00099         settings_hbox.setMargin(0)
00100 
00101         # start left column
00102         left_settings_vbox = QVBoxLayout()
00103         left_settings_vbox.setMargin(0)
00104 
00105         # frame id
00106         self.frame_id_line_edit = QLineEdit("/world")
00107         add_widget_with_frame(left_settings_vbox, self.frame_id_line_edit, "Frame ID:")
00108 
00109         # do closing step
00110         self.close_step_checkbox = QCheckBox()
00111         self.close_step_checkbox.setText("Do closing step")
00112         self.close_step_checkbox.setChecked(True)
00113         left_settings_vbox.addWidget(self.close_step_checkbox)
00114 
00115         # extra seperation
00116         self.extra_seperation_checkbox = QCheckBox()
00117         self.extra_seperation_checkbox.setText("Extra Seperation")
00118         self.extra_seperation_checkbox.setChecked(False)
00119         left_settings_vbox.addWidget(self.extra_seperation_checkbox)
00120 
00121         left_settings_vbox.addStretch()
00122 
00123         # number of steps
00124         self.step_number = generate_q_double_spin_box(1, 1, 50, 0, 1.0)
00125         add_widget_with_frame(left_settings_vbox, self.step_number, "Number Steps:")
00126 
00127         # start step index
00128         self.start_step_index = generate_q_double_spin_box(0, 0, 1000, 0, 1.0)
00129         add_widget_with_frame(left_settings_vbox, self.start_step_index, "Start Step Index:")
00130 
00131         # end left column
00132         settings_hbox.addLayout(left_settings_vbox, 1)
00133 
00134         # start center column
00135         center_settings_vbox = QVBoxLayout()
00136         center_settings_vbox.setMargin(0)
00137 
00138         # start foot selection
00139         self.start_foot_selection_combo_box = QComboBox()
00140         self.start_foot_selection_combo_box.addItem("AUTO")
00141         self.start_foot_selection_combo_box.addItem("LEFT")
00142         self.start_foot_selection_combo_box.addItem("RIGHT")
00143         add_widget_with_frame(center_settings_vbox, self.start_foot_selection_combo_box, "Start foot selection:")
00144 
00145         center_settings_vbox.addStretch()
00146 
00147         # step Distance
00148         self.step_distance = generate_q_double_spin_box(0.0, 0.0, 0.5, 2, 0.01)
00149         add_widget_with_frame(center_settings_vbox, self.step_distance, "Step Distance (m):")
00150 
00151         # side step distance
00152         self.side_step = generate_q_double_spin_box(0.0, 0.0, 0.2, 2, 0.01)
00153         add_widget_with_frame(center_settings_vbox, self.side_step, "Side Step (m):")
00154 
00155         # rotation per step
00156         self.step_rotation = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
00157         add_widget_with_frame(center_settings_vbox, self.step_rotation, "Step Rotation (deg):")
00158 
00159         # end center column
00160         settings_hbox.addLayout(center_settings_vbox, 1)
00161 
00162         # start right column
00163         right_settings_vbox = QVBoxLayout()
00164         right_settings_vbox.setMargin(0)
00165 
00166         # roll
00167         self.roll = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
00168         add_widget_with_frame(right_settings_vbox, self.roll, "Roll (deg):")
00169 
00170         # pitch
00171         self.pitch = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
00172         add_widget_with_frame(right_settings_vbox, self.pitch, "Pitch (deg):")
00173 
00174         # use terrain model
00175         self.use_terrain_model_checkbox = QCheckBox()
00176         self.use_terrain_model_checkbox.setText("Use Terrain Model")
00177         self.use_terrain_model_checkbox.setChecked(False)
00178         self.use_terrain_model_checkbox.stateChanged.connect(self.use_terrain_model_changed)
00179         right_settings_vbox.addWidget(self.use_terrain_model_checkbox)
00180 
00181         # override mode
00182         self.override_checkbox = QCheckBox()
00183         self.override_checkbox.setText("Override 3D")
00184         self.override_checkbox.setChecked(False)
00185         right_settings_vbox.addWidget(self.override_checkbox)
00186 
00187         right_settings_vbox.addStretch()
00188 
00189         # delta z
00190         self.dz = generate_q_double_spin_box(0.0, -0.5, 0.5, 2, 0.01)
00191         add_widget_with_frame(right_settings_vbox, self.dz, "delta z per step (m):")
00192 
00193         # end right column
00194         settings_hbox.addLayout(right_settings_vbox, 1)
00195 
00196         # end settings
00197         add_layout_with_frame(vbox, settings_hbox, "Settings:")
00198 
00199         # parameter set selection
00200         self.parameter_set_widget = QParameterSetWidget(logger = self.logger)
00201         add_widget_with_frame(vbox, self.parameter_set_widget, "Parameter Set:")
00202 
00203         # execute option
00204         if add_execute_widget:
00205             add_widget_with_frame(vbox, QExecuteStepPlanWidget(logger = self.logger, step_plan_topic = "step_plan"), "Execute:")
00206 
00207         # add error status widget
00208         add_widget_with_frame(vbox, error_status_widget, "Status:")
00209 
00210         # end widget
00211         widget.setLayout(vbox)
00212         #context.add_widget(widget)
00213 
00214         # init widget
00215         self.parameter_set_widget.param_cleared_signal.connect(self.param_cleared)
00216         self.parameter_set_widget.param_changed_signal.connect(self.param_selected)
00217         self.commands_set_enabled(False)
00218 
00219         # subscriber
00220         self.start_feet_sub = rospy.Subscriber("set_start_feet", Feet, self.set_start_feet_callback)
00221 
00222         # publisher
00223         self.step_plan_pub = rospy.Publisher("step_plan", StepPlan, queue_size=1)
00224 
00225         # action clients
00226         self.step_plan_request_client = actionlib.SimpleActionClient("step_plan_request", StepPlanRequestAction)
00227 
00228     def shutdown_plugin(self):
00229         print "Shutting down ..."
00230         print "Done!"
00231 
00232     def add_command_button(self, parent, text, command):
00233         button = QPushButton(text)
00234         self.command_mapper.setMapping(button, command)
00235         button.clicked.connect(self.command_mapper.map)
00236         parent.addWidget(button)
00237         self.command_buttons.append(button)
00238         return button
00239 
00240     def set_start_feet_callback(self, feet):
00241         self.start_feet = feet
00242 
00243     # message publisher
00244     def _publish_step_plan_request(self, walk_command):
00245         params = PatternParameters()
00246         params.steps                = self.step_number.value()
00247         params.mode                 = walk_command
00248         params.close_step           = self.close_step_checkbox.isChecked()
00249         params.extra_seperation     = self.extra_seperation_checkbox.isChecked()
00250         params.use_terrain_model    = self.use_terrain_model_checkbox.isChecked()
00251         params.override             = self.override_checkbox.isChecked() and not self.use_terrain_model_checkbox.isChecked()
00252         params.roll                 = math.radians(self.roll.value())
00253         params.pitch                = math.radians(self.pitch.value())
00254         params.dz                   = self.dz.value()
00255 
00256         params.step_distance_forward   = self.step_distance.value()
00257         params.step_distance_sideward  = self.side_step.value()
00258         params.turn_angle              = math.radians(self.step_rotation.value())
00259 
00260         request = StepPlanRequest()
00261         request.header = std_msgs.msg.Header()
00262         request.header.stamp = rospy.Time.now()
00263         request.header.frame_id = self.frame_id_line_edit.text()
00264         request.start = self.start_feet
00265         request.start_step_index = self.start_step_index.value()
00266 
00267         if self.start_foot_selection_combo_box.currentText() == "AUTO":
00268             request.start_foot_selection = StepPlanRequest.AUTO
00269         elif self.start_foot_selection_combo_box.currentText() == "LEFT":
00270             request.start_foot_selection = StepPlanRequest.LEFT
00271         elif self.start_foot_selection_combo_box.currentText() == "RIGHT":
00272             request.start_foot_selection = StepPlanRequest.RIGHT
00273         else:
00274             self.logger.log_error("Unknown start foot selection mode ('" + self.start_foot_selection_combo_box.currentText() + "')!")
00275             return;
00276 
00277         if walk_command == PatternParameters.FORWARD:
00278             params.mode = PatternParameters.SAMPLING
00279         elif walk_command == PatternParameters.BACKWARD:
00280             params.mode                      = PatternParameters.SAMPLING
00281             params.step_distance_forward    *= -1;
00282             params.step_distance_sideward   *= -1;
00283             params.turn_angle               *= -1;
00284 
00285         request.pattern_parameters = params
00286         request.planning_mode = StepPlanRequest.PLANNING_MODE_PATTERN
00287         request.parameter_set_name.data = self.parameter_set_widget.current_parameter_set_name()
00288 
00289         print "Send request = ", request
00290 
00291         # send request
00292         if self.step_plan_request_client.wait_for_server(rospy.Duration(0.5)):
00293             self.logger.log_info("Sending footstep plan request...")
00294 
00295             goal = StepPlanRequestGoal()
00296             goal.plan_request = request;
00297             self.step_plan_request_client.send_goal(goal)
00298 
00299             if self.step_plan_request_client.wait_for_result(rospy.Duration(5.0)):
00300                 self.logger.log_info("Received footstep plan!")
00301                 self.logger.log(self.step_plan_request_client.get_result().status)
00302                 self.step_plan_pub.publish(self.step_plan_request_client.get_result().step_plan)
00303             else:
00304                 self.logger.log_error("Didn't received any results. Check communcation!")
00305         else:
00306             self.logger.log_error("Can't connect to footstep planner action server!")
00307 
00308     def commands_set_enabled(self, enable):
00309         for button in self.command_buttons:
00310             button.setEnabled(enable)
00311 
00312     @Slot()
00313     def param_cleared(self):
00314         self.commands_set_enabled(False)
00315 
00316     @Slot(str)
00317     def param_selected(self, name):
00318         self.commands_set_enabled(True)
00319 
00320     @Slot(int)
00321     def use_terrain_model_changed(self, state):
00322         enable_override = True
00323         if state == Qt.Checked:
00324             enable_override = False
00325         self.roll.setEnabled(enable_override)
00326         self.pitch.setEnabled(enable_override)
00327         self.override_checkbox.setEnabled(enable_override)
00328         self.dz.setEnabled(enable_override)