예제 #1
0
class Top(Plugin):

    NODE_FIELDS   = [             'pid', 'get_cpu_percent', 'get_memory_percent', 'get_num_threads']
    OUT_FIELDS    = ['node_name', 'pid', 'cpu_percent',     'memory_percent',     'num_threads'    ]
    FORMAT_STRS   = ['%s',        '%s',  '%0.2f',           '%0.2f',              '%s'             ]
    NODE_LABELS   = ['Node',      'PID', 'CPU %',           'Mem %',              'Num Threads'    ]
    SORT_TYPE     = [str,         str,   float,             float,                float            ]
    TOOLTIPS      = {
        0: ('cmdline', lambda x: '\n'.join(textwrap.wrap(' '.join(x)))),
        3: ('memory_info', lambda x: ('Resident: %0.2f MiB, Virtual: %0.2f MiB' % (x[0]/2**20, x[1]/2**20)))
    }

    _node_info = NodeInfo()

    name_filter = re.compile('')

    def __init__(self, context):
        super(Top, self).__init__(context)
        # Give QObjects reasonable names
        self.setObjectName('Top')

        # Process standalone plugin command-line arguments
        from argparse import ArgumentParser
        parser = ArgumentParser()
        # Add argument(s) to the parser.
        parser.add_argument("-q", "--quiet", action="store_true",
                      dest="quiet",
                      help="Put plugin in silent mode")
        args, unknowns = parser.parse_known_args(context.argv())
        # if not args.quiet:
        #     print 'arguments: ', args
        #     print 'unknowns: ', unknowns

        self._selected_node = ''
        self._selected_node_lock = RLock()

        # Setup the toolbar
        self._toolbar = QToolBar()
        self._filter_box = QLineEdit()
        self._regex_box  = QCheckBox()
        self._regex_box.setText('regex')
        self._toolbar.addWidget(QLabel('Filter'))
        self._toolbar.addWidget(self._filter_box)
        self._toolbar.addWidget(self._regex_box)

        self._filter_box.returnPressed.connect(self.update_filter)
        self._regex_box.stateChanged.connect(self.update_filter)

        # Create a container widget and give it a layout
        self._container = QWidget()
        self._container.setWindowTitle('Process Monitor')
        self._layout    = QVBoxLayout()
        self._container.setLayout(self._layout)

        self._layout.addWidget(self._toolbar)

        # Create the table widget
        self._table_widget = QTreeWidget()
        self._table_widget.setObjectName('TopTable')
        self._table_widget.setColumnCount(len(self.NODE_LABELS))
        self._table_widget.setHeaderLabels(self.NODE_LABELS)
        self._table_widget.itemClicked.connect(self._tableItemClicked)
        self._table_widget.setSortingEnabled(True)
        self._table_widget.setAlternatingRowColors(True)

        self._layout.addWidget(self._table_widget)
        context.add_widget(self._container)

        # Add a button for killing nodes
        self._kill_button = QPushButton('Kill Node')
        self._layout.addWidget(self._kill_button)
        self._kill_button.clicked.connect(self._kill_node)

        # Update twice since the first cpu% lookup will always return 0
        self.update_table()
        self.update_table()

        self._table_widget.resizeColumnToContents(0)

        # Start a timer to trigger updates
        self._update_timer = QTimer()
        self._update_timer.setInterval(1000)
        self._update_timer.timeout.connect(self.update_table)
        self._update_timer.start()

    def _tableItemClicked(self, item, column):
        with self._selected_node_lock:
            self._selected_node = item.text(0)

    def update_filter(self, *args):
        if self._regex_box.isChecked():
            expr = self._filter_box.text()
        else:
            expr = re.escape(self._filter_box.text())
        self.name_filter = re.compile(expr)
        self.update_table()

    def _kill_node(self):
        self._node_info.kill_node(self._selected_node)

    def update_one_item(self, row, info):
        twi = TopWidgetItem()
        for col, field in enumerate(self.OUT_FIELDS):
            val = info[field]
            twi.setText(col, self.FORMAT_STRS[col] % val)
        self._table_widget.insertTopLevelItem(row, twi)

        for col, (key, func) in self.TOOLTIPS.iteritems():
            twi.setToolTip(col, func(info[key]))

        with self._selected_node_lock:
            if twi.text(0) == self._selected_node:
                twi.setSelected(True)

        self._table_widget.setItemHidden(twi, len(self.name_filter.findall(info['node_name'])) == 0)

    def update_table(self):
        self._table_widget.clear()
        infos = self._node_info.get_all_node_fields(self.NODE_FIELDS)
        for nx, info in enumerate(infos):
            self.update_one_item(nx, info)

    def shutdown_plugin(self):
        self._update_timer.stop()

    def save_settings(self, plugin_settings, instance_settings):        
        instance_settings.set_value('filter_text', self._filter_box.text())
        instance_settings.set_value('is_regex', int(self._regex_box.checkState()))

    def restore_settings(self, plugin_settings, instance_settings):
        self._filter_box.setText(instance_settings.value('filter_text'))
        is_regex_int = instance_settings.value('is_regex')
        if is_regex_int:
            self._regex_box.setCheckState(Qt.CheckState(is_regex_int))
        else:
            self._regex_box.setCheckState(Qt.CheckState(0))
        self.update_filter()
