BaseControllerInterface.hpp

#pragma once
 
#include <any>
#include <chrono>
#include <mutex>
 
#include <controller_interface/controller_interface.hpp>
#include <controller_interface/helpers.hpp>
#include <realtime_tools/realtime_buffer.h>
#include <realtime_tools/realtime_publisher.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>
 
#include <state_representation/parameters/ParameterMap.hpp>
 
#include <modulo_core/EncodedState.hpp>
#include <modulo_core/Predicate.hpp>
#include <modulo_core/communication/MessagePair.hpp>
#include <modulo_core/exceptions.hpp>
#include <modulo_core/translators/message_writers.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>
 
#include <modulo_utils/parsing.hpp>
 
#include <modulo_core/concepts.hpp>
 
namespace modulo_controllers {
 
typedef std::variant<
    std::shared_ptr<rclcpp::Subscription<modulo_core::EncodedState>>,
    std::shared_ptr<rclcpp::Subscription<std_msgs::msg::Bool>>,
    std::shared_ptr<rclcpp::Subscription<std_msgs::msg::Float64>>,
    std::shared_ptr<rclcpp::Subscription<std_msgs::msg::Float64MultiArray>>,
    std::shared_ptr<rclcpp::Subscription<std_msgs::msg::Int32>>,
    std::shared_ptr<rclcpp::Subscription<std_msgs::msg::String>>, std::any>
    SubscriptionVariant;
 
typedef std::variant<
    realtime_tools::RealtimeBuffer<std::shared_ptr<modulo_core::EncodedState>>,
    realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Bool>>,
    realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Float64>>,
    realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Float64MultiArray>>,
    realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Int32>>,
    realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::String>>, std::any>
    BufferVariant;
 
typedef std::tuple<
    std::shared_ptr<state_representation::State>, std::shared_ptr<rclcpp::Publisher<modulo_core::EncodedState>>,
    realtime_tools::RealtimePublisherSharedPtr<modulo_core::EncodedState>>
    EncodedStatePublishers;
typedef std::pair<
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Bool>>,
    realtime_tools::RealtimePublisherSharedPtr<std_msgs::msg::Bool>>
    BoolPublishers;
typedef std::pair<
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Float64>>,
    realtime_tools::RealtimePublisherSharedPtr<std_msgs::msg::Float64>>
    DoublePublishers;
typedef std::pair<
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Float64MultiArray>>,
    realtime_tools::RealtimePublisherSharedPtr<std_msgs::msg::Float64MultiArray>>
    DoubleVecPublishers;
typedef std::pair<
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Int32>>,
    realtime_tools::RealtimePublisherSharedPtr<std_msgs::msg::Int32>>
    IntPublishers;
typedef std::pair<
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::String>>,
    realtime_tools::RealtimePublisherSharedPtr<std_msgs::msg::String>>
    StringPublishers;
typedef std::pair<std::any, std::any> CustomPublishers;
 
typedef std::variant<
    EncodedStatePublishers, BoolPublishers, DoublePublishers, DoubleVecPublishers, IntPublishers, StringPublishers,
    CustomPublishers>
    PublisherVariant;
 
struct ControllerInput {
  ControllerInput() = default;
  ControllerInput(BufferVariant buffer_variant) : buffer(std::move(buffer_variant)) {}
  BufferVariant buffer;
  std::chrono::time_point<std::chrono::steady_clock> timestamp;
};
 
struct ControllerServiceResponse {
  bool success;
  std::string message;
};
 
class BaseControllerInterface : public controller_interface::ControllerInterface {
public:
  BaseControllerInterface();
 
  CallbackReturn on_init() override;
 
  CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
 
protected:
  void add_parameter(
      const std::shared_ptr<state_representation::ParameterInterface>& parameter, const std::string& description,
      bool read_only = false);
 
  template<typename T>
  void add_parameter(const std::string& name, const T& value, const std::string& description, bool read_only = false);
 
