update

Signed-off-by: Jakub Delicat <jakub.delicat@pwr.edu.pl>

.pre-commit-config.yaml

@@ -0,0 +1,14 @@
+# .pre-commit-config.yaml
+repos:
+-   repo: https://github.com/pre-commit/pre-commit-hooks
+    rev: v4.1.0  # this is optional, use `pre-commit autoupdate` to get the latest rev!
+    hooks:
+    -   id: check-yaml
+    -   id: end-of-file-fixer
+    -   id: trailing-whitespace
+
+-   repo: https://github.com/psf/black
+    rev: 23.7.0
+    hooks:
+    -   id: black
+        name: Format Python Code (black)

CMakeLists.txt

@@ -1,201 +0,0 @@
-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)

package.xml

@@ -1,35 +1,24 @@
 <?xml version="1.0"?>
-<package format="2">
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
   <name>safety_master_plugin</name>
-  <version>0.0.0</version>
+  <version>0.2.0</version>
-  <description>The safety_master_plugin package</description>
+  <description>TODO: Package description</description>
-
+  <maintainer email="l15@kcir.pwr.edu.pl">Laboratory L1.5</maintainer>
-  <maintainer email="aleksander.bojda@pwr.edu.pl">Aleksander Bojda</maintainer>
 
+  <author email="aleksander.bojda@pwr.edu.pl">Aleksander Bojda</author>
+  <author email="jakub.delicat@pwr.edu.pl">Jakub Delicat</author>
   <license>MIT</license>
 
-  <buildtool_depend>catkin</buildtool_depend>
+  <depend>rclpy</depend>
+  <depend>rqt_gui</depend>
+  <depend>rqt_gui_py</depend>
 
-  <build_depend>rospy</build_depend>
+  <depend>std_msgs</depend>
-  <build_depend>rqt_gui</build_depend>
+  <depend>ros2aria_msgs</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>
+    <build_type>ament_python</build_type>
     <rqt_gui plugin="${prefix}/plugin.xml"/>
   </export>
 </package>

plugin.xml

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

safety_master_plugin/enums/clutch_state.py

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

safety_master_plugin/master_callback_handler.py

@@ -1,6 +1,6 @@
 from python_qt_binding.QtCore import QObject
 from python_qt_binding.QtCore import pyqtSignal
-from robot_info import RobotInfo
+from .robot_info import RobotInfo
 
 class CallbackHandler(QObject):
     robot_info_signal = pyqtSignal(object)

safety_master_plugin/master_plugin.py

@@ -0,0 +1,127 @@
+from qt_gui.plugin import Plugin
+from python_qt_binding.QtCore import QTimer
+
+from safety_master_plugin.master_restrictions import Restrictions
+
+from safety_master_plugin.master_info import MasterInfo
+from safety_master_plugin.master_widget import MasterWidget
+from safety_master_plugin.stop_state import StopState as SS
+from safety_master_plugin.master_callback_handler import CallbackHandler
+from safety_master_plugin.ros_master_wrapper import ROSWrapper
+from safety_master_plugin.qt_master_wrapper import QtWrapper
+
+
+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.set_Qt_callback_functions()
+
+        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()
+
+        # timer to consecutively send twist messages
+        self._update_parameter_timer = QTimer(self)
+        self._update_parameter_timer.timeout.connect(
+            self._ros_wrapper.qt_timer_callback
+        )
+        self._update_parameter_timer.start(100)
+
+        # Stopping master at the beginning
+        self.disable_robots()
+
+    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):
+        """Handles master stop state when it changes e. g. on button click and confirmed."""
+        if self._master_info.master_stop_state == SS.STOPPED:
+            self.enable_robots()
+        elif self._master_info.master_stop_state == SS.RUNNING:
+            self.disable_robots()
+        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 enable_robots(self):
+        self._master_info.master_stop_state = SS.RUNNING
+        self._ros_wrapper.when_robots_enabled()
+        self._qt_wrapper.when_robots_enabled()
+
+    def disable_robots(self):
+        self._master_info.master_stop_state = SS.STOPPED
+        self._ros_wrapper.when_robots_disabled()
+        self._qt_wrapper.when_robots_disabled()
+
+    def restrictions_changed(self, restrictions):
+        self._master_info.restrictions = restrictions
+        self._ros_wrapper.restrictions_changed(restrictions)
+
+    def shutdown_plugin(self):
+        self._ros_wrapper.unregister_node()

