ros2 init
This commit is contained in:
Jakub Delicat
parent
e5b53c255e
commit
8ab6d10cf6
201
CMakeLists.txt
201
CMakeLists.txt
@ -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
37
package.xml
@ -1,35 +1,24 @@
|
||||
<?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>
|
||||
<version>0.0.0</version>
|
||||
<description>The safety_master_plugin package</description>
|
||||
|
||||
<maintainer email="aleksander.bojda@pwr.edu.pl">Aleksander Bojda</maintainer>
|
||||
<version>0.2.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="l15@kcir.pwr.edu.pl">Laboratory L1.5</maintainer>
|
||||
|
||||
<author email="aleksander.bojda@pwr.edu.pl">Aleksander Bojda</author>
|
||||
<author email="jakub.delicat@pwr.edu.pl">Jakub Delicat</author>
|
||||
<license>MIT</license>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<depend>rclpy</depend>
|
||||
<depend>rqt_gui</depend>
|
||||
<depend>rqt_gui_py</depend>
|
||||
|
||||
<build_depend>rospy</build_depend>
|
||||
<build_depend>rqt_gui</build_depend>
|
||||
<build_depend>rqt_gui_py</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_depend>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>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>ros2aria_msgs</depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
<rqt_gui plugin="${prefix}/plugin.xml"/>
|
||||
</export>
|
||||
</package>
|
||||
|
||||
@ -1,11 +1,11 @@
|
||||
<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>
|
||||
Safety system master plugin
|
||||
</description>
|
||||
<qtgui>
|
||||
<group>
|
||||
<label>Safety system</label>
|
||||
<label>L1.5 Safety system</label>
|
||||
<icon type="theme">folder</icon>
|
||||
</group>
|
||||
<label>Master</label>
|
||||
|
||||
0
safety_master_plugin/__init__.py
Normal file
0
safety_master_plugin/__init__.py
Normal file
0
safety_master_plugin/enums/__init__.py
Normal file
0
safety_master_plugin/enums/__init__.py
Normal file
6
safety_master_plugin/enums/clutch_state.py
Normal file
6
safety_master_plugin/enums/clutch_state.py
Normal file
@ -0,0 +1,6 @@
|
||||
|
||||
|
||||
class ClutchState:
|
||||
ENGAGED = 4
|
||||
DISENGAGED = 5
|
||||
UNKNOWN = 6
|
||||
@ -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)
|
||||
@ -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()
|
||||
|
||||
@ -73,7 +80,6 @@ class MasterPlugin(Plugin):
|
||||
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)
|
||||
@ -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):
|
||||
11
safety_master_plugin/master_plugin_node.py
Normal file
11
safety_master_plugin/master_plugin_node.py
Normal 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()
|
||||
@ -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')
|
||||
|
||||
@ -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):
|
||||
@ -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:
|
||||
|
||||
@ -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,29 +101,22 @@ 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):
|
||||
_robot_info = RobotInfo(0)
|
||||
_robot_info.update_robot_info(msg)
|
||||
@ -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
|
||||
@ -157,3 +148,12 @@ class ROSWrapper:
|
||||
msg.angular_velocity.data = restrictions.angular_velocity
|
||||
|
||||
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
6
safety_master_plugin/stop_state.py
Normal file
@ -0,0 +1,6 @@
|
||||
|
||||
|
||||
class StopState:
|
||||
STOPPED = 1
|
||||
RUNNING = 2
|
||||
UNKNOWN = 3
|
||||
9
scripts/safety_master_plugin
Executable file
9
scripts/safety_master_plugin
Executable 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
5
setup.cfg
Normal file
@ -0,0 +1,5 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/safety_master_plugin
|
||||
[install]
|
||||
install_scripts=$base/lib/safety_master_plugin
|
||||
|
||||
35
setup.py
35
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)
|
||||
Loading…
Reference in New Issue
Block a user