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)