feat(controllers): only publish predicates on value change (#124)

CHANGELOG.md

@@ -33,6 +33,7 @@ Release Versions:
 - feat: try catch lifecycle transitions in components and controllers (#120)
 - refactor: change predicate topic of controllers (#121)
 - feat(components): only publish predicates on value change (#123)
+- feat(controllers): only publish predicates on value change (#124)
 
 ## 4.2.2
 

source/modulo_components/include/modulo_components/ComponentInterface.hpp

@@ -475,6 +475,7 @@ class ComponentInterface {
   /**
    * @brief Populate a Prediate message with the name and the value of a predicate.
    * @param name The name of the predicate
+   * @param value The value of the predicate
   */
   modulo_interfaces::msg::Predicate get_predicate_message(const std::string& name, bool value) const;
 
@@ -519,7 +520,7 @@ class ComponentInterface {
    * @param predicate_name The name of the predicate to publish
    * @param value The value of the predicate
    */
-  void publish_predicate(const std::string& predicate_name, bool value);
+  void publish_predicate(const std::string& predicate_name, bool value) const;
 
   /**
    * @brief Helper function to send a vector of transforms through a transform broadcaster

source/modulo_components/src/ComponentInterface.cpp

@@ -227,8 +227,8 @@ bool ComponentInterface::get_predicate(const std::string& predicate_name) const
   try {
     return predicate_it->second.get_value();
   } catch (const std::exception& ex) {
-    RCLCPP_ERROR_STREAM_THROTTLE(
-        this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000,
+    RCLCPP_ERROR_STREAM(
+        this->node_logging_->get_logger(),
         "Failed to evaluate callback of predicate '" << predicate_it->first << "', returning false: " << ex.what());
   }
   return false;
@@ -572,7 +572,7 @@ modulo_interfaces::msg::Predicate ComponentInterface::get_predicate_message(cons
   return message;
 }
 
-void ComponentInterface::publish_predicate(const std::string& name, bool value) {
+void ComponentInterface::publish_predicate(const std::string& name, bool value) const {
   auto message(this->predicate_message_);
   message.predicates.push_back(this->get_predicate_message(name, value));
   this->predicate_publisher_->publish(message);

source/modulo_controllers/include/modulo_controllers/ControllerInterface.hpp

@@ -10,7 +10,7 @@
 #include <state_representation/parameters/ParameterMap.hpp>
 
 #include <modulo_core/EncodedState.hpp>
-#include <modulo_core/PredicateVariant.hpp>
+#include <modulo_core/Predicate.hpp>
 #include <modulo_core/communication/MessagePair.hpp>
 #include <modulo_core/exceptions.hpp>
 #include <modulo_core/translators/message_writers.hpp>
@@ -486,36 +486,24 @@ class ControllerInterface : public controller_interface::ControllerInterface {
    */
   rcl_interfaces::msg::SetParametersResult on_set_parameters_callback(const std::vector<rclcpp::Parameter>& parameters);
 
-  /**
-   * @brief Add a predicate to the map of predicates.
-   * @param name The name of the predicate
-   * @param predicate The predicate variant
-   */
-  void add_variant_predicate(const std::string& name, const modulo_core::PredicateVariant& predicate);
-
-  /**
-   * @brief Set the predicate given as parameter, if the predicate is not found does not do anything.
-   * @param name The name of the predicate
-   * @param predicate The predicate variant
-   */
-  void set_variant_predicate(const std::string& name, const modulo_core::PredicateVariant& predicate);
-
   /**
    * @brief Populate a Prediate message with the name and the value of a predicate.
    * @param name The name of the predicate
+   * @param value The value of the predicate
   */
-  modulo_interfaces::msg::Predicate get_predicate_message(const std::string& name) const;
+  modulo_interfaces::msg::Predicate get_predicate_message(const std::string& name, bool value) const;
 
   /**
    * @brief Helper function to publish a predicate.
-   * @param name The name of the predicate to publish
+   * @param predicate_name The name of the predicate to publish
+   * @param value The value of the predicate
    */
-  void publish_predicate(const std::string& name) const;
+  void publish_predicate(const std::string& predicate_name, bool value) const;
 
   /**
    * @brief Helper function to publish all predicates.
    */
-  void publish_predicates() const;
+  void publish_predicates();
 
   /**
    * @brief Validate a signal name by parsing the name and checking the maps of registered signals
@@ -608,10 +596,10 @@ class ControllerInterface : public controller_interface::ControllerInterface {
   std::map<std::string, std::shared_ptr<rclcpp::Service<modulo_interfaces::srv::StringTrigger>>>
       string_services_;///< Map of StringTrigger services
 
-  std::map<std::string, modulo_core::PredicateVariant> predicates_; ///< Map of predicates
+  std::map<std::string, modulo_core::Predicate> predicates_; ///< Map of predicates
   std::shared_ptr<rclcpp::Publisher<modulo_interfaces::msg::PredicateCollection>>
       predicate_publisher_; ///< Predicate publisher
-  std::map<std::string, bool> triggers_; ///< Map of triggers
+  std::vector<std::string> triggers_; ///< Vector of triggers
   modulo_interfaces::msg::PredicateCollection predicate_message_;
   std::shared_ptr<rclcpp::TimerBase> predicate_timer_;
 

source/modulo_controllers/src/ControllerInterface.cpp

@@ -402,124 +402,124 @@ bool ControllerInterface::on_validate_parameter_callback(const std::shared_ptr<P
   return true;
 }
 
-void ControllerInterface::add_predicate(const std::string& name, bool predicate) {
-  add_variant_predicate(name, PredicateVariant(predicate));
+void ControllerInterface::add_predicate(const std::string& predicate_name, bool predicate_value) {
+  add_predicate(predicate_name, [predicate_value]() { return predicate_value; });
 }
 
-void ControllerInterface::add_predicate(const std::string& name, const std::function<bool(void)>& predicate) {
-  add_variant_predicate(name, PredicateVariant(predicate));
-}
-
-void ControllerInterface::add_variant_predicate(
-    const std::string& name, const PredicateVariant& predicate) {
-  if (name.empty()) {
+void ControllerInterface::add_predicate(
+    const std::string& predicate_name, const std::function<bool(void)>& predicate_function) {
+  if (predicate_name.empty()) {
     RCLCPP_ERROR(get_node()->get_logger(), "Failed to add predicate: Provide a non empty string as a name.");
     return;
   }
-  if (predicates_.find(name) != predicates_.end()) {
-    RCLCPP_WARN(get_node()->get_logger(), "Predicate with name '%s' already exists, overwriting.", name.c_str());
+  if (predicates_.find(predicate_name) != predicates_.end()) {
+    RCLCPP_WARN(
+        get_node()->get_logger(), "Predicate with name '%s' already exists, overwriting.", predicate_name.c_str());
   } else {
-    RCLCPP_DEBUG(get_node()->get_logger(), "Adding predicate '%s'.", name.c_str());
+    RCLCPP_DEBUG(get_node()->get_logger(), "Adding predicate '%s'.", predicate_name.c_str());
+  }
+  try {
+    this->predicates_.insert_or_assign(predicate_name, modulo_core::Predicate(predicate_function));
+  } catch (const std::exception& ex) {
+    RCLCPP_ERROR(
+        get_node()->get_logger(), "Failed to evaluate callback of predicate '%s', returning false: %s",
+        predicate_name.c_str(), ex.what());
   }
-  predicates_.insert_or_assign(name, predicate);
 }
 
 bool ControllerInterface::get_predicate(const std::string& predicate_name) const {
-  auto predicate_iterator = predicates_.find(predicate_name);
-  // if there is no predicate with that name simply return false with an error message
-  if (predicate_iterator == predicates_.end()) {
+  auto predicate_it = predicates_.find(predicate_name);
+  if (predicate_it == predicates_.end()) {
     RCLCPP_ERROR_THROTTLE(
         get_node()->get_logger(), *get_node()->get_clock(), 1000,
         "Failed to get predicate '%s': Predicate does not exists, returning false.", predicate_name.c_str());
     return false;
   }
-  // try to get the value from the variant as a bool
-  auto* ptr_value = std::get_if<bool>(&predicate_iterator->second);
-  if (ptr_value) {
-    return *ptr_value;
-  }
-  // if previous check failed, it means the variant is actually a callback function
-  auto callback_function = std::get<std::function<bool(void)>>(predicate_iterator->second);
-  bool value = false;
   try {
-    value = (callback_function) ();
+    return predicate_it->second.get_value();
   } catch (const std::exception& ex) {
     RCLCPP_ERROR_THROTTLE(
         get_node()->get_logger(), *get_node()->get_clock(), 1000,
         "Failed to evaluate callback of predicate '%s', returning false: %s", predicate_name.c_str(), ex.what());
   }
-  return value;
-}
-
-void ControllerInterface::set_predicate(const std::string& name, bool predicate) {
-  set_variant_predicate(name, PredicateVariant(predicate));
+  return false;
 }
 
-void ControllerInterface::set_predicate(const std::string& name, const std::function<bool(void)>& predicate) {
-  set_variant_predicate(name, PredicateVariant(predicate));
+void ControllerInterface::set_predicate(const std::string& predicate_name, bool predicate_value) {
+  set_predicate(predicate_name, [predicate_value]() { return predicate_value; });
 }
 
-void ControllerInterface::set_variant_predicate(
-    const std::string& name, const PredicateVariant& predicate) {
-  auto predicate_iterator = predicates_.find(name);
-  if (predicate_iterator == predicates_.end()) {
+void ControllerInterface::set_predicate(
+    const std::string& predicate_name, const std::function<bool(void)>& predicate_function) {
+  auto predicate_it = predicates_.find(predicate_name);
+  if (predicate_it == predicates_.end()) {
     RCLCPP_ERROR_THROTTLE(
         get_node()->get_logger(), *get_node()->get_clock(), 1000,
-        "Failed to set predicate '%s': Predicate does not exist.", name.c_str());
+        "Failed to set predicate '%s': Predicate does not exist.", predicate_name.c_str());
     return;
   }
-  predicate_iterator->second = predicate;
-  publish_predicate(name);// TODO: do we want that
+  predicate_it->second.set_predicate(predicate_function);
+  if (auto new_predicate = predicate_it->second.query(); new_predicate) {
+    publish_predicate(predicate_name, *new_predicate);
+  }
 }
 
 void ControllerInterface::add_trigger(const std::string& trigger_name) {
   if (trigger_name.empty()) {
     RCLCPP_ERROR(get_node()->get_logger(), "Failed to add trigger: Provide a non empty string as a name.");
     return;
   }
-  if (triggers_.find(trigger_name) != triggers_.end() || predicates_.find(trigger_name) != predicates_.end()) {
+  if (std::find(triggers_.cbegin(), triggers_.cend(), trigger_name) != triggers_.cend()) {
     RCLCPP_ERROR(
-        get_node()->get_logger(), "Failed to add trigger: there is already a trigger or predicate with name '%s'.",
+        get_node()->get_logger(), "Failed to add trigger: there is already a trigger with name '%s'.",
         trigger_name.c_str());
     return;
   }
-  triggers_.insert_or_assign(trigger_name, false);
-  add_predicate(trigger_name, [this, trigger_name] {
-    auto value = this->triggers_.at(trigger_name);
-    this->triggers_.at(trigger_name) = false;
-    return value;
-  });
+  if (predicates_.find(trigger_name) != predicates_.end()) {
+    RCLCPP_ERROR(
+        get_node()->get_logger(), "Failed to add trigger: there is already a predicate with name '%s'.",
+        trigger_name.c_str());
+    return;
+  }
+  this->triggers_.push_back(trigger_name);
+  this->add_predicate(trigger_name, false);
 }
 
 void ControllerInterface::trigger(const std::string& trigger_name) {
-  if (triggers_.find(trigger_name) == triggers_.end()) {
+  if (std::find(triggers_.cbegin(), triggers_.cend(), trigger_name) != triggers_.cend()) {
     RCLCPP_ERROR(
         get_node()->get_logger(), "Failed to trigger: could not find trigger with name  '%s'.", trigger_name.c_str());
     return;
   }
-  triggers_.at(trigger_name) = true;
-  publish_predicate(trigger_name);
+  this->set_predicate(trigger_name, true);
+  // reset the trigger to be published on the next step
+  this->predicates_.at(trigger_name).set_predicate([]() { return false; });
 }
 
-modulo_interfaces::msg::Predicate ControllerInterface::get_predicate_message(const std::string& name) const {
+modulo_interfaces::msg::Predicate
+ControllerInterface::get_predicate_message(const std::string& name, bool value) const {
   modulo_interfaces::msg::Predicate message;
   message.predicate = name;
-  message.value = get_predicate(name);
+  message.value = value;
   return message;
 }
 
-void ControllerInterface::publish_predicate(const std::string& name) const {
+void ControllerInterface::publish_predicate(const std::string& predicate_name, bool value) const {
   auto message(predicate_message_);
-  message.predicates.push_back(get_predicate_message(name));
+  message.predicates.push_back(get_predicate_message(predicate_name, value));
   predicate_publisher_->publish(message);
 }
 
-void ControllerInterface::publish_predicates() const {
+void ControllerInterface::publish_predicates() {
   auto message(predicate_message_);
-  for (const auto& predicate : predicates_) {
-    message.predicates.push_back(get_predicate_message(predicate.first));
+  for (auto predicate_it = predicates_.begin(); predicate_it != predicates_.end(); ++predicate_it) {
+    if (auto new_predicate = predicate_it->second.query(); new_predicate) {
+      message.predicates.push_back(get_predicate_message(predicate_it->first, *new_predicate));
+    }
+  }
+  if (message.predicates.size()) {
+    predicate_publisher_->publish(message);
   }
-  predicate_publisher_->publish(message);
 }
 
 std::string ControllerInterface::validate_and_declare_signal(