.pre-commit-config.yaml

@@ -1,14 +0,0 @@
-# .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

@@ -0,0 +1,204 @@
+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,39 +1,35 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>safety_user_plugin</name>
-  <version>0.2.0</version>
+  <version>0.0.0</version>
   <description>The safety_user_plugin package</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>
 
-  <build_depend>rclpy</build_depend>
+  <build_depend>rospy</build_depend>
   <build_depend>rqt_gui</build_depend>
   <build_depend>rqt_gui_py</build_depend>
   <build_depend>std_msgs</build_depend>
-  <build_depend>ros2aria_msgs</build_depend>
+  <build_depend>rosaria_msgs</build_depend>
   
-  <build_export_depend>rclpy</build_export_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>ros2aria_msgs</build_export_depend>
+  <build_export_depend>rosaria_msgs</build_export_depend>
 
-  <exec_depend>rclpy</exec_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>ros2aria_msgs</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_user_plugin" type="safety_user_plugin.user_plugin.UserPlugin" base_class_type="rqt_gui_py::Plugin">
+  <class name="safety_user_plugin" type="user_plugin.UserPlugin" base_class_type="rqt_gui_py::Plugin">
     <description>
       Safety system user plugin
     </description>
     <qtgui>
       <group>
-        <label>L1.5 Safety system</label>
+        <label>Safety system</label>
         <icon type="theme">folder</icon>
       </group>
       <label>User</label>

safety_user_plugin/robot_info.py

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

@@ -1,311 +0,0 @@
-#!/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_plugin_node.py

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

@@ -1,22 +0,0 @@
-#!/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/english.py

@@ -1,16 +1,16 @@
 #!/usr/bin/env python
 # coding=utf-8
 
-from safety_user_plugin.language import LanguageTexts
+from 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,39 +18,35 @@ 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 = "PIONEER{0} selected!"
+    robot_selected_text = 'PIONIER{0} selected!'
-    robot_released_text = "Robot released"
+    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_clicked_text = 'MasterSTOP button pressed. Stopping the robots'
-    master_stop_released_text = "MasterSTOP button released. Robot can be unlocked"
+    master_stop_released_text = 'MasterSTOP button released. Robot can be unlocked'
 
-    robot_stopped_text = "Robot stopped"
+    robot_stopped_text = 'Robot stopped'
-    robot_started_text = "Robot started"
+    robot_started_text = 'Robot started'
 
-    clutch_disengaged_text = "Disengaging the clutch"
+    clutch_disengaged_text = 'Disengaging the clutch'
-    clutch_engaged_text = "Engaging the clutch"
+    clutch_engaged_text = 'Engaging the clutch'
+
+    obstacle_detected_text = 'Obstacle detected. Robot stopped'
+    obstacle_removed_text = 'Obstacle removed'
 
-    obstacle_detected_text = "Obstacle detected. Robot stopped"
-    obstacle_removed_text = "Obstacle removed"
     
     # Logger error/problem messages
