Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions CHANGELOG.md
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@ Release Versions:
- refactor(controllers): split up base class (#135)
- release: use updated base image (#139)
- fix(controllers): remove duplicate function (#140)
- chore: format repository (#142)

## 4.2.2

Expand Down
2 changes: 1 addition & 1 deletion aica-package.toml
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
#syntax=ghcr.io/aica-technology/package-builder:v1.1.0-rc0006
#syntax=ghcr.io/aica-technology/package-builder:v1.1.1

[metadata]
version = "5.0.0-rc0008"
Expand Down
43 changes: 23 additions & 20 deletions source/modulo_components/include/modulo_components/Component.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -65,8 +65,7 @@ class Component : public rclcpp::Node, public ComponentInterface {
template<typename DataT>
void add_output(
const std::string& signal_name, const std::shared_ptr<DataT>& data, const std::string& default_topic = "",
bool fixed_topic = false, bool publish_on_step = true
);
bool fixed_topic = false, bool publish_on_step = true);

private:
/**
Expand All @@ -81,75 +80,79 @@ class Component : public rclcpp::Node, public ComponentInterface {
void on_execute();

// TODO hide ROS methods
using ComponentInterface::get_parameter;
using ComponentInterface::create_output;
using ComponentInterface::evaluate_periodic_callbacks;
using ComponentInterface::get_parameter;
using ComponentInterface::inputs_;
using ComponentInterface::outputs_;
using ComponentInterface::periodic_outputs_;
using ComponentInterface::publish_predicates;
using ComponentInterface::publish_outputs;
using ComponentInterface::evaluate_periodic_callbacks;
using ComponentInterface::publish_predicates;
using rclcpp::Node::get_parameter;

std::thread execute_thread_; ///< The execution thread of the component
bool started_; ///< Flag that indicates if execution has started or not
std::thread execute_thread_;///< The execution thread of the component
bool started_; ///< Flag that indicates if execution has started or not
};

template<typename DataT>
inline void Component::add_output(
const std::string& signal_name, const std::shared_ptr<DataT>& data, const std::string& default_topic,
bool fixed_topic, bool publish_on_step
) {
bool fixed_topic, bool publish_on_step) {
using namespace modulo_core::communication;
try {
auto parsed_signal_name =
this->create_output(PublisherType::PUBLISHER, signal_name, data, default_topic, fixed_topic, publish_on_step);
auto topic_name = this->get_parameter_value<std::string>(parsed_signal_name + "_topic");
RCLCPP_DEBUG_STREAM(this->get_logger(),
"Adding output '" << parsed_signal_name << "' with topic name '" << topic_name << "'.");
RCLCPP_DEBUG_STREAM(
this->get_logger(), "Adding output '" << parsed_signal_name << "' with topic name '" << topic_name << "'.");
auto message_pair = this->outputs_.at(parsed_signal_name)->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());
this->outputs_.at(parsed_signal_name) =
std::make_shared<PublisherHandler<rclcpp::Publisher<std_msgs::msg::Bool>, std_msgs::msg::Bool>>(
PublisherType::PUBLISHER, publisher)->create_publisher_interface(message_pair);
PublisherType::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());
this->outputs_.at(parsed_signal_name) =
std::make_shared<PublisherHandler<rclcpp::Publisher<std_msgs::msg::Float64>, std_msgs::msg::Float64>>(
PublisherType::PUBLISHER, publisher)->create_publisher_interface(message_pair);
PublisherType::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());
this->outputs_.at(parsed_signal_name) = std::make_shared<
PublisherHandler<
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>, std_msgs::msg::Float64MultiArray>>(
PublisherType::PUBLISHER, publisher)->create_publisher_interface(message_pair);
this->outputs_.at(parsed_signal_name) = std::make_shared<PublisherHandler<
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>, std_msgs::msg::Float64MultiArray>>(
PublisherType::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());
this->outputs_.at(parsed_signal_name) =
std::make_shared<PublisherHandler<rclcpp::Publisher<std_msgs::msg::Int32>, std_msgs::msg::Int32>>(
PublisherType::PUBLISHER, publisher)->create_publisher_interface(message_pair);
PublisherType::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());
this->outputs_.at(parsed_signal_name) =
std::make_shared<PublisherHandler<rclcpp::Publisher<std_msgs::msg::String>, std_msgs::msg::String>>(
PublisherType::PUBLISHER, publisher)->create_publisher_interface(message_pair);
PublisherType::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());
this->outputs_.at(parsed_signal_name) =
std::make_shared<PublisherHandler<rclcpp::Publisher<modulo_core::EncodedState>, modulo_core::EncodedState>>(
PublisherType::PUBLISHER, publisher)->create_publisher_interface(message_pair);
PublisherType::PUBLISHER, publisher)
->create_publisher_interface(message_pair);
break;
}
}
Expand Down
Loading