  virtual bool
  on_validate_parameter_callback(const std::shared_ptr<state_representation::ParameterInterface>& parameter);
 
  [[nodiscard]] std::shared_ptr<state_representation::ParameterInterface> get_parameter(const std::string& name) const;
 
  template<typename T>
  T get_parameter_value(const std::string& name) const;
 
  template<typename T>
  void set_parameter_value(const std::string& name, const T& value);
 
  template<typename T>
  void add_assignment(const std::string& assignment_name);
 
  template<typename T>
  void set_assignment(const std::string& assignment_name, const T& assignment_value);
 
  template<typename T>
  T get_assignment(const std::string& assignment_name) const;
 
  void add_predicate(const std::string& predicate_name, bool predicate_value);
 
  void add_predicate(const std::string& predicate_name, const std::function<bool(void)>& predicate_function);
 
  [[nodiscard]] bool get_predicate(const std::string& predicate_name) const;
 
  void set_predicate(const std::string& predicate_name, bool predicate_value);
 
  void set_predicate(const std::string& predicate_name, const std::function<bool(void)>& predicate_function);
 
  void add_trigger(const std::string& trigger_name);
 
  void trigger(const std::string& trigger_name);
 
  template<typename T>
  void add_input(const std::string& name, const std::string& topic_name = "");
 
  template<typename T>
  void add_output(const std::string& name, const std::string& topic_name = "");
 
  template<typename T>
  std::optional<T> read_input(const std::string& name);
 
  template<typename T>
  void write_output(const std::string& name, const T& data);
 
  void add_service(const std::string& service_name, const std::function<ControllerServiceResponse(void)>& callback);
 
  void add_service(
      const std::string& service_name,
      const std::function<ControllerServiceResponse(const std::string& string)>& callback);
 
  void
  add_service_lockfree(const std::string& service_name, const std::function<ControllerServiceResponse(void)>& callback);
 
  void add_service_lockfree(
      const std::string& service_name,
      const std::function<ControllerServiceResponse(const std::string& string)>& callback);
 
  void add_tf_listener();
 
  [[nodiscard]] state_representation::CartesianPose lookup_transform(
      const std::string& frame, const std::string& reference_frame, const tf2::TimePoint& time_point,
      const tf2::Duration& duration);
 
  [[nodiscard]] state_representation::CartesianPose lookup_transform(
      const std::string& frame, const std::string& reference_frame = "world", double validity_period = -1.0,
      const tf2::Duration& duration = tf2::Duration(std::chrono::microseconds(10)));
 
  void add_tf_broadcaster();
 
  void add_static_tf_broadcaster();
 
  void send_transform(const state_representation::CartesianPose& transform);
 
  void send_transforms(const std::vector<state_representation::CartesianPose>& transforms);
 
  void send_static_transform(const state_representation::CartesianPose& transform);
 
  void send_static_transforms(const std::vector<state_representation::CartesianPose>& transforms);
 
  [[nodiscard]] rclcpp::QoS get_qos() const;
 
  void set_qos(const rclcpp::QoS& qos);
 
  bool is_active() const;
 
  std::timed_mutex& get_command_mutex();
 
private:
  bool validate_parameter(const std::shared_ptr<state_representation::ParameterInterface>& parameter);
 
  void pre_set_parameters_callback(std::vector<rclcpp::Parameter>& parameters);
 
  rcl_interfaces::msg::SetParametersResult on_set_parameters_callback(const std::vector<rclcpp::Parameter>& parameters);
 
  modulo_interfaces::msg::Predicate get_predicate_message(const std::string& name, bool value) const;
 
  void publish_predicate(const std::string& predicate_name, bool value) const;
 
  void publish_predicates();
 
  std::string validate_and_declare_signal(
      const std::string& signal_name, const std::string& type, const std::string& default_topic,
      bool fixed_topic = false);
 
