Load whole plugin from lab15-19

CMakeLists.txt

@@ -0,0 +1,201 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(safety_master_plugin)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  rospy
+  rqt_gui
+  rqt_gui_py
+  std_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES safety_master_plugin
+#  CATKIN_DEPENDS rospy std_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/safety_master_plugin.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/safety_master_plugin_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+# add_dependencies(safety_user_plugin ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+install(PROGRAMS
+#   scripts/my_python_script
+	DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_safety_master_plugin.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

launch/master_plugin.launch

@@ -0,0 +1,7 @@
+<launch>
+  <node pkg="rqt_gui"
+        type="rqt_gui"
+        name="$(anon master_plugin)"
+        args="--standalone safety_master_plugin"
+  />
+</launch>

package.xml

@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>safety_master_plugin</name>
+  <version>0.0.0</version>
+  <description>The safety_master_plugin package</description>
+
+  <maintainer email="aleksander.bojda@pwr.edu.pl">Aleksander Bojda</maintainer>
+
+  <license>MIT</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>rospy</build_depend>
+  <build_depend>rqt_gui</build_depend>
+  <build_depend>rqt_gui_py</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>rosaria_msgs</build_depend>
+
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>rqt_gui</build_export_depend>
+  <build_export_depend>rqt_gui_py</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>rosaria_msgs</build_export_depend>
+
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>rqt_gui</exec_depend>
+  <exec_depend>rqt_gui_py</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+  <exec_depend>rosaria_msgs</exec_depend>
+
+  <export>
+    <rqt_gui plugin="${prefix}/plugin.xml"/>
+  </export>
+</package>

plugin.xml

@@ -0,0 +1,15 @@
+<library path="scripts">
+  <class name="safety_master_plugin" type="master_plugin.MasterPlugin" base_class_type="rqt_gui_py::Plugin">
+    <description>
+      Safety system master plugin
+    </description>
+    <qtgui>
+      <group>
+        <label>Safety system</label>
+        <icon type="theme">folder</icon>
+      </group>
+      <label>Master</label>
+      <icon type="theme">system-help</icon>
+    </qtgui>
+  </class>
+</library>

scripts/enums/clutch_state.py

@@ -0,0 +1,6 @@
+
+
+class ClutchState:
+	ENGAGED = 4
+	DISENGAGED = 5
+	UNKNOWN = 6

scripts/enums/stop_state.py

@@ -0,0 +1,6 @@
+
+
+class StopState:
+	STOPPED = 1
+	RUNNING = 2
+	UNKNOWN = 3

scripts/master_callback_handler.py

@@ -0,0 +1,21 @@
+from python_qt_binding.QtCore import QObject
+from python_qt_binding.QtCore import pyqtSignal
+from robot_info import RobotInfo
+
+class CallbackHandler(QObject):
+    robot_info_signal = pyqtSignal(object)
+    add_robot_signal = pyqtSignal(int)
+    remove_robot_signal = pyqtSignal(int)
+    
+
+    def __init__(self):
+        super(CallbackHandler, self).__init__()    
+
+    def connect_robot_info_signal(self,slot):
+        self.robot_info_signal.connect(slot)
+
+    def connect_add_robot_signal(self,slot):
+        self.add_robot_signal.connect(slot)
+
+    def connect_remove_robot_signal(self,slot):
+        self.remove_robot_signal.connect(slot)

scripts/master_info.py

@@ -0,0 +1,16 @@
+#!/usr/bin/env python
+
+# from robot_info import RobotInfo
+
+class MasterInfo:
+
+    def __init__(self):
+        self.robots_id_list = []
+        self.master_stop_state = None
+        self.restrictions = None
+
+    def get_master_stop_state(self):
+        return self.master_stop_state
+
+    def get_restrictions(self):
+        return self.restrictions

scripts/master_plugin.py

