from rclpy.time import Time
import os
import yaml
import threading
from core.service_client import ServiceClient
from cognitive_node_interfaces.msg import Activation
from cognitive_node_interfaces.srv import SendSpace, SaveModel
from cognitive_node_interfaces.msg import SuccessRate
from core_interfaces.srv import GetNodeFromLTM
from core.utils import perception_msg_to_dict, separate_perceptions
from cognitive_nodes.episodic_buffer import EpisodicBuffer
from cognitive_nodes.episode import episode_msg_to_obj
[docs]
class File():
"""A MDB file."""
def __init__(self, ident, file_name, node, **params):
"""Init attributes when a new object is created."""
self.ident = ident
self.file_name = file_name
self.file_object = None
self.node = node
def __getstate__(self):
"""
Return the object to be serialize with PyYAML as the result of removing the unpicklable entries.
:return: A dictionary representing the serializable state of the object.
:rtype: dict
"""
state = self.__dict__.copy()
del state["file_object"]
return state
[docs]
def close(self):
"""Close de underlying file."""
if self.file_object:
self.file_object.close()
[docs]
def write(self):
"Method that writes the data in the file"
raise NotImplementedError
[docs]
def write_episode(self, msg):
"Writes data related to an episode"
return None
def _write_file(self, data):
"""Write data to the file."""
if self.file_object:
self.file_object.write(data)
self.file_object.flush()
[docs]
class FileGoodness(File):
"""A file where several goodness statistics about an experiment are stored."""
[docs]
def write(self):
"""Write statistics data."""
formatted_goals = {goal: f"{reward:.1f}" for goal, reward in sorted(self.node.current_episode.reward_list.items())}
current_world = self.node.current_world if self.node.current_world else "None"
self._write_file(
str(self.node.iteration)
+ "\t"
+ current_world
+ "\t"
+ str(f"{formatted_goals}")
+ "\t"
+ self.node.current_policy
+ "\t"
+ str(self.node.sensorial_changes_val)
+ "\t"
+ str(self.node.n_cnodes)
+ "\n"
)
[docs]
class FilePNodesSuccess(File):
"""A file that records whether a P-node's activation has been successful or not."""
[docs]
def write(self):
"""Write success."""
for pnode, success in self.node.pnodes_success.items():
if success is not None:
self._write_file(
str(self.node.iteration)
+ "\t"
+ pnode
+ "\t"
+ str(success)
+ "\n"
)
self.node.pnodes_success[pnode] = None
[docs]
class FileTrialsSuccess(File):
"""A file that records whether a P-node's activation has been successful or not."""
[docs]
def write(self):
"""Write success."""
for iteration, trial, iterations, success in self.node.trials_data:
self._write_file(
str(iteration)
+ "\t"
+ str(trial)
+ "\t"
+ str(iterations)
+ "\t"
+ str(success)
+ "\n"
)
self.file_object.flush()
self.node.trials_data = []
[docs]
class FilePNodesContent(File):
"""A file that saves the contents of the P-nodes."""
def __init__(self, ident, file_name, node, save_interval=100, **params):
super().__init__(ident, file_name, node, **params)
self.save_interval = save_interval
[docs]
def create_pnode_client(self, pnode_name):
"""
Create client to request P-Node's space.
:param pnode_name: Name of the P-Node.
:type pnode_name: str
"""
if pnode_name not in self.created_clients:
pnode_client = ServiceClient(SendSpace, 'pnode/' + str(pnode_name) + '/send_space')
self.created_clients[pnode_name] = pnode_client
[docs]
def write(self):
"""Writes P-Nodes contents."""
write_needed = (self.save_interval > 0 and self.node.iteration % self.save_interval == 0) or (self.node.iteration == self.node.iterations)
if "PNode" in self.node.LTM_cache and write_needed:
for pnode in self.node.LTM_cache["PNode"]:
if pnode not in self.created_clients:
self.create_pnode_client(pnode)
if self.created_clients[pnode]:
response = self.created_clients[pnode].send_request()
labels = response.labels
if labels:
if not self.header_finished:
self.finish_header(labels)
if labels == self.labels:
data = response.data
confidences = response.confidences
j = 0
for confidence in confidences:
self._write_file(str(self.node.iteration) + "\t")
self._write_file(pnode + "\t")
for i in range(j, len(labels)+j):
self._write_file(str(data[i]) + "\t")
self._write_file(str(confidence) + "\n")
j = j + len(labels)
else:
self._write_file("ERROR. LABELS DO NOT MATCH BETWEEN PNODES\n")
[docs]
class FileLastIterationPNodesContent(FilePNodesContent):
"""A file that saves the contents of the P-nodes at the end of an experiment."""
[docs]
def write(self):
"""Writes P-Nodes contents"""
if "PNode" in self.node.LTM_cache and self.node.iteration == self.node.iterations:
for pnode in self.node.LTM_cache["PNode"]:
self.create_pnode_client(pnode)
if self.created_clients[pnode]:
response = self.created_clients[pnode].send_request()
labels = response.labels
if labels:
if not self.header_finished:
self.finish_header(labels)
if labels == self.labels:
data = response.data
confidences = response.confidences
j = 0
for confidence in confidences:
self._write_file(str(self.node.iterations) + "\t")
self._write_file(pnode + "\t")
for i in range(j, len(labels)+j):
self._write_file(str(data[i]) + "\t")
self._write_file(str(confidence) + "\n")
j = j + len(labels)
else:
self._write_file("ERROR. LABELS DO NOT MATCH BETWEEN PNODES.\n")
[docs]
class FileGoalsContent(File):
"""A file that saves the contents of the Goals at the end of an experiment."""
def __init__(self, ident, file_name, node, save_interval=100, **params):
super().__init__(ident, file_name, node, **params)
self.save_interval = save_interval
[docs]
def create_goal_client(self, goal_name):
"""
Create client to request Goal's space.
:param goal_name: Name of the Goal.
:type goal_name: str
"""
if goal_name not in self.created_clients:
goal_client = ServiceClient(SendSpace, 'goal/' + str(goal_name) + '/send_space')
self.created_clients[goal_name] = goal_client
[docs]
def write(self):
"""Writes Goals contents."""
if "Goal" in self.node.LTM_cache and self.node.iteration % self.save_interval == 0: #TODO Vary iterations
for goal in self.node.LTM_cache["Goal"]:
if goal not in self.created_clients:
self.create_goal_client(goal)
if self.created_clients[goal]:
response = self.created_clients[goal].send_request()
self.node.get_logger().info(f"Writing data for goal {goal}. Points: {len(response.confidences)}")
labels = response.labels
if labels:
if not self.header_finished:
self.finish_header(labels)
if labels == self.labels:
data = response.data
confidences = response.confidences
j = 0
for confidence in confidences:
self._write_file(str(self.node.iteration) + "\t")
self._write_file(goal + "\t")
for i in range(j, len(labels)+j):
self._write_file(str(data[i]) + "\t")
self._write_file(str(confidence) + "\n")
j = j + len(labels)
else:
self._write_file("ERROR. LABELS DO NOT MATCH BETWEEN GOALS\n")
[docs]
class FileLastIterationGoalsContent(FileGoalsContent):
"""A file that saves the contents of the Goals at the end of an experiment."""
[docs]
def write(self):
"""Writes Goals contents."""
if "Goal" in self.node.LTM_cache and self.node.iteration == self.node.iterations:
for goal in self.node.LTM_cache["Goal"]:
self.create_goal_client(goal)
if self.created_clients[goal]:
response = self.created_clients[goal].send_request()
labels = response.labels
if labels:
if not self.header_finished:
self.finish_header(labels)
if labels == self.labels:
data = response.data
confidences = response.confidences
j = 0
for confidence in confidences:
self._write_file(str(self.node.iterations) + "\t")
self._write_file(goal + "\t")
for i in range(j, len(labels)+j):
self._write_file(str(data[i]) + "\t")
self._write_file(str(confidence) + "\n")
j = j + len(labels)
else:
self._write_file("ERROR. LABELS DO NOT MATCH BETWEEN GOALS.\n")
else:
self.created_clients[goal] = None
[docs]
class FileNeighbors(File):
"""A file that saves the neighbors of each node (Method specific to track the subgoals created by effectance)."""
[docs]
def write(self):
"""Writes neighbors list."""
if self.node.iteration == self.node.iterations:
response = self.ltm_client.send_request(name="")
nodes = yaml.safe_load(response.data)
for goal in nodes['Goal']:
if 'reach' in goal or 'goal_' in goal:
self._write_file(str(goal) + "\t")
self._write_file(str(nodes['Goal'][goal]["neighbors"][0]["name"]) + "\t")
if 'reach' in goal:
self._write_file(str(nodes['Goal'][goal]["neighbors"][1]["name"]) + "\n")
else:
self._write_file("\n")
[docs]
class FileNeighborsFull(File):
"""A file that saves the full neighbor tree at the end of an experiment."""
[docs]
def write(self):
"""Writes neighbors list for each node."""
if self.node.iteration == self.node.iterations:
response = self.ltm_client.send_request(name="")
nodes = yaml.safe_load(response.data)
for goal in nodes['Goal']:
self._write_file(str(goal) + "\t")
self._write_file(str(nodes['Goal'][goal]["neighbors"]) + "\n")
[docs]
class FileEpisodesDataset(File):
"""A file that records the episodes published"""
def __init__(self, **kwargs):
super().__init__(**kwargs)
self.episodic_buffer = EpisodicBuffer(self.node, main_size=None, secondary_size=0, inputs=["old_perception", "action",
"parent_policy",
"perception", "reward_list"])
self.semaphore = threading.Semaphore()
[docs]
def write_episode(self, msg):
self.semaphore.acquire()
episode = episode_msg_to_obj(msg)
self.node.get_logger().debug(f"Received episode to write: {episode}")
self.episodic_buffer.add_episode(episode)
self.node.get_logger().debug(f"Episodic buffer size: {self.episodic_buffer.main_size}")
self.semaphore.release()
[docs]
def write(self):
return None
[docs]
def close(self):
self.semaphore.acquire()
dataframe = self.episodic_buffer.get_dataframes()[0]
if dataframe is not None:
dataframe.to_csv(self.file_object)
super().close()
self.semaphore.release()
[docs]
class FileDrivesPerceptions(File):
"""A file where several drive evaluation statistics about an experiment are stored."""
[docs]
def write(self):
"""Write the drive evaluation and missions."""
list_drives ={}
drives = self.node.LTM_cache["Drive"].items()
missions = self.node.LTM_cache["RobotPurpose"].items()
for drive, info in drives:
if "activation" in info:
d_activation = info["activation"]
for mission, n_info in missions:
drive_tag = drive.replace("_drive", "")
mission_tag = mission.replace("_mission", "")
if "activation" in n_info and drive_tag == mission_tag:
n_activation = n_info["activation"]
list_drives[drive] = ["evaluation: ", d_activation/n_activation if n_activation != 0 else 0.0]
formatted = {}
for key, value in self.node.perception_cache.items():
data_list = value.get("data", [])
if data_list:
clean_data = {
k: v for k, v in data_list[0].items()
}
formatted[key] = clean_data
self.file_object.write(
str(self.node.iteration)
+ "\t"
+ self.node.current_world
+ "\t"
+ str(list_drives)
+ "\t"
+ self.node.current_policy
+ "\t"
+ str(formatted)
+ "\t"
+ "\n"
)
[docs]
class FileWorldModelSuccess(File):
"""A file that records the success of the world model predictions."""
def __init__(self, ident, file_name, node, **params):
super().__init__(ident, file_name, node, **params)
self.subscriptions = {}
[docs]
def write(self):
# Check world model nodes in LTM cache
world_models = self.node.LTM_cache.get("WorldModel", {})
for wm_name in world_models:
if wm_name not in self.subscriptions:
self.subscriptions[wm_name] = self.node.create_subscription(
SuccessRate,
f'world_model/{wm_name}/prediction_error',
self.success_rate_callback,
1,
callback_group=self.node.cbgroup_client
)
def success_rate_callback(self, msg):
self._write_file(
str(self.node.iteration)
+ "\t"
+ msg.node_name
+ "\t"
+ f"{msg.success_rate:.4f}"
+ "\n"
)
[docs]
class FileSaveModels(File):
"""A file that triggers the saving of models in nodes."""
def __init__(self, ident, file_name, node, save_interval=100, **params):
super().__init__(ident, file_name, node, **params)
self.folder_path = None
self.save_model_clients = {}
self.save_interval = save_interval
self.node_type_mapping = {
"DeliberativeModel": "deliberative_model",
"PNode": "pnode",
"WorldModel": "world_model",
"UtilityModel": "utility_model"
}
self.consecutive_index = 0
[docs]
def write(self):
# Trigger save models in all nodes in LTM cache
write_needed = (self.save_interval > 0 and self.node.iteration % self.save_interval == 0) or (self.node.iteration == self.node.iterations)
if write_needed:
for node_type in self.node.LTM_cache:
if node_type in self.node_type_mapping:
service_prefix = self.node_type_mapping[node_type]
for node_name in self.node.LTM_cache[node_type]:
if node_name not in self.save_model_clients:
self.save_model_clients[node_name] = ServiceClient(
SaveModel,
f'{service_prefix}/{node_name}/save_model'
)
self.save_model_clients[node_name].send_request(prefix=f"{self.file_name}_{self.consecutive_index}/", suffix=f"_iter_{self.node.iteration}")
[docs]
def close(self):
return None