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