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

package.xml

@@ -1,24 +1,35 @@
 <?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="2">
-<package format="3">
   <name>safety_master_plugin</name>
-  <version>0.2.0</version>
+  <version>0.0.0</version>
-  <description>TODO: Package description</description>
+  <description>The safety_master_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>
 
-  <depend>rclpy</depend>
+  <buildtool_depend>catkin</buildtool_depend>
-  <depend>rqt_gui</depend>
-  <depend>rqt_gui_py</depend>
 
-  <depend>std_msgs</depend>
+  <build_depend>rospy</build_depend>
-  <depend>ros2aria_msgs</depend>
+  <build_depend>rqt_gui</build_depend>
+  <build_depend>rqt_gui_py</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>rosaria_msgs</build_depend>
+
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>rqt_gui</build_export_depend>
+  <build_export_depend>rqt_gui_py</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>rosaria_msgs</build_export_depend>
+
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>rqt_gui</exec_depend>
+  <exec_depend>rqt_gui_py</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+  <exec_depend>rosaria_msgs</exec_depend>
 
   <export>
-    <build_type>ament_python</build_type>
     <rqt_gui plugin="${prefix}/plugin.xml"/>
   </export>
 </package>

plugin.xml

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

safety_master_plugin/enums/clutch_state.py

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

safety_master_plugin/master_plugin.py

@@ -1,127 +0,0 @@
-from qt_gui.plugin import Plugin
-from python_qt_binding.QtCore import QTimer
-
-from safety_master_plugin.master_restrictions import Restrictions
-
-from safety_master_plugin.master_info import MasterInfo
-from safety_master_plugin.master_widget import MasterWidget
-from safety_master_plugin.stop_state import StopState as SS
-from safety_master_plugin.master_callback_handler import CallbackHandler
-from safety_master_plugin.ros_master_wrapper import ROSWrapper
-from safety_master_plugin.qt_master_wrapper import QtWrapper
-
-
-class MasterPlugin(Plugin):
-    def __init__(self, context):
-        super(MasterPlugin, self).__init__(context)
-        self.setObjectName("Master Plugin - L1.5 safety system")
-
-        self._widget = MasterWidget(context)
-        self._ros_wrapper = ROSWrapper()
-        self._qt_wrapper = QtWrapper(self._widget)
-        self.set_Qt_callback_functions()
-
-        self._master_info = MasterInfo()
-
-        self.cbhandler = CallbackHandler()
-        self.cbhandler.connect_robot_info_signal(self._qt_wrapper.update_robot_info)
-        self.cbhandler.connect_add_robot_signal(self._qt_wrapper.add_robot)
-        self.cbhandler.connect_remove_robot_signal(self._qt_wrapper.remove_robot)
-
-        self._qt_wrapper.connect_signals()
-        self._qt_wrapper.set_sliders_to_initial_values()
-
-        # Setup functions to get _master_info states from _ros_wrapper
-        self._ros_wrapper.setup_get_master_stop_state_function(
-            self._master_info.get_master_stop_state
-        )
-        self._ros_wrapper.setup_get_restrictions_function(
-            self._master_info.get_restrictions
-        )
-
-        self.set_callback_functions()
-
-        # timer to consecutively send twist messages
-        self._update_parameter_timer = QTimer(self)
-        self._update_parameter_timer.timeout.connect(
-            self._ros_wrapper.qt_timer_callback
-        )
-        self._update_parameter_timer.start(100)
-
-        # Stopping master at the beginning
-        self.disable_robots()
-
-    def set_callback_functions(self):
-        self.set_ROS_callback_functions()
-        self.set_Qt_callback_functions()
-
-    def set_ROS_callback_functions(self):
-        self._ros_wrapper.set_robots_list_update_callback(
-            self.handle_robots_list_update
-        )
-        self._ros_wrapper.set_robot_info_update_callback(self.handle_robot_info_update)
-
-    def set_Qt_callback_functions(self):
-        self._qt_wrapper.set_master_stop_state_updated_callback(
-            self.handle_master_stop_state_updated
-        )
-        self._qt_wrapper.set_restrictions_updated_callback(
-            self.handle_restrictions_updated
-        )
-
-    def handle_robots_list_update(self, robots_id_list):
-        self.update_robots_list(robots_id_list)
-
-    def handle_robot_info_update(self, robot_info):
-        self.update_robot_info(robot_info)
-
-    def handle_master_stop_state_updated(self):
-        """Handles master stop state when it changes e. g. on button click and confirmed."""
-        if self._master_info.master_stop_state == SS.STOPPED:
-            self.enable_robots()
-        elif self._master_info.master_stop_state == SS.RUNNING:
-            self.disable_robots()
-        else:
-            raise ValueError("master_stop_state value undefined")
-
-    def handle_restrictions_updated(self, restrictions):
-        self.restrictions_changed(restrictions)
-
-    def update_robots_list(self, robots_id_list):
-        for robot_id in robots_id_list:
-            if robot_id not in self._master_info.robots_id_list:
-                self.new_robot_registered(robot_id)
-
-        for robot_id in self._master_info.robots_id_list:
-            if robot_id not in robots_id_list:
-                self.robot_unregistered(robot_id)
-
-        self._master_info.robots_id_list = robots_id_list
-
-    def new_robot_registered(self, robot_id):
-        self._ros_wrapper.add_robot_subscriber(robot_id)
-        self.cbhandler.add_robot_signal.emit(robot_id)
-
-    def robot_unregistered(self, robot_id):
-        self._ros_wrapper.remove_robot_subscriber(robot_id)
-        self.cbhandler.remove_robot_signal.emit(robot_id)
-
-    def update_robot_info(self, robot_info):
-        self.cbhandler.robot_info_signal.emit(robot_info)
-
-    def enable_robots(self):
-        self._master_info.master_stop_state = SS.RUNNING
-        self._ros_wrapper.when_robots_enabled()
-        self._qt_wrapper.when_robots_enabled()
-
-    def disable_robots(self):
-        self._master_info.master_stop_state = SS.STOPPED
-        self._ros_wrapper.when_robots_disabled()
-        self._qt_wrapper.when_robots_disabled()
-
-    def restrictions_changed(self, restrictions):
-        self._master_info.restrictions = restrictions
-        self._ros_wrapper.restrictions_changed(restrictions)
-
-    def shutdown_plugin(self):
-        self._ros_wrapper.unregister_node()

