def contact_service(resolved_name, timeout=10.0):
try:
uri = master.lookupService(resolved_name)
except rosgraph.MasterException:
return False
addr = rospy.core.parse_rosrpc_uri(uri)
if rosgraph.network.use_ipv6():
s = socket.socket(socket.AF_INET6, socket.SOCK_STREAM)
else:
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
try:
# we always want to timeout just in case we're connecting
# to a down service.
s.settimeout(timeout)
logdebug('connecting to ' + str(addr))
s.connect(addr)
h = { 'probe' : '1', 'md5sum' : '*',
'callerid' : rospy.core.get_caller_id(),
'service': resolved_name }
rosgraph.network.write_ros_handshake_header(s, h)
return True
finally:
if s is not None:
s.close()
def handle(self, transport, header):
"""
Process incoming request. This method should be run in its
own thread. If header['persistent'] is set to 1, method will
block until connection is broken.
@param transport: transport instance
@type transport: L{TCPROSTransport}
@param header: headers from client
@type header: dict
"""
if 'persistent' in header and \
header['persistent'].lower() in ['1', 'true']:
persistent = True
else:
persistent = False
if header.get('probe', None) == '1':
#this will likely do more in the future
transport.close()
return
handle_done = False
while not handle_done:
try:
requests = transport.receive_once()
for request in requests:
self._handle_request(transport, request)
if not persistent:
handle_done = True
except rospy.exceptions.TransportTerminated as e:
if not persistent:
logerr("incoming connection failed: %s"%e)
logdebug("service[%s]: transport terminated"%self.resolved_name)
handle_done = True
transport.close()
def __init__(self,
name,
service_class,
handler,
buff_size=DEFAULT_BUFF_SIZE,
error_handler=None):
super(ServiceImpl, self).__init__(name, service_class)
if not name or not isstring(name):
raise ValueError("service name is not a non-empty string")
# #2202
if not rosgraph.names.is_legal_name(name):
import warnings
warnings.warn(
"'%s' is not a legal ROS graph resource name. This may cause problems with other ROS tools"
% name,
stacklevel=2)
self.handler = handler
if error_handler is not None:
self.error_handler = error_handler
self.registered = False
self.seq = 0
self.done = False
self.buff_size = buff_size
start_tcpros_server() #lazy-init the tcprosserver
host, port = get_tcpros_server_address()
self.uri = '%s%s:%s' % (rospy.core.ROSRPC, host, port)
logdebug("... service URL is %s" % self.uri)
self.protocol = TCPService(self.resolved_name, service_class,
self.buff_size)
logdebug("[%s]: new Service instance" % self.resolved_name)
def contact_service(resolved_name, timeout=10.0):
try:
uri = master.lookupService(resolved_name)
except rosgraph.MasterException:
return False
addr = rospy.core.parse_rosrpc_uri(uri)
if rosgraph.network.use_ipv6():
s = socket.socket(socket.AF_INET6, socket.SOCK_STREAM)
else:
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
try:
# we always want to timeout just in case we're connecting
# to a down service.
s.settimeout(timeout)
logdebug('connecting to ' + str(addr))
s.connect(addr)
h = {
'probe': '1',
'md5sum': '*',
'callerid': rospy.core.get_caller_id(),
'service': resolved_name
}
rosgraph.network.write_ros_handshake_header(s, h)
return True
finally:
if s is not None:
s.close()
def handle(self, transport, header):
"""
Process incoming request. This method should be run in its
own thread. If header['persistent'] is set to 1, method will
block until connection is broken.
@param transport: transport instance
@type transport: L{TCPROSTransport}
@param header: headers from client
@type header: dict
"""
if 'persistent' in header and \
header['persistent'].lower() in ['1', 'true']:
persistent = True
else:
persistent = False
if header.get('probe', None) == '1':
#this will likely do more in the future
transport.close()
return
handle_done = False
while not handle_done:
try:
requests = transport.receive_once()
for request in requests:
self._handle_request(transport, request)
if not persistent:
handle_done = True
except rospy.exceptions.TransportTerminated as e:
if not persistent:
logerr("incoming connection failed: %s" % e)
logdebug("service[%s]: transport terminated" %
self.resolved_name)
handle_done = True
transport.close()
def run(self):
"""
Main TCP receive loop. Should be run in a separate thread -- use start()
to do this automatically.
"""
self.is_shutdown = False
if not self.server_sock:
raise ROSInternalException("%s did not connect"%self.__class__.__name__)
while not self.is_shutdown:
try:
(client_sock, client_addr) = self.server_sock.accept()
except socket.timeout:
continue
except IOError as e:
(errno, msg) = e.args
if errno == 4: #interrupted system call
continue
raise
if self.is_shutdown:
break
try:
#leave threading decisions up to inbound_handler
self.inbound_handler(client_sock, client_addr)
except socket.error as e:
if not self.is_shutdown:
traceback.print_exc()
logwarn("Failed to handle inbound connection due to socket error: %s"%e)
logdebug("TCPServer[%s] shutting down", self.port)
def run(self):
"""
Main TCP receive loop. Should be run in a separate thread -- use start()
to do this automatically.
"""
self.is_shutdown = False
if not self.server_sock:
raise ROSInternalException("%s did not connect"%self.__class__.__name__)
while not self.is_shutdown:
try:
(client_sock, client_addr) = self.server_sock.accept()
except socket.timeout:
continue
except ConnectionAbortedError:
continue
except IOError as e:
(e_errno, msg, *_) = e.args
if e_errno == errno.EINTR: #interrupted system call
continue
if not self.is_shutdown:
raise
if self.is_shutdown:
break
try:
#leave threading decisions up to inbound_handler
self.inbound_handler(client_sock, client_addr)
except socket.error as e:
if not self.is_shutdown:
traceback.print_exc()
logwarn("Failed to handle inbound connection due to socket error: %s"%e)
logdebug("TCPServer[%s] shutting down", self.port)
def __init__(self, name, service_class, handler, buff_size=DEFAULT_BUFF_SIZE):
super(ServiceImpl, self).__init__(name, service_class)
if not name or not isstring(name):
raise ValueError("service name is not a non-empty string")
# #2202
if not rosgraph.names.is_legal_name(name):
import warnings
warnings.warn("'%s' is not a legal ROS graph resource name. This may cause problems with other ROS tools"%name, stacklevel=2)
self.handler = handler
self.registered = False
self.seq = 0
self.done = False
self.buff_size=buff_size
start_tcpros_server() #lazy-init the tcprosserver
host, port = get_tcpros_server_address()
self.uri = '%s%s:%s'%(rospy.core.ROSRPC, host, port)
logdebug("... service URL is %s"%self.uri)
self.protocol = TCPService(self.resolved_name, service_class, self.buff_size)
logdebug("[%s]: new Service instance"%self.resolved_name)
def _connect_topic_thread(self, topic, uri):
try:
code, msg, _ = self.handler._connect_topic(topic, uri)
if code != 1:
logdebug("Unable to connect subscriber to publisher [%s] for topic [%s]: %s", uri, topic, msg)
except Exception as e:
if not is_shutdown():
logdebug("Unable to connect to publisher [%s] for topic [%s]: %s"%(uri, topic, traceback.format_exc()))
def connect(self, dest_addr, dest_port, endpoint_id, timeout=None):
"""
Establish TCP connection to the specified
address/port. connect() always calls L{write_header()} and
L{read_header()} after the connection is made
@param dest_addr: destination IP address
@type dest_addr: str
@param dest_port: destination port
@type dest_port: int
@param endpoint_id: string identifier for connection (for statistics)
@type endpoint_id: str
@param timeout: (optional keyword) timeout in seconds
@type timeout: float
@raise TransportInitError: if unable to create connection
"""
try:
self.endpoint_id = endpoint_id
self.dest_address = (dest_addr, dest_port)
if rosgraph.network.use_ipv6():
s = socket.socket(socket.AF_INET6, socket.SOCK_STREAM)
else:
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
if _is_use_tcp_keepalive():
# OSX (among others) does not define these options
if hasattr(socket, 'TCP_KEEPCNT') and \
hasattr(socket, 'TCP_KEEPIDLE') and \
hasattr(socket, 'TCP_KEEPINTVL'):
# turn on KEEPALIVE
s.setsockopt(socket.SOL_SOCKET, socket.SO_KEEPALIVE, 1)
# - # keepalive failures before actual connection failure
s.setsockopt(socket.SOL_TCP, socket.TCP_KEEPCNT, 9)
# - timeout before starting KEEPALIVE process
s.setsockopt(socket.SOL_TCP, socket.TCP_KEEPIDLE, 60)
# - interval to send KEEPALIVE after IDLE timeout
s.setsockopt(socket.SOL_TCP, socket.TCP_KEEPINTVL, 10)
if timeout is not None:
s.settimeout(timeout)
self.socket = s
logdebug('connecting to ' + str(dest_addr) + ' ' + str(dest_port))
self.socket.connect((dest_addr, dest_port))
self.write_header()
self.read_header()
self.local_endpoint = self.socket.getsockname()
self.remote_endpoint = (dest_addr, dest_port)
except TransportInitError as tie:
rospyerr(
"Unable to initiate TCP/IP socket to %s:%s (%s): %s" %
(dest_addr, dest_port, endpoint_id, traceback.format_exc()))
raise
except Exception as e:
#logerr("Unknown error initiating TCP/IP socket to %s:%s (%s): %s"%(dest_addr, dest_port, endpoint_id, str(e)))
rospywarn(
"Unknown error initiating TCP/IP socket to %s:%s (%s): %s" %
(dest_addr, dest_port, endpoint_id, traceback.format_exc()))
# FATAL: no reconnection as error is unknown
self.close()
raise TransportInitError(str(e)) #re-raise i/o error
def receive_loop(self, msgs_callback):
"""
Receive messages until shutdown
@param msgs_callback: callback to invoke for new messages received
@type msgs_callback: fn([msg])
"""
# - use assert here as this would be an internal error, aka bug
logger.debug("receive_loop for [%s]", self.name)
try:
while not self.done and not is_shutdown():
try:
if self.socket is not None:
msgs = self.receive_once()
if not self.done and not is_shutdown():
msgs_callback(msgs, self)
else:
self._reconnect()
except TransportTerminated as e:
logdebug("[%s] failed to receive incoming message : %s" % (self.name, str(e)))
rospydebug("[%s] failed to receive incoming message: %s" % (self.name, traceback.format_exc()))
break
except TransportException as e:
# set socket to None so we reconnect
try:
if self.socket is not None:
try:
self.socket.shutdown()
except:
pass
finally:
self.socket.close()
except:
pass
self.socket = None
except DeserializationError as e:
#TODO: how should we handle reconnect in this case?
logerr("[%s] error deserializing incoming request: %s"%(self.name, str(e)))
rospyerr("[%s] error deserializing incoming request: %s"%(self.name, traceback.format_exc()))
except:
# in many cases this will be a normal hangup, but log internally
try:
#1467 sometimes we get exceptions due to
#interpreter shutdown, so blanket ignore those if
#the reporting fails
rospydebug("exception in receive loop for [%s], may be normal. Exception is %s",self.name, traceback.format_exc())
except: pass
rospydebug("receive_loop[%s]: done condition met, exited loop"%self.name)
finally:
if not self.done:
self.close()
def spin(self):
"""
Let service run and take over thread until service or node
shutdown. Use this method to keep your scripts from exiting
execution.
"""
try:
while not rospy.core.is_shutdown() and not self.done:
time.sleep(0.5)
except KeyboardInterrupt:
logdebug("keyboard interrupt, shutting down")
def spin(self):
"""
Let service run and take over thread until service or node
shutdown. Use this method to keep your scripts from exiting
execution.
"""
try:
while not rospy.core.is_shutdown() and not self.done:
time.sleep(0.5)
except KeyboardInterrupt:
logdebug("keyboard interrupt, shutting down")
def _init_node_params(argv, node_name):
"""
Uploads private params to the parameter server. Private params are specified
via command-line remappings.
"""
# #1027: load in param name mappings
import roslib.params
params = roslib.params.load_command_line_node_params(argv)
for param_name, param_value in params.items():
logdebug("setting param %s to %s" % (param_name, param_value))
set_param(roslib.names.PRIV_NAME + param_name, param_value)
def _init_node_params(argv, node_name):
"""
Uploads private params to the parameter server. Private params are specified
via command-line remappings.
"""
# #1027: load in param name mappings
import roslib.params
params = roslib.params.load_command_line_node_params(argv)
for param_name, param_value in params.items():
logdebug("setting param %s to %s"%(param_name, param_value))
set_param(roslib.names.PRIV_NAME + param_name, param_value)
def connect(self, dest_addr, dest_port, endpoint_id, timeout=None):
"""
Establish TCP connection to the specified
address/port. connect() always calls L{write_header()} and
L{read_header()} after the connection is made
@param dest_addr: destination IP address
@type dest_addr: str
@param dest_port: destination port
@type dest_port: int
@param endpoint_id: string identifier for connection (for statistics)
@type endpoint_id: str
@param timeout: (optional keyword) timeout in seconds
@type timeout: float
@raise TransportInitError: if unable to create connection
"""
try:
self.endpoint_id = endpoint_id
self.dest_address = (dest_addr, dest_port)
if rosgraph.network.use_ipv6():
s = socket.socket(socket.AF_INET6, socket.SOCK_STREAM)
else:
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
if _is_use_tcp_keepalive():
# OSX (among others) does not define these options
if hasattr(socket, 'TCP_KEEPCNT') and \
hasattr(socket, 'TCP_KEEPIDLE') and \
hasattr(socket, 'TCP_KEEPINTVL'):
# turn on KEEPALIVE
s.setsockopt(socket.SOL_SOCKET, socket.SO_KEEPALIVE, 1)
# - # keepalive failures before actual connection failure
s.setsockopt(socket.SOL_TCP, socket.TCP_KEEPCNT, 9)
# - timeout before starting KEEPALIVE process
s.setsockopt(socket.SOL_TCP, socket.TCP_KEEPIDLE, 60)
# - interval to send KEEPALIVE after IDLE timeout
s.setsockopt(socket.SOL_TCP, socket.TCP_KEEPINTVL, 10)
if timeout is not None:
s.settimeout(timeout)
self.socket = s
logdebug('connecting to ' + str(dest_addr)+ ' ' + str(dest_port))
self.socket.connect((dest_addr, dest_port))
self.write_header()
self.read_header()
except TransportInitError as tie:
rospyerr("Unable to initiate TCP/IP socket to %s:%s (%s): %s"%(dest_addr, dest_port, endpoint_id, traceback.format_exc()))
raise
except Exception as e:
#logerr("Unknown error initiating TCP/IP socket to %s:%s (%s): %s"%(dest_addr, dest_port, endpoint_id, str(e)))
rospywarn("Unknown error initiating TCP/IP socket to %s:%s (%s): %s"%(dest_addr, dest_port, endpoint_id, traceback.format_exc()))
# FATAL: no reconnection as error is unknown
self.close()
raise TransportInitError(str(e)) #re-raise i/o error
def _init_node_params(argv, node_name):
"""
Uploads private params to the parameter server. Private params are specified
via command-line remappings.
@raises: ROSInitException
"""
# #1027: load in param name mappings
params = load_command_line_node_params(argv)
for param_name, param_value in params.items():
logdebug("setting param %s to %s" % (param_name, param_value))
set_param(rosgraph.names.PRIV_NAME + param_name, param_value)
def _init_node_params(argv, node_name):
"""
Uploads private params to the parameter server. Private params are specified
via command-line remappings.
@raises: ROSInitException
"""
# #1027: load in param name mappings
params = load_command_line_node_params(argv)
for param_name, param_value in params.items():
logdebug("setting param %s to %s"%(param_name, param_value))
set_param(rosgraph.names.PRIV_NAME + param_name, param_value)
def shutdown(self, reason=''):
"""
Stop this service
@param reason: human-readable shutdown reason
@type reason: str
"""
self.done = True
logdebug('[%s].shutdown: reason [%s]'%(self.resolved_name, reason))
try:
#TODO: make service manager configurable
get_service_manager().unregister(self.resolved_name, self)
except Exception as e:
logerr("Unable to unregister with master: "+traceback.format_exc())
raise ServiceException("Unable to connect to master: %s"%e)
def shutdown(self, reason=''):
"""
Stop this service
@param reason: human-readable shutdown reason
@type reason: str
"""
self.done = True
logdebug('[%s].shutdown: reason [%s]'%(self.resolved_name, reason))
try:
#TODO: make service manager configurable
get_service_manager().unregister(self.resolved_name, self)
except Exception as e:
logerr("Unable to unregister with master: "+traceback.format_exc())
raise ServiceException("Unable to connect to master: %s"%e)
def spin():
"""
Blocks until ROS node is shutdown. Yields activity to other threads.
@raise ROSInitException: if node is not in a properly initialized state
"""
if not rospy.core.is_initialized():
raise rospy.exceptions.ROSInitException("client code must call rospy.init_node() first")
logdebug("node[%s, %s] entering spin(), pid[%s]", rospy.core.get_caller_id(), rospy.core.get_node_uri(), os.getpid())
try:
while not rospy.core.is_shutdown():
rospy.rostime.wallsleep(0.5)
except KeyboardInterrupt:
logdebug("keyboard interrupt, shutting down")
rospy.core.signal_shutdown('keyboard interrupt')
def spin():
"""
Blocks until ROS node is shutdown. Yields activity to other threads.
@raise ROSInitException: if node is not in a properly initialized state
"""
if not rospy.core.is_initialized():
raise rospy.exceptions.ROSInitException("client code must call rospy.init_node() first")
logdebug("node[%s, %s] entering spin(), pid[%s]", rospy.core.get_caller_id(), rospy.core.get_node_uri(), os.getpid())
try:
while not rospy.core.is_shutdown():
rospy.rostime.wallsleep(0.5)
except KeyboardInterrupt:
logdebug("keyboard interrupt, shutting down")
rospy.core.signal_shutdown('keyboard interrupt')
def _create_server_sock(self):
"""
binds the server socket. ROS_IP/ROS_HOSTNAME may restrict
binding to loopback interface.
"""
if rosgraph.network.use_ipv6():
server_sock = socket.socket(socket.AF_INET6, socket.SOCK_STREAM)
else:
server_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
server_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
logdebug('binding to ' + str(rosgraph.network.get_bind_address()) + ' ' + str(self.port))
server_sock.bind((rosgraph.network.get_bind_address(), self.port))
(self.addr, self.port) = server_sock.getsockname()[0:2]
logdebug('bound to ' + str(self.addr) + ' ' + str(self.port))
server_sock.listen(5)
return server_sock
def _create_server_sock(self):
"""
binds the server socket. ROS_IP/ROS_HOSTNAME may restrict
binding to loopback interface.
"""
if rosgraph.network.use_ipv6():
server_sock = socket.socket(socket.AF_INET6, socket.SOCK_STREAM)
else:
server_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
server_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
logdebug('binding to ' + str(rosgraph.network.get_bind_address()) + ' ' + str(self.port))
server_sock.bind((rosgraph.network.get_bind_address(), self.port))
(self.addr, self.port) = server_sock.getsockname()[0:2]
logdebug('bound to ' + str(self.addr) + ' ' + str(self.port))
server_sock.listen(5)
return server_sock
class TCPServer(object):
"""
Simple server that accepts inbound TCP/IP connections and hands
them off to a handler function. TCPServer obeys the
ROS_IP/ROS_HOSTNAME environment variables
"""
def __init__(self, inbound_handler, port=0):
"""
Setup a server socket listening on the specified port. If the
port is omitted, will choose any open port.
@param inbound_handler: handler to invoke with
new connection
@type inbound_handler: fn(sock, addr)
@param port: port to bind to, omit/0 to bind to any
@type port: int
"""
self.port = port #will get overwritten if port=0
self.addr = None #set at socket bind
self.is_shutdown = False
self.inbound_handler = inbound_handler
try:
self.server_sock = self._create_server_sock()
except:
self.server_sock = None
raise
def start(self):
"""Runs the run() loop in a separate thread"""
thread.start_new_thread(self.run, ())
def run(self):
"""
Main TCP receive loop. Should be run in a separate thread -- use start()
to do this automatically.
"""
self.is_shutdown = False
if not self.server_sock:
raise ROSInternalException("%s did not connect" %
self.__class__.__name__)
while not self.is_shutdown:
try:
(client_sock, client_addr) = self.server_sock.accept()
except socket.timeout:
continue
if self.is_shutdown:
break
try:
#leave threading decisions up to inbound_handler
self.inbound_handler(client_sock, client_addr)
except socket.error, e:
if not self.is_shutdown:
traceback.print_exc()
logwarn(
"Failed to handle inbound connection due to socket error: %s"
% e)
logdebug("TCPServer[%s] shutting down", self.port)
def write_data(self, data):
"""
Write raw data to transport
@raise TransportInitialiationError: could not be initialized
@raise TransportTerminated: no longer open for publishing
"""
if not self.socket:
raise TransportInitError(
"TCPROS transport was not successfully initialized")
if self.done:
raise TransportTerminated("connection closed")
try:
#TODO: get rid of sendalls and replace with async-style publishing
self.socket.sendall(data.encode())
self.stat_bytes += len(data)
self.stat_num_msg += 1
except IOError as xxx_todo_changeme:
#for now, just document common errno's in code
(errno, msg) = xxx_todo_changeme.args
#for now, just document common errno's in code
if errno == 32: #broken pipe
logdebug("ERROR: Broken Pipe")
self.close()
raise TransportTerminated(str(errno) + msg)
raise #re-raise
except socket.error as xxx_todo_changeme1:
#for now, just document common errno's in code
(errno, msg) = xxx_todo_changeme1.args
#for now, just document common errno's in code
if errno == 32: #broken pipe
logdebug("[%s]: Closing connection [%s] due to broken pipe",
self.name, self.endpoint_id)
self.close()
raise TransportTerminated(msg)
elif errno == 104: #connection reset by peer
logdebug("[%s]: Peer [%s] has closed connection", self.name,
self.endpoint_id)
self.close()
raise TransportTerminated(msg)
else:
rospydebug("unknown socket error writing data: %s",
traceback.format_exc())
logdebug(
"[%s]: closing connection [%s] due to unknown socket error: %s",
self.name, self.endpoint_id, msg)
self.close()
raise TransportTerminated(str(errno) + ' ' + msg)
#except:
#TODO: try to figure out common errors here
#raise
return True
def write_data(self, data):
"""
Write raw data to transport
@raise TransportInitialiationError: could not be initialized
@raise TransportTerminated: no longer open for publishing
"""
if not self.socket:
raise TransportInitError(
"TCPROS transport was not successfully initialized")
if self.done:
raise TransportTerminated("connection closed")
try:
#TODO: get rid of sendalls and replace with async-style publishing
self.socket.sendall(data)
self.stat_bytes += len(data)
self.stat_num_msg += 1
except IOError, (errno, msg):
#for now, just document common errno's in code
if errno == 32: #broken pipe
logdebug("ERROR: Broken Pipe")
self.close()
raise TransportTerminated(str(errno) + msg)
raise #re-raise
def write_data(self, data):
"""
Write raw data to transport
@raise TransportInitialiationError: could not be initialized
@raise TransportTerminated: no longer open for publishing
"""
if not self.socket:
raise TransportInitError("TCPROS transport was not successfully initialized")
if self.done:
raise TransportTerminated("connection closed")
try:
#TODO: get rid of sendalls and replace with async-style publishing
self.socket.sendall(data.encode())
self.stat_bytes += len(data)
self.stat_num_msg += 1
except IOError as xxx_todo_changeme:
#for now, just document common errno's in code
(errno, msg) = xxx_todo_changeme.args
#for now, just document common errno's in code
if errno == 32: #broken pipe
logdebug("ERROR: Broken Pipe")
self.close()
raise TransportTerminated(str(errno)+msg)
raise #re-raise
except socket.error as xxx_todo_changeme1:
#for now, just document common errno's in code
(errno, msg) = xxx_todo_changeme1.args
#for now, just document common errno's in code
if errno == 32: #broken pipe
logdebug("[%s]: Closing connection [%s] due to broken pipe", self.name, self.endpoint_id)
self.close()
raise TransportTerminated(msg)
elif errno == 104: #connection reset by peer
logdebug("[%s]: Peer [%s] has closed connection", self.name, self.endpoint_id)
self.close()
raise TransportTerminated(msg)
else:
rospydebug("unknown socket error writing data: %s",traceback.format_exc())
logdebug("[%s]: closing connection [%s] due to unknown socket error: %s", self.name, self.endpoint_id, msg)
self.close()
raise TransportTerminated(str(errno)+' '+msg)
#except:
#TODO: try to figure out common errors here
#raise
return True
def connect(self, dest_addr, dest_port, endpoint_id, timeout=None):
"""
Establish TCP connection to the specified
address/port. connect() always calls L{write_header()} and
L{read_header()} after the connection is made
@param dest_addr: destination IP address
@type dest_addr: str
@param dest_port: destination port
@type dest_port: int
@param endpoint_id: string identifier for connection (for statistics)
@type endpoint_id: str
@param timeout: (optional keyword) timeout in seconds
@type timeout: float
@raise TransportInitError: if unable to create connection
"""
# first make sure that if ROS_HOSTNAME=localhost, we will not attempt
# to connect to anything other than localhost
if ("ROS_HOSTNAME" in os.environ) and (os.environ["ROS_HOSTNAME"] == "localhost"):
if not rosgraph.network.is_local_address(dest_addr):
msg = "attempted to connect to non-local host [%s] from a node launched with ROS_HOSTNAME=localhost" % (dest_addr)
logwarn(msg)
self.close()
raise TransportInitError(msg) # bubble up
# now we can proceed with trying to connect.
try:
self.endpoint_id = endpoint_id
self.dest_address = (dest_addr, dest_port)
if rosgraph.network.use_ipv6():
s = socket.socket(socket.AF_INET6, socket.SOCK_STREAM)
else:
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
if _is_use_tcp_keepalive():
# OSX (among others) does not define these options
if hasattr(socket, 'TCP_KEEPCNT') and \
hasattr(socket, 'TCP_KEEPIDLE') and \
hasattr(socket, 'TCP_KEEPINTVL'):
# turn on KEEPALIVE
s.setsockopt(socket.SOL_SOCKET, socket.SO_KEEPALIVE, 1)
# - # keepalive failures before actual connection failure
s.setsockopt(socket.SOL_TCP, socket.TCP_KEEPCNT, 9)
# - timeout before starting KEEPALIVE process
s.setsockopt(socket.SOL_TCP, socket.TCP_KEEPIDLE, 60)
# - interval to send KEEPALIVE after IDLE timeout
s.setsockopt(socket.SOL_TCP, socket.TCP_KEEPINTVL, 10)
if timeout is not None:
s.settimeout(timeout)
self.socket = s
logdebug('connecting to ' + str(dest_addr)+ ' ' + str(dest_port))
self.socket.connect((dest_addr, dest_port))
self.write_header()
self.read_header()
self.local_endpoint = self.socket.getsockname()
self.remote_endpoint = (dest_addr, dest_port)
except TransportInitError as tie:
rospyerr("Unable to initiate TCP/IP socket to %s:%s (%s): %s"%(dest_addr, dest_port, endpoint_id, traceback.format_exc()))
raise
except Exception as e:
#logerr("Unknown error initiating TCP/IP socket to %s:%s (%s): %s"%(dest_addr, dest_port, endpoint_id, str(e)))
rospywarn("Unknown error initiating TCP/IP socket to %s:%s (%s): %s"%(dest_addr, dest_port, endpoint_id, traceback.format_exc()))
# check for error type and reason. On unknown errors the socket will be closed
# to avoid reconnection and error reproduction
if not isinstance(e, socket.error):
# FATAL: no reconnection as error is unknown
self.close()
elif not isinstance(e, socket.timeout) and e.errno not in [
errno.ENETDOWN, errno.ENETUNREACH, errno.ENETRESET,
errno.ECONNABORTED, errno.ETIMEDOUT, errno.EHOSTDOWN, errno.EHOSTUNREACH]:
# reconnect in follow cases, otherwise close the socket:
# 1. socket.timeout: on timeouts caused by delays on wireless links
# 2. ENETDOWN (100), ENETUNREACH (101), ENETRESET (102), ECONNABORTED (103):
# while using ROS_HOSTNAME ros binds to a specific interface. Theses errors
# are thrown on interface shutdown e.g. on reconnection in LTE networks
# 3. ETIMEDOUT (110): same like 1. (for completeness)
# 4. EHOSTDOWN (112), EHOSTUNREACH (113): while network and/or DNS-server is not reachable
#
# no reconnection as error is not 1.-4.
self.close()
raise TransportInitError(str(e)) #re-raise i/o error
def connect(self, dest_addr, dest_port, endpoint_id, timeout=None):
"""
Establish TCP connection to the specified
address/port. connect() always calls L{write_header()} and
L{read_header()} after the connection is made
@param dest_addr: destination IP address
@type dest_addr: str
@param dest_port: destination port
@type dest_port: int
@param endpoint_id: string identifier for connection (for statistics)
@type endpoint_id: str
@param timeout: (optional keyword) timeout in seconds
@type timeout: float
@raise TransportInitError: if unable to create connection
"""
# first make sure that if ROS_HOSTNAME=localhost, we will not attempt
# to connect to anything other than localhost
if ("ROS_HOSTNAME" in os.environ) and (os.environ["ROS_HOSTNAME"] == "localhost"):
if not rosgraph.network.is_local_address(dest_addr):
msg = "attempted to connect to non-local host [%s] from a node launched with ROS_HOSTNAME=localhost" % (dest_addr)
logwarn(msg)
self.close()
raise TransportInitError(msg) # bubble up
# now we can proceed with trying to connect.
try:
self.endpoint_id = endpoint_id
self.dest_address = (dest_addr, dest_port)
if rosgraph.network.use_ipv6():
s = socket.socket(socket.AF_INET6, socket.SOCK_STREAM)
else:
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
if _is_use_tcp_keepalive():
# OSX (among others) does not define these options
if hasattr(socket, 'TCP_KEEPCNT') and \
hasattr(socket, 'TCP_KEEPIDLE') and \
hasattr(socket, 'TCP_KEEPINTVL'):
# turn on KEEPALIVE
s.setsockopt(socket.SOL_SOCKET, socket.SO_KEEPALIVE, 1)
# - # keepalive failures before actual connection failure
s.setsockopt(socket.SOL_TCP, socket.TCP_KEEPCNT, 9)
# - timeout before starting KEEPALIVE process
s.setsockopt(socket.SOL_TCP, socket.TCP_KEEPIDLE, 60)
# - interval to send KEEPALIVE after IDLE timeout
s.setsockopt(socket.SOL_TCP, socket.TCP_KEEPINTVL, 10)
if timeout is not None:
s.settimeout(timeout)
self.socket = s
logdebug('connecting to ' + str(dest_addr)+ ' ' + str(dest_port))
self.socket.connect((dest_addr, dest_port))
self.write_header()
self.read_header()
self.local_endpoint = self.socket.getsockname()
self.remote_endpoint = (dest_addr, dest_port)
except TransportInitError as tie:
rospyerr("Unable to initiate TCP/IP socket to %s:%s (%s): %s"%(dest_addr, dest_port, endpoint_id, traceback.format_exc()))
raise
except Exception as e:
#logerr("Unknown error initiating TCP/IP socket to %s:%s (%s): %s"%(dest_addr, dest_port, endpoint_id, str(e)))
rospywarn("Unknown error initiating TCP/IP socket to %s:%s (%s): %s"%(dest_addr, dest_port, endpoint_id, traceback.format_exc()))
# FATAL: no reconnection as error is unknown
self.close()
raise TransportInitError(str(e)) #re-raise i/o error
try:
#TODO: get rid of sendalls and replace with async-style publishing
self.socket.sendall(data)
self.stat_bytes += len(data)
self.stat_num_msg += 1
except IOError, (errno, msg):
#for now, just document common errno's in code
if errno == 32: #broken pipe
logdebug("ERROR: Broken Pipe")
self.close()
raise TransportTerminated(str(errno) + msg)
raise #re-raise
except socket.error, (errno, msg):
#for now, just document common errno's in code
if errno == 32: #broken pipe
logdebug("[%s]: Closing connection [%s] due to broken pipe",
self.name, self.endpoint_id)
self.close()
raise TransportTerminated(msg)
elif errno == 104: #connection reset by peer
logdebug("[%s]: Peer [%s] has closed connection", self.name,
self.endpoint_id)
self.close()
raise TransportTerminated(msg)
else:
rospydebug("unknown socket error writing data: %s",
traceback.format_exc())
logdebug(
"[%s]: closing connection [%s] due to unknown socket error: %s",
self.name, self.endpoint_id, msg)
self.close()
raise TransportTerminated(str(errno) + ' ' + msg)
def stopTwist(self):
vel_msg = self.getTwist(0, 0)
print(vel_msg)
# self.log.write(str(vel_msg) + "\n")
self.log.close()
self.velocity_publisher.publish(vel_msg)
""" Main function """
if __name__ == '__main__':
rospy.init_node('turtlebot_controller', anonymous=True)
x = turtlebot()
"""
Blocks until ROS node is shutdown. Yields activity to other threads.
@raise ROSInitException: if node is not in a properly initialized state
"""
if not rospy.core.is_initialized():
raise rospy.exceptions.ROSInitException(
"client code must call rospy.init_node() first")
logdebug("node[%s, %s] entering spin(), pid[%s]",
rospy.core.get_caller_id(), rospy.core.get_node_uri(),
os.getpid())
try:
while not rospy.core.is_shutdown():
x.onlineTracking()
rospy.core.signal_shutdown('tracking ended')
x.stopTwist()
except KeyboardInterrupt:
logdebug("keyboard interrupt, shutting down")
rospy.core.signal_shutdown('keyboard interrupt')
def init_node(name, argv=None, anonymous=False, log_level=None, disable_rostime=False, disable_rosout=False, disable_signals=False, xmlrpc_port=0, tcpros_port=0):
"""
Register client node with the master under the specified name.
This MUST be called from the main Python thread unless
disable_signals is set to True. Duplicate calls to init_node are
only allowed if the arguments are identical as the side-effects of
this method are not reversible.
@param name: Node's name. This parameter must be a base name,
meaning that it cannot contain namespaces (i.e. '/')
@type name: str
@param argv: Command line arguments to this program, including
remapping arguments (default: sys.argv). If you provide argv
to init_node(), any previously created rospy data structure
(Publisher, Subscriber, Service) will have invalid
mappings. It is important that you call init_node() first if
you wish to provide your own argv.
@type argv: [str]
@param anonymous: if True, a name will be auto-generated for the
node using name as the base. This is useful when you wish to
have multiple instances of the same node and don't care about
their actual names (e.g. tools, guis). name will be used as
the stem of the auto-generated name. NOTE: you cannot remap
the name of an anonymous node.
@type anonymous: bool
@param log_level: log level for sending message to /rosout and log
file, which is INFO by default. For convenience, you may use
rospy.DEBUG, rospy.INFO, rospy.ERROR, rospy.WARN, rospy.FATAL
@type log_level: int
@param disable_signals: If True, rospy will not register its own
signal handlers. You must set this flag if (a) you are unable
to call init_node from the main thread and/or you are using
rospy in an environment where you need to control your own
signal handling (e.g. WX). If you set this to True, you should
call rospy.signal_shutdown(reason) to initiate clean shutdown.
NOTE: disable_signals is overridden to True if
roslib.is_interactive() is True.
@type disable_signals: bool
@param disable_rostime: for internal testing only: suppresses
automatic subscription to rostime
@type disable_rostime: bool
@param disable_rosout: for internal testing only: suppress
auto-publication of rosout
@type disable_rostime: bool
@param xmlrpc_port: If provided, it will use this port number for the client
XMLRPC node.
@type xmlrpc_port: int
@param tcpros_port: If provided, the TCPROS server will listen for
connections on this port
@type tcpros_port: int
@raise ROSInitException: if initialization/registration fails
@raise ValueError: if parameters are invalid (e.g. name contains a namespace or is otherwise illegal)
"""
if argv is None:
argv = sys.argv
else:
# reload the mapping table. Any previously created rospy data
# structure does *not* reinitialize based on the new mappings.
rospy.names.reload_mappings(argv)
# this test can be eliminated once we change from warning to error in the next check
if rosgraph.names.SEP in name:
raise ValueError("namespaces are not allowed in node names")
if not rosgraph.names.is_legal_base_name(name):
import warnings
warnings.warn("'%s' is not a legal ROS base name. This may cause problems with other ROS tools"%name, stacklevel=2)
global _init_node_args
# #972: allow duplicate init_node args if calls are identical
# NOTE: we don't bother checking for node name aliases (e.g. 'foo' == '/foo').
if _init_node_args:
if _init_node_args != (name, argv, anonymous, log_level, disable_rostime, disable_signals):
raise rospy.exceptions.ROSException("rospy.init_node() has already been called with different arguments: "+str(_init_node_args))
else:
return #already initialized
# for scripting environments, we don't want to use the ROS signal
# handlers
disable_signals = disable_signals or roslib.is_interactive()
_init_node_args = (name, argv, anonymous, log_level, disable_rostime, disable_signals)
if not disable_signals:
# NOTE: register_signals must be called from main thread
rospy.core.register_signals() # add handlers for SIGINT/etc...
else:
logdebug("signal handlers for rospy disabled")
# check for name override
mappings = rospy.names.get_mappings()
if '__name' in mappings:
# use rosgraph version of resolve_name to avoid remapping
name = rosgraph.names.resolve_name(mappings['__name'], rospy.core.get_caller_id())
if anonymous:
logdebug("[%s] WARNING: due to __name setting, anonymous setting is being changed to false"%name)
anonymous = False
if anonymous:
# not as good as a uuid/guid, but more readable. can't include
# hostname as that is not guaranteed to be a legal ROS name
name = "%s_%s_%s"%(name, os.getpid(), int(time.time()*1000))
resolved_node_name = rospy.names.resolve_name(name)
rospy.core.configure_logging(resolved_node_name)
# #1810
rospy.names.initialize_mappings(resolved_node_name)
logger = logging.getLogger("rospy.client")
logger.info("init_node, name[%s], pid[%s]", resolved_node_name, os.getpid())
# node initialization blocks until registration with master
node = rospy.impl.init.start_node(os.environ, resolved_node_name, port=xmlrpc_port, tcpros_port=tcpros_port)
rospy.core.set_node_uri(node.uri)
rospy.core.add_shutdown_hook(node.shutdown)
if rospy.core.is_shutdown():
logger.warn("aborting node initialization as shutdown has been triggered")
raise rospy.exceptions.ROSInitException("init_node interrupted before it could complete")
# upload private params (set via command-line) to parameter server
_init_node_params(argv, name)
rospy.core.set_initialized(True)
if not disable_rosout:
rospy.impl.rosout.init_rosout()
rospy.impl.rosout.load_rosout_handlers(log_level)
if not disable_rostime:
if not rospy.impl.simtime.init_simtime():
raise rospy.exceptions.ROSInitException("Failed to initialize time. Please check logs for additional details")
else:
rospy.rostime.set_rostime_initialized(True)
logdebug("init_node, name[%s], pid[%s]", resolved_node_name, os.getpid())
# advertise logging level services
Service('~get_loggers', GetLoggers, _get_loggers)
Service('~set_logger_level', SetLoggerLevel, _set_logger_level)
def start(self, uri, master_uri):
"""
Start the RegManager. This should be passed in as an argument to a thread
starter as the RegManager is designed to spin in its own thread
@param uri: URI of local node
@type uri: str
@param master_uri: Master URI
@type master_uri: str
"""
self.registered = False
self.master_uri = master_uri
self.uri = uri
first = True
tm = get_topic_manager()
sm = get_service_manager()
ns = get_namespace()
caller_id = get_caller_id()
if not master_uri or master_uri == uri:
registered = True
master = None
else:
registered = False
master = xmlrpcapi(master_uri)
self.logger.info("Registering with master node %s", master_uri)
while not registered and not is_shutdown():
try:
try:
# prevent TopicManager and ServiceManager from accepting registrations until we are done
tm.lock.acquire()
sm.lock.acquire()
pub, sub, srv = tm.get_publications(), tm.get_subscriptions(), sm.get_services()
for resolved_name, data_type in pub:
self.logger.info("Registering publisher topic [%s] type [%s] with master", resolved_name, data_type)
code, msg, val = master.registerPublisher(caller_id, resolved_name, data_type, uri)
if code != 1:
logfatal("cannot register publication topic [%s] with master: %s"%(resolved_name, msg))
signal_shutdown("master/node incompatibility with register publisher")
for resolved_name, data_type in sub:
self.logger.info("registering subscriber topic [%s] type [%s] with master", resolved_name, data_type)
code, msg, val = master.registerSubscriber(caller_id, resolved_name, data_type, uri)
if code != 1:
logfatal("cannot register subscription topic [%s] with master: %s"%(resolved_name, msg))
signal_shutdown("master/node incompatibility with register subscriber")
else:
self.publisher_update(resolved_name, val)
for resolved_name, service_uri in srv:
self.logger.info("registering service [%s] uri [%s] with master", resolved_name, service_uri)
code, msg, val = master.registerService(caller_id, resolved_name, service_uri, uri)
if code != 1:
logfatal("cannot register service [%s] with master: %s"%(resolved_name, msg))
signal_shutdown("master/node incompatibility with register service")
registered = True
# Subscribe to updates to our state
get_registration_listeners().add_listener(self)
finally:
sm.lock.release()
tm.lock.release()
if pub or sub:
logdebug("Registered [%s] with master node %s", caller_id, master_uri)
else:
logdebug("No topics to register with master node %s", master_uri)
except Exception as e:
if first:
# this use to print to console always, arguable whether or not this should be subjected to same configuration options as logging
logerr("Unable to immediately register with master node [%s]: master may not be running yet. Will keep trying."%master_uri)
first = False
time.sleep(0.2)
self.registered = True
self.run()
def init_node(name,
argv=None,
anonymous=False,
log_level=None,
disable_rostime=False,
disable_rosout=False,
disable_signals=False):
"""
Register client node with the master under the specified name.
This MUST be called from the main Python thread unless
disable_signals is set to True. Duplicate calls to init_node are
only allowed if the arguments are identical as the side-effects of
this method are not reversible.
@param name: Node's name. This parameter must be a base name,
meaning that it cannot contain namespaces (i.e. '/')
@type name: str
@param argv: Command line arguments to this program, including
remapping arguments (default: sys.argv). If you provide argv
to init_node(), any previously created rospy data structure
(Publisher, Subscriber, Service) will have invalid
mappings. It is important that you call init_node() first if
you wish to provide your own argv.
@type argv: [str]
@param anonymous: if True, a name will be auto-generated for the
node using name as the base. This is useful when you wish to
have multiple instances of the same node and don't care about
their actual names (e.g. tools, guis). name will be used as
the stem of the auto-generated name. NOTE: you cannot remap
the name of an anonymous node.
@type anonymous: bool
@param log_level: log level for sending message to /rosout and log
file, which is INFO by default. For convenience, you may use
rospy.DEBUG, rospy.INFO, rospy.ERROR, rospy.WARN, rospy.FATAL
@type log_level: int
@param disable_signals: If True, rospy will not register its own
signal handlers. You must set this flag if (a) you are unable
to call init_node from the main thread and/or you are using
rospy in an environment where you need to control your own
signal handling (e.g. WX). If you set this to True, you should
call rospy.signal_shutdown(reason) to initiate clean shutdown.
NOTE: disable_signals is overridden to True if
roslib.is_interactive() is True.
@type disable_signals: bool
@param disable_rostime: for internal testing only: suppresses
automatic subscription to rostime
@type disable_rostime: bool
@param disable_rosout: for internal testing only: suppress
auto-publication of rosout
@type disable_rostime: bool
@raise ROSInitException: if initialization/registration fails
@raise ValueError: if parameters are invalid (e.g. name contains a namespace or is otherwise illegal)
"""
if argv is None:
argv = sys.argv
else:
# reload the mapping table. Any previously created rospy data
# structure does *not* reinitialize based on the new mappings.
rospy.names.reload_mappings(argv)
# this test can be eliminated once we change from warning to error in the next check
if rosgraph.names.SEP in name:
raise ValueError("namespaces are not allowed in node names")
if not rosgraph.names.is_legal_base_name(name):
import warnings
warnings.warn(
"'%s' is not a legal ROS base name. This may cause problems with other ROS tools"
% name,
stacklevel=2)
global _init_node_args
# #972: allow duplicate init_node args if calls are identical
# NOTE: we don't bother checking for node name aliases (e.g. 'foo' == '/foo').
if _init_node_args:
if _init_node_args != (name, argv, anonymous, log_level,
disable_rostime, disable_signals):
raise rospy.exceptions.ROSException(
"rospy.init_node() has already been called with different arguments: "
+ str(_init_node_args))
else:
return #already initialized
# for scripting environments, we don't want to use the ROS signal
# handlers
disable_signals = disable_signals or roslib.is_interactive()
_init_node_args = (name, argv, anonymous, log_level, disable_rostime,
disable_signals)
if not disable_signals:
# NOTE: register_signals must be called from main thread
rospy.core.register_signals() # add handlers for SIGINT/etc...
else:
logdebug("signal handlers for rospy disabled")
# check for name override
mappings = rospy.names.get_mappings()
if '__name' in mappings:
# use rosgraph version of resolve_name to avoid remapping
name = rosgraph.names.resolve_name(mappings['__name'],
rospy.core.get_caller_id())
if anonymous:
logdebug(
"[%s] WARNING: due to __name setting, anonymous setting is being changed to false"
% name)
anonymous = False
if anonymous:
# not as good as a uuid/guid, but more readable. can't include
# hostname as that is not guaranteed to be a legal ROS name
name = "%s_%s_%s" % (name, os.getpid(), int(time.time() * 1000))
resolved_node_name = rospy.names.resolve_name(name)
rospy.core.configure_logging(resolved_node_name)
# #1810
rospy.names.initialize_mappings(resolved_node_name)
logger = logging.getLogger("rospy.client")
logger.info("init_node, name[%s], pid[%s]", resolved_node_name,
os.getpid())
# node initialization blocks until registration with master
node = rospy.impl.init.start_node(os.environ, resolved_node_name)
rospy.core.set_node_uri(node.uri)
rospy.core.add_shutdown_hook(node.shutdown)
if rospy.core.is_shutdown():
logger.warn(
"aborting node initialization as shutdown has been triggered")
raise rospy.exceptions.ROSInitException(
"init_node interrupted before it could complete")
# upload private params (set via command-line) to parameter server
_init_node_params(argv, name)
rospy.core.set_initialized(True)
if not disable_rosout:
rospy.impl.rosout.init_rosout()
rospy.impl.rosout.load_rosout_handlers(log_level)
if not disable_rostime:
if not rospy.impl.simtime.init_simtime():
raise rospy.exceptions.ROSInitException(
"Failed to initialize time. Please check logs for additional details"
)
else:
rospy.rostime.set_rostime_initialized(True)
logdebug("init_node, name[%s], pid[%s]", resolved_node_name, os.getpid())
# advertise logging level services
Service('~get_loggers', GetLoggers, _get_loggers)
Service('~set_logger_level', SetLoggerLevel, _set_logger_level)
def connect(self, dest_addr, dest_port, endpoint_id, timeout=None):
"""
Establish TCP connection to the specified
address/port. connect() always calls L{write_header()} and
L{read_header()} after the connection is made
@param dest_addr: destination IP address
@type dest_addr: str
@param dest_port: destination port
@type dest_port: int
@param endpoint_id: string identifier for connection (for statistics)
@type endpoint_id: str
@param timeout: (optional keyword) timeout in seconds
@type timeout: float
@raise TransportInitError: if unable to create connection
"""
# first make sure that if ROS_HOSTNAME=localhost, we will not attempt
# to connect to anything other than localhost
if ("ROS_HOSTNAME" in os.environ) and (os.environ["ROS_HOSTNAME"] == "localhost"):
if not rosgraph.network.is_local_address(dest_addr):
msg = "attempted to connect to non-local host [%s] from a node launched with ROS_HOSTNAME=localhost" % (dest_addr)
logwarn(msg)
self.close()
raise TransportInitError(msg) # bubble up
# now we can proceed with trying to connect.
try:
self.endpoint_id = endpoint_id
self.dest_address = (dest_addr, dest_port)
if rosgraph.network.use_ipv6():
s = socket.socket(socket.AF_INET6, socket.SOCK_STREAM)
else:
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
if _is_use_tcp_keepalive():
# OSX (among others) does not define these options
if hasattr(socket, 'TCP_KEEPCNT') and \
hasattr(socket, 'TCP_KEEPIDLE') and \
hasattr(socket, 'TCP_KEEPINTVL'):
# turn on KEEPALIVE
s.setsockopt(socket.SOL_SOCKET, socket.SO_KEEPALIVE, 1)
# - # keepalive failures before actual connection failure
s.setsockopt(socket.SOL_TCP, socket.TCP_KEEPCNT, 9)
# - timeout before starting KEEPALIVE process
s.setsockopt(socket.SOL_TCP, socket.TCP_KEEPIDLE, 60)
# - interval to send KEEPALIVE after IDLE timeout
s.setsockopt(socket.SOL_TCP, socket.TCP_KEEPINTVL, 10)
if timeout is not None:
s.settimeout(timeout)
self.socket = s
logdebug('connecting to ' + str(dest_addr)+ ' ' + str(dest_port))
self.socket.connect((dest_addr, dest_port))
self.write_header()
self.read_header()
self.local_endpoint = self.socket.getsockname()
self.remote_endpoint = (dest_addr, dest_port)
except TransportInitError as tie:
rospyerr("Unable to initiate TCP/IP socket to %s:%s (%s): %s"%(dest_addr, dest_port, endpoint_id, traceback.format_exc()))
raise
except Exception as e:
#logerr("Unknown error initiating TCP/IP socket to %s:%s (%s): %s"%(dest_addr, dest_port, endpoint_id, str(e)))
rospywarn("Unknown error initiating TCP/IP socket to %s:%s (%s): %s"%(dest_addr, dest_port, endpoint_id, traceback.format_exc()))
# check for error type and reason. On unknown errors the socket will be closed
# to avoid reconnection and error reproduction
if not isinstance(e, socket.error):
# FATAL: no reconnection as error is unknown
self.close()
elif not isinstance(e, socket.timeout) and e.errno not in [100, 101, 102, 103, 110, 112, 113]:
# reconnect in follow cases, otherwise close the socket:
# 1. socket.timeout: on timeouts caused by delays on wireless links
# 2. ENETDOWN (100), ENETUNREACH (101), ENETRESET (102), ECONNABORTED (103):
# while using ROS_HOSTNAME ros binds to a specific interface. Theses errors
# are thrown on interface shutdown e.g. on reconnection in LTE networks
# 3. ETIMEDOUT (110): same like 1. (for completeness)
# 4. EHOSTDOWN (112), EHOSTUNREACH (113): while network and/or DNS-server is not reachable
#
# no reconnection as error is not 1.-4.
self.close()
raise TransportInitError(str(e)) #re-raise i/o error