safety_master_plugin/master_plugin_node.py

@@ -0,0 +1,11 @@
+from safety_master_plugin.master_plugin import MasterPlugin
+from rqt_gui.main import Main
+import sys
+
+def main():
+    """Run the plugin."""
+    Main().main(sys.argv, standalone="safety_master_plugin")
+
+
+if __name__ == "__main__":
+    main()

safety_master_plugin/master_widget.py

@@ -1,7 +1,6 @@
 #!/usr/bin/env python
 import os
-import rospkg
+from ament_index_python.packages import get_package_share_directory
-
 from python_qt_binding.QtWidgets import QWidget
 from python_qt_binding import loadUi
 
@@ -9,7 +8,7 @@ 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')
+        ui_file = os.path.join(get_package_share_directory('safety_master_plugin'), 'ui', 'master_view.ui')
         loadUi(ui_file, self)
         self.setObjectName('Master Plugin - L1.5 safety system')
 

safety_master_plugin/qt_master_wrapper.py

@@ -1,16 +1,12 @@
 #!/usr/bin/env python
 # coding=utf-8
-import os
-import rospy
-import rospkg
-
 import math
 import datetime
+from ament_index_python.packages import get_package_share_directory
 
-from enums.clutch_state import ClutchState as CS
+from safety_master_plugin.clutch_state import ClutchState as CS
-from enums.stop_state import StopState as SS
+from safety_master_plugin.stop_state import StopState as SS
-
+from safety_master_plugin.master_restrictions import Restrictions
-from master_restrictions import Restrictions
 
 from python_qt_binding.QtGui import QPixmap
 from python_qt_binding.QtGui import QTextCursor
@@ -24,21 +20,25 @@ from python_qt_binding.QtCore import Qt
 
 
 class QtWrapper:
-
+    def __init__(self, widget):
-    def __init__(self,widget):
         self.widget = widget
         self.displayed_robots_id_list = []
 
+        self.master_stop_state_updated_callback = None
+        self.restrictions_updated_callback = None
+
         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'))
+        ui_directory_path = "{0}/ui".format(
+            get_package_share_directory("safety_master_plugin")
+        )
 
-        self.distance_pixmap = QPixmap('{0}/Distance.png'.format(ui_directory_path))
+        self.distance_pixmap = QPixmap(f"{ui_directory_path}/Distance.png")
-        self.angular_pixmap = QPixmap('{0}/Angular.png'.format(ui_directory_path))
+        self.angular_pixmap = QPixmap(f"{ui_directory_path}/Angular.png")
-        self.linear_pixmap = QPixmap('{0}/Linear.png'.format(ui_directory_path))
+        self.linear_pixmap = QPixmap(f"{ui_directory_path}/Linear.png")
         self.widget.distanceImage.setPixmap(self.distance_pixmap)
         self.widget.angularImage.setPixmap(self.angular_pixmap)
         self.widget.linearImage.setPixmap(self.linear_pixmap)
@@ -53,189 +53,215 @@ class QtWrapper:
         self.widget.linearSlider.setValue(100)
 
     def init_robots_list(self):
