def connect(self, client_sock, header):
self._rtcout.RTC_PARANOID("connect()")
topic_name = header['topic']
md5sum = header['md5sum']
type_name = header['type']
caller_id = header['callerid']
if self._topic != topic_name:
self._rtcout.RTC_INFO("Topic name is not match(%s:%s)",
(topic_name, self._topic))
return
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("type 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
fields = {
'topic': topic_name,
'message_definition': info_message_definition,
'tcp_nodelay': '0',
'md5sum': info_md5sum,
'type': info_type,
'callerid': self._callerid
}
try:
stat_bytes = write_ros_handshake_header(client_sock, fields)
except rosgraph.network.ROSHandshakeException:
self._rtcout.RTC_ERROR("write ROS handshake exception")
return
self._topicmgr = ROSTopicManager.instance()
sub = self._topicmgr.getSubscriberLink(client_sock)
if sub is not None:
guard_con = OpenRTM_aist.Guard.ScopedLock(self._con_mutex)
sub.setTopic(topic_name)
sub.setCallerID(caller_id)
sub.setStatBytes(stat_bytes)
self._tcp_connecters.append(sub)
def init(self, prop):
self._rtcout.RTC_PARANOID("init()")
if not prop.propertyNames():
self._rtcout.RTC_DEBUG("Property is empty.")
return
self._topicmgr = ROSTopicManager.instance()
if self._topicmgr.existSubscriber(self):
self._rtcout.RTC_VERBOSE("Subscriber already exists.")
return
self._messageType = prop.getProperty(
"marshaling_type", "ros:std_msgs/Float32")
self._topic = prop.getProperty("ros.topic", "chatter")
self._topic = "/" + self._topic
self._roscorehost = prop.getProperty("ros.roscore.host", "localhost")
self._roscoreport = prop.getProperty("ros.roscore.port", "11311")
self._rtcout.RTC_VERBOSE("topic name: %s", self._topic)
self._rtcout.RTC_VERBOSE(
"roscore address: %s:%s",
(self._roscorehost,
self._roscoreport))
self._callerid = prop.getProperty("ros.node.name")
if not self._callerid:
self._callerid = str(OpenRTM_aist.uuid1())
self._callerid = "/" + self._callerid
factory = ROSMessageInfo.ROSMessageInfoList.instance()
info = factory.getInfo(self._messageType)
if info:
info_type = info.datatype()
else:
self._rtcout.RTC_ERROR("can not found %s", self._messageType)
return
self._rtcout.RTC_VERBOSE("caller id: %s", self._callerid)
self._topicmgr.addSubscriber(self)
self._client = xmlrpclib.ServerProxy(
'http://' + self._roscorehost + ":" + self._roscoreport)
try:
_, __, val = self._client.registerSubscriber(
self._callerid, self._topic, info_type, self._topicmgr.getURI())
except xmlrpclib.Fault as err:
self._rtcout.RTC_ERROR("XML-RPC ERROR: %s", err.faultString)
return
self.connect(self._callerid, self._topic, val)
def init(self, prop):
self._rtcout.RTC_PARANOID("init()")
if not prop.propertyNames():
self._rtcout.RTC_DEBUG("Property is empty.")
return
self._topicmgr = ROSTopicManager.instance()
if self._topicmgr.existPublisher(self):
self._rtcout.RTC_VERBOSE("Publisher already exists.")
return
self._properties = prop
self._messageType = prop.getProperty("marshaling_type",
"ros:std_msgs/Float32")
self._topic = prop.getProperty("ros.topic", "chatter")
self._topic = "/" + self._topic
self._roscorehost = prop.getProperty("ros.roscore.host", "localhost")
self._roscoreport = prop.getProperty("ros.roscore.port", "11311")
self._rtcout.RTC_VERBOSE("topic name: %s", self._topic)
self._rtcout.RTC_VERBOSE("roscore address: %s:%s",
(self._roscorehost, self._roscoreport))
self._callerid = "/" + prop.getProperty("ros.node.name", "rtcomp")
if OpenRTM_aist.toBool(prop.getProperty("ros.node.anonymous"), "YES",
"NO", False):
self._callerid = self._callerid + "_" + \
str(os.getpid()) + "_" + str(int(time.time()*1000))
self._rtcout.RTC_VERBOSE("caller id: %s", self._callerid)
self._topicmgr.addPublisher(self)
self._client = xmlrpclib.ServerProxy('http://' + self._roscorehost +
":" + self._roscoreport)
messageType = "std_msgs/Float32"
messageTypeList = self._messageType.split(":")
if len(messageType) >= 2:
messageType = messageTypeList[1]
try:
self._client.registerPublisher(self._callerid,
self._topic, messageType,
self._topicmgr.getURI())
except xmlrpclib.Fault as err:
self._rtcout.RTC_ERROR("XML-RPC ERROR: %s", err.faultString)
def init(self, prop):
self._rtcout.RTC_PARANOID("init()")
if not prop.propertyNames():
self._rtcout.RTC_DEBUG("Property is empty.")
return
self._topicmgr = ROSTopicManager.instance()
if self._topicmgr.existPublisher(self):
self._rtcout.RTC_VERBOSE("Publisher already exists.")
return
self._properties = prop
self._messageType = prop.getProperty("marshaling_type",
"ros:std_msgs/Float32")
self._topic = prop.getProperty("ros.topic", "chatter")
self._topic = "/" + self._topic
self._roscorehost = prop.getProperty("ros.roscore.host", "localhost")
self._roscoreport = prop.getProperty("ros.roscore.port", "11311")
self._rtcout.RTC_VERBOSE("topic name: %s", self._topic)
self._rtcout.RTC_VERBOSE("roscore address: %s:%s",
(self._roscorehost, self._roscoreport))
self._callerid = prop.getProperty("ros.node.name")
if not self._callerid:
self._callerid = str(OpenRTM_aist.uuid1())
self._callerid = "/" + self._callerid
self._rtcout.RTC_VERBOSE("caller id: %s", self._callerid)
self._topicmgr.addPublisher(self)
self._client = xmlrpclib.ServerProxy('http://' + self._roscorehost +
":" + self._roscoreport)
try:
self._client.registerPublisher(self._callerid, self._topic,
'std_msgs/Float32',
self._topicmgr.getURI())
except xmlrpclib.Fault as err:
self._rtcout.RTC_ERROR("XML-RPC ERROR: %s", err.faultString)
def postShutdown(self): ROSTopicManager.shutdown_global()
def connect(self, caller_id, topic, publishers):
self._rtcout.RTC_VERBOSE("connect()")
if topic != self._topic:
self._rtcout.RTC_INFO("Topic name is not match(%s:%s)",
(topic, self._topic))
return
for uri in publishers:
pub = PublisherLink(caller_id=caller_id,
topic=topic,
xmlrpc_uri=uri)
guard_con = OpenRTM_aist.Guard.ScopedLock(self._con_mutex)
if pub in self._tcp_connecters:
continue
del guard_con
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
self._topicmgr = ROSTopicManager.instance()
listener = SubListener(self, sock)
task = threading.Thread(target=listener.recieve, args=())
self._topicmgr.addPublisherLink(sock, caller_id, topic, uri,
listener, task)
pub = self._topicmgr.getPublisherLink(sock)
if pub is not None:
guard_con = OpenRTM_aist.Guard.ScopedLock(self._con_mutex)
self._tcp_connecters.append(pub)
del guard_con
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
self._rtcout.RTC_VERBOSE("Subscriber Listener thread start")
task.start()
def init(self, prop):
self._rtcout.RTC_PARANOID("init()")
if not prop.propertyNames():
self._rtcout.RTC_DEBUG("Property is empty.")
return
self._topicmgr = ROSTopicManager.instance()
if self._topicmgr.existSubscriber(self):
self._rtcout.RTC_VERBOSE("Subscriber already exists.")
return
self._messageType = prop.getProperty("marshaling_type",
"ros:std_msgs/Float32")
self._topic = prop.getProperty("ros.topic", "chatter")
self._topic = "/" + self._topic
self._roscorehost = prop.getProperty("ros.roscore.host")
self._roscoreport = prop.getProperty("ros.roscore.port")
if not self._roscorehost and not self._roscoreport:
self._rtcout.RTC_VERBOSE(
"Get the IP address and port number of ros master from environment variable %s.",
ROSInPort.ROS_MASTER_URI)
env = os.getenv(ROSInPort.ROS_MASTER_URI)
if env:
self._rtcout.RTC_VERBOSE("$%s: %s",
(ROSInPort.ROS_MASTER_URI, env))
env = env.replace("http://", "")
env = env.replace("https://", "")
envsplit = env.split(":")
self._roscorehost = envsplit[0]
if len(envsplit) >= 2:
self._roscoreport = envsplit[1]
if not self._roscorehost:
self._roscorehost = ROSInPort.ROS_DEFAULT_MASTER_ADDRESS
if not self._roscoreport:
self._roscoreport = ROSInPort.ROS_DEFAULT_MASTER_PORT
self._rtcout.RTC_VERBOSE("topic name: %s", self._topic)
self._rtcout.RTC_VERBOSE("roscore address: %s:%s",
(self._roscorehost, self._roscoreport))
self._callerid = "/" + prop.getProperty("ros.node.name", "rtcomp")
if OpenRTM_aist.toBool(prop.getProperty("ros.node.anonymous"), "YES",
"NO", False):
self._callerid = self._callerid + "_" + \
str(os.getpid()) + "_" + str(int(time.time()*1000))
factory = ROSMessageInfo.ROSMessageInfoList.instance()
info = factory.getInfo(self._messageType)
if info:
info_type = info.datatype()
else:
self._rtcout.RTC_ERROR("can not found %s", self._messageType)
return
self._rtcout.RTC_VERBOSE("caller id: %s", self._callerid)
self._topicmgr.addSubscriber(self)
self._client = xmlrpclib.ServerProxy('http://' + self._roscorehost +
":" + self._roscoreport)
try:
_, __, val = self._client.registerSubscriber(
self._callerid, self._topic, info_type,
self._topicmgr.getURI())
except xmlrpclib.Fault as err:
self._rtcout.RTC_ERROR("XML-RPC ERROR: %s", err.faultString)
return
self.connect(self._callerid, self._topic, val)