Skip to content

Commit

Permalink
feat: optimize creation and configuration of lifecycle publishers (#149)
Browse files Browse the repository at this point in the history
  • Loading branch information
@domire8 @bpapaspyros
domire8 authored and bpapaspyros committed Oct 4, 2024
1 parent 955f345 commit 2494235
Show file tree
Hide file tree
Showing 2 changed files with 63 additions and 82 deletions.
90 changes: 9 additions & 81 deletions source/modulo_components/include/modulo_components/LifecycleComponent.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -277,7 +277,7 @@ class LifecycleComponent : public rclcpp_lifecycle::LifecycleNode, public Compon
std::map<
std::string,
std::function<std::shared_ptr<modulo_core::communication::PublisherInterface>(const std::string& topic_name)>>
output_configuration_callables_;///< Map of output configuration callables
custom_output_configuration_callables_;///< Map of custom output configuration callables
};

template<typename DataT>
Expand All @@ -292,94 +292,22 @@ inline void LifecycleComponent::add_output(
return;
}
try {
auto parsed_signal_name = this->create_output(
modulo_core::communication::PublisherType::LIFECYCLE_PUBLISHER, signal_name, data, default_topic, fixed_topic,
publish_on_step);

using modulo_core::communication::MessageType;
using modulo_core::communication::PublisherHandler;
using modulo_core::communication::PublisherInterface;
using modulo_core::communication::PublisherType;

auto parsed_signal_name = this->create_output(
PublisherType::LIFECYCLE_PUBLISHER, signal_name, data, default_topic, fixed_topic, publish_on_step);

auto message_pair = this->outputs_.at(parsed_signal_name)->get_message_pair();
switch (message_pair->get_type()) {
case MessageType::BOOL: {
this->output_configuration_callables_.insert_or_assign(
if (message_pair->get_type() == modulo_core::communication::MessageType::CUSTOM_MESSAGE) {
if constexpr (modulo_core::concepts::CustomT<DataT>) {
this->custom_output_configuration_callables_.insert_or_assign(
parsed_signal_name, [this, message_pair](const std::string& topic_name) {
auto publisher = this->create_publisher<std_msgs::msg::Bool>(topic_name, this->get_qos());
return std::make_shared<PublisherHandler<
rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::Bool>, std_msgs::msg::Bool>>(
auto publisher = this->create_publisher<DataT>(topic_name, this->get_qos());
return std::make_shared<PublisherHandler<rclcpp_lifecycle::LifecyclePublisher<DataT>, DataT>>(
PublisherType::LIFECYCLE_PUBLISHER, publisher)
->create_publisher_interface(message_pair);
});
break;
}
case MessageType::FLOAT64: {
this->output_configuration_callables_.insert_or_assign(
parsed_signal_name, [this, message_pair](const std::string& topic_name) {
auto publisher = this->create_publisher<std_msgs::msg::Float64>(topic_name, this->get_qos());
return std::make_shared<PublisherHandler<
rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::Float64>, std_msgs::msg::Float64>>(
PublisherType::LIFECYCLE_PUBLISHER, publisher)
->create_publisher_interface(message_pair);
});
break;
}
case MessageType::FLOAT64_MULTI_ARRAY: {
this->output_configuration_callables_.insert_or_assign(
parsed_signal_name, [this, message_pair](const std::string& topic_name) {
auto publisher = this->create_publisher<std_msgs::msg::Float64MultiArray>(topic_name, this->get_qos());
return std::make_shared<PublisherHandler<
rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::Float64MultiArray>,
std_msgs::msg::Float64MultiArray>>(PublisherType::LIFECYCLE_PUBLISHER, publisher)
->create_publisher_interface(message_pair);
});
break;
}
case MessageType::INT32: {
this->output_configuration_callables_.insert_or_assign(
parsed_signal_name, [this, message_pair](const std::string& topic_name) {
auto publisher = this->create_publisher<std_msgs::msg::Int32>(topic_name, this->get_qos());
return std::make_shared<PublisherHandler<
rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::Int32>, std_msgs::msg::Int32>>(
PublisherType::LIFECYCLE_PUBLISHER, publisher)
->create_publisher_interface(message_pair);
});
break;
}
case MessageType::STRING: {
this->output_configuration_callables_.insert_or_assign(
parsed_signal_name, [this, message_pair](const std::string& topic_name) {
auto publisher = this->create_publisher<std_msgs::msg::String>(topic_name, this->get_qos());
return std::make_shared<PublisherHandler<
rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::String>, std_msgs::msg::String>>(
PublisherType::LIFECYCLE_PUBLISHER, publisher)
->create_publisher_interface(message_pair);
});
break;
}
case MessageType::ENCODED_STATE: {
this->output_configuration_callables_.insert_or_assign(
parsed_signal_name, [this, message_pair](const std::string& topic_name) {
auto publisher = this->create_publisher<modulo_core::EncodedState>(topic_name, this->get_qos());
return std::make_shared<PublisherHandler<
rclcpp_lifecycle::LifecyclePublisher<modulo_core::EncodedState>, modulo_core::EncodedState>>(
PublisherType::LIFECYCLE_PUBLISHER, publisher)
->create_publisher_interface(message_pair);
});
break;
}
case MessageType::CUSTOM_MESSAGE: {
if constexpr (modulo_core::concepts::CustomT<DataT>) {
this->output_configuration_callables_.insert_or_assign(
parsed_signal_name, [this, message_pair](const std::string& topic_name) {
auto publisher = this->create_publisher<DataT>(topic_name, this->get_qos());
return std::make_shared<PublisherHandler<rclcpp_lifecycle::LifecyclePublisher<DataT>, DataT>>(
PublisherType::LIFECYCLE_PUBLISHER, publisher)
->create_publisher_interface(message_pair);
});
}
break;
}
}
} catch (const modulo_core::exceptions::AddSignalException& ex) {
Expand Down
55 changes: 54 additions & 1 deletion source/modulo_components/src/LifecycleComponent.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -243,7 +243,60 @@ bool LifecycleComponent::configure_outputs() {
auto topic_name = this->get_parameter_value<std::string>(name + "_topic");
RCLCPP_DEBUG_STREAM(
this->get_logger(), "Configuring output '" << name << "' with topic name '" << topic_name << "'.");
interface = this->output_configuration_callables_.at(name)(topic_name);
auto message_pair = interface->get_message_pair();
switch (message_pair->get_type()) {
case MessageType::BOOL: {
auto publisher = this->create_publisher<std_msgs::msg::Bool>(topic_name, this->get_qos());
interface = std::make_shared<PublisherHandler<LifecyclePublisher<std_msgs::msg::Bool>, std_msgs::msg::Bool>>(
PublisherType::LIFECYCLE_PUBLISHER, publisher)
->create_publisher_interface(message_pair);
break;
}
case MessageType::FLOAT64: {
auto publisher = this->create_publisher<std_msgs::msg::Float64>(topic_name, this->get_qos());
interface =
std::make_shared<PublisherHandler<LifecyclePublisher<std_msgs::msg::Float64>, std_msgs::msg::Float64>>(
PublisherType::LIFECYCLE_PUBLISHER, publisher)
->create_publisher_interface(message_pair);
break;
}
case MessageType::FLOAT64_MULTI_ARRAY: {
auto publisher = this->create_publisher<std_msgs::msg::Float64MultiArray>(topic_name, this->get_qos());
interface = std::make_shared<PublisherHandler<
LifecyclePublisher<std_msgs::msg::Float64MultiArray>, std_msgs::msg::Float64MultiArray>>(
PublisherType::LIFECYCLE_PUBLISHER, publisher)
->create_publisher_interface(message_pair);
break;
}
case MessageType::INT32: {
auto publisher = this->create_publisher<std_msgs::msg::Int32>(topic_name, this->get_qos());
interface =
std::make_shared<PublisherHandler<LifecyclePublisher<std_msgs::msg::Int32>, std_msgs::msg::Int32>>(
PublisherType::LIFECYCLE_PUBLISHER, publisher)
->create_publisher_interface(message_pair);
break;
}
case MessageType::STRING: {
auto publisher = this->create_publisher<std_msgs::msg::String>(topic_name, this->get_qos());
interface =
std::make_shared<PublisherHandler<LifecyclePublisher<std_msgs::msg::String>, std_msgs::msg::String>>(
PublisherType::LIFECYCLE_PUBLISHER, publisher)
->create_publisher_interface(message_pair);
break;
}
case MessageType::ENCODED_STATE: {
auto publisher = this->create_publisher<modulo_core::EncodedState>(topic_name, this->get_qos());
interface = std::make_shared<
PublisherHandler<LifecyclePublisher<modulo_core::EncodedState>, modulo_core::EncodedState>>(
PublisherType::LIFECYCLE_PUBLISHER, publisher)
->create_publisher_interface(message_pair);
break;
}
case MessageType::CUSTOM_MESSAGE: {
interface = this->custom_output_configuration_callables_.at(name)(topic_name);
break;
}
}
} catch (const modulo_core::exceptions::CoreException& ex) {
success = false;
RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to configure output '" << name << "': " << ex.what());
Expand Down

0 comments on commit 2494235

Please sign in to comment.