def listRobots(request, action):
''' Generates a list of all robots (depending on RosConfigurator, confManager)
and returns them back as json
TODO DL, currently a List of containing 'topics' with a list of topics is returned
Better would be a list of robotIds with their corresponding topics and types
'''
robots = getRobots(False)
data = []
for robot_name in robots.keys():
robot_data = {"name": robot_name, "topics": []}
robot = robots[robot_name]
for topic_name in robot["topics"]:
topic = robot["topics"][topic_name]
topic_data = {"name": topic_name, "pubsub": topic["type"]}
if type(topic["msg"]) is dict:
topic_data["type"] = "Custom"
topic_data["structure"] = topic["msg"]
else:
topic_data["type"] = topic["msg"]
topic_data["structure"] = ROS_TOPIC_AS_DICT[topic_name]
robot_data["topics"].append(topic_data)
data.append(robot_data)
# Return data and success
end_request(request, ('Content-Type', 'application/json'), 200,
json.dumps(data))
def onRobots(request, action):
## \brief Handle robot list request
# \param client request
# \param action
robots = getRobots(False, True)
data = []
for robot_name in robots.keys():
robot_data = {"name": robot_name, "topics": []}
robot = robots[robot_name]
for topic_name in robot["topics"]:
topic = robot["topics"][topic_name]
topic_data = {"name": topic_name, "pubsub": topic["type"]}
if type(topic["msg"]) is dict:
topic_data["type"] = "Custom"
topic_data["structure"] = topic["msg"]
else:
topic_data["type"] = topic["msg"]
topic_data["structure"] = ros2Definition(
ROBOT_TOPICS[robot_name][
topic["type"]][topic_name]["class"]())
robot_data["topics"].append(topic_data)
data.append(robot_data)
request.send_response(200)
request.send_header('Content-type', 'application/json')
setCors(request)
request.end_headers()
request.wfile.write(json.dumps(data))
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 listTopics(request, action):
''' Generates a list of all topics (depending on RosConfigurator, confManager)
and returns them back as json
'''
robots = getRobots(False)
data = []
for topic in robots.keys():
robot_data = {"topic": topic,
"pubSub": robots[topic][1],
"messageType": robots[topic][0] }
robot_data["structure"] = ROS_TOPIC_AS_DICT[topic]
data.append(robot_data)
# Return data and success
end_request(request, ('Content-Type', 'application/json'), 200, json.dumps(data))
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)
server = FirosServer("0.0.0.0", port)
except Exception as ex:
sys.stderr.write('CB_COMMUNICATION_FAILED')
exit(1)
else:
def signal_handler(signal, frame):
Log("INFO", ('\nExiting from the application'))
TopicHandler.unregisterAll()
topicManager.removeListeners()
server.close()
Log("INFO", ('\nExit'))
sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)
signal.signal(signal.SIGTERM, signal_handler)
launchSetup()
Log("INFO", "\nStarting Firos...")
Log("INFO", "---------------------------------\n")
loadMsgHandlers(confManager.getRobots(True, True))
connectionListeners()
topicManager.setListeners()
MapServer.load()
Log("INFO", "\nPress Ctrl+C to Exit\n")
server.start()
def __init__(self):
'''Initialize the class by creating the required publishers
and subscribers.
'''
Log("INFO", ('\nCreating a new FEATS handler...'))
self.firstRun = True
self.status = 'idle'
self.paused = False # stores whether the robot is in a paused state or not
# Get Orion configuration
self.configData = self.get_cb_config()
self.lastBattery = 0.0
self.idleGoal = False # this variable stores whether the robot is moving to an idle station or not
self.context_id = ""
self.workorder_id = ""
self.heartbeat_timer = None
# Init ROS publishers
self.routePlannerXYTPub = rospy.Publisher('/route_planner/goalXYT', Vector3, queue_size=3)
self.routePlannerPausePub = rospy.Publisher('/route_planner/cancel', String, queue_size=3)
self.routePlannerResumePub = rospy.Publisher('/route_planner/resume', String, queue_size=3)
self.locationPub = rospy.Publisher('/' + C.ROBOT_ID + '/location', Pose, queue_size=3)
self.statusPub = rospy.Publisher('/' + C.ROBOT_ID + '/status', String, queue_size=3)
self.batteryPub = rospy.Publisher('/' + C.ROBOT_ID + '/battery', Int32, queue_size=3, latch=True)
self.heartbeatPub = rospy.Publisher('/' + C.ROBOT_ID + '/heartbeat', String, queue_size=3)
self.connectionPub = rospy.Publisher('/' + C.ROBOT_ID + '/connection', Bool, queue_size=3, latch=True)
self.selfStatusPub = rospy.Publisher('/feats/status', String, queue_size=3)
# Init ROS subscribers
rospy.Subscriber('/battery/level', Float32, self.battery_cb)
rospy.Subscriber('/feats/status', String, self.status_cb)
rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, self.location_cb)
rospy.Subscriber('/charging/plugged', Bool, self.charging_cb)
rospy.Subscriber('/ui/goal/cancel', String, self.cancel_cb)
rospy.Subscriber('/ui/goal/resume', String, self.resume_cb)
rospy.Subscriber('/ui/ready', String, self.ready_cb)
## Set Configuration
data = self.configData['contextbroker']
if "address" not in data or "port" not in data:
raise Exception("No Context-Broker specified!")
self.data = data
self.CB_BASE_URL = "http://{}:{}/v2/entities/".format(data["address"], data["port"])
# Get topics (already with robot_id from config)
topics = confManager.getRobots(True)
# Subscribe to topics in topics.json
for key in topics:
if topics[key][1] == 'publisher':
Log("INFO", ('\nSubscribing to ' + key))
topic_type = key.split('/')[2]
if topic_type == 'refDestination':
rospy.Subscriber(key, String, self.ref_destination_cb)
elif topic_type == 'action':
rospy.Subscriber(key, String, self.action_cb)
Log("INFO", ('\nFEATS handler initialized!'))
# Send first heartbeat
self.send_heartbeat()
self.statusPub.publish(self.status)
# Main loop
self.loop()
try:
server = FirosServer("0.0.0.0", port)
except Exception as ex:
sys.stderr.write('CB_COMMUNICATION_FAILED')
exit(1)
else:
def signal_handler(signal, frame):
Log("INFO", ('\nExiting from the application'))
TopicHandler.unregisterAll()
topicManager.removeListeners()
server.close()
Log("INFO", ('\nExit'))
sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)
signal.signal(signal.SIGTERM, signal_handler)
launchSetup()
Log("INFO", "\nStarting Firos...")
Log("INFO", "---------------------------------\n")
loadMsgHandlers(confManager.getRobots(True, True))
connectionListeners()
topicManager.setListeners()
MapServer.load()
Log("INFO", "\nPress Ctrl+C to Exit\n")
server.start()
