def loadGUI(self):
"""Load the GUI from the .py file that was generated by the .ui file
using the pyside-uic tool."""
# import generated .py file here to prevent circular reference
from sim.main_window import Ui_main_window
self.main = Ui_main_window()
self.main.setupUi(self)
from sim.settings_dialog import Ui_settings_dialog
self.settings = Ui_settings_dialog()
self.dialog = QtGui.QDialog()
self.settings.setupUi(self.dialog)
self.connect_signals_to_slots()
# Add items to pull down menus
self.settings.laser_combo.addItems(
["Custom", "SICK LMS111", "Hokuyo URG-04LX"])
self.settings.robot_combo.addItems(
["Custom", "Clearpath Husky A200", "MobileRobots P3AT"])
def loadGUI(self):
"""Load the GUI from the .py file that was generated by the .ui file
using the pyside-uic tool."""
# import generated .py file here to prevent circular reference
from sim.main_window import Ui_main_window
self.main = Ui_main_window()
self.main.setupUi(self)
from sim.settings_dialog import Ui_settings_dialog
self.settings = Ui_settings_dialog()
self.dialog = QtGui.QDialog()
self.settings.setupUi(self.dialog)
self.connect_signals_to_slots()
# Add items to pull down menus
self.settings.laser_combo.addItems(["Custom", "SICK LMS111",
"Hokuyo URG-04LX"])
self.settings.robot_combo.addItems(["Custom", "Clearpath Husky A200",
"MobileRobots P3AT"])
class MainWindow(QtGui.QMainWindow):
"""The main window. This window displays all the widgets."""
def __init__(self):
super(MainWindow, self).__init__()
self.robot = mod.Robot()
self.loadGUI()
# Place and scale the logo
pkg_dir = rospkg.RosPack().get_path('msl_sim')
pixmap = QtGui.QPixmap(
os.path.join(pkg_dir, 'src', 'img', 'msl_logo.png'))
self.main.logo_label.setPixmap(pixmap)
self.main.logo_label.setAlignment(QtCore.Qt.AlignCenter)
# Set initial scale of main plot
self.main.graphics_view.set_scale(0.6)
# Give the zoomed in plotting area the same scene as the zoomed out one
self.main.graphics_view_zoom.setScene(self.main.graphics_view.scene())
# Initialize the camera in the zoomed in plotting window
self.main.graphics_view_zoom.initializeCamera(self.robot)
# Give the zoomed-out plotting area a copy of the zoomed-in plotting
# area so it can change it based on its timers
self.main.graphics_view.zoom = self.main.graphics_view_zoom
# Give the plotting area the same robot as the rest of the GUI and start
# updating it via timers
self.main.graphics_view.robot = self.robot
self.main.graphics_view.initialiseRobot()
self.settings_to_default()
# Start a timer that updates the labels
self.label_timer = QtCore.QTimer()
self.label_timer.timeout.connect(self.update_info_labels)
self.label_timer.start()
# Start timers that update the plot and the model
self.main.graphics_view.start_timers()
# Initialize the ROS parameters
self.initialize_parameters()
# --------------------------------------------------------------------------
# SETUP METHODS
# --------------------------------------------------------------------------
def connect_signals_to_slots(self):
# -----
# ROBOT
# -----
self.settings.robot_ang_vel_box.valueChanged.connect(
self.robot_ang_vel_changed)
self.settings.robot_ang_vel_slider.valueChanged.connect(
self.robot_ang_vel_changed)
self.settings.robot_combo.currentIndexChanged.connect(
self.robot_combo_changed)
self.settings.robot_length_box.valueChanged.connect(
self.robot_length_changed)
self.settings.robot_length_slider.valueChanged.connect(
lambda: self.robot_length_changed(self.settings.robot_length_slider
.value() / 10.0))
self.settings.robot_wheel_rad_box.valueChanged.connect(
self.robot_wheel_rad_changed)
self.settings.robot_wheel_rad_slider.valueChanged.connect(
lambda: self.robot_wheel_rad_changed(
self.settings.robot_wheel_rad_slider.value() / 100.0))
self.settings.robot_wheelbase_box.valueChanged.connect(
self.robot_wheelbase_changed)
self.settings.robot_wheelbase_slider.valueChanged.connect(
lambda: self.robot_wheelbase_changed(
self.settings.robot_wheelbase_slider.value() / 10.0))
self.settings.robot_width_box.valueChanged.connect(
self.robot_width_changed)
self.settings.robot_width_slider.valueChanged.connect(
lambda: self.robot_width_changed(self.settings.robot_width_slider.
value() / 10.0))
self.settings.robot_vel_box.valueChanged.connect(
self.robot_vel_changed)
self.settings.robot_vel_slider.valueChanged.connect(
lambda: self.robot_vel_changed(self.settings.robot_vel_slider.
value() / 10.0))
# --------
# COMPASS
# --------
self.settings.compass_freq_box.valueChanged.connect(
self.compass_freq_changed)
self.settings.compass_freq_slider.valueChanged.connect(
self.compass_freq_changed)
self.settings.compass_noise_box.valueChanged.connect(
self.compass_noise_changed)
self.settings.compass_noise_slider.valueChanged.connect(
self.compass_noise_changed)
# --------
# GPS
# --------
self.settings.gps_freq_box.valueChanged.connect(self.gps_freq_changed)
self.settings.gps_freq_slider.valueChanged.connect(
self.gps_freq_changed)
self.settings.gps_noise_box.valueChanged.connect(
self.gps_noise_changed)
self.settings.gps_noise_slider.valueChanged.connect(
lambda: self.gps_noise_changed(self.settings.gps_noise_slider.
value() / 100.0))
# --------
# GYROSCOPE
# --------
self.settings.gyro_freq_box.valueChanged.connect(
self.gyro_freq_changed)
self.settings.gyro_freq_slider.valueChanged.connect(
self.gyro_freq_changed)
self.settings.gyro_noise_box.valueChanged.connect(
self.gyro_noise_changed)
self.settings.gyro_noise_slider.valueChanged.connect(
lambda: self.gyro_noise_changed(self.settings.gyro_noise_slider.
value() / 100.0))
# --------
# ODOMETER
# --------
self.settings.odom_freq_box.valueChanged.connect(
self.odom_freq_changed)
self.settings.odom_freq_slider.valueChanged.connect(
self.odom_freq_changed)
self.settings.odom_noise_box.valueChanged.connect(
self.odom_noise_changed)
self.settings.odom_noise_slider.valueChanged.connect(
lambda: self.odom_noise_changed(self.settings.odom_noise_slider.
value() / 10.0))
self.settings.odom_res_box.valueChanged.connect(self.odom_res_changed)
self.settings.odom_res_slider.valueChanged.connect(
lambda: self.odom_res_changed(self.settings.odom_res_slider.value(
) / 100.0))
# -----
# LASER
# -----
self.settings.laser_combo.currentIndexChanged.connect(
self.laser_combo_changed)
self.settings.laser_freq_box.valueChanged.connect(
self.laser_freq_changed)
self.settings.laser_freq_slider.valueChanged.connect(
self.laser_freq_changed)
self.settings.laser_max_bear_box.valueChanged.connect(
self.laser_max_bear_changed)
self.settings.laser_max_bear_slider.valueChanged.connect(
self.laser_max_bear_changed)
self.settings.laser_min_bear_box.valueChanged.connect(
self.laser_min_bear_changed)
self.settings.laser_min_bear_slider.valueChanged.connect(
self.laser_min_bear_changed)
self.settings.laser_noise_box.valueChanged.connect(
self.laser_noise_changed)
self.settings.laser_noise_slider.valueChanged.connect(
lambda: self.laser_noise_changed(self.settings.laser_noise_slider.
value() / 100.0))
self.settings.laser_range_box.valueChanged.connect(
self.laser_range_changed)
self.settings.laser_range_slider.valueChanged.connect(
self.laser_range_changed)
self.settings.laser_res_box.valueChanged.connect(
self.laser_res_changed)
self.settings.laser_res_slider.valueChanged.connect(
lambda: self.laser_res_changed(self.settings.laser_res_slider.
value() / 10.0))
# -----
# MAPPING
# -----
self.main.load_map_button.clicked.connect(self.load_map)
# -----
# SETTINGS
# -----
self.main.settings_button.clicked.connect(self.open_settings_dialog)
# -----
# OTHER
# -----
self.settings.ang_vel_inc_box.valueChanged.connect(
self.ang_vel_inc_changed)
self.settings.laser_check.stateChanged.connect(
self.laser_check_changed)
self.settings.map_check.stateChanged.connect(self.map_check_changed)
self.settings.vel_inc_box.valueChanged.connect(self.vel_inc_changed)
self.main.zoom_in_button.clicked.connect(self.zoom_in)
self.main.zoom_out_button.clicked.connect(self.zoom_out)
def loadGUI(self):
"""Load the GUI from the .py file that was generated by the .ui file
using the pyside-uic tool."""
# import generated .py file here to prevent circular reference
from sim.main_window import Ui_main_window
self.main = Ui_main_window()
self.main.setupUi(self)
from sim.settings_dialog import Ui_settings_dialog
self.settings = Ui_settings_dialog()
self.dialog = QtGui.QDialog()
self.settings.setupUi(self.dialog)
self.connect_signals_to_slots()
# Add items to pull down menus
self.settings.laser_combo.addItems(
["Custom", "SICK LMS111", "Hokuyo URG-04LX"])
self.settings.robot_combo.addItems(
["Custom", "Clearpath Husky A200", "MobileRobots P3AT"])
# --------------------------------------------------------------------------
# SLOTS
# --------------------------------------------------------------------------
# -----
# ROBOT
# -----
def robot_ang_vel_changed(self, value):
self.settings.robot_ang_vel_slider.setValue(value)
self.settings.robot_ang_vel_box.setValue(value)
self.robot.max_ang_vel = value * math.pi / 180
def robot_combo_changed(self, value):
if value == 0: # Custom
self.toggle_enable_robot_settings(True)
elif value == 1: # Clearpath Husky A200
self.robot_length_changed(d.HUSKY_LENGTH)
self.robot_width_changed(d.HUSKY_WIDTH)
self.robot_wheel_rad_changed(d.HUSKY_WHEEL_RAD)
self.robot_wheelbase_changed(d.HUSKY_WHEELBASE)
self.robot_vel_changed(d.HUSKY_MAX_VEL)
self.robot_ang_vel_changed(d.HUSKY_MAX_ANG_VEL)
self.toggle_enable_robot_settings(False)
elif value == 2: # MobileRobots P3AT
self.robot_length_changed(d.P3AT_LENGTH)
self.robot_width_changed(d.P3AT_WIDTH)
self.robot_wheel_rad_changed(d.P3AT_WHEEL_RAD)
self.robot_wheelbase_changed(d.P3AT_WHEELBASE)
self.robot_vel_changed(d.P3AT_MAX_VEL)
self.robot_ang_vel_changed(d.P3AT_MAX_ANG_VEL)
self.toggle_enable_robot_settings(False)
def robot_length_changed(self, value):
self.settings.robot_length_slider.setValue(int(10 * value))
self.settings.robot_length_box.setValue(value)
self.robot.set_length(value)
def robot_wheel_rad_changed(self, value):
self.settings.robot_wheel_rad_slider.setValue(int(100 * value))
self.settings.robot_wheel_rad_box.setValue(value)
self.robot.wheel_rad = value
rospy.set_param('~robot_wheel_radius', value)
def robot_wheelbase_changed(self, value):
self.settings.robot_wheelbase_slider.setValue(int(10 * value))
self.settings.robot_wheelbase_box.setValue(value)
self.robot.wheelbase = value
rospy.set_param('~robot_track', value)
def robot_width_changed(self, value):
self.settings.robot_width_slider.setValue(int(10 * value))
self.settings.robot_width_box.setValue(value)
self.robot.set_width(value)
def robot_vel_changed(self, value):
self.settings.robot_vel_slider.setValue(int(10 * value))
self.settings.robot_vel_box.setValue(value)
self.robot.max_vel = value
# --------
# COMPASS
# --------
def compass_freq_changed(self, value):
self.settings.compass_freq_slider.setValue(value)
self.settings.compass_freq_box.setValue(value)
self.robot.compass.freq = value
self.main.graphics_view.set_timer_frequencies()
def compass_noise_changed(self, value):
self.settings.compass_noise_slider.setValue(value)
self.settings.compass_noise_box.setValue(value)
self.robot.compass.noise = math.radians(value)
rospy.set_param('~compass_noise', math.radians(value))
# --------
# GPS
# --------
def gps_freq_changed(self, value):
self.settings.gps_freq_slider.setValue(value)
self.settings.gps_freq_box.setValue(value)
self.robot.gps.freq = value
self.main.graphics_view.set_timer_frequencies()
def gps_noise_changed(self, value):
self.settings.gps_noise_slider.blockSignals(True)
self.settings.gps_noise_slider.setValue(int(100 * value))
self.settings.gps_noise_slider.blockSignals(False)
self.settings.gps_noise_box.setValue(value)
self.robot.gps.noise = value
rospy.set_param('~gps_noise', value)
# --------
# GYRO
# --------
def gyro_freq_changed(self, value):
self.settings.gyro_freq_slider.setValue(value)
self.settings.gyro_freq_box.setValue(value)
self.robot.gyroscope.freq = value
self.main.graphics_view.set_timer_frequencies()
def gyro_noise_changed(self, value):
self.settings.gyro_noise_slider.blockSignals(True)
self.settings.gyro_noise_slider.setValue(int(100 * value))
self.settings.gyro_noise_slider.blockSignals(False)
self.settings.gyro_noise_box.setValue(value)
self.robot.gyroscope.noise = math.radians(value)
rospy.set_param('~gyro_noise', math.radians(value))
# --------
# ODOMETER
# --------
def odom_freq_changed(self, value):
self.settings.odom_freq_slider.setValue(value)
self.settings.odom_freq_box.setValue(value)
self.robot.odometer.freq = value
self.main.graphics_view.set_timer_frequencies()
def odom_noise_changed(self, value):
self.settings.odom_noise_slider.setValue(int(10 * value))
self.settings.odom_noise_box.setValue(value)
self.robot.odometer.noise = value
rospy.set_param('~encoder_noise', value)
def odom_res_changed(self, value):
self.settings.odom_res_slider.blockSignals(True)
self.settings.odom_res_slider.setValue(int(100 * value))
self.settings.odom_res_slider.blockSignals(False)
self.settings.odom_res_box.setValue(value)
self.robot.odometer.res = value
rospy.set_param('~encoder_resolution', value)
# -----
# LASER
# -----
def laser_combo_changed(self, value):
if value == 0: # Custom
self.toggle_enable_laser_settings(True)
elif value == 1: # SICK LMS111
self.laser_range_changed(d.SICK_111_RANGE)
self.laser_min_bear_changed(d.SICK_111_MIN_ANGLE)
self.laser_max_bear_changed(d.SICK_111_MAX_ANGLE)
self.laser_res_changed(d.SICK_111_RES)
self.laser_freq_changed(d.SICK_111_FREQ)
self.laser_noise_changed(d.SICK_111_NOISE)
self.toggle_enable_laser_settings(False)
elif value == 2: # Hokuyo URG-04LX
self.laser_range_changed(d.HOK_04_RANGE)
self.laser_min_bear_changed(d.HOK_04_MIN_ANGLE)
self.laser_max_bear_changed(d.HOK_04_MAX_ANGLE)
self.laser_res_changed(d.HOK_04_RES)
self.laser_freq_changed(d.HOK_04_FREQ)
self.laser_noise_changed(d.HOK_04_NOISE)
self.toggle_enable_laser_settings(False)
def laser_freq_changed(self, value):
self.settings.laser_freq_slider.setValue(value)
self.settings.laser_freq_box.setValue(value)
self.robot.laser.freq = value
self.main.graphics_view.set_timer_frequencies()
def laser_max_bear_changed(self, value):
self.settings.laser_max_bear_box.setValue(value)
self.settings.laser_max_bear_slider.setValue(value)
min_value = self.settings.laser_min_bear_slider.value()
if value > min_value:
self.robot.laser.max_angle = value
else:
self.robot.laser.min_angle = value
def laser_min_bear_changed(self, value):
self.settings.laser_min_bear_box.setValue(value)
self.settings.laser_min_bear_slider.setValue(value)
max_value = self.settings.laser_max_bear_slider.value()
if value < max_value:
self.robot.laser.min_angle = value
else:
self.robot.laser.max_angle = value
def laser_noise_changed(self, value):
self.settings.laser_noise_slider.setValue(int(100 * value))
self.settings.laser_noise_box.setValue(value)
self.robot.laser.noise = value
rospy.set_param('~laser_noise', value)
def laser_range_changed(self, value):
self.settings.laser_range_box.setValue(value)
self.settings.laser_range_slider.setValue(value)
self.robot.laser.range = value
def laser_res_changed(self, value):
self.settings.laser_res_slider.setValue(int(10 * value))
self.settings.laser_res_box.setValue(value)
self.robot.laser.resolution = value
# -------
# MAPPING
# -------
def clear_map(self):
self.main.graphics_view.delete_map()
def load_map(self):
self.settings.map_check.setCheckState(QtCore.Qt.Checked)
filename, _ = QtGui.QFileDialog.getOpenFileName(
self, 'Open map file', os.getenv('HOME'))
if filename:
self.main.graphics_view.draw_map_from_file(filename)
# -------
# SETTINGS
# -------
def open_settings_dialog(self):
self.dialog.exec_()
# -------
# ROS
# -------
def initialize_parameters(self):
rospy.set_param('~robot_wheel_radius', self.robot.wheel_rad)
rospy.set_param('~robot_track', self.robot.wheelbase)
rospy.set_param('~laser_noise', self.robot.laser.noise)
rospy.set_param('~encoder_resolution', self.robot.odometer.res)
rospy.set_param('~encoder_noise', self.robot.odometer.noise)
rospy.set_param('~gps_noise', self.robot.gps.noise)
rospy.set_param('~gyro_noise', self.robot.gyroscope.noise)
rospy.set_param('~compass_noise', self.robot.compass.noise)
# -----
# OTHER
# -----
def ang_vel_inc_changed(self, value):
self.settings.ang_vel_inc_box.setValue(value)
self.ang_vel_inc = value
def keyPressEvent(self, event):
"""Adjusts the translational and angular velocites of the robot. Does
not allow the velocities to go beyond the maximum and minimums. Sets
the velocities to zero at zero-crossings (sign changes)."""
vel_inc = self.vel_inc
ang_vel_inc = self.ang_vel_inc * math.pi / 180
# W KEY PRESSED: INCREASE VELOCITY
if event.key() == QtCore.Qt.Key_W:
# Go to zero on sign changes
if self.robot.vel < 0 and self.robot.vel + vel_inc > 0:
self.robot.vel = 0
# Change only if limit is not exceeded
elif self.robot.vel < self.robot.max_vel:
# Go to limit if increment would go beyond it
if self.robot.vel + vel_inc > self.robot.max_vel:
self.robot.vel = self.robot.max_vel
else:
self.robot.vel += vel_inc
# S KEY PRESSED: DECREASE VELOCITY
elif event.key() == QtCore.Qt.Key_S:
# Go to zero on sign changes
if self.robot.vel > 0 and self.robot.vel - vel_inc < 0:
self.robot.vel = 0
# Change only if limit is not exceeded
elif self.robot.vel > -self.robot.max_vel:
# Go to limit if increment would go beyond it
if self.robot.vel - vel_inc < -self.robot.max_vel:
self.robot.vel = -self.robot.max_vel
else:
self.robot.vel -= vel_inc
# A KEY PRESSED: INCREASE ANGULAR VELOCITY
elif event.key() == QtCore.Qt.Key_A:
# Go to zero on sign changes
if self.robot.ang_vel < 0 and self.robot.ang_vel + ang_vel_inc > 0:
self.robot.ang_vel = 0
# Change only if limit is not exceeded
elif self.robot.ang_vel < self.robot.max_ang_vel:
# Go to limit if increment would go beyond it
if (self.robot.ang_vel + ang_vel_inc > self.robot.max_ang_vel):
self.robot.ang_vel = self.robot.max_ang_vel
else:
self.robot.ang_vel += ang_vel_inc
# D KEY PRESSED: DECREASE ANGULAR VELOCITY
elif event.key() == QtCore.Qt.Key_D:
# Go to zero on sign changes
if self.robot.ang_vel > 0 and self.robot.ang_vel - ang_vel_inc < 0:
self.robot.ang_vel = 0
# Change only if limit is not exceeded
elif self.robot.ang_vel > -self.robot.max_ang_vel:
# Go to limit if increment would go beyond it
if (self.robot.ang_vel - ang_vel_inc <
-self.robot.max_ang_vel):
self.robot.ang_vel = -self.robot.max_ang_vel
else:
self.robot.ang_vel -= ang_vel_inc
# SPACE BAR PRESSED: STOP
elif event.key() == QtCore.Qt.Key_Space:
self.robot.vel = 0
self.robot.ang_vel = 0
def laser_check_changed(self, value):
self.main.graphics_view.show_beams = value
def map_check_changed(self, value):
self.main.graphics_view.toggle_map(value)
def vel_inc_changed(self, value):
self.settings.vel_inc_box.setValue(value)
self.vel_inc = value
# --------------------------------------------------------------------------
# UPDATE/CHANGE GUI ELEMENTS
# --------------------------------------------------------------------------
def toggle_enable_laser_settings(self, b):
self.settings.laser_range_slider.setEnabled(b)
self.settings.laser_range_box.setEnabled(b)
self.settings.laser_min_bear_slider.setEnabled(b)
self.settings.laser_max_bear_slider.setEnabled(b)
self.settings.laser_min_bear_box.setEnabled(b)
self.settings.laser_max_bear_box.setEnabled(b)
self.settings.laser_res_slider.setEnabled(b)
self.settings.laser_res_box.setEnabled(b)
self.settings.laser_freq_slider.setEnabled(b)
self.settings.laser_freq_box.setEnabled(b)
self.settings.laser_noise_slider.setEnabled(b)
self.settings.laser_noise_box.setEnabled(b)
def toggle_enable_robot_settings(self, b):
self.settings.robot_length_slider.setEnabled(b)
self.settings.robot_length_box.setEnabled(b)
self.settings.robot_width_slider.setEnabled(b)
self.settings.robot_width_box.setEnabled(b)
self.settings.robot_wheel_rad_slider.setEnabled(b)
self.settings.robot_wheel_rad_box.setEnabled(b)
self.settings.robot_wheelbase_slider.setEnabled(b)
self.settings.robot_wheelbase_box.setEnabled(b)
self.settings.robot_vel_slider.setEnabled(b)
self.settings.robot_vel_box.setEnabled(b)
self.settings.robot_ang_vel_slider.setEnabled(b)
self.settings.robot_ang_vel_box.setEnabled(b)
def update_info_labels(self):
self.main.pose_label.setText(
"%0.2f m, %0.2f m, %d deg" %
(self.robot.x, self.robot.y, 180 / math.pi * self.robot.heading))
self.main.velocity_label.setText("%0.2f m/s" % self.robot.vel)
self.main.ang_vel_label.setText(
"%d deg/s" % int(180 / math.pi * self.robot.ang_vel))
# --------------------------------------------------------------------------
# UTILITY METHODS
# --------------------------------------------------------------------------
def settings_to_default(self):
# -----
# ROBOT
# -----
self.robot_length_changed(d.ROBOT_LENGTH)
self.robot_width_changed(d.ROBOT_WIDTH)
self.robot_wheel_rad_changed(d.ROBOT_WHEEL_RAD)
self.robot_wheelbase_changed(d.ROBOT_WHEELBASE)
self.robot_vel_changed(d.ROBOT_MAX_VEL)
self.robot_ang_vel_changed(d.ROBOT_MAX_ANG_VEL)
# --------
# COMPASS
# --------
self.compass_freq_changed(d.COMPASS_FREQUENCY)
self.compass_noise_changed(d.COMPASS_NOISE)
# --------
# GPS
# --------
self.gps_freq_changed(d.GPS_FREQUENCY)
self.gps_noise_changed(d.GPS_NOISE)
# --------
# GYRO
# --------
self.gyro_freq_changed(d.GYRO_FREQUENCY)
self.gyro_noise_changed(d.GYRO_NOISE)
# --------
# ODOMETER
# --------
self.odom_res_changed(d.ODOM_RES)
self.odom_freq_changed(d.ODOM_FREQ)
self.odom_noise_changed(d.ODOM_NOISE)
# -----
# LASER
# -----
self.laser_range_changed(d.LASER_RANGE)
self.laser_min_bear_changed(d.LASER_MIN_ANGLE)
self.laser_max_bear_changed(d.LASER_MAX_ANGLE)
self.laser_res_changed(d.LASER_RES)
self.laser_freq_changed(d.LASER_FREQ)
self.laser_noise_changed(d.LASER_NOISE)
# -----
# OTHER
# -----
self.vel_inc_changed(d.VELOCITY_INCREMENT)
self.ang_vel_inc_changed(d.ANG_VELOCITY_INCREMENT)
def zoom_in(self):
self.main.graphics_view.set_scale(1.1)
def zoom_out(self):
self.main.graphics_view.set_scale(0.9)
class MainWindow(QtGui.QMainWindow):
"""The main window. This window displays all the widgets."""
def __init__(self):
super(MainWindow, self).__init__()
self.robot = mod.Robot()
self.loadGUI()
# Place and scale the logo
pkg_dir = rospkg.RosPack().get_path('msl_sim')
pixmap = QtGui.QPixmap(os.path.join(pkg_dir, 'src', 'img', 'msl_logo.png'))
self.main.logo_label.setPixmap(pixmap)
self.main.logo_label.setAlignment(QtCore.Qt.AlignCenter)
# Set initial scale of main plot
self.main.graphics_view.set_scale(0.6)
# Give the zoomed in plotting area the same scene as the zoomed out one
self.main.graphics_view_zoom.setScene(self.main.graphics_view.scene())
# Initialize the camera in the zoomed in plotting window
self.main.graphics_view_zoom.initializeCamera(self.robot)
# Give the zoomed-out plotting area a copy of the zoomed-in plotting
# area so it can change it based on its timers
self.main.graphics_view.zoom = self.main.graphics_view_zoom
# Give the plotting area the same robot as the rest of the GUI and start
# updating it via timers
self.main.graphics_view.robot = self.robot
self.main.graphics_view.initialiseRobot()
self.settings_to_default()
# Start a timer that updates the labels
self.label_timer = QtCore.QTimer()
self.label_timer.timeout.connect(self.update_info_labels)
self.label_timer.start()
# Start timers that update the plot and the model
self.main.graphics_view.start_timers()
# Initialize the ROS parameters
self.initialize_parameters()
# --------------------------------------------------------------------------
# SETUP METHODS
# --------------------------------------------------------------------------
def connect_signals_to_slots(self):
# -----
# ROBOT
# -----
self.settings.robot_ang_vel_box.valueChanged.connect(
self.robot_ang_vel_changed)
self.settings.robot_ang_vel_slider.valueChanged.connect(
self.robot_ang_vel_changed)
self.settings.robot_combo.currentIndexChanged.connect(
self.robot_combo_changed)
self.settings.robot_length_box.valueChanged.connect(
self.robot_length_changed)
self.settings.robot_length_slider.valueChanged.connect(
lambda: self.robot_length_changed(
self.settings.robot_length_slider.value()/10.0))
self.settings.robot_wheel_rad_box.valueChanged.connect(
self.robot_wheel_rad_changed)
self.settings.robot_wheel_rad_slider.valueChanged.connect(
lambda: self.robot_wheel_rad_changed(
self.settings.robot_wheel_rad_slider.value()/100.0))
self.settings.robot_wheelbase_box.valueChanged.connect(
self.robot_wheelbase_changed)
self.settings.robot_wheelbase_slider.valueChanged.connect(
lambda: self.robot_wheelbase_changed(
self.settings.robot_wheelbase_slider.value()/10.0))
self.settings.robot_width_box.valueChanged.connect(
self.robot_width_changed)
self.settings.robot_width_slider.valueChanged.connect(
lambda: self.robot_width_changed(
self.settings.robot_width_slider.value()/10.0))
self.settings.robot_vel_box.valueChanged.connect(self.robot_vel_changed)
self.settings.robot_vel_slider.valueChanged.connect(
lambda: self.robot_vel_changed(
self.settings.robot_vel_slider.value()/10.0))
# --------
# COMPASS
# --------
self.settings.compass_freq_box.valueChanged.connect(self.compass_freq_changed)
self.settings.compass_freq_slider.valueChanged.connect(self.compass_freq_changed)
self.settings.compass_noise_box.valueChanged.connect(self.compass_noise_changed)
self.settings.compass_noise_slider.valueChanged.connect(self.compass_noise_changed)
# --------
# GPS
# --------
self.settings.gps_freq_box.valueChanged.connect(self.gps_freq_changed)
self.settings.gps_freq_slider.valueChanged.connect(self.gps_freq_changed)
self.settings.gps_noise_box.valueChanged.connect(self.gps_noise_changed)
self.settings.gps_noise_slider.valueChanged.connect(
lambda: self.gps_noise_changed(
self.settings.gps_noise_slider.value()/100.0))
# --------
# GYROSCOPE
# --------
self.settings.gyro_freq_box.valueChanged.connect(self.gyro_freq_changed)
self.settings.gyro_freq_slider.valueChanged.connect(self.gyro_freq_changed)
self.settings.gyro_noise_box.valueChanged.connect(self.gyro_noise_changed)
self.settings.gyro_noise_slider.valueChanged.connect(
lambda: self.gyro_noise_changed(
self.settings.gyro_noise_slider.value()/100.0))
# --------
# ODOMETER
# --------
self.settings.odom_freq_box.valueChanged.connect(self.odom_freq_changed)
self.settings.odom_freq_slider.valueChanged.connect(self.odom_freq_changed)
self.settings.odom_noise_box.valueChanged.connect(self.odom_noise_changed)
self.settings.odom_noise_slider.valueChanged.connect(
lambda: self.odom_noise_changed(
self.settings.odom_noise_slider.value()/10.0))
self.settings.odom_res_box.valueChanged.connect(self.odom_res_changed)
self.settings.odom_res_slider.valueChanged.connect(
lambda: self.odom_res_changed(
self.settings.odom_res_slider.value()/100.0))
# -----
# LASER
# -----
self.settings.laser_combo.currentIndexChanged.connect(self.laser_combo_changed)
self.settings.laser_freq_box.valueChanged.connect(
self.laser_freq_changed)
self.settings.laser_freq_slider.valueChanged.connect(
self.laser_freq_changed)
self.settings.laser_max_bear_box.valueChanged.connect(
self.laser_max_bear_changed)
self.settings.laser_max_bear_slider.valueChanged.connect(
self.laser_max_bear_changed)
self.settings.laser_min_bear_box.valueChanged.connect(
self.laser_min_bear_changed)
self.settings.laser_min_bear_slider.valueChanged.connect(
self.laser_min_bear_changed)
self.settings.laser_noise_box.valueChanged.connect(
self.laser_noise_changed)
self.settings.laser_noise_slider.valueChanged.connect(
lambda: self.laser_noise_changed(
self.settings.laser_noise_slider.value()/100.0))
self.settings.laser_range_box.valueChanged.connect(
self.laser_range_changed)
self.settings.laser_range_slider.valueChanged.connect(
self.laser_range_changed)
self.settings.laser_res_box.valueChanged.connect(
self.laser_res_changed)
self.settings.laser_res_slider.valueChanged.connect(
lambda: self.laser_res_changed(
self.settings.laser_res_slider.value()/10.0))
# -----
# MAPPING
# -----
self.main.load_map_button.clicked.connect(self.load_map)
# -----
# SETTINGS
# -----
self.main.settings_button.clicked.connect(self.open_settings_dialog)
# -----
# OTHER
# -----
self.settings.ang_vel_inc_box.valueChanged.connect(self.ang_vel_inc_changed)
self.settings.laser_check.stateChanged.connect(self.laser_check_changed)
self.settings.map_check.stateChanged.connect(self.map_check_changed)
self.settings.vel_inc_box.valueChanged.connect(self.vel_inc_changed)
self.main.zoom_in_button.clicked.connect(self.zoom_in)
self.main.zoom_out_button.clicked.connect(self.zoom_out)
def loadGUI(self):
"""Load the GUI from the .py file that was generated by the .ui file
using the pyside-uic tool."""
# import generated .py file here to prevent circular reference
from sim.main_window import Ui_main_window
self.main = Ui_main_window()
self.main.setupUi(self)
from sim.settings_dialog import Ui_settings_dialog
self.settings = Ui_settings_dialog()
self.dialog = QtGui.QDialog()
self.settings.setupUi(self.dialog)
self.connect_signals_to_slots()
# Add items to pull down menus
self.settings.laser_combo.addItems(["Custom", "SICK LMS111",
"Hokuyo URG-04LX"])
self.settings.robot_combo.addItems(["Custom", "Clearpath Husky A200",
"MobileRobots P3AT"])
# --------------------------------------------------------------------------
# SLOTS
# --------------------------------------------------------------------------
# -----
# ROBOT
# -----
def robot_ang_vel_changed(self, value):
self.settings.robot_ang_vel_slider.setValue(value)
self.settings.robot_ang_vel_box.setValue(value)
self.robot.max_ang_vel = value * math.pi/180
def robot_combo_changed(self, value):
if value == 0: # Custom
self.toggle_enable_robot_settings(True)
elif value == 1: # Clearpath Husky A200
self.robot_length_changed(d.HUSKY_LENGTH)
self.robot_width_changed(d.HUSKY_WIDTH)
self.robot_wheel_rad_changed(d.HUSKY_WHEEL_RAD)
self.robot_wheelbase_changed(d.HUSKY_WHEELBASE)
self.robot_vel_changed(d.HUSKY_MAX_VEL)
self.robot_ang_vel_changed(d.HUSKY_MAX_ANG_VEL)
self.toggle_enable_robot_settings(False)
elif value == 2: # MobileRobots P3AT
self.robot_length_changed(d.P3AT_LENGTH)
self.robot_width_changed(d.P3AT_WIDTH)
self.robot_wheel_rad_changed(d.P3AT_WHEEL_RAD)
self.robot_wheelbase_changed(d.P3AT_WHEELBASE)
self.robot_vel_changed(d.P3AT_MAX_VEL)
self.robot_ang_vel_changed(d.P3AT_MAX_ANG_VEL)
self.toggle_enable_robot_settings(False)
def robot_length_changed(self, value):
self.settings.robot_length_slider.setValue(int(10*value))
self.settings.robot_length_box.setValue(value)
self.robot.set_length(value)
def robot_wheel_rad_changed(self, value):
self.settings.robot_wheel_rad_slider.setValue(int(100*value))
self.settings.robot_wheel_rad_box.setValue(value)
self.robot.wheel_rad = value
rospy.set_param('~robot_wheel_radius', value)
def robot_wheelbase_changed(self, value):
self.settings.robot_wheelbase_slider.setValue(int(10*value))
self.settings.robot_wheelbase_box.setValue(value)
self.robot.wheelbase = value
rospy.set_param('~robot_track', value)
def robot_width_changed(self, value):
self.settings.robot_width_slider.setValue(int(10*value))
self.settings.robot_width_box.setValue(value)
self.robot.set_width(value)
def robot_vel_changed(self, value):
self.settings.robot_vel_slider.setValue(int(10*value))
self.settings.robot_vel_box.setValue(value)
self.robot.max_vel = value
# --------
# COMPASS
# --------
def compass_freq_changed(self, value):
self.settings.compass_freq_slider.setValue(value)
self.settings.compass_freq_box.setValue(value)
self.robot.compass.freq = value
self.main.graphics_view.set_timer_frequencies()
def compass_noise_changed(self, value):
self.settings.compass_noise_slider.setValue(value)
self.settings.compass_noise_box.setValue(value)
self.robot.compass.noise = math.radians(value)
rospy.set_param('~compass_noise', math.radians(value))
# --------
# GPS
# --------
def gps_freq_changed(self, value):
self.settings.gps_freq_slider.setValue(value)
self.settings.gps_freq_box.setValue(value)
self.robot.gps.freq = value
self.main.graphics_view.set_timer_frequencies()
def gps_noise_changed(self, value):
self.settings.gps_noise_slider.blockSignals(True)
self.settings.gps_noise_slider.setValue(int(100*value))
self.settings.gps_noise_slider.blockSignals(False)
self.settings.gps_noise_box.setValue(value)
self.robot.gps.noise = value
rospy.set_param('~gps_noise', value)
# --------
# GYRO
# --------
def gyro_freq_changed(self, value):
self.settings.gyro_freq_slider.setValue(value)
self.settings.gyro_freq_box.setValue(value)
self.robot.gyroscope.freq = value
self.main.graphics_view.set_timer_frequencies()
def gyro_noise_changed(self, value):
self.settings.gyro_noise_slider.blockSignals(True)
self.settings.gyro_noise_slider.setValue(int(100*value))
self.settings.gyro_noise_slider.blockSignals(False)
self.settings.gyro_noise_box.setValue(value)
self.robot.gyroscope.noise = math.radians(value)
rospy.set_param('~gyro_noise', math.radians(value))
# --------
# ODOMETER
# --------
def odom_freq_changed(self, value):
self.settings.odom_freq_slider.setValue(value)
self.settings.odom_freq_box.setValue(value)
self.robot.odometer.freq = value
self.main.graphics_view.set_timer_frequencies()
def odom_noise_changed(self, value):
self.settings.odom_noise_slider.setValue(int(10*value))
self.settings.odom_noise_box.setValue(value)
self.robot.odometer.noise = value
rospy.set_param('~encoder_noise', value)
def odom_res_changed(self, value):
self.settings.odom_res_slider.blockSignals(True)
self.settings.odom_res_slider.setValue(int(100*value))
self.settings.odom_res_slider.blockSignals(False)
self.settings.odom_res_box.setValue(value)
self.robot.odometer.res = value
rospy.set_param('~encoder_resolution', value)
# -----
# LASER
# -----
def laser_combo_changed(self, value):
if value == 0: # Custom
self.toggle_enable_laser_settings(True)
elif value == 1: # SICK LMS111
self.laser_range_changed(d.SICK_111_RANGE)
self.laser_min_bear_changed(d.SICK_111_MIN_ANGLE)
self.laser_max_bear_changed(d.SICK_111_MAX_ANGLE)
self.laser_res_changed(d.SICK_111_RES)
self.laser_freq_changed(d.SICK_111_FREQ)
self.laser_noise_changed(d.SICK_111_NOISE)
self.toggle_enable_laser_settings(False)
elif value == 2: # Hokuyo URG-04LX
self.laser_range_changed(d.HOK_04_RANGE)
self.laser_min_bear_changed(d.HOK_04_MIN_ANGLE)
self.laser_max_bear_changed(d.HOK_04_MAX_ANGLE)
self.laser_res_changed(d.HOK_04_RES)
self.laser_freq_changed(d.HOK_04_FREQ)
self.laser_noise_changed(d.HOK_04_NOISE)
self.toggle_enable_laser_settings(False)
def laser_freq_changed(self, value):
self.settings.laser_freq_slider.setValue(value)
self.settings.laser_freq_box.setValue(value)
self.robot.laser.freq = value
self.main.graphics_view.set_timer_frequencies()
def laser_max_bear_changed(self, value):
self.settings.laser_max_bear_box.setValue(value)
self.settings.laser_max_bear_slider.setValue(value)
min_value = self.settings.laser_min_bear_slider.value()
if value > min_value:
self.robot.laser.max_angle = value
else:
self.robot.laser.min_angle = value
def laser_min_bear_changed(self, value):
self.settings.laser_min_bear_box.setValue(value)
self.settings.laser_min_bear_slider.setValue(value)
max_value = self.settings.laser_max_bear_slider.value()
if value < max_value:
self.robot.laser.min_angle = value
else:
self.robot.laser.max_angle = value
def laser_noise_changed(self, value):
self.settings.laser_noise_slider.setValue(int(100*value))
self.settings.laser_noise_box.setValue(value)
self.robot.laser.noise = value
rospy.set_param('~laser_noise', value)
def laser_range_changed(self, value):
self.settings.laser_range_box.setValue(value)
self.settings.laser_range_slider.setValue(value)
self.robot.laser.range = value
def laser_res_changed(self, value):
self.settings.laser_res_slider.setValue(int(10*value))
self.settings.laser_res_box.setValue(value)
self.robot.laser.resolution = value
# -------
# MAPPING
# -------
def clear_map(self):
self.main.graphics_view.delete_map()
def load_map(self):
self.settings.map_check.setCheckState(QtCore.Qt.Checked)
filename, _ = QtGui.QFileDialog.getOpenFileName(self, 'Open map file', os.getenv('HOME'))
if filename:
self.main.graphics_view.draw_map_from_file(filename)
# -------
# SETTINGS
# -------
def open_settings_dialog(self):
self.dialog.exec_()
# -------
# ROS
# -------
def initialize_parameters(self):
rospy.set_param('~robot_wheel_radius', self.robot.wheel_rad)
rospy.set_param('~robot_track', self.robot.wheelbase)
rospy.set_param('~laser_noise', self.robot.laser.noise)
rospy.set_param('~encoder_resolution', self.robot.odometer.res)
rospy.set_param('~encoder_noise', self.robot.odometer.noise)
rospy.set_param('~gps_noise', self.robot.gps.noise)
rospy.set_param('~gyro_noise', self.robot.gyroscope.noise)
rospy.set_param('~compass_noise', self.robot.compass.noise)
# -----
# OTHER
# -----
def ang_vel_inc_changed(self, value):
self.settings.ang_vel_inc_box.setValue(value)
self.ang_vel_inc = value
def keyPressEvent(self, event):
"""Adjusts the translational and angular velocites of the robot. Does
not allow the velocities to go beyond the maximum and minimums. Sets
the velocities to zero at zero-crossings (sign changes)."""
vel_inc = self.vel_inc
ang_vel_inc = self.ang_vel_inc * math.pi/180
# W KEY PRESSED: INCREASE VELOCITY
if event.key() == QtCore.Qt.Key_W:
# Go to zero on sign changes
if self.robot.vel < 0 and self.robot.vel + vel_inc > 0:
self.robot.vel = 0
# Change only if limit is not exceeded
elif self.robot.vel < self.robot.max_vel:
# Go to limit if increment would go beyond it
if self.robot.vel + vel_inc > self.robot.max_vel:
self.robot.vel = self.robot.max_vel
else:
self.robot.vel += vel_inc
# S KEY PRESSED: DECREASE VELOCITY
elif event.key() == QtCore.Qt.Key_S:
# Go to zero on sign changes
if self.robot.vel > 0 and self.robot.vel - vel_inc < 0:
self.robot.vel = 0
# Change only if limit is not exceeded
elif self.robot.vel > -self.robot.max_vel:
# Go to limit if increment would go beyond it
if self.robot.vel - vel_inc < -self.robot.max_vel:
self.robot.vel = -self.robot.max_vel
else:
self.robot.vel -= vel_inc
# A KEY PRESSED: INCREASE ANGULAR VELOCITY
elif event.key() == QtCore.Qt.Key_A:
# Go to zero on sign changes
if self.robot.ang_vel < 0 and self.robot.ang_vel + ang_vel_inc > 0:
self.robot.ang_vel = 0
# Change only if limit is not exceeded
elif self.robot.ang_vel < self.robot.max_ang_vel:
# Go to limit if increment would go beyond it
if (self.robot.ang_vel + ang_vel_inc > self.robot.max_ang_vel):
self.robot.ang_vel = self.robot.max_ang_vel
else:
self.robot.ang_vel += ang_vel_inc
# D KEY PRESSED: DECREASE ANGULAR VELOCITY
elif event.key() == QtCore.Qt.Key_D:
# Go to zero on sign changes
if self.robot.ang_vel > 0 and self.robot.ang_vel - ang_vel_inc < 0:
self.robot.ang_vel = 0
# Change only if limit is not exceeded
elif self.robot.ang_vel > -self.robot.max_ang_vel:
# Go to limit if increment would go beyond it
if (self.robot.ang_vel - ang_vel_inc < -self.robot.max_ang_vel):
self.robot.ang_vel = -self.robot.max_ang_vel
else:
self.robot.ang_vel -= ang_vel_inc
# SPACE BAR PRESSED: STOP
elif event.key() == QtCore.Qt.Key_Space:
self.robot.vel = 0
self.robot.ang_vel = 0
def laser_check_changed(self, value):
self.main.graphics_view.show_beams = value
def map_check_changed(self, value):
self.main.graphics_view.toggle_map(value)
def vel_inc_changed(self, value):
self.settings.vel_inc_box.setValue(value)
self.vel_inc = value
# --------------------------------------------------------------------------
# UPDATE/CHANGE GUI ELEMENTS
# --------------------------------------------------------------------------
def toggle_enable_laser_settings(self, b):
self.settings.laser_range_slider.setEnabled(b)
self.settings.laser_range_box.setEnabled(b)
self.settings.laser_min_bear_slider.setEnabled(b)
self.settings.laser_max_bear_slider.setEnabled(b)
self.settings.laser_min_bear_box.setEnabled(b)
self.settings.laser_max_bear_box.setEnabled(b)
self.settings.laser_res_slider.setEnabled(b)
self.settings.laser_res_box.setEnabled(b)
self.settings.laser_freq_slider.setEnabled(b)
self.settings.laser_freq_box.setEnabled(b)
self.settings.laser_noise_slider.setEnabled(b)
self.settings.laser_noise_box.setEnabled(b)
def toggle_enable_robot_settings(self, b):
self.settings.robot_length_slider.setEnabled(b)
self.settings.robot_length_box.setEnabled(b)
self.settings.robot_width_slider.setEnabled(b)
self.settings.robot_width_box.setEnabled(b)
self.settings.robot_wheel_rad_slider.setEnabled(b)
self.settings.robot_wheel_rad_box.setEnabled(b)
self.settings.robot_wheelbase_slider.setEnabled(b)
self.settings.robot_wheelbase_box.setEnabled(b)
self.settings.robot_vel_slider.setEnabled(b)
self.settings.robot_vel_box.setEnabled(b)
self.settings.robot_ang_vel_slider.setEnabled(b)
self.settings.robot_ang_vel_box.setEnabled(b)
def update_info_labels(self):
self.main.pose_label.setText("%0.2f m, %0.2f m, %d deg" % (self.robot.x,
self.robot.y, 180/math.pi*self.robot.heading))
self.main.velocity_label.setText("%0.2f m/s" % self.robot.vel)
self.main.ang_vel_label.setText("%d deg/s" % int(
180/math.pi*self.robot.ang_vel))
# --------------------------------------------------------------------------
# UTILITY METHODS
# --------------------------------------------------------------------------
def settings_to_default(self):
# -----
# ROBOT
# -----
self.robot_length_changed(d.ROBOT_LENGTH)
self.robot_width_changed(d.ROBOT_WIDTH)
self.robot_wheel_rad_changed(d.ROBOT_WHEEL_RAD)
self.robot_wheelbase_changed(d.ROBOT_WHEELBASE)
self.robot_vel_changed(d.ROBOT_MAX_VEL)
self.robot_ang_vel_changed(d.ROBOT_MAX_ANG_VEL)
# --------
# COMPASS
# --------
self.compass_freq_changed(d.COMPASS_FREQUENCY)
self.compass_noise_changed(d.COMPASS_NOISE)
# --------
# GPS
# --------
self.gps_freq_changed(d.GPS_FREQUENCY)
self.gps_noise_changed(d.GPS_NOISE)
# --------
# GYRO
# --------
self.gyro_freq_changed(d.GYRO_FREQUENCY)
self.gyro_noise_changed(d.GYRO_NOISE)
# --------
# ODOMETER
# --------
self.odom_res_changed(d.ODOM_RES)
self.odom_freq_changed(d.ODOM_FREQ)
self.odom_noise_changed(d.ODOM_NOISE)
# -----
# LASER
# -----
self.laser_range_changed(d.LASER_RANGE)
self.laser_min_bear_changed(d.LASER_MIN_ANGLE)
self.laser_max_bear_changed(d.LASER_MAX_ANGLE)
self.laser_res_changed(d.LASER_RES)
self.laser_freq_changed(d.LASER_FREQ)
self.laser_noise_changed(d.LASER_NOISE)
# -----
# OTHER
# -----
self.vel_inc_changed(d.VELOCITY_INCREMENT)
self.ang_vel_inc_changed(d.ANG_VELOCITY_INCREMENT)
def zoom_in(self):
self.main.graphics_view.set_scale(1.1)
def zoom_out(self):
self.main.graphics_view.set_scale(0.9)
