def run(self):
"""
Main RegManager thread loop.
Periodically checks the update
queue and generates topic connections
"""
#Connect the topics
while not self.handler.done and not is_shutdown():
cond = self.cond
try:
cond.acquire()
if not self.updates:
cond.wait(0.5)
if self.updates:
#work from the end as these are the most up-to-date
topic, uris = self.updates.pop()
#filter out older updates for same topic
self.updates = [x for x in self.updates if x[0] != topic]
else:
topic = uris = None
finally:
if cond is not None:
cond.release()
#call _connect_topic on all URIs as it can check to see whether
#or not a connection exists.
if uris and not self.handler.done:
for uri in uris:
# #1141: have to multithread this to prevent a bad publisher from hanging us
_thread.start_new_thread(self._connect_topic_thread, (topic, uri))
def reg_added(self, resolved_name, data_type_or_uri, reg_type):
"""
RegistrationListener callback
@param resolved_name: resolved name of topic or service
@type resolved_name: str
@param data_type_or_uri: either the data type (for topic regs) or the service URI (for service regs).
@type data_type_or_uri: str
@param reg_type: Valid values are L{Registration.PUB}, L{Registration.SUB}, L{Registration.SRV}
@type reg_type: str
"""
#TODO: this needs to be made robust to master outages
master_uri = self.master_uri
if not master_uri:
self.logger.error(
"Registrar: master_uri is not set yet, cannot inform master of registration"
)
else:
master = xmlrpcapi(master_uri)
args = (get_caller_id(), resolved_name, data_type_or_uri, self.uri)
registered = False
first = True
while not registered and not is_shutdown():
try:
if reg_type == Registration.PUB:
self.logger.debug(
"master.registerPublisher(%s, %s, %s, %s)" % args)
code, msg, val = master.registerPublisher(*args)
if code != 1:
logfatal(
"unable to register publication [%s] with master: %s"
% (resolved_name, msg))
elif reg_type == Registration.SUB:
self.logger.debug(
"master.registerSubscriber(%s, %s, %s, %s)" % args)
code, msg, val = master.registerSubscriber(*args)
if code == 1:
self.publisher_update(resolved_name, val)
else:
# this is potentially worth exiting over. in the future may want to add a retry
# timer
logfatal(
"unable to register subscription [%s] with master: %s"
% (resolved_name, msg))
elif reg_type == Registration.SRV:
self.logger.debug(
"master.registerService(%s, %s, %s, %s)" % args)
code, msg, val = master.registerService(*args)
if code != 1:
logfatal(
"unable to register service [%s] with master: %s"
% (resolved_name, msg))
registered = True
except Exception as e:
if first:
msg = "Unable to register with master node [%s]: master may not be running yet. Will keep trying." % master_uri
self.logger.error(str(e) + "\n" + msg)
print(msg)
first = False
time.sleep(0.2)
def callService(self, service_uri, service, type, *args, **kwds):
'''
Calls the service and return the response.
To call the service the ServiceProxy can't be used, because it uses
environment variables to determine the URI of the running service. In our
case this service can be running using another ROS master. The changes on the
environment variables is not thread safe.
So the source code of the rospy.SerivceProxy (tcpros_service.py) was modified.
@param service_uri: the URI of the service
@type service_uri: C{str}
@param service: full service name (with name space)
@type service: C{str}
@param type: service class
@type type: ServiceDefinition: service class
@param args: arguments to remote service
@param kwds: message keyword arguments
@return: the tuple of request and response.
@rtype: C{(request object, response object)}
@raise StartException: on error
@see: L{rospy.SerivceProxy}
'''
from rospy.core import parse_rosrpc_uri, is_shutdown
from rospy.msg import args_kwds_to_message
from rospy.exceptions import TransportInitError, TransportException
from rospy.impl.tcpros_base import TCPROSTransport, TCPROSTransportProtocol, DEFAULT_BUFF_SIZE
from rospy.impl.tcpros_service import TCPROSServiceClient
from rospy.service import ServiceException
request = args_kwds_to_message(type._request_class, args, kwds)
transport = None
protocol = TCPROSServiceClient(service, type, headers={})
transport = TCPROSTransport(protocol, service)
# initialize transport
dest_addr, dest_port = parse_rosrpc_uri(service_uri)
# connect to service
transport.buff_size = DEFAULT_BUFF_SIZE
try:
transport.connect(dest_addr, dest_port, service_uri)
except TransportInitError as e:
# can be a connection or md5sum mismatch
raise StartException("unable to connect to service: %s"%e)
transport.send_message(request, 0)
try:
responses = transport.receive_once()
if len(responses) == 0:
raise StartException("service [%s] returned no response"%service)
elif len(responses) > 1:
raise StartException("service [%s] returned multiple responses: %s"%(service, len(responses)))
except TransportException as e:
# convert lower-level exception to exposed type
if is_shutdown():
raise StartException("node shutdown interrupted service call")
else:
raise StartException("transport error completing service call: %s"%(str(e)))
except ServiceException, e:
raise StartException("Service error: %s"%(str(e)))
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 reg_added(self, resolved_name, data_type_or_uri, reg_type):
"""
RegistrationListener callback
@param resolved_name: resolved name of topic or service
@type resolved_name: str
@param data_type_or_uri: either the data type (for topic regs) or the service URI (for service regs).
@type data_type_or_uri: str
@param reg_type: Valid values are L{Registration.PUB}, L{Registration.SUB}, L{Registration.SRV}
@type reg_type: str
"""
# TODO: this needs to be made robust to master outages
master_uri = self.master_uri
if not master_uri:
self.logger.error("Registrar: master_uri is not set yet, cannot inform master of registration")
else:
master = xmlrpcapi(master_uri)
args = (get_caller_id(), resolved_name, data_type_or_uri, self.uri)
registered = False
first = True
while not registered and not is_shutdown():
try:
if reg_type == Registration.PUB:
self.logger.debug("master.registerPublisher(%s, %s, %s, %s)" % args)
code, msg, val = master.registerPublisher(*args)
if code != 1:
logfatal("unable to register publication [%s] with master: %s" % (resolved_name, msg))
elif reg_type == Registration.SUB:
self.logger.debug("master.registerSubscriber(%s, %s, %s, %s)" % args)
code, msg, val = master.registerSubscriber(*args)
if code == 1:
self.publisher_update(resolved_name, val)
else:
# this is potentially worth exiting over. in the future may want to add a retry
# timer
logfatal("unable to register subscription [%s] with master: %s" % (resolved_name, msg))
elif reg_type == Registration.SRV:
self.logger.debug("master.registerService(%s, %s, %s, %s)" % args)
code, msg, val = master.registerService(*args)
if code != 1:
logfatal("unable to register service [%s] with master: %s" % (resolved_name, msg))
registered = True
except Exception as e:
if first:
msg = (
"Unable to register with master node [%s]: master may not be running yet. Will keep trying."
% master_uri
)
self.logger.error(str(e) + "\n" + msg)
print(msg)
first = False
time.sleep(0.2)
def run(self):
"""
Main RegManager thread loop.
Periodically checks the update
queue and generates topic connections
"""
#Connect the topics
while not self.handler.done and not is_shutdown():
cond = self.cond
try:
cond.acquire()
if not self.updates:
cond.wait(0.5)
if self.updates:
#work from the end as these are the most up-to-date
topic, uris = self.updates.pop()
#print "update", topic, uris
#filter out older updates for same topic
#self.updates = [x for x in self.updates if x[0] != topic]
self.updates = [
x for x in self.updates
if x[0] != topic or x[1] != uris
]
else:
topic = uris = None
finally:
if cond is not None:
cond.release()
#call _connect_topic on all URIs as it can check to see whether
#or not a connection exists.
if uris and not self.handler.done:
for uri in uris:
self.logger.debug("pub update:(%s,%s)" % (topic, uri))
# #1141: have to multithread this to prevent a bad publisher from hanging us
#print topic, uri
t = threading.Thread(target=self._connect_topic_thread,
args=(topic, uri))
t.setDaemon(True)
t.start()
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 callService(self, service_uri, service, service_type, service_args=[]):
'''
Calls the service and return the response.
To call the service the ServiceProxy can't be used, because it uses
environment variables to determine the URI of the running service. In our
case this service can be running using another ROS master. The changes on the
environment variables is not thread safe.
So the source code of the rospy.SerivceProxy (tcpros_service.py) was modified.
:param str service_uri: the URI of the service
:param str service: full service name (with name space)
:param service_type: service class
:type service_type: ServiceDefinition: service class
:param service_args: arguments
:return: the tuple of request and response.
:rtype: (request object, response object)
:raise StartException: on error
:see: rospy.SerivceProxy<http://docs.ros.org/kinetic/api/rospy/html/rospy.impl.tcpros_service.ServiceProxy-class.html>
'''
service = str(service)
rospy.loginfo("Call service %s[%s]: %s, %s", utf8(service),
utf8(service_uri), utf8(service_type),
utf8(service_args))
from rospy.core import parse_rosrpc_uri, is_shutdown
# from rospy.msg import args_kwds_to_message
from rospy.exceptions import TransportInitError, TransportException
from rospy.impl.tcpros_base import TCPROSTransport, DEFAULT_BUFF_SIZE # ,TCPROSTransportProtocol
from rospy.impl.tcpros_service import TCPROSServiceClient
from rospy.service import ServiceException
request = service_type._request_class()
import genpy
try:
now = rospy.get_rostime()
import std_msgs.msg
keys = {'now': now, 'auto': std_msgs.msg.Header(stamp=now)}
genpy.message.fill_message_args(request, service_args, keys)
except genpy.MessageException as e:
def argsummary(args):
if type(args) in [tuple, list]:
return '\n'.join([
' * %s (type %s)' % (a, type(a).__name__) for a in args
])
else:
return ' * %s (type %s)' % (args, type(args).__name__)
raise StartException(
"Incompatible arguments to call service:\n%s\nProvided arguments are:\n%s\n\nService arguments are: [%s]"
% (e, argsummary(service_args),
genpy.message.get_printable_message_args(request)))
# request = args_kwds_to_message(type._request_class, args, kwds)
protocol = TCPROSServiceClient(service, service_type, headers={})
transport = TCPROSTransport(protocol, service)
# initialize transport
dest_addr, dest_port = parse_rosrpc_uri(service_uri)
# connect to service
transport.buff_size = DEFAULT_BUFF_SIZE
try:
transport.connect(dest_addr, dest_port, service_uri, timeout=5)
except TransportInitError as e:
# can be a connection or md5sum mismatch
raise StartException(''.join(
["unable to connect to service: ",
utf8(e)]))
transport.send_message(request, 0)
try:
responses = transport.receive_once()
if len(responses) == 0:
raise StartException("service [%s] returned no response" %
service)
elif len(responses) > 1:
raise StartException(
"service [%s] returned multiple responses: %s" %
(service, len(responses)))
except TransportException as e:
# convert lower-level exception to exposed type
if is_shutdown():
raise StartException("node shutdown interrupted service call")
else:
raise StartException(
"transport error completing service call: %s" % (utf8(e)))
except ServiceException as e:
raise StartException("Service error: %s" % (utf8(e)))
finally:
transport.close()
transport = None
return request, responses[0] if len(responses) > 0 else None
def callService(self, service_uri, service, service_type, service_args=[]):
'''
Calls the service and return the response.
To call the service the ServiceProxy can't be used, because it uses
environment variables to determine the URI of the running service. In our
case this service can be running using another ROS master. The changes on the
environment variables is not thread safe.
So the source code of the rospy.SerivceProxy (tcpros_service.py) was modified.
@param service_uri: the URI of the service
@type service_uri: C{str}
@param service: full service name (with name space)
@type service: C{str}
@param service_type: service class
@type service_type: ServiceDefinition: service class
@param service_args: arguments
@return: the tuple of request and response.
@rtype: C{(request object, response object)}
@raise StartException: on error
@see: U{rospy.SerivceProxy<http://docs.ros.org/kinetic/api/rospy/html/rospy.impl.tcpros_service.ServiceProxy-class.html>}
'''
service = str(service)
rospy.loginfo("Call service %s[%s]: %s, %s", utf8(service), utf8(service_uri), utf8(service_type), utf8(service_args))
from rospy.core import parse_rosrpc_uri, is_shutdown
# from rospy.msg import args_kwds_to_message
from rospy.exceptions import TransportInitError, TransportException
from rospy.impl.tcpros_base import TCPROSTransport, DEFAULT_BUFF_SIZE # ,TCPROSTransportProtocol
from rospy.impl.tcpros_service import TCPROSServiceClient
from rospy.service import ServiceException
request = service_type._request_class()
import genpy
try:
now = rospy.get_rostime()
import std_msgs.msg
keys = {'now': now, 'auto': std_msgs.msg.Header(stamp=now)}
genpy.message.fill_message_args(request, service_args, keys)
except genpy.MessageException as e:
def argsummary(args):
if type(args) in [tuple, list]:
return '\n'.join([' * %s (type %s)' % (a, type(a).__name__) for a in args])
else:
return ' * %s (type %s)' % (args, type(args).__name__)
raise StartException("Incompatible arguments to call service:\n%s\nProvided arguments are:\n%s\n\nService arguments are: [%s]" % (e, argsummary(service_args), genpy.message.get_printable_message_args(request)))
# request = args_kwds_to_message(type._request_class, args, kwds)
protocol = TCPROSServiceClient(service, service_type, headers={})
transport = TCPROSTransport(protocol, service)
# initialize transport
dest_addr, dest_port = parse_rosrpc_uri(service_uri)
# connect to service
transport.buff_size = DEFAULT_BUFF_SIZE
try:
transport.connect(dest_addr, dest_port, service_uri, timeout=5)
except TransportInitError as e:
# can be a connection or md5sum mismatch
raise StartException(''.join(["unable to connect to service: ", utf8(e)]))
transport.send_message(request, 0)
try:
responses = transport.receive_once()
if len(responses) == 0:
raise StartException("service [%s] returned no response" % service)
elif len(responses) > 1:
raise StartException("service [%s] returned multiple responses: %s" % (service, len(responses)))
except TransportException as e:
# convert lower-level exception to exposed type
if is_shutdown():
raise StartException("node shutdown interrupted service call")
else:
raise StartException("transport error completing service call: %s" % (utf8(e)))
except ServiceException, e:
raise StartException("Service error: %s" % (utf8(e)))
def callService(self, service_uri, service, type, *args, **kwds):
'''
Calls the service and return the response.
To call the service the ServiceProxy can't be used, because it uses
environment variables to determine the URI of the running service. In our
case this service can be running using another ROS master. The changes on the
environment variables is not thread safe.
So the source code of the rospy.SerivceProxy (tcpros_service.py) was modified.
@param service_uri: the URI of the service
@type service_uri: C{str}
@param service: full service name (with name space)
@type service: C{str}
@param type: service class
@type type: ServiceDefinition: service class
@param args: arguments to remote service
@param kwds: message keyword arguments
@return: the tuple of request and response.
@rtype: C{(request object, response object)}
@raise StartException: on error
@see: L{rospy.SerivceProxy}
'''
from rospy.core import parse_rosrpc_uri, is_shutdown
from rospy.msg import args_kwds_to_message
from rospy.exceptions import TransportInitError, TransportException
from rospy.impl.tcpros_base import TCPROSTransport, TCPROSTransportProtocol, DEFAULT_BUFF_SIZE
from rospy.impl.tcpros_service import TCPROSServiceClient
from rospy.service import ServiceException
request = args_kwds_to_message(type._request_class, args, kwds)
transport = None
protocol = TCPROSServiceClient(service, type, headers={})
transport = TCPROSTransport(protocol, service)
# initialize transport
dest_addr, dest_port = parse_rosrpc_uri(service_uri)
# connect to service
transport.buff_size = DEFAULT_BUFF_SIZE
try:
transport.connect(dest_addr, dest_port, service_uri)
except TransportInitError as e:
# can be a connection or md5sum mismatch
raise StartException("unable to connect to service: %s" % e)
transport.send_message(request, 0)
try:
responses = transport.receive_once()
if len(responses) == 0:
raise StartException("service [%s] returned no response" %
service)
elif len(responses) > 1:
raise StartException(
"service [%s] returned multiple responses: %s" %
(service, len(responses)))
except TransportException as e:
# convert lower-level exception to exposed type
if is_shutdown():
raise StartException("node shutdown interrupted service call")
else:
raise StartException(
"transport error completing service call: %s" % (str(e)))
except ServiceException, e:
raise StartException("Service error: %s" % (str(e)))
def robotController():
rospy.init_node('test_mover', anonymous=True)
pubToTopic = rospy.Publisher("p3dx/cmd_vel", Twist, queue_size=10)
vel_msg = Twist()
#inputs, controller values, consts
speed = 0.5
angle = 20
#conversion to degrees from radian
angular_speed = angle * PI / 180
print("Move Joystick, to control the Robot.")
print("Conncecting to server...")
ws = create_connection("")
print("Conncection established.")
while not is_shutdown():
print("Receiving...")
while True:
msgReceived = ws.recv()
print("Received '%s'" % msgReceived)
stringToJson = json.loads(msgReceived)
direction = stringToJson['direction']
robotID = stringToJson['robotID']
#Initial value is none.
print('INITIAL')
vel_msg.angular.z = 0
vel_msg.linear.x = 0
pubToTopic.publish(vel_msg)
if (robotID == 'kakaroto'):
if ((direction == 'UP' or direction == 'up')):
vel_msg.linear.x = abs(speed)
vel_msg.angular.z = 0
print("Going forward, current speed :", speed)
elif ((direction == 'UP RIGHT' or direction == 'up right')):
vel_msg.linear.x = abs(speed)
vel_msg.angular.z = abs(angular_speed)
pubToTopic.publish(vel_msg)
print("Going UP-RIGHT, current speed :", speed)
elif ((direction == 'RIGHT' or direction == 'right')):
vel_msg.linear.x = 0
vel_msg.angular.z = abs(angular_speed)
print("Turning right")
elif ((direction == 'DOWN RIGHT'
or direction == 'down right')):
vel_msg.linear.x = -abs(speed)
vel_msg.angular.z = -abs(angular_speed)
print("Going DOWN-RIGHT, current speed :", speed)
elif ((direction == 'DOWN' or direction == 'down')):
vel_msg.linear.x = -abs(speed)
vel_msg.angular.z = 0
print("Going backwards, current speed :", speed)
elif ((direction == 'DOWN LEFT' or direction == 'down left')):
vel_msg.linear.x = -abs(speed)
vel_msg.angular.z = abs(angular_speed)
print("Going DOWN-LEFT, current speed :", speed)
elif ((direction == 'LEFT' or direction == 'left')):
vel_msg.linear.x = 0
vel_msg.angular.z = -abs(angular_speed)
print("Turning left")
elif ((direction == 'UP LEFT' or direction == 'up left')):
vel_msg.linear.x = abs(speed)
vel_msg.angular.z = -abs(angular_speed)
print("Going UP-LEFT, current speed :", speed)
elif ((direction == 'STOPPED' or direction == 'stopped')):
vel_msg.linear.x = 0
vel_msg.angular.z = 0
print("Stopped")
#Publishing velocities (vel_msg) to Topic
pubToTopic.publish(vel_msg)
#Stop robot if we receive no direction or another user wants to take control.
else:
vel_msg.angular.z = 0
vel_msg.linear.x = 0
print("robotID not corresponding, stopping robot.")
#To stop the robot when CTRL+C is perfomrmed
print("CTRL+C to go back to terminal.")
rospy.spin()
ws.close()
