def test_get_service_class_by_name(self):
import rosservice
try:
rosservice.get_service_class_by_name('fake')
self.fail("should have raised")
except rosservice.ROSServiceException, e:
self.assertEquals("Service [fake] is not available.", str(e))
def test_get_service_class_by_name(self):
import rosservice
try:
rosservice.get_service_class_by_name('fake')
self.fail("should have raised")
except rosservice.ROSServiceException, e:
self.assertEquals("Service [fake] is not available.", str(e))
def pepper_answer(self):
# In ROS, nodes are uniquely named. If two nodes with the same
# name are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'listener' node so that multiple listeners can
# run simultaneously.
rospy.init_node('pepper_answer', anonymous=False)
self.last_rec_label = "nobody"
self.hog_face_detector = dlib.get_frontal_face_detector()
self.bridge = CvBridge()
self.label = "nobody"
self.tresh = 2
self.already = False
self.guest_gender = 0
self.guest_glasses = 0
self.guest_age = 20
# publishers
#self.pepper_string_pub = rospy.Publisher("/speech", String, queue_size=1)
self.pepper_face_recognized_pub = rospy.Publisher("/which_guest",
String,
queue_size=1)
self.pepper_prop_pub = rospy.Publisher("/person_characteristics",
PersonCharacteristics,
queue_size=1)
self.pepper_go_to_pub = rospy.Publisher("/position_command",
PositionCommand,
queue_size=1)
# services
self.rec_srv = rospy.ServiceProxy(
"/recognize", rosservice.get_service_class_by_name("/recognize"))
self.an_srv = rospy.ServiceProxy(
"/annotate", rosservice.get_service_class_by_name("/annotate"))
self.obj_srv = rospy.ServiceProxy(
"/object_recognition",
rosservice.get_service_class_by_name("/object_recognition"))
self.prop_srv = rospy.ServiceProxy(
"/get_face_properties",
rosservice.get_service_class_by_name("/get_face_properties"))
# subscribers
#rospy.Subscriber("/recognitions", Recognitions, self.callback_recognition)
#rospy.Subscriber("/naoqi_driver/camera/front/image_raw", Image, self.callback_image)
rospy.Subscriber("/camera/raw", Image, self.callback_image)
rospy.Subscriber("/person_name", String, self.callback_label)
rospy.Subscriber("/current_location", String, self.callback_currentloc)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
def _refresh_loggers(self, node):
"""
Store a list of loggers available for passed in node.
:param node: name of the node to query, ''str''
:raises: :exc:`ROSTopicException` If topic type cannot be determined or loaded
"""
self._current_loggers = []
self._current_levels = {}
# Construct the service name, taking into account our namespace
servicename = rosgraph.names.ns_join(
rosgraph.names.ns_join(rosgraph.names.get_ros_namespace(), node),
'get_loggers')
# Construct the service name, taking into account our namespace
servicename = rosgraph.names.resolve_name(
servicename, rosgraph.names.get_ros_namespace())
try:
service = rosservice.get_service_class_by_name(servicename)
except rosservice.ROSServiceException as e:
raise ROSConsoleException(
"node '%s' doesn't exist or doesn't support query: %s" % (node, e))
request = service._request_class()
proxy = rospy.ServiceProxy(str(servicename), service)
try:
response = proxy(request)
except rospy.ServiceException as e:
raise ROSConsoleException("node '%s' logger request failed: %s" % (node, e))
if response._slot_types[0] == 'roscpp/Logger[]':
for logger in getattr(response, response.__slots__[0]):
self._current_loggers.append(logger.name)
self._current_levels[logger.name] = logger.level
else:
raise ROSConsoleException(repr(response))
def __init__(self, context, service_name):
self._service_name = service_name
self._service = rosservice.get_service_class_by_name(self._service_name)
self._service_proxy = rospy.ServiceProxy(self._service_name, self._service)
self._widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_copter'), 'resource', 'SrvWidget.ui')
loadUi(ui_file, self._widget)
context.addWidget(self._widget)
self._widget.service_label.setText("Service " + self._service_name)
self._request = self._service._request_class()
self._inputs = []
for slot_name, type_name in zip(self._request.__slots__, self._request._slot_types):
if type_name in ["float32", "float64"]:
self._inputs.append(FloatInputWidget(self._widget.input_container, slot_name))
elif type_name in ["int8", "uint8", "int16", "uint16", "int32", "uint32", "int64", "uint64"]:
self._inputs.append(IntInputWidget(self._widget.input_container, slot_name))
elif type_name in ["string"]:
self._inputs.append(StringInputWidget(self._widget.input_container, slot_name))
else:
print "Service input type", type_name, "needs to be implemented!"
print "Service", self._service_name, "is not available."
self._widget.close()
self._widget.apply_button.clicked.connect(self._init_msf)
def listen_to(self, service_name):
rospy.loginfo("Tapping service: {}".format(service_name))
# block until the service is available
rospy.wait_for_service(service_name)
# determine which node provides the given service
server = rosservice.get_service_node(service_name)
assert not server is None
# get the class used by this service
service_cls = rosservice.get_service_class_by_name(service_name)
# create a persistent proxy to that service
# inject a persistent connection into the proxy, so that when we replace
# the original service, we can still forward messages onto the old one
proxy = rospy.ServiceProxy(service_name, service_cls, persistent=True)
# TODO: listen for failures
# http://docs.ros.org/jade/api/rospy/html/rospy.impl.tcpros_service-pysrc.html#ServiceProxy
service_uri = self.master.lookupService(proxy.resolved_name)
(dest_addr, dest_port) = rospy.core.parse_rosrpc_uri(service_uri)
proxy.transport = TCPROSTransport(proxy.protocol, proxy.resolved_name)
proxy.transport.buff_size = proxy.buff_size
proxy.transport.connect(dest_addr, dest_port, service_uri)
# record the URI of the original service, so we can restore it later
self.tapped[service_name] = service_uri
# create a new, tapped service, with the same name
tap = lambda r: self.__handler(server, service_name, proxy, r)
rospy.Service(service_name, service_cls, tap)
rospy.loginfo("Tapped service: {}".format(service_name))
def call_service_util(service_name, service_args, service_class=None):
rospy.init_node('rosservice', anonymous=True, disable_signals=True)
if service_class is None:
service_class = get_service_class_by_name(service_name)
request = service_class._request_class()
try:
now = rospy.get_rostime()
keys = {'now': now, 'auto': std_msgs.msg.Header(stamp=now)}
genpy.message.fill_message_args(request, service_args, keys=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 ROSServiceException(
"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)))
try:
return request, rospy.ServiceProxy(service_name,
service_class)(request)
except rospy.ServiceException as e:
raise ROSServiceException(str(e))
except genpy.SerializationError as e:
raise ROSServiceException("Unable to send request. One of the fields has an incorrect type:\n"+\
" %s\n\nsrv file:\n%s"%(e, rosmsg.get_srv_text(service_class._type)))
except rospy.ROSSerializationException as e:
raise ROSServiceException("Unable to send request. One of the fields has an incorrect type:\n"+\
" %s\n\nsrv file:\n%s"%(e, rosmsg.get_srv_text(service_class._type)))
def init_call(self, process):
try:
service_name = process["call"]["service_name"]
service_name = self.get_name(service_name)
service_class = rosservice.get_service_class_by_name(service_name)
timeout_srv = 1.0
if "timeout" in process["call"]:
timeout_srv = process["call"]["timeout"]
req = service_class._request_class()
for arg in process["call"]["service_args"]:
exec(arg)
d = {}
d["name"] = "call"
d["verbose"] = self.is_verbose(process["call"])
d["def_msg"] = ("Calling service '{}'".format(service_name),
"info", req)
d["func"] = "self.call(**kwargs)"
d["kwargs"] = {}
d["kwargs"]["service_name"] = service_name
d["kwargs"]["service_class"] = service_class
d["kwargs"]["req"] = req
d["kwargs"]["verbose"] = self.is_verbose(process["call"])
d["kwargs"]["timeout_srv"] = timeout_srv
self.processes.append(d)
except Exception as e:
self.event_cb(self.init_err_str.format("call", str(e)), "warn")
self.processes.append("not_initialised")
def send_logger_change_message(self, node, logger, level):
"""
Send a logger level change request to 'node'.
:param node: name of the node to chaange, ''str''
:param logger: name of the logger to change, ''str''
:param level: name of the level to change, ''str''
:returns: True if the response is valid, ''bool''
:returns: False if the request raises an exception or would not change the state, ''bool''
"""
# Construct the service name, taking into account our namespace
servicename = rosgraph.names.ns_join(
rosgraph.names.ns_join(rosgraph.names.get_ros_namespace(), node),
'set_logger_level')
# Construct the service name, taking into account our namespace
servicename = rosgraph.names.resolve_name(
servicename, rosgraph.names.get_ros_namespace())
if self._current_levels[logger] == level:
return False
service = rosservice.get_service_class_by_name(servicename)
request = service._request_class()
request.logger = logger
request.level = level
proxy = rospy.ServiceProxy(str(servicename), service)
try:
proxy(request)
self._current_levels[logger] = level.upper()
except rospy.ServiceException as e:
raise ROSConsoleException("node '%s' logger request failed: %s" %
(node, e))
return True
def send_logger_change_message(self, node, logger, level):
"""
Sends a logger level change request to 'node'.
:param node: name of the node to chaange, ''str''
:param logger: name of the logger to change, ''str''
:param level: name of the level to change, ''str''
:returns: True if the response is valid, ''bool''
:returns: False if the request raises an exception or would not change the cached state, ''bool''
"""
servicename = node + '/set_logger_level'
if self._current_levels[logger].lower() == level.lower():
return False
service = rosservice.get_service_class_by_name(servicename)
request = service._request_class()
setattr(request, 'logger', logger)
setattr(request, 'level', level)
proxy = rospy.ServiceProxy(str(servicename), service)
try:
proxy(request)
self._current_levels[logger] = level.upper()
except rospy.ServiceException as e:
qWarning('SetupDialog.level_changed(): request:\n%r' % (request))
qWarning('SetupDialog.level_changed() "%s":\n%s' %
(servicename, e))
return False
return True
def __call_service(self, service_name, service_args, service_class=None):
import std_msgs.msg
if service_class is None:
service_class = get_service_class_by_name(service_name)
request = service_class._request_class()
try:
now = rospy.get_rostime()
keys = {'now': now, 'auto': std_msgs.msg.Header(stamp=now)}
roslib.message.fill_message_args(request, service_args, keys=keys)
except roslib.message.ROSMessageException, 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__)
return (
"Incompatible arguments to call service:\n%s\n"
"Provided arguments are:\n%s\n\nService arguments are: [%s]"
% (
e, argsummary(service_args),
roslib.message.get_printable_message_args(request)))
def _refresh_loggers(self, node):
"""
Stores a list of loggers available for passed in node
:param node: name of the node to query, ''str''
"""
self._current_loggers = []
self._current_levels = {}
servicename = node + '/get_loggers'
try:
service = rosservice.get_service_class_by_name(servicename)
except rosservice.ROSServiceIOException as e:
qWarning('During get_service_class_by_name "%s":\n%s' %
(servicename, e))
return False
request = service._request_class()
proxy = rospy.ServiceProxy(str(servicename), service)
try:
response = proxy(request)
except rospy.ServiceException as e:
qWarning('SetupDialog.get_loggers(): request:\n%s' %
(type(request)))
qWarning('SetupDialog.get_loggers() "%s":\n%s' % (servicename, e))
return False
if response._slot_types[0] == 'roscpp/Logger[]':
for logger in getattr(response, response.__slots__[0]):
self._current_loggers.append(getattr(logger, 'name'))
self._current_levels[getattr(logger, 'name')] = getattr(
logger, 'level')
else:
qWarning(repr(response))
return False
return True
def get_service_type(self, service_name):
if service_name not in self.service_types:
try:
self.service_types[service_name] = rosservice.get_service_class_by_name(service_name)
except ROSServiceException as e:
raise JoyTeleopException("service {} could not be loaded: {}".format(service_name, str(e)))
return self.service_types[service_name]
def send_logger_change_message(self, node, logger, level):
"""
Send a logger level change request to 'node'.
:param node: name of the node to chaange, ''str''
:param logger: name of the logger to change, ''str''
:param level: name of the level to change, ''str''
:returns: True if the response is valid, ''bool''
:returns: False if the request raises an exception or would not change the state, ''bool''
"""
# Construct the service name, taking into account our namespace
servicename = rosgraph.names.ns_join(
rosgraph.names.ns_join(rosgraph.names.get_ros_namespace(), node),
'set_logger_level')
# Construct the service name, taking into account our namespace
servicename = rosgraph.names.resolve_name(
servicename, rosgraph.names.get_ros_namespace())
if self._current_levels[logger] == level:
return False
service = rosservice.get_service_class_by_name(servicename)
request = service._request_class()
request.logger = logger
request.level = level
proxy = rospy.ServiceProxy(str(servicename), service)
try:
proxy(request)
self._current_levels[logger] = level.upper()
except rospy.ServiceException as e:
raise ROSConsoleException("node '%s' logger request failed: %s" % (node, e))
return True
def __init__(self):
super(ServiceCallerWidget, self).__init__()
self.setObjectName('ServiceCallerWidget')
# subsribe to service for gui
rospy.wait_for_service('/start_stop_clock')
self._services = rosservice.get_service_class_by_name(
"/start_stop_clock")
self._service_info = {}
self._service_info['service_name'] = "/start_stop_clock"
self._service_info['service_class'] = self._services
self._service_info['service_proxy'] = rospy.ServiceProxy(
"/start_stop_clock",
self._service_info['service_class'],
persistent=True)
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_simtime_plugin'), 'resource',
'rqt_simtime.ui')
loadUi(ui_file, self, {'ExtendedComboBox': ExtendedComboBox})
self.call_service_button.setIcon(
QIcon.fromTheme('media-playback-start'))
# self.RTF_val_box.valueChanged.connect(self.changeRTF)
# populate boxes for the first time
self.dtSize_val = None
self.PubFreq_val = None
self.init_box_values()
# self.dtSize_val = self.dtSize_val_box.value()
self.dtSize_val_box.editingFinished.connect(self.change_dt)
# self.PubFreq_val = self.PubFreq_val_box.value()
self.PubFreq_val_box.editingFinished.connect(self.change_dt)
def _refresh_loggers(self, node):
"""
Stores a list of loggers available for passed in node
:param node: name of the node to query, ''str''
"""
self._current_loggers = []
self._current_levels = {}
servicename = node + '/get_loggers'
try:
service = rosservice.get_service_class_by_name(servicename)
except rosservice.ROSServiceIOException as e:
qWarning('During get_service_class_by_name "%s":\n%s' % (servicename, e))
return False
request = service._request_class()
proxy = rospy.ServiceProxy(str(servicename), service)
try:
response = proxy(request)
except rospy.ServiceException as e:
qWarning('SetupDialog.get_loggers(): request:\n%s' % (type(request)))
qWarning('SetupDialog.get_loggers() "%s":\n%s' % (servicename, e))
return False
if response._slot_types[0] == 'roscpp/Logger[]':
for logger in getattr(response, response.__slots__[0]):
self._current_loggers.append(getattr(logger, 'name'))
self._current_levels[getattr(logger, 'name')] = getattr(logger, 'level')
else:
qWarning(repr(response))
return False
return True
def send_logger_change_message(self, node, logger, level):
"""
Sends a logger level change request to 'node'.
:param node: name of the node to chaange, ''str''
:param logger: name of the logger to change, ''str''
:param level: name of the level to change, ''str''
:returns: True if the response is valid, ''bool''
:returns: False if the request raises an exception or would not change the cached state, ''bool''
"""
servicename = node + '/set_logger_level'
if self._current_levels[logger].lower() == level.lower():
return False
service = rosservice.get_service_class_by_name(servicename)
request = service._request_class()
setattr(request, 'logger', logger)
setattr(request, 'level', level)
proxy = rospy.ServiceProxy(str(servicename), service)
try:
proxy(request)
self._current_levels[logger] = level.upper()
except rospy.ServiceException as e:
qWarning('SetupDialog.level_changed(): request:\n%r' % (request))
qWarning('SetupDialog.level_changed() "%s":\n%s' % (servicename, e))
return False
return True
def call_service_util(service_name, service_args, service_class=None):
rospy.init_node('rosservice', anonymous=True, disable_signals=True)
if service_class is None:
service_class = get_service_class_by_name(service_name)
request = service_class._request_class()
try:
now = rospy.get_rostime()
keys = {'now': now, 'auto': std_msgs.msg.Header(stamp=now) }
genpy.message.fill_message_args(request, service_args, keys=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 ROSServiceException("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)))
try:
return request, rospy.ServiceProxy(service_name, service_class)(request)
except rospy.ServiceException as e:
raise ROSServiceException(str(e))
except genpy.SerializationError as e:
raise ROSServiceException("Unable to send request. One of the fields has an incorrect type:\n"+\
" %s\n\nsrv file:\n%s"%(e, rosmsg.get_srv_text(service_class._type)))
except rospy.ROSSerializationException as e:
raise ROSServiceException("Unable to send request. One of the fields has an incorrect type:\n"+\
" %s\n\nsrv file:\n%s"%(e, rosmsg.get_srv_text(service_class._type)))
def response(req):
sentinel = False
x = req._connection_header
currentService = str(x['service']) + "_prime"
client = str(x['callerid'])
srv = subprocess.Popen("rosservice type " +currentService + " | rossrv show", shell=True, stdout=subprocess.PIPE).stdout.read()
serviceArguments = []
serviceArgumentsValues = []
serviceReturns = []
serviceReturnsValues = []
for line in srv.split('\n'):
if line == '---':
sentinel = True
continue
if sentinel == True and line != '':
if '/' in line.split(' ')[0]:
serviceExpeptions.append(currentService)
continue
serviceReturns.append(line.split(' ')[-1])
else:
if line != '':
serviceArguments.append(line.split(' ')[-1])
hasNoArguments = True
if str(rosservice.get_service_args(currentService)) != "":
hasNoArguments = False
for x in serviceArguments:
exec ('serviceArgumentsValues.append(req.'+ str(x) +')')
Response = rospy.ServiceProxy(currentService, rosservice.get_service_class_by_name(currentService))
toChange = ['.', ':', ' ']
time = str(datetime.now())
time = time.translate(None, '-'.join(toChange))
if hasNoArguments == False:
exec generateReturn(serviceArgumentsValues)
for x in serviceReturns:
if re.match("^[A-Za-z0-9_-]*$", x) and x != '':
if currentService in serviceExpeptions:
serviceReturnsValues.append(currentService)
break
exec('serviceReturnsValues.append(toReturn.'+ str(x) +')')
else:
toReturn = Response()
for x in serviceReturns:
if re.match("^[A-Za-z0-9_-]*$", x) and x != '':
if currentService in serviceExpeptions:
serviceReturnsValues.append(currentService)
break
exec('serviceReturnsValues.append(toReturn.'+ str(x) +')')
report = str(generateReportSrviceCalls(currentService,serviceArguments,serviceArgumentsValues,serviceReturns,serviceReturnsValues,client))
currentReported.append(report)
return toReturn
def response(req):
sentinel = False
x = req._connection_header
currentService = str(x['service']) + "_prime"
client = str(x['callerid'])
srv = subprocess.Popen("rosservice type " +currentService + " | rossrv show", shell=True, stdout=subprocess.PIPE).stdout.read()
serviceArguments = []
serviceArgumentsValues = []
serviceReturns = []
serviceReturnsValues = []
for line in srv.split('\n'):
if line == '---':
sentinel = True
continue
if sentinel == True and line != '':
if '/' in line.split(' ')[0]:
serviceExpeptions.append(currentService)
continue
serviceReturns.append(line.split(' ')[-1])
else:
if line != '':
serviceArguments.append(line.split(' ')[-1])
hasNoArguments = True
if str(rosservice.get_service_args(currentService)) != "":
hasNoArguments = False
for x in serviceArguments:
exec ('serviceArgumentsValues.append(req.'+ str(x) +')')
Response = rospy.ServiceProxy(currentService, rosservice.get_service_class_by_name(currentService))
toChange = ['.', ':', ' ']
time = str(datetime.now())
time = time.translate(None, '-'.join(toChange))
if hasNoArguments == False:
exec generateReturn(serviceArgumentsValues)
for x in serviceReturns:
if re.match("^[A-Za-z0-9_-]*$", x) and x != '':
if currentService in serviceExpeptions:
serviceReturnsValues.append(currentService)
break
exec('serviceReturnsValues.append(toReturn.'+ str(x) +')')
else:
toReturn = Response()
for x in serviceReturns:
if re.match("^[A-Za-z0-9_-]*$", x) and x != '':
if currentService in serviceExpeptions:
serviceReturnsValues.append(currentService)
break
exec('serviceReturnsValues.append(toReturn.'+ str(x) +')')
report = str(generateReportSrviceCalls(currentService,serviceArguments,serviceArgumentsValues,serviceReturns,serviceReturnsValues,client))
currentReported.append(report)
return toReturn
def release_emergency_stop(self):
if 'reset_motorstop' in self.av_activate_services :
try:
rospy.wait_for_service('/reset_motorstop', timeout=0.5)
reset = rospy.ServiceProxy('reset_motorstop', rosservice.get_service_class_by_name('reset_motorstop'))
reset()
#print "reset"
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s"%e)
def set_emergency_stop(self):
if 'emergency_stop' in self.av_stop_services:
try:
rospy.wait_for_service('emergency_stop', timeout=0.5)
stop = rospy.ServiceProxy('emergency_stop', rosservice.get_service_class_by_name('/emergency_stop'))
stop()
rospy.loginfo("NO GO node stop")
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s"%e)
def start_stop_scheduler(self, val):
if "task_executor/set_execution_status" in self.av_stop_services:
try:
rospy.wait_for_service('task_executor/set_execution_status', timeout=0.5)
schstop = rospy.ServiceProxy('task_executor/set_execution_status', rosservice.get_service_class_by_name('task_executor/set_execution_status'))
schstop(val)
#print "Schedule Execute %s" %val
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s"%e)
def set_free_run(self, val):
if "/enable_motors" in self.av_stop_services:
try:
rospy.wait_for_service('enable_motors', timeout=0.5)
sfrun = rospy.ServiceProxy('enable_motors', rosservice.get_service_class_by_name('enable_motors'))
sfrun(val)
#print "Free run %s" %val
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s"%e)
def release_emergency_stop(self):
if '/reset_motorstop' in self.av_activate_services :
try:
rospy.wait_for_service('/reset_motorstop', timeout=0.5)
reset = rospy.ServiceProxy('/reset_motorstop', rosservice.get_service_class_by_name('/reset_motorstop'))
reset()
print "reset"
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s"%e)
def set_free_run(self, val):
if "/enable_motors" in self.av_stop_services:
try:
rospy.wait_for_service('/enable_motors', timeout=0.5)
sfrun = rospy.ServiceProxy('/enable_motors', rosservice.get_service_class_by_name('/enable_motors'))
sfrun(val)
print "Free run %s" %val
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s"%e)
def callback(self,event):
rospy.wait_for_service(self._service_name)
service_class = rosservice.get_service_class_by_name(self._service_name)
try:
service = rospy.ServiceProxy(self._service_name,service_class)
service()
rospy.loginfo('Service %s called.', self._service_name)
except rospy.ServiceException, e:
rospy.logwarn('Service %s call failed: %s',self._service_name,e)
def start_stop_scheduler(self, val):
if "/task_executor/set_execution_status" in self.av_stop_services:
try:
rospy.wait_for_service('/task_executor/set_execution_status', timeout=0.5)
schstop = rospy.ServiceProxy('/task_executor/set_execution_status', rosservice.get_service_class_by_name('/task_executor/set_execution_status'))
schstop(val)
print "Schedule Execute %s" %val
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s"%e)
def set_emergency_stop(self):
if '/emergency_stop' in self.av_stop_services:
try:
rospy.wait_for_service('/emergency_stop', timeout=0.5)
stop = rospy.ServiceProxy('/emergency_stop', rosservice.get_service_class_by_name('/emergency_stop'))
stop()
rospy.loginfo("NO GO node stop")
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s"%e)
def _create_detect_service_client(self):
srv_name = "/detect"
if self._srv_detect:
self._srv_detect.close()
if srv_name in rosservice.get_service_list():
rospy.loginfo("Creating proxy for service '%s'" % srv_name)
self._srv_detect = rospy.ServiceProxy(
srv_name, rosservice.get_service_class_by_name(srv_name))
def newServiceHandler(service):
primeState = False
non_prime = "OPERATOR___"
if service.split('_')[-1] == 'prime':
primeState = True
non_prime = non_prime + service.split('_prime')[0]
s = rospy.Service(non_prime,rosservice.get_service_class_by_name(str(service)),response)
else:
noServiceIntermediator.append(service)
return 'Passed'
def callback(self, event):
rospy.wait_for_service(self._service_name)
service_class = rosservice.get_service_class_by_name(
self._service_name)
try:
service = rospy.ServiceProxy(self._service_name, service_class)
service()
rospy.loginfo('Service %s called.', self._service_name)
except rospy.ServiceException, e:
rospy.logwarn('Service %s call failed: %s', self._service_name, e)
def set_emergency_stop(self):
if '/emergency_stop' in self.av_stop_services:
self.info= self.info + '\t - Set Emergency Stop\n'
try:
rospy.wait_for_service('/emergency_stop', timeout=0.5)
stop = rospy.ServiceProxy('/emergency_stop', rosservice.get_service_class_by_name('/emergency_stop'))
stop()
# "stop"
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s"%e)
def newServiceHandler(service):
primeState = False
non_prime = "OPERATOR___"
if service.split('_')[-1] == 'prime':
primeState = True
non_prime = non_prime + service.split('_prime')[0]
s = rospy.Service(non_prime,rosservice.get_service_class_by_name(str(service)),response)
else:
noServiceIntermediator.append(service)
return 'Passed'
def call_service( self, string ):
service_name = string
service_class = rosservice.get_service_class_by_name( service_name )
service = rospy.ServiceProxy(service_name, service_class)
request = service_class._request_class()
try:
response = service()
except rospy.ServiceException as e:
qWarning('service_caller: request:\n%r' % (request))
qWarning('service_caller: error calling service "%s":\n%s' % (self._service_info['service_name'], e))
def _create_service_client(self, srv_name):
"""
Method that creates a client service proxy to call either the GetFaceProperties.srv or the Recognize.srv
:param srv_name:
"""
if self._srv:
self._srv.close()
if srv_name in rosservice.get_service_list():
rospy.loginfo("Creating proxy for service '%s'" % srv_name)
self._srv = rospy.ServiceProxy(srv_name, rosservice.get_service_class_by_name(srv_name))
def set_emergency_stop(self):
if 'emergency_stop' in self.av_stop_services:
self.info = self.info + '\t - Set Emergency Stop\n'
try:
rospy.wait_for_service('emergency_stop', timeout=0.5)
stop = rospy.ServiceProxy(
'emergency_stop',
rosservice.get_service_class_by_name('emergency_stop'))
stop()
# "stop"
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s" % e)
def on_refresh_services_button_clicked(self):
service_names = rosservice.get_service_list()
self._services = {}
for service_name in service_names:
try:
self._services[service_name] = rosservice.get_service_class_by_name(service_name)
#qDebug('ServiceCaller.on_refresh_services_button_clicked(): found service %s using class %s' % (service_name, self._services[service_name]))
except (rosservice.ROSServiceException, rosservice.ROSServiceIOException) as e:
qWarning('ServiceCaller.on_refresh_services_button_clicked(): could not get class of service %s:\n%s' % (service_name, e))
self.service_combo_box.clear()
self.service_combo_box.addItems(sorted(service_names))
def _create_service_client(self, srv_name):
"""
Create a service client proxy
:param srv_name: Name of the service
"""
if self._srv:
self._srv.close()
if srv_name in rosservice.get_service_list():
rospy.loginfo("Creating proxy for service '%s'" % srv_name)
self._srv = rospy.ServiceProxy(
srv_name, rosservice.get_service_class_by_name(srv_name))
def call_service(service_name, result_msg, result_value, *args):
# Add leading slash to service_name. ROS qualifies all services with a
# leading slash.
# Find the class that ROS autogenerates from srv files that is associated
# with this service.
ServiceClass = rosservice.get_service_class_by_name(service_name)
rospy.wait_for_service(service_name, 1)
service_proxy = rospy.ServiceProxy(service_name, ServiceClass)
res = service_proxy(*args)
print(service_name)
print("-----------######-------------")
print(res)
assert getattr(res, "success", result_value)
def call_service(service_name, result_msg, result_value, *args):
# Add leading slash to service_name. ROS qualifies all services with a
# leading slash.
# Find the class that ROS autogenerates from srv files that is associated
# with this service.
ServiceClass = rosservice.get_service_class_by_name(service_name)
rospy.wait_for_service(service_name, 1)
service_proxy = rospy.ServiceProxy(service_name, ServiceClass)
res = service_proxy(*args)
print(service_name)
print("-----------######-------------")
print(res)
assert getattr(res, "success", result_value)
def start_stop_scheduler(self, val):
rospy.loginfo("Pause task executor")
if not val:
self.info= self.info + '\t - Pause task executor\n'
if "/task_executor/set_execution_status" in self.av_stop_services:
try:
rospy.wait_for_service('/task_executor/set_execution_status', timeout=0.5)
schstop = rospy.ServiceProxy('/task_executor/set_execution_status', rosservice.get_service_class_by_name('/task_executor/set_execution_status'))
schstop(val)
rospy.loginfo("Schedule Execute %s", val)
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s", e)
def on_refresh_services_button_clicked(self):
service_names = rosservice.get_service_list()
self._services = {}
for service_name in service_names:
try:
self._services[service_name] = rosservice.get_service_class_by_name(service_name)
#qDebug('ServiceCaller.on_refresh_services_button_clicked(): found service %s using class %s' % (service_name, self._services[service_name]))
except (rosservice.ROSServiceException, rosservice.ROSServiceIOException) as e:
qWarning('ServiceCaller.on_refresh_services_button_clicked(): could not get class of service %s:\n%s' % (service_name, e))
except Exception as e:
qWarning('ServiceCaller.on_refresh_services_button_clicked(): failed to load class of service %s:\n%s' % (service_name, e))
self.service_combo_box.clear()
self.service_combo_box.addItems(sorted(self._services.keys()))
def on_pushButton_refresh_clicked(self):
self._services = {}
# Select calibration services
service_names = rosservice.get_service_list()
for service_name in service_names:
try:
current_service = rosservice.get_service_class_by_name(service_name)
if current_service.__name__ == CALIBRATION_SERVICE_TYPE_NAME:
self._services[service_name] = current_service
except (rosservice.ROSServiceException, rosservice.ROSServiceIOException) as e:
qWarning('on_pushButton_refresh_clicked(): could not get class of service %s:\n%s' % (service_name, e))
self._widget.comboBox_services.clear()
self._widget.comboBox_services.addItems(sorted(self._services.keys()))
def get_map(service_name):
map_key = service_name
if not _map_cache.has_key(map_key):
service_class = rosservice.get_service_class_by_name('/' + service_name)
service_proxy = rospy.ServiceProxy(service_name, service_class)
resp = service_proxy()
size = (resp.map.info.width, resp.map.info.height)
im = Image.frombuffer('L', size, resp.map.data, "raw", 'L', 0, -1)
remap = {0:255, 100: 0, 255:128}
im = im.point(lambda x: remap.get(x, 0))
resolution = resp.map.info.resolution
_map_cache[map_key] = (im, resolution)
else:
(im, resolution) = _map_cache[map_key]
return (im, resolution)
def refresh_services(namespace_ros, server, servicesdict, idx,
services_object_opc):
rosservices = rosservice.get_service_list(namespace=namespace_ros)
for service_name_ros in rosservices:
try:
# If we already have create the service before
if service_name_ros not in servicesdict or servicesdict[
service_name_ros] is None:
serviceName = OpcUaROSService(
server, services_object_opc, idx, service_name_ros,
rosservice.get_service_class_by_name(service_name_ros))
assert (serviceName is not None
) # Do not insert Nones into our service list
servicesdict[service_name_ros] = serviceName
except (rosservice.ROSServiceException,
rosservice.ROSServiceIOException) as e:
try:
rospy.logerr("Error when trying to refresh services, " +
str(e))
except TypeError as e2:
rospy.logerr(
"Error when logging an Exception, can't convert everything to string"
)
# Remove services which are no longer in the list
# use extra iteration as to not get "dict changed during iteration" errors
# rosservices = rosservice.get_service_list() # TODO RH: removed double call to get_service_list, check if this break stuff
newServices = {
newServiceName: servicesdict[newServiceName]
for newServiceName in servicesdict.keys()
if newServiceName in rosservices
}
for serviceName in servicesdict.keys(
): # Check if any old services no longer exist
if serviceName not in newServices:
service = servicesdict[serviceName]
# Delete all children of pruned nodes
service.recursive_delete_items(
server.server.get_node(ua.NodeId(serviceName, idx)))
# Clean up empty parents
if service.parent.get_children():
server.server.delete_nodes([service.parent])
# TODO move this function to ros_server, and directly assign dict
servicesdict.clear()
servicesdict.update(newServices)
def _create_service_client(self, srv_name):
"""
Method that creates a client service proxy to call either the GetFaceProperties.srv or the Classify2D.srv
:param srv_name:
"""
if self._srv:
self._srv.close()
if srv_name in rosservice.get_service_list():
rospy.loginfo("Creating proxy for service '%s'" % srv_name)
self._srv = rospy.ServiceProxy(
srv_name, rosservice.get_service_class_by_name(srv_name))
# get id-label mapping
if rospy.has_param('object_labels'):
self.id_label_list = rospy.get_param('object_labels')
print("get id-label dict by rosparam")
def _extra_setup(self, timeout):
# Confirm the service is actually there
try:
rospy.wait_for_service(self.service_name,
timeout if timeout else None)
except rospy.ROSException:
self.logger.error(
"%s.setup() could not find a Service with name \"%s\"" %
(self.name, self.service_name))
return False
# Get the service class type & a service proxy
self._service_class = rosservice.get_service_class_by_name(
self.service_name)
self._service_proxy = rospy.ServiceProxy(self.service_name,
self._service_class)
return True
def start_stop_scheduler(self, val):
rospy.loginfo("Pause task executor")
if not val:
self.info = self.info + '\t - Pause task executor\n'
if "task_executor/set_execution_status" in self.av_stop_services:
try:
rospy.wait_for_service('task_executor/set_execution_status',
timeout=0.5)
schstop = rospy.ServiceProxy(
'task_executor/set_execution_status',
rosservice.get_service_class_by_name(
'task_executor/set_execution_status'))
schstop(val)
rospy.loginfo("Schedule Execute %s", val)
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s", e)
def perform_service_call(service_name):
"""
POST /api/<version>/service/<service_name>
POST to a service to change it somehow. service_name may be a path.
For example, to start an environmental recipe in an environment, use the
start_recipe service:
POST /api/<version>/service/<environment_id>/start_recipe {"recipe_id": <id>}
"""
# Add leading slash to service_name. ROS qualifies all services with a
# leading slash.
service_name = '/' + service_name
# Find the class that ROS autogenerates from srv files that is associated
# with this service.
try:
ServiceClass = rosservice.get_service_class_by_name(service_name)
except rosservice.ROSServiceException as e:
return error(e)
# If we made it this far, the service exists.
# Wait for service to be ready before attempting to use it.
try:
rospy.wait_for_service(service_name, 1)
except rospy.ROSException as e:
raise socket.error()
# Get JSON args if they exist. If they don't, treat as if empty array
# was passed.
json = request.get_json(silent=True)
args = json if json else []
service_proxy = rospy.ServiceProxy(service_name, ServiceClass)
try:
# Unpack the list of args and pass to service_proxy function.
res = service_proxy(*args)
except (rospy.ServiceException, TypeError) as e:
return error(e)
status_code = 200 if getattr(res, "success", True) else 400
data = {k: getattr(res, k) for k in res.__slots__}
return jsonify(data), status_code
def call_service(self):
sel = self.tree_view.selectedIndexes()[0]
service_name = self.model.getFullName(sel)
service_name = rosgraph.names.script_resolve_name("rosservice", service_name)
service_class = rosservice.get_service_class_by_name(service_name)
glob = {
"math": math
}
try:
service_args = eval("[" + self.args_lineedit.text() + "]", glob, {})
#service_args = eval(self.args_lineedit.text(), glob, {})
#if type(service_args) == tuple:
#service_args = list(service_args)
#else:
#service_args = [service_args]
service_proxy = rospy.ServiceProxy(service_name, service_class)
#req, res = rosservice.call_service(service_name, service_args, service_class)
res = service_proxy(*service_args)
print "response: %s" % res
except Exception as e:
self.response_label.setText("<i>" + str(e) + "</i>")
print "exception: %s" % e
return
self.response_label.setText(str(res))
def handle_ROS(self, path, qdict):
cstring = self.headers.get('Cookie', '')
cookie = Cookie.BaseCookie()
cookie.load(cstring)
path_parts = path.split("/")
username = ""
cmd = path_parts[2]
## parse out the username from the cookie
if cookie.has_key('MB_L1'):
l1 = cookie['MB_L1'].value
if l1.startswith("V1/"):
code = l1[3:]
parts = code.split(":", 1)
username = parts[0]
## Make a service call
## Args can be passed as an ordered list of parameters:
## /ros/service/<service_name>?args=<arg1>&args=<arg2>&<...>
## or as a dictionary of named parameters
## /ros/service/<service_name>?json=<json-encoded dictionary of args>
if cmd == "ping":
self.send_success("{'ping': %f}" % time.time())
if cmd == "service":
name = "/".join(path_parts[3:])
callback = qdict.get("callback", [''])[0]
try:
service_class = rosservice.get_service_class_by_name("/" + name)
except ROSServiceException:
self.send_failure()
return
request = service_class._request_class()
args = qdict.get('args', None)
if not args:
args = qdict.get('json', ['{}'])[0]
args = eval(args)
try:
now = rospy.get_rostime()
keys = { 'now': now, 'auto': rospy.Header(stamp=now) }
roslib.message.fill_message_args(request, args, keys=keys)
except MessageException:
raise ROSServiceException("Not enough arguments to call service.\n"+\
"Args should be: [%s], but are actually [%s] " %
(roslib.message.get_printable_message_args(request), args))
rospy.logdebug("service call: name=%s, args=%s" % (name, args))
rospy.wait_for_service(name)
service_proxy = rospy.ServiceProxy(name, service_class)
try:
msg = service_proxy(request)
except rospy.ServiceException:
self.send_failure()
return
msg = rosjson.ros_message_to_json(msg)
rospy.logdebug("service call: name=%s, resp=%s" % (name, msg))
self.send_success(msg, callback=callback)
return
if cmd == "publish":
topic = string.join(path_parts[3:], "/")
callback = qdict.get("callback", [''])[0]
topic_type = qdict.get("topic_type", [''])[0]
message_klass = roslib.message.get_message_class(topic_type)
message = message_klass()
args = qdict.get('args', None)
if not args:
args = qdict.get('json', ['{}'])[0]
args = eval(args)
try:
now = rospy.get_rostime()
keys = { 'now': now, 'auto': rospy.Header(stamp=now) }
roslib.message.fill_message_args(message, args, keys=keys)
except MessageException:
raise ROSServiceException("Not enough arguments to call service.\n"+\
"Args should be: [%s], but are actually [%s] " %
(roslib.message.get_printable_message_args(message), args))
rospy.logdebug("publish: topic=%s, topic_type=%s, args=%s" % (topic, topic_type, args))
pub = rospy.Publisher(topic, message_klass, latch=True)
try:
pub.publish(message)
except rospy.ServiceException:
self.send_failure()
return
self.send_success(callback=callback)
return
if cmd == "receive":
since = int(qdict.get('since', ['0'])[0])
topics = qdict.get("topic", "")
callback = qdict.get("callback", [''])[0]
nowait = int(qdict.get('nowait', ['0'])[0])
self._do_GET_topic(topics, since, nowait=nowait, callback=callback)
return
def call_service(service_name, *args, **kwargs):
rospy.wait_for_service(service_name)
service_type = get_service_class_by_name(service_name)
proxy = rospy.ServiceProxy(service_name, service_type)
return proxy(*args, **kwargs)