예제 #2
0
class StepInterfaceWidget(QObject):

    command_buttons = []
    start_feet = Feet()

    def __init__(self, context, add_execute_widget=True):
        super(StepInterfaceWidget, self).__init__()

        # init signal mapper
        self.command_mapper = QSignalMapper(self)
        self.command_mapper.mapped.connect(self._publish_step_plan_request)

        # start widget
        widget = context
        error_status_widget = QErrorStatusWidget()
        self.logger = Logger(error_status_widget)
        vbox = QVBoxLayout()

        # start control box
        controls_hbox = QHBoxLayout()

        # left coloumn
        left_controls_vbox = QVBoxLayout()
        left_controls_vbox.setMargin(0)

        self.add_command_button(left_controls_vbox, "Rotate Left",
                                PatternParameters.ROTATE_LEFT)
        self.add_command_button(left_controls_vbox, "Strafe Left",
                                PatternParameters.STRAFE_LEFT)
        self.add_command_button(left_controls_vbox, "Step Up",
                                PatternParameters.STEP_UP)
        self.add_command_button(left_controls_vbox, "Center on Left",
                                PatternParameters.FEET_REALIGN_ON_LEFT)

        left_controls_vbox.addStretch()
        controls_hbox.addLayout(left_controls_vbox, 1)

        # center coloumn
        center_controls_vbox = QVBoxLayout()
        center_controls_vbox.setMargin(0)

        self.add_command_button(center_controls_vbox, "Forward",
                                PatternParameters.FORWARD)
        self.add_command_button(center_controls_vbox, "Backward",
                                PatternParameters.BACKWARD)
        self.add_command_button(center_controls_vbox, "Step Over",
                                PatternParameters.STEP_OVER)
        self.add_command_button(center_controls_vbox, "Center Feet",
                                PatternParameters.FEET_REALIGN_ON_CENTER)
        self.add_command_button(center_controls_vbox, "Wide Stance",
                                PatternParameters.WIDE_STANCE)

        center_controls_vbox.addStretch()
        controls_hbox.addLayout(center_controls_vbox, 1)

        # right coloumn
        right_controls_vbox = QVBoxLayout()
        right_controls_vbox.setMargin(0)

        self.add_command_button(right_controls_vbox, "Rotate Right",
                                PatternParameters.ROTATE_RIGHT)
        self.add_command_button(right_controls_vbox, "Strafe Right",
                                PatternParameters.STRAFE_RIGHT)
        self.add_command_button(right_controls_vbox, "Step Down",
                                PatternParameters.STEP_DOWN)
        self.add_command_button(right_controls_vbox, "Center on Right",
                                PatternParameters.FEET_REALIGN_ON_RIGHT)

        right_controls_vbox.addStretch()
        controls_hbox.addLayout(right_controls_vbox, 1)

        # end control box
        add_layout_with_frame(vbox, controls_hbox, "Commands:")

        # start settings
        settings_hbox = QHBoxLayout()
        settings_hbox.setMargin(0)

        # start left column
        left_settings_vbox = QVBoxLayout()
        left_settings_vbox.setMargin(0)

        # frame id
        self.frame_id_line_edit = QLineEdit("/world")
        add_widget_with_frame(left_settings_vbox, self.frame_id_line_edit,
                              "Frame ID:")

        # do closing step
        self.close_step_checkbox = QCheckBox()
        self.close_step_checkbox.setText("Do closing step")
        self.close_step_checkbox.setChecked(True)
        left_settings_vbox.addWidget(self.close_step_checkbox)

        # extra seperation
        self.extra_seperation_checkbox = QCheckBox()
        self.extra_seperation_checkbox.setText("Extra Seperation")
        self.extra_seperation_checkbox.setChecked(False)
        left_settings_vbox.addWidget(self.extra_seperation_checkbox)

        left_settings_vbox.addStretch()

        # number of steps
        self.step_number = generate_q_double_spin_box(1, 1, 50, 0, 1.0)
        add_widget_with_frame(left_settings_vbox, self.step_number,
                              "Number Steps:")

        # start step index
        self.start_step_index = generate_q_double_spin_box(0, 0, 1000, 0, 1.0)
        add_widget_with_frame(left_settings_vbox, self.start_step_index,
                              "Start Step Index:")

        # end left column
        settings_hbox.addLayout(left_settings_vbox, 1)

        # start center column
        center_settings_vbox = QVBoxLayout()
        center_settings_vbox.setMargin(0)

        # start foot selection
        self.start_foot_selection_combo_box = QComboBox()
        self.start_foot_selection_combo_box.addItem("AUTO")
        self.start_foot_selection_combo_box.addItem("LEFT")
        self.start_foot_selection_combo_box.addItem("RIGHT")
        add_widget_with_frame(center_settings_vbox,
                              self.start_foot_selection_combo_box,
                              "Start foot selection:")

        center_settings_vbox.addStretch()

        # step Distance
        self.step_distance = generate_q_double_spin_box(0.0, 0.0, 0.5, 2, 0.01)
        add_widget_with_frame(center_settings_vbox, self.step_distance,
                              "Step Distance (m):")

        # side step distance
        self.side_step = generate_q_double_spin_box(0.0, 0.0, 0.2, 2, 0.01)
        add_widget_with_frame(center_settings_vbox, self.side_step,
                              "Side Step (m):")

        # rotation per step
        self.step_rotation = generate_q_double_spin_box(
            0.0, -30.0, 30.0, 0, 1.0)
        add_widget_with_frame(center_settings_vbox, self.step_rotation,
                              "Step Rotation (deg):")

        # end center column
        settings_hbox.addLayout(center_settings_vbox, 1)

        # start right column
        right_settings_vbox = QVBoxLayout()
        right_settings_vbox.setMargin(0)

        # roll
        self.roll = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
        add_widget_with_frame(right_settings_vbox, self.roll, "Roll (deg):")

        # pitch
        self.pitch = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
        add_widget_with_frame(right_settings_vbox, self.pitch, "Pitch (deg):")

        # use terrain model
        self.use_terrain_model_checkbox = QCheckBox()
        self.use_terrain_model_checkbox.setText("Use Terrain Model")
        self.use_terrain_model_checkbox.setChecked(False)
        self.use_terrain_model_checkbox.stateChanged.connect(
            self.use_terrain_model_changed)
        right_settings_vbox.addWidget(self.use_terrain_model_checkbox)

        # override mode
        self.override_checkbox = QCheckBox()
        self.override_checkbox.setText("Override 3D")
        self.override_checkbox.setChecked(False)
        right_settings_vbox.addWidget(self.override_checkbox)

        right_settings_vbox.addStretch()

        # delta z
        self.dz = generate_q_double_spin_box(0.0, -0.5, 0.5, 2, 0.01)
        add_widget_with_frame(right_settings_vbox, self.dz,
                              "delta z per step (m):")

        # end right column
        settings_hbox.addLayout(right_settings_vbox, 1)

        # end settings
        add_layout_with_frame(vbox, settings_hbox, "Settings:")

        # parameter set selection
        self.parameter_set_widget = QParameterSetWidget(logger=self.logger)
        add_widget_with_frame(vbox, self.parameter_set_widget,
                              "Parameter Set:")

        # execute option
        if add_execute_widget:
            add_widget_with_frame(
                vbox,
                QExecuteStepPlanWidget(logger=self.logger,
                                       step_plan_topic="step_plan"),
                "Execute:")

        # add error status widget
        add_widget_with_frame(vbox, error_status_widget, "Status:")

        # end widget
        widget.setLayout(vbox)
        #context.add_widget(widget)

        # init widget
        self.parameter_set_widget.param_cleared_signal.connect(
            self.param_cleared)
        self.parameter_set_widget.param_changed_signal.connect(
            self.param_selected)
        self.commands_set_enabled(False)

        # subscriber
        self.start_feet_sub = rospy.Subscriber("set_start_feet", Feet,
                                               self.set_start_feet_callback)

        # publisher
        self.step_plan_pub = rospy.Publisher("step_plan",
                                             StepPlan,
                                             queue_size=1)

        # action clients
        self.step_plan_request_client = actionlib.SimpleActionClient(
            "step_plan_request", StepPlanRequestAction)

    def shutdown_plugin(self):
        print "Shutting down ..."
        print "Done!"

    def add_command_button(self, parent, text, command):
        button = QPushButton(text)
        self.command_mapper.setMapping(button, command)
        button.clicked.connect(self.command_mapper.map)
        parent.addWidget(button)
        self.command_buttons.append(button)
        return button

    def set_start_feet_callback(self, feet):
        self.start_feet = feet

    # message publisher
    def _publish_step_plan_request(self, walk_command):
        params = PatternParameters()
        params.steps = self.step_number.value()
        params.mode = walk_command
        params.close_step = self.close_step_checkbox.isChecked()
        params.extra_seperation = self.extra_seperation_checkbox.isChecked()
        params.use_terrain_model = self.use_terrain_model_checkbox.isChecked()
        params.override = self.override_checkbox.isChecked(
        ) and not self.use_terrain_model_checkbox.isChecked()
        params.roll = math.radians(self.roll.value())
        params.pitch = math.radians(self.pitch.value())
        params.dz = self.dz.value()

        params.step_distance_forward = self.step_distance.value()
        params.step_distance_sideward = self.side_step.value()
        params.turn_angle = math.radians(self.step_rotation.value())

        request = StepPlanRequest()
        request.header = std_msgs.msg.Header()
        request.header.stamp = rospy.Time.now()
        request.header.frame_id = self.frame_id_line_edit.text()
        request.start = self.start_feet
        request.start_step_index = self.start_step_index.value()

        if self.start_foot_selection_combo_box.currentText() == "AUTO":
            request.start_foot_selection = StepPlanRequest.AUTO
        elif self.start_foot_selection_combo_box.currentText() == "LEFT":
            request.start_foot_selection = StepPlanRequest.LEFT
        elif self.start_foot_selection_combo_box.currentText() == "RIGHT":
            request.start_foot_selection = StepPlanRequest.RIGHT
        else:
            self.logger.log_error(
                "Unknown start foot selection mode ('" +
                self.start_foot_selection_combo_box.currentText() + "')!")
            return

        if walk_command == PatternParameters.FORWARD:
            params.mode = PatternParameters.SAMPLING
        elif walk_command == PatternParameters.BACKWARD:
            params.mode = PatternParameters.SAMPLING
            params.step_distance_forward *= -1
            params.step_distance_sideward *= -1
            params.turn_angle *= -1

        request.pattern_parameters = params
        request.planning_mode = StepPlanRequest.PLANNING_MODE_PATTERN
        request.parameter_set_name.data = self.parameter_set_widget.current_parameter_set_name(
        )

        print "Send request = ", request

        # send request
        if self.step_plan_request_client.wait_for_server(rospy.Duration(0.5)):
            self.logger.log_info("Sending footstep plan request...")

            goal = StepPlanRequestGoal()
            goal.plan_request = request
            self.step_plan_request_client.send_goal(goal)

            if self.step_plan_request_client.wait_for_result(
                    rospy.Duration(5.0)):
                self.logger.log_info("Received footstep plan!")
                self.logger.log(
                    self.step_plan_request_client.get_result().status)
                self.step_plan_pub.publish(
                    self.step_plan_request_client.get_result().step_plan)
            else:
                self.logger.log_error(
                    "Didn't received any results. Check communcation!")
        else:
            self.logger.log_error(
                "Can't connect to footstep planner action server!")

    def commands_set_enabled(self, enable):
        for button in self.command_buttons:
            button.setEnabled(enable)

    @Slot()
    def param_cleared(self):
        self.commands_set_enabled(False)

    @Slot(str)
    def param_selected(self, name):
        self.commands_set_enabled(True)

    @Slot(int)
    def use_terrain_model_changed(self, state):
        enable_override = True
        if state == Qt.Checked:
            enable_override = False
        self.roll.setEnabled(enable_override)
        self.pitch.setEnabled(enable_override)
        self.override_checkbox.setEnabled(enable_override)
        self.dz.setEnabled(enable_override)
