diff --git a/CMakeLists.txt b/CMakeLists.txt index e2619ad..b080318 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,7 +9,8 @@ catkin_python_setup() add_service_files( FILES - Scan.srv + GetROSModel.srv + GetROSSystemModel.srv ) generate_messages() @@ -18,10 +19,6 @@ catkin_package( CATKIN_DEPENDS message_runtime ) -install(DIRECTORY launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - catkin_install_python(PROGRAMS scripts/ros_node scripts/print_snapshot scripts/java_snapshot diff --git a/scripts/ros_node b/scripts/ros_node index 893630e..aa73dbe 100755 --- a/scripts/ros_node +++ b/scripts/ros_node @@ -8,26 +8,24 @@ import rospy class RosGraphNode(): def __init__(self): - rospy.Service('scan_ros_graph', rg_srv.Scan, self.create_snapshot) - - def create_snapshot(self, request): - answer = rg_srv.ScanResponse() - answer.success, answer.message, answer.rosgraph = self._create_snapshot() + rospy.Service('get_ros_model', rg_srv.GetROSModel, + self.get_ros_model) + + rospy.Service('get_rossystem_model', rg_srv.GetROSSystemModel, + self.get_rossystem_model) + + def get_ros_model(self, request): + answer = rg_srv.GetROSModelResponse() + answer.success, answer.message, answer.model = rg.create_java_ros_model() return answer - def _create_snapshot(self): - try: - snapshot = rg.create_ros_graph_snapshot() - dump = dict() - dump['nodes']=dict() - for node in snapshot: - dump['nodes'][node.name] = node.dump_yaml() - return True, "Scanning Succeeded", str(dump) - except: - return False, "Scanning Failed", "" + def get_rossystem_model(self, request): + answer = rg_srv.GetROSSystemModelResponse() + answer.success, answer.message, answer.model = rg.create_java_system_model() + return answer if __name__ == "__main__": rospy.init_node("ros_graph_parser_node") RosGraphNode() - rospy.spin() \ No newline at end of file + rospy.spin() diff --git a/src/ros_graph_parser/core_class.py b/src/ros_graph_parser/core_class.py index bf227cc..cd12949 100644 --- a/src/ros_graph_parser/core_class.py +++ b/src/ros_graph_parser/core_class.py @@ -2,8 +2,9 @@ import rosgraph -ACTION_FILTER = ['goal','cancel'] -ACTION_FILTER2= ['status', 'result', 'feedback'] +ACTION_FILTER = ['goal', 'cancel'] +ACTION_FILTER2 = ['status', 'result', 'feedback'] + def get_namespace(name): try: @@ -13,6 +14,7 @@ def get_namespace(name): print(ex) return "" + class Interface(object): def __init__(self, name, itype): self.resolved = name @@ -25,19 +27,20 @@ def __eq__(self, other): return True else: return False - + def get_dict(self): - return {"Type":self.itype, "Name": self.resolved, - "Namespace":self.namespace, "Minimal":self.minimal} - + return {"Type": self.itype, "Name": self.resolved, + "Namespace": self.namespace, "Minimal": self.minimal} + def str_format(self, indent=""): - return ("%sType: %s\n%sName: %s\n%sNamespace: %s\n%sMinimal: %s\n")%( - indent, self.itype, indent, self.resolved, indent, + return ("%sType: %s\n%sName: %s\n%sNamespace: %s\n%sMinimal: %s\n") % ( + indent, self.itype, indent, self.resolved, indent, self.namespace, indent, self.minimal) def java_format(self, indent="", name_type="", interface_type=""): - return ("%s%s { name '%s' %s '%s'}")%( - indent, name_type, self.resolved, interface_type, self.itype.replace("/",".")) + return ("%s%s { name '%s' %s '%s'}") % ( + indent, name_type, self.resolved, interface_type, self.itype.replace("/", ".")) + class InterfaceSet(set): @@ -45,19 +48,19 @@ def get_with_name(self, name): for elem in self: if name == elem.resolved: return elem - raise KeyError("Interface with Name '%s' not found."%(name)) + raise KeyError("Interface with Name '%s' not found." % (name)) def get_with_minimal(self, name): for elem in self: if name == elem.minimal: return elem - raise KeyError("Interface with Minimal Name '%s' not found."%(name)) - + raise KeyError("Interface with Minimal Name '%s' not found." % (name)) + def get_with_type(self, itype): for elem in self: if itype == elem.itype: return elem - raise KeyError("No Interface of Type '%s' found."%(itype)) + raise KeyError("No Interface of Type '%s' found." % (itype)) def remove_with_name(self, name): self.remove(self.get_with_name(name)) @@ -71,11 +74,12 @@ def str_format(self, indent=""): def java_format_ros_model(self, indent="", name_type="", interface_type="", name_block=""): if len(self) == 0: return "" - str_ = ("\n%s%s {\n")%(indent, name_block) + str_ = ("\n%s%s {\n") % (indent, name_block) for elem in self: - str_+= elem.java_format(indent+" ", name_type, interface_type) +",\n" + str_ += elem.java_format(indent+" ", + name_type, interface_type) + ",\n" str_ = str_[:-2] - str_+="}" + str_ += "}" return str_ def java_format_system_model(self, indent="", name_type="", name_type2="", node_name="", pkg_name="", name_type3=""): @@ -83,12 +87,187 @@ def java_format_system_model(self, indent="", name_type="", name_type2="", node_ return "" if not name_type3: name_type3 = name_type2 - str_ = ("%sRos%s {\n")%(indent, name_type) + str_ = ("%sRos%s {\n") % (indent, name_type) for elem in self: - str_ += ("%s Ros%s '%s' {Ref%s '%s.%s.%s.%s'},\n")%( + str_ += ("%s Ros%s '%s' {Ref%s '%s.%s.%s.%s'},\n") % ( indent, name_type3, elem.resolved, name_type2, pkg_name, node_name, node_name, elem.resolved) str_ = str_[:-2] - str_+="}\n" + str_ += "}\n" + return str_ + + def get_list(self): + return [x.get_dict() for x in self] + + def __str__(self): + self.str_format() + + def iteritems(self): + return [(x.resolved, x.itype) for x in self] + + def iterkeys(self): + return [x.resolved for x in self] + + +class ParameterInterface(object): + + def __init__(self, name, value, itype): + self.resolved = name + self.namespace = get_namespace(name) + self.minimal = name[len(self.namespace)-1:] + self.value = value + self.itype = self.get_type(value) + self.count = 0 + + def __eq__(self, other): + if self.value == other.value and self.resolved == other.resolved: + return True + else: + return False + + def get_type(self, value): + itype = type(value) + itype = (str(itype)).replace("", "") + if itype == 'float': + return 'Double' + elif itype == 'bool': + return 'Boolean' + elif itype == 'int': + return 'Integer' + elif itype == 'str': + return 'String' + elif itype == 'list' or itype == 'dict': + if ":" in str(value): + return 'Struc' + else: + return 'List' + else: + return itype + + def set_value(self, value, indent): + str_param_value = "" + if self.itype == "String": + str_param_value += "'"+self.value+"'" + elif self.itype == "Boolean": + str_param_value += str(self.value).lower() + elif self.itype == "List": + str_param_value += str(self.value).replace( + "[", "{").replace("]", "}") + elif self.itype == 'Struc': + str_param_value += self.value_struc(self.value[0], indent+" ") + else: + str_param_value += str(value) + return str_param_value + + def get_dict(self): + return {"Value": self.value, "Name": self.resolved, + "Namespace": self.namespace, "Minimal": self.minimal} + + def str_format(self, indent=""): + return ("%sType: %s\n%sName: %s\n%sNamespace: %s\n%sMinimal: %s\n") % ( + indent, self.value, indent, self.resolved, indent, + self.namespace, indent, self.minimal) + + def java_format(self, indent="", value=""): + str_param = "%sParameter { name '%s' type %s " % ( + indent, self.resolved, self.itype) + if self.itype == 'Struc': + str_param += self.types_struc(self.value[0], indent) + #str_param = str_param[:-2] + if self.itype == 'List': + str_param += self.form_list(self.value) + str_param += "}" + return str_param + + def types_struc(self, struc_dict, indent): + str_param = "{\n" + indent_new = indent+" " + for struc_element in struc_dict: + sub_name = struc_element + sub_value = struc_dict[struc_element] + sub_type = self.get_type(sub_value) + str_param += "%s'%s' %s" % (indent_new, sub_name, sub_type) + if sub_type == 'List': + str_param += self.form_list(sub_value) + if isinstance(sub_value, dict): + str_param += self.types_struc( + struc_dict[struc_element], indent_new) + str_param += ",\n" + str_param = str_param[:-2] + str_param += "}" + indent_new = "" + return str_param + + def value_struc(self, struc_dict, indent): + str_param = "{\n" + indent_new = indent+" " + for struc_element in struc_dict: + sub_name = struc_element + sub_value = struc_dict[struc_element] + sub_type = self.get_type(sub_value) + str_param += "%s{ '%s' { value " % (indent_new, sub_name) + if sub_type == "String": + sub_value = "'"+sub_value+"'" + if sub_type == 'List': + sub_value = str(sub_value).replace( + "[", "{").replace("]", "}").replace("{{", "{").replace("}}", "}") + if sub_type == "Boolean": + sub_value = str(sub_value).lower() + if isinstance(sub_value, dict): + str_param += self.value_struc( + struc_dict[struc_element], indent_new) + self.count = self.count + 1 + else: + str_param += "%s}}" % (sub_value) + str_param += ",\n" + str_param = str_param[:-2] + str_param += "}" + if self.count == 1: + str_param += "}}" + self.count = self.count - 1 + indent_new = "" + return str_param + + def form_list(self, value_in): + str_param = "{" + for i in value_in: + str_param += self.get_type(i) + if self.get_type(i) == "List": + str_param += self.form_list(i) + str_param += "," + str_param = str_param[:-1] + str_param += "}" + return str_param + + +class ParameterSet(set): + + def str_format(self, indent=""): + str_ = "" + for elem in self: + str_ += elem.str_format(indent) + "\n" + return str_ + + def java_format_ros_model(self, indent="", value="", name_block=""): + if len(self) == 0: + return "" + str_ = ("\n%s%s {\n") % (indent, name_block) + for elem in self: + str_ += elem.java_format(indent+" ", value) + ",\n" + str_ = str_[:-2] + str_ += "}" + return str_ + + def java_format_system_model(self, indent="", name_type="", name_type2="", node_name="", pkg_name="", name_type3=""): + if len(self) == 0: + return "" + if not name_type3: + name_type3 = name_type2 + str_ = ("%sRos%s {\n") % (indent, name_type) + for elem in self: + str_ += ("%s Ros%s '%s' {Ref%s '%s.%s.%s.%s' value %s},\n") % ( + indent, name_type3, elem.resolved, name_type2, pkg_name, node_name, node_name, elem.resolved, elem.set_value(elem.value, indent)) + str_ = str_[:-2] + str_ += "}\n" return str_ def get_list(self): @@ -102,7 +281,7 @@ def iteritems(self): def iterkeys(self): return [x.resolved for x in self] - + class Node(object): def __init__(self, name=""): @@ -111,7 +290,9 @@ def __init__(self, name=""): self.action_servers = InterfaceSet() self.publishers = InterfaceSet() self.subscribers = InterfaceSet() - self.services = InterfaceSet() + self.service_clients = InterfaceSet() + self.service_servers = InterfaceSet() + self.params = ParameterSet() def get_namespace(self): return get_namespace(self.name) @@ -129,7 +310,7 @@ def _clean_action_server(self): self.subscribers.remove_with_name(name+name_) for name_ in ACTION_FILTER2: self.publishers.remove_with_name(name+name_) - + def check_actions(self): # Check Action client for topic_name, topic_type in self.publishers.iteritems(): @@ -140,8 +321,8 @@ def check_actions(self): for name in ACTION_FILTER2: if not (_action_name + name in self.subscribers.iterkeys()): continue - _action_type = topic_type[:-10] # Hardcoded ActionGoal - self.action_clients.add(Interface(_action_name,_action_type)) + _action_type = topic_type[:-10] # Hardcoded ActionGoal + self.action_clients.add(Interface(_action_name, _action_type)) self._clean_action_client() # Check Action Server for topic_name, topic_type in self.subscribers.iteritems(): @@ -152,48 +333,70 @@ def check_actions(self): for name in ACTION_FILTER2: if not (_action_name + name in self.publishers.iterkeys()): continue - _action_type = topic_type[:-10] # Hardcode ActionGoal - self.action_servers.add(Interface(_action_name,_action_type)) + _action_type = topic_type[:-10] # Hardcode ActionGoal + self.action_servers.add(Interface(_action_name, _action_type)) self._clean_action_server() def dump_print(self): - _str="" - _str = "Node: \n\t%s"%(self.name) - _str = _str +"\tPublishers:\n%s"%(self.publishers.str_format('\t\t')) - _str = _str +"\tSubscribers:\n%s"%(self.subscribers.str_format('\t\t')) - _str = _str +"\tServices:\n%s"%(self.services.str_format('\t\t')) - _str = _str +"\tActionClients:\n%s"%(self.action_clients.str_format('\t\t')) - _str = _str +"\tActionServers:\n%s"%(self.action_servers.str_format('\t\t')) + _str = "" + _str = "Node: \n\t%s" % (self.name) + _str = _str + \ + "\tPublishers:\n%s" % (self.publishers.str_format('\t\t')) + _str = _str + \ + "\tSubscribers:\n%s" % (self.subscribers.str_format('\t\t')) + _str = _str + "\tServices:\n%s" % (self.service_servers.str_format('\t\t')) + _str = _str + \ + "\tActionClients:\n%s" % (self.action_clients.str_format('\t\t')) + _str = _str + \ + "\tActionServers:\n%s" % (self.action_servers.str_format('\t\t')) + _str = _str + "\tParameters:\n%s" % (self.params.str_format('\t\t')) _str = _str + ("\n") print(_str) def dump_yaml(self): - yaml_dict=dict() + yaml_dict = dict() yaml_dict['Publishers'] = self.publishers.get_list() yaml_dict['Subscribers'] = self.subscribers.get_list() - yaml_dict['Services'] = self.services.get_list() + yaml_dict['Services'] = self.service_servers.get_list() yaml_dict['ActionClients'] = self.action_clients.get_list() yaml_dict['ActionServers'] = self.action_servers.get_list() + yaml_dict['Parameters'] = self.params.get_list() return yaml_dict def dump_java_ros_model(self): - ros_model_str=" Artifact "+self.name+" {\n" - ros_model_str+=" node Node { name "+ self.name+"\n" - ros_model_str+=self.services.java_format_ros_model(" ", "ServiceServer", "service","serviceserver") - ros_model_str+=self.publishers.java_format_ros_model(" ", "Publisher", "message","publisher") - ros_model_str+=self.subscribers.java_format_ros_model(" ", "Subscriber", "message", "subscriber") - ros_model_str+=self.action_servers.java_format_ros_model(" ", "ActionServer", "action","actionserver") - ros_model_str+=self.action_clients.java_format_ros_model(" ", "ActionClient", "action","actionclient") - ros_model_str+="}},\n" + ros_model_str = " Artifact "+self.name+" {\n" + ros_model_str += " node Node { name " + self.name+"\n" + ros_model_str += self.service_servers.java_format_ros_model( + " ", "ServiceServer", "service", "serviceserver") + ros_model_str += self.service_clients.java_format_ros_model( + " ", "ServiceClients", "service", "serviceclient") + ros_model_str += self.publishers.java_format_ros_model( + " ", "Publisher", "message", "publisher") + ros_model_str += self.subscribers.java_format_ros_model( + " ", "Subscriber", "message", "subscriber") + ros_model_str += self.action_servers.java_format_ros_model( + " ", "ActionServer", "action", "actionserver") + ros_model_str += self.action_clients.java_format_ros_model( + " ", "ActionClient", "action", "actionclient") + ros_model_str += self.params.java_format_ros_model( + " ", "Parameters", "parameter") + ros_model_str += "}},\n" return ros_model_str def dump_java_system_model(self, package=""): - system_model_str=" ComponentInterface { name '"+self.name+"'\n" - system_model_str+=self.publishers.java_format_system_model(" ", "Publishers", "Publisher", self.name, package) - system_model_str+=self.subscribers.java_format_system_model(" ", "Subscribers", "Subscriber",self.name, package) - system_model_str+=self.services.java_format_system_model(" ", "SrvServers", "Server", self.name, package, "ServiceServer") - system_model_str+=self.action_servers.java_format_system_model(" ", "ActionServers", "Server", self.name, package) - system_model_str+=self.action_clients.java_format_system_model(" ", "ActionClients", "Client", self.name, package) - system_model_str+="},\n" + system_model_str = " ComponentInterface { name '" + \ + self.name+"'\n" + system_model_str += self.publishers.java_format_system_model( + " ", "Publishers", "Publisher", self.name, package) + system_model_str += self.subscribers.java_format_system_model( + " ", "Subscribers", "Subscriber", self.name, package) + system_model_str += self.service_servers.java_format_system_model( + " ", "SrvServers", "Server", self.name, package, "ServiceServer") + system_model_str += self.action_servers.java_format_system_model( + " ", "ActionServers", "ActionServer", self.name, package) + system_model_str += self.action_clients.java_format_system_model( + " ", "ActionClients", "ActionClient", self.name, package) + system_model_str += self.params.java_format_system_model( + " ", "Parameters", "Parameter", self.name, package) + system_model_str += "},\n" return system_model_str - diff --git a/src/ros_graph_parser/snapshot.py b/src/ros_graph_parser/snapshot.py index 1b3b4b2..5a9e9e0 100644 --- a/src/ros_graph_parser/snapshot.py +++ b/src/ros_graph_parser/snapshot.py @@ -1,16 +1,18 @@ #!/usr/bin/env python import rosgraph +import rosparam import rosservice import ros_graph_parser.core_class as rg import yaml -BLACK_LIST_PARAM = ['/rosdistro', '/rosversion', '/run_id'] +BLACK_LIST_PARAM = ['/rosdistro', '/rosversion', '/run_id','robot_description','/docker_control/stations','/docking_laser_filter/scan_filter_chain','/bms/diagnostics','/station_detector/stations','/scan_unifier_filter/scan_filter_chain'] BLACK_LIST_TOPIC = ["/tf", "/tf_static", "/rosout", "/clock"] BLACK_LIST_SERV = ["/set_logger_level", "/get_loggers"] BLACK_LIST_NODE = ["/rosout"] -ACTION_FILTER = ['cancel','goal','status', 'result', 'feedback'] +ACTION_FILTER = ['cancel', 'goal', 'status', 'result', 'feedback'] + def check_black_list(name, black_list): for bl_ in black_list: @@ -18,6 +20,7 @@ def check_black_list(name, black_list): return False return True + def create_ros_graph_snapshot(): master = rosgraph.Master('snapshot') node_names = list() @@ -25,6 +28,7 @@ def create_ros_graph_snapshot(): params = list() services_dict = dict() topics_dict = dict() + parameter_dict = dict() if not(master.is_online()): print("Error: ROSMaster not found") @@ -33,17 +37,21 @@ def create_ros_graph_snapshot(): for param_name in master.getParamNames(): if param_name not in BLACK_LIST_PARAM and not(param_name.startswith('/roslaunch')): params.append(param_name) - state = master.getSystemState() #get the system state + state = master.getSystemState() # get the system state pubs, subs, services = state - #get all topics type + # get all topics type topic_list = master.getTopicTypes() for topic, topic_type in topic_list: topics_dict[topic] = topic_type - #get all service types + # get all service types for service_name, _ in services: - services_dict[service_name] = rosservice.get_service_type(service_name) + try: + services_dict[service_name] = rosservice.get_service_type( + service_name) + except: + pass # Get all nodes for s in state: @@ -51,7 +59,7 @@ def create_ros_graph_snapshot(): for n in l: if n not in BLACK_LIST_NODE: node_names.append(n) - + node_names = list(set(node_names)) for n in node_names: node = rg.Node(n) @@ -73,45 +81,77 @@ def create_ros_graph_snapshot(): node.check_actions() nodes.append(node) - + + node_param = rg.Node("parameters_node") + for param_name in params: + node_param.params.add(rg.ParameterInterface( + param_name, master.getParam(param_name), type(master.getParam(param_name)))) + nodes.append(node_param) + return nodes - + + def dump_print(snapshot): for node in snapshot: node.dump_print() + def dump_yaml(snapshot, file): dump = dict() - dump['nodes']=dict() + dump['nodes'] = dict() for node in snapshot: dump['nodes'][node.name] = node.dump_yaml() with open(file, 'w') as outfile: yaml.dump(dump, outfile, default_flow_style=False) + def dump_java_ros_model(snapshot, ros_model_file, pkg_name): - ros_model_str = "PackageSet { package { \n" - ros_model_str+=" CatkinPackage "+pkg_name +" { " - ros_model_str+="artifact {\n" - for node in snapshot: - ros_model_str+=node.dump_java_ros_model() - ros_model_str = ros_model_str[:-2] - ros_model_str+="\n}}}}" + sucess, text_msg, ros_model_str = create_java_ros_model(pkg_name) with open(ros_model_file, 'w') as outfile: outfile.write(ros_model_str) + def dump_java_system_model(snapshot, system_name, system_model_file, pkg_name): - system_model_str = "RosSystem { Name '%s'\n"%system_name - system_model_str+=" RosComponents ( \n" - for node in snapshot: - system_model_str+=node.dump_java_system_model(pkg_name) - system_model_str = system_model_str[:-2] - system_model_str+="\n)}" + sucess, text_msg, system_model_str = create_java_system_model( + system_name, pkg_name) with open(system_model_file, 'w') as outfile: outfile.write(system_model_str) + +def create_java_ros_model(pkg_name="dummy_pkg"): + try: + snapshot = create_ros_graph_snapshot() + ros_model_str = "PackageSet { package { \n" + ros_model_str += " CatkinPackage "+pkg_name + " { " + ros_model_str += "artifact {\n" + for node in snapshot: + ros_model_str += node.dump_java_ros_model() + ros_model_str = ros_model_str[:-2] + ros_model_str += "\n}}}}" + except: + return False, "Scanning Failed", "" + return True, "Scanning succeeded", ros_model_str + + +def create_java_system_model(system_name="dummy_system", pkg_name="dummy_pkg"): + try: + snapshot = create_ros_graph_snapshot() + system_model_str = "RosSystem { Name '%s'\n" % system_name + system_model_str += " RosComponents ( \n" + for node in snapshot: + system_model_str += node.dump_java_system_model(pkg_name) + system_model_str = system_model_str[:-2] + system_model_str += "\n)}" + except: + return False, "Scanning Failed", "" + + return True, "Scanning succeeded", system_model_str + + if __name__ == "__main__": snapshot = create_ros_graph_snapshot() dump_print(snapshot) dump_yaml(snapshot, "ros_snapshot.yml") dump_java_ros_model(snapshot, "dump.ros", "dummy_package") - dump_java_system_model(snapshot, "mysystem", "Mysystem.rossystem", "dummy_package") + dump_java_system_model(snapshot, "mysystem", + "Mysystem.rossystem", "dummy_package") diff --git a/srv/Scan.srv b/srv/GetROSModel.srv similarity index 64% rename from srv/Scan.srv rename to srv/GetROSModel.srv index 671734e..bf6c3fa 100644 --- a/srv/Scan.srv +++ b/srv/GetROSModel.srv @@ -1,5 +1,5 @@ - #Empty Trigger +#Empty Trigger --- bool success # indicate successful run of triggered service string message # informational, e.g. for error messages -string rosgraph # yaml format of rosgraph (pickled) \ No newline at end of file +string model # yaml format of rosgraph (pickled) \ No newline at end of file diff --git a/srv/GetROSSystemModel.srv b/srv/GetROSSystemModel.srv new file mode 100644 index 0000000..bf6c3fa --- /dev/null +++ b/srv/GetROSSystemModel.srv @@ -0,0 +1,5 @@ +#Empty Trigger +--- +bool success # indicate successful run of triggered service +string message # informational, e.g. for error messages +string model # yaml format of rosgraph (pickled) \ No newline at end of file