@@ -0,0 +1,110 @@
+from ros_master_wrapper import ROSWrapper
+from qt_master_wrapper import QtWrapper
+from master_restrictions import Restrictions
+
+from master_info import MasterInfo
+from master_widget import MasterWidget
+
+from qt_gui.plugin import Plugin
+
+from enums.stop_state import StopState as SS
+
+from master_callback_handler import CallbackHandler
+
+class MasterPlugin(Plugin):
+
+    def __init__(self,context):
+        super(MasterPlugin, self).__init__(context)
+        self.setObjectName('Master Plugin - L1.5 safety system')
+
+        self._widget = MasterWidget(context)
+        self._ros_wrapper = ROSWrapper()
+        self._qt_wrapper = QtWrapper(self._widget)
+        self._master_info = MasterInfo()
+
+        self.cbhandler = CallbackHandler()
+        self.cbhandler.connect_robot_info_signal(self._qt_wrapper.update_robot_info)
+        self.cbhandler.connect_add_robot_signal(self._qt_wrapper.add_robot)
+        self.cbhandler.connect_remove_robot_signal(self._qt_wrapper.remove_robot)
+
+        
+        # self._qt_wrapper.connect_signals()
+        # self._qt_wrapper.set_sliders_to_initial_values()
+
+        
+        # Setup functions to get _master_info states from _ros_wrapper
+        self._ros_wrapper.setup_get_master_stop_state_function(self._master_info.get_master_stop_state)
+        self._ros_wrapper.setup_get_restrictions_function(self._master_info.get_restrictions)
+
+        self.set_callback_functions()
+        self._qt_wrapper.connect_signals()
+
+        # Stopping master at the beginning
+        self.master_stopped()
+
+    def set_callback_functions(self):
+        self.set_ROS_callback_functions()
+        self.set_Qt_callback_functions()
+
+    def set_ROS_callback_functions(self):
+        self._ros_wrapper.set_robots_list_update_callback(self.handle_robots_list_update)
+        self._ros_wrapper.set_robot_info_update_callback(self.handle_robot_info_update)
+
+    def set_Qt_callback_functions(self):
+        self._qt_wrapper.set_master_stop_state_updated_callback(self.handle_master_stop_state_updated)
+        self._qt_wrapper.set_restrictions_updated_callback(self.handle_restrictions_updated)
+        
+  
+    def handle_robots_list_update(self,robots_id_list):
+        self.update_robots_list(robots_id_list)
+
+    def handle_robot_info_update(self,robot_info):
+        self.update_robot_info(robot_info)
+
+    def handle_master_stop_state_updated(self):
+        if self._master_info.master_stop_state == SS.RUNNING:
+            self.master_stopped()
+        elif self._master_info.master_stop_state == SS.STOPPED:
+             self.master_started()          
+        else:
+            raise ValueError('master_stop_state value undefined') 
+
+    def handle_restrictions_updated(self,restrictions):
+        self.restrictions_changed(restrictions)
+    
+    def update_robots_list(self,robots_id_list):
+
+        for robot_id in robots_id_list:
+            if robot_id not in self._master_info.robots_id_list:
+                self.new_robot_registered(robot_id)
+
+        for robot_id in self._master_info.robots_id_list:
+            if robot_id not in robots_id_list:
+                self.robot_unregistered(robot_id)
+
+        self._master_info.robots_id_list = robots_id_list
+
+    def new_robot_registered(self,robot_id):
+        self._ros_wrapper.add_robot_subscriber(robot_id)
+        self.cbhandler.add_robot_signal.emit(robot_id)
+
+    def robot_unregistered(self,robot_id):
+        self._ros_wrapper.remove_robot_subscriber(robot_id)
+        self.cbhandler.remove_robot_signal.emit(robot_id)
+
+    def update_robot_info(self,robot_info):
+        self.cbhandler.robot_info_signal.emit(robot_info)
+
+    def master_started(self):
+        self._master_info.master_stop_state = SS.RUNNING
+        self._ros_wrapper.master_started()
+        self._qt_wrapper.master_started()
+
+    def master_stopped(self):
+        self._master_info.master_stop_state = SS.STOPPED
+        self._ros_wrapper.master_stopped()
+        self._qt_wrapper.master_stopped()
+
+    def restrictions_changed(self,restrictions):
+        self._master_info.restrictions = restrictions
+        self._ros_wrapper.restrictions_changed(restrictions)

scripts/master_restrictions.py

@@ -0,0 +1,13 @@
+#!/usr/bin/env python
+
+class Restrictions:
+
+    def __init__(self,distance,angular_velocity,linear_velocity):
+        self.distance = distance
+        self.angular_velocity = angular_velocity
+        self.linear_velocity = linear_velocity
+
+    def update_restrictions(self,restrictions_msg):
+        self.distance = restrictions_msg.distance.data
+        self.linear_velocity = restrictions_msg.linear_velocity.data
+        self.angular_velocity = restrictions_msg.angular_velocity.data