예제 #3
0
class PatternGeneratorWidget(QObject):

    enable_pattern_generator = False

    def __init__(self, context):
        super(PatternGeneratorWidget, self).__init__()

        # publisher
        self.pattern_generator_params_pub = rospy.Publisher(
            'pattern_generator/set_params',
            PatternGeneratorParameters,
            queue_size=1)

        # start widget
        widget = context

        # start upper part
        hbox = QHBoxLayout()

        # start left column
        left_vbox = QVBoxLayout()

        # start button
        start_command = QPushButton("Start")
        left_vbox.addWidget(start_command)

        # simulation checkbox
        self.simulation_mode_checkbox = QCheckBox()
        self.simulation_mode_checkbox.setText("Simulation Mode")
        self.simulation_mode_checkbox.setChecked(False)
        left_vbox.addWidget(self.simulation_mode_checkbox)

        # realtime checkbox
        self.realtime_mode_checkbox = QCheckBox()
        self.realtime_mode_checkbox.setText("Realtime Mode")
        self.realtime_mode_checkbox.setChecked(False)
        left_vbox.addWidget(self.realtime_mode_checkbox)

        # joystick checkbox
        self.joystick_mode_checkbox = QCheckBox()
        self.joystick_mode_checkbox.setText("Joystick Mode")
        self.joystick_mode_checkbox.setChecked(False)
        left_vbox.addWidget(self.joystick_mode_checkbox)

        # ignore invalid steps checkbox
        self.ignore_invalid_steps_checkbox = QCheckBox()
        self.ignore_invalid_steps_checkbox.setText("Ignore Invalid Steps")
        self.ignore_invalid_steps_checkbox.setChecked(False)
        left_vbox.addWidget(self.ignore_invalid_steps_checkbox)

        # foot seperation
        self.foot_seperation = generate_q_double_spin_box(
            0.2, 0.15, 0.3, 2, 0.01)
        self.foot_seperation.valueChanged.connect(self.callback_spin_box)
        add_widget_with_frame(left_vbox, self.foot_seperation,
                              "Foot Seperation (m):")

        # delta x
        self.delta_x = generate_q_double_spin_box(0.0, -0.4, 0.4, 2, 0.01)
        self.delta_x.valueChanged.connect(self.callback_spin_box)
        add_widget_with_frame(left_vbox, self.delta_x, "dX (m):")

        # delta y
        self.delta_y = generate_q_double_spin_box(0.0, -2.2, 2.2, 2, 0.01)
        self.delta_y.valueChanged.connect(self.callback_spin_box)
        add_widget_with_frame(left_vbox, self.delta_y, "dY (m):")

        # delta yaw
        self.delta_yaw = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
        self.delta_yaw.valueChanged.connect(self.callback_spin_box)
        add_widget_with_frame(left_vbox, self.delta_yaw, "dYaw (deg):")

        # roll
        self.roll = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
        self.roll.valueChanged.connect(self.callback_spin_box)
        add_widget_with_frame(left_vbox, self.roll, "Roll (deg):")

        # pitch
        self.pitch = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
        self.pitch.valueChanged.connect(self.callback_spin_box)
        add_widget_with_frame(left_vbox, self.pitch, "Pitch (deg):")

        # end left column
        left_vbox.addStretch()
        hbox.addLayout(left_vbox, 1)

        # start right column
        right_vbox = QVBoxLayout()

        # stop button
        stop_command = QPushButton("Stop")
        right_vbox.addWidget(stop_command)

        # ignore collision
        self.collision_checkbox = QCheckBox()
        self.collision_checkbox.setText("Ignore Collision")
        self.collision_checkbox.setChecked(True)
        right_vbox.addWidget(self.collision_checkbox)

        # override 3D
        self.override_checkbox = QCheckBox()
        self.override_checkbox.setText("Override 3D")
        self.override_checkbox.setChecked(False)
        right_vbox.addWidget(self.override_checkbox)

        # end right coloumn
        right_vbox.addStretch()
        hbox.addLayout(right_vbox, 1)

        # add upper part
        hbox.setMargin(0)
        vbox = QVBoxLayout()
        vbox.addLayout(hbox)

        # parameter set selection
        self.parameter_set_widget = QParameterSetWidget()
        add_widget_with_frame(vbox, self.parameter_set_widget,
                              "Parameter Set:")

        # end widget
        widget.setLayout(vbox)
        #context.add_widget(widget)

        # signal connections
        start_command.clicked.connect(self.start_command_callback)
        stop_command.clicked.connect(self.stop_command_callback)
        self.joystick_mode_checkbox.clicked.connect(
            self.joystick_mode_check_callback)
        self.ignore_invalid_steps_checkbox.clicked.connect(
            self._publish_parameters)

    def shutdown_plugin(self):
        print "Shutting down ..."
        self.pattern_generator_params_pub.unregister()
        print "Done!"

    # message publisher
    def _publish_parameters(self):
        params = PatternGeneratorParameters()

        params.enable = self.enable_pattern_generator
        params.simulation_mode = self.simulation_mode_checkbox.isChecked()
        params.joystick_mode = self.joystick_mode_checkbox.isChecked()
        params.ignore_invalid_steps = self.ignore_invalid_steps_checkbox.isChecked(
        )
        params.d_step.position.x = self.delta_x.value()
        params.d_step.position.y = self.delta_y.value()
        params.d_step.position.z = 0
        q = tf.transformations.quaternion_from_euler(
            math.radians(self.roll.value()), math.radians(self.pitch.value()),
            math.radians(self.delta_yaw.value()))
        params.d_step.orientation.x = q[0]
        params.d_step.orientation.y = q[1]
        params.d_step.orientation.z = q[2]
        params.d_step.orientation.w = q[3]
        params.foot_seperation = self.foot_seperation.value()
        params.parameter_set_name.data = self.parameter_set_widget.current_parameter_set_name(
        )

        print "Send stepping command = \n", params
        self.pattern_generator_params_pub.publish(params)

    # Define system command strings
    def start_command_callback(self):
        self.enable_pattern_generator = True
        self._publish_parameters()

    def stop_command_callback(self):
        self.enable_pattern_generator = False
        self._publish_parameters()

    def callback_spin_box(self, value_as_int):
        if self.realtime_mode_checkbox.isChecked():
            self._publish_parameters()

    def joystick_mode_check_callback(self):
        self.enable_pattern_generator = False
        self._publish_parameters()
