00001
00002 import rospy
00003 from roslaunch.config import ROSLaunchConfig
00004 from roslaunch.core import Param
00005
00006 class LaunchtreeArg(object):
00007 def __init__(self, name, default=None, value=None, doc=None):
00008 self.name = name
00009 self.default = default
00010 self.value = value
00011 self.doc = doc
00012 def merge(self, other):
00013 if self.default is None and other.default is not None:
00014 self.default = other.default
00015 if self.value is None and other.value is not None:
00016 self.value = other.value
00017 if self.doc is None and other.doc is not None:
00018 self.doc = other.doc
00019
00020 class LaunchtreeRemap(object):
00021 def __init__(self, from_topic, to_topic):
00022 self.from_topic = from_topic
00023 self.to_topic = to_topic
00024
00025 class LaunchtreeParam(Param):
00026 def __init__(self, key, value, command):
00027 super(LaunchtreeParam, self).__init__(key, value)
00028 self.command = command
00029
00030 class LaunchtreeRosparam(object):
00031 def __init__(self, command, filename):
00032 self.command = command
00033 self.filename = filename
00034
00035 class LaunchtreeConfig(ROSLaunchConfig):
00036
00037 def __init__(self):
00038 super(LaunchtreeConfig, self).__init__()
00039
00040 self._tree_stack = list()
00041 self.tree = dict()
00042 self.idx = 0
00043
00044 def push_level(self, tree_level, unique=False):
00045 if unique:
00046 tree_level += ':%d' % self.idx
00047 self.idx += 1
00048 self._tree_stack.append(tree_level)
00049 return tree_level
00050
00051 def pop_level(self):
00052 return self._tree_stack.pop()
00053
00054 def _add_to_tree(self, key, instance):
00055 level = self.tree
00056 for launch in self._tree_stack:
00057 if not launch in level:
00058 level[launch] = dict()
00059 elif not isinstance(level[launch], dict):
00060 old_instance = level[launch]
00061 level[launch] = dict()
00062 level[launch + ':%d' % self.idx] = old_instance
00063 self.idx += 1
00064 level = level[launch]
00065 if level.has_key(key):
00066 if isinstance(level[key], dict) and not '_root' in level[key]:
00067
00068 level[key]['_root'] = instance
00069 elif isinstance(instance, LaunchtreeArg) and isinstance(level[key], LaunchtreeArg):
00070 level[key].merge(instance)
00071 else:
00072 level[key + ':%d' % self.idx] = instance
00073 self.idx += 1
00074 else:
00075 level[key] = instance
00076
00077 def add_executable(self, exe):
00078 result = super(LaunchtreeConfig, self).add_executable(exe)
00079 self._add_to_tree(exe.command, exe)
00080 return result
00081
00082 def add_param(self, p, filename=None, verbose=True, command=None):
00083 p.inconsistent = False
00084 if p.key in self.params and (p.value != self.params[p.key].value or self.params[p.key].inconsistent):
00085 p.inconsistent = True
00086 self.params[p.key].inconsistent = True
00087 rospy.logwarn('Inconsistent param: %s\n - %s: %s\n - %s: %s' %
00088 (p.key, p.key, str(p.value), self.params[p.key].key, str(self.params[p.key].value)))
00089 result = super(LaunchtreeConfig, self).add_param(p, filename, verbose)
00090 self._add_to_tree(p.key, p)
00091 return result
00092
00093 def add_machine(self, m, verbose=True):
00094 result = super(LaunchtreeConfig, self).add_machine(m, verbose)
00095 self._add_to_tree(m.name, m)
00096 return result
00097
00098 def add_test(self, test, verbose=True):
00099 result = super(LaunchtreeConfig, self).add_test(test, verbose)
00100 self._add_to_tree(test.name, test)
00101 return result
00102
00103 def add_node(self, node, core=False, verbose=True):
00104 result = super(LaunchtreeConfig, self).add_node(node, core, verbose)
00105 self._add_to_tree(node.name, node)
00106 return result
00107
00108 def add_arg(self, name, default=None, value=None, doc=None):
00109 self._add_to_tree(name, LaunchtreeArg(name, default, value, doc))
00110
00111 def add_remap(self, from_topic, to_topic):
00112 self._add_to_tree(from_topic, LaunchtreeRemap(from_topic, to_topic))
00113
00114 def add_rosparam(self, command, filename, unique_name):
00115 self._add_to_tree(unique_name, LaunchtreeRosparam(command, filename))