Fixed log pioneer id

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,204 +0,0 @@
-cmake_minimum_required(VERSION 2.8.3)
-project(safety_user_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
-  geometry_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
-#   RestrictionsMsg.msg
-#   RobotInfoMsg.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
-#   geometry_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(
-	CATKIN_DEPENDS message_runtime
-#  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})
-
-## 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,39 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>safety_user_plugin</name>
-  <version>0.0.0</version>
+  <version>0.2.0</version>
   <description>The safety_user_plugin package</description>
 
-  <maintainer email="aleksander.bojda@pwr.edu.pl">Aleksander Bojda</maintainer>
+  <maintainer email="l15@kcir.pwr.edu.pl">Laboratory L1.5</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>
 
-  <build_depend>rospy</build_depend>
+  <build_depend>rclpy</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_depend>ros2aria_msgs</build_depend>
 
-  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>rclpy</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>
+  <build_export_depend>ros2aria_msgs</build_export_depend>
 
-  <exec_depend>rospy</exec_depend>
+  <exec_depend>rclpy</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>
+  <exec_depend>ros2aria_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_user_plugin" type="user_plugin.UserPlugin" base_class_type="rqt_gui_py::Plugin">
+  <class name="safety_user_plugin" type="safety_user_plugin.user_plugin.UserPlugin" base_class_type="rqt_gui_py::Plugin">
     <description>
       Safety system user plugin
     </description>
     <qtgui>
       <group>
-        <label>Safety system</label>
+        <label>L1.5 Safety system</label>
         <icon type="theme">folder</icon>
       </group>
       <label>User</label>

safety_user_plugin/english.py

@@ -1,16 +1,16 @@
 #!/usr/bin/env python
 # coding=utf-8
 
-from language import LanguageTexts
+from safety_user_plugin.language import LanguageTexts
+
 
 class EnglishTexts(LanguageTexts):
-
     # Warning popup for robot unlocking
     warning_text_1 = "ATTENTION"
     warning_text_2 = "Are You sure You want to unlock the robot?"
 
     # Button texts
-    select_text = 'Select'
+    select_text = "Select"
 
     stop_robot_text = "STOP robot"
     unlock_robot_text = "UNLOCK robot"
@@ -18,35 +18,39 @@ class EnglishTexts(LanguageTexts):
     disengage_clutch_text = "Disengage clutch"
     engage_clutch_text = "Engage clutch"
 
-    release_robot_text = 'Release robot'
-
+    release_robot_text = "Release robot"
 
     # Logger info texts
-    robot_selected_text = 'PIONIER{0} selected!'
-    robot_released_text = 'Robot released'
+    robot_selected_text = "PIONEER{0} selected!"
+    robot_released_text = "Robot released"
     stopped_and_engaged_text = "Robot will be stopped and clutch engaged"
 
-    master_stop_clicked_text = 'MasterSTOP button pressed. Stopping the robots'
-    master_stop_released_text = 'MasterSTOP button released. Robot can be unlocked'
+    master_stop_clicked_text = "MasterSTOP button pressed. Stopping the robots"
+    master_stop_released_text = "MasterSTOP button released. Robot can be unlocked"
 
-    robot_stopped_text = 'Robot stopped'
-    robot_started_text = 'Robot started'
+    robot_stopped_text = "Robot stopped"
+    robot_started_text = "Robot started"
 
-    clutch_disengaged_text = 'Disengaging the clutch'
-    clutch_engaged_text = 'Engaging the clutch'
-
-    obstacle_detected_text = 'Obstacle detected. Robot stopped'
-    obstacle_removed_text = 'Obstacle removed'
+    clutch_disengaged_text = "Disengaging the clutch"
+    clutch_engaged_text = "Engaging the clutch"
 
+    obstacle_detected_text = "Obstacle detected. Robot stopped"
+    obstacle_removed_text = "Obstacle removed"
 
     # Logger error/problem messages
-    selection_error_text = 'Select the PIONEER from robots list first'
+    master_plugin_inactive_text = (
+        "Master is inactive. Ask the teacher about the master plugin."
+    )
+    selection_error_text = "Select the PIONEER from robots list first"
     cannot_start_masterstop_text = "Robot can't be started. MasterSTOP pressed!"
     cannot_start_obstacle_text = "Robot can't be started. Obstacle detected!"
-    cannot_select_robot_text = "Robot can't be selected. It is already selected by another group"
-    connection_to_robot_lost_text = 'Connection lost with robot PIONIER{0}'
-    connection_to_master_lost_text = 'Connection lost with masterSTOP. Ask the teacher to run it'
-
+    cannot_select_robot_text = (
+        "Robot can't be selected. It is already selected by another group"
+    )
+    connection_to_robot_lost_text = "Connection lost with robot PIONEER{0}"
+    connection_to_master_lost_text = (
+        "Connection lost with masterSTOP. Ask the teacher to run it"
+    )
 
     # Description labels
     linear_text = '<html><head/><body><p align="center"><span style=" font-weight:600;">Linear velocity</span></p></body></html>'
@@ -59,7 +63,5 @@ class EnglishTexts(LanguageTexts):
     restrictions_text = '<html><head/><body><p align="center"><span style=" font-weight:600;">Restrictions</span></p></body></html>'
     history_text = '<html><head/><body><p align="center"><span style=" font-weight:600;">Message history</span></p></body></html>'
 
-
-
     def __init__(self):
         pass

safety_user_plugin/polish.py

@@ -1,16 +1,16 @@
 #!/usr/bin/env python
 # coding=utf-8
 
-from language import LanguageTexts
+from safety_user_plugin.language import LanguageTexts
+
 
 class PolishTexts(LanguageTexts):
-
     # Warning popup for robot unlocking
     warning_text_1 = "UWAGA"
     warning_text_2 = "Na pewno chcesz odblokować robota?"
 
     # Button texts
-    select_text = 'Wybierz'
+    select_text = "Wybierz"
 
     stop_robot_text = "Zatrzymaj robota"
     unlock_robot_text = "Odblokuj robota"
@@ -18,35 +18,45 @@ class PolishTexts(LanguageTexts):
     disengage_clutch_text = "Rozłącz sprzęgło"
     engage_clutch_text = "Połącz sprzęgło"
 
-    release_robot_text = 'Zwolnij robota'
-
+    release_robot_text = "Zwolnij robota"
 
     # Logger info texts
-    robot_selected_text = 'PIONIER{0} wybrany!'
-    robot_released_text = 'Odłączam robota'
+    robot_selected_text = "PIONEER{0} wybrany!"
+    robot_released_text = "Odłączam robota"
     stopped_and_engaged_text = "Robot zostanie zatrzymany i sprzęgnięty"
 
-    master_stop_clicked_text = 'Przycisk masterSTOP został naciśnięty. Zatrzymuje roboty'
-    master_stop_released_text = 'Przycisk masterSTOP odciśnięty. Mozesz uruchomić robota'
+    master_stop_clicked_text = (
+        "Przycisk masterSTOP został naciśnięty. Zatrzymuje roboty"
+    )
+    master_stop_released_text = (
+        "Przycisk masterSTOP odciśnięty. Mozesz uruchomić robota"
+    )
 
-    robot_stopped_text = 'Robot zatrzymany'
-    robot_started_text = 'Robot wystartował'
+    robot_stopped_text = "Robot zatrzymany"
+    robot_started_text = "Robot wystartował"
 
-    clutch_disengaged_text = 'Rozprzęgam sprzęgło'
-    clutch_engaged_text = 'Sprzęgam sprzegło'
-
-    obstacle_detected_text = 'Przeszkoda wykryta. Zatrzymuję robota'
-    obstacle_removed_text = 'Przeszkoda usunięta'
+    clutch_disengaged_text = "Rozprzęgam sprzęgło"
+    clutch_engaged_text = "Sprzęgam sprzegło"
 
+    obstacle_detected_text = "Przeszkoda wykryta. Zatrzymuję robota"
+    obstacle_removed_text = "Przeszkoda usunięta"
 
     # Logger error/problem messages
-    selection_error_text = 'Najpierw wybierz PIONIERA z listy robotów'
-    cannot_start_masterstop_text = 'Nie można wystartować robota. MasterSTOP wciśnięty!'
-    cannot_start_obstacle_text = 'Nie mozna wystartować. Przeszkoda w polu działania robota'
-    cannot_select_robot_text = 'Nie mozna wybrać robota. Robot został już wybrany przez inną grupę'
-    connection_to_robot_lost_text = 'Utrata połączenia z robotem PIONIER{0}'
-    connection_to_master_lost_text = 'Utrata połączenia z masterstopem. Poproś prowadzącego o jego uruchomienie'
-
+    master_plugin_inactive_text = (
+        "Master nieaktywny. Zapytaj prowadzącego o odpalenie programu."
+    )
+    selection_error_text = "Najpierw wybierz PIONEERA z listy robotów"
+    cannot_start_masterstop_text = "Nie można wystartować robota. MasterSTOP wciśnięty!"
+    cannot_start_obstacle_text = (
+        "Nie mozna wystartować. Przeszkoda w polu działania robota"
+    )
+    cannot_select_robot_text = (
+        "Nie mozna wybrać robota. Robot został już wybrany przez inną grupę"
+    )
+    connection_to_robot_lost_text = "Utrata połączenia z robotem PIONEER{0}"
+    connection_to_master_lost_text = (
+        "Utrata połączenia z masterstopem. Poproś prowadzącego o jego uruchomienie"
+    )
 
     # Description labels
     linear_text = '<html><head/><body><p align="center"><span style=" font-weight:600;">Prędkość liniowa</span></p></body></html>'

safety_user_plugin/qt_wrapper.py

@@ -2,53 +2,54 @@
 # 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 ament_index_python.packages import get_package_share_directory
+
+
+from safety_user_plugin.enums.clutch_state import ClutchState as CS
+from safety_user_plugin.enums.stop_state import StopState as SS
 
 from python_qt_binding.QtGui import QPixmap
 from python_qt_binding.QtGui import QTextCursor
 from python_qt_binding.QtWidgets import QMessageBox
 
-from polish import PolishTexts as PT
-from english import EnglishTexts as ET
+from safety_user_plugin.polish import PolishTexts as PT
+from safety_user_plugin.english import EnglishTexts as ET
+
 
 class QtWrapper:
-
     def __init__(self, widget):
         self.widget = widget
         self.displayed_robots_id_list = []
         
-        lang = rospy.get_param('lang')
+        lang = os.getenv("LANG")
 
-        if lang == 'eng':
-            self.language = ET
-            self.log_info('Started english version of plugin NEW')
-        elif lang == 'pol':
+        if lang[:2] == "pl":
             self.language = PT
-            self.log_info('Polska wersja pluginu uruchomiona')
+            self.log_info("Polska wersja pluginu uruchomiona")
         else:
-            raise ValueError('language parameter has value different than "eng" or "pol"')
+            self.language = ET
+            self.log_info("Started english version of plugin NEW")
 
         self.clear_robot_info()
 
-        ui_directory_path = '{0}/ui'.format(rospkg.RosPack().get_path('safety_user_plugin'))
+        ui_directory_path = "{0}/ui".format(
+            get_package_share_directory("safety_user_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.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.scaled(66, 46))
         self.widget.angularImage.setPixmap(self.angular_pixmap)
         self.widget.linearImage.setPixmap(self.linear_pixmap.scaled(20, 48))
 
-        self.ok_pixmap = QPixmap('{0}/Ok.jpg'.format(ui_directory_path))
-        self.cancel_pixmap = QPixmap('{0}/Cancel.jpg'.format(ui_directory_path))
+        self.ok_pixmap = QPixmap("{0}/Ok.png".format(ui_directory_path))
+        self.cancel_pixmap = QPixmap("{0}/Cancel.png".format(ui_directory_path))
 
     def disconnect_signals(self):
         self.widget.clutchButton.clicked.disconnect()
@@ -56,7 +57,9 @@ class QtWrapper:
         self.widget.choiceButton.clicked.disconnect()
 
     def connect_robot_signals(self):
-        self.widget.clutchButton.clicked.connect(self.handle_clutchButton_clicked_callback)
+        self.widget.clutchButton.clicked.connect(
+            self.handle_clutchButton_clicked_callback
+        )
         self.widget.stopButton.clicked.connect(self.handle_stopButton_clicked)
         self.widget.choiceButton.clicked.connect(self.handle_closeButton_clicked)
 
@@ -65,62 +68,76 @@ class QtWrapper:
         self.widget.stopButton.clicked.connect(self.handle_not_selected_error)
         self.widget.choiceButton.clicked.connect(self.handle_openButton_clicked)
 
-
     def handle_emitted_signal(self, method_name):
-        exec('self.{0}()'.format(method_name))
+        exec("self.{0}()".format(method_name))
 
     def handle_emitted_signal_with_list_argument(self, method_name, argument):
-        if method_name == 'update_selected_robot_info':
+        if method_name == "update_selected_robot_info":
             self.update_selected_robot_info(argument[0])
 
         else:
-            method_with_argument = 'self.{0}({1})'.format(method_name,argument[0])
+            method_with_argument = "self.{0}({1})".format(method_name, argument[0])
             exec(method_with_argument)
 
-
     def handle_stopButton_clicked(self):
         if self.robot_state == SS.RUNNING:
             self.handle_stopButton_clicked_callback()
         else:
-            reply = QMessageBox.warning(self.widget,self.language.warning_text_1,self.language.warning_text_2,QMessageBox.Yes,QMessageBox.No)
+            reply = QMessageBox.warning(
+                self.widget,
+                self.language.warning_text_1,
+                self.language.warning_text_2,
+                QMessageBox.Yes,
+                QMessageBox.No,
+            )
 
             if reply == QMessageBox.Yes:
                 self.handle_stopButton_clicked_callback()
 
-
     def get_selected_robot_id(self):
         current_text = self.widget.robotsList.currentText()
 
-        if 'PIONIER' in current_text:
+        if "PIONEER" in current_text:
             return int(current_text[7:])
         else:
             return None
 
     def display_clutch_on(self):
         # self.widget.clutchLabel.setPixmap(self.ok_pixmap.scaled(40,40))
-        self.widget.clutchButton.setStyleSheet("QPushButton { color: black; background-color: green; font: bold 20px}")
+        self.widget.clutchButton.setStyleSheet(
+            "QPushButton { color: black; background-color: green; font: bold 20px}"
+        )
         self.widget.clutchButton.setText(self.language.disengage_clutch_text)
 
     def display_clutch_off(self):
         # self.widget.clutchLabel.setPixmap(self.cancel_pixmap.scaled(40,40))
-        self.widget.clutchButton.setStyleSheet("QPushButton { color: black; background-color: red; font: bold 20px}")
+        self.widget.clutchButton.setStyleSheet(
+            "QPushButton { color: black; background-color: red; font: bold 20px}"
+        )
         self.widget.clutchButton.setText(self.language.engage_clutch_text)
 
     def display_state_on(self):
         self.widget.stateLabel.setPixmap(self.ok_pixmap.scaled(40, 40))
-        self.widget.stopButton.setStyleSheet("QPushButton { color: black; background-color: green; font: bold 20px}")
+        self.widget.stopButton.setStyleSheet(
+            "QPushButton { color: black; background-color: green; font: bold 20px}"
+        )
         self.widget.stopButton.setText(self.language.stop_robot_text)
         self.robot_state = SS.RUNNING
 
     def display_state_off(self):
         self.widget.stateLabel.setPixmap(self.cancel_pixmap.scaled(40, 40))
-        self.widget.stopButton.setStyleSheet("QPushButton { color: black; background-color: red; font: bold 20px}")
+        self.widget.stopButton.setStyleSheet(
+            "QPushButton { color: black; background-color: red; font: bold 20px}"
+        )
         self.widget.stopButton.setText(self.language.unlock_robot_text)
         self.robot_state = SS.STOPPED
 
     def handle_not_selected_error(self):
         self.log_error(self.language.selection_error_text)
 
+    def log_master_inactive(self):
+        self.log_warning(self.language.master_plugin_inactive_text)
+
     def handle_openButton_clicked(self):
         selected_robot_id = self.get_selected_robot_id()
 
@@ -147,7 +164,7 @@ class QtWrapper:
     def select_robot(self, robot_id):
         self.disconnect_signals()
         self.connect_robot_signals()
-        self.log_info(self.language.robot_selected_text.format(robot_id)) 
+        self.log_info(self.language.robot_selected_text.format(robot_id+1))
         self.log_info(self.language.stopped_and_engaged_text)
 
         self.widget.choiceButton.setText(self.language.release_robot_text)
@@ -160,7 +177,7 @@ class QtWrapper:
 
     def update_robots_list_gui(self, robots_id_list):
         robots_id_list.sort()
-        id_strings_list = (('PIONIER'+str(x)) for x in robots_id_list)
+        id_strings_list = (("PIONEER" + str(x)) for x in robots_id_list)
 
         robots_to_add = []
         robots_to_remove = []
@@ -179,7 +196,6 @@ class QtWrapper:
         for robot_id in robots_to_add:
             self.add_robot_to_list(robot_id)
 
-
     def remove_robot_from_list(self, robot_id):
         count = self.widget.robotsList.count()
 
@@ -189,19 +205,21 @@ class QtWrapper:
                 self.displayed_robots_id_list.remove(robot_id)
                 return
 
-
     def add_robot_to_list(self, robot_id):
-        self.widget.robotsList.addItem('PIONIER{0}'.format(robot_id))
+        self.widget.robotsList.addItem("PIONEER{0}".format(robot_id))
         self.displayed_robots_id_list.append(robot_id)
 
-
     def update_selected_robot_info(self, robot_info):
-        linear_vel = math.sqrt(robot_info.linear_velocity[0]**2 + robot_info.linear_velocity[1]**2)
+        linear_vel = math.sqrt(
+            robot_info.linear_velocity[0] ** 2 + robot_info.linear_velocity[1] ** 2
+        )
 
-        self.widget.idLabel.setText('PIONIER{0}'.format(robot_info.robot_id))
+        self.widget.idLabel.setText("PIONEER{0}".format(robot_info.robot_id))
         self.widget.batteryLabel.setText("{:.2f}".format(robot_info.battery_voltage))
         self.widget.linearLabel.setText("{:.2f}".format(linear_vel))
-        self.widget.angularLabel.setText("{:.2f}".format(robot_info.angular_velocity[2]))
+        self.widget.angularLabel.setText(
+            "{:.2f}".format(robot_info.angular_velocity[2])
+        )
 
         # self.log_info(str(robot_info.clutch == CS.ENGAGED))
 
@@ -254,39 +272,51 @@ class QtWrapper:
         self.log_error(self.language.cannot_select_robot_text)
 
     def update_restrictions_gui(self, restrictions):
-        self.widget.distanceRestrictionLabel.setText("{:.2f}".format(restrictions.distance))
-        self.widget.linearRestrictionLabel.setText("{:.2f}".format(restrictions.linear_velocity))
-        self.widget.angularRestrictionLabel.setText("{:.2f}".format(restrictions.angular_velocity))       
+        self.widget.distanceRestrictionLabel.setText(
+            "{:.2f}".format(restrictions.distance)
+        )
+        self.widget.linearRestrictionLabel.setText(
+            "{:.2f}".format(restrictions.linear_velocity)
+        )
+        self.widget.angularRestrictionLabel.setText(
+            "{:.2f}".format(restrictions.angular_velocity)
+        )
 
     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"> {0} {1}</font><br>'.format(time,info_text))
+        self.widget.logsBrowser.insertHtml(
+            '<font color="black"> {0} {1}</font><br>'.format(time, info_text)
+        )
         self.scroll_to_bottom()
         # self.widget.logsBrowser.insertHtml(str(self.logger_counter) + '\t[INFO]\t' + info_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.widget.logsBrowser.insertHtml(
+            '<font color="orange"> {0} {1}</font><br>'.format(time, warning_text)
+        )
         self.scroll_to_bottom()
         # self.widget.logsBrowser.append(str(self.logger_counter) + '\t[WARN]\t' + warning_text)
 
     def log_error(self, error_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="red"> {0} {1}</font><br>'.format(time,error_text))
+        self.widget.logsBrowser.insertHtml(
+            '<font color="red"> {0} {1}</font><br>'.format(time, error_text)
+        )
         self.scroll_to_bottom()
         # self.widget.logsBrowser.append(str(self.logger_counter) + '\t[ERROR]\t' + error_text)
 
@@ -311,20 +341,20 @@ class QtWrapper:
         scrollbar.setValue(scrollbar.maximum())
 
     def clear_robot_info(self):
-        self.widget.idLabel.setText('-')
-        self.widget.batteryLabel.setText('-')
-        self.widget.linearLabel.setText('-')
-        self.widget.angularLabel.setText('-')
-        self.widget.stateLabel.setText('-')
-        self.widget.masterstopLabel.setText('-')
-        self.widget.obstaclestopLabel.setText('-')
+        self.widget.idLabel.setText("-")
+        self.widget.batteryLabel.setText("-")
+        self.widget.linearLabel.setText("-")
+        self.widget.angularLabel.setText("-")
+        self.widget.stateLabel.setText("-")
+        self.widget.masterstopLabel.setText("-")
+        self.widget.obstaclestopLabel.setText("-")
 
         self.widget.choiceButton.setText(self.language.select_text)
 
-        self.widget.stopButton.setText('-')
+        self.widget.stopButton.setText("-")
         self.widget.stopButton.setStyleSheet("")
 
-        self.widget.clutchButton.setText('-')
+        self.widget.clutchButton.setText("-")
         self.widget.clutchButton.setStyleSheet("")
 
         self.widget.angularText.setText(self.language.angular_text)

safety_user_plugin/robot_info.py

@@ -0,0 +1,40 @@
+#!/usr/bin/env python
+
+from safety_user_plugin.enums.clutch_state import ClutchState as CS
+from safety_user_plugin.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
+        self.obstacle_detected = 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,
+        ]
+        self.obstacle_detected = robot_info_msg.obstacle_detected.data
+
+        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_user_plugin/ros_wrapper.py

@@ -0,0 +1,311 @@
+#!/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_user_plugin.robot_info import RobotInfo
+from safety_user_plugin.restrictions import Restrictions
+
+from safety_user_plugin.enums.clutch_state import ClutchState as CS
+from safety_user_plugin.enums.stop_state import StopState as SS
+
+
+class ROSWrapper(Node):
+    def __init__(self):
+        # TODO: mogą być problemy z tymi samymi nazwami node'ów, gry różne kompy będą miały odpalony
+        super().__init__("safety_user_plugin")
+        self.robots_list_update_callback = None
+        self.selected_robot_info_update_callback = None
+        self.master_stop_update_callback = None
+        self.restrictions_update_callback = None
+        self.master_connection_lost_callback = None
+        self.robot_connection_lost_callback = None
+
+        self.get_user_stop_state = None
+        self.get_clutch_state = None
+
+        self.robot_info_subscriber = None
+        self.master_stop_subscriber = None
+        self.restrictions_subscriber = None
+
+        self.user_stop_publisher = None
+        self.user_stop_state_to_publish = True
+        self.clutch_publisher = None
+
+        self.robots_list_timer = None
+        self.connection_timer = None
+        self.master_connection_timer = None
+        self.robot_stop_timer = None
+
+        self.time_of_last_msg_of_user_stop = [
+            None for i in range(6)
+        ]
+        self.user_stop_subscribers = [
+            None for i in range(6)
+        ]
+        self.robots_hz_value = [
+            None for i in range(6)
+        ]
+        
+        self.hz_update_timer = self.create_timer(0.1, self.update_taken_robots)
+
+        self._qos = QoSProfile(
+            reliability=QoSReliabilityPolicy.RELIABLE,
+            durability=QoSDurabilityPolicy.VOLATILE,
+            history=QoSHistoryPolicy.SYSTEM_DEFAULT,
+            liveliness=QoSLivelinessPolicy.AUTOMATIC,
+            depth=10,
+        )
+        self._create_user_stops_subscribers_and_callbacks()
+
+    def _create_user_stops_subscribers_and_callbacks(self):
+        for i in range(6):
+            self.user_stop_subscribers[i] = self.create_subscription(
+                Bool,
+                f"/pioneer{i+1}/user_stop",
+                lambda msg, i_in = i: self._update_user_stop_period(msg, i_in),
+                qos_profile=self._qos,
+            )
+
+    def _update_user_stop_period(self, msg, robot_id):
+        self.time_of_last_msg_of_user_stop[robot_id] = self.get_clock().now()
+
+    def setup_subscribers_and_publishers(self, robot_id):
+        self.robot_info_subscriber = self.create_subscription(
+            RobotInfoMsg,
+            f"/pioneer{robot_id}/robot_info",
+            self.handle_selected_robot_info_update,
+            qos_profile=self._qos,
+        )
+
+        self.master_stop_subscriber = self.create_subscription(
+            Bool,
+            "/pioneers/master_stop",
+            self.handle_master_stop_update,
+            qos_profile=self._qos,
+        )
+        self.restrictions_subscriber = self.create_subscription(
+            RestrictionsMsg,
+            "/pioneers/restrictions",
+            self.handle_restrictions_update,
+            qos_profile=self._qos,
+        )
+
+        self.user_stop_publisher = self.create_publisher(
+            Bool, f"/pioneer{robot_id}/user_stop", 10
+        )
+        self.clutch_publisher = self.create_publisher(
+            Bool, f"/pioneer{robot_id}/clutch", 10
+        )
+
+        self.shutdown_timer(self.connection_timer)
+        self.connection_timer = self.create_timer(
+            5.0, self.handle_robot_connection_lost
+        )
+
+        self.shutdown_timer(self.master_connection_timer)
+        self.master_connection_timer = self.create_timer(
+            1.5, self.handle_master_connection_lost
+        )
+
+        self.shutdown_timer(self.robot_stop_timer)
+        self.robot_stop_timer = self.create_timer(
+            0.1, self.publish_robot_stop
+        )
+
+    def unsubscribe(self, subscriber):
+        if subscriber is not None:
+            self.destroy_subscription(subscriber)
+            subscriber = None
+
+    def unregister_publisher(self, publisher):
+        while publisher is not None:
+            self.destroy_publisher(publisher)
+            publisher = None
+
+    def shutdown_timer(self, timer):
+        while timer is not None:
+            self.destroy_timer(timer)
+            timer = None
+
+    def publish_robot_stop(self):
+        if self.user_stop_publisher is not None:
+            msg = Bool()
+            msg.data = self.user_stop_state_to_publish
+            self.user_stop_publisher.publish(msg)
+    
+    def update_taken_robots(self):
+        for i, last_time in zip(range(6), self.time_of_last_msg_of_user_stop):
+            if last_time is None:
+                self.robots_hz_value[i] = None
+                continue
+
+            self.robots_hz_value[i] =  1.0/float((self.get_clock().now() - last_time).nanoseconds) * 10e9
+            if self.robots_hz_value[i] < 10.0:
+                self.robots_hz_value[i] = None
+
+    def handle_robot_connection_lost(self):
+        self.shutdown_timer(self.connection_timer)
+        self.robot_connection_lost_callback()
+
+    def handle_master_connection_lost(self):
+        self.shutdown_timer(self.master_connection_timer)
+        self.master_connection_lost_callback()
+
+    def restart_connection_timer(self):
+        self.shutdown_timer(self.connection_timer)
+        self.connection_timer = self.create_timer(
+            5.0, self.handle_robot_connection_lost
+        )
+
+    def restart_master_connection_timer(self):
+        self.shutdown_timer(self.master_connection_timer)
+        self.master_connection_timer = self.create_timer(
+            1.5, self.handle_master_connection_lost
+        )
+
+    def cancel_subscribers_and_publishers(self):
+        # self.shutdown_timer(self.periodic_timer)
+        self.unsubscribe(self.robot_info_subscriber)
+        self.unsubscribe(self.master_stop_subscriber)
+        self.unsubscribe(self.restrictions_subscriber)
+        self.unregister_publisher(self.user_stop_publisher)
+        self.unregister_publisher(self.clutch_publisher)
+
+    def unregister_node(self):
+        self.release_robot()
+        self.cancel_subscribers_and_publishers()
+        print("Unregister node.")
+
+    def select_robot(self, robot_id):
+        self.setup_subscribers_and_publishers(robot_id+1)
+
+    def setup_get_user_stop_state_function(self, function):
+        self.get_user_stop_state = function
+
+    def setup_get_clutch_state_function(self, function):
+        self.get_clutch_state = function
+
+    # ROSWrapper Callbacks
+    def get_robots_list_and_refresh_subscribers(self):
+        robots_id_list = self._get_robots_list()
+        self.robots_list_update_callback(robots_id_list)
+
+    def _get_robots_list(self):
+        robots_id_list = []
+        # NOTE: 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)
+
+        return robots_id_list
+
+    def release_robot(self):
+        # self.shutdown_timer(self.periodic_timer)
+        self.shutdown_timer(self.robot_stop_timer)
+        self.shutdown_timer(self.connection_timer)
+        self.shutdown_timer(self.master_connection_timer)
+
+        self.disengage_clutch()
+        self.user_robot_disabled()
+        self.unsubscribe(self.robot_info_subscriber)
+        self.unsubscribe(self.restrictions_subscriber)
+        self.unsubscribe(self.master_stop_subscriber)
+
+        self.unregister_publisher(self.user_stop_publisher)
+        self.unregister_publisher(self.clutch_publisher)
+
+
+    def handle_selected_robot_info_update(self, msg):
+        # Restarting connection timer to avoid raising robot_connection_lost_callback
+        self.restart_connection_timer()
+
+        _robot_info = RobotInfo(0)
+        _robot_info.update_robot_info(msg)
+        self.selected_robot_info_update_callback(_robot_info)
+
+    def handle_master_stop_update(self, msg):
+        # Restarting master connection timer to avoid raising master_connection_lost_callback
+        self.restart_master_connection_timer()
+
+        master_stop_state = SS.UNKNOWN
+        if msg.data is False:
+            master_stop_state = SS.RUNNING
+        else:
+            master_stop_state = SS.STOPPED
+
+        self.master_stop_update_callback(master_stop_state)
+
+    def handle_restrictions_update(self, msg):
+        restrictions = Restrictions(msg)
+        self.restrictions_update_callback(restrictions)
+
+    # UserPlugin Callbacks
+    def set_robots_list_update_callback(self, callback_function):
+        self.robots_list_update_callback = callback_function
+        self.robots_list_timer = self.create_timer(
+            0.5, self.get_robots_list_and_refresh_subscribers
+        )
+
+    def set_selected_robot_info_update_callback(self, callback_function):
+        self.selected_robot_info_update_callback = callback_function
+
+    def set_master_stop_update_callback(self, callback_function):
+        self.master_stop_update_callback = callback_function
+
+    def set_restrictions_update_callback(self, callback_function):
+        self.restrictions_update_callback = callback_function
+
+    def set_robot_connection_lost_callback(self, callback_function):
+        self.robot_connection_lost_callback = callback_function
+
+    def set_master_connection_lost_callback(self, callback_function):
+        self.master_connection_lost_callback = callback_function
+
+    def engage_clutch(self):
+        if self.clutch_publisher is not None:
+            msg = Bool()
+            msg.data = True
+            self.clutch_publisher.publish(msg)
+
+    def disengage_clutch(self):
+        if self.clutch_publisher is not None:
+            msg = Bool()
+            msg.data = False
+            self.clutch_publisher.publish(msg)
+
+    def user_robot_enabled(self):
+        if self.user_stop_publisher is not None:
+            self.user_stop_state_to_publish = False
+            msg = Bool()
+            msg.data = self.user_stop_state_to_publish
+            self.user_stop_publisher.publish(msg)
+
+
+    def user_robot_disabled(self):
+        if self.user_stop_publisher is not None:
+            self.user_stop_state_to_publish = True
+            msg = Bool()
+            msg.data = self.user_stop_state_to_publish
+            self.user_stop_publisher.publish(msg)
+
+    def qt_timer_callback(self):
+        rclpy.spin_once(self, timeout_sec=0.001)

safety_user_plugin/user_info.py

@@ -1,9 +1,9 @@
 #!/usr/bin/env python
 
-from robot_info import RobotInfo
+from safety_user_plugin.robot_info import RobotInfo
+
 
 class UserInfo:
-
     def __init__(self):
         self.selected_robot = None
         self.robots_id_list = []

safety_user_plugin/user_plugin.py

@@ -5,14 +5,15 @@ import os
 from qt_gui.plugin import Plugin
 from python_qt_binding.QtCore import QObject
 from python_qt_binding.QtCore import pyqtSignal
+from python_qt_binding.QtCore import QTimer
 
-from qt_wrapper import QtWrapper
-from ros_wrapper import ROSWrapper
-from user_widget import UserWidget
-from user_info import UserInfo
+from safety_user_plugin.qt_wrapper import QtWrapper
+from safety_user_plugin.ros_wrapper import ROSWrapper
+from safety_user_plugin.user_widget import UserWidget
+from safety_user_plugin.user_info import UserInfo
 
-from enums.clutch_state import ClutchState as CS
-from enums.stop_state import StopState as SS
+from safety_user_plugin.enums.clutch_state import ClutchState as CS
+from safety_user_plugin.enums.stop_state import StopState as SS
 
 
 class CallbackHandler(QObject):
@@ -30,11 +31,9 @@ class CallbackHandler(QObject):
 
 
 class UserPlugin(Plugin):
-
     def __init__(self, context):
         super(UserPlugin, self).__init__(context)
-        self.setObjectName('User Plugin - L1.5 safety system')
-        # setStyleSheet("background-color:black;")
+        self.setObjectName("User Plugin - L1.5 safety system")
         self.cbhandler = CallbackHandler()
 
         self._widget = UserWidget(context)
@@ -47,19 +46,31 @@ class UserPlugin(Plugin):
         self._user_info.clutch_state = CS.UNKNOWN
 
         # Setup functions to get _user_info states from _ros_wrapper
-        self._ros_wrapper.setup_get_user_stop_state_function(self._user_info.get_user_stop_state)
-        self._ros_wrapper.setup_get_clutch_state_function(self._user_info.get_clutch_state)
+        self._ros_wrapper.setup_get_user_stop_state_function(
+            self._user_info.get_user_stop_state
+        )
+        self._ros_wrapper.setup_get_clutch_state_function(
+            self._user_info.get_clutch_state
+        )
 
-        # self._ros_wrapper.setup_node()
         self.set_callback_functions()
 
         # At the end!
         self._qt_wrapper.connect_signals()
-        self._ros_wrapper.set_robots_list_update_callback(self.handle_robots_list_update)
+        self._ros_wrapper.set_robots_list_update_callback(
+            self.handle_robots_list_update
+        )
         self.cbhandler.signal.connect(self._qt_wrapper.handle_emitted_signal)
-        self.cbhandler.signal_with_list_argument.connect(self._qt_wrapper.handle_emitted_signal_with_list_argument)
-
+        self.cbhandler.signal_with_list_argument.connect(
+            self._qt_wrapper.handle_emitted_signal_with_list_argument
+        )
 
+        # 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)
 
     def call_qt_wrapper_method(self, method_name):
         self.cbhandler.signal.emit(method_name)
@@ -75,62 +86,70 @@ class UserPlugin(Plugin):
         self.set_ROS_callback_functions()
 
     def set_ROS_callback_functions(self):
-        self._ros_wrapper.set_selected_robot_info_update_callback(self.handle_selected_robot_info_update)
-        self._ros_wrapper.set_master_stop_update_callback(self.handle_master_stop_update)
-        self._ros_wrapper.set_restrictions_update_callback(self.handle_restrictions_update)
-        self._ros_wrapper.set_robot_connection_lost_callback(self.handle_robot_connection_lost)
-        self._ros_wrapper.set_master_connection_lost_callback(self.handle_master_connection_lost)
+        self._ros_wrapper.set_selected_robot_info_update_callback(
+            self.handle_selected_robot_info_update
+        )
+        self._ros_wrapper.set_master_stop_update_callback(
+            self.handle_master_stop_update
+        )
+        self._ros_wrapper.set_restrictions_update_callback(
+            self.handle_restrictions_update
+        )
+        self._ros_wrapper.set_robot_connection_lost_callback(
+            self.handle_robot_connection_lost
+        )
+        self._ros_wrapper.set_master_connection_lost_callback(
+            self.handle_master_connection_lost
+        )
 
     def set_Qt_callback_functions(self):
         self._qt_wrapper.set_robot_selection_callback(self.handle_robot_selection)
         self._qt_wrapper.set_robot_release_callback(self.handle_robot_release)
-        self._qt_wrapper.set_user_stop_state_updated_callback(self.handle_user_stop_state_updated)
+        self._qt_wrapper.set_user_stop_state_updated_callback(
+            self.handle_user_stop_state_updated
+        )
         self._qt_wrapper.set_clutch_switched_callback(self.handle_clutch_switched)
 
-       
-
-
     # ROS callback functions
     def handle_robots_list_update(self, robots_id_list):
         self._user_info.update_robots_id_list(robots_id_list)
         self._qt_wrapper.update_robots_list_gui(robots_id_list)
 
     def handle_selected_robot_info_update(self, robot_info):
-        if robot_info != None:
+        if robot_info is not None:
             self.update_selected_robot_info(robot_info)
         else:
-            raise RuntimeError('Updated robot_info is NoneType object')
+            raise RuntimeError("Updated robot_info is NoneType object")
 
     def handle_master_stop_update(self, master_stop_state):
-        if (master_stop_state != self._user_info.master_stop_state):
+        if master_stop_state != self._user_info.master_stop_state:
             self._user_info.master_stop_state = master_stop_state
             if master_stop_state == SS.STOPPED:
                 self.master_stopped()
             elif master_stop_state == SS.RUNNING:
                 self.master_started()
             else:
-                raise ValueError('Undefined master_stop_state received')           
+                raise ValueError("Undefined master_stop_state received")
 
     def handle_restrictions_update(self, restrictions):
         self._qt_wrapper.update_restrictions_gui(restrictions)
 
-
     # Qt callback functions
     def handle_robot_selection(self, robot_id):
-        if self._user_info.selected_robot == None:
-            if self._ros_wrapper.robots_hz_value[robot_id] == None:
-                    self.select_robot(robot_id)
+        if self._user_info.selected_robot is None:
+            if self._ros_wrapper.robots_hz_value[robot_id-1] is None:
+                self.select_robot(robot_id-1)
             else:
                 self._qt_wrapper.robot_selected_by_another_group_notify()
 
         else:
-            raise RuntimeError('User already selected robot')
+            raise RuntimeError("User already selected robot")
 
     def handle_robot_release(self):
         if self._user_info.selected_robot != None:
             self.release_robot()
         else:
-            raise RuntimeError('Cannot release. No robot selected')
+            raise RuntimeError("Cannot release. No robot selected")
 
     def handle_user_stop_state_updated(self):
         if self._user_info.selected_robot.obstacle_detected == True:
@@ -144,9 +163,9 @@ class UserPlugin(Plugin):
                 elif self._user_info.user_stop_state == SS.STOPPED:
                     self.user_started()
                 else:
-                    raise ValueError('user_stop_state value undefined')            
+                    self._qt_wrapper.log_master_inactive()
             else:
-                raise ValueError('master_stop_state value undefined') 
+                self._qt_wrapper.log_master_inactive()
 
     def handle_clutch_switched(self):
         if self._user_info.clutch_state == CS.ENGAGED:
@@ -154,14 +173,16 @@ class UserPlugin(Plugin):
         elif self._user_info.clutch_state == CS.DISENGAGED:
             self.engage_clutch()
         else:
-            raise ValueError('clutch_state value undefined') 
+            raise ValueError("clutch_state value undefined")
 
     def handle_robot_connection_lost(self):
         if self._user_info.selected_robot != None:
             lost_robot_id = self._user_info.selected_robot.robot_id
             self.connection_to_robot_lost(lost_robot_id)
         else:
-            raise RuntimeError('Connection lost callback received when no robot was selected')
+            raise RuntimeError(
+                "Connection lost callback received when no robot was selected"
+            )
 
     def handle_master_connection_lost(self):
         self.connection_to_master_lost()
@@ -172,44 +193,37 @@ class UserPlugin(Plugin):
 
         if self._user_info.selected_robot != None:
             self.user_stopped()
-            self.call_qt_wrapper_method('master_stopped')
-            # self._qt_wrapper.master_stopped()
+            self.call_qt_wrapper_method("master_stopped")
 
     def master_started(self):
         self._user_info.master_stop_state = SS.RUNNING
 
         if self._user_info.selected_robot != None:
-            self.call_qt_wrapper_method('master_started')
-            # self._qt_wrapper.master_started()
+            self.call_qt_wrapper_method("master_started")
 
     def user_stopped(self):
         self._user_info.user_stop_state = SS.STOPPED
-        self._ros_wrapper.user_stopped()
-        self.call_qt_wrapper_method('user_stopped')
-        # self._qt_wrapper.user_stopped()
+        self._ros_wrapper.user_robot_disabled()
+        self.call_qt_wrapper_method("user_stopped")
 
     def user_started(self):
         self._user_info.user_stop_state = SS.RUNNING
-        self._ros_wrapper.user_started()
-        self.call_qt_wrapper_method('user_started')
-        # self._qt_wrapper.user_started()
+        self._ros_wrapper.user_robot_enabled()
+        self.call_qt_wrapper_method("user_started")
 
     def engage_clutch(self):
         self._user_info.clutch_state = CS.ENGAGED
         self._ros_wrapper.engage_clutch()
-        self.call_qt_wrapper_method('engage_clutch')
-        # self._qt_wrapper.engage_clutch()
+        self.call_qt_wrapper_method("engage_clutch")
 
     def disengage_clutch(self):
         self._user_info.clutch_state = CS.DISENGAGED
         self._ros_wrapper.disengage_clutch()
-        self.call_qt_wrapper_method('disengage_clutch')
-        # self._qt_wrapper.disengage_clutch()
+        self.call_qt_wrapper_method("disengage_clutch")
 
     def select_robot(self, robot_id):
         self._ros_wrapper.select_robot(robot_id)
-        # self._qt_wrapper.select_robot(robot_id)
-        self.call_qt_wrapper_method_with_argument('select_robot',[robot_id])
+        self.call_qt_wrapper_method_with_argument("select_robot", [robot_id])
         self._user_info.select_robot(robot_id)
 
         # Stop robot and engage clutch after taking control over it to put it in starting state
@@ -223,37 +237,43 @@ class UserPlugin(Plugin):
 
     def release_robot(self):
         self._ros_wrapper.release_robot()
-        self.call_qt_wrapper_method('release_robot')
-        # self._qt_wrapper.release_robot()
+        self.call_qt_wrapper_method("release_robot")
         self._user_info.release_robot()
         self._user_info.master_stop_state = SS.UNKNOWN
 
     def connection_to_robot_lost(self, lost_robot_id):
-        # pass
         self._ros_wrapper.release_robot()
         self._user_info.release_robot()
-        self.call_qt_wrapper_method_with_argument('connection_to_robot_lost',[lost_robot_id])
-        # self._qt_wrapper.connection_to_robot_lost(lost_robot_id)
+        self.call_qt_wrapper_method_with_argument(
+            "connection_to_robot_lost", [lost_robot_id]
+        )
 
     def connection_to_master_lost(self):
-        if self._user_info.selected_robot != None:
+        if self._user_info.selected_robot is not None:
             self.master_stopped()
-            self.call_qt_wrapper_method('connection_to_master_lost')
+            self.call_qt_wrapper_method("connection_to_master_lost")
 
     def obstacle_detected(self):
-        self.call_qt_wrapper_method('obstacle_detected')
+        self.call_qt_wrapper_method("obstacle_detected")
         self.user_stopped()
 
     def obstacle_removed(self):
-        self.call_qt_wrapper_method('obstacle_removed')
+        self.call_qt_wrapper_method("obstacle_removed")
 
     def update_selected_robot_info(self, robot_info):
-        if self._user_info.selected_robot != None:
-            if robot_info.obstacle_detected == True and self._user_info.selected_robot.obstacle_detected == False:
+        if self._user_info.selected_robot is not None:
+            if (
+                robot_info.obstacle_detected is True
+                and self._user_info.selected_robot.obstacle_detected is False
+            ):
                 self.obstacle_detected()
-            elif robot_info.obstacle_detected == False and self._user_info.selected_robot.obstacle_detected == True:
+            elif (
+                robot_info.obstacle_detected is False
+                and self._user_info.selected_robot.obstacle_detected is True
+            ):
                 self.obstacle_removed()
 
         self._user_info.update_selected_robot_info(robot_info)
-        self.call_qt_wrapper_method_with_argument('update_selected_robot_info',[robot_info])
-        # self._qt_wrapper.update_selected_robot_info(robot_info)
+        self.call_qt_wrapper_method_with_argument(
+            "update_selected_robot_info", [robot_info]
+        )

safety_user_plugin/user_plugin_node.py

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

safety_user_plugin/user_widget.py

@@ -0,0 +1,22 @@
+#!/usr/bin/env python
+import os
+from ament_index_python.packages import get_package_share_directory
+
+from python_qt_binding.QtWidgets import QWidget
+from python_qt_binding import loadUi
+
+
+class UserWidget(QWidget):
+    def __init__(self, context):
+        super(UserWidget, self).__init__()
+        ui_file = os.path.join(
+            get_package_share_directory("safety_user_plugin"), "ui", "user_view.ui"
+        )
+        loadUi(ui_file, self)
+        self.setObjectName("User Plugin - L1.5 safety system")
+
+        if context.serial_number() > 1:
+            self.setWindowTitle(
+                self.windowTitle() + (" (%d)" % context.serial_number())
+            )
+        context.add_widget(self)

scripts/robot_info.py

@@ -1,32 +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
-        self.obstacle_detected = 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]
-        self.obstacle_detected = robot_info_msg.obstacle_detected.data
-
-        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_wrapper.py

@@ -1,269 +0,0 @@
-#!/usr/bin/env python
-import rospy
-import rospkg
-import rostopic
-
-from std_msgs.msg import Bool
-from rosaria_msgs.msg import RobotInfoMsg
-from rosaria_msgs.msg import RestrictionsMsg
-
-from robot_info import RobotInfo
-from restrictions import Restrictions
-
-from enums.clutch_state import ClutchState as CS
-from enums.stop_state import StopState as SS
-
-class ROSWrapper:
-
-    def __init__(self):
-        self.robots_list_update_callback = None
-        self.selected_robot_info_update_callback = None
-        self.master_stop_update_callback = None
-        self.restrictions_update_callback = None
-
-        self.get_user_stop_state = None
-        self.get_clutch_state = None
-
-        self.robot_info_subscriber = None
-        self.master_stop_subscriber = None
-        self.restrictions_subscriber = None
-
-        self.user_stop_publisher = None
-        self.clutch_publisher = None
-
-        self.periodic_timer = None
-        self.robots_list_timer = None
-        self.connection_timer = None
-        self.master_connection_timer = None
-        self.hz_update_timer = rospy.Timer(rospy.Duration(0.2),self.update_hz_values)
-
-        self.robots_hz_subscribers_dict = {}
-        self.rostopics_hz_dict = {}
-        self.robots_hz_value = {}
-
-    # def setup_node(self):
-    #     rospy.init_node('safety_user_plugin', anonymous=True)
-
-    def setup_subscribers_and_publishers(self,robot_id):
-        robot_name = 'PIONIER' + str(robot_id)
-
-        self.robot_info_subscriber = rospy.Subscriber('/{0}/RosAria/robot_info'.format(robot_name), RobotInfoMsg, self.handle_selected_robot_info_update)
-        self.master_stop_subscriber = rospy.Subscriber('/PIONIER/master_stop', Bool, self.handle_master_stop_update)
-        self.restrictions_subscriber = rospy.Subscriber('/PIONIER/restrictions', RestrictionsMsg, self.handle_restrictions_update)
-
-        self.user_stop_publisher = rospy.Publisher('/{0}/RosAria/user_stop'.format(robot_name),Bool,queue_size = 1)
-        self.clutch_publisher = rospy.Publisher('/{0}/RosAria/clutch'.format(robot_name),Bool,queue_size = 1)
-
-        if self.periodic_timer != None:
-            self.shutdown_timer(self.periodic_timer)
-        self.periodic_timer = rospy.Timer(rospy.Duration(0.1),self.publish_periodic_update)
-        print("NEW")
-        if self.connection_timer != None:
-            self.shutdown_timer(self.connection_timer)        
-        self.connection_timer = rospy.Timer(rospy.Duration(5),self.handle_robot_connection_lost)
-
-        if self.master_connection_timer != None:
-            self.shutdown_timer(self.master_connection_timer)       
-        self.master_connection_timer = rospy.Timer(rospy.Duration(1.5),self.handle_master_connection_lost)
-
-        if self.hz_update_timer != None:
-            self.shutdown_timer(self.hz_update_timer)       
-        self.hz_update_timer = rospy.Timer(rospy.Duration(0.2),self.update_hz_values)
-
-    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):
-        stop_state = self.get_user_stop_state()
-        # clutch_state = self.get_clutch_state()
-
-        if stop_state == SS.RUNNING:
-            self.user_started()
-        elif stop_state == SS.STOPPED:
-            self.user_stopped()
-        else:
-            raise ValueError('stop_state UNKNOWN when attempting to publish periodic update')
-
-        # if clutch_state == CS.ENGAGED:
-        #     self.engage_clutch()
-        # elif clutch_state == CS.DISENGAGED:
-        #     self.disengage_clutch()
-        # else:
-        #     raise ValueError('clutch_state UNKNOWN when attempting to publish periodic update')
-
-    def update_hz_values(self,event):
-        for key in self.rostopics_hz_dict:
-            self.robots_hz_value[key] = self.rostopics_hz_dict[key].get_hz('/PIONIER{0}/RosAria/user_stop'.format(key))
-
-
-    def handle_robot_connection_lost(self,event):
-        self.shutdown_timer(self.connection_timer)
-        self.robot_connection_lost_callback()
-
-    def handle_master_connection_lost(self,event):
-        self.shutdown_timer(self.master_connection_timer)
-        self.master_connection_lost_callback()
-
-    def cancel_subscribers_and_publishers(self):
-        self.shutdown_timer(self.periodic_timer)
-        self.unsubscribe(self.robot_info_subscriber)
-        self.unsubscribe(self.master_stop_subscriber)
-        self.unsubscribe(self.restrictions_subscriber)
-        self.unregister_publisher(self.user_stop_publisher)
-        self.unregister_publisher(self.clutch_publisher)
-
-        self.periodic_timer = None
-        self.robot_info_subscriber = None
-        self.master_stop_subscriber = None
-        self.restrictions_subscriber = None
-        self.user_stop_publisher = None
-        self.clutch_publisher = None
-
-    def restart_connection_timer(self):
-        if self.connection_timer != None:
-            self.connection_timer.shutdown()
-            self.connection_timer = rospy.Timer(rospy.Duration(1.5),self.handle_robot_connection_lost)
-        else:
-            raise RuntimeError('Attempting to restart connection_timer when it is not initialized')
-
-    def restart_master_connection_timer(self):
-        if self.master_connection_timer != None:
-            self.master_connection_timer.shutdown()
-            self.master_connection_timer = rospy.Timer(rospy.Duration(1.5),self.handle_master_connection_lost)
-        else:
-            raise RuntimeError('Attempting to restart master_connection_timer when it is not initialized')
-
-    def unregister_node(self):
-        self.cancel_subscribers_and_publishers()
-        rospy.signal_shutdown('Closing safety user plugin')
-
-    def select_robot(self,robot_id):
-        self.setup_subscribers_and_publishers(robot_id)
-        
-    def setup_get_user_stop_state_function(self,function):
-        self.get_user_stop_state = function
-
-    def setup_get_clutch_state_function(self,function):
-        self.get_clutch_state = 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)
-
-
-        # Adding new hz subscribers
-        for robot_id in robots_id_list:
-            if robot_id not in self.robots_hz_subscribers_dict:
-                # Add hz subscriber
-                self.rostopics_hz_dict[robot_id] = rostopic.ROSTopicHz(-1)
-                self.robots_hz_subscribers_dict[robot_id] = rospy.Subscriber('/PIONIER{0}/RosAria/user_stop'.format(robot_id),Bool,
-                    self.rostopics_hz_dict[robot_id].callback_hz,callback_args='/PIONIER{0}/RosAria/user_stop'.format(robot_id))
-
-        # Removing old hz subscribers
-        for robot_key in self.robots_hz_subscribers_dict:
-            if robot_key not in robots_id_list:
-                # Remove old subscriber
-                self.unsubscribe(self.robots_hz_subscribers_dict[robot_id])
-                self.robots_hz_subscribers_dict[robot_id] = None
-                self.rostopics_hz_dict[robot_id] = None
-
-
-        self.robots_list_update_callback(robots_id_list)
-
-
-    def release_robot(self):
-        self.unsubscribe(self.robot_info_subscriber)
-        self.shutdown_timer(self.periodic_timer)
-        self.shutdown_timer(self.connection_timer)
-        self.shutdown_timer(self.master_connection_timer)
-
-    def handle_selected_robot_info_update(self,msg):
-        # Restarting connection timer to avoid raising robot_connection_lost_callback
-        self.restart_connection_timer()
-
-        _robot_info = RobotInfo(0)
-        _robot_info.update_robot_info(msg)
-        self.selected_robot_info_update_callback(_robot_info)
-
-    def handle_master_stop_update(self,msg):
-        # Restarting master connection timer to avoid raising master_connection_lost_callback
-        self.restart_master_connection_timer()
-
-        master_stop_state = SS.UNKNOWN
-        if msg.data == True:
-            master_stop_state = SS.RUNNING
-        else:
-            master_stop_state = SS.STOPPED
-
-        self.master_stop_update_callback(master_stop_state)
-
-    def handle_restrictions_update(self,msg):
-        restrictions = Restrictions(msg)
-        self.restrictions_update_callback(restrictions)
-
-    # UserPlugin 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_selected_robot_info_update_callback(self,callback_function):
-        self.selected_robot_info_update_callback = callback_function
-
-    def set_master_stop_update_callback(self,callback_function):
-        self.master_stop_update_callback = callback_function
-
-    def set_restrictions_update_callback(self,callback_function):
-        self.restrictions_update_callback = callback_function
-
-    def set_robot_connection_lost_callback(self,callback_function):
-        self.robot_connection_lost_callback = callback_function
-
-    def set_master_connection_lost_callback(self,callback_function):
-        self.master_connection_lost_callback = callback_function
-
-    def engage_clutch(self):
-        msg = Bool()
-        msg.data = True
-        self.clutch_publisher.publish(msg)
-
-    def disengage_clutch(self):
-        msg = Bool()
-        msg.data = False
-        self.clutch_publisher.publish(msg)
-
-    def user_started(self):
-        msg = Bool()
-        msg.data = True
-        self.user_stop_publisher.publish(msg)
-
-    def user_stopped(self):
-        msg = Bool()
-        msg.data = False
-        self.user_stop_publisher.publish(msg)
-

scripts/safety_user_plugin

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

scripts/user_widget.py

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

setup.cfg

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

setup.py

@@ -1,8 +1,31 @@
-from distutils.core import setup
-from catkin_pkg.python_setup import generate_distutils_setup
+import os
+from glob import glob
+from setuptools import setup
 
-setup_args = generate_distutils_setup(
-    package_dir={'': 'scripts'}
+
+package_name = 'safety_user_plugin'
+submodules = 'safety_user_plugin/enums'
+setup(
+    name=package_name,
+    version='0.2.0',
+    packages=[package_name, submodules],
+    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': ['user_plugin_node = safety_user_plugin.user_plugin_node:main'],
+    },
 )
-
-setup(**setup_args)