def onDisConnect(request, action): ''' Removes the robot specified via url like '/disconnect/ROBOT_ID' from ROS-Publisher and Ros-Subscriber We only are here when the URl is like: '/disconnect/ROBOT_ID' ''' partURL = request.path # If at the end is a slash we remove it simply if "/" is partURL[-1]: partURL = partURL[:-1] # Get ROBOT_ID, which is the last element topic = partURL[11:] # Get everything after "/disconnect" # Iterate through every topic and unregister, then delete it if topic in ROS_PUBLISHER: ROS_PUBLISHER[topic].unregister() del ROS_PUBLISHER[topic] Log("INFO", "Disconnecting publisher on '{}'".format(topic)) RosConfigurator.removeTopic(topic) if topic in ROS_SUBSCRIBER: ROS_SUBSCRIBER[topic].unregister() del ROS_SUBSCRIBER[topic] Log("INFO", "Disconnecting subscriber on '{}'".format(topic)) RosConfigurator.removeTopic(topic) # Return success end_request(request, None, 200, "")
def signal_handler(signal, frame): Log("INFO", ('\nExiting from the application')) TopicHandler.unregisterAll() topicManager.removeListeners() server.close() Log("INFO", ('\nExit')) sys.exit(0)
def refreshSubscriptions(self): ## \brief Refresh exisiting subscriptions on context broker refreshAllContexts() for subscription in self.subscriptions.values(): subscriber_dict = self._generateSubscription( subscription["namespace"], subscription["data_type"], subscription["topics"], subscription["id"]) subscriber_dict.pop("entities", None) subscriber_dict.pop("reference", None) url = "http://{}:{}/NGSI10/contextSubscriptions/{}".format( DATA_CONTEXTBROKER["ADDRESS"], DATA_CONTEXTBROKER["PORT"], subscription["id"]) subscriber_json = json.dumps(subscriber_dict) response_body = self._sendRequest(url, subscriber_json, 'PUT') if response_body is not None: if "subscribeError" in response_body: Log("ERROR", "Error Refreshing subscription") Log( "ERROR", response_body["subscribeError"]["errorCode"] ["details"]) elif "orionError" in response_body: Log("ERROR", "Error Refreshing subscription") Log("ERROR", response_body["orionError"]["details"]) else: Log( "INFO", "Refreshed Connection to Context Broker with id {}". format(subscription["id"]))
def subscribeThread(self, robotID, topic): ''' A Subscription-Thread. Its Life-Cycle is as follows: -> Subscribe -> Delete old Subs-ID -> Save new Subs-ID -> Wait -> robotID: A string corresponding to the robotID topic: The Topic (string) to subscribe to. ''' while True: # Subscribe jsonData = self.subscribeJSONGenerator(robotID, topic) response = requests.post(self.CB_BASE_URL + "/v2/subscriptions", data=jsonData, headers={'Content-Type': 'application/json'}) self._checkResponse(response, created=True, robTop=(robotID, topic)) if 'Location' in response.headers: newSubID = response.headers['Location'] # <- get subscription-ID else: Log("WARNING", "Firos was not able to subscribe to topic: {} for robot {}".format(topic, robotID)) # Unsubscribe if robotID in self.subscriptionIds and topic in self.subscriptionIds[robotID]: response = requests.delete(self.CB_BASE_URL + self.subscriptionIds[robotID][topic]) self._checkResponse(response, subID=self.subscriptionIds[robotID][topic]) # Save new ID self.subscriptionIds[robotID][topic] = newSubID # Wait time.sleep(int(self.data["subscription"]["subscription_length"] * self.data["subscription"]["subscription_refresh_delay"])) # sleep Length * Refresh-Rate (where 0 < Refresh-Rate < 1) Log("INFO", "Refreshing Subscription for " + robotID + " and topic: " + str(topic))
def signal_handler(signal, frame): Log("INFO", ('\nExiting from the application')) RosTopicHandler.unregisterAll() #server.close() Log("INFO", ('\nExit')) rospy.signal_shutdown('Quitting...') print("Quit ROS") sys.exit(0)
def getRobots(refresh=False): ''' This retrieves the current configuration from FIROS. Here we load the `robots.json` and the 'whitelist.json' In case the `whitelist.json` is Empty: We do get the current robots from ROS and are adding them as subscribers (by default). In a small ROS-World this usually is no problem. But in an environment with many robots we might send a lot of data. ''' try: # Retrieves the whitelist.json. If it does not exists, it returns all topics. topics_regex = copy.deepcopy(RosConfigurator.systemTopics(refresh)) # Retrieves the robots.json. topics_json = getTopicsByJson() if len(topics_json) == 0: Log( "ERROR", "The file 'topics.json' is either empty or does not exist!\n\nExiting" ) sys.exit(1) # check if structure is as needed for key in topics_json.keys(): if len(topics_json[key]) != 2: Log( "ERROR", "The topic: '{}', does not have a list of length 2 (topics.json)! \n\nExiting" .format(key)) sys.exit(1) if not key.startswith("/"): Log( "ERROR", "The topic: '{}', does not start with '/' (topics.json)! \n\nExiting" .format(key)) sys.exit(1) if topics_json[key][1] not in ["publisher", "subscriber"]: Log( "ERROR", "The topic: '{}', does not specify publisher or subscriber (topics.json)! \n\nExiting" .format(key)) sys.exit(1) # Merge both dictionaties: # Here topics_json overrides entries in topics_regex: topics_regex.update(topics_json) topics = topics_regex return topics except Exception as e: traceback.print_exc() Log("ERROR", e) return {}
def loadMsgHandlers(topics_data): ''' This method initializes The Publisher and Subscriber for ROS and the Subscribers for the Context-broker (based on ROS-Publishers). It also initializes the structs for ROS messages (types, dicts and classes) For each topic, the ROS message-structs are initialized. Then (depending on Publisher or Subscriber) the corrsponding rospy Publisher/Subscriber is generated and added in its struct. topics_data: The data, as in topics.json (and whitelist) specified. ''' Log("INFO", "Getting configuration data") Log("INFO", "Generating topic handlers:") # Generate for topic in topics_data.keys(): # for each topic and topic in topics_data: # Load specific message from robot_data msg = str(topics_data[topic][0]) theclass = LibLoader.loadFromSystem(msg, topic) # Add specific message in struct to not load it again later. if theclass._type not in ROS_MESSAGE_CLASSES: ROS_MESSAGE_CLASSES[theclass._type] = theclass # setting Class # Create, if not already, a dictionary from the corresponding message-type if topic not in ROS_TOPIC_AS_DICT: ROS_TOPIC_AS_DICT[topic] = rosMsg2Dict(theclass()) # Set the topic class-type, which is for each topic always the same ROS_TOPIC_TYPE[topic] = theclass._type # Create Publisher or Subscriber if topics_data[topic][1].lower() == "subscriber": # Case it is a subscriber, add it in subscribers additionalArgsCallback = { "topic": topic } # Add addtional Infos about topic ROS_SUBSCRIBER[topic] = rospy.Subscriber(topic, theclass, _publishToCBRoutine, additionalArgsCallback) ROS_SUBSCRIBER_LAST_MESSAGE[ topic] = None # No message currently published else: # Case it is a publisher, add it in publishers ROS_PUBLISHER[topic] = rospy.Publisher( topic, theclass, queue_size=C.ROS_SUB_QUEUE_SIZE, latch=True) # After initializing ROS-PUB/SUBs, intitialize ContextBroker-Subscriber based on ROS-Publishers for each robot CloudPubSub.subscribe(ROS_PUBLISHER.keys(), ROS_TOPIC_TYPE, ROS_TOPIC_AS_DICT) Log("INFO", "\n") Log("INFO", "Subscribed to " + str(list(ROS_PUBLISHER.keys())) + "\n")
def unregisterAll(): ## \brief Unregister from all ROS topics CloudSubscriber.disconnectAll() MapHandler.mapRemover() Log("INFO", "Unsubscribing topics...") for subscriber in subscribers: subscriber.unregister() for robot_name in ROBOT_TOPICS: for topic in ROBOT_TOPICS[robot_name]["subscriber"]: ROBOT_TOPICS[robot_name]["subscriber"][topic]["subscriber"].unregister() Log("INFO", "Unsubscribed topics\n")
def _checkResponse(self, response, robTop=None, subID=None, created=False): ''' If a not good response from ContextBroker is received, the error will be printed. response: The response from ContextBroker robTop: A string Tuple (robotId, topic), for the curretn robot/topic subID: The Subscription ID string, which should get deleted created: Creation or Deletion of a subscription (bool) ''' if not response.ok: if created: Log("ERROR", "Could not create subscription for Robot {} and topic {} in Context-Broker :".format(robTop[0], robTop[1])) Log("ERROR", response.content) else: Log("WARNING", "Could not delete subscription {} from Context-Broker :".format(subID)) Log("WARNING", response.content)
def start(self): ## \brief start FIROS http server # \param self sa = self.httpd.socket.getsockname() Log("INFO", "\nServing HTTP on", sa[0], "port", sa[1], "...") while not self.stopped: self.httpd.handle_request()
def onDisConnect(request, action): ''' Removes the robot specified via url like '/disconnect/ROBOT_ID' from ROS-Publisher and Ros-Subscriber We only are here when the URl is like: '/robot/disconnect/ROBOT_ID' TODO DL RosConfigurator is here used? ''' partURL = request.path # If at the end is a slash we remove it simply if "/" is partURL[-1]: partURL = partURL[:-1] # Get ROBOT_ID, which is the last element robotID = partURL.split("/")[-1] Log("INFO", "Disconnecting robot '{}'".format(robotID)) # Iterate through every topic and unregister, then delete it if robotID in ROS_PUBLISHER: for topic in ROS_PUBLISHER[robotID]: ROS_PUBLISHER[robotID][topic].unregister() del ROS_PUBLISHER[robotID] RosConfigurator.removeRobot(robotID) if robotID in ROS_SUBSCRIBER: for topic in ROS_SUBSCRIBER[robotID]: ROS_SUBSCRIBER[robotID][topic].unregister() del ROS_SUBSCRIBER[robotID] RosConfigurator.removeRobot(robotID) # Return success end_request(request, None, 200, "")
def getRobots(refresh=False, withJson=True): ## \brief Get robots managed by firos # \param Refresh the robot list # \param Merge the robots with the configurtation JSON try: robots = copy.deepcopy(RosConfigurator.systemTopics(refresh)) if withJson: robots_json = getRobotsByJson() for robot_name in robots_json: robot_name = str(robot_name) if robot_name not in robots: robots[robot_name] = {"topics": {}} for topic_name in robots_json[robot_name]["topics"]: topic = robots_json[robot_name]["topics"][topic_name] robots[robot_name]["topics"][str(topic_name)] = { "msg": str(topic["msg"]) if type(topic["msg"]) is str else topic["msg"], "type": str(topic["type"]) } return robots except Exception as e: traceback.print_exc() Log("ERROR", e) return {}
def unregisterAll(): global SHUTDOWN_SIGNAL ''' First set the SHUTDOWN_SIGNAL, then unregister all subscriptions on ContextBroker, delete all created Entities on Context Broker and unregister subscriptions from ROS ''' SHUTDOWN_SIGNAL = True CloudPubSub.unsubscribe() CloudPubSub.unpublish() Log("INFO", "Unsubscribing topics...") for subscriber in subscribers: subscriber.unregister() for topic in ROS_SUBSCRIBER: ROS_SUBSCRIBER[topic].unregister() Log("INFO", "Unsubscribed topics\n")
def _robotConnection(data): ''' This resets firos into its original state TODO DL reset, instead of connect? TODO DL Add real connect for only one Robot? ''' robot_name = data.data Log("INFO", "Connected robot: " + robot_name) loadMsgHandlers(confManager.getRobots(True))
def _robotDisconnection(data): ''' Unregisters from a given topic data: The String which was sent to firos ''' topic = str(data.data) if topic in ROS_PUBLISHER: for topic in ROS_PUBLISHER[topic]: ROS_PUBLISHER[topic][topic].unregister() Log("INFO", "Disconnected publisher for: " + topic) del ROS_PUBLISHER[topic] if topic in ROS_SUBSCRIBER: for topic in ROS_SUBSCRIBER[topic]: ROS_SUBSCRIBER[topic][topic].unregister() Log("INFO", "Disconnected subscriber for: " + topic) del ROS_SUBSCRIBER[topic]
def onConnect(request, action): ## \brief Launch robot search and connection # \param client request # \param action Log("INFO", "Connecting robots") loadMsgHandlers(RosConfigurator.systemTopics(True)) request.send_response(200) request.end_headers() request.wfile.write("")
def _publish(context_id, datatype, attributes, connection, sendCommand=True): ## \brief Publish data of an entity in context broker # \param entity name # \param entity type # \param entity attributes # \param context broker to send to if context_id not in posted_history: posted_history[context_id] = {} commands = [] attr2Send = [] current = time.time() * 1000 for attribute in attributes: if attribute["name"] not in posted_history[context_id]: posted_history[context_id][attribute["name"]] = 0 if (current - posted_history[context_id][attribute["name"]] ) > PUBLISH_FREQUENCY: commands.append(attribute["name"]) attr2Send.append(attribute) posted_history[context_id][attribute["name"]] = current if len(commands) > 0: if (sendCommand): attr2Send.insert(0, { "name": "COMMAND", "type": "COMMAND", "value": commands }) data = { "contextElements": [{ "id": context_id, "type": datatype, "isPattern": "false", "attributes": attr2Send }], "updateAction": "APPEND" } url = "http://{}:{}/NGSI10/updateContext".format( connection["ADDRESS"], connection["PORT"]) data_json = json.dumps(data) try: request = urllib2.Request(url, data_json, { 'Content-Type': 'application/json', 'Accept': 'application/json' }) response = urllib2.urlopen(request) response_body = json.loads(response.read()) response.close() if "errorCode" in response_body: rospy.logerr("Error sending data to Context Broker:") rospy.logerr(response_body["errorCode"]["details"]) except Exception as ex: Log("ERROR", ex.reason) return None
def subscribe(self, namespace, data_type, robot): ## \brief Subscribe to entities' changes # \param entity name # \param entity type # \param robot object if namespace not in self.subscriptions: registerContext(namespace, data_type, robot) topics = robot["publisher"].keys() Log( "INFO", "Subscribing on context broker to " + data_type + " " + namespace + " and topics: " + str(topics)) subscription = { "namespace": namespace, "data_type": data_type, "topics": topics } url = "http://{}:{}/NGSI10/subscribeContext".format( DATA_CONTEXTBROKER["ADDRESS"], DATA_CONTEXTBROKER["PORT"]) subscriber_json = json.dumps( self._generateSubscription(namespace, data_type, topics)) response_body = self._sendRequest(url, subscriber_json) if response_body is not None: if "subscribeError" in response_body: Log("ERROR", "Error Subscribing to Context Broker:") Log( "ERROR", response_body["subscribeError"]["errorCode"] ["details"]) os.kill(os.getpid(), signal.SIGINT) else: subscription["id"] = response_body["subscribeResponse"][ "subscriptionId"] self.subscriptions[namespace] = subscription Log( "INFO", "Connected to Context Broker with id {}".format( subscription["id"])) if self.refresh_thread is None: self.refresh_thread = thread.start_new_thread( self._refreshSubscriptions, ( "CBSub-Refresh", 2, ))
def getRobots(refresh=False): ''' This retrieves the current configuration from FIROS. Here we load the `robots.json` and the 'whitelist.json' In case the `whitelist.json` is Empty: We do get the current robots from ROS and are adding them as subscribers (by default). In a small ROS-World this usually is no problem. But in an environment with many robots we might send a lot of data. ''' try: # Retrieves the whitelist.json. If it does not exists, it returns all topics. robots = copy.deepcopy(RosConfigurator.systemTopics(refresh)) # Retrieves the robots.json. robots_json = getRobotsByJson() if len(robots_json) == 0: Log( "ERROR", "The file 'robots.json' is either empty or does not exist!\n\nExiting" ) sys.exit(1) # Merge robots.json into whitelist.json (overwrite if neccessary) for robot_name in robots_json: robot_name = str(robot_name) if robot_name not in robots: robots[robot_name] = {"topics": {}} for topic_name in robots_json[robot_name]["topics"]: topic = robots_json[robot_name]["topics"][topic_name] # Overwrite or add! robots[robot_name]["topics"][str(topic_name)] = { "msg": str(topic["msg"]), "type": str(topic["type"]) } return robots except Exception as e: traceback.print_exc() Log("ERROR", e) return {}
def onConnect(request, action): ''' This resets firos into its original state TODO DL reset, instead of connect? TODO DL Add real connect for only one Robot? ''' Log("INFO", "Connecting topics") loadMsgHandlers(RosConfigurator.systemTopics(True)) # Return Success end_request(request, None, 200, "")
def launchSetup(main=False): retcode = 0 Log("INFO", "\nStarting Firos setup...") Log("INFO", "---------------------------------\n") Log("INFO", "\nGenerating Message Description Files\n") try: robots = confManager.getRobots(True, True) current_path = os.path.dirname(os.path.abspath(__file__)) outdir = os.path.join(current_path, "include/ros/") retcode = genpy_firos.genmain(robots, generator.MsgGenerator(genmsg.msg_loader.load_msg_from_string), outdir) Log("INFO", "\nSuccesfully generated\n") if main: sys.exit(retcode or 0) except Exception as e: rospy.logerr("\nSomething wrong happened\n") traceback.print_exc() Log("ERROR", e) sys.exit(retcode or 0)
def deleteEntity(self, namespace, data_type, removeContext=True): ## \brief Delete entity from context broker # \param entity name # \param entity type Log("INFO", "DELETING: ", namespace, data_type) operation_json = json.dumps({ "contextElements": [{ "type": data_type, "isPattern": "false", "id": namespace }], "updateAction": "DELETE" }) url = "http://{}:{}/NGSI10/updateContext".format( DATA_CONTEXTBROKER["ADDRESS"], DATA_CONTEXTBROKER["PORT"]) response_body = self._sendRequest(url, operation_json) if response_body is not None: if "errorCode" in response_body: Log("ERROR", "Error deleting entity") Log("ERROR", response_body["errorCode"]["details"]) elif "orionError" in response_body: Log("ERROR", "Error deleting entity") Log("ERROR", response_body["orionError"]["details"]) else: Log("INFO", "Deleted entity " + namespace) if removeContext: deleteContext(namespace, True)
def _responseCheck(self, response, attrAction=0, topEnt=None): ''' Check if Response is ok (2XX and some 3XX). If not print an individual Error. response: the actual response attrAction: One of [0, 1, 2] which maps to -> [Creation, Update, Deletion] topEnt: the String of an Entity or a topic, which was used ''' if not response.ok: if attrAction == 0: Log( "WARNING", "Could not create Entitiy {} in Contextbroker :".format( topEnt)) Log("WARNING", response.content) elif attrAction == 1: Log( "ERROR", "Cannot update attributes in Contextbroker for topic: {} :" .format(topEnt)) Log("ERROR", response.content) else: Log( "WARNING", "Could not delete Entitiy {} in Contextbroker :".format( topEnt)) Log("WARNING", response.content)
def robotDisconnection(data): ## \brief Handle robot diconnection # \param robot data dict (name) robot_name = data.data Log("INFO", "Disconnected robot: " + robot_name) if robot_name in ROBOT_TOPICS: CloudSubscriber.deleteEntity(robot_name, DEFAULT_CONTEXT_TYPE) CloudSubscriber.disconnect(robot_name, True) for topic in ROBOT_TOPICS[robot_name]["publisher"]: ROBOT_TOPICS[robot_name]["publisher"][topic]["publisher"].unregister() for topic in ROBOT_TOPICS[robot_name]["subscriber"]: ROBOT_TOPICS[robot_name]["subscriber"][topic]["subscriber"].unregister() RosConfigurator.removeRobot(robot_name)
def start(self): ''' This start the HTTPServer and notifies thread_event ''' # Get Port and HostName and save port sa = self.httpd.socket.getsockname() self.port = sa[1] Log("INFO", "\nListening for Context-Broker-Messages on: ", C.MAP_SERVER_ADRESS, ":", sa[1]) # Notify and start handling Requests self.thread_event.set() while not self.stopped: self.httpd.handle_request()
def loadMsgHandlers(robot_data): ## \brief Load ROS publishers/subscribers based on robot data # \param robot data Log("INFO", "Getting configuration data") Log("INFO", "Generating topic handlers:") msg_types = {} for robotName in robot_data: robotName = str(robotName) robot = robot_data[robotName] if robotName not in ROBOT_TOPICS: ROBOT_TOPICS[robotName] = { "publisher": {}, "subscriber": {} } Log("INFO", " -" + robotName) for topicName in robot['topics']: topic = robot['topics'][topicName] topicName = str(topicName) Log("INFO", " -" + topicName) extra = {"robot": robotName, "topic": topicName} if type(topic['msg']) is dict: _topic_name = robotName + ".msg." + topicName module = LibLoader.loadFromFile(os.path.join(TOPIC_BASE_PATH, robotName+topicName+".py")) theclass = getattr(module, topicName) else: _final_name = topic['msg'].split('.')[-1] _topic_name = str(topic['msg']) if _final_name in globals(): theclass = globals()[_final_name] else: theclass = LibLoader.loadFromSystem(topic['msg']) extra["type"] = _topic_name msg_types[_topic_name] = { "name": _topic_name, "type": "rosmsg", "value": json.dumps(ros2Definition(theclass())).replace('"', SEPARATOR_CHAR) } if topic["type"].lower() == "publisher": ROBOT_TOPICS[robotName]["publisher"][topicName] = { "msg": str(topic['msg']), "class": theclass, "publisher": rospy.Publisher(robotName + "/" + topicName, theclass, queue_size=DEFAULT_QUEUE_SIZE) } elif topic["type"].lower() == "subscriber": ROBOT_TOPICS[robotName]["subscriber"][topicName] = { "msg": str(topic['msg']), "class": theclass, "subscriber": rospy.Subscriber(robotName + "/" + topicName, theclass, _callback, extra) } Log("INFO", "\n") CloudSubscriber.subscribe(robotName, DEFAULT_CONTEXT_TYPE, ROBOT_TOPICS[robotName]) Log("INFO", "Subscribed to " + robotName + "'s topics\n") CloudPublisher.publishMsg(msg_types.values()) MapHandler.mapPublisher()
def subscribe(self, topicList, topicTypes, msgDefintions): ''' topicList: A list of topics msgDefintions: The Messages-Definitions from ROS This method only gets called once (or multiple times, if we get a reset!)! So we need to make sure, that in this file 'RosTopicHandler.publish' is called somehow independently after some Signal arrived (from elsewhere). Keep in mind that Firos can get a Reset-Signal, in this case, this method is called again. Make sure that this method can get called multiple times! In this Context-Broker-Subscriber we spawn ONE HTTP-Base-Server in another Thread, which will be used, so that the Context-Broker can notify us after it received a Message. In addition to that, the Context-Broker needs to know how to notify us. This is solved by adding subscriptions into the Context-Broker, which we need to manually maintain. Again we start a Thread for each Topic which handles the Subscriptions So After everything is set up, Firos can be notified, explicitly here the method: """CBServer.CBHandler.do_post""" is invoked by Notification. This method handles the Conversion back into a conform "ROS-Message". After we did the Conversion, we simply need to call """RosTopicHandler.publish""" ''' # Do nothing if no Configuratuion if self.noConf: return ### Start the HTTPServer and wait until it is ready! if not self.serverIsRunning: server_ready = threading.Event() self.server = CBServer(server_ready) thread.start_new_thread(self.server.start, ()) self.serverIsRunning = True server_ready.wait() # If not already subscribed, start a new thread which handles the subscription for each topic for an robot. # And only If the topic list is not empty! for topic in topicList: if topic not in self.subscriptionIds: Log( "INFO", "Subscribing on Context-Broker to topics: " + str(list(topicList))) thread.start_new_thread( self.subscribeThread, (topic, topicTypes, msgDefintions)) #Start Thread via subscription
def getWhiteList(restore=False): ''' This retrieves the Whitelist.json Configuration and overwrites the data, depending on the restore-Flag ''' global whitelist if whitelist == {} or restore: json_path = C.PATH + "/whitelist.json" whitelist = json.load(open(json_path)) if len(whitelist) == 0: Log( "WARNING", "The 'whitelist.json' was not set. You might want to use a whitelist to avoid subscribing to every existing topic!" ) return whitelist
def _init_searchpath_for_available_msgs_on_system(): ''' Initializes the systempath for genpy for available Messages. To achieve this, we use the Environment-Variable 'ROS_PACKAGE_PATH' which usually should be available. ''' if len(LibLoader.systemPath) != 0: # SystemPath is already initialized return if "ROS_PACKAGE_PATH" not in os.environ: Log( "WARNING", "The ENV 'ROS_PACKAGE_PATH' is not set. Unable to search for Messages on this system" ) pkgs_paths = os.environ.get('ROS_PACKAGE_PATH').split(":") searchpath = dict() for pkg_path in pkgs_paths: # Retrieve all folder and Files from pkg_path which ends with ".msg" result = [ os.path.join(dp, f) for dp, dn, filenames in os.walk(pkg_path) for f in filenames if os.path.splitext(f)[1] == '.msg' ] for found_msg in result: # found_msg is like '/opt/.../share/PACKAGE_NAME/msg/THE_MESSAGE.msg' # with very few exceptions (in case CMakeLists.txt tells Catkin to look for the Messages in another Folder) # TODO DL Maybe we can look inside the parent folders for the nearest CMakeLists.txt-File. The pkg_name is then the # folder name, where the CMakeLists.txt-File is (always the case?) if "/firos/" in found_msg: # Skip the Project itself continue path_parts = found_msg.split("/") pkg_name = path_parts[path_parts.index("msg") - 1] if pkg_name in searchpath: # Skip already added package continue # Retrieve Package Folder and add it to the searchpath path = '/'.join(path_parts[0:path_parts.index("msg")]) + "/msg" searchpath[pkg_name] = [path] LibLoader.systemPath = searchpath
def _robotDisconnection(data): ''' Unregisters from a given robotID by a ROBOT data: The String which was sent to firos ''' robotID = str(data.data) Log("INFO", "Disconnected robot: " + robotID) if robotID in ROS_PUBLISHER: for topic in ROS_PUBLISHER[robotID]: ROS_PUBLISHER[robotID][topic].unregister() del ROS_PUBLISHER[robotID] if robotID in ROS_SUBSCRIBER: for topic in ROS_SUBSCRIBER[robotID]: ROS_SUBSCRIBER[robotID][topic].unregister() del ROS_SUBSCRIBER[robotID]