  void create_input(const ControllerInput& input, const std::string& name, const std::string& topic_name);
 
  void create_output(const PublisherVariant& publishers, const std::string& name, const std::string& topic_name);
 
  template<typename T>
  std::shared_ptr<rclcpp::Subscription<T>> create_subscription(const std::string& name, const std::string& topic_name);
 
  bool check_input_valid(const std::string& name) const;
 
  template<typename PublisherT, typename MsgT, typename T>
  void write_std_output(const std::string& name, const T& data);
 
  void add_inputs();
 
  void add_outputs();
 
  template<bool acquire_lock, typename CallbackT>
  void create_service(const std::string& service_name, const std::function<CallbackT>& callback)
    requires(
        std::is_same_v<CallbackT, ControllerServiceResponse(const std::string&)>
        || std::is_same_v<CallbackT, ControllerServiceResponse(void)>);
 
  std::string validate_service_name(const std::string& service_name, const std::string& type) const;
 
  [[nodiscard]] geometry_msgs::msg::TransformStamped lookup_ros_transform(
      const std::string& frame, const std::string& reference_frame, const tf2::TimePoint& time_point,
      const tf2::Duration& duration);
 
  template<typename T>
  void publish_transforms(
      const std::vector<state_representation::CartesianPose>& transforms, const std::shared_ptr<T>& tf_broadcaster,
      bool is_static = false);
 
  state_representation::ParameterMap parameter_map_;
  std::unordered_map<std::string, bool> read_only_parameters_;
  std::shared_ptr<rclcpp::node_interfaces::PreSetParametersCallbackHandle>
      pre_set_parameter_cb_handle_;
  std::shared_ptr<rclcpp::node_interfaces::OnSetParametersCallbackHandle>
      on_set_parameter_cb_handle_;
  rcl_interfaces::msg::SetParametersResult set_parameters_result_;
  bool pre_set_parameter_callback_called_ = false;
 
  std::vector<SubscriptionVariant> subscriptions_;
  std::map<std::string, ControllerInput> inputs_; 
  std::map<std::string, std::shared_ptr<modulo_core::communication::MessagePairInterface>>
      input_message_pairs_;                        
  std::map<std::string, PublisherVariant> outputs_;
  double input_validity_period_;                   
  rclcpp::QoS qos_ = rclcpp::QoS(10);              
 
  std::map<std::string, std::shared_ptr<rclcpp::Service<modulo_interfaces::srv::EmptyTrigger>>>
      empty_services_;
  std::map<std::string, std::shared_ptr<rclcpp::Service<modulo_interfaces::srv::StringTrigger>>>
      string_services_;
 
  state_representation::ParameterMap assignments_map_;                                         
  std::shared_ptr<rclcpp::Publisher<modulo_interfaces::msg::Assignment>> assignment_publisher_;
 
  std::map<std::string, modulo_core::Predicate> predicates_;
  std::shared_ptr<rclcpp::Publisher<modulo_interfaces::msg::PredicateCollection>>
      predicate_publisher_;          
  std::vector<std::string> triggers_;
  modulo_interfaces::msg::PredicateCollection predicate_message_;
  std::shared_ptr<rclcpp::TimerBase> predicate_timer_;
 
  std::timed_mutex command_mutex_;
 
