def __init__(self, osc_name, osc_port, regtype='_osc._udp', **kwargs):
"""
Initialize OscInterface.
@type osc_name: C{str}
@param osc_name: Name of the Bonjour service to register.
@type osc_port: C{int}
@param osc_port: Port that the OSC server will listen on.
@type regtype: C{str}
@param regtype: An appropriate registration type. Currently only
'_osc._udp' is supported.
"""
rospy.init_node("osc_interface", **kwargs)
rospy.core.add_shutdown_hook(self._shutdown_by_ros)
reactor.addSystemEventTrigger('after', 'shutdown',
self._shutdown_by_reactor)
self.ros_name = rospy.get_name()
self.osc_name = rospy.get_param("~osc_name", osc_name)
self.osc_port = rospy.get_param("~port", osc_port)
self.osc_regtype = rospy.get_param("~regtype", regtype)
self.print_fallback = rospy.get_param("~print_fallback", True)
if self.print_fallback:
rospy.loginfo("Logging all unhandled messages to rospy.loginfo")
# Bonjour Server
self._bonjour_server = Bonjour(self.osc_name, self.osc_port,
self.osc_regtype,
debug=rospy.logdebug,
info=rospy.logdebug,
error=rospy.logdebug)
self._bonjour_server.setClientCallback(self.bonjour_client_callback)
reactor.callInThread(self._bonjour_server.run, daemon=True)
self._clients = {}
self._clients_lock = threading.Lock()
# Twisted OSC receiver
self._osc_receiver = RosOscReceiver()
listener = async.DatagramServerProtocol(self._osc_receiver)
self._osc_receiver_port = reactor.listenUDP(self.osc_port,
listener)
# Twisted OSC Sender
self._osc_sender = async.DatagramClientProtocol()
self._osc_sender_port = reactor.listenUDP(0, self._osc_sender)
# Add OSC callbacks
self._osc_receiver.fallback = self.fallback
def test_bonjour(self):
try:
b = Bonjour("Bonjour listener", 1234, '_touchosceditor._tcp')
b.setClientCallback(self.clientCallback)
b.run_browser()
while self.clients == {}:
time.sleep(1.0)
self.assertEqual(self.clients.keys()[0], self.name.encode('utf-8'),
"Bonjour: Registration Failed")
vals = self.clients.values()[0]
self.assertEqual(vals['port'], self.port,
"Bonjour: Registered Port Mismatch")
finally:
b.stop_browser()
def test_bonjour(self):
try:
b = Bonjour("Bonjour listener", 1234, '_touchosceditor._tcp')
b.setClientCallback(self.clientCallback)
b.run_browser()
while self.clients == {}:
time.sleep(1.0)
self.assertEqual(self.clients.keys()[0], self.name.encode('utf-8'), "Bonjour: Registration Failed")
vals = self.clients.values()[0]
self.assertEqual(vals['port'], self.port, "Bonjour: Registered Port Mismatch")
finally:
b.stop_browser()
class OscInterface(object):
"""
Base OSC ROS Node
This class handles the most basic interaction between ROS and the OSC
interface.
@ivar _bonjour_server: Bonjour registration and browse server
@ivar _osc_sender: OSC Protocol send interface
@ivar _osc_receiver: OSC Protocol receiver interface
"""
def __init__(self, osc_name, osc_port, regtype='_osc._udp', **kwargs):
"""
Initialize OscInterface.
@type osc_name: C{str}
@param osc_name: Name of the Bonjour service to register.
@type osc_port: C{int}
@param osc_port: Port that the OSC server will listen on.
@type regtype: C{str}
@param regtype: An appropriate registration type. Currently only
'_osc._udp' is supported.
"""
rospy.init_node("osc_interface", **kwargs)
rospy.core.add_shutdown_hook(self._shutdown_by_ros)
reactor.addSystemEventTrigger('after', 'shutdown',
self._shutdown_by_reactor)
self.ros_name = rospy.get_name()
self.osc_name = rospy.get_param("~osc_name", osc_name)
self.osc_port = rospy.get_param("~port", osc_port)
self.osc_regtype = rospy.get_param("~regtype", regtype)
self.print_fallback = rospy.get_param("~print_fallback", True)
if self.print_fallback:
rospy.loginfo("Logging all unhandled messages to rospy.loginfo")
# Bonjour Server
self._bonjour_server = Bonjour(self.osc_name, self.osc_port,
self.osc_regtype,
debug=rospy.logdebug,
info=rospy.logdebug,
error=rospy.logdebug)
self._bonjour_server.setClientCallback(self.bonjour_client_callback)
reactor.callInThread(self._bonjour_server.run, daemon=True)
self._clients = {}
self._clients_lock = threading.Lock()
# Twisted OSC receiver
self._osc_receiver = RosOscReceiver()
listener = async.DatagramServerProtocol(self._osc_receiver)
self._osc_receiver_port = reactor.listenUDP(self.osc_port,
listener)
# Twisted OSC Sender
self._osc_sender = async.DatagramClientProtocol()
self._osc_sender_port = reactor.listenUDP(0, self._osc_sender)
# Add OSC callbacks
self._osc_receiver.fallback = self.fallback
@property
def clients(self):
"""
Clients detected via the Bonjour browse service
"""
return copy.copy(self._clients)
def bonjour_client_callback(self, client_list):
"""
Callback when Bonjour client list is updated.
@type client_list: C{dict}
@param client_list: A dictionary of clients
"""
if type(client_list) is not dict:
raise ValueError("Bonjour Client Callback requires dict type")
else:
with self._clients_lock:
new = set()
for service_name, service_dict in client_list.iteritems():
new.add(service_name)
try:
self._clients[service_dict["ip"]] = OscClient(
service_name,
service_dict["hostname"],
service_dict["ip"],
service_dict["port"])
except KeyError:
exc_type, exc_value, exc_traceback = sys.exc_info()
traceback.print_tb(exc_traceback, limit=1,
file=sys.stdout)
traceback.print_exception(exc_type, exc_value,
exc_traceback, limit=5,
file=sys.stdout)
old = set(self._clients.keys())
for removed in (old - new):
del self._clients[removed]
def fallback(self, address_list, value_list, client_address):
"""
Fallback handler for otherwise unhandled messages.
@type address_list: C{list}
@type value_list: C{list}
@type client_address: C{list}
"""
if self.print_fallback:
rospy.loginfo(address_list)
rospy.loginfo(value_list)
rospy.loginfo(client_address)
def _shutdown_by_reactor(self):
"""
Reactor shutdown callback. Sends a signal to rospy to shutdown the ROS
interfaces
"""
rospy.signal_shutdown("Reactor shutting down.")
def _shutdown_by_ros(self, *args):
"""
ROS shutdown callback. Sends a signal to reactor to shutdown.
"""
reactor.fireSystemEvent('shutdown')
class OscInterface(object):
"""
Base OSC ROS Node
This class handles the most basic interaction between ROS and the OSC
interface.
@ivar _bonjour_server: Bonjour registration and browse server
@ivar _osc_sender: OSC Protocol send interface
@ivar _osc_receiver: OSC Protocol receiver interface
"""
def __init__(self, osc_name, osc_port, regtype='_osc._udp', **kwargs):
"""
Initialize OscInterface.
@type osc_name: C{str}
@param osc_name: Name of the Bonjour service to register.
@type osc_port: C{int}
@param osc_port: Port that the OSC server will listen on.
@type regtype: C{str}
@param regtype: An appropriate registration type. Currently only
'_osc._udp' is supported.
"""
rospy.init_node("osc2twist_interface", **kwargs)
rospy.core.add_shutdown_hook(self._shutdown_by_ros)
reactor.addSystemEventTrigger('after', 'shutdown',
self._shutdown_by_reactor)
#self.Imu_pub = rospy.Publisher('andosc_teleop/imu', Imu)
self.Twist_pub = rospy.Publisher('/cmd_vel', Twist)
self.ros_name = rospy.get_name()
self.osc_name = rospy.get_param("~osc_name", osc_name)
self.osc_port = rospy.get_param("~port", osc_port)
self.osc_regtype = rospy.get_param("~regtype", regtype)
self.print_fallback = rospy.get_param("~print_fallback", True)
self.ori_msg_name = rospy.get_param("~ori_msg_name", "ori")
self.mrmr_msg_name = rospy.get_param("~mrmr_msg_name", "mrmr accelerometer")
self.max_speed = rospy.get_param("~max_speed", 0.2)
self.max_turn_rate = rospy.get_param("~max_turn_rate", 0.2)
self.dead_band = rospy.get_param("~dead_band", 10)
self.max_angle = rospy.get_param("~max_angle", 30)
if self.print_fallback:
rospy.loginfo("Logging all unhandled messages to rospy.loginfo")
# Bonjour Server
self._bonjour_server = Bonjour(self.osc_name, self.osc_port,
self.osc_regtype,
debug=rospy.logdebug,
info=rospy.logdebug,
error=rospy.logdebug)
self._bonjour_server.setClientCallback(self.bonjour_client_callback)
reactor.callInThread(self._bonjour_server.run, daemon=True)
self._clients = {}
self._clients_lock = threading.Lock()
# Twisted OSC receiver
self._osc_receiver = RosOscReceiver()
listener = async.DatagramServerProtocol(self._osc_receiver)
self._osc_receiver_port = reactor.listenUDP(self.osc_port,
listener)
# Twisted OSC Sender
self._osc_sender = async.DatagramClientProtocol()
self._osc_sender_port = reactor.listenUDP(0, self._osc_sender)
# Add OSC callbacks
self._osc_receiver.fallback = self.fallback
@property
def clients(self):
"""
Clients detected via the Bonjour browse service
"""
return copy.copy(self._clients)
def bonjour_client_callback(self, client_list):
"""
Callback when Bonjour client list is updated.
@type client_list: C{dict}
@param client_list: A dictionary of clients
"""
if type(client_list) is not dict:
raise ValueError("Bonjour Client Callback requires dict type")
else:
with self._clients_lock:
new = set()
for service_name, service_dict in client_list.iteritems():
new.add(service_name)
try:
self._clients[service_dict["ip"]] = OscClient(
service_name,
service_dict["hostname"],
service_dict["ip"],
service_dict["port"])
except KeyError:
exc_type, exc_value, exc_traceback = sys.exc_info()
traceback.print_tb(exc_traceback, limit=1,
file=sys.stdout)
traceback.print_exception(exc_type, exc_value,
exc_traceback, limit=5,
file=sys.stdout)
old = set(self._clients.keys())
for removed in (old - new):
del self._clients[removed]
def fallback(self, address_list, value_list, client_address):
"""
Fallback handler for otherwise unhandled messages.
@type address_list: C{list}
@type value_list: C{list}
@type client_address: C{list}
"""
"""
address_list: ['mrmr accelerometer 2 iPad3-ATT']
value_list: [0.492000013589859, 0.4970000088214874, 0.007000000216066837]
client_address: ('192.168.11.60', 61730)
['mrmr pushbutton 3 iPad3-ATT']
[1.0]
('192.168.11.60', 60659)
['mrmr pushbutton 3 iPad3-ATT']
[0.0]
"""
if self.print_fallback:
rospy.loginfo(address_list)
rospy.loginfo(value_list)
rospy.loginfo(client_address)
a=address_list[0]
twist_data = Twist()
if a.find(self.mrmr_msg_name) >=0:
y = (value_list[0]-0.5)*(self.max_angle+self.dead_band)*4
x = (value_list[1]-0.5)*(self.max_angle+self.dead_band)*4
twist_data.linear.x = (self.max_speed/self.max_angle) *self.deadband_limits(self.max_angle ,-self.max_angle ,x,self.dead_band)
twist_data.angular.z = (self.max_turn_rate/self.max_angle)*self.deadband_limits(self.max_angle ,-self.max_angle ,y,self.dead_band)
self.Twist_pub.publish(twist_data)
if a.find(self.ori_msg_name) >=0:
#twist_data.header.stamp = rospy.Time.now()
twist_data.linear.x = (self.max_speed/self.max_angle) *self.deadband_limits(self.max_angle ,-self.max_angle ,value_list[1],self.dead_band)
twist_data.angular.z = (self.max_turn_rate/self.max_angle)*self.deadband_limits(self.max_angle ,-self.max_angle ,value_list[2],self.dead_band)
self.Twist_pub.publish(twist_data)
def _shutdown_by_reactor(self):
"""
Reactor shutdown callback. Sends a signal to rospy to shutdown the ROS
interfaces
"""
rospy.signal_shutdown("Reactor shutting down.")
def _shutdown_by_ros(self, *args):
"""
ROS shutdown callback. Sends a signal to reactor to shutdown.
"""
reactor.fireSystemEvent('shutdown')
def limits(self,Hi,Lo,D_in):
if (D_in > Hi ):
return Hi
elif ( Lo > D_in ):
return Lo
else:
return D_in
def deadband_limits(self,Hi,Lo,D_in,dead_band):
if (D_in > dead_band):
D_in2=D_in-dead_band
elif (D_in < -dead_band):
D_in2=D_in+dead_band
else:
D_in2=0
if (D_in2 > Hi ):
return Hi
elif ( Lo > D_in2 ):
return Lo
else:
return D_in2