safety_master_plugin/master_plugin_node.py

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

safety_master_plugin/robot_info.py

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

safety_master_plugin/ros_master_wrapper.py

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

safety_master_plugin/setup.cfg

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

safety_master_plugin/stop_state.py

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

scripts/master_callback_handler.py

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

scripts/master_plugin.py

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

scripts/master_widget.py

@@ -1,6 +1,7 @@
 #!/usr/bin/env python
 import os
-from ament_index_python.packages import get_package_share_directory
+import rospkg
+
 from python_qt_binding.QtWidgets import QWidget
 from python_qt_binding import loadUi
 
@@ -8,7 +9,7 @@ class MasterWidget(QWidget):
 
     def __init__(self,context):
         super(MasterWidget, self).__init__()
-        ui_file = os.path.join(get_package_share_directory('safety_master_plugin'), 'ui', 'master_view.ui')
+        ui_file = os.path.join(rospkg.RosPack().get_path('safety_master_plugin'), 'ui', 'master_view.ui')
         loadUi(ui_file, self)
         self.setObjectName('Master Plugin - L1.5 safety system')
 

scripts/qt_master_wrapper.py

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

scripts/robot_info.py

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

scripts/ros_master_wrapper.py

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

scripts/safety_master_plugin

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

setup.cfg

@@ -1,5 +0,0 @@
-[develop]
-script_dir=$base/lib/safety_master_plugin
-[install]
-install_scripts=$base/lib/safety_master_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_master_plugin'
+    package_dir={'': 'scripts'}
-
-setup(
-    name=package_name,
-    version='0.2.0',
-    packages=[package_name],
-    data_files=[
-        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
-        ('share/' + package_name, ['package.xml']),
-        ('share/' + package_name, ['plugin.xml']),
-        (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*.launch.py'))),
-        (os.path.join('share', package_name, 'ui'), glob(os.path.join('ui', '*.png'))),
-        (os.path.join('share', package_name, 'ui'), glob(os.path.join('ui', '*.ui'))),
-        (os.path.join('lib/', package_name), glob(os.path.join('ui', '*.ui'))),
-    ],
-    install_requires=['setuptools'],
-    zip_safe=True,
-    maintainer='jdelicat',
-    maintainer_email='jakub.delicat@pwr.edu.pl',
-    description='TODO: Package description',
-    license='TODO: License declaration',
-    tests_require=['pytest'],
-    entry_points={
-        'console_scripts': ['master_plugin_node = safety_master_plugin.master_plugin_node:main'],
-    },
 )
+
+setup(**setup_args)