Capability

package.xml

@@ -14,7 +14,7 @@
   <exec_depend>controller_manager_msgs</exec_depend>
   <exec_depend>diagnostic_msgs</exec_depend>
   <exec_depend>ros_graph_parser</exec_depend>
-  <exec_depend>rospkg</exec_depend>
+  <exec_depend>python-rospkg</exec_depend>
 
   <export>
   </export>

resources/cob4-25_manipualtion.rossystem

@@ -0,0 +1,12 @@
+RosSystem { Name 'cob4-25_manipulation_capability'
+    RosComponents ( 
+        ComponentInterface { name 'manipulation'
+            RosPublishers {
+                RosPublisher '/tf' {RefPublisher 'cob4./base/driver./base/driver./tf'}}
+            RosSubscribers {
+                RosSubscriber '/arm_right/joint_trajectory_controller/command' {RefSubscriber 'cob4./arm_right/driver./arm_right/driver./arm_right/joint_trajectory_controller/command'},
+                RosSubscriber '/arm_left/joint_trajectory_controller/command' {RefSubscriber 'cob4./arm_left/driver./arm_left/driver./arm_left/joint_trajectory_controller/command'}}
+            RosParameters {
+                RosParameter 'robot_description' { RefParameter "cob4.parameters_node.parameters_node./robot_description"}
+            }}
+)}

resources/cob4-25_navigation.rossystem

@@ -0,0 +1,11 @@
+RosSystem { Name 'cob4-25_navigation_capability'
+    RosComponents ( 
+        ComponentInterface { name 'navigation'
+            RosPublishers {
+                RosPublisher '/scan_unified' {RefPublisher 'cob4./scan_unifier_b1_2193_6719186776096933542./scan_unifier_b1_2193_6719186776096933542./scan_unified'},
+                RosPublisher '/tf' {RefPublisher 'cob4./base/driver./base/driver./tf'},
+                RosPublisher '/base/odometry_controller/odometry' {RefPublisher 'cob4./base/driver./base/driver./base/odometry_controller/odometry'}}
+            RosSubscribers {
+                RosSubscriber '/base/twist_controller/command' { RefSubscriber 'cob4./base/driver./base/driver./base/twist_controller/command'}
+        }}
+)}

src/rosgraph_monitor/observers/graph_observer.py

@@ -20,7 +20,7 @@ def __init__(self, name):
             name, '/get_rossystem_model', GetROSSystemModel)
         rospack = rospkg.RosPack()
         # TODO: path to model shouldn't be hardcoded
-        self.model_path = os.path.join(rospack.get_path('rosgraph_monitor'), "resources/cob4-25.rossystem")
+        self.model_path = os.path.join(rospack.get_path('rosgraph_monitor'), "resources/cob4-25_navigation.rossystem")
         self._rossystem_parser = ModelParser(self.model_path)
 
     def diagnostics_from_response(self, resp):
@@ -32,11 +32,11 @@ def diagnostics_from_response(self, resp):
         dynamic_model = parser.parse()
         static_model = self._rossystem_parser.parse()
 
-        missing_interfaces, additional_interfaces, incorrect_params = self.compare_models(
+        missing_interfaces, additional_interfaces, incorrect_params, missing_pubs, missing_subs = self.compare_models(
             static_model, dynamic_model)
 
         status_msgs = list()
-        if (not missing_interfaces) & (not additional_interfaces) & (not incorrect_params):
+        if (not missing_interfaces) & (not additional_interfaces) & (not incorrect_params) & (not missing_pubs) & (not missing_subs):
             status_msg = DiagnosticStatus()
             status_msg.level = DiagnosticStatus.OK
             status_msg.name = "ROS Graph"
@@ -69,6 +69,20 @@ def diagnostics_from_response(self, resp):
                         KeyValue(params[0], str(params[1])))
                 status_msgs.append(status_msg)
 
+            # There has to be a for-loop for each capability
+            # But for now we are handling only one capability at a time
+            status_msg = DiagnosticStatus()
+            status_msg.level = DiagnosticStatus.ERROR
+            status_msg.name = "capability"
+            status_msg.message = "Capability incompatible"
+            # Do this for all interfaces
+            if missing_pubs:
+                status_msg.values.append(KeyValue("publishers", "".join(missing_pubs)))
+            if missing_subs:
+                status_msg.values.append(KeyValue("subscribers", "".join(missing_subs)))
+            status_msgs.append(status_msg)
+            print(status_msg)
+
         return status_msgs
 
     # find out missing and additional interfaces
@@ -80,6 +94,23 @@ def compare_models(self, model_ref, model_current):
         set_current = set((strip_slash(x.interface_name[0]))
                           for x in model_current.interfaces)
 
+        #compare interfaces within components excluding the components
+        pub_ref = set()
+        sub_ref = set()
+        for interface in model_ref.interfaces:
+            for pub in interface.publishers:
+                pub_ref.add(strip_slash(pub.pub_name[0]))
+            for sub in interface.subscribers:
+                sub_ref.add(strip_slash(sub.sub_name[0]))
+
+        pub_current = set()
+        sub_current = set()
+        for interface in model_current.interfaces:
+            for pub in interface.publishers:
+                pub_current.add(strip_slash(pub.pub_name[0]))
+            for sub in interface.subscribers:
+                sub_current.add(strip_slash(sub.sub_name[0]))
+
         # similarly for all interfaces within the node?
         # or only for topic connections?
         # does LED's code capture topic connections?
@@ -120,4 +151,4 @@ def compare_models(self, model_ref, model_current):
                 pass
 
         # returning missing_interfaces, additional_interfaces
-        return list(set_ref - set_current), list(set_current - set_ref), incorrect_params
+        return list(set_ref - set_current), list(set_current - set_ref), incorrect_params, list(pub_ref - pub_current), list(sub_ref - sub_current)

src/rosgraph_monitor/parser.py

@@ -87,13 +87,13 @@ def __init__(self, model, isFile=True):
         _action_servers = Keyword("RosActionServers").suppress()
         _action_server = Keyword("RosActionServer").suppress(
         ) | Keyword("RosServer").suppress()
-        _ref_server = Keyword("RefServer").suppress()
+        _ref_server = Keyword("RefServer").suppress() | Keyword("RefActionServer").suppress()
 
         # Actio Clients Def
         _action_clients = Keyword("RosActionClients").suppress()
         _action_client = Keyword("RosActionClient").suppress(
         ) | Keyword("RosClient").suppress()
-        _ref_action_client = Keyword("RefClient").suppress()
+        _ref_action_client = Keyword("RefClient").suppress() | Keyword("RefActionClient").suppress()
 
         # Topic Connections Def
         _topic_connections = Keyword("TopicConnections").suppress()
@@ -196,8 +196,8 @@ def parse(self):
                 self._parse_from_file()
             else:
                 self._parse_from_string()
-        except Exception as e:
-            print(e.args)   # Should set a default 'result'?
+        except ParseException as e:
+            print(ParseException.explain(e, depth=0))   # Should set a default 'result'?
         return self._result