class PatternGeneratorWidget(QObject):

    enable_pattern_generator = False

    def __init__(self, context):
        super(PatternGeneratorWidget, self).__init__()

        # publisher
        self.pattern_generator_params_pub = rospy.Publisher('pattern_generator/set_params', PatternGeneratorParameters, queue_size = 1)

        # start widget
        widget = context

        # start upper part
        hbox = QHBoxLayout()

        # start left column
        left_vbox = QVBoxLayout()

        # start button
        start_command = QPushButton("Start")
        start_command.clicked.connect(self.start_command_callback)
        left_vbox.addWidget(start_command)

        # simulation checkbox
        self.simulation_mode_checkbox = QCheckBox()
        self.simulation_mode_checkbox.setText("Simulation Mode")
        self.simulation_mode_checkbox.setChecked(False)
        left_vbox.addWidget(self.simulation_mode_checkbox)

        # realtime checkbox
        self.realtime_mode_checkbox = QCheckBox()
        self.realtime_mode_checkbox.setText("Realtime Mode")
        self.realtime_mode_checkbox.setChecked(False)
        left_vbox.addWidget(self.realtime_mode_checkbox)

        # joystick checkbox
        self.joystick_mode_checkbox = QCheckBox()
        self.joystick_mode_checkbox.setText("Joystick Mode")
        self.joystick_mode_checkbox.setChecked(False)
        self.joystick_mode_checkbox.clicked.connect(self.joystick_mode_check_callback)
        left_vbox.addWidget(self.joystick_mode_checkbox)

        # foot seperation
        self.foot_seperation = generate_q_double_spin_box(0.2, 0.15, 0.3, 2, 0.01)
        self.foot_seperation.valueChanged.connect(self.callback_spin_box)
        add_widget_with_frame(left_vbox, self.foot_seperation, "Foot Seperation (m):")

        # delta x
        self.delta_x = generate_q_double_spin_box(0.0, -0.4, 0.4, 2, 0.01)
        self.delta_x.valueChanged.connect(self.callback_spin_box)
        add_widget_with_frame(left_vbox, self.delta_x, "dX (m):")

        # delta y
        self.delta_y = generate_q_double_spin_box(0.0, -2.2, 2.2, 2, 0.01)
        self.delta_y.valueChanged.connect(self.callback_spin_box)
        add_widget_with_frame(left_vbox, self.delta_y, "dY (m):")

        # delta yaw
        self.delta_yaw = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
        self.delta_yaw.valueChanged.connect(self.callback_spin_box)
        add_widget_with_frame(left_vbox, self.delta_yaw, "dYaw (deg):")

        # roll
        self.roll = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
        self.roll.valueChanged.connect(self.callback_spin_box)
        add_widget_with_frame(left_vbox, self.roll, "Roll (deg):")

        # pitch
        self.pitch = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
        self.pitch.valueChanged.connect(self.callback_spin_box)
        add_widget_with_frame(left_vbox, self.pitch, "Pitch (deg):")

        # end left column
        left_vbox.addStretch()
        hbox.addLayout(left_vbox, 1)



        # start right column
        right_vbox = QVBoxLayout()

        # stop button
        stop_command = QPushButton("Stop")
        stop_command.clicked.connect(self.stop_command_callback)
        right_vbox.addWidget(stop_command)

        # ignore collision
        self.collision_checkbox = QCheckBox()
        self.collision_checkbox.setText("Ignore Collision")
        self.collision_checkbox.setChecked(True)
        right_vbox.addWidget(self.collision_checkbox)

        # override 3D
        self.override_checkbox = QCheckBox()
        self.override_checkbox.setText("Override 3D")
        self.override_checkbox.setChecked(False)
        right_vbox.addWidget(self.override_checkbox)

        # end right coloumn
        right_vbox.addStretch()
        hbox.addLayout(right_vbox, 1)
        
        # add upper part
        hbox.setMargin(0)
        vbox = QVBoxLayout()
        vbox.addLayout(hbox)

        # parameter set selection
        self.parameter_set_widget = QParameterSetWidget()
        add_widget_with_frame(vbox, self.parameter_set_widget, "Parameter Set:")

        # end widget
        widget.setLayout(vbox)
        #context.add_widget(widget)

    def shutdown_plugin(self):
        print "Shutting down ..."
        self.pattern_generator_params_pub.unregister()
        print "Done!"

    # message publisher
    def _publish_parameters(self):
        params = PatternGeneratorParameters()

        params.enable = self.enable_pattern_generator
        params.simulation_mode = self.simulation_mode_checkbox.isChecked()
        params.joystick_mode = self.joystick_mode_checkbox.isChecked()
        params.d_step.position.x = self.delta_x.value()
        params.d_step.position.y = self.delta_y.value()
        params.d_step.position.z = 0
        q = tf.transformations.quaternion_from_euler(math.radians(self.roll.value()), math.radians(self.pitch.value()), math.radians(self.delta_yaw.value()))
        params.d_step.orientation.x = q[0]
        params.d_step.orientation.y = q[1]
        params.d_step.orientation.z = q[2]
        params.d_step.orientation.w = q[3]
        params.foot_seperation = self.foot_seperation.value()
        params.parameter_set_name.data = self.parameter_set_widget.current_parameter_set_name()

        print "Send stepping command = \n",params
        self.pattern_generator_params_pub.publish(params)

    # Define system command strings
    def start_command_callback(self):
        self.enable_pattern_generator = True
        self._publish_parameters()

    def stop_command_callback(self):
        self.enable_pattern_generator = False
        self._publish_parameters()

    def callback_spin_box(self, value_as_int):
        if (self.realtime_mode_checkbox.isChecked()):
            self._publish_parameters()

    def joystick_mode_check_callback(self):
        self.enable_pattern_generator = False
        self._publish_parameters()
