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 get_services():
try:
services = get_service_list()
content = []
for i in services:
content.append([i, get_service_type(i), get_service_node(i)])
return jsonify(status=0, result=content)
except:
return jsonify(status=-1, result=[])
def get_service_info(service_name):
"""
GET /api/<version>/service/<service_name>
GET information about a ROS service.
"""
service_name = "/" + service_name
service_type = rosservice.get_service_type(service_name)
if not service_type:
return error("No such service exists")
return jsonify({
"request_type": service_type,
"node": rosservice.get_service_node(service_name),
"args": rosservice.get_service_args(service_name).split(" ")
})
def get_service_info(service_name):
"""
GET /api/<version>/service/<service_name>
GET information about a ROS service.
"""
service_name = "/" + service_name
service_type = rosservice.get_service_type(service_name)
if not service_type:
return error("No such service exists")
return jsonify({
"request_type": service_type,
"node": rosservice.get_service_node(service_name),
"args": rosservice.get_service_args(service_name).split(" ")
})
def generateReportSrviceCalls(service,serviceArguments, serviceArgumentsValues,serviceReturns,serviceReturnsValues, client):
x = rosservice.get_service_headers(service,rosservice.get_service_uri(service))
toReturn = {'srv':str(x['type']), 'client':client, 'server':str(rosservice.get_service_node(service)), 'service_name': service}
reqSubReport = []
respSubReport = {}
for x in range (0,len(serviceArguments)):
reqSubReport.append(str(serviceArgumentsValues[x]))
# for x in range (0, len(serviceReturns)):
# if service in serviceExpeptions:
# respSubReport['ExtendedReport'] = 'ExtendedReport'
# break
# respSubReport[str(serviceReturns[x])] = str(serviceReturnsValues[x])
toReturn['req'] = ','.join(reqSubReport)
#toReturn['resp'] = respSubReport
return toReturn
def generateReportSrviceCalls(service,serviceArguments, serviceArgumentsValues,serviceReturns,serviceReturnsValues, client):
x = rosservice.get_service_headers(service,rosservice.get_service_uri(service))
toReturn = {'srv':str(x['type']), 'client':client, 'server':str(rosservice.get_service_node(service)), 'service_name': service}
reqSubReport = []
respSubReport = {}
for x in range (0,len(serviceArguments)):
reqSubReport.append(str(serviceArgumentsValues[x]))
# for x in range (0, len(serviceReturns)):
# if service in serviceExpeptions:
# respSubReport['ExtendedReport'] = 'ExtendedReport'
# break
# respSubReport[str(serviceReturns[x])] = str(serviceReturnsValues[x])
toReturn['req'] = ','.join(reqSubReport)
#toReturn['resp'] = respSubReport
return toReturn
def register(self, registration):
'''
Registers a rule with the local master.
@param registration : registration details
@type utils.Registration
@return the updated registration object (only adds an anonymously generated local node name)
@rtype utils.Registration
'''
# rograph.Master doesn't care whether the node is prefixed with slash or not, but we use it to
# compare registrations later in FlippedInterface._is_registration_in_remote_rule()
registration.local_node = "/" + self._get_anonymous_node_name(
registration.connection.rule.node)
rospy.logdebug("Gateway : registering a new node [%s] for [%s]" %
(registration.local_node, registration))
# Then do we need checkIfIsLocal? Needs lots of parsing time, and the outer class should
# already have handle that.
node_master = rosgraph.Master(registration.local_node)
if registration.connection.rule.type == rocon_python_comms.PUBLISHER:
try:
node_master.registerPublisher(
registration.connection.rule.name,
registration.connection.type_info,
registration.connection.xmlrpc_uri)
return registration
except (socket.error, socket.gaierror) as e:
rospy.logerr(
"Gateway : got socket error trying to register a publisher on the local master [%s][%s]"
% (registration.connection.rule.name, str(e)))
return None
except rosgraph.masterapi.Error as e:
rospy.logerr(
"Gateway : got error trying to register a publisher on the local master [%s][%s]"
% (registration.connection.rule.name, str(e)))
return None
except rosgraph.masterapi.Failure as e:
rospy.logerr(
"Gateway : failed to register a publisher on the local master [%s][%s]"
% (registration.connection.rule.name, str(e)))
return None
elif registration.connection.rule.type == rocon_python_comms.SUBSCRIBER:
try:
self._register_subscriber(node_master,
registration.connection.rule.name,
registration.connection.type_info,
registration.connection.xmlrpc_uri)
return registration
except (socket.error, socket.gaierror) as e:
rospy.logerr(
"Gateway : got socket error trying to register a subscriber on the local master [%s][%s]"
% (registration.connection.rule.name, str(e)))
return None
except rosgraph.masterapi.Error as e:
rospy.logerr(
"Gateway : got error trying to register a subscriber on the local master [%s][%s]"
% (registration.connection.rule.name, str(e)))
return None
except rosgraph.masterapi.Failure as e:
rospy.logerr(
"Gateway : failed to register a subscriber on the local master [%s][%s]"
% (registration.connection.rule.name, str(e)))
return None
elif registration.connection.rule.type == rocon_python_comms.SERVICE:
node_name = rosservice.get_service_node(
registration.connection.rule.name)
if node_name is not None:
rospy.logwarn(
"Gateway : tried to register a service that is already locally available, aborting [%s][%s]"
% (registration.connection.rule.name, node_name))
return None
else:
if registration.connection.rule.name is None:
rospy.logerr(
"Gateway : tried to register a service with name set to None [%s, %s, %s]"
% (registration.connection.rule.name,
registration.connection.type_info,
registration.connection.xmlrpc_uri))
return None
if registration.connection.type_info is None:
rospy.logerr(
"Gateway : tried to register a service with type_info set to None [%s, %s, %s]"
% (registration.connection.rule.name,
registration.connection.type_info,
registration.connection.xmlrpc_uri))
return None
if registration.connection.xmlrpc_uri is None:
rospy.logerr(
"Gateway : tried to register a service with xmlrpc_uri set to None [%s, %s, %s]"
% (registration.connection.rule.name,
registration.connection.type_info,
registration.connection.xmlrpc_uri))
return None
try:
node_master.registerService(
registration.connection.rule.name,
registration.connection.type_info,
registration.connection.xmlrpc_uri)
return registration
except (socket.error, socket.gaierror) as e:
rospy.logerr(
"Gateway : got socket error trying to register a service on the local master [%s][%s]"
% (registration.connection.rule.name, str(e)))
return None
except rosgraph.masterapi.Error as e:
rospy.logerr(
"Gateway : got error trying to register a service on the local master [%s][%s]"
% (registration.connection.rule.name, str(e)))
return None
except rosgraph.masterapi.Failure as e:
rospy.logerr(
"Gateway : failed to register a service on the local master [%s][%s]"
% (registration.connection.rule.name, str(e)))
return None
elif registration.connection.rule.type == rocon_python_comms.ACTION_SERVER:
try:
# Need to update these with self._register_subscriber
self._register_subscriber(
node_master, registration.connection.rule.name + "/goal",
registration.connection.type_info + "ActionGoal",
registration.connection.xmlrpc_uri)
self._register_subscriber(
node_master, registration.connection.rule.name + "/cancel",
"actionlib_msgs/GoalID",
registration.connection.xmlrpc_uri)
node_master.registerPublisher(
registration.connection.rule.name + "/status",
"actionlib_msgs/GoalStatusArray",
registration.connection.xmlrpc_uri)
node_master.registerPublisher(
registration.connection.rule.name + "/feedback",
registration.connection.type_info + "ActionFeedback",
registration.connection.xmlrpc_uri)
node_master.registerPublisher(
registration.connection.rule.name + "/result",
registration.connection.type_info + "ActionResult",
registration.connection.xmlrpc_uri)
return registration
except (socket.error, socket.gaierror) as e:
rospy.logerr(
"Gateway : got socket error trying to register an action server on the local master [%s][%s]"
% (registration.connection.rule.name, str(e)))
return None
except rosgraph.masterapi.Error as e:
rospy.logerr(
"Gateway : got error trying to register an action server on the local master [%s][%s]"
% (registration.connection.rule.name, str(e)))
return None
except rosgraph.masterapi.Failure as e:
rospy.logerr(
"Gateway : failed to register an action server on the local master [%s][%s]"
% (registration.connection.rule.name, str(e)))
return None
elif registration.connection.rule.type == rocon_python_comms.ACTION_CLIENT:
try:
node_master.registerPublisher(
registration.connection.rule.name + "/goal",
registration.connection.type_info + "ActionGoal",
registration.connection.xmlrpc_uri)
node_master.registerPublisher(
registration.connection.rule.name + "/cancel",
"actionlib_msgs/GoalID",
registration.connection.xmlrpc_uri)
self._register_subscriber(
node_master, registration.connection.rule.name + "/status",
"actionlib_msgs/GoalStatusArray",
registration.connection.xmlrpc_uri)
self._register_subscriber(
node_master,
registration.connection.rule.name + "/feedback",
registration.connection.type_info + "ActionFeedback",
registration.connection.xmlrpc_uri)
self._register_subscriber(
node_master, registration.connection.rule.name + "/result",
registration.connection.type_info + "ActionResult",
registration.connection.xmlrpc_uri)
return registration
except (socket.error, socket.gaierror) as e:
rospy.logerr(
"Gateway : got socket error trying to register an action server on the local master [%s][%s]"
% (registration.connection.rule.name, str(e)))
return None
except rosgraph.masterapi.Error as e:
rospy.logerr(
"Gateway : got error trying to register an action server on the local master [%s][%s]"
% (registration.connection.rule.name, str(e)))
return None
except rosgraph.masterapi.Failure as e:
rospy.logerr(
"Gateway : failed to register an action server on the local master [%s][%s]"
% (registration.connection.rule.name, str(e)))
return None
else:
rospy.logerr("Gateway : tried to register unknown rule type [%s]" %
(registration.connection.rule.type))
return None
def getServiceNode(service):
""" Returns the name of the node that is providing the given service, or empty string """
node = rosservice.get_service_node(service)
if node==None:
node = ""
return node
if heart_beat_count == 39:
new_env = os.environ.copy()
new_env['ROS_PACKAGE_PATH'] = ROS_PACKAGE_PATH
if sub_process_thread != None:
sub_process_thread_ps_process = psutil.Process(
pid=sub_process_thread.pid)
for child in sub_process_thread_ps_process.children(
recursive=True):
child.kill()
sub_process_thread_ps_process.kill()
sub_process_thread = None
else:
cmd = None
if galileo_status.navStatus == 1 and not rplidar_flag:
# 打开雷达电机
if rosservice.get_service_node("/start_motor") is not None:
cmd = "rosservice call /start_motor"
sub_process_thread = subprocess.Popen(cmd,
shell=True,
env=new_env)
# elif rplidar_flag and galileo_status.navStatus == 0:
# # 关闭雷达电机
# if rosservice.get_service_node("/stop_motor") is not None:
# cmd = "rosservice call /stop_motor"
# sub_process_thread = subprocess.Popen(
# cmd, shell=True, env=new_env)
if heart_beat_count == 1:
rplidar_flag = False
# 每秒广播一次
if broadcast_count == 10:
def getServiceNode(service):
""" Returns the name of the node that is providing the given service, or empty string """
node = rosservice.get_service_node(service)
if node == None:
node = ""
return node
def register(self, registration):
'''
Registers a rule with the local master.
@param registration : registration details
@type utils.Registration
@return the updated registration object (only adds an anonymously generated local node name)
@rtype utils.Registration
'''
registration.local_node = self._get_anonymous_node_name(
registration.connection.rule.node)
rospy.logdebug("Gateway : registering a new node [%s] for [%s]" %
(registration.local_node, registration))
# Then do we need checkIfIsLocal? Needs lots of parsing time, and the outer class should
# already have handle that.
node_master = rosgraph.Master(registration.local_node)
if registration.connection.rule.type == PUBLISHER:
node_master.registerPublisher(registration.connection.rule.name,
registration.connection.type_info,
registration.connection.xmlrpc_uri)
return registration
elif registration.connection.rule.type == SUBSCRIBER:
self._register_subscriber(node_master,
registration.connection.rule.name,
registration.connection.type_info,
registration.connection.xmlrpc_uri)
return registration
elif registration.connection.rule.type == SERVICE:
if rosservice.get_service_node(registration.connection.rule.name):
rospy.logwarn(
"Gateway : tried to register a service that is already locally available, aborting [%s]"
% registration.connection.rule.name)
return None
else:
node_master.registerService(registration.connection.rule.name,
registration.connection.type_info,
registration.connection.xmlrpc_uri)
return registration
elif registration.connection.rule.type == ACTION_SERVER:
# Need to update these with self._register_subscriber
self._register_subscriber(
node_master, registration.connection.rule.name + "/goal",
registration.connection.type_info + "ActionGoal",
registration.connection.xmlrpc_uri)
self._register_subscriber(
node_master, registration.connection.rule.name + "/cancel",
"actionlib_msgs/GoalID", registration.connection.xmlrpc_uri)
node_master.registerPublisher(
registration.connection.rule.name + "/status",
"actionlib_msgs/GoalStatusArray",
registration.connection.xmlrpc_uri)
node_master.registerPublisher(
registration.connection.rule.name + "/feedback",
registration.connection.type_info + "ActionFeedback",
registration.connection.xmlrpc_uri)
node_master.registerPublisher(
registration.connection.rule.name + "/result",
registration.connection.type_info + "ActionResult",
registration.connection.xmlrpc_uri)
return registration
elif registration.connection.rule.type == ACTION_CLIENT:
node_master.registerPublisher(
registration.connection.rule.name + "/goal",
registration.connection.type_info + "ActionGoal",
registration.connection.xmlrpc_uri)
node_master.registerPublisher(
registration.connection.rule.name + "/cancel",
"actionlib_msgs/GoalID", registration.connection.xmlrpc_uri)
self._register_subscriber(
node_master, registration.connection.rule.name + "/status",
"actionlib_msgs/GoalStatusArray",
registration.connection.xmlrpc_uri)
self._register_subscriber(
node_master, registration.connection.rule.name + "/feedback",
registration.connection.type_info + "ActionFeedback",
registration.connection.xmlrpc_uri)
self._register_subscriber(
node_master, registration.connection.rule.name + "/result",
registration.connection.type_info + "ActionResult",
registration.connection.xmlrpc_uri)
return registration
return None
if heart_beat_count == 39:
new_env = os.environ.copy()
new_env['ROS_PACKAGE_PATH'] = ROS_PACKAGE_PATH
if sub_process_thread != None:
sub_process_thread_ps_process = psutil.Process(
pid=sub_process_thread.pid)
for child in sub_process_thread_ps_process.children(recursive=True):
child.kill()
sub_process_thread_ps_process.kill()
sub_process_thread = None
else:
cmd = None
if galileo_status.navStatus == 1 and not rplidar_flag:
# 打开雷达电机
if rosservice.get_service_node("/start_motor") is not None:
cmd = "rosservice call /start_motor"
sub_process_thread = subprocess.Popen(
cmd, shell=True, env=new_env)
# elif rplidar_flag and galileo_status.navStatus == 0:
# # 关闭雷达电机
# if rosservice.get_service_node("/stop_motor") is not None:
# cmd = "rosservice call /stop_motor"
# sub_process_thread = subprocess.Popen(
# cmd, shell=True, env=new_env)
if heart_beat_count == 1:
rplidar_flag = False
# 每秒广播一次
if broadcast_count == 10:
broadcast_count = 0
