diff --git a/CMakeLists.txt b/CMakeLists.txt deleted file mode 100644 index 7108bca..0000000 --- a/CMakeLists.txt +++ /dev/null @@ -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) diff --git a/package.xml b/package.xml index a4f058c..f4f12d8 100644 --- a/package.xml +++ b/package.xml @@ -1,35 +1,24 @@ - + + safety_master_plugin - 0.0.0 - The safety_master_plugin package - - Aleksander Bojda + 0.2.0 + TODO: Package description + Laboratory L1.5 + Aleksander Bojda + Jakub Delicat MIT - catkin + rclpy + rqt_gui + rqt_gui_py - rospy - rqt_gui - rqt_gui_py - std_msgs - rosaria_msgs - - rospy - rqt_gui - rqt_gui_py - std_msgs - rosaria_msgs - - rospy - rqt_gui - rqt_gui_py - std_msgs - message_runtime - rosaria_msgs + std_msgs + ros2aria_msgs + ament_python diff --git a/plugin.xml b/plugin.xml index b4423a5..098c1e9 100644 --- a/plugin.xml +++ b/plugin.xml @@ -1,11 +1,11 @@ - + Safety system master plugin - + folder diff --git a/scripts/enums/__init__.py b/resource/safety_master_plugin similarity index 100% rename from scripts/enums/__init__.py rename to resource/safety_master_plugin diff --git a/safety_master_plugin/__init__.py b/safety_master_plugin/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/scripts/enums/clutch_state.py b/safety_master_plugin/clutch_state.py similarity index 100% rename from scripts/enums/clutch_state.py rename to safety_master_plugin/clutch_state.py diff --git a/safety_master_plugin/enums/__init__.py b/safety_master_plugin/enums/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/safety_master_plugin/enums/clutch_state.py b/safety_master_plugin/enums/clutch_state.py new file mode 100644 index 0000000..9479b62 --- /dev/null +++ b/safety_master_plugin/enums/clutch_state.py @@ -0,0 +1,6 @@ + + +class ClutchState: + ENGAGED = 4 + DISENGAGED = 5 + UNKNOWN = 6 \ No newline at end of file diff --git a/scripts/enums/stop_state.py b/safety_master_plugin/enums/stop_state.py similarity index 100% rename from scripts/enums/stop_state.py rename to safety_master_plugin/enums/stop_state.py diff --git a/scripts/master_callback_handler.py b/safety_master_plugin/master_callback_handler.py similarity index 94% rename from scripts/master_callback_handler.py rename to safety_master_plugin/master_callback_handler.py index c616d5d..bba7c96 100644 --- a/scripts/master_callback_handler.py +++ b/safety_master_plugin/master_callback_handler.py @@ -1,6 +1,6 @@ from python_qt_binding.QtCore import QObject from python_qt_binding.QtCore import pyqtSignal -from robot_info import RobotInfo +from .robot_info import RobotInfo class CallbackHandler(QObject): robot_info_signal = pyqtSignal(object) diff --git a/scripts/master_info.py b/safety_master_plugin/master_info.py similarity index 100% rename from scripts/master_info.py rename to safety_master_plugin/master_info.py diff --git a/scripts/master_plugin.py b/safety_master_plugin/master_plugin.py similarity index 75% rename from scripts/master_plugin.py rename to safety_master_plugin/master_plugin.py index 92f9160..9cf10d9 100755 --- a/scripts/master_plugin.py +++ b/safety_master_plugin/master_plugin.py @@ -1,15 +1,17 @@ -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 .ros_master_wrapper import ROSWrapper +# from .qt_master_wrapper import QtWrapper 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): @@ -20,6 +22,8 @@ class MasterPlugin(Plugin): 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() @@ -27,10 +31,8 @@ class MasterPlugin(Plugin): 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() - + 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) @@ -39,6 +41,11 @@ class MasterPlugin(Plugin): self.set_callback_functions() 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 self.master_stopped() @@ -53,8 +60,8 @@ class MasterPlugin(Plugin): 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) @@ -65,15 +72,14 @@ class MasterPlugin(Plugin): if self._master_info.master_stop_state == SS.RUNNING: self.master_stopped() elif self._master_info.master_stop_state == SS.STOPPED: - self.master_started() + self.master_started() else: - raise ValueError('master_stop_state value undefined') + 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): - + 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) @@ -89,7 +95,7 @@ class MasterPlugin(Plugin): self.cbhandler.add_robot_signal.emit(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) def update_robot_info(self,robot_info): @@ -102,7 +108,7 @@ class MasterPlugin(Plugin): def master_stopped(self): self._master_info.master_stop_state = SS.STOPPED - self._ros_wrapper.master_stopped() + # self._ros_wrapper.master_stopped() self._qt_wrapper.master_stopped() def restrictions_changed(self,restrictions): diff --git a/safety_master_plugin/master_plugin_node.py b/safety_master_plugin/master_plugin_node.py new file mode 100644 index 0000000..99dc77d --- /dev/null +++ b/safety_master_plugin/master_plugin_node.py @@ -0,0 +1,11 @@ +from safety_master_plugin.master_plugin import MasterPlugin +from rqt_gui.main import Main +import sys + +def main(): + """Run the plugin.""" + Main().main(sys.argv, standalone="safety_master_plugin") + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/scripts/master_restrictions.py b/safety_master_plugin/master_restrictions.py similarity index 100% rename from scripts/master_restrictions.py rename to safety_master_plugin/master_restrictions.py diff --git a/scripts/master_widget.py b/safety_master_plugin/master_widget.py similarity index 73% rename from scripts/master_widget.py rename to safety_master_plugin/master_widget.py index ffa07f5..c5ec45f 100644 --- a/scripts/master_widget.py +++ b/safety_master_plugin/master_widget.py @@ -1,7 +1,6 @@ #!/usr/bin/env python import os -import rospkg - +from ament_index_python.packages import get_package_share_directory from python_qt_binding.QtWidgets import QWidget from python_qt_binding import loadUi @@ -9,7 +8,7 @@ class MasterWidget(QWidget): def __init__(self,context): super(MasterWidget, self).__init__() - ui_file = os.path.join(rospkg.RosPack().get_path('safety_master_plugin'), 'ui', 'master_view.ui') + ui_file = os.path.join(get_package_share_directory('safety_master_plugin'), 'ui', 'master_view.ui') loadUi(ui_file, self) self.setObjectName('Master Plugin - L1.5 safety system') diff --git a/scripts/qt_master_wrapper.py b/safety_master_plugin/qt_master_wrapper.py similarity index 93% rename from scripts/qt_master_wrapper.py rename to safety_master_plugin/qt_master_wrapper.py index a97dde9..efbb508 100644 --- a/scripts/qt_master_wrapper.py +++ b/safety_master_plugin/qt_master_wrapper.py @@ -1,16 +1,14 @@ #!/usr/bin/env python # coding=utf-8 import os -import rospy -import rospkg import math import datetime +from ament_index_python.packages import get_package_share_directory -from enums.clutch_state import ClutchState as CS -from enums.stop_state import StopState as SS - -from master_restrictions import Restrictions +from safety_master_plugin.clutch_state import ClutchState as CS +from safety_master_plugin.stop_state import StopState as SS +from safety_master_plugin.master_restrictions import Restrictions from python_qt_binding.QtGui import QPixmap from python_qt_binding.QtGui import QTextCursor @@ -34,7 +32,7 @@ class QtWrapper: # 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.angular_pixmap = QPixmap('{0}/Angular.png'.format(ui_directory_path)) @@ -129,7 +127,7 @@ class QtWrapper: def update_robot_info(self,robot_info): items_to_add = [] - items_to_add.append(QStandardItem('PIONIER{}'.format(robot_info.robot_id))) + items_to_add.append(QStandardItem('pioneer{}'.format(robot_info.robot_id))) items_to_add.append(QStandardItem('{:.2f}'.format(robot_info.battery_voltage))) 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_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: for item in items_list: row_number = item.row() @@ -186,26 +184,26 @@ class QtWrapper: def add_robot(self,robot_id): if robot_id not in self.displayed_robots_id_list: - new_robot_info=QStandardItem('PIONIER{}'.format(robot_id)) + new_robot_info=QStandardItem('pioneer{}'.format(robot_id)) new_robot_info.setTextAlignment(Qt.AlignCenter) new_robot_info.setEditable(0) # new_robot_info.setBackground(QBrush(Qt.white)) self.stdItemModel.setItem(self.stdItemModel.rowCount(),0,new_robot_info) self.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: raise RuntimeError('Adding robot, which is already in master GUI') 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: for item in items_list: row_number = item.row() self.stdItemModel.removeRow(row_number) - self.log_info('PIONIER{0} rozłączony'.format(robot_id)) + self.log_info('pioneer{0} rozłączony'.format(robot_id)) self.displayed_robots_id_list.remove(robot_id) def log_info(self,info_text): diff --git a/scripts/robot_info.py b/safety_master_plugin/robot_info.py similarity index 88% rename from scripts/robot_info.py rename to safety_master_plugin/robot_info.py index 4f50b36..e170ea4 100644 --- a/scripts/robot_info.py +++ b/safety_master_plugin/robot_info.py @@ -1,7 +1,7 @@ #!/usr/bin/env python -from enums.clutch_state import ClutchState as CS -from enums.stop_state import StopState as SS +from safety_master_plugin.clutch_state import ClutchState as CS +from safety_master_plugin.stop_state import StopState as SS class RobotInfo: diff --git a/scripts/ros_master_wrapper.py b/safety_master_plugin/ros_master_wrapper.py similarity index 52% rename from scripts/ros_master_wrapper.py rename to safety_master_plugin/ros_master_wrapper.py index 0a51d8a..9d24c9c 100644 --- a/scripts/ros_master_wrapper.py +++ b/safety_master_plugin/ros_master_wrapper.py @@ -1,19 +1,21 @@ #!/usr/bin/env python -import rospy -import rospkg +import rclpy +from rclpy.node import Node, NodeNameNonExistentError +from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy, QoSLivelinessPolicy, QoSDurabilityPolicy from std_msgs.msg import Bool -from rosaria_msgs.msg import RobotInfoMsg -from rosaria_msgs.msg import RestrictionsMsg +from ros2aria_msgs.msg import RobotInfoMsg +from ros2aria_msgs.msg import RestrictionsMsg -from robot_info import RobotInfo -from master_restrictions import Restrictions +from safety_master_plugin.robot_info import RobotInfo +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): + super().__init__('safety_master_plugin') self.robots_list_update_callback = None self.robot_info_update_callback = None @@ -21,46 +23,46 @@ class ROSWrapper: self.master_stop_publisher = None self.restrictions_publisher = None - self.robots_list_timer = None - self.periodic_timer = None - 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): - 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) + self.master_stop_publisher = self.create_publisher(Bool, '/pioneers/master_stop', 10) + self.restrictions_publisher = self.create_publisher(RestrictionsMsg, '/pioneers/restrictions', 10) 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 + topic_name = '/pioneer{0}/robot_info'.format(robot_id) + self.robot_info_subscribers[robot_id] = self.create_subscription(RobotInfoMsg, topic_name, self.handle_robot_info_update, 10) 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): - 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 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 unsubscribe(self,subscriber): + # if subscriber != None: + # subscriber.unregister() - def unregister_publisher(self,publisher): - if publisher != None: - publisher.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): + def publish_periodic_update(self): restrictions = self.get_restrictions() restrictions_msg = RestrictionsMsg() restrictions_msg.distance.data = restrictions.distance @@ -82,9 +84,6 @@ class ROSWrapper: 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) @@ -102,30 +101,23 @@ class ROSWrapper: self.get_restrictions = function # ROSWrapper Callbacks - def get_robots_list(self,event): + def get_robots_list(self): 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: + # Można to zrobić lepiej np. czekać na publikowaną wiadomość na robot_info + for pioneer_id in range(6): + published_topics_list = [] + try: + published_topics_list = self.get_publisher_names_and_types_by_node('ros2aria', '/pioneer'+str(pioneer_id)) + except NodeNameNonExistentError: 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) + + if len(published_topics_list): + robots_id_list.append(pioneer_id) 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.update_robot_info(msg) self.robot_info_update_callback(_robot_info) @@ -134,7 +126,6 @@ class ROSWrapper: # 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 @@ -156,4 +147,13 @@ class ROSWrapper: msg.linear_velocity.data = restrictions.linear_velocity msg.angular_velocity.data = restrictions.angular_velocity - self.restrictions_publisher.publish(msg) \ No newline at end of file + 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) diff --git a/safety_master_plugin/stop_state.py b/safety_master_plugin/stop_state.py new file mode 100644 index 0000000..17d5939 --- /dev/null +++ b/safety_master_plugin/stop_state.py @@ -0,0 +1,6 @@ + + +class StopState: + STOPPED = 1 + RUNNING = 2 + UNKNOWN = 3 \ No newline at end of file diff --git a/scripts/safety_master_plugin b/scripts/safety_master_plugin new file mode 100755 index 0000000..c150a3a --- /dev/null +++ b/scripts/safety_master_plugin @@ -0,0 +1,9 @@ +#!/usr/bin/env python3 + +import sys + +from safety_master_plugin.master_plugin import MasterPlugin +from rqt_gui.main import Main + + +sys.exit(Main().main(sys.argv, standalone='safety_master_plugin.master_plugin.MasterPlugin')) \ No newline at end of file diff --git a/setup.cfg b/setup.cfg new file mode 100644 index 0000000..bcb12a6 --- /dev/null +++ b/setup.cfg @@ -0,0 +1,5 @@ +[develop] +script_dir=$base/lib/safety_master_plugin +[install] +install_scripts=$base/lib/safety_master_plugin + diff --git a/setup.py b/setup.py index 25facb1..170b230 100644 --- a/setup.py +++ b/setup.py @@ -1,8 +1,31 @@ -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup +import os +from glob import glob +from setuptools import setup -setup_args = generate_distutils_setup( - package_dir={'': 'scripts'} + +package_name = 'safety_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) \ No newline at end of file