Added 2 custom messages. Further changes in user plugin

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(
+ add_message_files(
-#   FILES
+   FILES
-#   Message1.msg
+   RestrictionsMsg.msg
-#   Message2.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(
+ generate_messages(
-#   DEPENDENCIES
+   DEPENDENCIES
-#   std_msgs
+   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

safety_user_plugin/msg/RestrictionsMsg.msg

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

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

safety_user_plugin/package.xml

@@ -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"/>

safety_user_plugin/scripts/restrictions.py

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

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.robot_id = robot_info_msg.robot_id.data
-        self.battery_voltage = robot_info_msg.battery_voltage
+        self.battery_voltage = robot_info_msg.battery_voltage.data
-        self.linear_velocity = robot_info_msg.linear_velocity
+        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.angular_velocity
+        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
+        self.state = robot_info_msg.state.data
-        self.clutch = robot_info_msg.clutch
+        self.clutch = robot_info_msg.clutch.data

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

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)