def get_topic_type_info(topic_name):
"""
subroutine for getting topic type information
(nearly identical to rostopic._get_topic_type, except it returns rest of name instead of fn)
:param topic_name: topic name of interest
:type topic_name: str
:returns: topic type, real topic name, and rest of name referenced
if the topic points to a field within a topic, e.g. /rosout/msg, ``str, str, str``
"""
try:
node_name = rospy.get_name()
master = rosgraph.Master(node_name)
val = master.getTopicTypes()
except:
raise ROSException("unable to get list of topics from master")
matches = [(t, t_type) for t, t_type in val
if t == topic_name or topic_name.startswith(t + '/')]
if matches:
t, t_type = matches[0]
if t_type == roslib.names.ANYTYPE:
return None, None, None
if t_type == topic_name:
return t_type, None
return t_type, t, topic_name[len(t):]
else:
return None, None, None
def _move(self, target, low_force_mode=False, wait=True):
self._set_digital_out(17, 1 if low_force_mode else 0)
self._set_digital_out(16, target)
self._ready = False
if wait and not self._wait_for(
lambda: self._ready and self._state == target):
raise ROSException('Cannot move the gripper')
def disable(self):
"""Disable gripper.
Set the tool voltage to zero.
Raises:
ROSException: timeout exception
"""
self._set_tool_voltage(0)
if not self._wait_for(lambda: self._tool_voltage == 0):
raise ROSException('Cannot disable the gripper')
def innermethod(req):
rospy.loginfo(">>>> Got request for a %s. Trying to parse the parameters %s as JSON content..."%(m[0], str(req)))
params = [encode8bits(json.loads(p)) for p in req.params]
rospy.loginfo(">>>> ok! querying oro-server for %s with parameters %s..."%(m[0], str(params)))
try:
handler = getattr(oro, m[0])
except AttributeError as ae:
raise ROSException("Ooops...the RPC method " + m[0] + " is not provided by ORO.")
return OroServerQueryResponse(json.dumps(handler(*params)))
def enable(self):
"""Enable gripper.
Set the UR tool voltage to 24V and wait until the gripper becomes ready.
Raises:
ROSException: timeout exception
"""
self._set_tool_voltage(24)
if not self._wait_for(
lambda: self._tool_voltage == 24 and self._ready):
raise ROSException('Cannot enable the gripper')
def get_param_client_widget(self):
"""
@rtype: ParamClientWidget (QWidget)
@return: None if param_client is not yet generated.
@raise ROSException:
"""
if not self._param_client_widget:
logging.debug('get param_client={}'.format(
self._param_client
))
logging.debug('In get_param_client_widget 1')
if not self._param_client:
self.connect_param_server()
logging.debug('In get_param_client_widget 2')
timeout = 3 * 100
loop = 0
# Loop until _param_client is set. self._param_client gets
# set from different thread (in ParamserverConnectThread).
while self._param_client is None:
# Avoid deadlock
if timeout < loop:
# Make itself unclickable
self.setEnabled(False)
raise ROSException('param client failed')
time.sleep(0.01)
loop += 1
logging.debug('In get_param_client_widget loop#{}'.format(loop))
logging.debug('In get_param_client_widget 4')
self._param_client_widget = ParamClientWidget(
self._param_client, self._raw_param_name
)
# Creating the ParamClientWidget transfers ownership of the
# _param_client to it. If it is destroyed from Qt, we need to
# clear our reference to it and stop the param server thread we
# had.
self._param_client_widget.destroyed.connect(
self.clear_param_client_widget)
self._param_client_widget.destroyed.connect(
self.disconnect_param_server)
logging.debug('In get_param_client_widget 5')
else:
pass
return self._param_client_widget
def get_dynreconf_widget(self):
"""
@rtype: DynreconfClientWidget (QWidget)
@return: None if dynreconf_client is not yet generated.
@raise ROSException:
"""
if not self._dynreconfclient_widget:
rospy.logdebug('get dynreconf_client={}'.format(
self._dynreconf_client))
rospy.logdebug('In get_dynreconf_widget 1')
if not self._dynreconf_client:
self.connect_param_server()
rospy.logdebug('In get_dynreconf_widget 2')
timeout = 3 * 100
loop = 0
# Loop until _dynreconf_client is set. self._dynreconf_client gets
# set from different thread (in ParamserverConnectThread).
while self._dynreconf_client == None:
#Avoid deadlock
if timeout < loop:
#Make itself unclickable
self.setEnabled(False)
raise ROSException('dynreconf client failed')
time.sleep(0.01)
loop += 1
rospy.logdebug('In get_dynreconf_widget loop#{}'.format(loop))
rospy.logdebug('In get_dynreconf_widget 4')
self._dynreconfclient_widget = DynreconfClientWidget(
self._dynreconf_client, self._param_name_raw)
# Creating the DynreconfClientWidget transfers ownership of the _dynreconf_client
# to it. If it is destroyed from Qt, we need to clear our reference to it and
# stop the param server thread we had.
self._dynreconfclient_widget.destroyed.connect(
self.clear_dynreconfclient_widget)
self._dynreconfclient_widget.destroyed.connect(
self.disconnect_param_server)
rospy.logdebug('In get_dynreconf_widget 5')
else:
pass
return self._dynreconfclient_widget
def scoped_name(caller_id, name):
"""
Convert the global caller_id to a relative name within the namespace. For example, for
namespace '/foo' and name '/foo/bar/name', the return value will
be 'bar/name'
WARNING: scoped_name does not validate that name is actually within
the supplied namespace.
@param caller_id: caller ID, in canonical form
@type caller_id: str
@param name: name to scope
@type name: str
@return: name scoped to the caller_id's namespace.
@rtype: str
"""
if not is_global(caller_id):
raise ROSException("caller_id must be global")
return canonicalize_name(name)[len(namespace(caller_id)):]
def get_dynreconf_widget(self):
"""
@rtype: DynreconfClientWidget (QWidget)
@return: None if dynreconf_client is not yet generated.
@raise ROSException:
"""
if not self._dynreconfclient_widget:
rospy.logdebug('get dynreconf_client={}'.format(
self._dynreconf_client))
rospy.logdebug('In get_dynreconf_widget 1')
if not self._dynreconf_client:
self.connect_param_server()
rospy.logdebug('In get_dynreconf_widget 2')
timeout = 3 * 100
loop = 0
# Loop until _dynreconf_client is set. self._dynreconf_client gets
# set from different thread (in ParamserverConnectThread).
while self._dynreconf_client == None:
#Avoid deadlock
if timeout < loop:
#Make itself unclickable
self.setEnabled(False)
raise ROSException('dynreconf client failed')
time.sleep(0.01)
loop += 1
rospy.logdebug('In get_dynreconf_widget loop#{}'.format(loop))
rospy.logdebug('In get_dynreconf_widget 4')
self._dynreconfclient_widget = DynreconfClientWidget(
self._dynreconf_client,
self._param_name_raw)
rospy.logdebug('In get_dynreconf_widget 5')
else:
pass
return self._dynreconfclient_widget
def wait_for_service(service, timeout=None):
"""
Blocks until service is available. Use this in
initialization code if your program depends on a
service already running.
@param service: name of service
@type service: str
@param timeout: timeout time in seconds, None for no
timeout. NOTE: timeout=0 is invalid as wait_for_service actually
contacts the service, so non-blocking behavior is not
possible. For timeout=0 uses cases, just call the service without
waiting.
@type timeout: double
@raise ROSException: if specified timeout is exceeded
@raise ROSInterruptException: if shutdown interrupts wait
"""
master = rosgraph.Master(rospy.names.get_caller_id())
def contact_service(resolved_name, timeout=10.0):
try:
uri = master.lookupService(resolved_name)
except rosgraph.MasterException:
return False
addr = rospy.core.parse_rosrpc_uri(uri)
if rosgraph.network.use_ipv6():
s = socket.socket(socket.AF_INET6, socket.SOCK_STREAM)
else:
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
try:
# we always want to timeout just in case we're connecting
# to a down service.
s.settimeout(timeout)
logdebug('connecting to ' + str(addr))
s.connect(addr)
h = {
'probe': '1',
'md5sum': '*',
'callerid': rospy.core.get_caller_id(),
'service': resolved_name
}
rosgraph.network.write_ros_handshake_header(s, h)
return True
finally:
if s is not None:
s.close()
if timeout == 0.:
raise ValueError("timeout must be non-zero")
resolved_name = rospy.names.resolve_name(service)
first = False
if timeout:
timeout_t = time.time() + timeout
while not rospy.core.is_shutdown() and time.time() < timeout_t:
try:
if contact_service(resolved_name, timeout_t - time.time()):
return
time.sleep(0.3)
except KeyboardInterrupt:
# re-raise
rospy.core.logdebug(
"wait_for_service: received keyboard interrupt, assuming signals disabled and re-raising"
)
raise
except: # service not actually up
if first:
first = False
rospy.core.logerr(
"wait_for_service(%s): failed to contact [%s], will keep trying"
% (resolved_name, uri))
if rospy.core.is_shutdown():
raise ROSInterruptException("rospy shutdown")
else:
raise ROSException(
"timeout exceeded while waiting for service %s" %
resolved_name)
else:
while not rospy.core.is_shutdown():
try:
if contact_service(resolved_name):
return
time.sleep(0.3)
except KeyboardInterrupt:
# re-raise
rospy.core.logdebug(
"wait_for_service: received keyboard interrupt, assuming signals disabled and re-raising"
)
raise
except: # service not actually up
if first:
first = False
rospy.core.logerr(
"wait_for_service(%s): failed to contact [%s], will keep trying"
% (resolved_name, uri))
if rospy.core.is_shutdown():
raise ROSInterruptException("rospy shutdown")
def refresh_topics(self):
"""
refresh tree view items
@raise ROSException
"""
topic_list = self._selected_topics
if topic_list == None:
topic_list = rospy.get_published_topics()
if topic_list == None:
raise ROSException(
"Not even a single topic found published. " +
"Check network configuration")
else: # Topics to show are specified.
topic_specifiers_server_all = None
topic_specifiers_required = None
rospy.logdebug(
"refresh_topics) self._selected_topics={}".format(topic_list))
if self._select_topic_type == self.SELECT_BY_NAME:
topic_specifiers_server_all = [
name for name, type in rospy.get_published_topics()
]
topic_specifiers_required = [name for name, type in topic_list]
elif self._select_topic_type == self.SELECT_BY_MSGTYPE:
# The topics that are required (by whoever uses this class).
topic_specifiers_required = [type for name, type in topic_list]
# The required topics that match with published topics.
topics_match = [(name, type)
for name, type in rospy.get_published_topics()
if type in topic_specifiers_required]
topic_list = topics_match
rospy.logdebug(
'selected & published topic types={}'.format(topic_list))
rospy.logdebug('server_all={}\nrequired={}\ntlist={}'.format(
topic_specifiers_server_all, topic_specifiers_required,
topic_list))
# topics_not_published = [n for n in topic_specifiers_required
# if not n in topic_specifiers_server_all]
if len(topic_list) == 0:
raise ROSException("None of the following required topics " +
"are found. \r (NAME, TYPE): {}".format(
self._selected_topics))
if self._current_topic_list != topic_list:
self._current_topic_list = topic_list
# start new topic dict
new_topics = {}
for topic_name, topic_type in topic_list:
# if topic is new or has changed its type
if topic_name not in self._topics or \
self._topics[topic_name]['type'] != topic_type:
# create new TopicInfo
topic_info = TopicInfo(topic_name)
# if successful, add it to the dict and tree view
if topic_info._topic_name:
topic_item = self._recursive_create_widget_items(
self.topics_tree_widget, topic_name, topic_type,
topic_info.message_class())
new_topics[topic_name] = {
'item': topic_item,
'info': topic_info,
'type': topic_type,
}
else:
# if topic has been seen before, copy it to new dict and
# remove it from the old one
new_topics[topic_name] = self._topics[topic_name]
del self._topics[topic_name]
# clean up old topics
for topic_name in self._topics.keys():
self._topics[topic_name]['info'].stop_monitoring()
index = self.topics_tree_widget.indexOfTopLevelItem(
self._topics[topic_name]['item'])
self.topics_tree_widget.takeTopLevelItem(index)
del self._topics[topic_name]
# switch to new topic dict
self._topics = new_topics
self._update_topics_data()
return o.encode('ascii')
if __name__ == "__main__":
if len(sys.argv) >= 3:
HOST = sys.argv[1]
PORT = int(sys.argv[2])
else:
print usage()
sys.exit(1)
rospy.loginfo("Connecting to oro-server on " + HOST + ":" + str(PORT) + "...")
try:
oro = connect(MAX_CONNECTION_TRIES)
#print "Available methods:\n"
#for srv in oro.rpc_methods:
# print "\t- %s"%srv[0]
oro_ros_server()
except OroServerError as ose:
rospy.logerr('Oups! An error occured on ORO server!')
print(ose)
raise ROSException("An error occured on ORO server! Check the oro_ros node log!")
finally:
if (oro):
oro.close()