  std::map<std::string, std::function<void(CustomPublishers&, const std::string&)>>
      custom_output_configuration_callables_;
  std::map<std::string, std::function<void(const std::string&, const std::string&)>>
      custom_input_configuration_callables_;
 
  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;                                
  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;                   
  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;             
  std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_tf_broadcaster_;
};
 
template<typename T>
inline void BaseControllerInterface::add_parameter(
    const std::string& name, const T& value, const std::string& description, bool read_only) {
  if (name.empty()) {
    RCLCPP_ERROR(get_node()->get_logger(), "Failed to add parameter: Provide a non empty string as a name.");
    return;
  }
  add_parameter(state_representation::make_shared_parameter(name, value), description, read_only);
}
 
template<typename T>
inline T BaseControllerInterface::get_parameter_value(const std::string& name) const {
  try {
    return this->parameter_map_.template get_parameter_value<T>(name);
  } catch (const state_representation::exceptions::InvalidParameterException& ex) {
    throw modulo_core::exceptions::ParameterException(
        "Failed to get parameter value of parameter '" + name + "': " + ex.what());
  }
}
 
template<typename T>
inline void BaseControllerInterface::set_parameter_value(const std::string& name, const T& value) {
  try {
    std::vector<rclcpp::Parameter> parameters{
        modulo_core::translators::write_parameter(state_representation::make_shared_parameter(name, value))};
    pre_set_parameters_callback(parameters);
    pre_set_parameter_callback_called_ = true;
    auto result = get_node()->set_parameters(parameters).at(0);
    if (!result.successful) {
      RCLCPP_ERROR_THROTTLE(
          get_node()->get_logger(), *get_node()->get_clock(), 1000,
          "Failed to set parameter value of parameter '%s': %s", name.c_str(), result.reason.c_str());
    }
  } catch (const std::exception& ex) {
    RCLCPP_ERROR_THROTTLE(
        get_node()->get_logger(), *get_node()->get_clock(), 1000, "Failed to set parameter value of parameter '%s': %s",
        name.c_str(), ex.what());
  }
}
 
template<typename T>
inline void BaseControllerInterface::add_input(const std::string& name, const std::string& topic_name) {
  if constexpr (modulo_core::concepts::CustomT<T>) {
    auto buffer = std::make_shared<realtime_tools::RealtimeBuffer<std::shared_ptr<T>>>();
    auto input = ControllerInput(buffer);
    auto parsed_name = validate_and_declare_signal(name, "input", topic_name);
    if (!parsed_name.empty()) {
      inputs_.insert_or_assign(parsed_name, input);
      custom_input_configuration_callables_.insert_or_assign(
          name, [this](const std::string& name, const std::string& topic) {
            auto subscription =
                get_node()->create_subscription<T>(topic, qos_, [this, name](const std::shared_ptr<T> message) {
                  auto buffer_variant = std::get<std::any>(inputs_.at(name).buffer);
                  auto buffer = std::any_cast<std::shared_ptr<realtime_tools::RealtimeBuffer<std::shared_ptr<T>>>>(
                      buffer_variant);
                  buffer->writeFromNonRT(message);
                  inputs_.at(name).timestamp = std::chrono::steady_clock::now();
                });
            subscriptions_.push_back(subscription);
          });
    }
  } else {
    auto buffer = realtime_tools::RealtimeBuffer<std::shared_ptr<modulo_core::EncodedState>>();
    auto input = ControllerInput(buffer);
    auto parsed_name = validate_and_declare_signal(name, "input", topic_name);
    if (!parsed_name.empty()) {
      inputs_.insert_or_assign(parsed_name, input);
      input_message_pairs_.insert_or_assign(
          parsed_name,
          modulo_core::communication::make_shared_message_pair(std::make_shared<T>(), get_node()->get_clock()));
    }
  }
}
 
template<>
inline void BaseControllerInterface::add_input<bool>(const std::string& name, const std::string& topic_name) {
  auto buffer = realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Bool>>();
  auto input = ControllerInput(buffer);
  create_input(input, name, topic_name);
}
 
template<>
inline void BaseControllerInterface::add_input<double>(const std::string& name, const std::string& topic_name) {
  auto buffer = realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Float64>>();
  auto input = ControllerInput(buffer);
  create_input(input, name, topic_name);
}
 
template<>
inline void
BaseControllerInterface::add_input<std::vector<double>>(const std::string& name, const std::string& topic_name) {
  auto buffer = realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Float64MultiArray>>();
  auto input = ControllerInput(buffer);
  create_input(input, name, topic_name);
}
 
template<>
inline void BaseControllerInterface::add_input<int>(const std::string& name, const std::string& topic_name) {
  auto buffer = realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Int32>>();
  auto input = ControllerInput(buffer);
  create_input(input, name, topic_name);
}
 
template<>
inline void BaseControllerInterface::add_input<std::string>(const std::string& name, const std::string& topic_name) {
  auto buffer = realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::String>>();
  auto input = ControllerInput(buffer);
  create_input(input, name, topic_name);
}
 
template<typename T>
inline std::shared_ptr<rclcpp::Subscription<T>>
BaseControllerInterface::create_subscription(const std::string& name, const std::string& topic_name) {
  return get_node()->create_subscription<T>(topic_name, qos_, [this, name](const std::shared_ptr<T> message) {
    std::get<realtime_tools::RealtimeBuffer<std::shared_ptr<T>>>(inputs_.at(name).buffer).writeFromNonRT(message);
    inputs_.at(name).timestamp = std::chrono::steady_clock::now();
  });
}
 
template<typename T>
inline void BaseControllerInterface::add_output(const std::string& name, const std::string& topic_name) {
  if constexpr (modulo_core::concepts::CustomT<T>) {
    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(parsed_name, PublisherT());
      custom_output_configuration_callables_.insert_or_assign(
          name, [this](CustomPublishers& pub, const std::string& topic) {
            auto publisher = get_node()->create_publisher<T>(topic, qos_);
            pub.first = publisher;
            pub.second = std::make_shared<realtime_tools::RealtimePublisher<T>>(publisher);
          });
    }
  } else {
    std::shared_ptr<state_representation::State> state_ptr = std::make_shared<T>();
    create_output(EncodedStatePublishers(state_ptr, {}, {}), name, topic_name);
  }
}
 
