L1.5/safety_master_plugin
L1.5/safety_master_plugin
6
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

ros2 init

Browse Source
This commit is contained in:
Jakub Delicat 2023-08-09 16:18:19 +02:00
parent e5b53c255e
commit 8ab6d10cf6
22 changed files with 182 additions and 331 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

201
CMakeLists.txt
View File

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

37
package.xml
View File

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

4
plugin.xml
View File

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

0
scripts/enums/__init__.py → resource/safety_master_plugin
View File

0
safety_master_plugin/__init__.py Normal file
View File

0
scripts/enums/clutch_state.py → safety_master_plugin/clutch_state.py
View File

0
safety_master_plugin/enums/__init__.py Normal file
View File

6
safety_master_plugin/enums/clutch_state.py Normal file
View File

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

0
scripts/enums/stop_state.py → safety_master_plugin/enums/stop_state.py
View File

2
scripts/master_callback_handler.py → safety_master_plugin/master_callback_handler.py
View File

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

0
scripts/master_info.py → safety_master_plugin/master_info.py
View File

48
scripts/master_plugin.py → safety_master_plugin/master_plugin.py
View File

@ -1,15 +1,17 @@
from ros_master_wrapper import ROSWrapper # from .ros_master_wrapper import ROSWrapper
from qt_master_wrapper import QtWrapper # 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 qt_gui.plugin import Plugin
from python_qt_binding.QtCore import QTimer
from enums.stop_state import StopState as SS 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
from master_callback_handler import CallbackHandler
class MasterPlugin(Plugin): class MasterPlugin(Plugin):
@ -20,6 +22,8 @@ class MasterPlugin(Plugin):
self._widget = MasterWidget(context) self._widget = MasterWidget(context)
self._ros_wrapper = ROSWrapper() self._ros_wrapper = ROSWrapper()
self._qt_wrapper = QtWrapper(self._widget) self._qt_wrapper = QtWrapper(self._widget)
self.set_Qt_callback_functions()
self._master_info = MasterInfo() self._master_info = MasterInfo()
self.cbhandler = CallbackHandler() self.cbhandler = CallbackHandler()
@ -27,10 +31,8 @@ class MasterPlugin(Plugin):
self.cbhandler.connect_add_robot_signal(self._qt_wrapper.add_robot) self.cbhandler.connect_add_robot_signal(self._qt_wrapper.add_robot)
self.cbhandler.connect_remove_robot_signal(self._qt_wrapper.remove_robot) self.cbhandler.connect_remove_robot_signal(self._qt_wrapper.remove_robot)
self._qt_wrapper.connect_signals()
# self._qt_wrapper.connect_signals() self._qt_wrapper.set_sliders_to_initial_values()
# self._qt_wrapper.set_sliders_to_initial_values()
# Setup functions to get _master_info states from _ros_wrapper # 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_master_stop_state_function(self._master_info.get_master_stop_state)
@ -39,6 +41,11 @@ class MasterPlugin(Plugin):
self.set_callback_functions() self.set_callback_functions()
self._qt_wrapper.connect_signals() self._qt_wrapper.connect_signals()
# timer to consecutively send twist messages
self._update_parameter_timer = QTimer(self)
self._update_parameter_timer.timeout.connect(self._ros_wrapper.timer_callback)
self._update_parameter_timer.start(100)
# Stopping master at the beginning # Stopping master at the beginning
self.master_stopped() self.master_stopped()
@ -53,8 +60,8 @@ class MasterPlugin(Plugin):
def set_Qt_callback_functions(self): 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_master_stop_state_updated_callback(self.handle_master_stop_state_updated)
self._qt_wrapper.set_restrictions_updated_callback(self.handle_restrictions_updated) self._qt_wrapper.set_restrictions_updated_callback(self.handle_restrictions_updated)
def handle_robots_list_update(self,robots_id_list): def handle_robots_list_update(self,robots_id_list):
self.update_robots_list(robots_id_list) self.update_robots_list(robots_id_list)
@ -65,15 +72,14 @@ class MasterPlugin(Plugin):
if self._master_info.master_stop_state == SS.RUNNING: if self._master_info.master_stop_state == SS.RUNNING:
self.master_stopped() self.master_stopped()
elif self._master_info.master_stop_state == SS.STOPPED: elif self._master_info.master_stop_state == SS.STOPPED:
self.master_started() self.master_started()
else: else:
raise ValueError('master_stop_state value undefined') raise ValueError('master_stop_state value undefined')
def handle_restrictions_updated(self,restrictions): def handle_restrictions_updated(self,restrictions):
self.restrictions_changed(restrictions) self.restrictions_changed(restrictions)
def update_robots_list(self,robots_id_list): def update_robots_list(self, robots_id_list):
for robot_id in robots_id_list: for robot_id in robots_id_list:
if robot_id not in self._master_info.robots_id_list: if robot_id not in self._master_info.robots_id_list:
self.new_robot_registered(robot_id) self.new_robot_registered(robot_id)
@ -89,7 +95,7 @@ class MasterPlugin(Plugin):
self.cbhandler.add_robot_signal.emit(robot_id) self.cbhandler.add_robot_signal.emit(robot_id)
def robot_unregistered(self,robot_id): def robot_unregistered(self,robot_id):
self._ros_wrapper.remove_robot_subscriber(robot_id) # self._ros_wrapper.remove_robot_subscriber(robot_id)
self.cbhandler.remove_robot_signal.emit(robot_id) self.cbhandler.remove_robot_signal.emit(robot_id)
def update_robot_info(self,robot_info): def update_robot_info(self,robot_info):
@ -102,7 +108,7 @@ class MasterPlugin(Plugin):
def master_stopped(self): def master_stopped(self):
self._master_info.master_stop_state = SS.STOPPED self._master_info.master_stop_state = SS.STOPPED
self._ros_wrapper.master_stopped() # self._ros_wrapper.master_stopped()
self._qt_wrapper.master_stopped() self._qt_wrapper.master_stopped()
def restrictions_changed(self,restrictions): def restrictions_changed(self,restrictions):

