diff --git a/scripts/include/contextbroker/cbPublisher.py b/scripts/include/contextbroker/cbPublisher.py index e99f72f..6371349 100644 --- a/scripts/include/contextbroker/cbPublisher.py +++ b/scripts/include/contextbroker/cbPublisher.py @@ -21,79 +21,126 @@ __status__ = "Developement" import os -import copy - import json -import rospy import requests from include.logger import Log from include.constants import DATA_CONTEXTBROKER from include.FiwareObjectConverter.objectFiwareConverter import ObjectFiwareConverter + +CB_BASE_URL = "http://{}:{}/v2/entities/".format(DATA_CONTEXTBROKER["ADDRESS"], DATA_CONTEXTBROKER["PORT"]) +CB_HEADER = {'Content-Type': 'application/json'} + class CbPublisher(object): + ''' The CbPublisher handles the Enities on CONTEXT_BROKER / v2 / entities . + It creates not creaed Entities and updates their attributes via 'publishToCB'. + On Shutdown the tracked Entities are deleted. + + Also the rawMsg is converted here via the Object Converter + + THIS IS THE ONLY FILE WHICH OPERATES ON /v2/entities + ''' + + # Keeps track of the posted Content on the ContextBroker + # via posted_history[ROBOT_ID][TOPIC] posted_history = {} - def publishToCB(self, robotID, topic, datatype, rawMsg, msgDefintionDict): - #TODO DL set publish frequency! + def publishToCB(self, robotID, topic, rawMsg, msgDefintionDict): + ''' This is the actual publish-Routine which updates and creates Entities on the + ContextBroker. It also keeps track via posted_history on already posted entities and topics + + robotID: A string corresponding to the Robot-Id + topic: Also a string, corresponding to the topic of the robot + rawMsg: the raw data directly obtained from rospy + msgDefintionDict: The Definition as obtained FROM ros2Definition TODO DL + + TODO DL During Runtime an Entitiy might get deleted, check it here! + TODO DL set publish frequency + ''' # if struct not initilized, intitilize it even on ContextBroker! if robotID not in self.posted_history: self.posted_history[robotID] = {} self.posted_history[robotID]['type'] = 'ROBOT' self.posted_history[robotID]['id'] = robotID + # Intitialize Entitiy/Robot-Construct on ContextBroker jsonStr = ObjectFiwareConverter.obj2Fiware(self.posted_history[robotID], ind=0, ignorePythonMetaData=True) - # TODO Error Checking! - url = "http://{}:{}/v2/entities".format(DATA_CONTEXTBROKER["ADDRESS"], DATA_CONTEXTBROKER["PORT"]) - response = requests.post(url, data=jsonStr, headers={'Content-Type': 'application/json'}) + response = requests.post(CB_BASE_URL, data=jsonStr, headers=CB_HEADER) + self._responseCheck(response, attrAction=0, topEnt=robotID) if topic not in self.posted_history[robotID]: self.posted_history[robotID][topic] = {} - # Check if descriptions are already added + # Check if descriptions are already added, if not execute again with descriptions! if 'descriptions' not in self.posted_history[robotID]: - self.posted_history[robotID]['descriptions'] = self.loadDescriptions(robotID) + self.posted_history[robotID]['descriptions'] = self._loadDescriptions(robotID) + if self.posted_history[robotID]['descriptions'] is not None: + self.publishToCB(robotID, 'descriptions', self.posted_history[robotID]['descriptions'], None) # check if previous posted topic type is the same, iff not, we do not post it to the context broker - if self.posted_history[robotID][topic] != {} and rawMsg._type != self.posted_history[robotID][topic]._type: - Log("WARNING", "Received Msg-Type '{}' but expected '{}' on Topic '{}'".format(rawMsg._type, self.posted_history[robotID][topic]._type, topic)) + if (self.posted_history[robotID][topic] != {} and topic != "descriptions" + and rawMsg._type != self.posted_history[robotID][topic]._type): + Log("ERROR", "Received Msg-Type '{}' but expected '{}' on Topic '{}'".format(rawMsg._type, self.posted_history[robotID][topic]._type, topic)) return # Replace previous rawMsg with current one self.posted_history[robotID][topic] = rawMsg - # Set Definition-Dict TODO DL do not rebuild every definition Dict! + # Set Definition-Dict definitionDict = {} definitionDict[topic] = msgDefintionDict completeJsonStr = ObjectFiwareConverter.obj2Fiware(self.posted_history[robotID], ind=0, dataTypeDict=definitionDict, ignorePythonMetaData=True) - # format json, so that the contextbroker accept it. + # format json, so that the contextbroker accepts it. partJsonStr = json.dumps({ topic: json.loads(completeJsonStr)[topic] }) - # TODO Error Checking! - url = "http://{}:{}/v2/entities/{}/attrs".format(DATA_CONTEXTBROKER["ADDRESS"], DATA_CONTEXTBROKER["PORT"], robotID) - response = requests.post(url, data=partJsonStr, headers={'Content-Type': 'application/json'}) + # Update attribute on ContextBroker + response = requests.post(CB_BASE_URL + robotID + "/attrs", data=partJsonStr, headers=CB_HEADER) + self._responseCheck(response, attrAction=1, topEnt=topic) def unpublishALLFromCB(self): - # Removes all on start-up created entities on shutdown - puburl = "http://{}:{}/v2/entities/".format(DATA_CONTEXTBROKER["ADDRESS"], DATA_CONTEXTBROKER["PORT"]) + ''' Removes all previously tracked Entities/Robots on ContextBroker + ''' for robotID in self.posted_history: - response = requests.delete(puburl + robotID) - # TODO DL ERRORCHecking! + response = requests.delete(CB_BASE_URL + robotID) + self._responseCheck(response, attrAction=2, topEnt=robotID) - def loadDescriptions(self, robotID): - # TODO DL refactor and no hardcoded reference to file! + def _loadDescriptions(self, robotID): + ''' Simply load the descriptions from the 'robotdescriptions.json'-file and + return its value + + robotID: The Robot-Id-String + ''' + # TODO DL no hardcoded reference to files! current_path = os.path.dirname(os.path.abspath(__file__)) json_path = current_path.replace("scripts/include/contextbroker", "config/robotdescriptions.json") description_data = json.load(open(json_path)) + # Check if a robotID has descriptions if robotID in description_data: if 'descriptions' in description_data[robotID]: return description_data[robotID]['descriptions'] - return None \ No newline at end of file + return None + + + 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/Robot {} in Contextbroker. Maybe it already exists?".format(topEnt)) + elif attrAction == 1: + Log("ERROR", "Cannot update attributes in Contextbroker for topic: {}".format(topEnt)) + else: + Log("WARNING", "Could not delete Entitiy/Robot {} in Contextbroker.".format(topEnt)) \ No newline at end of file diff --git a/scripts/include/contextbroker/cbQueryBuilder.py b/scripts/include/contextbroker/cbQueryBuilder.py index 919b427..27c7b3b 100644 --- a/scripts/include/contextbroker/cbQueryBuilder.py +++ b/scripts/include/contextbroker/cbQueryBuilder.py @@ -25,6 +25,7 @@ class CbQueryBuilder(object): # TODO DL used in requestHandler ## \brief Query data to context broker def findById(self, entity_id, data_type="ROBOT", isPattern=False): + # TODO DL change to v2, maybe not working? ## \brief Get entity data from context broker # \param entity name (can be regular expression) # \param entity type diff --git a/scripts/include/contextbroker/cbSubscriber.py b/scripts/include/contextbroker/cbSubscriber.py index 5f7c164..77c98aa 100644 --- a/scripts/include/contextbroker/cbSubscriber.py +++ b/scripts/include/contextbroker/cbSubscriber.py @@ -23,100 +23,142 @@ import time import thread import requests +import json from include.FiwareObjectConverter.objectFiwareConverter import ObjectFiwareConverter - -from include.constants import * +from include.constants import DATA_CONTEXTBROKER, IP, SERVER_PORT from include.logger import Log +CB_BASE_URL = "http://{}:{}".format(DATA_CONTEXTBROKER["ADDRESS"], DATA_CONTEXTBROKER["PORT"]) +FIROS_NOTIFY_URL = "http://{}:{}/firos".format(IP, SERVER_PORT) # TODO DL HTTP? + class CbSubscriber(object): - ## \brief Context broker subscription handler - subscriptions = {} # TODO DL remove? - subscriptionThreads = {} + ''' The CbSubscriber handles the subscriptions on the ContextBroker. + Only the url CONTEXT_BROKER / v2 / subcriptions is used here! + As on CbPublisher on shutdown all subscriptions are deleted form + ContextBroker. + + this Objects also converts the received data from ContextBroker + back into a Python-Object. + + Here each topic of an robot is subscribed seperately. + + THIS IS THE ONLY FILE WHICH OPERATES ON /v2/subscriptions + ''' + + # Saves the subscriptions IDs returned from ContextBroker. + # Follwoing Structure: subscriptionIds[ROBOT_ID][TOPIC] returns a sub-Id in String subscriptionIds = {} - refresh_thread = None - def subscribeToCB(self, robotID, topic): - # If not already subscribed, start a new thread which handles the subscription + def subscribeToCB(self, robotID, topicList): + ''' This method starts for each topic an own thread, which handles the subscription + + robotID: The string of the robotID + topicList: A list of topics, corresponding to the robotID + ''' + # 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! - if robotID not in self.subscriptions and topic: - Log("INFO", "Subscribing on context broker to " + robotID + " and topics: " + str(topic)) - self.subscriptionThreads[robotID] = thread.start_new_thread(self.subscribeThread, (robotID, topic)) - #Start Thread via subscription + if robotID not in self.subscriptionIds and topicList: + Log("INFO", "Subscribing on Context-Broker to " + robotID + " and topics: " + str(topicList)) + self.subscriptionIds[robotID] = {} + for topic in topicList: + thread.start_new_thread(self.subscribeThread, (robotID, topic)) #Start Thread via subscription def unsubscribeALLFromCB(self): - suburl = "http://{}:{}".format(DATA_CONTEXTBROKER["ADDRESS"], DATA_CONTEXTBROKER["PORT"]) + ''' Simply unsubscribed from all tracked subscriptions + ''' for robotID in self.subscriptionIds: - response = requests.delete(suburl + self.subscriptionIds[robotID]) - # TODO DL Error Handling! + for topic in self.subscriptionIds[robotID]: + response = requests.delete(CB_BASE_URL + self.subscriptionIds[robotID][topic]) + self._checkResponse(response, subID=self.subscriptionIds[robotID][topic]) + + def subscribeThread(self, robotID, topic): + ''' A Subscription-Thread. It Life-Cycle is as follows: + -> Subscribe -> Delete old Subs-ID -> Save new Subs-ID -> Wait -> - def subscribeThread(self, robotID, topicList): - # Subscribe New, Unsubscribe old and sleep. If time has passed re-do! - suburl = "http://{}:{}".format(DATA_CONTEXTBROKER["ADDRESS"], DATA_CONTEXTBROKER["PORT"]) - + robotID: A string corresponding to the robotID + topic: The Topic (string) to subscribe to. + ''' while True: # Subscribe - jsonData = self.subscribeJSONGenerator(robotID, topicList) - response = requests.post(suburl + "/v2/subscriptions", data=jsonData, headers={'Content-Type': 'application/json'}) + jsonData = self.subscribeJSONGenerator(robotID, topic) + response = requests.post(CB_BASE_URL + "/v2/subscriptions", data=jsonData, headers={'Content-Type': 'application/json'}) + self._checkResponse(response, created=True, robTop=(robotID, topic)) + newSubID = response.headers['Location'] # <- get subscription-ID # Unsubscribe - if robotID in self.subscriptionIds: - response = requests.delete(suburl + self.subscriptionIds[robotID]) - self.subscriptionIds[robotID] = newSubID - - #Sleep + if robotID in self.subscriptionIds and topic in self.subscriptionIds[robotID]: + response = requests.delete(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(290) # sleep TODO DL from config loaded seconds - Log("INFO", "Refreshing Subscription for " + robotID + " and topics: " + str(topicList)) - - - # TODO DL Error Handling! + Log("INFO", "Refreshing Subscription for " + robotID + " and topics: " + str(topic)) + def subscribeJSONGenerator(self, robotID, topic): + ''' This method returns the correct JSON-format to subscribe to the ContextBroker. + The Expiration-Date/Throttle and Type of robots is retreived here via configuration (TODO DL) - def subscribeJSONGenerator(self, robotID, topicList): - # This method generates the JSON for the 'v2/subscriptions'-API of Orion CB + robotID: The String of the Robot-Id. + topic: The actual topic to subscribe to. + ''' + # This struct correspondes to following JSON-format: + # https://fiware-orion.readthedocs.io/en/master/user/walkthrough_apiv2/index.html#subscriptions struct = { "subject": { "entities": [ { "id": str(robotID), - "type": "ROBOT" # Type is currently always a robot! #TODO DL load from cfg + "type": "ROBOT" # TODO DL load via Configuration } ], "condition": { - "attrs": topicList + "attrs": [str(topic)] } }, "notification": { "http": { - "url": "http://{}:{}/firos".format(IP, SERVER_PORT) # TODO DL HTTP? + "url": FIROS_NOTIFY_URL }, - "attrs": topicList + "attrs": [str(topic)] }, - "expires": time.strftime("%Y-%m-%dT%H:%M:%S.00Z", time.gmtime(time.time() + 300)) # TODO DL set expire in Config, ISO 8601, 300 secs + "expires": time.strftime("%Y-%m-%dT%H:%M:%S.00Z", time.gmtime(time.time() + 300)) # TODO DL load via Configuration, ISO 8601 # "throttling": 5 # TODO DL Maybe throttle? } return json.dumps(struct) + def convertReceivedDataFromCB(self, jsonData): + ''' This parses the Input Back into a TypeValue-object via the + Object-Converter. This method is here to uniform the Obejct-Conversions + in CbPublisher and CbSubscriber - def receivedData(self, robotID, topic, jsonData): - # parse the Input and let the TopicHandler (TODO DL remove from there) - # publish the data back into ROS + topic: The topic, which should be converted. + the topic should have "id", "type" and "TOPIC" in it + ''' kv = self.TypeValue() ObjectFiwareConverter.fiware2Obj(jsonData, kv, setAttr=True, useMetadata=False) return kv - ## DL - Back Parsing Stub Class - class TypeValue(object): - def __init__(self): - pass - # TODO DL remove wrapper! - def subscribe(self, namespace, data_type, robot): - # unused -> data_type - self.subscribeToCB(namespace, robot["publisher"].keys()) + class TypeValue(object): + ''' A Stub-Object to parse the received data + ''' + + + def _checkResponse(self, response, robTop=None, subID=None, created=False): + ''' RESPONSE TODO DL + ''' + if not response.ok: + if created: + Log("ERROR", "Could not create subscription for Robot {} and topic {} in Context-Broker".format(robTop[0], robTop[1])) + else: + Log("WARNING", "Could not delete subscription {} from Context-Broker".format(subID)) diff --git a/scripts/include/logger.py b/scripts/include/logger.py index 3179558..2575279 100644 --- a/scripts/include/logger.py +++ b/scripts/include/logger.py @@ -67,7 +67,6 @@ def Log(level, *args): if level == 'CRITICAL': _logger.critical(text) elif level == 'ERROR': - print "error" + text _logger.error(text) elif level == 'WARNING': _logger.warning(text) diff --git a/scripts/include/ros/rosutils.py b/scripts/include/ros/rosutils.py deleted file mode 100644 index 9ad8674..0000000 --- a/scripts/include/ros/rosutils.py +++ /dev/null @@ -1,71 +0,0 @@ -# MIT License -# -# Copyright (c) <2015> -# -# Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files -# (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, -# publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, -# subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF -# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE -# FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION -# WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - -__author__ = "Dominik Lux" -__credits__ = ["Peter Detzner"] -__maintainer__ = "Dominik Lux" -__version__ = "0.0.1a" -__status__ = "Developement" - -import importlib - -def obj2RosViaStruct(obj, dataStruct): - # TODO DL ERROR HANDLING! and refactor! - if 'type' in dataStruct and 'value' in dataStruct: - # Load Message-Class - l = dataStruct['type'].split(".") # see Fiware-Object-Coverter, explicit Types of ROS-Messages are retreived from there - moduleLoader = importlib.import_module(l[0] + ".msg") - msgClass = getattr(moduleLoader, l[1]) - instance = msgClass() - for attr in msgClass.__slots__: - if attr in obj and attr in dataStruct['value']: - # Check if obj AND dataStruct contains attr - if type(dataStruct['value'][attr]) is list: - l =[] - for it in range(len(dataStruct['value'][attr])): - l.append(obj2RosViaStruct(obj[attr][it], dataStruct['value'][attr][it])) - setattr(instance, attr, l) - else: - setattr(instance, attr, obj2RosViaStruct(obj[attr], dataStruct['value'][attr])) - return instance - else: - # Struct is {}: - if type(obj) is dict: - # if it is still a dict, convert into an Object with attributes - t = Temp() - for k in obj: - setattr(t, k, obj[k]) - return t - else: - # something more simple (int, long, float), return it - return obj - - -class Temp(object): - pass - -# TODO DL are Definitions still somewhere used? -def ros2Definition(msgInstance): - ## \brief Generate Ros object definition - # \param ROS Object instance - obj = {} - for key, t in zip(msgInstance.__slots__, msgInstance._slot_types): - attr = getattr(msgInstance, key) - if hasattr(attr, '__slots__'): - obj[key] = ros2Definition(attr) - else: - obj[key] = t - return obj diff --git a/scripts/include/ros/topicHandler.py b/scripts/include/ros/topicHandler.py index 1de8540..7afcbb7 100644 --- a/scripts/include/ros/topicHandler.py +++ b/scripts/include/ros/topicHandler.py @@ -22,6 +22,7 @@ import os import rospy +import importlib from include.logger import Log @@ -29,7 +30,6 @@ from include.libLoader import LibLoader from include.ros.rosConfigurator import RosConfigurator -from include.ros.rosutils import ros2Definition, obj2RosViaStruct from include.ros.dependencies.third_party import * # PubSub Handlers @@ -50,6 +50,9 @@ subscribers = [] topic_DataTypeDefinition = {} # filled in loadMsgHandlers with infos about the concrete Datatype, +# Actual ROS-classes used for instanciateROSMessage +ROS_MESSAGE_CLASSES = {} + def loadMsgHandlers(robot_data): ## \brief Load ROS publishers/subscribers based on robot data @@ -105,7 +108,8 @@ def loadMsgHandlers(robot_data): } Log("INFO", "\n") - CloudSubscriber.subscribe(robotName, DEFAULT_CONTEXT_TYPE, ROBOT_TOPICS[robotName]) + # CloudSubscriber.subscribe(robotName, DEFAULT_CONTEXT_TYPE, ROBOT_TOPICS[robotName]) + CloudSubscriber.subscribeToCB(str(robotName), ROBOT_TOPICS[robotName]["publisher"].keys()) Log("INFO", "Subscribed to " + robotName + "'s topics\n") @@ -124,7 +128,7 @@ def publishDirect(robotID, topic, obj, dataStruct): if robotID in ROBOT_TOPICS and topic in ROBOT_TOPICS[robotID]["publisher"]: instance = ROBOT_TOPICS[robotID]["publisher"][topic] if instance["class"]._type.replace("/", ".") == dataStruct['type']: # check if received and expected type are equal! - newMsg = obj2RosViaStruct(obj, dataStruct) # TODO DL Refactor ROBOT_TOPICS + newMsg = instanciateROSMessage(obj, dataStruct) # TODO DL Refactor ROBOT_TOPICS if "publisher" in instance: instance["publisher"].publish(newMsg) @@ -144,11 +148,85 @@ def unregisterAll(): def _publishToCBRoutine(data, args): # this Routine is executed on every received ROS-Message + # TODO DL topic_DataTypeDefinition can be omitted via ROS_MESSAGE_CLASSES robot = str(args['robot']) topic = str(args['topic']) datatype = ROBOT_TOPICS[robot]["subscriber"][topic]["msg"] - CloudPublisher.publishToCB(robot, topic, datatype, data, topic_DataTypeDefinition[datatype]) + CloudPublisher.publishToCB(robot, topic, data, topic_DataTypeDefinition[datatype]) + + + + + +#TODO Testing +def instanciateROSMessage(obj, dataStruct): + ''' This method instanciates via obj and dataStruct the actual ROS-Message like + "geometry_msgs.Twist". Explicitly it loads the ROS-Message-class (if not already done) + with the dataStruct["type"] if given and recursively sets all attributes of the Message. + ''' + # Check if type and value in datastruct, if not we default to a priimitive value + if 'type' in dataStruct and 'value' in dataStruct: + + # Load Message-Class only once! + if dataStruct['type'] not in ROS_MESSAGE_CLASSES: + msgType = dataStruct['type'].split(".") # see Fiware-Object-Coverter, explicit Types of ROS-Messages are retreived from there + moduleLoader = importlib.import_module(msgType[0] + ".msg") + msgClass = getattr(moduleLoader, msgType[1]) + ROS_MESSAGE_CLASSES[dataStruct['type']] = msgClass + #Instanciate Message + instance = ROS_MESSAGE_CLASSES[dataStruct['type']]() + + + for attr in ROS_MESSAGE_CLASSES[dataStruct['type']].__slots__: + # For each attribute in Message + if attr in obj and attr in dataStruct['value']: + # Check iff obj AND dataStruct contains attr + if type(dataStruct['value'][attr]) is list: + l =[] + for it in range(len(dataStruct['value'][attr])): + l.append(instanciateROSMessage(obj[attr][it], dataStruct['value'][attr][it])) + setattr(instance, attr, l) + else: + setattr(instance, attr, instanciateROSMessage(obj[attr], dataStruct['value'][attr])) + return instance + else: + # Struct is {}: + if type(obj) is dict: + # if it is still a dict, convert into an Object with attributes + t = Temp() + for k in obj: + setattr(t, k, obj[k]) + return t + else: + # something more simple (int, long, float), return it + return obj + + +class Temp(object): + ''' A Temp-object, to convert from a Dictionary into an object with attributes. + ''' + pass + + +# TODO DL are Definitions still somewhere used? +def ros2Definition(msgInstance): + ## \brief Generate Ros object definition + # \param ROS Object instance + obj = {} + for key, t in zip(msgInstance.__slots__, msgInstance._slot_types): + attr = getattr(msgInstance, key) + if hasattr(attr, '__slots__'): + obj[key] = ros2Definition(attr) + else: + obj[key] = t + return obj + + + + + + @@ -161,8 +239,6 @@ def robotDisconnection(data): 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"]: diff --git a/scripts/include/server/requestHandler.py b/scripts/include/server/requestHandler.py index 5705730..3a22005 100644 --- a/scripts/include/server/requestHandler.py +++ b/scripts/include/server/requestHandler.py @@ -28,7 +28,7 @@ from include.logger import Log from include.confManager import getRobots -from include.ros.rosutils import ros2Definition +from include.ros.topicHandler import ros2Definition from include.ros.rosConfigurator import RosConfigurator, setWhiteList from include.ros.topicHandler import TopicHandler, loadMsgHandlers, ROBOT_TOPICS # TODO DL Refactor! from include.contextbroker.cbSubscriber import CbSubscriber @@ -131,7 +131,7 @@ def requestFromCB(request, action): for topic in topics: if topic != 'id' and topic != 'type': dataStruct = buildTypeStruct(data[topic]) - obj = CloudSubscriber.receivedData(data['id'], topic, jsonData) + obj = CloudSubscriber.convertReceivedDataFromCB(jsonData) TopicHandler.publishDirect(data['id'], topic, getattr(obj, topic), dataStruct) diff --git a/scripts/test.py b/scripts/test.py deleted file mode 100755 index fc64788..0000000 --- a/scripts/test.py +++ /dev/null @@ -1,102 +0,0 @@ -#!/usr/bin/env python - -# MIT License -# -# Copyright (c) <2015> -# -# Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files -# (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, -# publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, -# subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF -# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE -# FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION -# WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - -import rospy -import time - -from include.constants import * -from include.pubsub.pubSubFactory import PublisherFactory - -from include.ros.rosutils import * - -from std_msgs.msg import String - -# Main function. -if __name__ == '__main__': - # Initialize the node and name it. - rospy.init_node('firos_testing') - - print "\nStarting Firos Testing..." - print "---------------------------------\n" - - initial = False - wait = True - - if initial: - initial_pos = "robot1" - ending_pos = "robot2" - else: - initial_pos = "robot2" - ending_pos = "robot1" - - - CloudPublisher = PublisherFactory.create() - # CloudPublisher.publish("talker2", DEFAULT_CONTEXT_TYPE, [{"name": "move", "type": "NotYet", "value": {"x_pose": time.time(), "y_pose": 56, "z_pose": 32}}]) - # CloudPublisher.publish("talker2", DEFAULT_CONTEXT_TYPE, [{"name": "talk", "type": "NotYet", "value": "Hola mundo!!!" + str(time.time())}]) - - # print CloudPublisher.createContent("cmd_vel", "NotYet", {"linear": {"x": 5.5, "y": 0.0, "z": 0.0},"angular": {"x": 0.0, "y": 0.0, "z": 0.0}}) - # CloudPublisher.publish("robot2", DEFAULT_CONTEXT_TYPE, [CloudPublisher.createContent("cmd_vel_mux/input/teleop", "NotYet", {"linear": {"x": -0.5, "y": 0.0, "z": 0.0},"angular": {"x": 0.0, "y": 0.0, "z": 0.0}})]) - CloudPublisher.publish("turtle1", DEFAULT_CONTEXT_TYPE, [CloudPublisher.createContent("cmd_vel", "NotYet", {"linear": {"x": -1.5, "y": 0.0, "z": 0.0},"angular": {"x": 0.0, "y": 0.0, "z": 0.0}})]) - # text = String() - # text.data = "Hello topic" - # CloudPublisher.publish("myrobot", DEFAULT_CONTEXT_TYPE, [CloudPublisher.createContent("topic", "NotYet", ros2Obj(text))]) - - - # CloudPublisher.publish(initial_pos, DEFAULT_CONTEXT_TYPE, [CloudPublisher.createContent("move_base_simple/goal", "NotYet", { - # "header": { - # "seq": 0, - # "stamp": "now", - # "frame_id": "map" - # }, - # "pose": { - # "position": { - # "x": 0.8, - # "y": 0.8, - # "z": 0.0 - # }, - # "orientation": { - # "w": 1.0 - # } - # } - # })]) - - # if wait: - # time.sleep(1.5) - - # CloudPublisher.publish(ending_pos, DEFAULT_CONTEXT_TYPE, [CloudPublisher.createContent("move_base_simple/goal", "NotYet", { - # "header": { - # "seq": 0, - # "stamp": "now", - # "frame_id": "map" - # }, - # "pose": { - # "position": { - # "x": -2.5, - # "y": -0.8, - # "z": 0.0 - # }, - # "orientation": { - # "w": 1.0 - # } - # } - # })]) - - # if wait: - # time.sleep(1) - -