template<>
inline void BaseControllerInterface::add_output<bool>(const std::string& name, const std::string& topic_name) {
  create_output(BoolPublishers(), name, topic_name);
}
 
template<>
inline void BaseControllerInterface::add_output<double>(const std::string& name, const std::string& topic_name) {
  create_output(DoublePublishers(), name, topic_name);
}
 
template<>
inline void
BaseControllerInterface::add_output<std::vector<double>>(const std::string& name, const std::string& topic_name) {
  create_output(DoubleVecPublishers(), name, topic_name);
}
 
template<>
inline void BaseControllerInterface::add_output<int>(const std::string& name, const std::string& topic_name) {
  create_output(IntPublishers(), name, topic_name);
}
 
template<>
inline void BaseControllerInterface::add_output<std::string>(const std::string& name, const std::string& topic_name) {
  create_output(StringPublishers(), name, topic_name);
}
 
template<typename T>
inline std::optional<T> BaseControllerInterface::read_input(const std::string& name) {
  if (!check_input_valid(name)) {
    return {};
  }
 
  if constexpr (modulo_core::concepts::CustomT<T>) {
    try {
      auto buffer_variant = std::get<std::any>(inputs_.at(name).buffer);
      auto buffer = std::any_cast<std::shared_ptr<realtime_tools::RealtimeBuffer<std::shared_ptr<T>>>>(buffer_variant);
      return **(buffer->readFromNonRT());
    } catch (const std::bad_any_cast& ex) {
      RCLCPP_ERROR(get_node()->get_logger(), "Failed to read custom input: %s", ex.what());
    }
    return {};
  } else {
    auto message =
        **std::get<realtime_tools::RealtimeBuffer<std::shared_ptr<modulo_core::EncodedState>>>(inputs_.at(name).buffer)
              .readFromNonRT();
    std::shared_ptr<state_representation::State> state;
    try {
      auto message_pair = input_message_pairs_.at(name);
      message_pair->read<modulo_core::EncodedState, state_representation::State>(message);
      state = message_pair->get_message_pair<modulo_core::EncodedState, state_representation::State>()->get_data();
    } catch (const std::exception& ex) {
      RCLCPP_WARN_THROTTLE(
          get_node()->get_logger(), *get_node()->get_clock(), 1000,
          "Could not read EncodedState message on input '%s': %s", name.c_str(), ex.what());
      return {};
    }
    if (state->is_empty()) {
      return {};
    }
    auto cast_ptr = std::dynamic_pointer_cast<T>(state);
    if (cast_ptr != nullptr) {
      return *cast_ptr;
    } else {
      RCLCPP_WARN_THROTTLE(
          get_node()->get_logger(), *get_node()->get_clock(), 1000,
          "Dynamic cast of message on input '%s' from type '%s' to type '%s' failed.", name.c_str(),
          get_state_type_name(state->get_type()).c_str(), get_state_type_name(T().get_type()).c_str());
    }
    return {};
  }
}
 
