def at_waypoint_cb(name_of_waypoint):
config.logger.debug("at_waypoint callback called with %s" %
name_of_waypoint)
x, y, ig1, ig2 = config.bot_cont.gazebo.get_bot_state()
config.tasks_finished.append(
DoneTasksfinished(x=x,
y=y,
sim_time=config.sim_time,
name=name_of_waypoint))
# get the next default plan
ct = config.tasks[0]
config.tasks = config.tasks[1:]
if ct != name_of_waypoint:
config.logger.debug(
"Task list in config and reported waypoint differ! %s vs %s"
% (ct, name_of_waypoint))
config.logger.debug("Task list is %s" % config.tasks)
if len(config.tasks) > 0:
config.plan = config.instruction_db.get_path(
name_of_waypoint, config.tasks[0])
if config.th_connected:
comms.send_status("at-waypoint callback", "at-waypoint")
else:
rospy.loginfo("at-waypoint")
rospy.loginfo(config.tasks_finished[-1])
def at_waypoint_cb(name_of_waypoint):
config.logger.debug("at_waypoint callback called with %s" % name_of_waypoint)
x , y , ig1 , ig2 = config.bot_cont.gazebo.get_bot_state()
config.tasks_finished.append(DoneTasksfinished(x=x,
y=y,
sim_time=rospy.Time.now().secs,
name=name_of_waypoint))
comms.send_status("at-waypoint callback", "at-waypoint")
def online_learning():
'''
This funciton is called aynchoniously by invoke_online_learning()
'''
global config
global comms
config.logger.debug("online-learning-started")
if config.th_connected:
comms.send_status("default_controller",
"online-learning-started",
sendxy=True,
sendtime=True)
try:
config.learner.start_online_learning()
except Exception as e:
config.logger.debug(
"online learning raised an exception; notifying the TH and then crashing"
)
comms.save_ps("learning_error")
if config.th_connected:
## copy out logs before posting error
if config.uuid and config.th_connected:
comms.sequester()
config.thApi.error_post(
Errorparams(error="learning-error",
message="exception raised: %s" % e))
else:
rospy.logerr("learning-error")
raise e
config.logger.debug("online-learning-done")
if config.th_connected:
comms.send_status("default_controller",
"online-learning-done",
sendxy=True,
sendtime=True)
config.learner.update_config_files()
# let's print the list of configurations the learner founds for debugging
with open(config.learner.config_list_file, 'r') as confg_file:
print("**Predicted**")
config_data = json.load(confg_file)
print(config_data)
with open(config.learner.config_list_file_true, 'r') as confg_file:
print("**True**")
config_data = json.load(confg_file)
print(config_data)
def worker():
rospy.sleep(5)
comms.send_status("__main__ in level %s" % ready_resp.level,
"live",
sendtime=False)
if config.th_connected:
## copy out logs before posting error
if config.uuid and config.th_connected:
comms.sequester()
thApi.error_post(
Errorparams(error="parsing-error",
message="exception raised: %s" % e))
else:
rospy.logerr("parsing-error")
raise e
if (ready_resp.level == "c") or (ready_resp.level == "d"):
logger.debug("offline-learning-started")
if config.th_connected:
comms.send_status("__main__",
"offline-learning-started",
sendxy=False,
sendtime=False)
try:
result = model_learner.start_learning()
except Exception as e:
logger.debug(
"learning raised an exception; notifying the TH and then crashing"
)
comms.save_ps("learning_error")
if config.th_connected:
## copy out logs before posting error
if config.uuid and config.th_connected:
comms.sequester()
logger.debug("writing checked /ready message to ~/ready")
fo = open(os.path.expanduser('~/ready'), 'w')
fo.write('%s' %ready_resp) #todo: this may or may not be JSON; check once we can run it
fo.close()
config.level = ready_resp.level
if ready_resp.level == "c":
try:
learner.Learn.get_true_model()
except Exception as e:
logger.debug("parsing raised an exception; notifying the TH and then crashing")
thApi.error_post(Errorparams(error="parsing-error",message="exception raised: %s" % e))
raise e
comms.send_status("__main__", "learning-started", False)
try:
result = learner.Learn.start_learning()
except Exception as e:
logger.debug("learning raised an exception; notifying the TH and then crashing")
thApi.error_post(Errorparams(error="learning-error",message="exception raised: %s" % e))
raise e
comms.send_status("__main__", "learning-done", False)
learner.Learn.dump_learned_model()
## ros launch
# Init me as a node
logger.debug("initializing cp1_ta ros node")
rospy.init_node("cp1_ta")
cp1_utils.init("cp1_ta")
def internal_post(CP1InternalStatus): # noqa: E501
"""internal_post
reports any internal status (including the error that may occured) from the backend that might be sent to the TA for internal bookeeping or forwarding to the TH # noqa: E501
:param CP1InternalStatus:
:type CP1InternalStatus: dict | bytes
:rtype: None
"""
if connexion.request.is_json:
CP1InternalStatus = CP1InternalStatus.from_dict(connexion.request.get_json()) # noqa: E501
config.logger.debug("TA internal status end point hit with status %s and message %s"
% (CP1InternalStatus.status, CP1InternalStatus.message))
if CP1InternalStatus.status == "learning-started":
config.logger.debug("internal got a deprecated status which is being ignored")
elif CP1InternalStatus.status == "learning-done":
config.logger.debug("internal got a deprecated status which is being ignored")
elif CP1InternalStatus.status == "adapt-started":
comms.send_status("internal", "adapt-started")
elif CP1InternalStatus.status == "adapt-done":
comms.send_status("internal", "adapt-done")
elif CP1InternalStatus.status == "charging-started":
comms.send_status("internal", "charging-started")
elif CP1InternalStatus.status == "charging-done":
comms.send_status("internal", "charging-done")
elif CP1InternalStatus.status == "parsing-error":
config.logger.debug("internal got a deprecated status which is being ignored")
elif CP1InternalStatus.status == "learning-error":
config.logger.debug("internal got a deprecated status which is being ignored")
elif CP1InternalStatus.status == "other-error":
config.logger.debug("sending error to the TH because of message %s" % CP1InternalStatus.message)
resp = config.thApi.error_post(Errorparams(error="other-error",message=CP1InternalStatus.message))
## these are the literal constants that come from rainbow. the
## constants above are from the API definition; there's some
## overlap and this is a little messy
elif CP1InternalStatus.status == "RAINBOW_READY":
comms.send_status("internal, rainbow ready in level %s" % config.ready_resp.level, "live", False)
elif CP1InternalStatus.status == "MISSION_SUCCEEDED":
config.logger.debug("internal got a rainbow mission message which is being ignored")
elif CP1InternalStatus.status == "MISSION_FAILED":
config.logger.debug("internal got a rainbow mission message which is being ignored")
elif CP1InternalStatus.status == "ADAPTING":
comms.send_status("internal", "adapt-started")
elif CP1InternalStatus.status == "ADAPTED":
comms.send_status("internal", "adapt-done")
elif CP1InternalStatus.status == "ADAPTED_FAILED":
comms.send_status("internal, adapted_failed", "adapt-done")
def internal_status_post(CP1InternalStatus): # noqa: E501
"""internal_status_post
reports any internal status (including the error that may occured) from the backend that might be sent to the TA for internal bookeeping or forwarding to the TH # noqa: E501
:param CP1InternalStatus:
:type CP1InternalStatus: dict | bytes
:rtype: None
"""
config.logger.debug("Call to internal-status")
success = True
try:
cp1_internal_status = CP1InternalStatus
if connexion.request.is_json:
config.logger.debug("Attempting from_dict")
cp1_internal_status = CP1IS.from_dict(
connexion.request.get_json()) # noqa: E501
config.logger.debug(
"TA internal status end point hit with status %s and message %s" %
(cp1_internal_status.status, cp1_internal_status.message))
if cp1_internal_status.status == "learning-started":
config.logger.debug(
"internal got a deprecated status which is being ignored")
elif cp1_internal_status.status == "learning-done":
config.logger.debug(
"internal got a deprecated status which is being ignored")
elif cp1_internal_status.status == "adapt-started":
comms.send_status("internal", "adapt-started")
elif cp1_internal_status.status == "adapt-done":
comms.send_status("internal", "adapt-done")
elif cp1_internal_status.status == "charging-started":
comms.send_status("internal", "charging-started")
elif cp1_internal_status.status == "charging-done":
comms.send_status("internal", "charging-done")
elif cp1_internal_status.status == "parsing-error":
config.logger.debug(
"internal got a deprecated status which is being ignored")
elif cp1_internal_status.status == "learning-error":
config.logger.debug(
"internal got a deprecated status which is being ignored")
elif cp1_internal_status.status == "other-error":
config.logger.debug(
"sending error to the TH because of message %s" %
cp1_internal_status.message)
# copy out logs before posting error
if config.uuid and config.th_connected:
comms.sequester()
resp = config.thApi.error_post(
Errorparams(error="other-error",
message=cp1_internal_status.message))
# these are the literal constants that come from rainbow. the
# constants above are from the API definition; there's some
# overlap and this is a little messy
elif cp1_internal_status.status == "RAINBOW_READY":
comms.send_status("internal, rainbow ready in level %s" %
config.ready_response.level,
"live",
sendxy=False)
elif cp1_internal_status.status == "MISSION_SUCCEEDED":
config.logger.debug(
"internal got a rainbow mission message which is being ignored"
)
elif cp1_internal_status.status == "MISSION_FAILED":
config.logger.debug(
"internal got a rainbow mission message which is being ignored"
)
elif cp1_internal_status.status == "ADAPTING":
comms.send_status("internal", "adapt-started")
elif cp1_internal_status.status == "ADAPTED":
comms.send_status("internal", "adapt-done")
elif cp1_internal_status.status == "ADAPTED_FAILED":
comms.send_status("internal, adapted_failed", "adapt-done")
elif cp1_internal_status.status == "PLAN":
config.plan = [
x.strip()
for x in ast.literal_eval(cp1_internal_status.message)
]
if not cp1_internal_status.sim_time == -1:
config.logger.debug(
"[WARN] ta got an internal plan status with sim_time %s, which is out of spec"
% cp1_internal_status.sim_time)
elif cp1_internal_status.status == "learning-requested":
success = invoke_online_learning()
except Exception as e:
config.logger.debug("Internal status got an exception: %s" % e)
ret = InternalStatusResponse200(success)
config.logger.debug("%s " % ret)
return ret