|
17 | 17 | #include <modulo_core/exceptions.hpp> |
18 | 18 | #include <modulo_core/translators/parameter_translators.hpp> |
19 | 19 |
|
20 | | -#include <modulo_interfaces/msg/assignment.hpp> |
21 | 20 | #include <modulo_interfaces/msg/predicate_collection.hpp> |
22 | 21 | #include <modulo_interfaces/srv/empty_trigger.hpp> |
23 | 22 | #include <modulo_interfaces/srv/string_trigger.hpp> |
@@ -163,34 +162,6 @@ class ComponentInterface { |
163 | 162 | virtual bool |
164 | 163 | on_validate_parameter_callback(const std::shared_ptr<state_representation::ParameterInterface>& parameter); |
165 | 164 |
|
166 | | - /** |
167 | | - * @brief Add an assignment to the map of assignments. |
168 | | - * @tparam T The type of the assignment |
169 | | - * @param assignment_name the name of the associated assignment |
170 | | - */ |
171 | | - template<typename T> |
172 | | - void add_assignment(const std::string& assignment_name); |
173 | | - |
174 | | - /** |
175 | | - * @brief Set the value of an assignment. |
176 | | - * @tparam T The type of the assignment |
177 | | - * @param assignment_name The name of the assignment to set |
178 | | - * @param assignment_value The value of the assignment |
179 | | - */ |
180 | | - template<typename T> |
181 | | - void set_assignment(const std::string& assignment_name, const T& assignment_value); |
182 | | - |
183 | | - /** |
184 | | - * @brief Get the value of an assignment. |
185 | | - * @tparam T The type of the assignment |
186 | | - * @param assignment_name The name of the assignment to get |
187 | | - * @throws modulo_core::exceptions::InvalidAssignmentException if the assignment does not exist or the type does not |
188 | | - match |
189 | | - @throws state_representation::exceptions::EmptyStateException if the assignment has not been set yet |
190 | | - */ |
191 | | - template<typename T> |
192 | | - T get_assignment(const std::string& assignment_name) const; |
193 | | - |
194 | 165 | /** |
195 | 166 | * @brief Add a predicate to the map of predicates. |
196 | 167 | * @param predicate_name the name of the associated predicate |
@@ -500,7 +471,7 @@ class ComponentInterface { |
500 | 471 | bool validate_parameter(const std::shared_ptr<state_representation::ParameterInterface>& parameter); |
501 | 472 |
|
502 | 473 | /** |
503 | | - * @brief Populate a Predicate message with the name and the value of a predicate. |
| 474 | + * @brief Populate a Prediate message with the name and the value of a predicate. |
504 | 475 | * @param name The name of the predicate |
505 | 476 | * @param value The value of the predicate |
506 | 477 | */ |
@@ -583,9 +554,6 @@ class ComponentInterface { |
583 | 554 | modulo_interfaces::msg::PredicateCollection predicate_message_; |
584 | 555 | std::vector<std::string> triggers_;///< List of triggers |
585 | 556 |
|
586 | | - state_representation::ParameterMap assignments_map_; ///< Map of assignments |
587 | | - std::shared_ptr<rclcpp::Publisher<modulo_interfaces::msg::Assignment>> assignment_publisher_;///< Assignment publisher |
588 | | - |
589 | 557 | std::map<std::string, std::shared_ptr<rclcpp::Service<modulo_interfaces::srv::EmptyTrigger>>> |
590 | 558 | empty_services_;///< Map of EmptyTrigger services |
591 | 559 | std::map<std::string, std::shared_ptr<rclcpp::Service<modulo_interfaces::srv::StringTrigger>>> |
@@ -852,79 +820,4 @@ inline void ComponentInterface::publish_transforms( |
852 | 820 | "Failed to send " << modifier << "transform: " << ex.what()); |
853 | 821 | } |
854 | 822 | } |
855 | | - |
856 | | -template<typename T> |
857 | | -inline void ComponentInterface::add_assignment(const std::string& assignment_name) { |
858 | | - std::string parsed_name = modulo_utils::parsing::parse_topic_name(assignment_name); |
859 | | - if (parsed_name.empty()) { |
860 | | - RCLCPP_ERROR_STREAM( |
861 | | - this->node_logging_->get_logger(), |
862 | | - "The parsed name for assignment '" + assignment_name |
863 | | - + "' is empty. Provide a string with valid characters for the assignment name ([a-z0-9_])."); |
864 | | - return; |
865 | | - } |
866 | | - if (assignment_name != parsed_name) { |
867 | | - RCLCPP_WARN_STREAM( |
868 | | - this->node_logging_->get_logger(), |
869 | | - "The parsed name for assignment '" + assignment_name + "' is '" + parsed_name |
870 | | - + "'. Use the parsed name to refer to this assignment."); |
871 | | - } |
872 | | - try { |
873 | | - this->assignments_map_.get_parameter(parsed_name); |
874 | | - RCLCPP_WARN_STREAM( |
875 | | - this->node_logging_->get_logger(), "Assignment with name '" + parsed_name + "' already exists, overwriting."); |
876 | | - } catch (const state_representation::exceptions::InvalidParameterException& ex) { |
877 | | - RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), "Adding assignment '" << parsed_name << "'."); |
878 | | - } |
879 | | - try { |
880 | | - assignments_map_.set_parameter(state_representation::make_shared_parameter<T>(parsed_name)); |
881 | | - } catch (const std::exception& ex) { |
882 | | - RCLCPP_ERROR_STREAM_THROTTLE( |
883 | | - this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000, |
884 | | - "Failed to add assignment '" << parsed_name << "': " << ex.what()); |
885 | | - } |
886 | | -} |
887 | | - |
888 | | -template<typename T> |
889 | | -void ComponentInterface::set_assignment(const std::string& assignment_name, const T& assignment_value) { |
890 | | - modulo_interfaces::msg::Assignment message; |
891 | | - std::shared_ptr<state_representation::ParameterInterface> assignment; |
892 | | - try { |
893 | | - assignment = this->assignments_map_.get_parameter(assignment_name); |
894 | | - } catch (const state_representation::exceptions::InvalidParameterException&) { |
895 | | - RCLCPP_ERROR_STREAM_THROTTLE( |
896 | | - this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000, |
897 | | - "Failed to set assignment '" << assignment_name << "': Assignment does not exist."); |
898 | | - return; |
899 | | - } |
900 | | - try { |
901 | | - assignment->set_parameter_value<T>(assignment_value); |
902 | | - } catch (const state_representation::exceptions::InvalidParameterCastException&) { |
903 | | - RCLCPP_ERROR_STREAM_THROTTLE( |
904 | | - this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000, |
905 | | - "Failed to set assignment '" << assignment_name << "': Incompatible value type."); |
906 | | - return; |
907 | | - } |
908 | | - message.node = this->node_base_->get_fully_qualified_name(); |
909 | | - message.assignment = modulo_core::translators::write_parameter(assignment).to_parameter_msg(); |
910 | | - this->assignment_publisher_->publish(message); |
911 | | -} |
912 | | - |
913 | | -template<typename T> |
914 | | -T ComponentInterface::get_assignment(const std::string& assignment_name) const { |
915 | | - std::shared_ptr<state_representation::ParameterInterface> assignment; |
916 | | - try { |
917 | | - assignment = this->assignments_map_.get_parameter(assignment_name); |
918 | | - } catch (const state_representation::exceptions::InvalidParameterException&) { |
919 | | - throw modulo_core::exceptions::InvalidAssignmentException( |
920 | | - "Failed to get value of assignment '" + assignment_name + "': Assignment does not exist."); |
921 | | - } |
922 | | - try { |
923 | | - return assignment->get_parameter_value<T>(); |
924 | | - } catch (const state_representation::exceptions::InvalidParameterCastException&) { |
925 | | - auto expected_type = state_representation::get_parameter_type_name(assignment->get_parameter_type()); |
926 | | - throw modulo_core::exceptions::InvalidAssignmentException( |
927 | | - "Incompatible type for assignment '" + assignment_name + "' defined with type '" + expected_type + "'."); |
928 | | - } |
929 | | -} |
930 | 823 | }// namespace modulo_components |
0 commit comments