template<>
inline std::optional<bool> BaseControllerInterface::read_input<bool>(const std::string& name) {
  if (!check_input_valid(name)) {
    return {};
  }
  // no need to check for emptiness of the pointer: timestamps are default constructed to 0, so an input being valid
  //  means that a message was received
  return (*std::get<realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Bool>>>(inputs_.at(name).buffer)
               .readFromNonRT())
      ->data;
}
 
template<>
inline std::optional<double> BaseControllerInterface::read_input<double>(const std::string& name) {
  if (!check_input_valid(name)) {
    return {};
  }
  return (*std::get<realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Float64>>>(inputs_.at(name).buffer)
               .readFromNonRT())
      ->data;
}
 
template<>
inline std::optional<std::vector<double>>
BaseControllerInterface::read_input<std::vector<double>>(const std::string& name) {
  if (!check_input_valid(name)) {
    return {};
  }
  return (*std::get<realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Float64MultiArray>>>(
               inputs_.at(name).buffer)
               .readFromNonRT())
      ->data;
}
 
template<>
inline std::optional<int> BaseControllerInterface::read_input<int>(const std::string& name) {
  if (!check_input_valid(name)) {
    return {};
  }
  return (*std::get<realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Int32>>>(inputs_.at(name).buffer)
               .readFromNonRT())
      ->data;
}
 
template<>
inline std::optional<std::string> BaseControllerInterface::read_input<std::string>(const std::string& name) {
  if (!check_input_valid(name)) {
    return {};
  }
  return (*std::get<realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::String>>>(inputs_.at(name).buffer)
               .readFromNonRT())
      ->data;
}
 
template<typename T>
inline void BaseControllerInterface::write_output(const std::string& name, const T& data) {
  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));
    } catch (const std::bad_variant_access&) {
      RCLCPP_WARN_THROTTLE(
          get_node()->get_logger(), *get_node()->get_clock(), 1000,
          "Could not retrieve publisher for output '%s': Invalid output type", name.c_str());
      return;
    }
 
    std::shared_ptr<realtime_tools::RealtimePublisher<T>> rt_pub;
    try {
      rt_pub = std::any_cast<std::shared_ptr<realtime_tools::RealtimePublisher<T>>>(publishers.second);
    } catch (const std::bad_any_cast& ex) {
      RCLCPP_ERROR_THROTTLE(
          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());
      return;
    }
    if (rt_pub && rt_pub->trylock()) {
      rt_pub->msg_ = data;
      rt_pub->unlockAndPublish();
    }
  } else {
    if (data.is_empty()) {
      RCLCPP_DEBUG_THROTTLE(
          get_node()->get_logger(), *get_node()->get_clock(), 1000,
          "Skipping publication of output '%s' due to emptiness of state", name.c_str());
      return;
    }
    EncodedStatePublishers publishers;
    try {
      publishers = std::get<EncodedStatePublishers>(outputs_.at(name));
    } catch (const std::bad_variant_access&) {
      RCLCPP_WARN_THROTTLE(
          get_node()->get_logger(), *get_node()->get_clock(), 1000,
          "Could not retrieve publisher for output '%s': Invalid output type", name.c_str());
      return;
    }
    if (const auto output_type = std::get<0>(publishers)->get_type(); output_type != data.get_type()) {
      RCLCPP_WARN_THROTTLE(
          get_node()->get_logger(), *get_node()->get_clock(), 1000,
          "Skipping publication of output '%s' due to wrong data type (expected '%s', got '%s')",
          state_representation::get_state_type_name(output_type).c_str(),
          state_representation::get_state_type_name(data.get_type()).c_str(), name.c_str());
      return;
    }
    auto rt_pub = std::get<2>(publishers);
    if (rt_pub && rt_pub->trylock()) {
      try {
        modulo_core::translators::write_message<T>(rt_pub->msg_, data, get_node()->get_clock()->now());
      } 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->unlockAndPublish();
    }
  }
}
 
