diff --git a/CHANGELOG.md b/CHANGELOG.md index a4f01c22..c6095d8c 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index f94beb07..b2b43f18 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -162,6 +163,34 @@ class ComponentInterface { virtual bool on_validate_parameter_callback(const std::shared_ptr& 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 + 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 + 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 + 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& 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 triggers_;///< List of triggers + state_representation::ParameterMap assignments_map_; ///< Map of assignments + std::shared_ptr> assignment_publisher_;///< Assignment publisher + std::map>> empty_services_;///< Map of EmptyTrigger services std::map>> @@ -820,4 +852,79 @@ inline void ComponentInterface::publish_transforms( "Failed to send " << modifier << "transform: " << ex.what()); } } + +template +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(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 +void ComponentInterface::set_assignment(const std::string& assignment_name, const T& assignment_value) { + modulo_interfaces::msg::Assignment message; + std::shared_ptr 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(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 +T ComponentInterface::get_assignment(const std::string& assignment_name) const { + std::shared_ptr 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(); + } 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 diff --git a/source/modulo_components/modulo_components/component_interface.py b/source/modulo_components/modulo_components/component_interface.py index 594a700f..d07ecd6e 100644 --- a/source/modulo_components/modulo_components/component_interface.py +++ b/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, diff --git a/source/modulo_components/src/ComponentInterface.cpp b/source/modulo_components/src/ComponentInterface.cpp index cc0b7c15..ce745380 100644 --- a/source/modulo_components/src/ComponentInterface.cpp +++ b/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( + this->node_parameters_, this->node_topics_, "/assignments", this->qos_); + this->predicate_publisher_ = rclcpp::create_publisher( this->node_parameters_, this->node_topics_, "/predicates", this->qos_); this->predicate_message_.node = this->node_base_->get_fully_qualified_name(); diff --git a/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp b/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp index 84016ad2..c0aa1d2b 100644 --- a/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp +++ b/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,6 +46,7 @@ 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; @@ -51,6 +54,7 @@ class ComponentInterfacePublicInterface : public ComponentInterface { 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; diff --git a/source/modulo_components/test/cpp/test_component_interface.cpp b/source/modulo_components/test/cpp/test_component_interface.cpp index 2d07f5ff..467875c9 100644 --- a/source/modulo_components/test/cpp/test_component_interface.cpp +++ b/source/modulo_components/test/cpp/test_component_interface.cpp @@ -7,6 +7,7 @@ #include #include "test_modulo_components/component_public_interfaces.hpp" +#include "state_representation/exceptions/EmptyStateException.hpp" #include @@ -41,6 +42,35 @@ class ComponentInterfaceTest : public ::testing::Test { using NodeTypes = ::testing::Types; TYPED_TEST_SUITE(ComponentInterfaceTest, NodeTypes); +TYPED_TEST(ComponentInterfaceTest, AddAssignment) { + this->component_->template add_assignment("an_assignment"); + // adding an assignment with empty name should fail + EXPECT_NO_THROW(this->component_->template add_assignment("")); + // adding an assignment with the same name should just overwrite + this->component_->template add_assignment("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("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("@@@@@@")); + EXPECT_EQ(this->component_->assignments_map_.get_parameter_list().size(), 2); +} + +TYPED_TEST(ComponentInterfaceTest, GetSetAssignment) { + this->component_->template add_assignment("int_assignment"); + + EXPECT_THROW(this->component_->template get_assignment("non_existent"), modulo_core::exceptions::InvalidAssignmentException); + EXPECT_NO_THROW(this->component_->set_assignment("non_existent", 5)); + + EXPECT_THROW(this->component_->template get_assignment("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_assignment"), 5); + EXPECT_THROW(this->component_->template get_assignment("int_assignment"), modulo_core::exceptions::InvalidAssignmentException); +} + TYPED_TEST(ComponentInterfaceTest, AddBoolPredicate) { this->component_->add_predicate("foo", true); auto predicate_iterator = this->component_->predicates_.find("foo"); diff --git a/source/modulo_components/test/python/test_component_interface.py b/source/modulo_components/test/python/test_component_interface.py index d1fba9ae..94b555cb 100644 --- a/source/modulo_components/test/python/test_component_interface.py +++ b/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" diff --git a/source/modulo_core/include/modulo_core/exceptions.hpp b/source/modulo_core/include/modulo_core/exceptions.hpp index feca6adc..eef2894d 100644 --- a/source/modulo_core/include/modulo_core/exceptions.hpp +++ b/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 diff --git a/source/modulo_core/modulo_core/exceptions.py b/source/modulo_core/modulo_core/exceptions.py index 08cada96..c901c710 100644 --- a/source/modulo_core/modulo_core/exceptions.py +++ b/source/modulo_core/modulo_core/exceptions.py @@ -74,3 +74,11 @@ class LookupJointPositionsException(CoreError): def __init__(self, message: str): super().__init__(message, "LookupJointPositionsException") + +class InvalidAssignmentError(CoreError): + """ + An exception class to notify errors when getting the value of an assignment. + """ + + def __init__(self, message: str): + super().__init__(message, "InvalidAssignmentError") diff --git a/source/modulo_interfaces/CMakeLists.txt b/source/modulo_interfaces/CMakeLists.txt index d3333e7c..a9bed941 100644 --- a/source/modulo_interfaces/CMakeLists.txt +++ b/source/modulo_interfaces/CMakeLists.txt @@ -2,10 +2,11 @@ cmake_minimum_required(VERSION 3.15) project(modulo_interfaces) find_package(rosidl_default_generators REQUIRED) +find_package(rcl_interfaces REQUIRED) find_package(std_msgs REQUIRED) file(GLOB MSGS RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} msg/*.msg) file(GLOB SRVS RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} srv/*.srv) -rosidl_generate_interfaces(${PROJECT_NAME} ${MSGS} ${SRVS} DEPENDENCIES std_msgs) +rosidl_generate_interfaces(${PROJECT_NAME} ${MSGS} ${SRVS} DEPENDENCIES rcl_interfaces std_msgs) ament_package() \ No newline at end of file diff --git a/source/modulo_interfaces/README.md b/source/modulo_interfaces/README.md index 82aea3a0..1e95a67c 100644 --- a/source/modulo_interfaces/README.md +++ b/source/modulo_interfaces/README.md @@ -28,6 +28,11 @@ The predicate message contains the predicate name and the current value (true or The predicate collection message contains a vector of predicate messages as well as the name of the node that emits the predicates. +### Assignment + +The assignment message contains the value and the name of the assignment as a parameter, as well as the name of the node +that emits the assignment. + ## Services Modulo classes provide a simplified method to add services which trigger a pre-defined callback function. diff --git a/source/modulo_interfaces/msg/Assignment.msg b/source/modulo_interfaces/msg/Assignment.msg new file mode 100644 index 00000000..87ce9898 --- /dev/null +++ b/source/modulo_interfaces/msg/Assignment.msg @@ -0,0 +1,2 @@ +string node +rcl_interfaces/Parameter assignment \ No newline at end of file