def test_get_local_addresses(self):
# mostly a tripwire test
from rosgraph.network import get_local_addresses
addrs = get_local_addresses()
assert type(addrs) == list
assert len(addrs)
# should be unaffected
os.environ['ROS_IP'] = 'bar'
assert addrs == get_local_addresses()
os.environ['ROS_HOSTNAME'] = 'foo'
assert addrs == get_local_addresses()
def test_get_local_addresses(self):
# mostly a tripwire test
from rosgraph.network import get_local_addresses
addrs = get_local_addresses()
assert type(addrs) == list
assert len(addrs)
# should be unaffected
os.environ['ROS_IP'] = 'bar'
assert addrs == get_local_addresses()
os.environ['ROS_HOSTNAME'] = 'foo'
assert addrs == get_local_addresses()
def __is_local(hostname):
'''
Test the hostname whether it is local or not. Uses socket.gethostbyname().
'''
try:
# If Python has ipv6 disabled but machine.address can be resolved somehow to an ipv6 address, then host[4][0] will be int
machine_ips = [
host[4][0]
for host in socket.getaddrinfo(hostname, 0, 0, 0, socket.SOL_TCP)
if isinstance(host[4][0], str)
]
except socket.gaierror:
with _LOCK:
rospy.logdebug("host::HOSTS_CACHE resolve %s failed" % hostname)
HOSTS_CACHE[hostname] = False
return False
local_addresses = ['localhost'] + get_local_addresses()
# check 127/8 and local addresses
result = ([
ip for ip in machine_ips if (ip.startswith('127.') or ip == '::1')
] != []) or (set(machine_ips) & set(local_addresses) != set())
with _LOCK:
rospy.logdebug("host::HOSTS_CACHE add %s:%s" % (hostname, result))
HOSTS_CACHE[hostname] = result
return result
def __is_local(cls, hostname):
try:
machine_addr = socket.gethostbyname(hostname)
except socket.gaierror:
import traceback
print(traceback.format_exc())
return False
local_addresses = ['localhost'] + get_local_addresses()
# check 127/8 and local addresses
result = machine_addr.startswith('127.') or machine_addr in local_addresses
return result
def __init__(self, interface, port):
'''
Creates a socket, bind it to a given interface+port for unicast send/receive.
IPv4 and IPv6 are supported.
@param interface: The interface to bind to
@type interface: str
@param port: the port to bind the socket
@type port: int
'''
self.interface = interface
self.port = port
addrinfo = None
# If interface isn't specified, try to find an non localhost interface to
# get some info for binding. Otherwise use localhost
if not self.interface:
ifaces = get_local_addresses()
self.interface = 'localhost'
for iface in ifaces:
if not (iface.startswith('127') or iface.startswith('::1')):
self.interface = iface
break
rospy.loginfo("+ Bind to unicast socket @(%s:%s)", self.interface,
port)
addrinfo = UcastSocket.getaddrinfo(self.interface)
else:
# Otherwise get the address info for the interface specified.
rospy.loginfo("+ Bind to specified unicast socket @(%s:%s)",
self.interface, port)
addrinfo = UcastSocket.getaddrinfo(self.interface)
# Configure socket type
socket.socket.__init__(self, addrinfo[0], socket.SOCK_DGRAM,
socket.IPPROTO_UDP)
# Allow multiple copies of this program on one machine
# Required to receive unicast UDP
if hasattr(socket, "SO_REUSEPORT"):
try:
self.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1)
except:
rospy.logwarn(
"SO_REUSEPORT failed: Protocol not available, some functions are not available."
)
# Bind to the port
try:
rospy.logdebug("Ucast bind to: (%s:%s)", addrinfo[4][0], port)
self.bind((addrinfo[4][0], port))
except socket.error as errobj:
msg = str(errobj)
rospy.logfatal(
"Unable to bind unicast to interface: %s, check that it exists: %s",
self.interface, msg)
raise
def __is_local(hostname):
'''
Test the hostname whether it is local or not. Uses socket.gethostbyname().
'''
try:
machine_addr = socket.gethostbyname(hostname)
except socket.gaierror:
with _LOCK:
HOSTS_CACHE[hostname] = False
return False
local_addresses = ['localhost'] + get_local_addresses()
# check 127/8 and local addresses
result = machine_addr.startswith('127.') or machine_addr in local_addresses
with _LOCK:
HOSTS_CACHE[hostname] = result
return result
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 is_local(hostname, wait=True):
'''
Test whether the given host name is the name of the local host or not.
:param str hostname: the name or IP of the host
:return: `True` if the hostname is local or None
:rtype: bool
:raise Exception: on errors while resolving host
'''
if not hostname:
return True
with _LOCK:
if hostname in HOSTS_CACHE:
if isinstance(HOSTS_CACHE[hostname], threading.Thread):
return False
return HOSTS_CACHE[hostname]
try:
socket.inet_aton(hostname)
local_addresses = ['localhost'] + get_local_addresses()
# check 127/8 and local addresses
result = hostname.startswith(
'127.') or hostname == '::1' or hostname in local_addresses
with _LOCK:
rospy.logdebug("host::HOSTS_CACHE add local %s:%s" %
(hostname, result))
HOSTS_CACHE[hostname] = result
return result
except socket.error:
# the hostname must be resolved => do it in a thread
if wait:
result = __is_local(hostname)
return result
else:
thread = threading.Thread(target=__is_local, args=((hostname, )))
thread.daemon = True
with _LOCK:
HOSTS_CACHE[hostname] = thread
thread.start()
return False
def is_local(cls, hostname, wait=False):
'''
Test whether the given host name is the name of the local host or not.
:param str hostname: the name or IP of the host
:return: True if the hostname is local or None
:rtype: bool
:raise Exception: on errors while resolving host
'''
if (hostname is None):
return True
try:
socket.inet_aton(hostname)
local_addresses = ['localhost'] + get_local_addresses()
# check 127/8 and local addresses
result = hostname.startswith('127.') or hostname in local_addresses
return result
except socket.error:
# the hostname must be resolved => do it in a thread
if wait:
result = cls.__is_local(hostname)
return result
return False
def run_node(startcfg):
'''
Start a node local or on specified host using a :class:`.startcfg.StartConfig`
:param startcfg: start configuration e.g. returned by :meth:`create_start_config`
:type startcfg: :class:`fkie_node_manager_daemon.startcfg.StartConfig`
:raise exceptions.StartException: on errors
:raise exceptions.BinarySelectionRequest: on multiple binaries
:see: :meth:`fkie_node_manager.host.is_local`
'''
hostname = startcfg.hostname
nodename = roslib.names.ns_join(startcfg.namespace, startcfg.name)
if not hostname or host.is_local(hostname, wait=True):
# run on local host
# interpret arguments with path elements
args = []
for arg in startcfg.args:
new_arg = arg
if arg.startswith('$(find'):
new_arg = interpret_path(arg)
rospy.logdebug("interpret arg '%s' to '%s'" % (arg, new_arg))
args.append(new_arg)
# set name and namespace of the node
if startcfg.name:
args.append("__name:=%s" % startcfg.name)
if startcfg.namespace:
args.append("__ns:=%s" % startcfg.namespace)
# add remap arguments
for key, val in startcfg.remaps.items():
args.append("%s:=%s" % (key, val))
cmd_type = startcfg.binary_path
# get binary path from package
if not cmd_type:
try:
cmd = roslib.packages.find_node(startcfg.package, startcfg.binary)
except (roslib.packages.ROSPkgException, rospkg.ResourceNotFound) as e:
# multiple nodes, invalid package
rospy.logwarn("resource not found: %s" % utf8(e))
raise exceptions.ResourceNotFound(startcfg.package, "resource not found: %s" % utf8(e))
if isstring(cmd):
cmd = [cmd]
if cmd is None or len(cmd) == 0:
raise exceptions.StartException('%s in package [%s] not found!' % (startcfg.binary, startcfg.package))
if len(cmd) > 1:
# Open selection for executables
err = 'Multiple executables with same name in package [%s] found:' % startcfg.package
raise exceptions.BinarySelectionRequest(cmd, err)
else:
cmd_type = cmd[0]
try:
global STARTED_BINARIES
STARTED_BINARIES[nodename] = (cmd_type, os.path.getmtime(cmd_type))
except Exception:
pass
cwd = get_cwd(startcfg.cwd, cmd_type)
# set environment
new_env = dict(os.environ)
# set display variable to local display
if 'DISPLAY' in startcfg.env:
if not startcfg.env['DISPLAY'] or startcfg.env['DISPLAY'] == 'remote':
del startcfg.env['DISPLAY']
#else:
# new_env['DISPLAY'] = ':0'
# add environment from launch
new_env.update(startcfg.env)
if startcfg.namespace:
new_env['ROS_NAMESPACE'] = startcfg.namespace
# set logging
if startcfg.logformat:
new_env['ROSCONSOLE_FORMAT'] = '%s' % startcfg.logformat
if startcfg.loglevel:
new_env['ROSCONSOLE_CONFIG_FILE'] = _rosconsole_cfg_file(startcfg.package, startcfg.loglevel)
# handle respawn
if startcfg.respawn:
if startcfg.respawn_delay > 0:
new_env['RESPAWN_DELAY'] = '%d' % startcfg.respawn_delay
respawn_params = _get_respawn_params(startcfg.fullname, startcfg.params)
if respawn_params['max'] > 0:
new_env['RESPAWN_MAX'] = '%d' % respawn_params['max']
if respawn_params['min_runtime'] > 0:
new_env['RESPAWN_MIN_RUNTIME'] = '%d' % respawn_params['min_runtime']
cmd_type = "%s %s %s" % (settings.RESPAWN_SCRIPT, startcfg.prefix, cmd_type)
else:
cmd_type = "%s %s" % (startcfg.prefix, cmd_type)
# check for masteruri
masteruri = startcfg.masteruri
if masteruri is None:
masteruri = masteruri_from_ros()
if masteruri is not None:
if 'ROS_MASTER_URI' not in startcfg.env:
new_env['ROS_MASTER_URI'] = masteruri
# host in startcfg is a nmduri -> get host name
ros_hostname = host.get_ros_hostname(masteruri, hostname)
if ros_hostname:
addr = socket.gethostbyname(ros_hostname)
if addr in set(ip for _n, ip in get_local_addresses()):
new_env['ROS_HOSTNAME'] = ros_hostname
# load params to ROS master
_load_parameters(masteruri, startcfg.params, startcfg.clear_params)
# start
cmd_str = utf8('%s %s %s' % (screen.get_cmd(startcfg.fullname, new_env, list(startcfg.env.keys())), cmd_type, ' '.join(args)))
rospy.loginfo("%s (launch_file: '%s', masteruri: %s)" % (cmd_str, startcfg.config_path, masteruri))
rospy.logdebug("environment while run node '%s': '%s'" % (cmd_str, new_env))
SupervisedPopen(shlex.split(cmd_str), cwd=cwd, env=new_env, object_id="run_node_%s" % startcfg.fullname, description="Run [%s]%s" % (utf8(startcfg.package), utf8(startcfg.binary)))
else:
nmduri = startcfg.nmduri
rospy.loginfo("remote run node '%s' at '%s'" % (nodename, nmduri))
startcfg.params.update(_params_to_package_path(startcfg.params))
startcfg.args = _args_to_package_path(startcfg.args)
# run on a remote machine
channel = remote.get_insecure_channel(nmduri)
if channel is None:
raise exceptions.StartException("Unknown launch manager url for host %s to start %s" % (host, startcfg.fullname))
lm = LaunchStub(channel)
lm.start_standalone_node(startcfg)
def __init__(self,
port,
mgroup,
ttl=20,
send_mcast=True,
listen_mcast=True):
'''
Creates a socket, bind it to a given port and join to a given multicast
group. IPv4 and IPv6 are supported.
@param port: the port to bind the socket
@type port: int
@param mgroup: the multicast group to join
@type mgroup: str
@param ttl: time to leave
@type ttl: int (Default: 20)
@param send_mcast: send multicast messages
@type send_mcast: bool (Default: True)
@param listen_mcast: listen to the multicast group
@type listen_mcast: bool (Default: True)
'''
self.port = port
self.receive_queue = queue.Queue()
self._send_queue = queue.Queue()
self._lock = threading.RLock()
self.send_mcast = send_mcast
self.listen_mcast = listen_mcast
self.unicast_only = not (send_mcast or listen_mcast)
self._closed = False
self._locals = get_local_addresses()
self._locals.append('localhost')
self.sock_5_error_printed = []
self.SOKET_ERRORS_NEEDS_RECONNECT = False
# get the group and interface. Especially for definition like [email protected]
# it also reads ~interface and ROS_IP
self.mgroup, self.interface = DiscoverSocket.normalize_mgroup(
mgroup, True)
self.unicast_socket = None
# get the AF_INET information for group to ensure that the address family
# of group is the same as for interface
addrinfo = UcastSocket.getaddrinfo(self.mgroup)
self.interface_ip = ''
if self.unicast_only:
# inform about no braodcasting
rospy.logwarn(
"Multicast disabled! This master is only by unicast reachable!"
)
if self.interface:
addrinfo = UcastSocket.getaddrinfo(self.interface, addrinfo[0])
if addrinfo is not None:
self.interface_ip = addrinfo[4][0]
self.unicast_socket = UcastSocket(self.interface_ip, port)
elif self.unicast_only:
self.unicast_socket = UcastSocket('', port)
rospy.logdebug("mgroup: %s", self.mgroup)
rospy.logdebug("interface : %s", self.interface)
rospy.logdebug("inet: %s", addrinfo)
socket.socket.__init__(self, addrinfo[0], socket.SOCK_DGRAM,
socket.IPPROTO_UDP)
self.addrinfo = addrinfo
if not self.unicast_only:
rospy.logdebug("Create multicast socket at ('%s', %d)",
self.mgroup, port)
# initialize multicast socket
# Allow multiple copies of this program on one machine
if hasattr(socket, "SO_REUSEPORT"):
try:
self.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1)
except:
rospy.logwarn(
"SO_REUSEPORT failed: Protocol not available, some functions are not available."
)
# Set Time-to-live (optional) and loop count
ttl_bin = struct.pack('@i', ttl)
if addrinfo[0] == socket.AF_INET: # IPv4
self.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_TTL,
ttl_bin)
self.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_LOOP, 1)
else: # IPv6
self.setsockopt(socket.IPPROTO_IPV6,
socket.IPV6_MULTICAST_HOPS, ttl_bin)
self.setsockopt(socket.IPPROTO_IPV6,
socket.IPV6_MULTICAST_LOOP, 1)
# Bind to the port
try:
# bind to default interfaces if not unicast socket was created
to_group = self.mgroup if self.unicast_socket is not None else ''
self.bind((to_group, port))
except socket.error as errobj:
msg = str(errobj)
rospy.logfatal(
"Unable to bind multicast to interface: %s, check that it exists: %s",
self.mgroup, msg)
raise
self.join_group()
if not self.unicast_only:
# create a thread to handle the received multicast messages
self._recv_thread = threading.Thread(
target=self.recv_loop_multicast)
self._recv_thread.start()
if self.unicast_socket is not None:
# create a thread to handle the received unicast messages
self._recv_thread = threading.Thread(target=self.recv_loop_unicast)
self._recv_thread.start()
self._send_tread = threading.Thread(target=self._send_loop_from_queue)
self._send_tread.start()