11
safety_master_plugin/master_plugin_node.py Normal file
View File

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

0
scripts/master_restrictions.py → safety_master_plugin/master_restrictions.py
View File

5
scripts/master_widget.py → safety_master_plugin/master_widget.py
View File

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

24
scripts/qt_master_wrapper.py → safety_master_plugin/qt_master_wrapper.py
View File

@ -1,16 +1,14 @@
#!/usr/bin/env python #!/usr/bin/env python
# coding=utf-8 # coding=utf-8
import os import os
import rospy
import rospkg
import math import math
import datetime import datetime
from ament_index_python.packages import get_package_share_directory
from enums.clutch_state import ClutchState as CS from safety_master_plugin.clutch_state import ClutchState as CS
from enums.stop_state import StopState as SS from safety_master_plugin.stop_state import StopState as SS
from safety_master_plugin.master_restrictions import Restrictions
from master_restrictions import Restrictions
from python_qt_binding.QtGui import QPixmap from python_qt_binding.QtGui import QPixmap
from python_qt_binding.QtGui import QTextCursor from python_qt_binding.QtGui import QTextCursor
@ -34,7 +32,7 @@ class QtWrapper:
# self.disable_sliders_tracking() # self.disable_sliders_tracking()
ui_directory_path = '{0}/ui'.format(rospkg.RosPack().get_path('safety_master_plugin')) ui_directory_path = '{0}/ui'.format(get_package_share_directory('safety_master_plugin'))
self.distance_pixmap = QPixmap('{0}/Distance.png'.format(ui_directory_path)) self.distance_pixmap = QPixmap('{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))
@ -129,7 +127,7 @@ class QtWrapper:
def update_robot_info(self,robot_info): def update_robot_info(self,robot_info):
items_to_add = [] items_to_add = []
items_to_add.append(QStandardItem('PIONIER{}'.format(robot_info.robot_id))) items_to_add.append(QStandardItem('pioneer{}'.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: if robot_info.state == SS.RUNNING:
@ -160,7 +158,7 @@ class QtWrapper:
items_to_add.append(QStandardItem('{:.2f}'.format(robot_info.angular_velocity[2]))) items_to_add.append(QStandardItem('{:.2f}'.format(robot_info.angular_velocity[2])))
items_list = self.stdItemModel.findItems('PIONIER{0}'.format(robot_info.robot_id),Qt.MatchExactly) items_list = self.stdItemModel.findItems('pioneer{0}'.format(robot_info.robot_id),Qt.MatchExactly)
if len(items_list) > 0: if len(items_list) > 0:
for item in items_list: for item in items_list:
row_number = item.row() row_number = item.row()
@ -186,26 +184,26 @@ class QtWrapper:
def add_robot(self,robot_id): def add_robot(self,robot_id):
if robot_id not in self.displayed_robots_id_list: if robot_id not in self.displayed_robots_id_list:
new_robot_info=QStandardItem('PIONIER{}'.format(robot_id)) new_robot_info=QStandardItem('pioneer{}'.format(robot_id))
new_robot_info.setTextAlignment(Qt.AlignCenter) new_robot_info.setTextAlignment(Qt.AlignCenter)
new_robot_info.setEditable(0) new_robot_info.setEditable(0)
# new_robot_info.setBackground(QBrush(Qt.white)) # new_robot_info.setBackground(QBrush(Qt.white))
self.stdItemModel.setItem(self.stdItemModel.rowCount(),0,new_robot_info) self.stdItemModel.setItem(self.stdItemModel.rowCount(),0,new_robot_info)
self.displayed_robots_id_list.append(robot_id) self.displayed_robots_id_list.append(robot_id)
self.log_info('PIONIER{0} połączony'.format(robot_id)) self.log_info('pioneer{0} połączony'.format(robot_id))
else: else:
raise RuntimeError('Adding robot, which is already in master GUI') raise RuntimeError('Adding robot, which is already in master GUI')
def remove_robot(self,robot_id): def remove_robot(self,robot_id):
items_list = self.stdItemModel.findItems('PIONIER{0}'.format(robot_id),Qt.MatchExactly) items_list = self.stdItemModel.findItems('pioneer{0}'.format(robot_id),Qt.MatchExactly)
if len(items_list) > 0: if len(items_list) > 0:
for item in items_list: for item in items_list:
row_number = item.row() row_number = item.row()
self.stdItemModel.removeRow(row_number) self.stdItemModel.removeRow(row_number)
self.log_info('PIONIER{0} rozłączony'.format(robot_id)) self.log_info('pioneer{0} rozłączony'.format(robot_id))
self.displayed_robots_id_list.remove(robot_id) self.displayed_robots_id_list.remove(robot_id)
def log_info(self,info_text): def log_info(self,info_text):

4
scripts/robot_info.py → safety_master_plugin/robot_info.py
View File

@ -1,7 +1,7 @@
#!/usr/bin/env python #!/usr/bin/env python
from enums.clutch_state import ClutchState as CS from safety_master_plugin.clutch_state import ClutchState as CS
from enums.stop_state import StopState as SS from safety_master_plugin.stop_state import StopState as SS
class RobotInfo: class RobotInfo:

116
scripts/ros_master_wrapper.py → safety_master_plugin/ros_master_wrapper.py
View File

@ -1,19 +1,21 @@
#!/usr/bin/env python #!/usr/bin/env python
import rospy import rclpy
import rospkg from rclpy.node import Node, NodeNameNonExistentError
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy, QoSLivelinessPolicy, QoSDurabilityPolicy
from std_msgs.msg import Bool from std_msgs.msg import Bool
from rosaria_msgs.msg import RobotInfoMsg from ros2aria_msgs.msg import RobotInfoMsg
from rosaria_msgs.msg import RestrictionsMsg from ros2aria_msgs.msg import RestrictionsMsg
from robot_info import RobotInfo from safety_master_plugin.robot_info import RobotInfo
from master_restrictions import Restrictions from safety_master_plugin.master_restrictions import Restrictions
from enums.stop_state import StopState as SS from safety_master_plugin.stop_state import StopState as SS
class ROSWrapper: class ROSWrapper(Node):
def __init__(self): def __init__(self):
super().__init__('safety_master_plugin')
self.robots_list_update_callback = None self.robots_list_update_callback = None
self.robot_info_update_callback = None self.robot_info_update_callback = None
@ -21,46 +23,46 @@ class ROSWrapper:
self.master_stop_publisher = None self.master_stop_publisher = None
self.restrictions_publisher = None self.restrictions_publisher = None
self.robots_list_timer = None
self.periodic_timer = None
self.setup_subscribers_and_publishers() self.setup_subscribers_and_publishers()
self._qos = QoSProfile(
reliability=QoSReliabilityPolicy.RELIABLE,
durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
history=QoSHistoryPolicy.SYSTEM_DEFAULT,
liveliness=QoSLivelinessPolicy.AUTOMATIC,
depth=10
)
self.subs = []
def setup_subscribers_and_publishers(self): def setup_subscribers_and_publishers(self):
self.master_stop_publisher = rospy.Publisher('/PIONIER/master_stop',Bool,queue_size = 1) self.master_stop_publisher = self.create_publisher(Bool, '/pioneers/master_stop', 10)
self.restrictions_publisher = rospy.Publisher('/PIONIER/restrictions',RestrictionsMsg,queue_size = 1) self.restrictions_publisher = self.create_publisher(RestrictionsMsg, '/pioneers/restrictions', 10)
self.periodic_timer = rospy.Timer(rospy.Duration(0.5),self.publish_periodic_update)
def add_robot_subscriber(self,robot_id): def add_robot_subscriber(self,robot_id):
if robot_id not in self.robot_info_subscribers: if robot_id not in self.robot_info_subscribers:
topic_name = '/PIONIER{0}/RosAria/robot_info'.format(robot_id) topic_name = '/pioneer{0}/robot_info'.format(robot_id)
new_sub = rospy.Subscriber(topic_name, RobotInfoMsg, self.handle_robot_info_update) self.robot_info_subscribers[robot_id] = self.create_subscription(RobotInfoMsg, topic_name, self.handle_robot_info_update, 10)
self.robot_info_subscribers[robot_id] = new_sub
else: else:
raise RuntimeError('Subscriber for PIONIER{0} already in dictionary'.format(robot_id)) raise RuntimeError('Subscriber for pioneer{0} already in dictionary'.format(robot_id))
def remove_robot_subscriber(self,robot_id): # def remove_robot_subscriber(self,robot_id):
if robot_id in self.robot_info_subscribers: # if robot_id in self.robot_info_subscribers:
self.unsubscribe(self.robot_info_subscribers[robot_id]) # self.unsubscribe(self.robot_info_subscribers[robot_id])
del self.robot_info_subscribers[robot_id] # del self.robot_info_subscribers[robot_id]
else: # else:
raise RuntimeError('Subscriber for PIONIER{0} not in dictionary. Cannot be removed'.format(robot_id)) # raise RuntimeError('Subscriber for PIONIER{0} not in dictionary. Cannot be removed'.format(robot_id))
def unsubscribe(self,subscriber): # def unsubscribe(self,subscriber):
if subscriber != None: # if subscriber != None:
subscriber.unregister() # subscriber.unregister()
def unregister_publisher(self,publisher): # def unregister_publisher(self,publisher):
if publisher != None: # if publisher != None:
publisher.unregister() # publisher.unregister()
def shutdown_timer(self,timer):
if timer != None:
timer.shutdown()
def publish_periodic_update(self,event): def publish_periodic_update(self):
restrictions = self.get_restrictions() restrictions = self.get_restrictions()
restrictions_msg = RestrictionsMsg() restrictions_msg = RestrictionsMsg()
restrictions_msg.distance.data = restrictions.distance restrictions_msg.distance.data = restrictions.distance
@ -82,9 +84,6 @@ class ROSWrapper:
self.master_stop_publisher.publish(stop_msg) self.master_stop_publisher.publish(stop_msg)
def cancel_subscribers_and_publishers(self): 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.master_stop_publisher)
self.unregister_publisher(self.restrictions_publisher) self.unregister_publisher(self.restrictions_publisher)
@ -102,30 +101,23 @@ class ROSWrapper:
self.get_restrictions = function self.get_restrictions = function
# ROSWrapper Callbacks # ROSWrapper Callbacks
def get_robots_list(self,event): def get_robots_list(self):
robots_id_list = [] robots_id_list = []
published_topics_list = rospy.get_published_topics(namespace='/') # Można to zrobić lepiej np. czekać na publikowaną wiadomość na robot_info
published_topics = [] for pioneer_id in range(6):
published_topics_list = []
for list_ in published_topics_list: try:
published_topics.append(list_[0]) published_topics_list = self.get_publisher_names_and_types_by_node('ros2aria', '/pioneer'+str(pioneer_id))
except NodeNameNonExistentError:
for topic in published_topics:
if topic.find('RosAria') ==-1 or topic.find('robot_info') == -1:
pass pass
else:
robot_number = topic.split('/') if len(published_topics_list):
robot_number = robot_number[1] robots_id_list.append(pioneer_id)
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) self.robots_list_update_callback(robots_id_list)
def handle_robot_info_update(self, msg):
def handle_robot_info_update(self,msg):
_robot_info = RobotInfo(0) _robot_info = RobotInfo(0)
_robot_info.update_robot_info(msg) _robot_info.update_robot_info(msg)
self.robot_info_update_callback(_robot_info) self.robot_info_update_callback(_robot_info)
@ -134,7 +126,6 @@ class ROSWrapper:
# MasterPlugin Callbacks # MasterPlugin Callbacks
def set_robots_list_update_callback(self,callback_function): def set_robots_list_update_callback(self,callback_function):
self.robots_list_update_callback = 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): def set_robot_info_update_callback(self,callback_function):
self.robot_info_update_callback = callback_function self.robot_info_update_callback = callback_function
@ -156,4 +147,13 @@ class ROSWrapper:
msg.linear_velocity.data = restrictions.linear_velocity msg.linear_velocity.data = restrictions.linear_velocity
msg.angular_velocity.data = restrictions.angular_velocity msg.angular_velocity.data = restrictions.angular_velocity
self.restrictions_publisher.publish(msg) self.restrictions_publisher.publish(msg)
def 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)

6
safety_master_plugin/stop_state.py Normal file
View File

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

9
scripts/safety_master_plugin Executable file
View File

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

5
setup.cfg Normal file
View File

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

35
setup.py
View File

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