class StepInterfaceWidget(QObject):

    command_buttons = []
    start_feet = Feet()

    def __init__(self, context, add_execute_widget = True):
        super(StepInterfaceWidget, self).__init__()

        # init signal mapper
        self.command_mapper = QSignalMapper(self)
        self.command_mapper.mapped.connect(self._publish_step_plan_request)

        # start widget
        widget = context
        error_status_widget = QErrorStatusWidget()
        self.logger = Logger(error_status_widget)
        vbox = QVBoxLayout()



        # start control box
        controls_hbox = QHBoxLayout()

        # left coloumn
        left_controls_vbox = QVBoxLayout()
        left_controls_vbox.setMargin(0)

        self.add_command_button(left_controls_vbox, "Rotate Left", PatternParameters.ROTATE_LEFT)
        self.add_command_button(left_controls_vbox, "Strafe Left", PatternParameters.STRAFE_LEFT)
        self.add_command_button(left_controls_vbox, "Step Up", PatternParameters.STEP_UP)
        self.add_command_button(left_controls_vbox, "Center on Left", PatternParameters.FEET_REALIGN_ON_LEFT)

        left_controls_vbox.addStretch()
        controls_hbox.addLayout(left_controls_vbox, 1)

        # center coloumn
        center_controls_vbox = QVBoxLayout()
        center_controls_vbox.setMargin(0)

        self.add_command_button(center_controls_vbox, "Forward", PatternParameters.FORWARD)
        self.add_command_button(center_controls_vbox, "Backward", PatternParameters.BACKWARD)
        self.add_command_button(center_controls_vbox, "Step Over", PatternParameters.STEP_OVER)
        self.add_command_button(center_controls_vbox, "Center Feet", PatternParameters.FEET_REALIGN_ON_CENTER)
        self.add_command_button(center_controls_vbox, "Wide Stance", PatternParameters.WIDE_STANCE)

        center_controls_vbox.addStretch()
        controls_hbox.addLayout(center_controls_vbox, 1)

        # right coloumn
        right_controls_vbox = QVBoxLayout()
        right_controls_vbox.setMargin(0)

        self.add_command_button(right_controls_vbox, "Rotate Right", PatternParameters.ROTATE_RIGHT)
        self.add_command_button(right_controls_vbox, "Strafe Right", PatternParameters.STRAFE_RIGHT)
        self.add_command_button(right_controls_vbox, "Step Down", PatternParameters.STEP_DOWN)
        self.add_command_button(right_controls_vbox, "Center on Right", PatternParameters.FEET_REALIGN_ON_RIGHT)

        right_controls_vbox.addStretch()
        controls_hbox.addLayout(right_controls_vbox, 1)

        # end control box
        add_layout_with_frame(vbox, controls_hbox, "Commands:")



        # start settings
        settings_hbox = QHBoxLayout()
        settings_hbox.setMargin(0)
        
        # start left column
        left_settings_vbox = QVBoxLayout()
        left_settings_vbox.setMargin(0)

        # frame id
        self.frame_id_line_edit = QLineEdit("/world")
        add_widget_with_frame(left_settings_vbox, self.frame_id_line_edit, "Frame ID:")

        # do closing step
        self.close_step_checkbox = QCheckBox()
        self.close_step_checkbox.setText("Do closing step")
        self.close_step_checkbox.setChecked(True)
        left_settings_vbox.addWidget(self.close_step_checkbox)

        # extra seperation
        self.extra_seperation_checkbox = QCheckBox()
        self.extra_seperation_checkbox.setText("Extra Seperation")
        self.extra_seperation_checkbox.setChecked(False)
        left_settings_vbox.addWidget(self.extra_seperation_checkbox)

        left_settings_vbox.addStretch()

        # number of steps
        self.step_number = generate_q_double_spin_box(1, 1, 50, 0, 1.0)
        add_widget_with_frame(left_settings_vbox, self.step_number, "Number Steps:")

        # start step index
        self.start_step_index = generate_q_double_spin_box(0, 0, 1000, 0, 1.0)
        add_widget_with_frame(left_settings_vbox, self.start_step_index, "Start Step Index:")

        # end left column
        settings_hbox.addLayout(left_settings_vbox, 1)



        # start center column
        center_settings_vbox = QVBoxLayout()
        center_settings_vbox.setMargin(0)

        # start foot selection
        self.start_foot_selection_combo_box = QComboBox()
        self.start_foot_selection_combo_box.addItem("AUTO")
        self.start_foot_selection_combo_box.addItem("LEFT")
        self.start_foot_selection_combo_box.addItem("RIGHT")
        add_widget_with_frame(center_settings_vbox, self.start_foot_selection_combo_box, "Start foot selection:")

        center_settings_vbox.addStretch()

        # step Distance
        self.step_distance = generate_q_double_spin_box(0.20, 0.0, 0.5, 2, 0.01)
        add_widget_with_frame(center_settings_vbox, self.step_distance, "Step Distance (m):")

        # side step distance
        self.side_step = generate_q_double_spin_box(0.0, 0.0, 0.2, 2, 0.01)
        add_widget_with_frame(center_settings_vbox, self.side_step, "Side Step (m):")

        # rotation per step
        self.step_rotation = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
        add_widget_with_frame(center_settings_vbox, self.step_rotation, "Step Rotation (deg):")

        # end center column
        settings_hbox.addLayout(center_settings_vbox, 1)



        # start right column
        right_settings_vbox = QVBoxLayout()
        right_settings_vbox.setMargin(0)

        # roll
        self.roll = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
        add_widget_with_frame(right_settings_vbox, self.roll, "Roll (deg):")

        # pitch
        self.pitch = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
        add_widget_with_frame(right_settings_vbox, self.pitch, "Pitch (deg):")

        # use terrain model
        self.use_terrain_model_checkbox = QCheckBox()
        self.use_terrain_model_checkbox.setText("Use Terrain Model")
        self.use_terrain_model_checkbox.setChecked(False)
        self.use_terrain_model_checkbox.stateChanged.connect(self.use_terrain_model_changed)
        right_settings_vbox.addWidget(self.use_terrain_model_checkbox)

        # override mode
        self.override_checkbox = QCheckBox()
        self.override_checkbox.setText("Override 3D")
        self.override_checkbox.setChecked(False)
        right_settings_vbox.addWidget(self.override_checkbox)

        right_settings_vbox.addStretch()

        # delta z
        self.dz = generate_q_double_spin_box(0.0, -0.5, 0.5, 2, 0.01)
        add_widget_with_frame(right_settings_vbox, self.dz, "delta z per step (m):")

        # end right column
        settings_hbox.addLayout(right_settings_vbox, 1)

        # end settings
        add_layout_with_frame(vbox, settings_hbox, "Settings:")



        # parameter set selection
        self.parameter_set_widget = QParameterSetWidget(logger = self.logger)
        add_widget_with_frame(vbox, self.parameter_set_widget, "Parameter Set:")

        # execute option
        if add_execute_widget:
            add_widget_with_frame(vbox, QExecuteStepPlanWidget(logger = self.logger, step_plan_topic = "step_plan"), "Execute:")

        # add error status widget
        add_widget_with_frame(vbox, error_status_widget, "Status:")

        # end widget
        widget.setLayout(vbox)
        #context.add_widget(widget)

        # init widget
        self.parameter_set_widget.param_cleared_signal.connect(self.param_cleared)
        self.parameter_set_widget.param_changed_signal.connect(self.param_selected)
        self.commands_set_enabled(False)

        # subscriber
        self.start_feet_sub = rospy.Subscriber("set_start_feet", Feet, self.set_start_feet_callback)

        # publisher
        self.step_plan_pub = rospy.Publisher("step_plan", StepPlan, queue_size=1)

        # action clients
        self.step_plan_request_client = actionlib.SimpleActionClient("step_plan_request", StepPlanRequestAction)

    def shutdown_plugin(self):
        print "Shutting down ..."
        print "Done!"

    def add_command_button(self, parent, text, command):
        button = QPushButton(text)
        self.command_mapper.setMapping(button, command)
        button.clicked.connect(self.command_mapper.map)
        parent.addWidget(button)
        self.command_buttons.append(button)
        return button

    def set_start_feet_callback(self, feet):
        self.start_feet = feet

    # message publisher
    def _publish_step_plan_request(self, walk_command):
        params = PatternParameters()
        params.steps                = self.step_number.value()
        params.mode                 = walk_command
        params.close_step           = self.close_step_checkbox.isChecked()
        params.extra_seperation     = self.extra_seperation_checkbox.isChecked()
        params.use_terrain_model    = self.use_terrain_model_checkbox.isChecked()
        params.override             = self.override_checkbox.isChecked() and not self.use_terrain_model_checkbox.isChecked()
        params.roll                 = math.radians(self.roll.value())
        params.pitch                = math.radians(self.pitch.value())
        params.dz                   = self.dz.value()

        params.step_distance_forward   = self.step_distance.value()
        params.step_distance_sideward  = self.side_step.value()
        params.turn_angle              = math.radians(self.step_rotation.value())

        request = StepPlanRequest()
        request.header = std_msgs.msg.Header()
        request.header.stamp = rospy.Time.now()
        request.header.frame_id = self.frame_id_line_edit.text()
        request.start = self.start_feet
        request.start_step_index = self.start_step_index.value()

        if (self.start_foot_selection_combo_box.currentText() == "AUTO"):
            request.start_foot_selection = StepPlanRequest.AUTO
        elif (self.start_foot_selection_combo_box.currentText() == "LEFT"):
            request.start_foot_selection = StepPlanRequest.LEFT
        elif (self.start_foot_selection_combo_box.currentText() == "RIGHT"):
            request.start_foot_selection = StepPlanRequest.RIGHT
        else:
            self.logger.log_error("Unknown start foot selection mode ('" + self.start_foot_selection_combo_box.currentText() + "')!")
            return;

        if (walk_command == PatternParameters.FORWARD):
            params.mode = PatternParameters.SAMPLING
        elif (walk_command == PatternParameters.BACKWARD):
            params.mode                      = PatternParameters.SAMPLING
            params.step_distance_forward    *= -1;
            params.step_distance_sideward   *= -1;
            params.turn_angle               *= -1;

        request.pattern_parameters = params
        request.planning_mode = StepPlanRequest.PLANNING_MODE_PATTERN
        request.parameter_set_name.data = self.parameter_set_widget.current_parameter_set_name()

        print "Send request = ", request

        # send request
        if (self.step_plan_request_client.wait_for_server(rospy.Duration(0.5))):
            self.logger.log_info("Sending footstep plan request...")

            goal = StepPlanRequestGoal()
            goal.plan_request = request;
            self.step_plan_request_client.send_goal(goal)

            if (self.step_plan_request_client.wait_for_result(rospy.Duration(5.0))):
                self.logger.log_info("Received footstep plan!")
                self.logger.log(self.step_plan_request_client.get_result().status)
                self.step_plan_pub.publish(self.step_plan_request_client.get_result().step_plan)
            else:
                self.logger.log_error("Didn't received any results. Check communcation!")
        else:
            self.logger.log_error("Can't connect to footstep planner action server!")

    def commands_set_enabled(self, enable):
        for button in self.command_buttons:
            button.setEnabled(enable) 

    @Slot()
    def param_cleared(self):
        self.commands_set_enabled(False)

    @Slot(str)
    def param_selected(self, name):
        self.commands_set_enabled(True)

    @Slot(int)
    def use_terrain_model_changed(self, state):
        enable_override = True
        if state == Qt.Checked:
            enable_override = False
        self.roll.setEnabled(enable_override)
        self.pitch.setEnabled(enable_override)
        self.override_checkbox.setEnabled(enable_override)
        self.dz.setEnabled(enable_override)
