def load(self, package, file, argv, autostart=False):
'''
Load the launch file configuration
@param package: the package containing the launch file, or empty string to load
the given file
@type package: C{str}
@param file: the launch file or complete path, if the package is empty
@type file: C{str}
@param argv: the argv needed to load the launch file
@type argv: C{str}
@param autostart: starts all nodes after load
@type autostart: C{bool}
'''
with self.__lock:
# shutdown the services to inform the caller about a new configuration.
if not self.runService is None:
self.runService.shutdown('reload config')
self.runService = None
if not self.listService is None:
self.listService.shutdown('reload config')
self.listService = None
self.nodes = [] # the name of nodes with namespace
self.sensors = {} # sensor descriptions
self.launch_file = launch_file = self.getPath(file, package)
rospy.loginfo("loading launch file: %s", launch_file)
self.masteruri = self._masteruri_from_ros()
self.roscfg = roslaunch.ROSLaunchConfig()
loader = roslaunch.XmlLoader()
argv = [a for a in argv if not a.startswith('__ns:=')]
# remove namespace from sys.argv to avoid load the launchfile info local namespace
sys.argv = [a for a in sys.argv if not a.startswith('__ns:=')]
# set the global environment to empty namespace
os.environ[ROS_NAMESPACE] = rospy.names.SEP
loader.load(launch_file, self.roscfg, verbose=False, argv=argv)
# create the list with node names
for item in self.roscfg.nodes:
if item.machine_name and not item.machine_name == 'localhost':
machine = self.roscfg.machines[item.machine_name]
if roslib.network.is_local_address(machine.address):
self.nodes.append(roslib.names.ns_join(item.namespace, item.name))
else:
self.nodes.append(roslib.names.ns_join(item.namespace, item.name))
# get the robot description
self.description_response = dr = ListDescriptionResponse()
dr.robot_name = ''
dr.robot_type = ''
dr.robot_descr = ''
for param, p in self.roscfg.params.items():
if param.endswith('robots'):
if isinstance(p.value, list):
if len(p.value) > 0 and len(p.value[0]) != 5:
print "WRONG format, expected: ['host(ROS master Name)', 'type', 'name', 'images', 'description'] -> ignore", param
else:
for entry in p.value:
try:
print entry[0], rospy.get_param('/mastername', '')
if not entry[0] or entry[0] == rospy.get_param('/mastername', ''):
dr.robot_name = self._decode(entry[2])
dr.robot_type = entry[1]
dr.robot_images = entry[3].split()
dr.robot_descr = self._decode(entry[4])
break
except:
pass
# get the sensor description
tmp_cap_dict = self.getCapabilitiesDesrc()
for machine, ns_dict in tmp_cap_dict.items():
if self.roscfg.machines.has_key(machine):
machine = self.roscfg.machines[machine].address
if not machine or roslib.network.is_local_address(machine):
for ns, group_dict in ns_dict.items():
for group, descr_dict in group_dict.items():
if descr_dict['nodes']:
cap = Capability()
cap.namespace = ns
cap.name = group
cap.type = descr_dict['type']
cap.images = list(descr_dict['images'])
cap.description = descr_dict['description']
cap.nodes = list(descr_dict['nodes'])
dr.capabilities.append(cap)
# initialize the ROS services
self._timed_service_creation()
#HACK to let the node_manager to update the view
# t = threading.Timer(2.0, self._timed_service_creation)
# t.start()
self.loadParams()
# self.timer = rospy.Timer(rospy.Duration(2), self.timed_service_creation, True)
# if self.nodes:
# self.runService = rospy.Service('~run', Task, self.rosservice_start_node)
# self.listServic = rospy.Service('~list_nodes', ListNodes, self.rosservice_list_nodes)
# except:
# import traceback
# print traceback.format_exc()
if autostart:
for n in self.nodes:
self.runNode(n)
def load(self, delay_service_creation=0.):
'''
Load the launch file configuration
'''
with self.__lock:
self._pending_starts.clear()
# shutdown the services to inform the caller about a new configuration.
if not self.runService is None:
self.runService.shutdown('reload config')
self.runService = None
if not self.listService is None:
self.listService.shutdown('reload config')
self.listService = None
self.nodes = [] # the name of nodes with namespace
self.sensors = {} # sensor descriptions
launch_path = self.getPath(self.launch_file, self.package)
rospy.loginfo("loading launch file: %s", launch_path)
self.masteruri = self._masteruri_from_ros()
self.roscfg = ROSLaunchConfig()
loader = XmlLoader()
argv = [a for a in sys.argv if not a.startswith('__ns:=')]
# remove namespace from sys.argv to avoid load the launchfile info local namespace
sys.argv = [a for a in sys.argv if not a.startswith('__ns:=')]
# set the global environment to empty namespace
os.environ[ROS_NAMESPACE] = rospy.names.SEP
loader.load(launch_path, self.roscfg, verbose=False, argv=argv)
# create the list with node names
for item in self.roscfg.nodes:
if item.machine_name and not item.machine_name == 'localhost':
machine = self.roscfg.machines[item.machine_name]
if roslib.network.is_local_address(machine.address):
self.nodes.append(
roslib.names.ns_join(item.namespace, item.name))
else:
self.nodes.append(
roslib.names.ns_join(item.namespace, item.name))
# get the robot description
self.description_response = dr = ListDescriptionResponse()
dr.robot_name = ''
dr.robot_type = ''
dr.robot_descr = ''
for param, p in self.roscfg.params.items():
if param.endswith('robots'):
if isinstance(p.value, list):
if len(p.value) > 0 and len(p.value[0]) != 5:
print "WRONG format, expected: ['host(ROS master Name)', 'type', 'name', 'images', 'description'] -> ignore", param
else:
for entry in p.value:
try:
print entry[0], rospy.get_param(
'/mastername', '')
if not entry[0] or entry[
0] == rospy.get_param(
'/mastername', ''):
dr.robot_name = self._decode(entry[2])
dr.robot_type = entry[1]
dr.robot_images = entry[3].split(',')
dr.robot_descr = self._decode(entry[4])
break
except:
pass
# get the sensor description
tmp_cap_dict = self.getCapabilitiesDesrc()
for machine, ns_dict in tmp_cap_dict.items():
if self.roscfg.machines.has_key(machine):
machine = self.roscfg.machines[machine].address
if not machine or roslib.network.is_local_address(machine):
for ns, group_dict in ns_dict.items():
for group, descr_dict in group_dict.items():
if descr_dict['nodes']:
cap = Capability()
cap.namespace = ns
cap.name = group
cap.type = descr_dict['type']
cap.images = list(descr_dict['images'])
cap.description = descr_dict['description']
cap.nodes = list(descr_dict['nodes'])
dr.capabilities.append(cap)
# load parameters into the ROS parameter server
if self.load_params_at_start:
self.loadParams()
# initialize the ROS services
#HACK to let the node_manager to update the view
if delay_service_creation > 0.:
t = threading.Timer(delay_service_creation,
self._timed_service_creation)
t.start()
else:
self._timed_service_creation()
# self.timer = rospy.Timer(rospy.Duration(2), self.timed_service_creation, True)
# if self.nodes:
# self.runService = rospy.Service('~run', Task, self.rosservice_start_node)
# self.listServic = rospy.Service('~list_nodes', ListNodes, self.rosservice_list_nodes)
# except:
# import traceback
# print traceback.format_exc()
if self.do_autostart:
if not self.parameter_loaded:
self.loadParams()
for n in self.nodes:
try:
self.runNode(n, self.do_autostart)
except Exception as e:
rospy.logwarn("Error while start %s: %s", n, e)
self.do_autostart = False
def load(self, delay_service_creation=0.):
'''
Load the launch file configuration
'''
with self.__lock:
self._pending_starts.clear()
# shutdown the services to inform the caller about a new configuration.
if self.runService is not None:
self.runService.shutdown('reload config')
self.runService = None
if self.listService is not None:
self.listService.shutdown('reload config')
self.listService = None
self.nodes = [] # the name of nodes with namespace
self.sensors = {} # sensor descriptions
launch_path = self.getPath(self.launch_file, self.package)
rospy.loginfo("loading launch file: %s", launch_path)
self.masteruri = self._masteruri_from_ros()
self.roscfg = ROSLaunchConfig()
loader = XmlLoader()
argv = self._filter_args(sys.argv)
# remove namespace from sys.argv to avoid load the launchfile info local namespace
sys.argv = list(argv)
# set the global environment to empty namespace
os.environ[ROS_NAMESPACE] = rospy.names.SEP
rospy.set_param('~argv_used', list(set(argv)))
loader.load(launch_path, self.roscfg, verbose=False, argv=argv)
# create the list with node names
for item in self.roscfg.nodes:
if item.machine_name and not item.machine_name == 'localhost':
machine = self.roscfg.machines[item.machine_name]
if roslib.network.is_local_address(machine.address):
self.nodes.append(roslib.names.ns_join(item.namespace, item.name))
else:
self.nodes.append(roslib.names.ns_join(item.namespace, item.name))
# get the robot description
self.description_response = dr = ListDescriptionResponse()
dr.robot_name = ''
dr.robot_type = ''
dr.robot_descr = ''
for param, p in self.roscfg.params.items():
if param.endswith('robots'):
if isinstance(p.value, list):
if len(p.value) > 0 and len(p.value[0]) != 5:
print "WRONG format, expected: ['host(ROS master Name)', 'type', 'name', 'images', 'description'] -> ignore", param
else:
for entry in p.value:
try:
print entry[0], rospy.get_param('/mastername', '')
if not entry[0] or entry[0] == rospy.get_param('/mastername', ''):
dr.robot_name = self._decode(entry[2])
dr.robot_type = entry[1]
dr.robot_images = entry[3].split(',')
dr.robot_descr = self._decode(entry[4])
break
except:
pass
# get the sensor description
tmp_cap_dict = self.getCapabilitiesDesrc()
for machine, ns_dict in tmp_cap_dict.items():
if machine in self.roscfg.machines:
machine = self.roscfg.machines[machine].address
if not machine or roslib.network.is_local_address(machine):
for ns, group_dict in ns_dict.items():
for group, descr_dict in group_dict.items():
if descr_dict['nodes']:
cap = Capability()
cap.namespace = ns
cap.name = group
cap.type = descr_dict['type']
cap.images = list(descr_dict['images'])
cap.description = descr_dict['description']
cap.nodes = list(descr_dict['nodes'])
dr.capabilities.append(cap)
# load parameters into the ROS parameter server
if self.load_params_at_start:
self.loadParams()
# initialize the ROS services
# HACK to let the node_manager to update the view
if delay_service_creation > 0.:
t = threading.Timer(delay_service_creation, self._timed_service_creation)
t.start()
else:
self._timed_service_creation()
# self.timer = rospy.Timer(rospy.Duration(2), self.timed_service_creation, True)
# if self.nodes:
# self.runService = rospy.Service('~run', Task, self.rosservice_start_node)
# self.listServic = rospy.Service('~list_nodes', ListNodes, self.rosservice_list_nodes)
# except:
# import traceback
# print traceback.format_exc()
if self.do_autostart:
if not self.parameter_loaded:
self.loadParams()
for n in self.nodes:
try:
self.runNode(n, self.do_autostart)
except Exception as e:
rospy.logwarn("Error while start %s: %s", n, e)
self.do_autostart = False