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