-    master_plugin_inactive_text = (
+    selection_error_text = 'Select the PIONEER from robots list first'
-        "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 = (
+    cannot_select_robot_text = "Robot can't be selected. It is already selected by another group"
-        "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'
-    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>'
@@ -63,5 +59,7 @@ 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

scripts/polish.py

@@ -1,16 +1,16 @@
 #!/usr/bin/env python
 # coding=utf-8
 
-from safety_user_plugin.language import LanguageTexts
+from 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,45 +18,35 @@ 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 = "PIONEER{0} wybrany!"
+    robot_selected_text = 'PIONIER{0} wybrany!'
-    robot_released_text = "Odłączam robota"
+    robot_released_text = 'Odłączam robota'
     stopped_and_engaged_text = "Robot zostanie zatrzymany i sprzęgnięty"   
 
-    master_stop_clicked_text = (
+    master_stop_clicked_text = 'Przycisk masterSTOP został naciśnięty. Zatrzymuje roboty'
-        "Przycisk masterSTOP został naciśnięty. Zatrzymuje roboty"
+    master_stop_released_text = 'Przycisk masterSTOP odciśnięty. Mozesz uruchomić robota'
-    )
-    master_stop_released_text = (
-        "Przycisk masterSTOP odciśnięty. Mozesz uruchomić robota"
-    )
 
-    robot_stopped_text = "Robot zatrzymany"
+    robot_stopped_text = 'Robot zatrzymany'
-    robot_started_text = "Robot wystartował"
+    robot_started_text = 'Robot wystartował'
 
-    clutch_disengaged_text = "Rozprzęgam sprzęgło"
+    clutch_disengaged_text = 'Rozprzęgam sprzęgło'
-    clutch_engaged_text = "Sprzęgam sprzegło"
+    clutch_engaged_text = 'Sprzęgam sprzegło'
+
+    obstacle_detected_text = 'Przeszkoda wykryta. Zatrzymuję robota'
+    obstacle_removed_text = 'Przeszkoda usunięta'
 
-    obstacle_detected_text = "Przeszkoda wykryta. Zatrzymuję robota"
-    obstacle_removed_text = "Przeszkoda usunięta"
 
     # Logger error/problem messages
-    master_plugin_inactive_text = (
+    selection_error_text = 'Najpierw wybierz PIONIERA z listy robotów'
-        "Master nieaktywny. Zapytaj prowadzącego o odpalenie programu."
+    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'
-    selection_error_text = "Najpierw wybierz PIONEERA z listy robotów"
+    cannot_select_robot_text = 'Nie mozna wybrać robota. Robot został już wybrany przez inną grupę'
-    cannot_start_masterstop_text = "Nie można wystartować robota. MasterSTOP wciśnięty!"
+    connection_to_robot_lost_text = 'Utrata połączenia z robotem PIONIER{0}'
-    cannot_start_obstacle_text = (
+    connection_to_master_lost_text = 'Utrata połączenia z masterstopem. Poproś prowadzącego o jego uruchomienie'
-        "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>'

scripts/qt_wrapper.py

@@ -2,54 +2,53 @@
 # 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 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
 
 from python_qt_binding.QtGui import QPixmap
 from python_qt_binding.QtGui import QTextCursor
 from python_qt_binding.QtWidgets import QMessageBox
 
-from safety_user_plugin.polish import PolishTexts as PT
+from polish import PolishTexts as PT
-from safety_user_plugin.english import EnglishTexts as ET
+from english import EnglishTexts as ET
-
 
 class QtWrapper:
+
     def __init__(self,widget):
         self.widget = widget
         self.displayed_robots_id_list = []
 
-        lang = os.getenv("LANG")
+        lang = rospy.get_param('lang')
 
-        if lang[:2] == "pl":
+        if lang == 'eng':
-            self.language = PT
-            self.log_info("Polska wersja pluginu uruchomiona")
-        else:
             self.language = ET
-            self.log_info("Started english version of plugin NEW")
+            self.log_info('Started english version of plugin NEW')
+        elif lang == 'pol':
+            self.language = PT
+            self.log_info('Polska wersja pluginu uruchomiona')
+        else:
+            raise ValueError('language parameter has value different than "eng" or "pol"')
 
         self.clear_robot_info()
         
-        ui_directory_path = "{0}/ui".format(
+        ui_directory_path = '{0}/ui'.format(rospkg.RosPack().get_path('safety_user_plugin'))
-            get_package_share_directory("safety_user_plugin")
-        )
 
-        self.distance_pixmap = QPixmap("{0}/Distance.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.angular_pixmap = QPixmap('{0}/Angular.png'.format(ui_directory_path))
-        self.linear_pixmap = QPixmap("{0}/Linear.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.png".format(ui_directory_path))
+        self.ok_pixmap = QPixmap('{0}/Ok.jpg'.format(ui_directory_path))
-        self.cancel_pixmap = QPixmap("{0}/Cancel.png".format(ui_directory_path))
+        self.cancel_pixmap = QPixmap('{0}/Cancel.jpg'.format(ui_directory_path))
 
     def disconnect_signals(self):
         self.widget.clutchButton.clicked.disconnect()
@@ -57,9 +56,7 @@ class QtWrapper:
         self.widget.choiceButton.clicked.disconnect()
 
     def connect_robot_signals(self):
-        self.widget.clutchButton.clicked.connect(
+        self.widget.clutchButton.clicked.connect(self.handle_clutchButton_clicked_callback)
-            self.handle_clutchButton_clicked_callback
-        )
         self.widget.stopButton.clicked.connect(self.handle_stopButton_clicked)
         self.widget.choiceButton.clicked.connect(self.handle_closeButton_clicked)
 
@@ -68,76 +65,62 @@ 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(
+            reply = QMessageBox.warning(self.widget,self.language.warning_text_1,self.language.warning_text_2,QMessageBox.Yes,QMessageBox.No)
-                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 "PIONEER" in current_text:
+        if 'PIONIER' 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(
+        self.widget.clutchButton.setStyleSheet("QPushButton { color: black; background-color: green; font: bold 20px}")
-            "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(
+        self.widget.clutchButton.setStyleSheet("QPushButton { color: black; background-color: red; font: bold 20px}")
-            "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(
+        self.widget.stopButton.setStyleSheet("QPushButton { color: black; background-color: green; font: bold 20px}")
-            "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(
+        self.widget.stopButton.setStyleSheet("QPushButton { color: black; background-color: red; font: bold 20px}")
-            "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()
 
@@ -164,7 +147,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+1))
+        self.log_info(self.language.robot_selected_text.format(robot_id)) 
         self.log_info(self.language.stopped_and_engaged_text)
 
         self.widget.choiceButton.setText(self.language.release_robot_text)
@@ -177,7 +160,7 @@ class QtWrapper:
 
     def update_robots_list_gui(self,robots_id_list):
         robots_id_list.sort()
-        id_strings_list = (("PIONEER" + str(x)) for x in robots_id_list)
+        id_strings_list = (('PIONIER'+str(x)) for x in robots_id_list)
 
         robots_to_add = []
         robots_to_remove = []
@@ -196,6 +179,7 @@ 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()
 
@@ -205,21 +189,19 @@ class QtWrapper:
                 self.displayed_robots_id_list.remove(robot_id)
                 return
             
+
     def add_robot_to_list(self,robot_id):
-        self.widget.robotsList.addItem("PIONEER{0}".format(robot_id))
+        self.widget.robotsList.addItem('PIONIER{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
-        )
 
-        self.widget.idLabel.setText("PIONEER{0}".format(robot_info.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)
+
+        self.widget.idLabel.setText('PIONIER{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(
+        self.widget.angularLabel.setText("{:.2f}".format(robot_info.angular_velocity[2]))
-            "{:.2f}".format(robot_info.angular_velocity[2])
-        )
 
         # self.log_info(str(robot_info.clutch == CS.ENGAGED))
         
@@ -272,51 +254,39 @@ class QtWrapper:
         self.log_error(self.language.cannot_select_robot_text)
 
     def update_restrictions_gui(self,restrictions):
-        self.widget.distanceRestrictionLabel.setText(
+        self.widget.distanceRestrictionLabel.setText("{:.2f}".format(restrictions.distance))
-            "{:.2f}".format(restrictions.distance)
+        self.widget.linearRestrictionLabel.setText("{:.2f}".format(restrictions.linear_velocity))
-        )
+        self.widget.angularRestrictionLabel.setText("{:.2f}".format(restrictions.angular_velocity))       
-        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(
+        self.widget.logsBrowser.insertHtml('<font color="black"> {0} {1}</font><br>'.format(time,info_text))
-            '<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(
+        self.widget.logsBrowser.insertHtml('<font color="orange"> {0} {1}</font><br>'.format(time,warning_text))
-            '<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(
+        self.widget.logsBrowser.insertHtml('<font color="red"> {0} {1}</font><br>'.format(time,error_text))
-            '<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)
 
@@ -341,20 +311,20 @@ class QtWrapper:
         scrollbar.setValue(scrollbar.maximum())
 
     def clear_robot_info(self):
-        self.widget.idLabel.setText("-")
+        self.widget.idLabel.setText('-')
-        self.widget.batteryLabel.setText("-")
+        self.widget.batteryLabel.setText('-')
-        self.widget.linearLabel.setText("-")
+        self.widget.linearLabel.setText('-')
-        self.widget.angularLabel.setText("-")
+        self.widget.angularLabel.setText('-')
-        self.widget.stateLabel.setText("-")
+        self.widget.stateLabel.setText('-')
-        self.widget.masterstopLabel.setText("-")
+        self.widget.masterstopLabel.setText('-')
-        self.widget.obstaclestopLabel.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)

scripts/robot_info.py

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

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

@@ -1,9 +0,0 @@
-#!/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_info.py

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

scripts/user_plugin.py

@@ -5,15 +5,14 @@ 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 safety_user_plugin.qt_wrapper import QtWrapper
+from qt_wrapper import QtWrapper
-from safety_user_plugin.ros_wrapper import ROSWrapper
+from ros_wrapper import ROSWrapper
-from safety_user_plugin.user_widget import UserWidget
+from user_widget import UserWidget
-from safety_user_plugin.user_info import UserInfo
+from user_info import UserInfo
 
-from safety_user_plugin.enums.clutch_state import ClutchState as CS
+from enums.clutch_state import ClutchState as CS
-from safety_user_plugin.enums.stop_state import StopState as SS
+from enums.stop_state import StopState as SS
 
 
 class CallbackHandler(QObject):
@@ -31,9 +30,11 @@ class CallbackHandler(QObject):
 
 
 class UserPlugin(Plugin):
+
     def __init__(self,context):
         super(UserPlugin, self).__init__(context)
-        self.setObjectName("User Plugin - L1.5 safety system")
+        self.setObjectName('User Plugin - L1.5 safety system')
+        # setStyleSheet("background-color:black;")
         self.cbhandler = CallbackHandler()
 
         self._widget = UserWidget(context)
@@ -46,31 +47,19 @@ 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._ros_wrapper.setup_get_user_stop_state_function(self._user_info.get_user_stop_state)
-            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_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._ros_wrapper.set_robots_list_update_callback(self.handle_robots_list_update)
-            self.handle_robots_list_update
-        )
         self.cbhandler.signal.connect(self._qt_wrapper.handle_emitted_signal)
-        self.cbhandler.signal_with_list_argument.connect(
+        self.cbhandler.signal_with_list_argument.connect(self._qt_wrapper.handle_emitted_signal_with_list_argument)
-            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)
@@ -86,70 +75,62 @@ class UserPlugin(Plugin):
         self.set_ROS_callback_functions()
         
     def set_ROS_callback_functions(self):
-        self._ros_wrapper.set_selected_robot_info_update_callback(
+        self._ros_wrapper.set_selected_robot_info_update_callback(self.handle_selected_robot_info_update)
-            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_master_stop_update_callback(
+        self._ros_wrapper.set_robot_connection_lost_callback(self.handle_robot_connection_lost)
-            self.handle_master_stop_update
+        self._ros_wrapper.set_master_connection_lost_callback(self.handle_master_connection_lost)
-        )
-        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._qt_wrapper.set_user_stop_state_updated_callback(self.handle_user_stop_state_updated)
-            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 is not None:
+        if robot_info != 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 is None:
+        if self._user_info.selected_robot == None:
-            if self._ros_wrapper.robots_hz_value[robot_id-1] is None:
+            if self._ros_wrapper.robots_hz_value[robot_id] == None:
-                self.select_robot(robot_id-1)
+                    self.select_robot(robot_id)
             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:
@@ -163,9 +144,9 @@ class UserPlugin(Plugin):
                 elif self._user_info.user_stop_state == SS.STOPPED:
                     self.user_started()
                 else:
-                    self._qt_wrapper.log_master_inactive()
+                    raise ValueError('user_stop_state value undefined')            
             else:
-                self._qt_wrapper.log_master_inactive()
+                raise ValueError('master_stop_state value undefined') 
 
     def handle_clutch_switched(self):
         if self._user_info.clutch_state == CS.ENGAGED:
@@ -173,16 +154,14 @@ 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(
+            raise RuntimeError('Connection lost callback received when no robot was selected')
-                "Connection lost callback received when no robot was selected"
-            )
 
     def handle_master_connection_lost(self):
         self.connection_to_master_lost()
@@ -193,37 +172,44 @@ class UserPlugin(Plugin):
         
         if self._user_info.selected_robot != None:
             self.user_stopped()
-            self.call_qt_wrapper_method("master_stopped")
+            self.call_qt_wrapper_method('master_stopped')
+            # self._qt_wrapper.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.call_qt_wrapper_method('master_started')
+            # self._qt_wrapper.master_started()
 
     def user_stopped(self):
         self._user_info.user_stop_state = SS.STOPPED
-        self._ros_wrapper.user_robot_disabled()
+        self._ros_wrapper.user_stopped()
-        self.call_qt_wrapper_method("user_stopped")
+        self.call_qt_wrapper_method('user_stopped')
+        # self._qt_wrapper.user_stopped()
 
     def user_started(self):
         self._user_info.user_stop_state = SS.RUNNING
-        self._ros_wrapper.user_robot_enabled()
+        self._ros_wrapper.user_started()
-        self.call_qt_wrapper_method("user_started")
+        self.call_qt_wrapper_method('user_started')
+        # self._qt_wrapper.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.call_qt_wrapper_method('engage_clutch')
+        # self._qt_wrapper.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.call_qt_wrapper_method('disengage_clutch')
+        # self._qt_wrapper.disengage_clutch()
 
     def select_robot(self,robot_id):
         self._ros_wrapper.select_robot(robot_id)
-        self.call_qt_wrapper_method_with_argument("select_robot", [robot_id])
+        # self._qt_wrapper.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
@@ -237,43 +223,37 @@ class UserPlugin(Plugin):
 
     def release_robot(self):
         self._ros_wrapper.release_robot()
-        self.call_qt_wrapper_method("release_robot")
+        self.call_qt_wrapper_method('release_robot')
+        # self._qt_wrapper.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(
+        self.call_qt_wrapper_method_with_argument('connection_to_robot_lost',[lost_robot_id])
-            "connection_to_robot_lost", [lost_robot_id]
+        # self._qt_wrapper.connection_to_robot_lost(lost_robot_id)
-        )
 
     def connection_to_master_lost(self):
-        if self._user_info.selected_robot is not None:
+        if self._user_info.selected_robot != 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 is not None:
+        if self._user_info.selected_robot != None:
-            if (
+            if robot_info.obstacle_detected == True and self._user_info.selected_robot.obstacle_detected == False:
-                robot_info.obstacle_detected is True
-                and self._user_info.selected_robot.obstacle_detected is False
-            ):
                 self.obstacle_detected()
-            elif (
+            elif robot_info.obstacle_detected == False and self._user_info.selected_robot.obstacle_detected == True:
-                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(
+        self.call_qt_wrapper_method_with_argument('update_selected_robot_info',[robot_info])
-            "update_selected_robot_info", [robot_info]
+        # self._qt_wrapper.update_selected_robot_info(robot_info)
-        )

scripts/user_widget.py

@@ -0,0 +1,18 @@
+#!/usr/bin/env python
+import os
+import rospkg
+
+from python_qt_binding.QtWidgets import QWidget
+from python_qt_binding import loadUi
+
+class 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

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

setup.py

@@ -1,31 +1,8 @@
-import os
+from distutils.core import setup
-from glob import glob
+from catkin_pkg.python_setup import generate_distutils_setup
-from setuptools import setup
 
-
+setup_args = generate_distutils_setup(
-package_name = 'safety_user_plugin'
+    package_dir={'': 'scripts'}
-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)