예제 #6
0
class Top(Plugin):

    NODE_FIELDS = [
        'pid', 'get_cpu_percent', 'get_memory_percent', 'get_num_threads'
    ]
    OUT_FIELDS = [
        'node_name', 'pid', 'cpu_percent', 'memory_percent', 'num_threads'
    ]
    FORMAT_STRS = ['%s', '%s', '%0.2f', '%0.2f', '%s']
    NODE_LABELS = ['Node', 'PID', 'CPU %', 'Mem %', 'Num Threads']
    SORT_TYPE = [str, str, float, float, float]
    TOOLTIPS = {
        0: ('cmdline', lambda x: '\n'.join(textwrap.wrap(' '.join(x)))),
        3: ('memory_info', lambda x:
            ('Resident: %0.2f MiB, Virtual: %0.2f MiB' %
             (x[0] / 2**20, x[1] / 2**20)))
    }

    _node_info = NodeInfo()

    name_filter = re.compile('')

    def __init__(self, context):
        super(Top, self).__init__(context)
        # Give QObjects reasonable names
        self.setObjectName('Top')

        # Process standalone plugin command-line arguments
        from argparse import ArgumentParser
        parser = ArgumentParser()
        # Add argument(s) to the parser.
        parser.add_argument("-q",
                            "--quiet",
                            action="store_true",
                            dest="quiet",
                            help="Put plugin in silent mode")
        args, unknowns = parser.parse_known_args(context.argv())
        # if not args.quiet:
        #     print 'arguments: ', args
        #     print 'unknowns: ', unknowns

        self._selected_node = ''
        self._selected_node_lock = RLock()

        # Setup the toolbar
        self._toolbar = QToolBar()
        self._filter_box = QLineEdit()
        self._regex_box = QCheckBox()
        self._regex_box.setText('regex')
        self._toolbar.addWidget(QLabel('Filter'))
        self._toolbar.addWidget(self._filter_box)
        self._toolbar.addWidget(self._regex_box)

        self._filter_box.returnPressed.connect(self.update_filter)
        self._regex_box.stateChanged.connect(self.update_filter)

        # Create a container widget and give it a layout
        self._container = QWidget()
        self._container.setWindowTitle('Process Monitor')
        self._layout = QVBoxLayout()
        self._container.setLayout(self._layout)

        self._layout.addWidget(self._toolbar)

        # Create the table widget
        self._table_widget = QTreeWidget()
        self._table_widget.setObjectName('TopTable')
        self._table_widget.setColumnCount(len(self.NODE_LABELS))
        self._table_widget.setHeaderLabels(self.NODE_LABELS)
        self._table_widget.itemClicked.connect(self._tableItemClicked)
        self._table_widget.setSortingEnabled(True)
        self._table_widget.setAlternatingRowColors(True)

        self._layout.addWidget(self._table_widget)
        context.add_widget(self._container)

        # Add a button for killing nodes
        self._kill_button = QPushButton('Kill Node')
        self._layout.addWidget(self._kill_button)
        self._kill_button.clicked.connect(self._kill_node)

        # Update twice since the first cpu% lookup will always return 0
        self.update_table()
        self.update_table()

        self._table_widget.resizeColumnToContents(0)

        # Start a timer to trigger updates
        self._update_timer = QTimer()
        self._update_timer.setInterval(1000)
        self._update_timer.timeout.connect(self.update_table)
        self._update_timer.start()

    def _tableItemClicked(self, item, column):
        with self._selected_node_lock:
            self._selected_node = item.text(0)

    def update_filter(self, *args):
        if self._regex_box.isChecked():
            expr = self._filter_box.text()
        else:
            expr = re.escape(self._filter_box.text())
        self.name_filter = re.compile(expr)
        self.update_table()

    def _kill_node(self):
        self._node_info.kill_node(self._selected_node)

    def update_one_item(self, row, info):
        twi = TopWidgetItem()
        for col, field in enumerate(self.OUT_FIELDS):
            val = info[field]
            twi.setText(col, self.FORMAT_STRS[col] % val)
        self._table_widget.insertTopLevelItem(row, twi)

        for col, (key, func) in self.TOOLTIPS.iteritems():
            twi.setToolTip(col, func(info[key]))

        with self._selected_node_lock:
            if twi.text(0) == self._selected_node:
                twi.setSelected(True)

        self._table_widget.setItemHidden(
            twi,
            len(self.name_filter.findall(info['node_name'])) == 0)

    def update_table(self):
        self._table_widget.clear()
        infos = self._node_info.get_all_node_fields(self.NODE_FIELDS)
        for nx, info in enumerate(infos):
            self.update_one_item(nx, info)

    def shutdown_plugin(self):
        self._update_timer.stop()

    def save_settings(self, plugin_settings, instance_settings):
        instance_settings.set_value('filter_text', self._filter_box.text())
        instance_settings.set_value('is_regex',
                                    int(self._regex_box.checkState()))

    def restore_settings(self, plugin_settings, instance_settings):
        self._filter_box.setText(instance_settings.value('filter_text'))
        is_regex_int = instance_settings.value('is_regex')
        if is_regex_int:
            self._regex_box.setCheckState(Qt.CheckState(is_regex_int))
        else:
            self._regex_box.setCheckState(Qt.CheckState(0))
        self.update_filter()