Merge branch 'feat/generic_controller_outputs' into feat/generic_cont…

…roller_inputs
aica-technology · Oct 3, 2024 · d30fa97 · d30fa97
2 parents 4c6da07 + 43d5a84
commit d30fa97
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 21 deletions.
diff --git a/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp b/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp
@@ -607,7 +607,7 @@ inline void BaseControllerInterface::add_output(const std::string& name, const s
     typedef std::pair<std::shared_ptr<rclcpp::Publisher<T>>, realtime_tools::RealtimePublisherSharedPtr<T>> PublisherT;
     auto parsed_name = validate_and_declare_signal(name, "output", topic_name);
     if (!parsed_name.empty()) {
-      outputs_.insert_or_assign(name, PublisherT());
+      outputs_.insert_or_assign(parsed_name, PublisherT());
       custom_output_configuration_callables_.insert_or_assign(
           name, [this](CustomPublishers& pub, const std::string& name) {
             std::shared_ptr<rclcpp::Publisher<T>> publisher =
@@ -751,13 +751,13 @@ inline std::optional<std::string> BaseControllerInterface::read_input<std::strin
 
 template<typename T>
 inline void BaseControllerInterface::write_output(const std::string& name, const T& data) {
-  if constexpr (modulo_core::concepts::CustomT<T>) {
-    if (outputs_.find(name) == outputs_.end()) {
-      RCLCPP_WARN_THROTTLE(
-          get_node()->get_logger(), *get_node()->get_clock(), 1000, "Could not find output '%s'", name.c_str());
-      return;
-    }
+  if (outputs_.find(name) == outputs_.end()) {
+    RCLCPP_WARN_THROTTLE(
+        get_node()->get_logger(), *get_node()->get_clock(), 1000, "Could not find output '%s'", name.c_str());
+    return;
+  }
 
+  if constexpr (modulo_core::concepts::CustomT<T>) {
     CustomPublishers publishers;
     try {
       publishers = std::get<CustomPublishers>(outputs_.at(name));
@@ -778,15 +778,8 @@ inline void BaseControllerInterface::write_output(const std::string& name, const
           get_node()->get_logger(), *get_node()->get_clock(), 1000,
           "Skipping publication of output '%s' due to wrong data type: %s", name.c_str(), ex.what());
     }
-
     if (rt_pub && rt_pub->trylock()) {
-      try {
-        rt_pub->msg_ = data;
-      } catch (const modulo_core::exceptions::MessageTranslationException& ex) {
-        RCLCPP_ERROR_THROTTLE(
-            get_node()->get_logger(), *get_node()->get_clock(), 1000, "Failed to publish output '%s': %s", name.c_str(),
-            ex.what());
-      }
+      rt_pub->msg_ = data;
       rt_pub->unlockAndPublish();
     }
   } else {
@@ -796,11 +789,6 @@ inline void BaseControllerInterface::write_output(const std::string& name, const
           "Skipping publication of output '%s' due to emptiness of state", name.c_str());
       return;
     }
-    if (outputs_.find(name) == outputs_.end()) {
-      RCLCPP_WARN_THROTTLE(
-          get_node()->get_logger(), *get_node()->get_clock(), 1000, "Could not find output '%s'", name.c_str());
-      return;
-    }
     EncodedStatePublishers publishers;
     try {
       publishers = std::get<EncodedStatePublishers>(outputs_.at(name));

diff --git a/source/modulo_controllers/src/BaseControllerInterface.cpp b/source/modulo_controllers/src/BaseControllerInterface.cpp
@@ -372,7 +372,7 @@ void BaseControllerInterface::create_output(
     const PublisherVariant& publishers, const std::string& name, const std::string& topic_name) {
   auto parsed_name = validate_and_declare_signal(name, "output", topic_name);
   if (!parsed_name.empty()) {
-    outputs_.insert_or_assign(name, publishers);
+    outputs_.insert_or_assign(parsed_name, publishers);
   }
 }