-        self.stdItemModel = QStandardItemModel( 0, 6, self.widget.robotsList) 
+        self.stdItemModel = QStandardItemModel(0, 6, self.widget.robotsList)
-        self.stdItemModel.setHeaderData( 0, Qt.Horizontal, 'Robot' )
+        self.stdItemModel.setHeaderData(0, Qt.Horizontal, "Robot")
-        self.stdItemModel.setHeaderData( 1, Qt.Horizontal, 'Stan baterii' )
+        self.stdItemModel.setHeaderData(1, Qt.Horizontal, "Stan baterii")
-        self.stdItemModel.setHeaderData( 2, Qt.Horizontal, 'Stan robota' )
+        self.stdItemModel.setHeaderData(2, Qt.Horizontal, "Stan robota")
-        self.stdItemModel.setHeaderData( 3, Qt.Horizontal, 'Stan sprzęgła' )
+        self.stdItemModel.setHeaderData(3, Qt.Horizontal, "Stan sprzęgła")
-        self.stdItemModel.setHeaderData( 4, Qt.Horizontal, 'Prędk. lin.' )
+        self.stdItemModel.setHeaderData(4, Qt.Horizontal, "Prędk. lin.")
-        self.stdItemModel.setHeaderData( 5, Qt.Horizontal, 'Prędk. obr.' )
+        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(
-        self.widget.robotsList.horizontalHeader().setSectionResizeMode(1, QHeaderView.Stretch)
+            0, QHeaderView.Stretch
-        self.widget.robotsList.horizontalHeader().setSectionResizeMode(2, QHeaderView.Stretch)
+        )
-        self.widget.robotsList.horizontalHeader().setSectionResizeMode(3, QHeaderView.Stretch)
+        self.widget.robotsList.horizontalHeader().setSectionResizeMode(
-        self.widget.robotsList.horizontalHeader().setSectionResizeMode(4, QHeaderView.Stretch)
+            1, QHeaderView.Stretch
-        self.widget.robotsList.horizontalHeader().setSectionResizeMode(5, 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):
-    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):
+    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)
+        distance = float(self.widget.distanceSlider.value()) / 100
-        angular_velocity = (float(self.widget.angularSlider.value())/100)
+        angular_velocity = float(self.widget.angularSlider.value()) / 100
-        linear_velocity = (float(self.widget.linearSlider.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)
+        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.widget.masterstopButton.clicked.connect(
+            self.handle_master_stop_button_clicked
+        )
 
         self.set_sliders_to_initial_values()
 
-    def handle_masterstopButton_clicked(self):
+    def handle_master_stop_button_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)
+            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.setStyleSheet(
-        self.widget.masterstopButton.setText('Zatrzymaj roboty')
+            "QPushButton { color: black; background-color: red; font: bold 20px}"
+        )
+        self.widget.masterstopButton.setText("Odblokuj roboty")
 
     def display_master_stop_off(self):
-        self.widget.masterstopButton.setStyleSheet("QPushButton { color: black; background-color: red; font: bold 20px}")
+        self.widget.masterstopButton.setStyleSheet(
-        self.widget.masterstopButton.setText('Odblokuj roboty')
+            "QPushButton { color: black; background-color: green; font: bold 20px}"
+        )
+        self.widget.masterstopButton.setText("Zatrzymaj roboty")
 
-
+    def update_robot_info(self, robot_info):
-    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(f"pioneer{robot_info.robot_id}"))
-        items_to_add.append(QStandardItem('{:.2f}'.format(robot_info.battery_voltage)))
+        items_to_add.append(QStandardItem("{:.2f}".format(robot_info.battery_voltage)))
 
         if robot_info.state == SS.RUNNING:
-            item = QStandardItem('Działa')
+            item = QStandardItem("Działa")
             item.setBackground(QBrush(Qt.green))
             items_to_add.append(item)
         elif robot_info.state == SS.STOPPED:
-            item = QStandardItem('Zatrzymany')
+            item = QStandardItem("Zatrzymany")
             item.setBackground(QBrush(Qt.red))
             items_to_add.append(item)
         else:
-            raise ValueError('Undefined value of robot_info.robot_state')
+            raise ValueError("Undefined value of robot_info.robot_state")
 
         if robot_info.clutch == CS.ENGAGED:
-            item = QStandardItem('Sprzęgnięte')
+            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 = QStandardItem("Rozprzęgnięte")
             item.setBackground(QBrush(Qt.red))
             items_to_add.append(item)
         else:
-            raise ValueError('Undefined value of robot_info.clutch_state')
+            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)
+        linear_vel = math.sqrt(
-        items_to_add.append(QStandardItem('{:.2f}'.format(linear_vel)))
+            robot_info.linear_velocity[0] ** 2 + robot_info.linear_velocity[1] ** 2
-        
+        )
-        items_to_add.append(QStandardItem('{:.2f}'.format(robot_info.angular_velocity[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(
-        items_list = self.stdItemModel.findItems('PIONIER{0}'.format(robot_info.robot_id),Qt.MatchExactly)
+            f"pioneer{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):
+        for i, item in enumerate(items_to_add):
             item.setTextAlignment(Qt.AlignCenter)
             item.setEditable(0)
-            self.stdItemModel.setItem(row_number,i,item)
+            self.stdItemModel.setItem(row_number, i, item)
 
-    def master_stopped(self):
+    def when_robots_disabled(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')
+        self.log_info("Przycisk masterSTOP zostal naciśnięty. Zatrzymuję roboty")
 
+    def when_robots_enabled(self):
+        self.master_stop_state = SS.RUNNING
+        self.display_master_stop_off()
+        self.log_info("Przycisk masterSTOP odciśnięty")
 
-    def add_robot(self,robot_id):
+    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 = QStandardItem("pioneer{}".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.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))
+            self.log_info(f"pioneer{robot_id} połączony")
         else:
-            raise RuntimeError('Adding robot, which is already in master GUI')
+            raise RuntimeError("Adding robot, which is already in master GUI")
 
-    def remove_robot(self,robot_id):
+    def remove_robot(self, robot_id):
-        items_list = self.stdItemModel.findItems('PIONIER{0}'.format(robot_id),Qt.MatchExactly)
+        items_list = self.stdItemModel.findItems(f"pioneer{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.log_info(f"pioneer{robot_id} rozłączony")
         self.displayed_robots_id_list.remove(robot_id)
 
-    def log_info(self,info_text):
+    def log_info(self, info_text):
-        time = datetime.datetime.now().strftime('[%H:%M:%S]')
+        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(
-        self.widget.logsBrowser.insertHtml('<font color="black"> {0} {1}</font><br>'.format(time,info_text))
+            f'<font color="black"> {time} {info_text}</font><br>'
+        )
         self.scroll_to_bottom()
 
-    def log_warning(self,warning_text):
+    def log_warning(self, warning_text):
-        time = datetime.datetime.now().strftime('[%H:%M:%S]')
+        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.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):

safety_master_plugin/robot_info.py

@@ -0,0 +1,38 @@
+#!/usr/bin/env python
+
+from safety_master_plugin.clutch_state import ClutchState as CS
+from safety_master_plugin.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 is True:
+            self.state = SS.RUNNING
+        else:
+            self.state = SS.STOPPED
+
+        if robot_info_msg.clutch.data is True:
+            self.clutch = CS.ENGAGED
+        else:
+            self.clutch = CS.DISENGAGED

safety_master_plugin/ros_master_wrapper.py

@@ -0,0 +1,192 @@
+#!/usr/bin/env python
+import rclpy
+from rclpy.node import Node, NodeNameNonExistentError
+from rclpy.qos import (
+    QoSProfile,
+    QoSReliabilityPolicy,
+    QoSHistoryPolicy,
+    QoSLivelinessPolicy,
+    QoSDurabilityPolicy,
+)
+
+from std_msgs.msg import Bool
+from ros2aria_msgs.msg import RobotInfoMsg
+from ros2aria_msgs.msg import RestrictionsMsg
+
+from safety_master_plugin.robot_info import RobotInfo
+from safety_master_plugin.master_restrictions import Restrictions
+
+from safety_master_plugin.stop_state import StopState as SS
+
+
+class ROSWrapper(Node):
+    def __init__(self):
+        super().__init__("safety_master_plugin")
+        self.robots_list_update_callback = None
+        self.robot_info_update_callback = None
+        self.get_master_stop_state = None
+        self.get_restrictions = None
+
+        self.robot_info_subscribers = {}
+        self.master_stop_publisher = None
+        self.restrictions_publisher = None
+
+        self.setup_subscribers_and_publishers()
+        self.subs = []
+
+    def setup_subscribers_and_publishers(self):
+        publishers_qos = QoSProfile(
+            reliability=QoSReliabilityPolicy.RELIABLE,
+            durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
+            history=QoSHistoryPolicy.SYSTEM_DEFAULT,
+            liveliness=QoSLivelinessPolicy.AUTOMATIC,
+            depth=10,
+        )
+
+        self.master_stop_publisher = self.create_publisher(
+            Bool, "/pioneers/master_stop", qos_profile=publishers_qos
+        )
+        self.restrictions_publisher = self.create_publisher(
+            RestrictionsMsg, "/pioneers/restrictions", qos_profile=publishers_qos
+        )
+
+    def add_robot_subscriber(self, robot_id):
+        if robot_id not in self.robot_info_subscribers:
+            topic_name = "/pioneer{0}/robot_info".format(robot_id)
+            subscription_qos = QoSProfile(
+                reliability=QoSReliabilityPolicy.RELIABLE,
+                durability=QoSDurabilityPolicy.VOLATILE,
+                history=QoSHistoryPolicy.SYSTEM_DEFAULT,
+                liveliness=QoSLivelinessPolicy.AUTOMATIC,
+                depth=10,
+            )
+
+            self.robot_info_subscribers[robot_id] = self.create_subscription(
+                RobotInfoMsg,
+                topic_name,
+                self.handle_robot_info_update,
+                qos_profile=subscription_qos,
+            )
+        else:
+            raise RuntimeError(
+                "Subscriber for pioneer{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 PIONEER{0} not in dictionary. Cannot be removed".format(
+                    robot_id
+                )
+            )
+
+    def unsubscribe(self, subscriber):
+        if subscriber is not None:
+            self.destroy_subscription(subscriber)
+
+    def unregister_publisher(self, publisher):
+        if publisher is not None:
+            self.destroy_publisher(publisher)
+
+    def publish_periodic_update(self):
+        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 = False
+        elif stop_state == SS.STOPPED:
+            stop_msg.data = True
+        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.when_robots_disabled()
+        
+        self.unregister_publisher(self.master_stop_publisher)
+        self.unregister_publisher(self.restrictions_publisher)
+
+        for subscriber in self.robot_info_subscribers.values():
+            self.unsubscribe(subscriber)
+
+    def unregister_node(self):
+        self.cancel_subscribers_and_publishers()
+
+    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):
+        robots_id_list = []
+
+        # Można to zrobić lepiej np. czekać na publikowaną wiadomość na robot_info
+        for pioneer_id in range(1, 7):
+            published_topics_list = []
+            try:
+                published_topics_list = self.get_publisher_names_and_types_by_node(
+                    "ros2aria", "/pioneer" + str(pioneer_id)
+                )
+            except NodeNameNonExistentError:
+                pass
+
+            if len(published_topics_list) > 0:
+                robots_id_list.append(pioneer_id)
+
+        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
+
+    def set_robot_info_update_callback(self, callback_function):
+        self.robot_info_update_callback = callback_function
+
+    def when_robots_enabled(self):
+        msg = Bool()
+        msg.data = False
+        self.master_stop_publisher.publish(msg)
+
+    def when_robots_disabled(self):
+        msg = Bool()
+        msg.data = True
+        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)
+
+    def qt_timer_callback(self):
+        if self.publish_periodic_update is not None:
+            self.publish_periodic_update()
+
+        if self.get_robots_list is not None:
+            self.get_robots_list()
+
+        rclpy.spin_once(self, timeout_sec=0.001)

safety_master_plugin/setup.cfg

@@ -0,0 +1,2 @@
+[flake8]
+max-line-length = 110

safety_master_plugin/stop_state.py

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

scripts/master_plugin.py

@@ -1,110 +0,0 @@
-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/robot_info.py

@@ -1,30 +0,0 @@
-#!/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

@@ -1,159 +0,0 @@
-#!/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)

scripts/safety_master_plugin

@@ -0,0 +1,9 @@
+#!/usr/bin/env python3
+
+import sys
+
+from safety_master_plugin.master_plugin import MasterPlugin
+from rqt_gui.main import Main
+
+
+sys.exit(Main().main(sys.argv, standalone='safety_master_plugin.master_plugin.MasterPlugin'))

setup.cfg

@@ -0,0 +1,5 @@
+[develop]
+script_dir=$base/lib/safety_master_plugin
+[install]
+install_scripts=$base/lib/safety_master_plugin
+

setup.py

@@ -1,8 +1,31 @@
-from distutils.core import setup
+import os
-from catkin_pkg.python_setup import generate_distutils_setup
+from glob import glob
+from setuptools import setup
 
-setup_args = generate_distutils_setup(
+
-    package_dir={'': 'scripts'}
+package_name = 'safety_master_plugin'
+
+setup(
+    name=package_name,
+    version='0.2.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        ('share/' + package_name, ['plugin.xml']),
+        (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*.launch.py'))),
+        (os.path.join('share', package_name, 'ui'), glob(os.path.join('ui', '*.png'))),
+        (os.path.join('share', package_name, 'ui'), glob(os.path.join('ui', '*.ui'))),
+        (os.path.join('lib/', package_name), glob(os.path.join('ui', '*.ui'))),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='jdelicat',
+    maintainer_email='jakub.delicat@pwr.edu.pl',
+    description='TODO: Package description',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': ['master_plugin_node = safety_master_plugin.master_plugin_node:main'],
+    },
 )
-
-setup(**setup_args)