class RobotSteering(Plugin): slider_factor = 1000.0 def __init__(self, context): super(RobotSteering, self).__init__(context) self.setObjectName('RobotSteering') self._publisher = None self._widget = QWidget() rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('rqt_robot_steering'), 'resource', 'RobotSteering.ui') loadUi(ui_file, self._widget) self._widget.setObjectName('RobotSteeringUi') if context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) context.add_widget(self._widget) self._widget.topic_line_edit.textChanged.connect( self._on_topic_changed) self._widget.stop_push_button.pressed.connect(self._on_stop_pressed) self._widget.x_linear_slider.valueChanged.connect( self._on_x_linear_slider_changed) self._widget.z_angular_slider.valueChanged.connect( self._on_z_angular_slider_changed) self._widget.increase_x_linear_push_button.pressed.connect( self._on_strong_increase_x_linear_pressed) self._widget.reset_x_linear_push_button.pressed.connect( self._on_reset_x_linear_pressed) self._widget.decrease_x_linear_push_button.pressed.connect( self._on_strong_decrease_x_linear_pressed) self._widget.increase_z_angular_push_button.pressed.connect( self._on_strong_increase_z_angular_pressed) self._widget.reset_z_angular_push_button.pressed.connect( self._on_reset_z_angular_pressed) self._widget.decrease_z_angular_push_button.pressed.connect( self._on_strong_decrease_z_angular_pressed) self._widget.max_x_linear_double_spin_box.valueChanged.connect( self._on_max_x_linear_changed) self._widget.min_x_linear_double_spin_box.valueChanged.connect( self._on_min_x_linear_changed) self._widget.max_z_angular_double_spin_box.valueChanged.connect( self._on_max_z_angular_changed) self._widget.min_z_angular_double_spin_box.valueChanged.connect( self._on_min_z_angular_changed) self.shortcut_w = QShortcut(QKeySequence(Qt.Key_W), self._widget) self.shortcut_w.setContext(Qt.ApplicationShortcut) self.shortcut_w.activated.connect(self._on_increase_x_linear_pressed) self.shortcut_x = QShortcut(QKeySequence(Qt.Key_X), self._widget) self.shortcut_x.setContext(Qt.ApplicationShortcut) self.shortcut_x.activated.connect(self._on_reset_x_linear_pressed) self.shortcut_s = QShortcut(QKeySequence(Qt.Key_S), self._widget) self.shortcut_s.setContext(Qt.ApplicationShortcut) self.shortcut_s.activated.connect(self._on_decrease_x_linear_pressed) self.shortcut_a = QShortcut(QKeySequence(Qt.Key_A), self._widget) self.shortcut_a.setContext(Qt.ApplicationShortcut) self.shortcut_a.activated.connect(self._on_increase_z_angular_pressed) self.shortcut_z = QShortcut(QKeySequence(Qt.Key_Z), self._widget) self.shortcut_z.setContext(Qt.ApplicationShortcut) self.shortcut_z.activated.connect(self._on_reset_z_angular_pressed) self.shortcut_d = QShortcut(QKeySequence(Qt.Key_D), self._widget) self.shortcut_d.setContext(Qt.ApplicationShortcut) self.shortcut_d.activated.connect(self._on_decrease_z_angular_pressed) self.shortcut_shift_w = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_W), self._widget) self.shortcut_shift_w.setContext(Qt.ApplicationShortcut) self.shortcut_shift_w.activated.connect( self._on_strong_increase_x_linear_pressed) self.shortcut_shift_x = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_X), self._widget) self.shortcut_shift_x.setContext(Qt.ApplicationShortcut) self.shortcut_shift_x.activated.connect( self._on_reset_x_linear_pressed) self.shortcut_shift_s = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_S), self._widget) self.shortcut_shift_s.setContext(Qt.ApplicationShortcut) self.shortcut_shift_s.activated.connect( self._on_strong_decrease_x_linear_pressed) self.shortcut_shift_a = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_A), self._widget) self.shortcut_shift_a.setContext(Qt.ApplicationShortcut) self.shortcut_shift_a.activated.connect( self._on_strong_increase_z_angular_pressed) self.shortcut_shift_z = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Z), self._widget) self.shortcut_shift_z.setContext(Qt.ApplicationShortcut) self.shortcut_shift_z.activated.connect( self._on_reset_z_angular_pressed) self.shortcut_shift_d = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_D), self._widget) self.shortcut_shift_d.setContext(Qt.ApplicationShortcut) self.shortcut_shift_d.activated.connect( self._on_strong_decrease_z_angular_pressed) self.shortcut_space = QShortcut(QKeySequence(Qt.Key_Space), self._widget) self.shortcut_space.setContext(Qt.ApplicationShortcut) self.shortcut_space.activated.connect(self._on_stop_pressed) self.shortcut_space = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Space), self._widget) self.shortcut_space.setContext(Qt.ApplicationShortcut) self.shortcut_space.activated.connect(self._on_stop_pressed) self._widget.stop_push_button.setToolTip( self._widget.stop_push_button.toolTip() + ' ' + self.tr('([Shift +] Space)')) self._widget.increase_x_linear_push_button.setToolTip( self._widget.increase_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] W)')) self._widget.reset_x_linear_push_button.setToolTip( self._widget.reset_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] X)')) self._widget.decrease_x_linear_push_button.setToolTip( self._widget.decrease_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] S)')) self._widget.increase_z_angular_push_button.setToolTip( self._widget.increase_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] A)')) self._widget.reset_z_angular_push_button.setToolTip( self._widget.reset_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] Z)')) self._widget.decrease_z_angular_push_button.setToolTip( self._widget.decrease_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] D)')) # timer to consecutively send twist messages self._update_parameter_timer = QTimer(self) self._update_parameter_timer.timeout.connect( self._on_parameter_changed) self._update_parameter_timer.start(100) self.zero_cmd_sent = False @Slot(str) def _on_topic_changed(self, topic): topic = str(topic) self._unregister_publisher() if topic == '': return try: self._publisher = rospy.Publisher(topic, Twist, queue_size=10) except TypeError: self._publisher = rospy.Publisher(topic, Twist) def _on_stop_pressed(self): self._widget.x_linear_slider.setValue(0) self._widget.z_angular_slider.setValue(0) def _on_x_linear_slider_changed(self): self._widget.current_x_linear_label.setText( '%0.2f m/s' % (self._widget.x_linear_slider.value() / RobotSteering.slider_factor)) self._on_parameter_changed() def _on_z_angular_slider_changed(self): self._widget.current_z_angular_label.setText( '%0.2f rad/s' % (self._widget.z_angular_slider.value() / RobotSteering.slider_factor)) self._on_parameter_changed() def _on_increase_x_linear_pressed(self): self._widget.x_linear_slider.setValue( self._widget.x_linear_slider.value() + self._widget.x_linear_slider.singleStep()) def _on_reset_x_linear_pressed(self): self._widget.x_linear_slider.setValue(0) def _on_decrease_x_linear_pressed(self): self._widget.x_linear_slider.setValue( self._widget.x_linear_slider.value() - self._widget.x_linear_slider.singleStep()) def _on_increase_z_angular_pressed(self): self._widget.z_angular_slider.setValue( self._widget.z_angular_slider.value() + self._widget.z_angular_slider.singleStep()) def _on_reset_z_angular_pressed(self): self._widget.z_angular_slider.setValue(0) def _on_decrease_z_angular_pressed(self): self._widget.z_angular_slider.setValue( self._widget.z_angular_slider.value() - self._widget.z_angular_slider.singleStep()) def _on_max_x_linear_changed(self, value): self._widget.x_linear_slider.setMaximum(value * RobotSteering.slider_factor) def _on_min_x_linear_changed(self, value): self._widget.x_linear_slider.setMinimum(value * RobotSteering.slider_factor) def _on_max_z_angular_changed(self, value): self._widget.z_angular_slider.setMaximum(value * RobotSteering.slider_factor) def _on_min_z_angular_changed(self, value): self._widget.z_angular_slider.setMinimum(value * RobotSteering.slider_factor) def _on_strong_increase_x_linear_pressed(self): self._widget.x_linear_slider.setValue( self._widget.x_linear_slider.value() + self._widget.x_linear_slider.pageStep()) def _on_strong_decrease_x_linear_pressed(self): self._widget.x_linear_slider.setValue( self._widget.x_linear_slider.value() - self._widget.x_linear_slider.pageStep()) def _on_strong_increase_z_angular_pressed(self): self._widget.z_angular_slider.setValue( self._widget.z_angular_slider.value() + self._widget.z_angular_slider.pageStep()) def _on_strong_decrease_z_angular_pressed(self): self._widget.z_angular_slider.setValue( self._widget.z_angular_slider.value() - self._widget.z_angular_slider.pageStep()) def _on_parameter_changed(self): self._send_twist( self._widget.x_linear_slider.value() / RobotSteering.slider_factor, self._widget.z_angular_slider.value() / RobotSteering.slider_factor) def _send_twist(self, x_linear, z_angular): if self._publisher is None: return twist = Twist() twist.linear.x = x_linear twist.linear.y = 0 twist.linear.z = 0 twist.angular.x = 0 twist.angular.y = 0 twist.angular.z = z_angular # Only send the zero command once so other devices can take control if x_linear == 0 and z_angular == 0: if not self.zero_cmd_sent: self.zero_cmd_sent = True self._publisher.publish(twist) else: self.zero_cmd_sent = False self._publisher.publish(twist) def _unregister_publisher(self): if self._publisher is not None: self._publisher.unregister() self._publisher = None def shutdown_plugin(self): self._update_parameter_timer.stop() self._unregister_publisher() def save_settings(self, plugin_settings, instance_settings): instance_settings.set_value('topic', self._widget.topic_line_edit.text()) instance_settings.set_value( 'vx_max', self._widget.max_x_linear_double_spin_box.value()) instance_settings.set_value( 'vx_min', self._widget.min_x_linear_double_spin_box.value()) instance_settings.set_value( 'vw_max', self._widget.max_z_angular_double_spin_box.value()) instance_settings.set_value( 'vw_min', self._widget.min_z_angular_double_spin_box.value()) def restore_settings(self, plugin_settings, instance_settings): value = instance_settings.value('topic', "/cmd_vel") value = rospy.get_param("~default_topic", value) self._widget.topic_line_edit.setText(value) value = self._widget.max_x_linear_double_spin_box.value() value = instance_settings.value('vx_max', value) value = rospy.get_param("~default_vx_max", value) self._widget.max_x_linear_double_spin_box.setValue(float(value)) value = self._widget.min_x_linear_double_spin_box.value() value = instance_settings.value('vx_min', value) value = rospy.get_param("~default_vx_min", value) self._widget.min_x_linear_double_spin_box.setValue(float(value)) value = self._widget.max_z_angular_double_spin_box.value() value = instance_settings.value('vw_max', value) value = rospy.get_param("~default_vw_max", value) self._widget.max_z_angular_double_spin_box.setValue(float(value)) value = self._widget.min_z_angular_double_spin_box.value() value = instance_settings.value('vw_min', value) value = rospy.get_param("~default_vw_min", value) self._widget.min_z_angular_double_spin_box.setValue(float(value))
class EUFSRobotSteeringGUI(Plugin): slider_factor = 1000.0 def __init__(self, context): super(EUFSRobotSteeringGUI, self).__init__(context) self.setObjectName('EUFSRobotSteeringGUI') rp = rospkg.RosPack() # Create QWidget self._widget = QWidget() # Get path to UI file which is a sibling of this file ui_file = os.path.join(rp.get_path('ros_can_sim'), 'resource', 'EUFSRobotSteeringGUI.ui') # Extend the widget with all attributes and children from UI file loadUi(ui_file, self._widget) self._widget.setObjectName('EUFSRobotSteeringGUI') # Show _widget.windowTitle on left-top of each plugin (when # it's set in _widget). This is useful when you open multiple # plugins at once. Also if you open multiple instances of your # plugin at once, these lines add number to make it easy to # tell from pane to pane. if context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) # Add widget to the user interface context.add_widget(self._widget) self._publisher = None self.inputs = ["Speed", "Acceleration", "Jerk"] self._widget.input_select_menu.addItems(self.inputs) self._widget.input_select_menu.setCurrentIndex(1) self._widget.topic_line_edit.textChanged.connect( self._on_topic_changed) self._widget.stop_push_button.pressed.connect(self._on_stop_pressed) self._widget.linear_slider.valueChanged.connect( self._on_linear_slider_changed) self._widget.angular_slider.valueChanged.connect( self._on_angular_slider_changed) self._widget.increase_linear_push_button.pressed.connect( self._on_strong_increase_linear_pressed) self._widget.reset_linear_push_button.pressed.connect( self._on_reset_linear_pressed) self._widget.decrease_linear_push_button.pressed.connect( self._on_strong_decrease_linear_pressed) self._widget.increase_angular_push_button.pressed.connect( self._on_strong_increase_angular_pressed) self._widget.reset_angular_push_button.pressed.connect( self._on_reset_angular_pressed) self._widget.decrease_angular_push_button.pressed.connect( self._on_strong_decrease_angular_pressed) self._widget.max_linear_double_spin_box.valueChanged.connect( self._on_max_linear_changed) self._widget.min_linear_double_spin_box.valueChanged.connect( self._on_min_linear_changed) self._widget.max_angular_double_spin_box.valueChanged.connect( self._on_max_angular_changed) self._widget.min_angular_double_spin_box.valueChanged.connect( self._on_min_angular_changed) self.shortcut_w = QShortcut(QKeySequence(Qt.Key_W), self._widget) self.shortcut_w.setContext(Qt.ApplicationShortcut) self.shortcut_w.activated.connect(self._on_increase_linear_pressed) self.shortcut_x = QShortcut(QKeySequence(Qt.Key_X), self._widget) self.shortcut_x.setContext(Qt.ApplicationShortcut) self.shortcut_x.activated.connect(self._on_reset_linear_pressed) self.shortcut_s = QShortcut(QKeySequence(Qt.Key_S), self._widget) self.shortcut_s.setContext(Qt.ApplicationShortcut) self.shortcut_s.activated.connect(self._on_decrease_linear_pressed) self.shortcut_a = QShortcut(QKeySequence(Qt.Key_A), self._widget) self.shortcut_a.setContext(Qt.ApplicationShortcut) self.shortcut_a.activated.connect(self._on_increase_angular_pressed) self.shortcut_z = QShortcut(QKeySequence(Qt.Key_Z), self._widget) self.shortcut_z.setContext(Qt.ApplicationShortcut) self.shortcut_z.activated.connect(self._on_reset_angular_pressed) self.shortcut_d = QShortcut(QKeySequence(Qt.Key_D), self._widget) self.shortcut_d.setContext(Qt.ApplicationShortcut) self.shortcut_d.activated.connect(self._on_decrease_angular_pressed) self.shortcut_shift_w = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_W), self._widget) self.shortcut_shift_w.setContext(Qt.ApplicationShortcut) self.shortcut_shift_w.activated.connect( self._on_strong_increase_linear_pressed) self.shortcut_shift_x = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_X), self._widget) self.shortcut_shift_x.setContext(Qt.ApplicationShortcut) self.shortcut_shift_x.activated.connect(self._on_reset_linear_pressed) self.shortcut_shift_s = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_S), self._widget) self.shortcut_shift_s.setContext(Qt.ApplicationShortcut) self.shortcut_shift_s.activated.connect( self._on_strong_decrease_linear_pressed) self.shortcut_shift_a = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_A), self._widget) self.shortcut_shift_a.setContext(Qt.ApplicationShortcut) self.shortcut_shift_a.activated.connect( self._on_strong_increase_angular_pressed) self.shortcut_shift_z = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Z), self._widget) self.shortcut_shift_z.setContext(Qt.ApplicationShortcut) self.shortcut_shift_z.activated.connect(self._on_reset_angular_pressed) self.shortcut_shift_d = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_D), self._widget) self.shortcut_shift_d.setContext(Qt.ApplicationShortcut) self.shortcut_shift_d.activated.connect( self._on_strong_decrease_angular_pressed) self.shortcut_space = QShortcut(QKeySequence(Qt.Key_Space), self._widget) self.shortcut_space.setContext(Qt.ApplicationShortcut) self.shortcut_space.activated.connect(self._on_stop_pressed) self.shortcut_space = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Space), self._widget) self.shortcut_space.setContext(Qt.ApplicationShortcut) self.shortcut_space.activated.connect(self._on_stop_pressed) self._widget.stop_push_button.setToolTip( self._widget.stop_push_button.toolTip() + ' ' + self.tr('([Shift +] Space)')) self._widget.increase_linear_push_button.setToolTip( self._widget.increase_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] W)')) self._widget.reset_linear_push_button.setToolTip( self._widget.reset_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] X)')) self._widget.decrease_linear_push_button.setToolTip( self._widget.decrease_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] S)')) self._widget.increase_angular_push_button.setToolTip( self._widget.increase_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] A)')) self._widget.reset_angular_push_button.setToolTip( self._widget.reset_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] Z)')) self._widget.decrease_angular_push_button.setToolTip( self._widget.decrease_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] D)')) # timer to consecutively send AckermannDriveStamped messages self._update_parameter_timer = QTimer(self) self._update_parameter_timer.timeout.connect( self._on_parameter_changed) self._update_parameter_timer.start(100) self.zero_cmd_sent = False @Slot(str) def _on_topic_changed(self, topic): topic = str(topic) self._unregister_publisher() if topic == '': return try: self._publisher = rospy.Publisher(topic, AckermannDriveStamped, queue_size=10) except TypeError: self._publisher = rospy.Publisher(topic, AckermannDriveStamped) def _on_stop_pressed(self): # If the current value of sliders is zero directly send stop AckermannDriveStamped msg if self._widget.linear_slider.value() == 0 and \ self._widget.angular_slider.value() == 0: self.zero_cmd_sent = False self._on_parameter_changed() else: self._widget.linear_slider.setValue(0) self._widget.angular_slider.setValue(0) def _on_linear_slider_changed(self): self._widget.current_linear_label.setText( '%0.2f m/s' % (self._widget.linear_slider.value() / EUFSRobotSteeringGUI.slider_factor)) self._on_parameter_changed() def _on_angular_slider_changed(self): self._widget.current_angular_label.setText( '%0.2f rad/s' % (self._widget.angular_slider.value() / EUFSRobotSteeringGUI.slider_factor)) self._on_parameter_changed() def _on_increase_linear_pressed(self): self._widget.linear_slider.setValue( self._widget.linear_slider.value() + self._widget.linear_slider.singleStep()) def _on_reset_linear_pressed(self): self._widget.linear_slider.setValue(0) def _on_decrease_linear_pressed(self): self._widget.linear_slider.setValue( self._widget.linear_slider.value() - self._widget.linear_slider.singleStep()) def _on_increase_angular_pressed(self): self._widget.angular_slider.setValue( self._widget.angular_slider.value() + self._widget.angular_slider.singleStep()) def _on_reset_angular_pressed(self): self._widget.angular_slider.setValue(0) def _on_decrease_angular_pressed(self): self._widget.angular_slider.setValue( self._widget.angular_slider.value() - self._widget.angular_slider.singleStep()) def _on_max_linear_changed(self, value): self._widget.linear_slider.setMaximum( value * EUFSRobotSteeringGUI.slider_factor) def _on_min_linear_changed(self, value): self._widget.linear_slider.setMinimum( value * EUFSRobotSteeringGUI.slider_factor) def _on_max_angular_changed(self, value): self._widget.angular_slider.setMaximum( value * EUFSRobotSteeringGUI.slider_factor) def _on_min_angular_changed(self, value): self._widget.angular_slider.setMinimum( value * EUFSRobotSteeringGUI.slider_factor) def _on_strong_increase_linear_pressed(self): self._widget.linear_slider.setValue( self._widget.linear_slider.value() + self._widget.linear_slider.pageStep()) def _on_strong_decrease_linear_pressed(self): self._widget.linear_slider.setValue( self._widget.linear_slider.value() - self._widget.linear_slider.pageStep()) def _on_strong_increase_angular_pressed(self): self._widget.angular_slider.setValue( self._widget.angular_slider.value() + self._widget.angular_slider.pageStep()) def _on_strong_decrease_angular_pressed(self): self._widget.angular_slider.setValue( self._widget.angular_slider.value() - self._widget.angular_slider.pageStep()) def _on_parameter_changed(self): self._send_ackermann_drive_stamped( self._widget.linear_slider.value() / EUFSRobotSteeringGUI.slider_factor, self._widget.angular_slider.value() / EUFSRobotSteeringGUI.slider_factor) def _send_ackermann_drive_stamped(self, linear, angular): if self._publisher is None: return drive = AckermannDriveStamped() drive.header.stamp = rospy.Time.now() drive.drive.acceleration = 0 drive.drive.speed = 0 drive.drive.jerk = 0 input = self._widget.input_select_menu.currentIndex() if self.inputs[input] == "Speed": drive.drive.speed = linear elif self.inputs[input] == "Acceleration": drive.drive.acceleration = linear elif self.inputs[input] == "Jerk": drive.drive.jerk = linear drive.drive.steering_angle = angular drive.drive.steering_angle_velocity = 0 # Only send the zero command once so other devices can take control if linear == 0 and angular == 0: if not self.zero_cmd_sent: self.zero_cmd_sent = True self._publisher.publish(drive) else: self.zero_cmd_sent = False self._publisher.publish(drive) def _unregister_publisher(self): if self._publisher is not None: self._publisher.unregister() self._publisher = None def shutdown_plugin(self): self._update_parameter_timer.stop() self._unregister_publisher() def save_settings(self, plugin_settings, instance_settings): instance_settings.set_value('input', self.input) instance_settings.set_value('topic', self._widget.topic_line_edit.text()) instance_settings.set_value( 'vx_max', self._widget.max_linear_double_spin_box.value()) instance_settings.set_value( 'vx_min', self._widget.min_linear_double_spin_box.value()) instance_settings.set_value( 'vw_max', self._widget.max_angular_double_spin_box.value()) instance_settings.set_value( 'vw_min', self._widget.min_angular_double_spin_box.value()) def restore_settings(self, plugin_settings, instance_settings): value = instance_settings.value('input', 1) value = rospy.get_param('~default_input', value) self.input = value value = instance_settings.value('topic', '/rqt/command') value = rospy.get_param('~default_topic', value) self._widget.topic_line_edit.setText(value) value = self._widget.max_linear_double_spin_box.value() value = instance_settings.value('vx_max', value) value = rospy.get_param('~default_vx_max', value) self._widget.max_linear_double_spin_box.setValue(float(value)) value = self._widget.min_linear_double_spin_box.value() value = instance_settings.value('vx_min', value) value = rospy.get_param('~default_vx_min', value) self._widget.min_linear_double_spin_box.setValue(float(value)) value = self._widget.max_angular_double_spin_box.value() value = instance_settings.value('vw_max', value) value = rospy.get_param('~default_vw_max', value) self._widget.max_angular_double_spin_box.setValue(float(value)) value = self._widget.min_angular_double_spin_box.value() value = instance_settings.value('vw_min', value) value = rospy.get_param('~default_vw_min', value) self._widget.min_angular_double_spin_box.setValue(float(value))
class ExamplesWidget(QWidget): def __init__(self, node): super(ExamplesWidget, self).__init__() self.setObjectName('ExamplesWidget') self.node = node self.REDRAW_INTERVAL = 30 self.PUBLISH_INTERVAL = 100 self.CMD_VEL_X_FACTOR = 1000.0 self.CMD_VEL_YAW_FACTOR = -10.0 pkg_name = 'rqt_example' ui_filename = 'rqt_example.ui' topic_name = 'cmd_vel' service_name = 'led_control' _, package_path = get_resource('packages', pkg_name) ui_file = os.path.join(package_path, 'share', pkg_name, 'resource', ui_filename) loadUi(ui_file, self) self.pub_velocity = Twist() self.pub_velocity.linear.x = 0.0 self.pub_velocity.angular.z = 0.0 self.sub_velocity = Twist() self.sub_velocity.linear.x = 0.0 self.sub_velocity.angular.z = 0.0 self.slider_x.setValue(0) self.lcd_number_x.display(0.0) self.lcd_number_yaw.display(0.0) qos = QoSProfile(depth=10) self.publisher = self.node.create_publisher(Twist, topic_name, qos) self.subscriber = self.node.create_subscription( Twist, topic_name, self.get_velocity, qos) self.service_server = self.node.create_service(SetBool, service_name, self.set_led_status) self.service_client = self.node.create_client(SetBool, service_name) self.publish_timer = QTimer(self) self.publish_timer.timeout.connect(self.send_velocity) self.publish_timer.start(self.PUBLISH_INTERVAL) self.update_timer = QTimer(self) self.update_timer.timeout.connect(self.update_indicators) self.update_timer.start(self.REDRAW_INTERVAL) self.push_button_w.pressed.connect(self.increase_linear_x) self.push_button_x.pressed.connect(self.decrease_linear_x) self.push_button_a.pressed.connect(self.increase_angular_z) self.push_button_d.pressed.connect(self.decrease_angular_z) self.push_button_s.pressed.connect(self.set_stop) self.push_button_w.setShortcut('w') self.push_button_x.setShortcut('x') self.push_button_a.setShortcut('a') self.push_button_d.setShortcut('d') self.push_button_s.setShortcut('s') self.shortcut_space = QShortcut(QKeySequence(Qt.Key_Space), self) self.shortcut_space.setContext(Qt.ApplicationShortcut) self.shortcut_space.activated.connect(self.push_button_s.pressed) self.radio_button_led_on.clicked.connect(self.call_led_service) self.radio_button_led_off.clicked.connect(self.call_led_service) self.radio_button_led_on.setShortcut('o') self.radio_button_led_off.setShortcut('f') def get_velocity(self, msg): self.sub_velocity = msg def set_led_status(self, request, response): if request.data: self.push_button_led_status.setText('ON') self.push_button_led_status.setStyleSheet( 'color: rgb(255, 170, 0);') response.success = True response.message = 'LED ON' elif not request.data: self.push_button_led_status.setText('OFF') self.push_button_led_status.setStyleSheet('') response.success = True response.message = 'LED OFF' else: response.success = False return response def increase_linear_x(self): self.pub_velocity.linear.x += 0.1 def decrease_linear_x(self): self.pub_velocity.linear.x -= 0.1 def increase_angular_z(self): self.pub_velocity.angular.z += 0.1 def decrease_angular_z(self): self.pub_velocity.angular.z -= 0.1 def set_stop(self): self.pub_velocity.linear.x = 0.0 self.pub_velocity.angular.z = 0.0 def call_led_service(self): request = SetBool.Request() if self.radio_button_led_on.isChecked(): request.data = True elif self.radio_button_led_off.isChecked(): request.data = False wait_count = 1 while not self.service_client.wait_for_service(timeout_sec=0.5): if wait_count > 5: return self.node.get_logger().error( 'Service not available #{0}'.format(wait_count)) wait_count += 1 future = self.service_client.call_async(request) while rclpy.ok(): if future.done(): if future.result() is not None: response = future.result() self.node.get_logger().info( 'Result of service call: {0}'.format(response.message)) else: self.node.get_logger().error('Error calling service') break def send_velocity(self): twist = Twist() twist.linear.x = self.pub_velocity.linear.x twist.linear.y = 0.0 twist.linear.z = 0.0 twist.angular.x = 0.0 twist.angular.y = 0.0 twist.angular.z = self.pub_velocity.angular.z self.publisher.publish(twist) def update_indicators(self): self.slider_x.setValue(self.sub_velocity.linear.x * self.CMD_VEL_X_FACTOR) self.dial_yaw.setValue(self.sub_velocity.angular.z * self.CMD_VEL_YAW_FACTOR) self.lcd_number_x.display(self.sub_velocity.linear.x) self.lcd_number_yaw.display(self.sub_velocity.angular.z) def shutdown_widget(self): self.update_timer.stop() self.publish_timer.stop() self.node.destroy_client(self.service_client) self.node.destroy_service(self.service_server) self.node.destroy_subscription(self.subscriber) self.node.destroy_publisher(self.publisher)
class RobotSteering(Plugin): slider_factor = 1000.0 def __init__(self, context): super(RobotSteering, self).__init__(context) self.setObjectName('RobotSteering') self._publisher = None self._widget = QWidget() rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('rqt_robot_steering'), 'resource', 'RobotSteering.ui') loadUi(ui_file, self._widget) self._widget.setObjectName('RobotSteeringUi') if context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) context.add_widget(self._widget) self._widget.topic_line_edit.textChanged.connect(self._on_topic_changed) self._widget.stop_push_button.pressed.connect(self._on_stop_pressed) self._widget.x_linear_slider.valueChanged.connect(self._on_x_linear_slider_changed) self._widget.z_angular_slider.valueChanged.connect(self._on_z_angular_slider_changed) self._widget.increase_x_linear_push_button.pressed.connect(self._on_strong_increase_x_linear_pressed) self._widget.reset_x_linear_push_button.pressed.connect(self._on_reset_x_linear_pressed) self._widget.decrease_x_linear_push_button.pressed.connect(self._on_strong_decrease_x_linear_pressed) self._widget.increase_z_angular_push_button.pressed.connect(self._on_strong_increase_z_angular_pressed) self._widget.reset_z_angular_push_button.pressed.connect(self._on_reset_z_angular_pressed) self._widget.decrease_z_angular_push_button.pressed.connect(self._on_strong_decrease_z_angular_pressed) self._widget.max_x_linear_double_spin_box.valueChanged.connect(self._on_max_x_linear_changed) self._widget.min_x_linear_double_spin_box.valueChanged.connect(self._on_min_x_linear_changed) self._widget.max_z_angular_double_spin_box.valueChanged.connect(self._on_max_z_angular_changed) self._widget.min_z_angular_double_spin_box.valueChanged.connect(self._on_min_z_angular_changed) self.shortcut_w = QShortcut(QKeySequence(Qt.Key_W), self._widget) self.shortcut_w.setContext(Qt.ApplicationShortcut) self.shortcut_w.activated.connect(self._on_increase_x_linear_pressed) self.shortcut_x = QShortcut(QKeySequence(Qt.Key_X), self._widget) self.shortcut_x.setContext(Qt.ApplicationShortcut) self.shortcut_x.activated.connect(self._on_reset_x_linear_pressed) self.shortcut_s = QShortcut(QKeySequence(Qt.Key_S), self._widget) self.shortcut_s.setContext(Qt.ApplicationShortcut) self.shortcut_s.activated.connect(self._on_decrease_x_linear_pressed) self.shortcut_a = QShortcut(QKeySequence(Qt.Key_A), self._widget) self.shortcut_a.setContext(Qt.ApplicationShortcut) self.shortcut_a.activated.connect(self._on_increase_z_angular_pressed) self.shortcut_z = QShortcut(QKeySequence(Qt.Key_Z), self._widget) self.shortcut_z.setContext(Qt.ApplicationShortcut) self.shortcut_z.activated.connect(self._on_reset_z_angular_pressed) self.shortcut_d = QShortcut(QKeySequence(Qt.Key_D), self._widget) self.shortcut_d.setContext(Qt.ApplicationShortcut) self.shortcut_d.activated.connect(self._on_decrease_z_angular_pressed) self.shortcut_shift_w = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_W), self._widget) self.shortcut_shift_w.setContext(Qt.ApplicationShortcut) self.shortcut_shift_w.activated.connect(self._on_strong_increase_x_linear_pressed) self.shortcut_shift_x = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_X), self._widget) self.shortcut_shift_x.setContext(Qt.ApplicationShortcut) self.shortcut_shift_x.activated.connect(self._on_reset_x_linear_pressed) self.shortcut_shift_s = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_S), self._widget) self.shortcut_shift_s.setContext(Qt.ApplicationShortcut) self.shortcut_shift_s.activated.connect(self._on_strong_decrease_x_linear_pressed) self.shortcut_shift_a = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_A), self._widget) self.shortcut_shift_a.setContext(Qt.ApplicationShortcut) self.shortcut_shift_a.activated.connect(self._on_strong_increase_z_angular_pressed) self.shortcut_shift_z = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Z), self._widget) self.shortcut_shift_z.setContext(Qt.ApplicationShortcut) self.shortcut_shift_z.activated.connect(self._on_reset_z_angular_pressed) self.shortcut_shift_d = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_D), self._widget) self.shortcut_shift_d.setContext(Qt.ApplicationShortcut) self.shortcut_shift_d.activated.connect(self._on_strong_decrease_z_angular_pressed) self.shortcut_space = QShortcut(QKeySequence(Qt.Key_Space), self._widget) self.shortcut_space.setContext(Qt.ApplicationShortcut) self.shortcut_space.activated.connect(self._on_stop_pressed) self.shortcut_space = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Space), self._widget) self.shortcut_space.setContext(Qt.ApplicationShortcut) self.shortcut_space.activated.connect(self._on_stop_pressed) self._widget.stop_push_button.setToolTip(self._widget.stop_push_button.toolTip() + ' ' + self.tr('([Shift +] Space)')) self._widget.increase_x_linear_push_button.setToolTip(self._widget.increase_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] W)')) self._widget.reset_x_linear_push_button.setToolTip(self._widget.reset_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] X)')) self._widget.decrease_x_linear_push_button.setToolTip(self._widget.decrease_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] S)')) self._widget.increase_z_angular_push_button.setToolTip(self._widget.increase_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] A)')) self._widget.reset_z_angular_push_button.setToolTip(self._widget.reset_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] Z)')) self._widget.decrease_z_angular_push_button.setToolTip(self._widget.decrease_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] D)')) # timer to consecutively send twist messages self._update_parameter_timer = QTimer(self) self._update_parameter_timer.timeout.connect(self._on_parameter_changed) self._update_parameter_timer.start(100) self.zero_cmd_sent = False @Slot(str) def _on_topic_changed(self, topic): topic = str(topic) self._unregister_publisher() try: self._publisher = rospy.Publisher(topic, Twist, queue_size=10) except TypeError: self._publisher = rospy.Publisher(topic, Twist) def _on_stop_pressed(self): self._widget.x_linear_slider.setValue(0) self._widget.z_angular_slider.setValue(0) def _on_x_linear_slider_changed(self): self._widget.current_x_linear_label.setText('%0.2f m/s' % (self._widget.x_linear_slider.value() / RobotSteering.slider_factor)) self._on_parameter_changed() def _on_z_angular_slider_changed(self): self._widget.current_z_angular_label.setText('%0.2f rad/s' % (self._widget.z_angular_slider.value() / RobotSteering.slider_factor)) self._on_parameter_changed() def _on_increase_x_linear_pressed(self): self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() + self._widget.x_linear_slider.singleStep()) def _on_reset_x_linear_pressed(self): self._widget.x_linear_slider.setValue(0) def _on_decrease_x_linear_pressed(self): self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() - self._widget.x_linear_slider.singleStep()) def _on_increase_z_angular_pressed(self): self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() + self._widget.z_angular_slider.singleStep()) def _on_reset_z_angular_pressed(self): self._widget.z_angular_slider.setValue(0) def _on_decrease_z_angular_pressed(self): self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() - self._widget.z_angular_slider.singleStep()) def _on_max_x_linear_changed(self, value): self._widget.x_linear_slider.setMaximum(value * RobotSteering.slider_factor) def _on_min_x_linear_changed(self, value): self._widget.x_linear_slider.setMinimum(value * RobotSteering.slider_factor) def _on_max_z_angular_changed(self, value): self._widget.z_angular_slider.setMaximum(value * RobotSteering.slider_factor) def _on_min_z_angular_changed(self, value): self._widget.z_angular_slider.setMinimum(value * RobotSteering.slider_factor) def _on_strong_increase_x_linear_pressed(self): self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() + self._widget.x_linear_slider.pageStep()) def _on_strong_decrease_x_linear_pressed(self): self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() - self._widget.x_linear_slider.pageStep()) def _on_strong_increase_z_angular_pressed(self): self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() + self._widget.z_angular_slider.pageStep()) def _on_strong_decrease_z_angular_pressed(self): self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() - self._widget.z_angular_slider.pageStep()) def _on_parameter_changed(self): self._send_twist(self._widget.x_linear_slider.value() / RobotSteering.slider_factor, self._widget.z_angular_slider.value() / RobotSteering.slider_factor) def _send_twist(self, x_linear, z_angular): if self._publisher is None: return twist = Twist() twist.linear.x = x_linear twist.linear.y = 0 twist.linear.z = 0 twist.angular.x = 0 twist.angular.y = 0 twist.angular.z = z_angular # Only send the zero command once so other devices can take control if x_linear == 0 and z_angular == 0: if not self.zero_cmd_sent: self.zero_cmd_sent = True self._publisher.publish(twist) else: self.zero_cmd_sent = False self._publisher.publish(twist) def _unregister_publisher(self): if self._publisher is not None: self._publisher.unregister() self._publisher = None def shutdown_plugin(self): self._update_parameter_timer.stop() self._unregister_publisher() def save_settings(self, plugin_settings, instance_settings): instance_settings.set_value('topic' , self._widget.topic_line_edit.text()) instance_settings.set_value('vx_max', self._widget.max_x_linear_double_spin_box.value()) instance_settings.set_value('vx_min', self._widget.min_x_linear_double_spin_box.value()) instance_settings.set_value('vw_max', self._widget.max_z_angular_double_spin_box.value()) instance_settings.set_value('vw_min', self._widget.min_z_angular_double_spin_box.value()) def restore_settings(self, plugin_settings, instance_settings): value = instance_settings.value('topic', "/cmd_vel") value = rospy.get_param("~default_topic", value) self._widget.topic_line_edit.setText(value) value = self._widget.max_x_linear_double_spin_box.value() value = instance_settings.value( 'vx_max', value) value = rospy.get_param("~default_vx_max", value) self._widget.max_x_linear_double_spin_box.setValue(float(value)) value = self._widget.min_x_linear_double_spin_box.value() value = instance_settings.value( 'vx_min', value) value = rospy.get_param("~default_vx_min", value) self._widget.min_x_linear_double_spin_box.setValue(float(value)) value = self._widget.max_z_angular_double_spin_box.value() value = instance_settings.value( 'vw_max', value) value = rospy.get_param("~default_vw_max", value) self._widget.max_z_angular_double_spin_box.setValue(float(value)) value = self._widget.min_z_angular_double_spin_box.value() value = instance_settings.value( 'vw_min', value) value = rospy.get_param("~default_vw_min", value) self._widget.min_z_angular_double_spin_box.setValue(float(value))
class ExamplesWidget(QWidget): redraw_interval = 30 publish_interval = 10 cmd_vel_x_factor = 1000.0 cmd_vel_yaw_factor = -10.0 def __init__(self, node): super(ExamplesWidget, self).__init__() self.setObjectName('ExamplesWidget') self.node = node pkg_name = 'examples_rqt' ui_filename = 'examples_rqt.ui' topic_name = 'cmd_vel' service_name = 'led_control' _, package_path = get_resource('packages', pkg_name) ui_file = os.path.join(package_path, 'share', pkg_name, 'resource', ui_filename) loadUi(ui_file, self) self.pub_velocity = Twist() self.pub_velocity.linear.x = 0.0 self.pub_velocity.angular.z = 0.0 self.sub_velocity = Twist() self.sub_velocity.linear.x = 0.0 self.sub_velocity.angular.z = 0.0 self.slider_x.setValue(0) self.lcd_number_x.display(0.0) self.lcd_number_yaw.display(0.0) qos_profile = 0 qos = self.get_qos(qos_profile) self.publisher = self.node.create_publisher(Twist, topic_name, qos) self.subscriber = self.node.create_subscription(Twist, topic_name, self.cmd_callback, qos) self.service_server = self.node.create_service(SetBool, service_name, self.srv_callback) self.service_client = self.node.create_client(SetBool, service_name) update_timer = QTimer(self) update_timer.timeout.connect(self.update_data) update_timer.start(self.redraw_interval) self.push_button_w.pressed.connect(self.on_increase_x_linear_pressed) self.push_button_x.pressed.connect(self.on_decrease_x_linear_pressed) self.push_button_a.pressed.connect(self.on_increase_z_angular_pressed) self.push_button_d.pressed.connect(self.on_decrease_z_angular_pressed) self.push_button_s.pressed.connect(self.on_stop_pressed) self.radio_button_led_on.clicked.connect(self.call_led_service) self.radio_button_led_off.clicked.connect(self.call_led_service) self.push_button_w.setShortcut('w') self.push_button_x.setShortcut('x') self.push_button_a.setShortcut('a') self.push_button_d.setShortcut('d') self.push_button_s.setShortcut('s') self.shortcut_space = QShortcut(QKeySequence(Qt.Key_Space), self) self.shortcut_space.setContext(Qt.ApplicationShortcut) self.shortcut_space.activated.connect(self.push_button_s.pressed) self.radio_button_led_on.setShortcut('o') self.radio_button_led_off.setShortcut('f') def get_qos(self, i): return { 0: QoSProfile(depth=10), 1: qos_profile_sensor_data, 2: qos_profile_parameters, 3: qos_profile_services_default, 4: qos_profile_parameter_events, 5: qos_profile_system_default, 6: qos_profile_action_status_default}[i] def cmd_callback(self, msg): self.sub_velocity = msg def srv_callback(self, request, response): if request.data: self.push_button_led_status.setText('ON') self.push_button_led_status.setStyleSheet('background-color: yellow') response.success = True response.message = 'LED ON' elif not request.data: self.push_button_led_status.setText('OFF') self.push_button_led_status.setStyleSheet('background-color: grey') response.success = True response.message = 'LED OFF' else: response.success = False return response def on_increase_x_linear_pressed(self): self.pub_velocity.linear.x += 0.01 self.send_velocity(self.pub_velocity.linear.x, self.pub_velocity.angular.z) def on_decrease_x_linear_pressed(self): self.pub_velocity.linear.x -= 0.01 self.send_velocity(self.pub_velocity.linear.x, self.pub_velocity.angular.z) def on_increase_z_angular_pressed(self): self.pub_velocity.angular.z += 0.1 self.send_velocity(self.pub_velocity.linear.x, self.pub_velocity.angular.z) def on_decrease_z_angular_pressed(self): self.pub_velocity.angular.z -= 0.1 self.send_velocity(self.pub_velocity.linear.x, self.pub_velocity.angular.z) def on_stop_pressed(self): self.pub_velocity.linear.x = 0.0 self.pub_velocity.angular.z = 0.0 self.send_velocity(self.pub_velocity.linear.x, self.pub_velocity.angular.z) def call_led_service(self): request = SetBool.Request() if self.radio_button_led_on.isChecked(): request.data = True elif self.radio_button_led_off.isChecked(): request.data = False wait_count = 1 while not self.service_client.wait_for_service(timeout_sec=0.5): if wait_count > 5: return self.node.get_logger().error('Service not available #{0}'.format(wait_count)) wait_count += 1 future = self.service_client.call_async(request) while rclpy.ok(): if future.done(): if future.result() is not None: response = future.result() self.node.get_logger().info( 'Result of service call: {0}'.format(response.message)) else: self.node.get_logger().error('Error calling service') break def send_velocity(self, x_linear, z_angular): if self.publisher is None: return twist = Twist() twist.linear.x = x_linear twist.linear.y = 0.0 twist.linear.z = 0.0 twist.angular.x = 0.0 twist.angular.y = 0.0 twist.angular.z = z_angular self.publisher.publish(twist) def update_data(self): self.slider_x.setValue(self.sub_velocity.linear.x * self.cmd_vel_x_factor) self.dial_yaw.setValue(self.sub_velocity.angular.z * self.cmd_vel_yaw_factor) self.lcd_number_x.display(self.sub_velocity.linear.x) self.lcd_number_yaw.display(self.sub_velocity.angular.z) def save_settings(self, plugin_settings, instance_settings): pass def restore_settings(self, plugin_settings, instance_settings): pass def trigger_configuration(self): pass