From ba57aae639501664c60b43398518ed40e11ca7c1 Mon Sep 17 00:00:00 2001 From: Olek Bojda Date: Tue, 18 Sep 2018 02:29:14 +0200 Subject: [PATCH] Added 2 custom messages. Further changes in user plugin --- safety_user_plugin/CMakeLists.txt | 22 +++++++++++++--------- safety_user_plugin/msg/RestrictionsMsg.msg | 3 +++ safety_user_plugin/msg/RobotInfoMsg.msg | 5 +++++ safety_user_plugin/package.xml | 5 ++++- safety_user_plugin/scripts/restrictions.py | 6 +++--- safety_user_plugin/scripts/robot_info.py | 12 ++++++------ safety_user_plugin/scripts/ros_wrapper.py | 8 ++++++-- safety_user_plugin/scripts/user_plugin.py | 3 ++- 8 files changed, 42 insertions(+), 22 deletions(-) create mode 100644 safety_user_plugin/msg/RestrictionsMsg.msg create mode 100644 safety_user_plugin/msg/RobotInfoMsg.msg diff --git a/safety_user_plugin/CMakeLists.txt b/safety_user_plugin/CMakeLists.txt index d054815..1822dc4 100644 --- a/safety_user_plugin/CMakeLists.txt +++ b/safety_user_plugin/CMakeLists.txt @@ -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 diff --git a/safety_user_plugin/msg/RestrictionsMsg.msg b/safety_user_plugin/msg/RestrictionsMsg.msg new file mode 100644 index 0000000..2b0aa55 --- /dev/null +++ b/safety_user_plugin/msg/RestrictionsMsg.msg @@ -0,0 +1,3 @@ +std_msgs/Float64 distance +std_msgs/Float64 linear_velocity +std_msgs/Float64 angular_velocity \ No newline at end of file diff --git a/safety_user_plugin/msg/RobotInfoMsg.msg b/safety_user_plugin/msg/RobotInfoMsg.msg new file mode 100644 index 0000000..00b7665 --- /dev/null +++ b/safety_user_plugin/msg/RobotInfoMsg.msg @@ -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 \ No newline at end of file diff --git a/safety_user_plugin/package.xml b/safety_user_plugin/package.xml index 005ad9e..d4203e5 100644 --- a/safety_user_plugin/package.xml +++ b/safety_user_plugin/package.xml @@ -14,16 +14,19 @@ rqt_gui rqt_gui_py std_msgs - + message_generation + rospy rqt_gui rqt_gui_py std_msgs + message_generation rospy rqt_gui rqt_gui_py std_msgs + message_runtime diff --git a/safety_user_plugin/scripts/restrictions.py b/safety_user_plugin/scripts/restrictions.py index 5c0cf41..bea4082 100644 --- a/safety_user_plugin/scripts/restrictions.py +++ b/safety_user_plugin/scripts/restrictions.py @@ -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 \ No newline at end of file + self.distance = restrictions_msg.distance.data + self.linear_velocity = restrictions_msg.linear_velocity.data + self.angular_velocity = restrictions_msg.angular_velocity.data \ No newline at end of file diff --git a/safety_user_plugin/scripts/robot_info.py b/safety_user_plugin/scripts/robot_info.py index ce48297..cc9493f 100644 --- a/safety_user_plugin/scripts/robot_info.py +++ b/safety_user_plugin/scripts/robot_info.py @@ -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 \ No newline at end of file + 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 \ No newline at end of file diff --git a/safety_user_plugin/scripts/ros_wrapper.py b/safety_user_plugin/scripts/ros_wrapper.py index 3b49efc..955d1ce 100644 --- a/safety_user_plugin/scripts/ros_wrapper.py +++ b/safety_user_plugin/scripts/ros_wrapper.py @@ -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 diff --git a/safety_user_plugin/scripts/user_plugin.py b/safety_user_plugin/scripts/user_plugin.py index e04e039..757e167 100755 --- a/safety_user_plugin/scripts/user_plugin.py +++ b/safety_user_plugin/scripts/user_plugin.py @@ -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)