feat(components): add assignment methods

CHANGELOG.md

@@ -36,6 +36,8 @@ computations, without blocking the control loop.
 - fix(controllers): check for finite wrench values (#216)
 - feat(controllers): allow control type change after construction (#217)
 - feat(controllers): implement lock-free service wrappers for demanding callbacks (#218)
+- feat(interfaces): add assignment message definition (#220)
+- feat(components): add assignment methods (#224, #227)
 
 ## 5.2.3
 

source/modulo_components/include/modulo_components/ComponentInterface.hpp

@@ -17,6 +17,7 @@
 #include <modulo_core/exceptions.hpp>
 #include <modulo_core/translators/parameter_translators.hpp>
 
+#include <modulo_interfaces/msg/assignment.hpp>
 #include <modulo_interfaces/msg/predicate_collection.hpp>
 #include <modulo_interfaces/srv/empty_trigger.hpp>
 #include <modulo_interfaces/srv/string_trigger.hpp>
@@ -162,6 +163,34 @@ class ComponentInterface {
   virtual bool
   on_validate_parameter_callback(const std::shared_ptr<state_representation::ParameterInterface>& parameter);
 
+  /**
+   * @brief Add an assignment to the map of assignments.
+   * @tparam T The type of the assignment
+   * @param assignment_name the name of the associated assignment
+   */
+  template<typename T>
+  void add_assignment(const std::string& assignment_name);
+
+  /**
+   * @brief Set the value of an assignment.
+   * @tparam T The type of the assignment   
+   * @param assignment_name The name of the assignment to set
+   * @param assignment_value The value of the assignment
+   */
+  template<typename T>
+  void set_assignment(const std::string& assignment_name, const T& assignment_value);
+
+  /**
+   * @brief Get the value of an assignment.
+   * @tparam T The type of the assignment   
+   * @param assignment_name The name of the assignment to get
+   * @throws modulo_core::exceptions::InvalidAssignmentException if the assignment does not exist or the type does not 
+     match
+     @throws state_representation::exceptions::EmptyStateException if the assignment has not been set yet 
+   */
+  template<typename T>
+  T get_assignment(const std::string& assignment_name) const;
+
   /**
    * @brief Add a predicate to the map of predicates.
    * @param predicate_name the name of the associated predicate
@@ -471,7 +500,7 @@ class ComponentInterface {
   bool validate_parameter(const std::shared_ptr<state_representation::ParameterInterface>& parameter);
 
   /**
-   * @brief Populate a Prediate message with the name and the value of a predicate.
+   * @brief Populate a Predicate message with the name and the value of a predicate.
    * @param name The name of the predicate
    * @param value The value of the predicate
   */
@@ -554,6 +583,9 @@ class ComponentInterface {
   modulo_interfaces::msg::PredicateCollection predicate_message_;
   std::vector<std::string> triggers_;///< List of triggers
 
+  state_representation::ParameterMap assignments_map_;                                         ///< Map of assignments
+  std::shared_ptr<rclcpp::Publisher<modulo_interfaces::msg::Assignment>> assignment_publisher_;///< Assignment publisher
+
   std::map<std::string, std::shared_ptr<rclcpp::Service<modulo_interfaces::srv::EmptyTrigger>>>
       empty_services_;///< Map of EmptyTrigger services
   std::map<std::string, std::shared_ptr<rclcpp::Service<modulo_interfaces::srv::StringTrigger>>>
@@ -820,4 +852,79 @@ inline void ComponentInterface::publish_transforms(
         "Failed to send " << modifier << "transform: " << ex.what());
   }
 }
+
+template<typename T>
+inline void ComponentInterface::add_assignment(const std::string& assignment_name) {
+  std::string parsed_name = modulo_utils::parsing::parse_topic_name(assignment_name);
+  if (parsed_name.empty()) {
+    RCLCPP_ERROR_STREAM(
+        this->node_logging_->get_logger(),
+        "The parsed name for assignment '" + assignment_name
+            + "' is empty. Provide a string with valid characters for the assignment name ([a-z0-9_]).");
+    return;
+  }
+  if (assignment_name != parsed_name) {
+    RCLCPP_WARN_STREAM(
+        this->node_logging_->get_logger(),
+        "The parsed name for assignment '" + assignment_name + "' is '" + parsed_name
+            + "'. Use the parsed name to refer to this assignment.");
+  }
+  try {
+    this->assignments_map_.get_parameter(parsed_name);
+    RCLCPP_WARN_STREAM(
+        this->node_logging_->get_logger(), "Assignment with name '" + parsed_name + "' already exists, overwriting.");
+  } catch (const state_representation::exceptions::InvalidParameterException& ex) {
+    RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), "Adding assignment '" << parsed_name << "'.");
+  }
+  try {
+    assignments_map_.set_parameter(state_representation::make_shared_parameter<T>(parsed_name));
+  } catch (const std::exception& ex) {
+    RCLCPP_ERROR_STREAM_THROTTLE(
+        this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000,
+        "Failed to add assignment '" << parsed_name << "': " << ex.what());
+  }
+}
+
+template<typename T>
+void ComponentInterface::set_assignment(const std::string& assignment_name, const T& assignment_value) {
+  modulo_interfaces::msg::Assignment message;
+  std::shared_ptr<state_representation::ParameterInterface> assignment;
+  try {
+    assignment = this->assignments_map_.get_parameter(assignment_name);
+  } catch (const state_representation::exceptions::InvalidParameterException&) {
+    RCLCPP_ERROR_STREAM_THROTTLE(
+        this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000,
+        "Failed to set assignment '" << assignment_name << "': Assignment does not exist.");
+    return;
+  }
+  try {
+    assignment->set_parameter_value<T>(assignment_value);
+  } catch (const state_representation::exceptions::InvalidParameterCastException&) {
+    RCLCPP_ERROR_STREAM_THROTTLE(
+        this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000,
+        "Failed to set assignment '" << assignment_name << "': Incompatible value type.");
+    return;
+  }
+  message.node = this->node_base_->get_fully_qualified_name();
+  message.assignment = modulo_core::translators::write_parameter(assignment).to_parameter_msg();
+  this->assignment_publisher_->publish(message);
+}
+
+template<typename T>
+T ComponentInterface::get_assignment(const std::string& assignment_name) const {
+  std::shared_ptr<state_representation::ParameterInterface> assignment;
+  try {
+    assignment = this->assignments_map_.get_parameter(assignment_name);
+  } catch (const state_representation::exceptions::InvalidParameterException&) {
+    throw modulo_core::exceptions::InvalidAssignmentException(
+        "Failed to get value of assignment '" + assignment_name + "': Assignment does not exist.");
+  }
+  try {
+    return assignment->get_parameter_value<T>();
+  } catch (const state_representation::exceptions::InvalidParameterCastException&) {
+    auto expected_type = state_representation::get_parameter_type_name(assignment->get_parameter_type());
+    throw modulo_core::exceptions::InvalidAssignmentException(
+        "Incompatible type for assignment '" + assignment_name + "' defined with type '" + expected_type + "'.");
+  }
+}
 }// namespace modulo_components

source/modulo_components/modulo_components/component_interface.py

@@ -10,6 +10,7 @@
 import modulo_core.translators.message_writers as modulo_writers
 import state_representation as sr
 from geometry_msgs.msg import TransformStamped
+from modulo_interfaces.msg import Assignment as AssignmentMsg
 from modulo_interfaces.msg import Predicate as PredicateMsg
 from modulo_interfaces.msg import PredicateCollection
 from modulo_interfaces.srv import EmptyTrigger, StringTrigger
@@ -46,6 +47,7 @@ def __init__(self, node_name: str, *args, **kwargs):
         super().__init__(node_name, *args, **node_kwargs)
         self.__step_lock = Lock()
         self.__parameter_dict: Dict[str, Union[str, sr.Parameter]] = {}
+        self.__assignment_dict: Dict[str, sr.Parameter] = {}
         self.__read_only_parameters: Dict[str, bool] = {}
         self.__pre_set_parameters_callback_called = False
         self.__set_parameters_result = SetParametersResult()
@@ -68,6 +70,9 @@ def __init__(self, node_name: str, *args, **kwargs):
         self.add_parameter(sr.Parameter("rate", 10.0, sr.ParameterType.DOUBLE),
                            "The rate in Hertz for all periodic callbacks")
 
+        self.__assignment_publisher = self.create_publisher(AssignmentMsg, "/assignments", self.__qos)
+        self.__assignment_message = AssignmentMsg()
+        self.__assignment_message.node = self.get_fully_qualified_name()
         self.__predicate_publisher = self.create_publisher(PredicateCollection, "/predicates", self.__qos)
         self.__predicate_message = PredicateCollection()
         self.__predicate_message.node = self.get_fully_qualified_name()
@@ -342,6 +347,70 @@ def add_predicate(self, name: str, predicate: Union[bool, Callable[[], bool]]) -
         except Exception as e:
             self.get_logger().error(f"Failed to add predicate '{name}': {e}")
 
+    def add_assignment(self, name: str, type: sr.ParameterType) -> None:
+        """
+        Add an assignment to the dictionary of assignments.
+
+        :param name: The name of the assignment
+        :param type: The type of the assignment
+        """
+        parsed_name = parse_topic_name(name)
+        if not parsed_name:
+            self.get_logger().error(
+                f"The parsed name for assignment '{name}' is empty. Provide a "
+                "string with valid characters for the assignment name ([a-z0-9_]).")
+            return        
+        if parsed_name != name:
+            self.get_logger().error(
+                f"The parsed name for assignment '{name}' is '{parsed_name}'. Use the parsed name "
+                "to refer to this assignment.")
+        if parsed_name in self.__assignment_dict.keys():
+            self.get_logger().warn(f"Assignment with name '{parsed_name}' already exists, overwriting.")
+        else:
+            self.get_logger().debug(f"Adding assignment '{parsed_name}'.")
+        try:
+            self.__assignment_dict[parsed_name] = sr.Parameter(parsed_name, type)
+        except Exception as e:
+            self.get_logger().error(f"Failed to add assignment '{parsed_name}': {e}")
+
+    def get_assignment(self, name: str) -> T:
+        """
+        Get the value of an assignment.
+
+        :param name: The name of the assignment to get
+        :raises InvalidAssignmentError: if the assignment does not exist
+        :raises EmptyStateError: if the assignment has not been set yet
+        :return: The value of the assignment, if the assignment exists and has been assigned
+        """
+        if name not in self.__assignment_dict.keys():
+            raise InvalidAssignmentError(f"Failed to get value of assignment '{name}': Assignment does not exist.")
+        if self.__assignment_dict[name].is_empty():
+            # TODO: remove after control libraries v9.3.1
+            raise sr.exceptions.EmptyStateError(f"{name} state is empty")
+        return self.__assignment_dict[name].get_value()
+
+    def set_assignment(self, name: str, value: T) -> None:
+        """
+        Set the value of an assignment.
+
+        :param name: The name of the assignment to set
+        :param value: The value of the assignment
+        """
+        if name not in self.__assignment_dict.keys():
+            self.get_logger().error(
+                f"Failed to set assignment '{name}': Assignment does not exist.", throttle_duration_sec=1.0)
+            return
+        try:
+            self.__assignment_dict[name].set_value(value)
+            ros_param = write_parameter(self.__assignment_dict[name])
+        except Exception as e:
+            self.get_logger().error(f"Failed to set assignment '{name}': {e}", throttle_duration_sec=1.0)
+            return
+
+        message = copy.copy(self.__assignment_message)
+        message.assignment = ros_param.to_parameter_msg()
+        self.__assignment_publisher.publish(message)
+
     def get_predicate(self, name: str) -> bool:
         """
         Get the value of the predicate given as parameter. If the predicate is not found or the callable function fails,

source/modulo_components/src/ComponentInterface.cpp

@@ -28,6 +28,9 @@ ComponentInterface::ComponentInterface(
       });
   this->add_parameter("rate", 10.0, "The rate in Hertz for all periodic callbacks", true);
 
+  this->assignment_publisher_ = rclcpp::create_publisher<modulo_interfaces::msg::Assignment>(
+      this->node_parameters_, this->node_topics_, "/assignments", this->qos_);
+
   this->predicate_publisher_ = rclcpp::create_publisher<modulo_interfaces::msg::PredicateCollection>(
       this->node_parameters_, this->node_topics_, "/predicates", this->qos_);
   this->predicate_message_.node = this->node_base_->get_fully_qualified_name();

source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp

@@ -21,6 +21,7 @@ class ComponentInterfacePublicInterface : public ComponentInterface {
   using ComponentInterface::add_input;
   using ComponentInterface::add_parameter;
   using ComponentInterface::add_predicate;
+  using ComponentInterface::add_assignment;
   using ComponentInterface::add_service;
   using ComponentInterface::add_static_tf_broadcaster;
   using ComponentInterface::add_tf_broadcaster;
@@ -30,6 +31,7 @@ class ComponentInterfacePublicInterface : public ComponentInterface {
   using ComponentInterface::declare_input;
   using ComponentInterface::declare_output;
   using ComponentInterface::empty_services_;
+  using ComponentInterface::get_assignment;
   using ComponentInterface::get_parameter;
   using ComponentInterface::get_parameter_value;
   using ComponentInterface::get_predicate;
@@ -44,13 +46,15 @@ class ComponentInterfacePublicInterface : public ComponentInterface {
   using ComponentInterface::parameter_map_;
   using ComponentInterface::periodic_outputs_;
   using ComponentInterface::predicates_;
+  using ComponentInterface::assignments_map_;
   using ComponentInterface::publish_output;
   using ComponentInterface::raise_error;
   using ComponentInterface::remove_input;
   using ComponentInterface::send_static_transform;
   using ComponentInterface::send_static_transforms;
   using ComponentInterface::send_transform;
   using ComponentInterface::send_transforms;
+  using ComponentInterface::set_assignment;
   using ComponentInterface::set_parameter_value;
   using ComponentInterface::set_predicate;
   using ComponentInterface::set_qos;

source/modulo_components/test/cpp/test_component_interface.cpp

@@ -7,6 +7,7 @@
 #include <modulo_utils/testutils/ServiceClient.hpp>
 
 #include "test_modulo_components/component_public_interfaces.hpp"
+#include "state_representation/exceptions/EmptyStateException.hpp"
 
 #include <sensor_msgs/msg/image.hpp>
 
@@ -41,6 +42,35 @@ class ComponentInterfaceTest : public ::testing::Test {
 using NodeTypes = ::testing::Types<rclcpp::Node, rclcpp_lifecycle::LifecycleNode>;
 TYPED_TEST_SUITE(ComponentInterfaceTest, NodeTypes);
 
+TYPED_TEST(ComponentInterfaceTest, AddAssignment) {
+  this->component_->template add_assignment<int>("an_assignment");
+  // adding an assignment with empty name should fail
+  EXPECT_NO_THROW(this->component_->template add_assignment<int>(""));
+  // adding an assignment with the same name should just overwrite
+  this->component_->template add_assignment<int>("an_assignment");
+  EXPECT_EQ(this->component_->assignments_map_.get_parameter_list().size(), 1);
+  // names should be cleaned up
+  EXPECT_NO_THROW(this->component_->template add_assignment<int>("7cleEaGn_AaSssiGNgn#ment"));
+  EXPECT_EQ(this->component_->assignments_map_.get_parameter_list().size(), 2);
+  // names without valid characters should fail
+  EXPECT_NO_THROW(this->component_->template add_assignment<int>("@@@@@@"));
+  EXPECT_EQ(this->component_->assignments_map_.get_parameter_list().size(), 2);
+}
+
+TYPED_TEST(ComponentInterfaceTest, GetSetAssignment) {
+  this->component_->template add_assignment<int>("int_assignment");
+
+  EXPECT_THROW(this->component_->template get_assignment<int>("non_existent"), modulo_core::exceptions::InvalidAssignmentException); 
+  EXPECT_NO_THROW(this->component_->set_assignment("non_existent", 5));
+
+  EXPECT_THROW(this->component_->template get_assignment<int>("int_assignment"), state_representation::exceptions::EmptyStateException); 
+  EXPECT_NO_THROW(this->component_->set_assignment("int_assignment", 5));
+  EXPECT_NO_THROW(this->component_->set_assignment("int_assignment", std::string("test")));
+
+  EXPECT_EQ(this->component_->template get_assignment<int>("int_assignment"), 5); 
+  EXPECT_THROW(this->component_->template get_assignment<std::string>("int_assignment"), modulo_core::exceptions::InvalidAssignmentException); 
+}
+
 TYPED_TEST(ComponentInterfaceTest, AddBoolPredicate) {
   this->component_->add_predicate("foo", true);
   auto predicate_iterator = this->component_->predicates_.find("foo");

source/modulo_components/test/python/test_component_interface.py

@@ -7,7 +7,7 @@
 import state_representation as sr
 from modulo_interfaces.srv import EmptyTrigger, StringTrigger
 from modulo_components.component_interface import ComponentInterface
-from modulo_core.exceptions import CoreError, LookupTransformError
+from modulo_core.exceptions import CoreError, LookupTransformError, InvalidAssignmentError
 from rclpy.qos import QoSProfile
 from std_msgs.msg import Bool, String
 from sensor_msgs.msg import JointState
@@ -71,6 +71,38 @@ def test_set_predicate(component_interface):
     assert not component_interface.get_predicate('bar')
 
 
+def test_add_assignment(component_interface):
+    component_interface.add_assignment('string_assignment', sr.ParameterType.STRING)
+    assert len(component_interface._ComponentInterface__assignment_dict) == 1
+    # adding an empty assignment should fail
+    component_interface.add_assignment('', sr.ParameterType.STRING)
+    assert len(component_interface._ComponentInterface__assignment_dict) == 1
+    # adding the assignment again should just overwrite
+    component_interface.add_assignment('string_assignment', sr.ParameterType.STRING)
+    assert len(component_interface._ComponentInterface__assignment_dict) == 1
+    # names should be cleaned up
+    component_interface.add_assignment('7cleEaGn_AaSssiGNgn#ment', sr.ParameterType.STRING)
+    assert 'clean_assignment' in component_interface._ComponentInterface__assignment_dict.keys()
+    assert len(component_interface._ComponentInterface__assignment_dict) == 2
+    # names without valid characters should fail
+    component_interface.add_assignment('@@@@@@@', sr.ParameterType.STRING)
+    assert len(component_interface._ComponentInterface__assignment_dict) == 2
+
+
+def test_get_set_assignment(component_interface):
+    component_interface.add_assignment('int_assignment', sr.ParameterType.INT)
+    with pytest.raises(InvalidAssignmentError):
+        component_interface.get_assignment('string_assignment')
+    component_interface.set_assignment('string_assignment', 'test')
+    with pytest.raises(sr.exceptions.EmptyStateError):
+        component_interface.get_assignment('int_assignment')
+    # setting the wrong type of value should fail
+    component_interface.set_assignment('int_assignment', 'test')
+    assert component_interface._ComponentInterface__assignment_dict['int_assignment'].is_empty()
+    component_interface.set_assignment('int_assignment', 5)
+    assert component_interface.get_assignment('int_assignment') == 5
+
+
 def test_declare_signal(component_interface):
     component_interface.declare_input("input", "test")
     assert component_interface.get_parameter_value("input_topic") == "test"

source/modulo_core/include/modulo_core/exceptions.hpp

@@ -42,6 +42,16 @@ class AddSignalException : public CoreException {
   explicit AddSignalException(const std::string& msg) : CoreException("AddSignalException", msg) {}
 };
 
+/**
+ * @class InvalidAssignmentException
+ * @brief An exception class to notify errors when getting the value of an assignment.
+ * @details This is an exception class to be thrown if there is a problem while getting the value of an assignment in a modulo class.
+ */
+class InvalidAssignmentException : public CoreException {
+public:
+  explicit InvalidAssignmentException(const std::string& msg) : CoreException("InvalidAssignmentException", msg) {}
+};
+
 /**
  * @class InvalidPointerCastException
  * @brief An exception class to notify if the result of getting an instance of a derived class through dynamic