scripts/master_widget.py

@@ -0,0 +1,18 @@
+#!/usr/bin/env python
+import os
+import rospkg
+
+from python_qt_binding.QtWidgets import QWidget
+from python_qt_binding import loadUi
+
+class MasterWidget(QWidget):
+
+    def __init__(self,context):
+        super(MasterWidget, self).__init__()
+        ui_file = os.path.join(rospkg.RosPack().get_path('safety_master_plugin'), 'ui', 'master_view.ui')
+        loadUi(ui_file, self)
+        self.setObjectName('Master Plugin - L1.5 safety system')
+
+        if context.serial_number() > 1:
+            self.setWindowTitle(self.windowTitle() + (' (%d)' % context.serial_number()))
+        context.add_widget(self)

scripts/qt_master_wrapper.py

@@ -0,0 +1,243 @@
+#!/usr/bin/env python
+# coding=utf-8
+import os
+import rospy
+import rospkg
+
+import math
+import datetime
+
+from enums.clutch_state import ClutchState as CS
+from enums.stop_state import StopState as SS
+
+from master_restrictions import Restrictions
+
+from python_qt_binding.QtGui import QPixmap
+from python_qt_binding.QtGui import QTextCursor
+from python_qt_binding.QtGui import QStandardItem
+from python_qt_binding.QtGui import QStandardItemModel
+from python_qt_binding.QtGui import QBrush
+from python_qt_binding.QtWidgets import QMessageBox
+from python_qt_binding.QtWidgets import QHeaderView
+from python_qt_binding.QtWidgets import QAbstractItemView
+from python_qt_binding.QtCore import Qt
+
+
+class QtWrapper:
+
+    def __init__(self,widget):
+        self.widget = widget
+        self.displayed_robots_id_list = []
+
+        self.master_stop_state = None
+        self.init_robots_list()
+
+        # self.disable_sliders_tracking()
+
+        ui_directory_path = '{0}/ui'.format(rospkg.RosPack().get_path('safety_master_plugin'))
+
+        self.distance_pixmap = QPixmap('{0}/Distance.png'.format(ui_directory_path))
+        self.angular_pixmap = QPixmap('{0}/Angular.png'.format(ui_directory_path))
+        self.linear_pixmap = QPixmap('{0}/Linear.png'.format(ui_directory_path))
+        self.widget.distanceImage.setPixmap(self.distance_pixmap)
+        self.widget.angularImage.setPixmap(self.angular_pixmap)
+        self.widget.linearImage.setPixmap(self.linear_pixmap)
+
+    def set_sliders_to_initial_values(self):
+        self.widget.distanceSlider.setMaximum(200)
+        self.widget.angularSlider.setMaximum(200)
+        self.widget.linearSlider.setMaximum(200)
+
+        self.widget.distanceSlider.setValue(75)
+        self.widget.angularSlider.setValue(100)
+        self.widget.linearSlider.setValue(100)
+
+    def init_robots_list(self):
+        self.stdItemModel = QStandardItemModel( 0, 6, self.widget.robotsList) 
+        self.stdItemModel.setHeaderData( 0, Qt.Horizontal, 'Robot' )
+        self.stdItemModel.setHeaderData( 1, Qt.Horizontal, 'Stan baterii' )
+        self.stdItemModel.setHeaderData( 2, Qt.Horizontal, 'Stan robota' )
+        self.stdItemModel.setHeaderData( 3, Qt.Horizontal, 'Stan sprzęgła' )
+        self.stdItemModel.setHeaderData( 4, Qt.Horizontal, 'Prędk. lin.' )
+        self.stdItemModel.setHeaderData( 5, Qt.Horizontal, 'Prędk. obr.' )
+
+        self.widget.robotsList.setModel(self.stdItemModel)
+
+        self.widget.robotsList.horizontalHeader().setSectionResizeMode(0, QHeaderView.Stretch)
+        self.widget.robotsList.horizontalHeader().setSectionResizeMode(1, QHeaderView.Stretch)
+        self.widget.robotsList.horizontalHeader().setSectionResizeMode(2, QHeaderView.Stretch)
+        self.widget.robotsList.horizontalHeader().setSectionResizeMode(3, QHeaderView.Stretch)
+        self.widget.robotsList.horizontalHeader().setSectionResizeMode(4, QHeaderView.Stretch)
+        self.widget.robotsList.horizontalHeader().setSectionResizeMode(5, QHeaderView.Stretch)
+
+        self.widget.robotsList.horizontalHeader().setSectionsMovable(False)
+        # self.widget.robotsList.setSelectionBehavior(QAbstractItemView.SelectRows)
+        self.widget.robotsList.setSelectionMode(QAbstractItemView.NoSelection)
+
+
+    def disable_sliders_tracking(self):
+        self.widget.distanceSlider.setTracking(False)
+        self.widget.angularSlider.setTracking(False)
+        self.widget.linearSlider.setTracking(False)
+
+
+    def set_master_stop_state_updated_callback(self,callback_function):
+        self.master_stop_state_updated_callback = callback_function
+
+    def set_restrictions_updated_callback(self,callback_function):
+        self.restrictions_updated_callback = callback_function
+
+    def handle_restrictions_update(self):
+        distance = (float(self.widget.distanceSlider.value())/100)
+        angular_velocity = (float(self.widget.angularSlider.value())/100)
+        linear_velocity = (float(self.widget.linearSlider.value())/100)
+
+        self.widget.distanceLabel.setText("{:.2f}".format(distance))
+        self.widget.angularLabel.setText("{:.2f}".format(angular_velocity))
+        self.widget.linearLabel.setText("{:.2f}".format(linear_velocity))
+
+        restrictions = Restrictions(distance,angular_velocity,linear_velocity)
+
+        self.restrictions_updated_callback(restrictions)
+
+
+    def connect_signals(self):
+        self.widget.distanceSlider.valueChanged.connect(self.handle_restrictions_update)
+        self.widget.angularSlider.valueChanged.connect(self.handle_restrictions_update)
+        self.widget.linearSlider.valueChanged.connect(self.handle_restrictions_update)
+        self.widget.masterstopButton.clicked.connect(self.handle_masterstopButton_clicked)
+
+        self.set_sliders_to_initial_values()
+
+    def handle_masterstopButton_clicked(self):
+        if self.master_stop_state == SS.RUNNING:
+            self.master_stop_state_updated_callback()
+        else:
+            reply = QMessageBox.warning(self.widget,"UWAGA","Na pewno chcesz odblokować wszystkie roboty?",QMessageBox.Yes,QMessageBox.No)
+            
+            if reply == QMessageBox.Yes:
+                self.master_stop_state_updated_callback()
+
+    def display_master_stop_on(self):
+        self.widget.masterstopButton.setStyleSheet("QPushButton { color: black; background-color: green; font: bold 20px}")
+        self.widget.masterstopButton.setText('Zatrzymaj roboty')
+
+    def display_master_stop_off(self):
+        self.widget.masterstopButton.setStyleSheet("QPushButton { color: black; background-color: red; font: bold 20px}")
+        self.widget.masterstopButton.setText('Odblokuj roboty')
+
+
+    def update_robot_info(self,robot_info):
+        items_to_add = []
+        items_to_add.append(QStandardItem('PIONIER{}'.format(robot_info.robot_id)))
+        items_to_add.append(QStandardItem('{:.2f}'.format(robot_info.battery_voltage)))
+
+        if robot_info.state == SS.RUNNING:
+            item = QStandardItem('Działa')
+            item.setBackground(QBrush(Qt.green))
+            items_to_add.append(item)
+        elif robot_info.state == SS.STOPPED:
+            item = QStandardItem('Zatrzymany')
+            item.setBackground(QBrush(Qt.red))
+            items_to_add.append(item)
+        else:
+            raise ValueError('Undefined value of robot_info.robot_state')
+
+        if robot_info.clutch == CS.ENGAGED:
+            item = QStandardItem('Sprzęgnięte')
+            item.setBackground(QBrush(Qt.green))
+            items_to_add.append(item)
+        elif robot_info.clutch == CS.DISENGAGED:
+            item = QStandardItem('Rozprzęgnięte')
+            item.setBackground(QBrush(Qt.red))
+            items_to_add.append(item)
+        else:
+            raise ValueError('Undefined value of robot_info.clutch_state')
+
+        linear_vel = math.sqrt(robot_info.linear_velocity[0]**2 + robot_info.linear_velocity[1]**2)
+        items_to_add.append(QStandardItem('{:.2f}'.format(linear_vel)))
+        
+        items_to_add.append(QStandardItem('{:.2f}'.format(robot_info.angular_velocity[2])))
+
+
+        items_list = self.stdItemModel.findItems('PIONIER{0}'.format(robot_info.robot_id),Qt.MatchExactly)
+        if len(items_list) > 0:
+            for item in items_list:
+                row_number = item.row()
+        else:
+            row_number = self.stdItemModel.rowCount()
+
+        for i,item in enumerate(items_to_add):
+            item.setTextAlignment(Qt.AlignCenter)
+            item.setEditable(0)
+            self.stdItemModel.setItem(row_number,i,item)
+
+    def master_stopped(self):
+        self.master_stop_state = SS.STOPPED
+        self.display_master_stop_off()
+        self.log_info('Przycisk masterSTOP zostal naciśnięty. Zatrzymuję roboty')
+
+    def master_started(self):
+        self.master_stop_state = SS.RUNNING
+        self.display_master_stop_on()
+        self.log_info('Przycisk masterSTOP odciśnięty')
+
+
+    def add_robot(self,robot_id):
+
+        if robot_id not in self.displayed_robots_id_list:
+            new_robot_info=QStandardItem('PIONIER{}'.format(robot_id))
+            new_robot_info.setTextAlignment(Qt.AlignCenter)
+            new_robot_info.setEditable(0)
+            # new_robot_info.setBackground(QBrush(Qt.white))
+            self.stdItemModel.setItem(self.stdItemModel.rowCount(),0,new_robot_info)
+
+            self.displayed_robots_id_list.append(robot_id)
+            self.log_info('PIONIER{0} połączony'.format(robot_id))
+        else:
+            raise RuntimeError('Adding robot, which is already in master GUI')
+
+    def remove_robot(self,robot_id):
+        items_list = self.stdItemModel.findItems('PIONIER{0}'.format(robot_id),Qt.MatchExactly)
+
+        if len(items_list) > 0:
+            for item in items_list:
+                row_number = item.row()
+                self.stdItemModel.removeRow(row_number)
+        
+        self.log_info('PIONIER{0} rozłączony'.format(robot_id))
+        self.displayed_robots_id_list.remove(robot_id)
+
+    def log_info(self,info_text):
+        time = datetime.datetime.now().strftime('[%H:%M:%S]')
+
+        cursor = self.widget.logsBrowser.textCursor()
+        cursor.movePosition(QTextCursor.End)
+        self.widget.logsBrowser.setTextCursor(cursor)
+        # self.widget.logsBrowser.insertHtml('<font color="black">' + str(time) + '. ' + info_text + '</font><br>')
+        self.widget.logsBrowser.insertHtml('<font color="black"> {0} {1}</font><br>'.format(time,info_text))
+        self.scroll_to_bottom()
+
+    def log_warning(self,warning_text):
+        time = datetime.datetime.now().strftime('[%H:%M:%S]')
+        
+        cursor = self.widget.logsBrowser.textCursor()
+        cursor.movePosition(QTextCursor.End)
+        self.widget.logsBrowser.setTextCursor(cursor)
+        self.widget.logsBrowser.textCursor().movePosition(QTextCursor.End)
+        self.widget.logsBrowser.insertHtml('<font color="orange"> {0} {1}</font><br>'.format(time,warning_text))
+        self.scroll_to_bottom()
+
+    def log_error(self,error_text):
+        time = datetime.datetime.now().strftime('[%H:%M:%S]')
+
+        cursor = self.widget.logsBrowser.textCursor()
+        cursor.movePosition(QTextCursor.End)
+        self.widget.logsBrowser.setTextCursor(cursor)
+        self.widget.logsBrowser.textCursor().movePosition(QTextCursor.End)
+        self.widget.logsBrowser.insertHtml('<font color="red"> {0} {1}</font><br>'.format(time,error_text))
+        self.scroll_to_bottom()
+
+    def scroll_to_bottom(self):
+        scrollbar = self.widget.logsBrowser.verticalScrollBar()
+        scrollbar.setValue(scrollbar.maximum())

