def _tcp_server_callback(self, sock, client_addr):
"""
TCPServer callback: detects incoming topic or service connection and passes connection accordingly
@param sock: socket connection
@type sock: socket.socket
@param client_addr: client address
@type client_addr: (str, int)
@raise TransportInitError: If transport cannot be succesfully initialized
"""
#TODOXXX:rewrite this logic so it is possible to create TCPROSTransport object first, set its protocol,
#and then use that to do the writing
try:
buff_size = 4096 # size of read buffer
if python3 == 0:
#initialize read_ros_handshake_header with BytesIO for Python 3 (instead of bytesarray())
header = read_ros_handshake_header(sock, StringIO(), buff_size)
else:
header = read_ros_handshake_header(sock, BytesIO(), buff_size)
if 'topic' in header:
err_msg = self.topic_connection_handler(
sock, client_addr, header)
elif 'service' in header:
err_msg = self.service_connection_handler(
sock, client_addr, header)
else:
err_msg = 'no topic or service name detected'
if err_msg:
# shutdown race condition: nodes that come up and down
# quickly can receive connections during teardown.
# We use is_shutdown_requested() because we can get
# into bad connection states during client shutdown
# hooks.
if not rospy.core.is_shutdown_requested():
write_ros_handshake_header(sock, {'error': err_msg})
raise TransportInitError(
"Could not process inbound connection: " + err_msg +
str(header))
else:
write_ros_handshake_header(sock,
{'error': 'node shutting down'})
return
except rospy.exceptions.TransportInitError as e:
logwarn(str(e))
if sock is not None:
sock.close()
except Exception as e:
# collect stack trace separately in local log file
if not rospy.core.is_shutdown_requested():
logwarn("Inbound TCP/IP connection failed from '%s': %s",
str(client_addr), e)
rospyerr("Inbound TCP/IP connection failed from '%s':\n%s",
str(client_addr), traceback.format_exc())
if sock is not None:
sock.close()
def _tcp_server_callback(self, sock, client_addr):
"""
TCPServer callback: detects incoming topic or service connection and passes connection accordingly
@param sock: socket connection
@type sock: socket.socket
@param client_addr: client address
@type client_addr: (str, int)
@raise TransportInitError: If transport cannot be succesfully initialized
"""
#TODOXXX:rewrite this logic so it is possible to create TCPROSTransport object first, set its protocol,
#and then use that to do the writing
try:
buff_size = 4096 # size of read buffer
if python3 == 0:
#initialize read_ros_handshake_header with BytesIO for Python 3 (instead of bytesarray())
header = read_ros_handshake_header(sock, StringIO(), buff_size)
else:
header = read_ros_handshake_header(sock, BytesIO(), buff_size)
if 'topic' in header:
err_msg = self.topic_connection_handler(sock, client_addr, header)
elif 'service' in header:
err_msg = self.service_connection_handler(sock, client_addr, header)
else:
err_msg = 'no topic or service name detected'
if err_msg:
# shutdown race condition: nodes that come up and down
# quickly can receive connections during teardown.
# We use is_shutdown_requested() because we can get
# into bad connection states during client shutdown
# hooks.
if not rospy.core.is_shutdown_requested():
write_ros_handshake_header(sock, {'error' : err_msg})
raise TransportInitError("Could not process inbound connection: "+err_msg+str(header))
else:
write_ros_handshake_header(sock, {'error' : 'node shutting down'})
return
except rospy.exceptions.TransportInitError as e:
logwarn(str(e))
if sock is not None:
sock.close()
except Exception as e:
# collect stack trace separately in local log file
if not rospy.core.is_shutdown_requested():
logwarn("Inbound TCP/IP connection failed: %s", e)
rospyerr("Inbound TCP/IP connection failed:\n%s", traceback.format_exc())
if sock is not None:
sock.close()
def run(self):
while not self._shutdownflag:
try:
(client_sock, _) = self._server_sock.accept()
self.addSubscriberLink(client_sock)
if sys.version_info[0] == 3:
header = read_ros_handshake_header(client_sock, BytesIO(),
65536)
else:
header = read_ros_handshake_header(client_sock, StringIO(),
65536)
fileno = client_sock.fileno()
poller = None
if hasattr(select, 'poll'):
ready = False
poller = select.poll()
poller.register(fileno, select.POLLOUT)
while not ready:
events = poller.poll()
for _, flag in events:
if flag & select.POLLOUT:
ready = True
else:
ready = None
while not ready:
try:
_, ready, _ = select.select([], [fileno], [])
except ValueError:
print("ValueError")
return
client_sock.setblocking(1)
guard_pub = OpenRTM_aist.Guard.ScopedLock(self._pub_mutex)
for publisher in self._publishers:
publisher.connect(client_sock, header)
del guard_pub
if poller:
poller.unregister(fileno)
except rosgraph.network.ROSHandshakeException:
print("read ROS handshake exception")
except BaseException:
print(OpenRTM_aist.Logger.print_exception())
def read_header(self):
"""
Read TCPROS header from active socket
@raise TransportInitError if header fails to validate
"""
sock = self.socket
if sock is None:
return
sock.setblocking(1)
self._validate_header(read_ros_handshake_header(sock, self.read_buff, self.protocol.buff_size))
def read_header(self):
"""
Read TCPROS header from active socket
@raise TransportInitError if header fails to validate
"""
sock = self.socket
if sock is None:
return
sock.setblocking(1)
# TODO: add bytes received to self.stat_bytes
self._validate_header(read_ros_handshake_header(sock, self.read_buff, self.protocol.buff_size))
def connect(self, caller_id, topic, publishers):
self._rtcout.RTC_VERBOSE("connect()")
if topic != self._topic:
self._rtcout.RTC_WARN(
"Topic name is not match(%s:%s)", (topic, self._topic))
return
for uri in publishers:
if uri in self._tcp_connecters:
continue
self._rtcout.RTC_PARANOID(
"connectTCP(%s, %s, %s)", (caller_id, topic, uri))
try:
pub = xmlrpclib.ServerProxy(uri)
ret, message, result = pub.requestTopic(
self._callerid, topic, [['TCPROS']])
except BaseException:
self._rtcout.RTC_ERROR("Failed connect %s", uri)
continue
if ret == -1:
self._rtcout.RTC_WARN("requestTopic error: %s", message)
continue
elif ret == 0:
self._rtcout.RTC_WARN("requestTopic error: %s", message)
continue
else:
_, dest_addr, dest_port = result
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
sock.setsockopt(socket.SOL_SOCKET, socket.SO_KEEPALIVE, 1)
sock.setsockopt(socket.SOL_TCP, socket.TCP_KEEPCNT, 9)
sock.setsockopt(socket.SOL_TCP, socket.TCP_KEEPIDLE, 60)
sock.setsockopt(socket.SOL_TCP, socket.TCP_KEEPINTVL, 10)
sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
sock.settimeout(60.0)
sock.connect((dest_addr, dest_port))
fileno = sock.fileno()
if hasattr(select, 'poll'):
poller = select.poll()
poller.register(fileno, select.POLLOUT)
ready = False
while not ready:
events = poller.poll()
for _, flag in events:
if flag & select.POLLOUT:
ready = True
else:
ready = None
while not ready:
try:
_, ready, _ = select.select([], [fileno], [])
except ValueError:
self._rtcout.RTC_ERROR("ValueError")
return
factory = ROSMessageInfo.ROSMessageInfoList.instance()
info = factory.getInfo(self._messageType)
if(info):
info_type = info.datatype()
info_md5sum = info.md5sum()
info_message_definition = info.message_definition()
else:
self._rtcout.RTC_ERROR(
"Can not found %s", self._messageType)
sock.setblocking(1)
fields = {'topic': topic,
'message_definition': info_message_definition,
'tcp_nodelay': '0',
'md5sum': info_md5sum,
'type': info_type,
'callerid': self._callerid}
try:
write_ros_handshake_header(sock, fields)
except rosgraph.network.ROSHandshakeException:
self._rtcout.RTC_ERROR("write ROS handshake header")
continue
if sys.version_info[0] == 3:
read_buff = BytesIO()
else:
read_buff = StringIO()
sock.setblocking(1)
try:
read_ros_handshake_header(sock, read_buff, 65536)
except rosgraph.network.ROSHandshakeException:
self._rtcout.RTC_ERROR("read ROS handshake header")
continue
listener = SubListener(self, sock, uri)
self._rtcout.RTC_VERBOSE("Subscriber Listener thread start")
task = threading.Thread(target=listener.recieve, args=())
task.start()
self._tcp_connecters[uri] = {
"socket": sock,
"listener": listener,
"thread": task,
"id": self._pubnum}
self._pubnum += 1
def connect(self, client_sock, addr):
self._rtcout.RTC_PARANOID("connect()")
if addr in self._tcp_connecters:
self._rtcout.RTC_DEBUG("%s already exist", addr)
return
try:
if sys.version_info[0] == 3:
header = read_ros_handshake_header(client_sock, BytesIO(),
65536)
else:
header = read_ros_handshake_header(client_sock, StringIO(),
65536)
except rosgraph.network.ROSHandshakeException:
self._rtcout.RTC_DEBUG("read ROS handshake exception")
return
except BaseException:
self._rtcout.RTC_ERROR(OpenRTM_aist.Logger.print_exception())
topic_name = header['topic']
md5sum = header['md5sum']
type_name = header['type']
self._rtcout.RTC_VERBOSE("Topic:%s", topic_name)
self._rtcout.RTC_VERBOSE("MD5sum:%s", md5sum)
self._rtcout.RTC_VERBOSE("Type:%s", type_name)
factory = ROSMessageInfo.ROSMessageInfoList.instance()
info = factory.getInfo(self._messageType)
if info:
info_type = info.datatype()
info_md5sum = info.md5sum()
info_message_definition = info.message_definition()
else:
self._rtcout.RTC_ERROR("can not found %s", self._messageType)
return
if info_type != type_name:
self._rtcout.RTC_WARN("topic name in not match(%s:%s)",
(info_type, type_name))
return
if info_md5sum != md5sum:
self._rtcout.RTC_WARN("MD5sum in not match(%s:%s)",
(info_md5sum, md5sum))
return
fileno = client_sock.fileno()
poller = None
if hasattr(select, 'poll'):
ready = False
poller = select.poll()
poller.register(fileno, select.POLLOUT)
while not ready:
events = poller.poll()
for _, flag in events:
if flag & select.POLLOUT:
ready = True
else:
ready = None
while not ready:
try:
_, ready, _ = select.select([], [fileno], [])
except ValueError:
self._rtcout.RTC_ERROR("ValueError")
return
client_sock.setblocking(1)
fields = {
'topic': topic_name,
'message_definition': info_message_definition,
'tcp_nodelay': '0',
'md5sum': info_md5sum,
'type': info_type,
'callerid': self._callerid
}
try:
write_ros_handshake_header(client_sock, fields)
except rosgraph.network.ROSHandshakeException:
self._rtcout.RTC_ERROR("write ROS handshake exception")
return
if poller:
poller.unregister(fileno)
self._tcp_connecters[addr] = {
"socket": client_sock,
"id": self._subnum,
"node": header['callerid']
}
self._subnum += 1