L1.5/l1.5-safety-system
6
0
Fork 0
Code Issues 1 Pull Requests Projects Releases Wiki Activity

Added 2 custom messages. Further changes in user plugin

Browse Source
This commit is contained in:
Olek Bojda 2018-09-18 02:29:14 +02:00
parent 1652612df7
commit ba57aae639
8 changed files with 42 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
safety_user_plugin/CMakeLists.txt
View File

@ -12,6 +12,8 @@ find_package(catkin REQUIRED COMPONENTS
rqt_gui
rqt_gui_py
std_msgs
geometry_msgs
message_generation
)
## System dependencies are found with CMake's conventions
@ -48,11 +50,11 @@ catkin_python_setup()
## * 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
# )
add_message_files(
FILES
RestrictionsMsg.msg
RobotInfoMsg.msg
)
## Generate services in the 'srv' folder
# add_service_files(
@ -69,10 +71,11 @@ catkin_python_setup()
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# std_msgs
# )
generate_messages(
DEPENDENCIES
std_msgs
geometry_msgs
)
################################################
## Declare ROS dynamic reconfigure parameters ##
@ -104,6 +107,7 @@ catkin_python_setup()
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
CATKIN_DEPENDS message_runtime
# INCLUDE_DIRS include
# LIBRARIES safety_master_plugin
# CATKIN_DEPENDS rospy std_msgs

3
safety_user_plugin/msg/RestrictionsMsg.msg Normal file
View File

@ -0,0 +1,3 @@
std_msgs/Float64 distance
std_msgs/Float64 linear_velocity
std_msgs/Float64 angular_velocity

5
safety_user_plugin/msg/RobotInfoMsg.msg Normal file
View File

@ -0,0 +1,5 @@
std_msgs/UInt8 robot_id
std_msgs/Float64 battery_voltage
geometry_msgs/Twist twist
std_msgs/Bool state
std_msgs/Bool clutch

3
safety_user_plugin/package.xml
View File

@ -14,16 +14,19 @@
<build_depend>rqt_gui</build_depend>
<build_depend>rqt_gui_py</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>message_generation</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>message_generation</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>
<export>
<rqt_gui plugin="${prefix}/plugin.xml"/>

6
safety_user_plugin/scripts/restrictions.py
View File

@ -3,6 +3,6 @@
class Restrictions:
def __init__(self,restrictions_msg):
self.distance = restrictions_msg.distance
self.linear_velocity = restrictions_msg.linear_velocity
self.angular_velocity = restrictions_msg.angular_velocity
self.distance = restrictions_msg.distance.data
self.linear_velocity = restrictions_msg.linear_velocity.data
self.angular_velocity = restrictions_msg.angular_velocity.data

12
safety_user_plugin/scripts/robot_info.py
View File

@ -3,9 +3,9 @@
class RobotInfo:
def __init__(self,robot_info_msg):
self.robot_id = robot_info_msg.robot_id
self.battery_voltage = robot_info_msg.battery_voltage
self.linear_velocity = robot_info_msg.linear_velocity
self.angular_velocity = robot_info_msg.angular_velocity
self.state = robot_info_msg.state
self.clutch = robot_info_msg.clutch
self.robot_id = robot_info_msg.robot_id.data
self.battery_voltage = robot_info_msg.battery_voltage.data
self.linear_velocity = [robot_info_msg.twist.linear.x,robot_info_msg.twist.linear.y,robot_info_msg.twist.linear.z]
self.angular_velocity = [robot_info_msg.twist.angular.x,robot_info_msg.twist.angular.y,robot_info_msg.twist.angular.z]
self.state = robot_info_msg.state.data
self.clutch = robot_info_msg.clutch.data

8
safety_user_plugin/scripts/ros_wrapper.py
View File

@ -31,6 +31,7 @@ class ROSWrapper:
self.clutch_publisher = None
self.periodic_timer = None
self.robots_list_timer = None
def setup_node(self):
rospy.init_node('safety_user_plugin', anonymous=True)
@ -133,7 +134,7 @@ class ROSWrapper:
return robots_list
# ROSWrapper Callbacks
def get_robots_list(self):
def get_robots_list(self,event):
robots_id_list = []
published_topics_list = rospy.get_published_topics(namespace='/')
@ -143,7 +144,9 @@ class ROSWrapper:
published_topics.append(list_[0])
for topic in published_topics:
if topic_find('RosAria') and topic_find('robot_state'):
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:]
@ -173,6 +176,7 @@ class ROSWrapper:
# UserPlugin Callbacks
def set_robots_list_update_callback(self,callback_function):
self.robots_list_update_callback = callback_function
self.robots_list_timer = rospy.Timer(rospy.Duration(0.5),self.get_robots_list)
def set_selected_robot_info_update_callback(self,callback_function):
self.selected_robot_info_update_callback = callback_function

3
safety_user_plugin/scripts/user_plugin.py
View File

@ -31,6 +31,7 @@ class UserPlugin(Plugin):
self._ros_wrapper.setup_get_user_stop_state_function(self._user_info.get_user_stop_state)
self._ros_wrapper.setup_get_clutch_state_function(self._user_info.get_clutch_state)
self._ros_wrapper.set_robots_list_update_callback(self.handle_robots_list_update)
# self.set_callback_functions()
# At the end!
@ -45,7 +46,7 @@ class UserPlugin(Plugin):
self.set_ROS_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_selected_robot_info_update_callback(self.handle_selected_robot_info_update)
self._ros_wrapper.set_master_stop_update_callback(self.handle_master_stop_update)
self._ros_wrapper.set_restrictions_update_callback(self.handle_restrictions_update)