scripts/robot_info.py

@@ -0,0 +1,30 @@
+#!/usr/bin/env python
+
+from enums.clutch_state import ClutchState as CS
+from enums.stop_state import StopState as SS
+
+class RobotInfo:
+
+    def __init__(self,robot_id):
+        self.robot_id = robot_id
+        self.battery_voltage = None
+        self.linear_velocity = None
+        self.angular_velocity = None
+        self.state = None
+        self.clutch = None
+
+    def update_robot_info(self,robot_info_msg):
+        self.robot_id = robot_info_msg.robot_id.data
+        self.battery_voltage = robot_info_msg.battery_voltage.data
+        self.linear_velocity = [robot_info_msg.twist.linear.x,robot_info_msg.twist.linear.y,robot_info_msg.twist.linear.z]
+        self.angular_velocity = [robot_info_msg.twist.angular.x,robot_info_msg.twist.angular.y,robot_info_msg.twist.angular.z]
+
+        if robot_info_msg.state.data == True:
+            self.state = SS.RUNNING
+        else:
+            self.state = SS.STOPPED
+
+        if robot_info_msg.clutch.data == True:
+            self.clutch = CS.ENGAGED
+        else:
+            self.clutch = CS.DISENGAGED

scripts/ros_master_wrapper.py

@@ -0,0 +1,159 @@
+#!/usr/bin/env python
+import rospy
+import rospkg
+
+from std_msgs.msg import Bool
+from rosaria_msgs.msg import RobotInfoMsg
+from rosaria_msgs.msg import RestrictionsMsg
+
+from robot_info import RobotInfo
+from master_restrictions import Restrictions
+
+from enums.stop_state import StopState as SS
+
+class ROSWrapper:
+
+    def __init__(self):
+        self.robots_list_update_callback = None
+        self.robot_info_update_callback = None
+
+        self.robot_info_subscribers = {}
+        self.master_stop_publisher = None
+        self.restrictions_publisher = None
+
+        self.robots_list_timer = None
+        self.periodic_timer = None
+
+        self.setup_subscribers_and_publishers()
+
+
+    def setup_subscribers_and_publishers(self):
+        self.master_stop_publisher = rospy.Publisher('/PIONIER/master_stop',Bool,queue_size = 1)
+        self.restrictions_publisher = rospy.Publisher('/PIONIER/restrictions',RestrictionsMsg,queue_size = 1)
+
+        self.periodic_timer = rospy.Timer(rospy.Duration(0.5),self.publish_periodic_update)
+
+    def add_robot_subscriber(self,robot_id):
+        if robot_id not in self.robot_info_subscribers:
+            topic_name = '/PIONIER{0}/RosAria/robot_info'.format(robot_id)
+            new_sub = rospy.Subscriber(topic_name, RobotInfoMsg, self.handle_robot_info_update)
+            self.robot_info_subscribers[robot_id] = new_sub
+        else:
+            raise RuntimeError('Subscriber for PIONIER{0} already in dictionary'.format(robot_id))
+
+    def remove_robot_subscriber(self,robot_id):
+        if robot_id in self.robot_info_subscribers:
+            self.unsubscribe(self.robot_info_subscribers[robot_id])
+            del self.robot_info_subscribers[robot_id]
+        else:
+            raise RuntimeError('Subscriber for PIONIER{0} not in dictionary. Cannot be removed'.format(robot_id))
+
+    def unsubscribe(self,subscriber):
+        if subscriber != None:
+            subscriber.unregister()
+
+    def unregister_publisher(self,publisher):
+        if publisher != None:
+            publisher.unregister()
+
+    def shutdown_timer(self,timer):
+        if timer != None:
+            timer.shutdown()
+
+    def publish_periodic_update(self,event):
+        restrictions = self.get_restrictions()
+        restrictions_msg = RestrictionsMsg()
+        restrictions_msg.distance.data = restrictions.distance
+        restrictions_msg.linear_velocity.data = restrictions.linear_velocity
+        restrictions_msg.angular_velocity.data = restrictions.angular_velocity
+
+        stop_state = self.get_master_stop_state()
+        stop_msg = Bool()
+
+        if stop_state == SS.RUNNING:
+            stop_msg.data = True
+        elif stop_state == SS.STOPPED:
+            stop_msg.data = False
+        else:
+            stop_msg.data = False
+            raise ValueError('stop_state UNKNOWN when attempting to publish periodic update')
+
+        self.restrictions_publisher.publish(restrictions_msg)
+        self.master_stop_publisher.publish(stop_msg)
+
+    def cancel_subscribers_and_publishers(self):
+        self.shutdown_timer(self.robots_list_timer)
+        self.shutdown_timer(self.periodic_timer)
+
+        self.unregister_publisher(self.master_stop_publisher)
+        self.unregister_publisher(self.restrictions_publisher)
+
+        for key, value in self.robot_info_subscribers.iteritems():
+            self.unsubscribe(value)
+
+    def unregister_node(self):
+        self.cancel_subscribers_and_publishers()
+        rospy.signal_shutdown('Closing safety master plugin')
+
+    def setup_get_master_stop_state_function(self,function):
+        self.get_master_stop_state = function
+
+    def setup_get_restrictions_function(self,function):
+        self.get_restrictions = function
+
+    # ROSWrapper Callbacks
+    def get_robots_list(self,event):
+        robots_id_list = []
+
+        published_topics_list = rospy.get_published_topics(namespace='/')
+        published_topics = []
+
+        for list_ in published_topics_list:
+            published_topics.append(list_[0])
+
+        for topic in published_topics:
+            if topic.find('RosAria') ==-1 or topic.find('robot_info') == -1:
+                pass
+            else:
+                robot_number = topic.split('/')
+                robot_number = robot_number[1]
+                robot_number = robot_number[7:]
+                if len(robot_number) > 0:
+                    robot_number = int(robot_number)
+                    robots_id_list.append(robot_number)
+
+        self.robots_list_update_callback(robots_id_list)
+
+
+    def handle_robot_info_update(self,msg):
+        _robot_info = RobotInfo(0)
+        _robot_info.update_robot_info(msg)
+        self.robot_info_update_callback(_robot_info)
+
+
+    # MasterPlugin Callbacks
+    def set_robots_list_update_callback(self,callback_function):
+        self.robots_list_update_callback = callback_function
+        self.robots_list_timer = rospy.Timer(rospy.Duration(0.5),self.get_robots_list)
+
+    def set_robot_info_update_callback(self,callback_function):
+        self.robot_info_update_callback = callback_function
+
+    def master_started(self):
+        msg = Bool()
+        msg.data = True
+        self.master_stop_publisher.publish(msg)
+
+    def master_stopped(self):
+        msg = Bool()
+        msg.data = False
+        self.master_stop_publisher.publish(msg)
+
+    def restrictions_changed(self,restrictions):
+        msg = RestrictionsMsg()
+
+        msg.distance.data = restrictions.distance
+        msg.linear_velocity.data = restrictions.linear_velocity
+        msg.angular_velocity.data = restrictions.angular_velocity
+
+        self.restrictions_publisher.publish(msg)

