def _prepareROSMaster(cls, masteruri):
if masteruri is None:
masteruri = masteruri_from_ros()
# start roscore, if needed
try:
if not os.path.isdir(screen.LOG_PATH):
os.makedirs(screen.LOG_PATH)
socket.setdefaulttimeout(3)
master = xmlrpcclient.ServerProxy(masteruri)
master.getUri(rospy.get_name())
# restart ROSCORE on different masteruri?, not now...
# master_uri = master.getUri(rospy.get_name())
# if masteruri != master_uri[2]:
# # kill the local roscore...
# raise
except Exception:
# run a roscore
master_host = get_hostname(masteruri)
if nm.is_local(master_host, True):
master_port = get_port(masteruri)
new_env = dict(os.environ)
new_env['ROS_MASTER_URI'] = masteruri
ros_hostname = nmdhost.get_ros_hostname(masteruri)
if ros_hostname:
new_env['ROS_HOSTNAME'] = ros_hostname
cmd_args = '%s roscore --port %d' % (screen.get_cmd('/roscore--%d' % master_port), master_port)
for n in [1, 2, 3, 4]:
try:
if n == 1:
print("Launch ROS Master in screen ... %s" % (cmd_args))
SupervisedPopen(shlex.split(cmd_args), env=new_env, object_id="ROSCORE", description="Start roscore")
elif n == 2:
print("ROS Master takes too long for start, wait for next 10 sec ...")
elif n == 3:
print("A really slow start, wait for last 10 sec ...")
# wait for roscore to avoid connection problems while init_node
result = -1
count = 1
while result == -1 and count < 11:
try:
master = xmlrpcclient.ServerProxy(masteruri)
result, _, _ = master.getUri(rospy.get_name()) # _:=uri, msg
return
except Exception:
time.sleep(1)
count += 1
if n == 4 and count >= 11:
raise StartException('Cannot connect to ROS-Master: %s\n--> please run "roscore" manually!' % utf8(masteruri))
except Exception as e:
raise Exception("Error while call '%s': %s" % (cmd_args, utf8(e)))
else:
raise Exception("ROS master '%s' is not reachable" % masteruri)
finally:
socket.setdefaulttimeout(None)
def _prepareROSMaster(cls, masteruri):
if not masteruri:
masteruri = masteruri_from_ros()
# start roscore, if needed
try:
if not os.path.isdir(screen.LOG_PATH):
os.makedirs(screen.LOG_PATH)
socket.setdefaulttimeout(3)
master = xmlrpcclient.ServerProxy(masteruri)
master.getUri(rospy.get_name())
except Exception:
# run a roscore
screen.test_screen()
master_host = get_hostname(masteruri)
if cls.is_local(master_host, True):
print("Start ROS-Master with %s ..." % masteruri)
master_port = get_port(masteruri)
new_env = dict(os.environ)
new_env['ROS_MASTER_URI'] = masteruri
ros_hostname = nmdhost.get_ros_hostname(masteruri)
if ros_hostname:
new_env['ROS_HOSTNAME'] = ros_hostname
cmd_args = '%s roscore --port %d' % (screen.get_cmd(
'/roscore--%d' % master_port), master_port)
try:
subprocess.Popen(shlex.split(cmd_args), env=new_env)
# wait for roscore to avoid connection problems while init_node
result = -1
count = 1
while result == -1 and count < 11:
try:
print(" retry connect to ROS master %d/10" %
count)
master = xmlrpcclient.ServerProxy(masteruri)
result, _, _ = master.getUri(
rospy.get_name()) # _:=uri, msg
except Exception:
time.sleep(1)
count += 1
if count >= 11:
raise StartException(
'Cannot connect to the ROS-Master: ' +
str(masteruri))
except Exception as e:
import sys
sys.stderr.write("%s\n" % e)
raise
else:
raise Exception("ROS master '%s' is not reachable" % masteruri)
finally:
socket.setdefaulttimeout(None)
def runNode(package, executable, name, args, prefix='', repawn=False, masteruri=None, loglevel=''):
'''
Runs a ROS node. Starts a roscore if needed.
'''
if not masteruri:
masteruri = masteruri_from_ros()
# start roscore, if needed
nm.StartHandler._prepareROSMaster(masteruri)
# start node
try:
cmd = roslib.packages.find_node(package, executable)
except roslib.packages.ROSPkgException as e:
# multiple nodes, invalid package
raise nm.StartException(str(e))
# handle different result types str or array of string (electric / fuerte)
if isstring(cmd):
cmd = [cmd]
if cmd is None or len(cmd) == 0:
raise nm.StartException(' '.join([executable, 'in package [', package, '] not found!\n\nThe package was created?\nIs the binary executable?\n']))
# create string for node parameter. Set arguments with spaces into "'".
node_params = ' '.join(''.join(["'", a, "'"]) if a.find(' ') > -1 else a for a in args[1:])
cmd_args = [screen.get_cmd(name), RESPAWN_SCRIPT if repawn else '', prefix, cmd[0], node_params]
print('run on remote host:', ' '.join(cmd_args))
# determine the current working path
arg_cwd = getCwdArg('__cwd', args)
cwd = nm.get_ros_home()
if not (arg_cwd is None):
if arg_cwd == 'ROS_HOME':
cwd = nm.get_ros_home()
elif arg_cwd == 'node':
cwd = os.path.dirname(cmd[0])
# set the masteruri to launch with other one master
new_env = dict(os.environ)
new_env['ROS_MASTER_URI'] = masteruri
ros_hostname = nmdhost.get_ros_hostname(masteruri)
if ros_hostname:
addr = socket.gethostbyname(ros_hostname)
if addr in set(ip for ip in get_local_addresses()):
new_env['ROS_HOSTNAME'] = ros_hostname
if loglevel:
new_env['ROSCONSOLE_CONFIG_FILE'] = rosconsole_cfg_file(package)
subprocess.Popen(shlex.split(str(' '.join(cmd_args))), cwd=cwd, env=new_env)
if len(cmd) > 1:
rospy.logwarn('Multiple executables are found! The first one was started! Exceutables:\n%s', str(cmd))
def runNodeWithoutConfig(cls,
host,
package,
binary,
name,
args=[],
masteruri=None,
use_nmd=True,
auto_pw_request=False,
user=None,
pw=None,
path=''):
'''
Start a node with using a launch configuration.
:param str hosturi: the host or ip to run the node
:param str package: the ROS package containing the binary
:param str binary: the binary of the node to execute
:param str name: the ROS name of the node (with name space)
:param args: the list with arguments passed to the binary
:type args: [str]
:param bool use_nmd: start the node using node manager daemon
:param bool auto_pw_request: opens question dialog directly, use True only if the method is called from the main GUI thread
:raise Exception: on errors while resolving host
:see: :meth:`fkie_node_manager.is_local()`
'''
# create the name with namespace
args2 = list(args)
fullname = roslib.names.ns_join(roslib.names.SEP, name)
namespace = ''
for a in args:
if a.startswith('__ns:='):
namespace = a.replace('__ns:=', '')
fullname = roslib.names.ns_join(namespace, name)
args2.append(''.join(['__name:=', name]))
# run on local host
if nm.is_local(host, wait=True):
if not use_nmd:
if path:
cmd = [path]
else:
try:
cmd = roslib.packages.find_node(package, binary)
except roslib.packages.ROSPkgException as e:
# multiple nodes, invalid package
raise StartException(utf8(e))
# handle different result types str or array of string
if isstring(cmd):
cmd = [cmd]
cmd_type = ''
if cmd is None or len(cmd) == 0:
raise StartException('%s in package [%s] not found!' %
(binary, package))
# compatibility for python scripts installed with catkin_install_python()
# avoid ask for select a binary
cmd = cls._remove_src_binary(cmd)
if len(cmd) > 1:
# Open selection for executables
err = 'Multiple executables with same name in package [%s] found' % package
bsel = nm.BinarySelectionRequest(cmd, err)
raise nm.InteractionNeededError(
bsel, cls.runNodeWithoutConfig, {
'host': host,
'package': package,
'binary': binary,
'name': name,
'args': args,
'masteruri': masteruri,
'use_nmd': use_nmd,
'auto_pw_request': auto_pw_request,
'user': user,
'pw': pw,
'path': path
})
else:
cmd_type = cmd[0]
new_env = {} # dict(os.environ)
if namespace:
new_env['ROS_NAMESPACE'] = namespace
if masteruri is not None:
cls._prepareROSMaster(masteruri)
new_env['ROS_MASTER_URI'] = masteruri
if 'ROS_HOSTNAME' in os.environ:
# set ROS_HOSTNAME only if node_manager has also one
ros_hostname = nmdhost.get_ros_hostname(masteruri, host)
if ros_hostname:
new_env['ROS_HOSTNAME'] = ros_hostname
if use_nmd:
nm.nmd().launch.start_standalone_node(nmdurl.nmduri(), package,
binary, name, namespace,
args, new_env, masteruri,
host)
else:
local_env = dict(os.environ)
local_env.update(new_env)
cmd_str = utf8(' '.join([
screen.get_cmd(fullname, local_env), cmd_type,
' '.join(args2)
]))
rospy.loginfo("Run without config: %s",
fullname if use_nmd else cmd_str)
SupervisedPopen(shlex.split(cmd_str),
env=local_env,
object_id="Run without config",
description="Run without config [%s]%s" %
(utf8(package), utf8(binary)))
else:
# run on a remote machine
startcmd = [
nm.settings().start_remote_script, '--package',
utf8(package), '--node_type',
utf8(binary), '--node_name',
utf8(fullname)
]
startcmd[len(startcmd):] = args2
if masteruri is not None:
startcmd.append('--masteruri')
startcmd.append(masteruri)
rospy.loginfo("Run remote on %s: %s", host, ' '.join(startcmd))
try:
_, stdout, stderr, ok = nm.ssh().ssh_exec(host,
startcmd,
user,
pw,
auto_pw_request,
close_stdin=True)
if ok:
output = stdout.read()
error = stderr.read()
stdout.close()
stderr.close()
if error:
rospy.logwarn("ERROR while start '%s': %s", name,
error)
raise StartException(''.join(
['The host "', host, '" reports:\n', error]))
if output:
if output.find("dn't") != -1:
rospy.logwarn("Warning while start '%s': %s", name,
output)
else:
rospy.loginfo("STDOUT while start '%s': %s", name,
output)
else:
if error:
rospy.logwarn("ERROR while start '%s': %s", name,
error)
raise StartException(''.join(
['The host "', host, '" reports:\n', error]))
except nm.AuthenticationRequest as e:
raise nm.InteractionNeededError(
e, cls.runNodeWithoutConfig, {
'host': host,
'package': package,
'binary': binary,
'name': name,
'args': args,
'masteruri': masteruri,
'use_nmd': use_nmd,
'auto_pw_request': auto_pw_request,
'user': user,
'pw': pw,
'path': path
})