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]