setup.py

@@ -0,0 +1,8 @@
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+setup_args = generate_distutils_setup(
+    package_dir={'': 'scripts'}
+)
+
+setup(**setup_args)

ui/master_view.ui

@@ -0,0 +1,210 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>Form</class>
+ <widget class="QWidget" name="Form">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>715</width>
+    <height>569</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>Form</string>
+  </property>
+  <layout class="QVBoxLayout" name="verticalLayout_5" stretch="0,0,2,0,1">
+   <item>
+    <widget class="QTableView" name="robotsList">
+     <property name="textElideMode">
+      <enum>Qt::ElideMiddle</enum>
+     </property>
+     <property name="sortingEnabled">
+      <bool>false</bool>
+     </property>
+    </widget>
+   </item>
+   <item>
+    <spacer name="verticalSpacer">
+     <property name="orientation">
+      <enum>Qt::Vertical</enum>
+     </property>
+     <property name="sizeHint" stdset="0">
+      <size>
+       <width>20</width>
+       <height>25</height>
+      </size>
+     </property>
+    </spacer>
+   </item>
+   <item>
+    <layout class="QHBoxLayout" name="horizontalLayout_2" stretch="3,2">
+     <item>
+      <layout class="QVBoxLayout" name="verticalLayout_4">
+       <item>
+        <widget class="QLabel" name="label_4">
+         <property name="text">
+          <string>Ograniczenia robotów</string>
+         </property>
+         <property name="alignment">
+          <set>Qt::AlignCenter</set>
+         </property>
+        </widget>
+       </item>
+       <item>
+        <spacer name="verticalSpacer_3">
+         <property name="orientation">
+          <enum>Qt::Vertical</enum>
+         </property>
+         <property name="sizeType">
+          <enum>QSizePolicy::Fixed</enum>
+         </property>
+         <property name="sizeHint" stdset="0">
+          <size>
+           <width>20</width>
+           <height>10</height>
+          </size>
+         </property>
+        </spacer>
+       </item>
+       <item>
+        <layout class="QHBoxLayout" name="horizontalLayout">
+         <item>
+          <layout class="QVBoxLayout" name="verticalLayout">
+           <item>
+            <widget class="QLabel" name="distanceImage">
+             <property name="text">
+              <string>Obrazek1</string>
+             </property>
+             <property name="alignment">
+              <set>Qt::AlignCenter</set>
+             </property>
+            </widget>
+           </item>
+           <item alignment="Qt::AlignHCenter">
+            <widget class="QSlider" name="distanceSlider">
+             <property name="orientation">
+              <enum>Qt::Vertical</enum>
+             </property>
+            </widget>
+           </item>
+           <item>
+            <widget class="QLabel" name="distanceLabel">
+             <property name="text">
+              <string>0.45</string>
+             </property>
+             <property name="alignment">
+              <set>Qt::AlignCenter</set>
+             </property>
+            </widget>
+           </item>
+          </layout>
+         </item>
+         <item>
+          <layout class="QVBoxLayout" name="verticalLayout_2">
+           <item>
+            <widget class="QLabel" name="angularImage">
+             <property name="text">
+              <string>Obrazek2</string>
+             </property>
+             <property name="alignment">
+              <set>Qt::AlignCenter</set>
+             </property>
+            </widget>
+           </item>
+           <item alignment="Qt::AlignHCenter">
+            <widget class="QSlider" name="angularSlider">
+             <property name="orientation">
+              <enum>Qt::Vertical</enum>
+             </property>
+            </widget>
+           </item>
+           <item>
+            <widget class="QLabel" name="angularLabel">
+             <property name="text">
+              <string>0.67</string>
+             </property>
+             <property name="alignment">
+              <set>Qt::AlignCenter</set>
+             </property>
+            </widget>
+           </item>
+          </layout>
+         </item>
+         <item>
+          <layout class="QVBoxLayout" name="verticalLayout_3">
+           <item>
+            <widget class="QLabel" name="linearImage">
+             <property name="text">
+              <string>Obrazek3</string>
+             </property>
+             <property name="alignment">
+              <set>Qt::AlignCenter</set>
+             </property>
+            </widget>
+           </item>
+           <item alignment="Qt::AlignHCenter">
+            <widget class="QSlider" name="linearSlider">
+             <property name="orientation">
+              <enum>Qt::Vertical</enum>
+             </property>
+            </widget>
+           </item>
+           <item>
+            <widget class="QLabel" name="linearLabel">
+             <property name="text">
+              <string>0.45</string>
+             </property>
+             <property name="alignment">
+              <set>Qt::AlignCenter</set>
+             </property>
+            </widget>
+           </item>
+          </layout>
+         </item>
+        </layout>
+       </item>
+      </layout>
+     </item>
+     <item>
+      <widget class="QPushButton" name="masterstopButton">
+       <property name="sizePolicy">
+        <sizepolicy hsizetype="Minimum" vsizetype="Expanding">
+         <horstretch>0</horstretch>
+         <verstretch>0</verstretch>
+        </sizepolicy>
+       </property>
+       <property name="minimumSize">
+        <size>
+         <width>0</width>
+         <height>80</height>
+        </size>
+       </property>
+       <property name="text">
+        <string>STOP</string>
+       </property>
+      </widget>
+     </item>
+    </layout>
+   </item>
+   <item>
+    <spacer name="verticalSpacer_2">
+     <property name="orientation">
+      <enum>Qt::Vertical</enum>
+     </property>
+     <property name="sizeHint" stdset="0">
+      <size>
+       <width>20</width>
+       <height>25</height>
+      </size>
+     </property>
+    </spacer>
+   </item>
+   <item>
+    <widget class="QTextBrowser" name="logsBrowser"/>
+   </item>
+  </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>