template<typename PublisherT, typename MsgT, typename T>
void BaseControllerInterface::write_std_output(const std::string& name, const T& data) {
  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;
  }
  PublisherT publishers;
  try {
    publishers = std::get<PublisherT>(outputs_.at(name));
  } catch (const std::bad_variant_access&) {
    RCLCPP_WARN_THROTTLE(
        get_node()->get_logger(), *get_node()->get_clock(), 1000,
        "Could not retrieve publisher for output '%s': Invalid output type", name.c_str());
    return;
  }
  auto rt_pub = publishers.second;
  if (rt_pub && rt_pub->trylock()) {
    rt_pub->msg_.data = data;
    rt_pub->unlockAndPublish();
  }
}
 
template<>
inline void BaseControllerInterface::write_output(const std::string& name, const bool& data) {
  write_std_output<BoolPublishers, std_msgs::msg::Bool, bool>(name, data);
}
 
template<>
inline void BaseControllerInterface::write_output(const std::string& name, const double& data) {
  write_std_output<DoublePublishers, std_msgs::msg::Float64, double>(name, data);
}
 
template<>
inline void BaseControllerInterface::write_output(const std::string& name, const std::vector<double>& data) {
  write_std_output<DoubleVecPublishers, std_msgs::msg::Float64MultiArray, std::vector<double>>(name, data);
}
 
template<>
inline void BaseControllerInterface::write_output(const std::string& name, const int& data) {
  write_std_output<IntPublishers, std_msgs::msg::Int32, int>(name, data);
}
 
template<>
inline void BaseControllerInterface::write_output(const std::string& name, const std::string& data) {
  write_std_output<StringPublishers, std_msgs::msg::String, std::string>(name, data);
}
 
template<bool acquire_lock, typename CallbackT>
inline void
BaseControllerInterface::create_service(const std::string& service_name, const std::function<CallbackT>& callback)
  requires(
      std::is_same_v<CallbackT, ControllerServiceResponse(const std::string&)>
      || std::is_same_v<CallbackT, ControllerServiceResponse(void)>)
{
  constexpr bool is_empty = std::is_same_v<CallbackT, ControllerServiceResponse(void)>;
  using ServiceT =
      std::conditional_t<is_empty, modulo_interfaces::srv::EmptyTrigger, modulo_interfaces::srv::StringTrigger>;
 
  auto parsed_service_name = validate_service_name(service_name, is_empty ? "empty" : "string");
  if (parsed_service_name.empty()) {
    return;
  }
 
  try {
    auto service = get_node()->create_service<ServiceT>(
        "~/" + parsed_service_name,
        [this, callback](
            const std::shared_ptr<typename ServiceT::Request> request,
            std::shared_ptr<typename ServiceT::Response> response) {
          try {
            auto run_callback = [&]() {
              ControllerServiceResponse callback_response;
              if constexpr (is_empty) {
                callback_response = callback();
              } else {
                callback_response = callback(request->payload);
              }
              response->success = callback_response.success;
              response->message = callback_response.message;
            };
 
            if constexpr (!acquire_lock) {
              run_callback();
            } else {
              std::unique_lock<std::timed_mutex> lock(this->command_mutex_, std::defer_lock);
              if (lock.try_lock_for(std::chrono::milliseconds{100})) {
                run_callback();
              } else {
                response->success = false;
                response->message = "Unable to acquire lock for command interface within 100ms";
              }
            }
          } catch (const std::exception& ex) {
            response->success = false;
            response->message = ex.what();
          }
        },
        qos_);
    if constexpr (is_empty) {
      empty_services_.insert_or_assign(parsed_service_name, service);
    } else {
      string_services_.insert_or_assign(parsed_service_name, service);
    }
  } catch (const std::exception& ex) {
    RCLCPP_ERROR(get_node()->get_logger(), "Failed to add service '%s': %s", parsed_service_name.c_str(), ex.what());
  }
}
 
