Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with
or
.
Download ZIP
Tree: 8fac79f4bd
Fetching contributors…

Cannot retrieve contributors at this time

executable file 224 lines (195 sloc) 8.74 kB
#!/usr/bin/env python
# Software License Agreement (BSD License)
#
# Copyright (c) 2010, I Heart Engineering
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above
# copyright notice, this list of conditions and the following
# disclaimer in the documentation and/or other materials provided
# with the distribution.
# * Neither the name of I Heart Engineering nor the names of its
# contributors may be used to endorse or promote products derived
# from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Roughly based on the code here; http://avahi.org/wiki/PythonPublishExample
import threading
import sys
import time
import socket
import dbus
import gobject
import avahi
from dbus.mainloop.glib import DBusGMainLoop
import roslib; roslib.load_manifest('zeroconf')
import rospy
from std_msgs.msg import String
from zeroconf.msg import *
from zeroconf.srv import *
_publish_master = None
_masters = {}
class Zeroconf(threading.Thread):
def __init__(self):
# init thread
threading.Thread.__init__(self)
# Gobjects are an event based model of evil, do not trust them,
DBusGMainLoop( set_as_default=True )
self._main_loop = gobject.MainLoop()
self._bus = dbus.SystemBus()
# Initialize iterface to DBUS Server
self._server = dbus.Interface(
self._bus.get_object( avahi.DBUS_NAME, avahi.DBUS_PATH_SERVER ),
avahi.DBUS_INTERFACE_SERVER )
# Magic? Yes, don't start threads without it.
# Why? I'm sure thats documented somewhere.
gobject.threads_init()
def __del__(self):
if not self._group is None:
self._group.Free()
def run(self):
try:
self._main_loop.run()
except KeyboardInterrupt or ROSInterruptException:
pass
def stop(self):
self._main_loop.quit()
del(self)
class ZeroconfDiscover():
def __init__(self,zeroconf):
self._zeroconf = zeroconf
self._service_type = rospy.get_param('~service_type', '_ros-master._tcp')
self._master_domain = rospy.get_param('~master_domain', 'local')
self._browser = None
if self._browser is None:
self._browser = dbus.Interface(self._zeroconf._bus.get_object(
avahi.DBUS_NAME,
self._zeroconf._server.ServiceBrowserNew(
avahi.IF_UNSPEC,
avahi.PROTO_UNSPEC,
self._service_type,
self._master_domain,
dbus.UInt32(0))),
avahi.DBUS_INTERFACE_SERVICE_BROWSER)
self._browser.connect_to_signal("ItemNew", self.resolve_service_add_callback)
self._browser.connect_to_signal("ItemRemove", self.resolve_service_del_callback)
def __del__(self):
del(self._browser)
def service_add_resolved_callback(self,*args):
global _masters
_masters[args[2]] = 'http://%s:%s'%(args[5],args[8])
def print_error_callback(self,*args):
rospy.logerr(args[0])
def resolve_service_add_callback(self,interface, protocol, name, stype, domain, flags):
self._zeroconf._server.ResolveService(interface, protocol, name, stype,
domain, avahi.PROTO_UNSPEC, dbus.UInt32(0) & avahi.LOOKUP_RESULT_CACHED,
reply_handler=self.service_add_resolved_callback,
error_handler=self.print_error_callback)
def resolve_service_del_callback(self,interface, protocol, name, stype, domain, flags,*args):
global _masters
del(_masters[name])
class ZeroconfPublisher():
def __init__(self,zeroconf):
self._zeroconf = zeroconf
# Set params
self._master_name = rospy.get_param('~master_name','Unknown')
self._service_type = rospy.get_param('~service_type', '_ros-master._tcp')
self._master_host = rospy.get_param('~master_host', socket.gethostname())
self._master_domain = rospy.get_param('~master_domain', None)
self._master_port = rospy.get_param('~master_port', 11311)
# The DBUS entry group
self._group = None
# Monitor server state changes
self._zeroconf._server.connect_to_signal( "StateChanged", self.state_changed_callback )
# Build initial state information
self.state_changed_callback( self._zeroconf._server.GetState() )
def __repr__(self):
# string representation of zeroconf announcement
return "%s : %s\tMASTER_URI: http://%s%s:%d" % (
self._master_name, self._service_type,
('localhost' if self._master_host is None else self._master_host),
('' if self._master_domain is None else "." + self._master_domain),
self._master_port)
def run(self):
#do stuff
#does this actually need a thread?
a = 1
def state_changed_callback(self,state):
if state == avahi.SERVER_COLLISION:
rospy.logwarn("Server name collision")
self.remove_service()
elif state == avahi.SERVER_RUNNING:
self.add_service()
def add_service(self):
if self._group is None:
self._group = dbus.Interface(
self._zeroconf._bus.get_object(
avahi.DBUS_NAME,
self._zeroconf._server.EntryGroupNew()),
avahi.DBUS_INTERFACE_ENTRY_GROUP)
self._group.connect_to_signal('StateChanged', self.group_state_changed_callback)
rospy.loginfo("Adding service '%s' of type '%s' ..." % (
self._master_name, self._service_type))
self._group.AddService(
avahi.IF_UNSPEC,
avahi.PROTO_UNSPEC,
dbus.UInt32(0),
self._master_name, self._service_type,
('' if self._master_domain is None else self._master_domain),
self._master_host + ('.local' if self._master_domain is None else "." + self._master_domain),
dbus.UInt16(str(self._master_port)),
avahi.string_array_to_txt_array(""))
self._group.Commit()
def remove_service(self):
if not self._group is None:
self._group.Reset()
def group_state_changed_callback(self,state, error):
if state == avahi.ENTRY_GROUP_REGISTERING:
rospy.logdebug("Registering Entry Group")
if state == avahi.ENTRY_GROUP_ESTABLISHED:
rospy.logdebug("Service established.")
elif state == avahi.ENTRY_GROUP_COLLISION:
rospy.logerror("ERROR: Service name collision")
self._zeroconf._main_loop.quit()
elif state == avahi.ENTRY_GROUP_FAILURE:
rospy.logerror("Error in group state changed", error)
self._zeroconf._main_loop.quit()
return
def handle_discover_masters(req):
rospy.logdebug("/zeroconf/discover_masters called")
response = []
for master in _masters.keys():
response.append(ROSMaster(str(master), str(_masters[master])))
return DiscoverMastersResponse(response)
if __name__ == '__main__':
#rospy.init_node('zeroconf_publisher',disable_signals=True)
rospy.init_node('zeroconf')
_publish_master = rospy.get_param('publish_master',True)
zconf = Zeroconf()
if _publish_master:
zconf_pub = ZeroconfPublisher(zconf)
zconf_disc = ZeroconfDiscover(zconf)
zconf.start()
rospy.Service('~discover_masters', DiscoverMasters, handle_discover_masters)
while not rospy.is_shutdown():
rospy.sleep(0.01)
rospy.spin()
zconf.stop()
Jump to Line
Something went wrong with that request. Please try again.