import rclpy
from rclpy.node import Node
from core.cognitive_node import CognitiveNode
import random
import numpy
from std_msgs.msg import Int64
from core.service_client import ServiceClient, ServiceClientAsync
from cognitive_node_interfaces.srv import GetActivation, SetActivation, Execute
from cognitive_node_interfaces.msg import Episode
from core.utils import perception_dict_to_msg, class_from_classname
[docs]
class Policy(CognitiveNode):
"""
Policy class.
"""
def __init__(self, name='policy', class_name='cognitive_nodes.policy.Policy', publisher_msg = None, publisher_topic = None, **params):
"""
Constructor for the Policy class.
Initializes a policy with the given name and registers it in the LTM.
It also creates a service for executing the policy.
:param name: The name of the policy.
:type name: str
:param class_name: The name of the Policy class.
:type class_name: str
:param publisher_msg: The publisher message to publicate the execution of the policy.
:type publisher: str
:param publisher_topic: The publisher topic to publicate the execution of the policy.
:type publisher: str
"""
super().__init__(name, 'cognitive_nodes.policy.Policy', **params)
self.set_activation_service = self.create_service(
SetActivation,
'policy/' + str(name) + '/set_activation',
self.set_activation_callback,
callback_group=self.cbgroup_server
)
self.execute_service = self.create_service(
Execute,
'policy/' + str(name) + '/execute',
self.execute_callback,
callback_group=self.cbgroup_server
)
episodes_topic = getattr(self, "Control", {}).get("episodes_topic", "")
self.episode_publisher = self.create_publisher(Episode, episodes_topic, 0)
self.configure_activation_inputs(self.neighbors)
[docs]
async def calculate_activation(self, perception=None, activation_list=None):
"""
Calculate the activation level of the policy by obtaining that of its neighboring CNodes
As in CNodes, an arbitrary perception can be propagated, calculating the final policy activation for that perception.
:param perception: Arbitrary perception.
:type perception: dict
:param activation_list: List of activations of the neighbors.
:type activation_list: list
:return: The activation of the Policy and its timestamp.
:rtype: cognitive_node_interfaces.msg.Activation
"""
if activation_list==None:
cnodes = [neighbor["name"] for neighbor in self.neighbors if neighbor["node_type"] == "CNode"]
if cnodes:
cnode_activations = []
for cnode in cnodes:
perception_msg = perception_dict_to_msg(perception)
service_name = 'cognitive_node/' + str(cnode) + '/get_activation'
if not service_name in self.node_clients:
self.node_clients[service_name] = ServiceClientAsync(self, GetActivation, service_name, self.cbgroup_client)
activation = await self.node_clients[service_name].send_request_async(perception = perception_msg)
cnode_activations.append(activation.activation)
self.activation.activation = float(numpy.max(cnode_activations))
else:
self.activation.activation = 0.0
self.activation.timestamp =self.get_clock().now().to_msg()
self.get_logger().debug(self.node_type + " activation for " + self.name + " = " + str(self.activation))
else:
if activation_list:
self.calculate_activation_max(activation_list)
else:
self.activation.activation=0.0
self.activation.timestamp=self.get_clock().now().to_msg()
return self.activation
[docs]
def execute_callback(self, request, response):
"""
Placeholder for the execution of the policy.
:param request: The request to execute the policy.
:type request: cognitive_node_interfaces.srv.Execute.Request
:param response: The response indicating the executed policy.
:type response: cognitive_node_interfaces.srv.Execute.Response
:raise NotImplementedError: This method should be implemented in subclasses.
"""
raise NotImplementedError
[docs]
def set_activation_callback(self, request, response):
"""
CNodes can modify a policy's activation.
:param request: The request that contains the new activation value.
:type request: cognitive_node_interfaces.srv.SetActivation.Request
:param response: The response indicating if the activation was set.
:type response: cognitive_node_interfaces.srv.SetActivation.Response
:return: The response indicating if the activation was set.
:rtype: cognitive_node_interfaces.srv.SetActivation.Response
"""
activation = request.activation
self.get_logger().info('Setting activation ' + str(activation) + '...')
self.activation.activation = activation
self.activation.timestamp = self.get_clock().now().to_msg()
response.set = True
return response
[docs]
class PolicyAsync(Policy):
"""
PolicyAsync class. Represents a policy that does not wait for completion of the execution.
"""
def __init__(self, name='policy', class_name='cognitive_nodes.policy.Policy', publisher_msg=None, publisher_topic=None, **params):
"""
Constructor for the PolicyAsync class.
:param name: The name of the policy.
:type name: str
:param class_name: The name of the base Policy class.
:type class_name: str
:param publisher_msg: The publisher message to publicate the execution of the policy.
:type publisher: str
:param publisher_topic: The publisher topic to publicate the execution of the policy.
:type publisher: str
"""
super().__init__(name, class_name, publisher_msg, publisher_topic, **params)
self.publisher_msg = publisher_msg
self.publisher = self.create_publisher(class_from_classname(publisher_msg), publisher_topic, 0)
[docs]
def execute_callback(self, request, response):
"""
Method that publishes the policy that must be exectuted, there should be a node that reads this message and executes the actual policy.
It logs the execution and returns the policy name in the response.
:param request: The request to execute the policy.
:type request: cognitive_node_interfaces.srv.Execute.Request
:param response: The response indicating the executed policy.
:type response: cognitive_node_interfaces.srv.Execute.Response
:return: The response with the executed policy name.
:rtype: cognitive_node_interfaces.srv.Execute.Response
"""
self.get_logger().info('Executing policy: ' + self.name + '...')
msg = class_from_classname(self.publisher_msg)()
msg.data = self.name
self.publisher.publish(msg)
response.policy = self.name
return response
[docs]
class PolicyBlocking(Policy):
"""
PolicyBlocking class. Represents a policy that waits for completion of the execution.
"""
def __init__(self, name='policy', class_name='cognitive_nodes.policy.Policy', service_msg=None, service_name=None, **params):
"""
Constructor for the PolicyBlocking class.
:param name: The name of the policy.
:type name: str
:param class_name: The name of the base Policy class.
:type class_name: str
:param service_msg: Message type of the service that executes the policy.
:type service_msg: ROS2 message type. Typically cognitive_node_interfaces.srv.Policy
:param service_name: Name of the service that executes the policy.
:type service_name: str
"""
super().__init__(name, class_name, service_msg, service_name, **params)
self.service_msg=service_msg
self.service_name=service_name
self.policy_service=ServiceClientAsync(self, class_from_classname(service_msg), service_name, self.cbgroup_client)
[docs]
async def execute_callback(self, request, response):
"""
Makes a service call to the server that handles the execution of the policy.
:param request: The request to execute the policy.
:type request: cognitive_node_interfaces.srv.Execute.Request
:param response: The response indicating the executed policy.
:type response: cognitive_node_interfaces.srv.Execute.Response
:return: The response with the executed policy name.
:rtype: cognitive_node_interfaces.srv.Execute.Response
"""
self.get_logger().info('Executing policy: ' + self.name + '...')
await self.policy_service.send_request_async(policy=self.name)
response.policy = self.name
return response
def main(args=None):
rclpy.init(args=args)
policy = Policy()
rclpy.spin(policy)
policy.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()