template<typename T>
inline void BaseControllerInterface::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(
        get_node()->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(
        get_node()->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(
        get_node()->get_logger(), "Assignment with name '" + parsed_name + "' already exists, overwriting.");
  } catch (const state_representation::exceptions::InvalidParameterException& ex) {
    RCLCPP_DEBUG_STREAM(get_node()->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(
        get_node()->get_logger(), *get_node()->get_clock(), 1000,
        "Failed to add assignment '" << parsed_name << "': " << ex.what());
  }
}
 
template<typename T>
inline void BaseControllerInterface::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 = assignments_map_.get_parameter(assignment_name);
  } catch (const state_representation::exceptions::InvalidParameterException&) {
    RCLCPP_ERROR_STREAM_THROTTLE(
        get_node()->get_logger(), *get_node()->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(
        get_node()->get_logger(), *get_node()->get_clock(), 1000,
        "Failed to set assignment '" << assignment_name << "': Incompatible value type.");
    return;
  }
  if (assignment_publisher_ == nullptr) {
    RCLCPP_ERROR_STREAM_THROTTLE(
        get_node()->get_logger(), *get_node()->get_clock(), 1000,
        "No assignment publisher configured. Make sure to add assignments `on_init` of the controller.");
    return;
  }
  message.node = get_node()->get_fully_qualified_name();
  message.assignment = modulo_core::translators::write_parameter(assignment).to_parameter_msg();
  assignment_publisher_->publish(message);
}
 
template<typename T>
inline T BaseControllerInterface::get_assignment(const std::string& assignment_name) const {
  std::shared_ptr<state_representation::ParameterInterface> assignment;
  try {
    assignment = 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 + "'.");
  }
}
 
template<typename T>
inline void BaseControllerInterface::publish_transforms(
    const std::vector<state_representation::CartesianPose>& transforms, const std::shared_ptr<T>& tf_broadcaster,
    bool is_static) {
  std::string modifier = is_static ? "static " : "";
  if (tf_broadcaster == nullptr) {
    RCLCPP_ERROR_STREAM_THROTTLE(
        this->get_node()->get_logger(), *this->get_node()->get_clock(), 1000,
        "Failed to send " << modifier << "transform: No " << modifier << "TF broadcaster configured.");
    return;
  }
  try {
    std::vector<geometry_msgs::msg::TransformStamped> transform_messages;
    transform_messages.reserve(transforms.size());
    for (const auto& tf : transforms) {
      geometry_msgs::msg::TransformStamped transform_message;
      modulo_core::translators::write_message(transform_message, tf, this->get_node()->get_clock()->now());
      transform_messages.emplace_back(transform_message);
    }
    tf_broadcaster->sendTransform(transform_messages);
  } catch (const std::exception& ex) {
    RCLCPP_ERROR_STREAM_THROTTLE(
        this->get_node()->get_logger(), *this->get_node()->get_clock(), 1000,
        "Failed to send " << modifier << "transform: " << ex.what());
  }
}
 
}// namespace modulo_controllers