From af5b440289dc9b3c56a58a0219b25831235c9611 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Wed, 2 Sep 2020 15:16:46 +0200 Subject: [PATCH 1/4] Fix rospkg dep --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index bc08070..ab15d08 100644 --- a/package.xml +++ b/package.xml @@ -14,7 +14,7 @@ controller_manager_msgs diagnostic_msgs ros_graph_parser - rospkg + python-rospkg From 29fc2b5bd1bb2b54836aed3473830dde834c11ac Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Wed, 2 Sep 2020 15:17:26 +0200 Subject: [PATCH 2/4] Fix ros_graph_parser keywords --- src/rosgraph_monitor/parser.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/rosgraph_monitor/parser.py b/src/rosgraph_monitor/parser.py index 0cb245e..9161851 100644 --- a/src/rosgraph_monitor/parser.py +++ b/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() From 0f986faa226ffe467f23551b78404cefce80397e Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Wed, 2 Sep 2020 15:18:00 +0200 Subject: [PATCH 3/4] show more exception info need to improve more --- src/rosgraph_monitor/parser.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/rosgraph_monitor/parser.py b/src/rosgraph_monitor/parser.py index 9161851..514f081 100644 --- a/src/rosgraph_monitor/parser.py +++ b/src/rosgraph_monitor/parser.py @@ -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 From 2fcac6fc2d11b9576a56000a92f5856a8663c281 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Wed, 2 Sep 2020 16:50:23 +0200 Subject: [PATCH 4/4] compares interfaces for capabilities currently checking for pubs & subs --- resources/cob4-25_manipualtion.rossystem | 12 ++++++ resources/cob4-25_navigation.rossystem | 11 ++++++ .../observers/graph_observer.py | 39 +++++++++++++++++-- 3 files changed, 58 insertions(+), 4 deletions(-) create mode 100644 resources/cob4-25_manipualtion.rossystem create mode 100644 resources/cob4-25_navigation.rossystem diff --git a/resources/cob4-25_manipualtion.rossystem b/resources/cob4-25_manipualtion.rossystem new file mode 100644 index 0000000..7b6a0b5 --- /dev/null +++ b/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"} + }} +)} diff --git a/resources/cob4-25_navigation.rossystem b/resources/cob4-25_navigation.rossystem new file mode 100644 index 0000000..40928cd --- /dev/null +++ b/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'} + }} +)} diff --git a/src/rosgraph_monitor/observers/graph_observer.py b/src/rosgraph_monitor/observers/graph_observer.py index 5b20578..48863ec 100644 --- a/src/rosgraph_monitor/observers/graph_observer.py +++ b/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)