src/openrtm_tools/rtmlaunch.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import roslib
4 roslib.load_manifest('openrtm_tools')
5 
6 import sys
7 import os
8 import time
9 import optparse
10 import socket
11 
12 from xml.dom.minidom import parse
13 
14 import rtctree
15 from rtshell import rtcon
16 from rtshell import path
17 from rtshell import state_control_base
18 from rtshell import rts_exceptions
19 
20 def alive_component(path, tree):
21  if tree.has_path(path) and tree.is_component(path):
22  node = tree.get_node(path)
23  return node.plain_state_string
24  else:
25  tree._root._remove_all_children() # reflesh
26  tree.load_servers_from_env()
27  return False
28 
29 def wait_component(cmd_path, tree):
30  count=0
31  path = rtctree.path.parse_path(cmd_path)[0]
32  node = alive_component(path, tree)
33  if not node:
34  while count < 30:
35  print >>sys.stderr, "\033[33m[rtmlaunch] Wait for ",cmd_path," ",count,"/30\033[0m"
36  node = alive_component(path, tree)
37  if node:
38  return node
39  count += 1
40  time.sleep(1)
41  raise rts_exceptions.NoSuchObjectError(cmd_path)
42  return node
43 
44 def check_connect(src_path, dest_path, tree):
45  src_path, src_port = rtctree.path.parse_path(src_path)
46  dest_path, dest_port = rtctree.path.parse_path(dest_path)
47  src_node = tree.get_node(src_path)
48  dest_node = tree.get_node(dest_path)
49  port = src_node.get_port_by_name(src_port)
50  for conn in port.connections:
51  for name, p in conn.ports:
52  tmp_dest_path, tmp_dest_port = rtctree.path.parse_path(name)
53  if dest_path[-1] == tmp_dest_path[-1] and dest_port == tmp_dest_port:
54  return True
55  return False
56 
57 def replace_arg_tag_by_env (input_path):
58  import re
59  ret_str = input_path
60  if len(input_path.split("$(arg")) > 1:
61  arg_str = input_path.split("$(arg")[1].split(")")[0]
62  env_str = os.getenv(arg_str.replace(" ", ""))
63  if env_str != None:
64  # print >>sys.stderr, "[rtmlaunch] Replace arg tag in %s from [$(arg%s)] to [%s]"%(input_path, arg_str, env_str)
65  ret_str = re.sub("\$\(arg"+arg_str+"\)",env_str,input_path)
66  return ret_str
67 
68 def rtconnect(nameserver, tags, tree):
69  for tag in tags:
70 
71  # check if/unless attribute in rtconnect tags
72  exec_flag = True
73  if tag.attributes.get(u'if'):
74  val = tag.attributes.get(u'if').value
75  arg = val.split(" ")[1].strip(")") # To "USE_WALKING"
76  exec_flag = get_flag_from_argv(arg)
77  if tag.attributes.get(u'unless'):
78  val = tag.attributes.get(u'unless').value
79  arg = val.split(" ")[1].strip(")") # To "USE_WALKING"
80  exec_flag = not get_flag_from_argv(arg)
81  if not exec_flag:
82  continue
83 
84  source_path = nameserver+"/"+tag.attributes.get("from").value
85  dest_path = nameserver+"/"+tag.attributes.get("to").value
86  source_path = replace_arg_tag_by_env(source_path)
87  dest_path = replace_arg_tag_by_env(dest_path)
88  # print >>sys.stderr, "[rtmlaunch] Connecting from %s to %s"%(source_path,dest_path)
89  source_full_path = path.cmd_path_to_full_path(source_path)
90  dest_full_path = path.cmd_path_to_full_path(dest_path)
91  if tag.attributes.get("subscription_type") != None:
92  sub_type = tag.attributes.get("subscription_type").value
93  sub_type = replace_arg_tag_by_env(sub_type);
94  if not sub_type in ['flush','new','periodic']:
95  print >>sys.stderr, sub_type+' is not a subscription type'
96  continue
97  else:
98  sub_type = 'flush' # this is default value
99  if sub_type == 'new':
100  push_policy = 'all'
101  # wait for proess
102  try:
103  wait_component(source_full_path, tree)
104  wait_component(dest_full_path, tree)
105  if check_connect(source_full_path,dest_full_path, tree):
106  continue
107  except Exception, e:
108  print >>sys.stderr, '\033[31m[rtmlaunch] [ERROR] Could not Connect (', source_full_path, ',', dest_full_path, '): ', e,'\033[0m'
109  return 1
110  #print source_path, source_full_path, dest_path, dest_full_path;
111  try:
112  sub_type = str(sub_type)
113  props = {'dataport.subscription_type': sub_type}
114  if tag.attributes.get("push_policy") != None:
115  push_policy = replace_arg_tag_by_env(str(tag.attributes.get("push_policy").value));
116  else:
117  push_policy = 'all'
118  if sub_type == 'new':
119  props['dataport.publisher.push_policy'] = push_policy
120  elif sub_type == 'periodic':
121  props['dataport.publisher.push_policy'] = push_policy
122  if tag.attributes.get("push_rate") != None:
123  props['dataport.push_rate'] = replace_arg_tag_by_env(str(tag.attributes.get("push_rate").value))
124  else:
125  props['dataport.push_rate'] = str('50.0')
126  if tag.attributes.get("buffer_length") != None:
127  props['dataport.buffer.length'] = replace_arg_tag_by_env(str(tag.attributes.get("buffer_length").value))
128  options = optparse.Values({'verbose': False, 'id': '', 'name': None, 'properties': props})
129  print >>sys.stderr, "[rtmlaunch] Connected from",source_path
130  print >>sys.stderr, "[rtmlaunch] to",dest_path
131  print >>sys.stderr, "[rtmlaunch] with",options
132  try :
133  rtcon.connect_ports(source_path, source_full_path, dest_path, dest_full_path, options, tree=tree)
134  except Exception, e_1_1_0: # openrtm 1.1.0
135  try:
136  rtcon.connect_ports([(source_path,source_full_path), (dest_path, dest_full_path)], options, tree=tree)
137  except Exception, e_1_0_0: # openrtm 1.0.0
138  print >>sys.stderr, '\033[31m[rtmlaunch] {0} did not work on both OpenRTM 1.0.0 and 1.1.0'.format(os.path.basename(sys.argv[1])),'\033[0m'
139  print >>sys.stderr, '\033[31m[rtmlaunch] OpenRTM 1.0.0 {0}'.format(e_1_0_0),'\033[0m'
140  print >>sys.stderr, '\033[31m[rtmlaunch] OpenRTM 1.1.0 {0}'.format(e_1_1_0),'\033[0m'
141  print >>sys.stderr, '\033[31m[rtmlaunch] This is very weird situation, Please check your network\033[0m'
142  print >>sys.stderr, '\033[31m[rtmlaunch] configuration with `ifconfig` on both robot and client side. \033[0m'
143  print >>sys.stderr, '\033[31m[rtmlaunch] Having multiple network interface sometimes causes problem, \033[0m'
144  print >>sys.stderr, '\033[31m[rtmlaunch] please see FAQ site http://www.openrtm.org/OpenRTM-aist/html/FAQ2FE38388E383A9E38396E383ABE382B7E383A5E383BCE38386E382A3E383B3E382B0.html#f2bc375d\033[0m'
145  print >>sys.stderr, '\033[31m[rtmlaunch] Issue related to this https://github.com/start-jsk/rtmros_hironx/issues/33\033[0m'
146  print >>sys.stderr, '\033[31m[rtmlaunch] ~/.ros/log may contains usefully informations\033[0m'
147  except Exception, e:
148  print >>sys.stderr, '\033[31m[rtmlaunch] {0}: {1}'.format(os.path.basename(sys.argv[1]), e),'\033[0m'
149  return 0
150 
151 def rtactivate(nameserver, tags, tree):
152  def activate_action(object, ec_index):
153  object.activate_in_ec(ec_index)
154  for tag in tags:
155 
156  # check if/unless attribute in rtactivate tags
157  exec_flag = True
158  if tag.attributes.get(u'if'):
159  val = tag.attributes.get(u'if').value
160  arg = val.split(" ")[1].strip(")") # To "USE_WALKING"
161  exec_flag = get_flag_from_argv(arg)
162  if tag.attributes.get(u'unless'):
163  val = tag.attributes.get(u'unless').value
164  arg = val.split(" ")[1].strip(")") # To "USE_WALKING"
165  exec_flag = not get_flag_from_argv(arg)
166  if not exec_flag:
167  continue
168 
169  cmd_path = nameserver+"/"+tag.attributes.get("component").value
170  full_path = path.cmd_path_to_full_path(cmd_path)
171  full_path = replace_arg_tag_by_env(full_path)
172  # print >>sys.stderr, "[rtmlaunch] activate %s"%(full_path)
173  try:
174  state = wait_component(full_path, tree)
175  if state == 'Active':
176  continue
177  else:
178  print >>sys.stderr, "[rtmlaunch] Activate <-",state,full_path
179  except Exception, e:
180  print >>sys.stderr, '\033[31m[rtmlaunch] Could not Activate (', cmd_path, ') : ', e,'\033[0m'
181  return 1
182  try:
183  options = optparse.Values({"ec_index": 0, 'verbose': False})
184  try :
185  state_control_base.alter_component_state(activate_action, cmd_path, full_path, options, tree=tree)
186  except Exception, e: # openrtm 1.1.0
187  state_control_base.alter_component_states(activate_action, [(cmd_path, full_path)], options, tree=tree)
188  except Exception, e:
189  print >>sys.stderr, '[rtmlaunch] {0}: {1}'.format(os.path.basename(sys.argv[0]), e)
190  return 1
191  return 0
192 
193 def main():
194  usage = '''Usage: %prog [launchfile]'''
195  if len(sys.argv) <= 1:
196  print >>sys.stderr, usage
197  return 1
198  fullpathname = sys.argv[1]
199  print >>sys.stderr, "[rtmlaunch] starting... ",fullpathname
200  try:
201  parser = parse(fullpathname)
202  nodes = parser.getElementsByTagName("launch")[0].childNodes
203  remove_nodes = []
204  for node in nodes:
205  if node.nodeName == u'group':
206  if node.attributes.get(u'if'):
207  val = node.getAttributeNode(u'if').value
208  arg = val.split(" ")[1].strip(")") # To "USE_WALKING"
209  if not get_flag_from_argv(arg):
210  remove_nodes.append(node)
211  if node.attributes.get(u'unless'):
212  val = node.getAttributeNode(u'unless').value
213  arg = val.split(" ")[1].strip(")") # To "USE_WALKING"
214  if get_flag_from_argv(arg):
215  remove_nodes.append(node)
216 
217  for remove_node in remove_nodes:
218  nodes.remove(remove_node)
219  except Exception,e:
220  print e
221  return 1
222 
223  if os.getenv("RTCTREE_NAMESERVERS") == None:
224  print >>sys.stderr, "[rtmlaunch] RTCTREE_NAMESERVERS is not set, use localhost:15005"
225  nameserver = "localhost:15005"
226  os.environ["RTCTREE_NAMESERVERS"] = nameserver
227  else:
228  nameserver = os.getenv("RTCTREE_NAMESERVERS")
229 
230  print >>sys.stderr, "\033[32m[rtmlaunch] RTCTREE_NAMESERVERS", nameserver, os.getenv("RTCTREE_NAMESERVERS"), "\033[0m"
231  print >>sys.stderr, "\033[32m[rtmlaunch] SIMULATOR_NAME", os.getenv("SIMULATOR_NAME","Simulator"), "\033[0m"
232 
233  try:
234  tree = rtctree.tree.RTCTree()
235  except Exception, e:
236  print >>sys.stderr, "\033[31m[rtmlaunch] Could not start rtmlaunch.py, Caught exception (", e, ")\033[0m"
237  # check if host is connected
238  try:
239  hostname = nameserver.split(':')[0]
240  ip_address = socket.gethostbyname(hostname)
241  except Exception, e:
242  print >>sys.stderr, "\033[31m[rtmlaunch] .. Could not find IP address of ", hostname, ", Caught exception (", e, ")\033[0m"
243  print >>sys.stderr, "\033[31m[rtmlaunch] .. Please check /etc/hosts or DNS setup\033[0m"
244  return 1
245  # in this case, it is likely you forget to run name serveer
246  print >>sys.stderr, "\033[31m[rtmlaunch] .. Could not connect to NameServer at ", nameserver, ", Caught exception (", e, ")\033[0m"
247  print >>sys.stderr, "\033[31m[rtmlaunch] .. Please make sure that you have NameServer running at %s/`\033[0m"%(nameserver)
248  print >>sys.stderr, "\033[31m[rtmlaunch] .. You can check with `rtls %s/`\033[0m"%(nameserver)
249  return 1
250  while 1:
251  print >>sys.stderr, "[rtmlaunch] check connection/activation"
252  rtconnect(nameserver, parser.getElementsByTagName("rtconnect"), tree)
253  rtactivate(nameserver, parser.getElementsByTagName("rtactivate"), tree)
254  time.sleep(10)
255  tree.add_name_server(nameserver, [])
256  if os.getenv("RTC_CONNECTION_CHECK_ONCE") and os.getenv("RTC_CONNECTION_CHECK_ONCE").lower() == "true":
257  print >>sys.stderr, "[rtmlaunch] break from rtmlaunch main loop."
258  print >>sys.stderr, "[rtmlaunch] If you check the rtc connection in the while loop, real-time loop becomes slow."
259  break
260 def get_flag_from_argv(arg):
261  for a in sys.argv:
262  if arg in a: # If "USE_WALKING" is in argv
263  return True if 'true' in a.split("=")[1] else False
264 
265 
266 
267 
openrtm_tools.rtmlaunch.main
def main()
Definition: src/openrtm_tools/rtmlaunch.py:193
openrtm_tools.rtmlaunch.alive_component
def alive_component(path, tree)
Definition: src/openrtm_tools/rtmlaunch.py:20
openrtm_tools.rtmlaunch.wait_component
def wait_component(cmd_path, tree)
Definition: src/openrtm_tools/rtmlaunch.py:29
openrtm_tools.rtmlaunch.check_connect
def check_connect(src_path, dest_path, tree)
Definition: src/openrtm_tools/rtmlaunch.py:44
openrtm_tools.rtmlaunch.replace_arg_tag_by_env
def replace_arg_tag_by_env(input_path)
Definition: src/openrtm_tools/rtmlaunch.py:57
openrtm_tools.rtmlaunch.rtactivate
def rtactivate(nameserver, tags, tree)
Definition: src/openrtm_tools/rtmlaunch.py:151
openrtm_tools.rtmlaunch.get_flag_from_argv
def get_flag_from_argv(arg)
Definition: src/openrtm_tools/rtmlaunch.py:260
openrtm_tools.rtmlaunch.rtconnect
def rtconnect(nameserver, tags, tree)
Definition: src/openrtm_tools/rtmlaunch.py:68

openrtm_tools
Author(s): Kei Okada
autogenerated on Mon May 10 2021 02:30:55