class Layer: def __init__(self, name,parent_widget): self._name = name self._is_active = True self.menu_action = QAction(self._name, parent_widget, triggered=self.toggle_active, checkable=True, checked=True) def toggle_active(self): self._is_active = self._is_active != True def set_active(self,active): self._is_active = active def draw(self): if self._is_active: self._draw() # Overrided by chlidren class def _draw(self): None def save_settings(self, plugin_settings, instance_settings): instance_settings.set_value(self._name + '_is_active', str(self._is_active)) #view_matrix_string = repr(self._gl_view.get_view_matrix()) #instance_settings.set_value('view_matrix', view_matrix_string) def restore_settings(self, plugin_settings, instance_settings): is_active = instance_settings.value(self._name + '_is_active') if is_active is not None : self._is_active = is_active == 'True' self.menu_action.setChecked(self._is_active)
class NavigationMapWidget(QWidget): def __init__(self, plugin): super(NavigationMapWidget, self).__init__() rp = rospkg.RosPack() try: rospy.wait_for_service('/proc_control/set_global_target', timeout=2) except rospy.ROSException: False ui_file = os.path.join(rp.get_path('rqt_navigation_map'), 'resource', 'mainWidget.ui') loadUi(ui_file, self) self._plugin = plugin self._topic_name = None self._odom_subscriber = None # create GL view self._gl_view = GLWidget() self._gl_view.unsetCursor() self._mapDrawer = MapDrawer(self) self._position = (0.0, 0.0, 0.0) self._mapDrawer.set_position(self._position) self._orientation = quaternion_about_axis(45.0, (0.0, 0.0, 1.0)) self._mapDrawer.set_orientation(self._orientation,45) # add GL view to widget layout self.layout().addWidget(self._gl_view) self._rotate_with_sub_activated = False self._lock_on_sub_activated = False self._yaw = 0 self._odom_subscriber = rospy.Subscriber('/proc_navigation/odom', Odometry, self._odom_callback) self.position_target_subscriber = rospy.Subscriber('/proc_control/current_target', PositionTarget, self._position_target_callback) self.set_global_target = rospy.ServiceProxy('/proc_control/set_global_target', SetPositionTarget) self._define_menu() def _define_menu(self): self._menu = QMenu(self._gl_view) layer_menu = QMenu('Layers', self._gl_view) for layer in self._mapDrawer.get_layers(): layer_menu.addAction(layer.menu_action) self._menu.addMenu(layer_menu) self._menu.addSeparator() self._view2dAction = QAction(self._gl_view.tr("2D View"), self._gl_view, triggered=self._set_default_view) self._menu.addAction(self._view2dAction) self._view3dAction = QAction(self._gl_view.tr("3D View"), self._gl_view, triggered=self._set_3d_view) self._menu.addAction(self._view3dAction) self._menu.addSeparator() #self._rotateSubAction = QAction(self._gl_view.tr("Rotate with Sub"), self, checkable=True, # triggered=self._rotate_with_sub) #self._menu.addAction(self._rotateSubAction) self._lockOnSubAction = QAction(self._gl_view.tr("Lock on Sub"), self, checkable=True, triggered=self._lock_on_sub) self._menu.addAction(self._lockOnSubAction) self._menu.addSeparator() self._menu.addAction(QAction('Reset Path', self, triggered=self._mapDrawer.reset_path)) setLocationAction = QAction('Set Location Target', self._gl_view, triggered=self._set_location_action) self._menu.addAction(setLocationAction) def _position_target_callback(self, target): self._mapDrawer.drawTarget(target.X, target.Y, target.Z) def _odom_callback(self, odom_data): vehicle_position_x = odom_data.pose.pose.position.x vehicle_position_y = odom_data.pose.pose.position.y vehicle_position_z = odom_data.pose.pose.position.z self._position = (vehicle_position_x, vehicle_position_y, vehicle_position_z) self._mapDrawer.set_position(self._position) self._yaw = odom_data.pose.pose.orientation.z self._orientation = quaternion_about_axis(math.radians(self._yaw), (0.0, 0.0, 1.0)) self._mapDrawer.set_orientation(self._orientation,self._yaw) def save_settings(self, plugin_settings, instance_settings): self._mapDrawer.save_settings(plugin_settings,instance_settings) view_matrix_string = repr(self._gl_view.get_view_matrix()) instance_settings.set_value('view_matrix', view_matrix_string) instance_settings.set_value('lock_on_sub_activated', str(self._lock_on_sub_activated)) def restore_settings(self, plugin_settings, instance_settings): self._mapDrawer.restore_settings(plugin_settings,instance_settings) view_matrix_string = instance_settings.value('view_matrix') try: view_matrix = eval(view_matrix_string) except Exception: view_matrix = None if view_matrix is not None: self._gl_view.set_view_matrix(view_matrix) else: self._set_default_view() lock_on_sub = instance_settings.value('lock_on_sub_activated') == 'True' if lock_on_sub is None: print 'Nothing stored for lock_on_sub' else: self._lock_on_sub(lock_on_sub) self._lockOnSubAction.setChecked(lock_on_sub) #rotate_with_sub = instance_settings.value('rotate_with_sub_activated') == 'True' #if rotate_with_sub is None: # print 'Nothing stored for lock_on_sub' #else: # self._rotate_with_sub(rotate_with_sub) # self._rotateSubAction.setChecked(rotate_with_sub) def _set_default_view(self): self._gl_view.makeCurrent() self._gl_view.reset_view() self._gl_view.translate((0, 0, -800)) def _set_3d_view(self): self._gl_view.makeCurrent() self._gl_view.reset_view() self._gl_view.rotate((0, 0, 1), 0) self._gl_view.rotate((1, 0, 0), -75) self._gl_view.translate((-100, -100, -500)) def _rotate_with_sub(self, checked): self._rotate_with_sub_activated = checked self._mapDrawer.set_rotate_with_sub_activated(checked) def _lock_on_sub(self, checked): self._lock_on_sub_activated = checked self._mapDrawer.set_lock_on_sub_activated(checked) self._view2dAction.setEnabled(not checked) self._view3dAction.setEnabled(not checked) def _gl_view_mouseReleaseEvent(self, event): if event.button() == Qt.RightButton: self._event_3dPoint = event self._menu.exec_(self._gl_view.mapToGlobal(event.pos())) def _set_location_action(self): x_cursor = self._event_3dPoint.pos().x() y_cursor = self._gl_view.height() - self._event_3dPoint.pos().y() x, y, z = self._gl_view.unproject_mouse_on_scene(QPoint(x_cursor, y_cursor)) position_x = x / self._mapDrawer.resolution_meters position_y = y / self._mapDrawer.resolution_meters position_z = z / self._mapDrawer.resolution_meters self._mapDrawer.drawTarget(position_x, position_y, position_z) rospy.loginfo('Set Target selected at (%.2f, %.2f)', position_x, position_y) self.set_global_target(X=position_x, Y=position_y, Z=position_z, ROLL=0, PITCH=0, YAW=self._yaw) def shutdown_plugin(self): print 'Shutting down'