diff --git a/cs4home_core/CMakeLists.txt b/cs4home_core/CMakeLists.txt index 774d5a3..54b823a 100644 --- a/cs4home_core/CMakeLists.txt +++ b/cs4home_core/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.8) +cmake_minimum_required(VERSION 3.10) project(cs4home_core) set(CMAKE_BUILD_TYPE debug) @@ -11,11 +11,13 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) +find_package(rclcpp_cascade_lifecycle REQUIRED) find_package(rclcpp_components REQUIRED) set(dependencies rclcpp rclcpp_lifecycle + rclcpp_cascade_lifecycle rclcpp_components ) diff --git a/cs4home_core/include/cs4home_core/Afferent.hpp b/cs4home_core/include/cs4home_core/Afferent.hpp index 30750cc..16d55dd 100644 --- a/cs4home_core/include/cs4home_core/Afferent.hpp +++ b/cs4home_core/include/cs4home_core/Afferent.hpp @@ -12,133 +12,190 @@ // See the License for the specific language governing permissions and // limitations under the License. - #ifndef CS4HOME_CORE__AFFERENT_HPP_ #define CS4HOME_CORE__AFFERENT_HPP_ +#include #include -#include #include -#include #include +#include -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "rclcpp/rclcpp.hpp" +#include "rclcpp/create_generic_subscription.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/rclcpp.hpp" #include "rclcpp/serialization.hpp" -#include "rclcpp/create_generic_subscription.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" -namespace cs4home_core -{ +namespace cs4home_core { /** * @class Afferent - * @brief Manages afferent processing in robotic nodes, including message handling, - * subscriptions, and modes for processing serialized data. + * @brief Manages afferent processing in robotic nodes, including message + * handling, subscriptions, and modes for processing serialized data. */ -class Afferent -{ +class Afferent { public: RCLCPP_SMART_PTR_DEFINITIONS(Afferent) /** - * @enum EfferentProcessMode - * @brief Defines processing modes for serialized message handling. - */ - enum EfferentProcessMode {CALLBACK, ONDEMAND}; + * @enum AfferentProcessMode + * @brief Defines processing modes for handling incoming serialized messages + * in an Afferent component. + * + * - CALLBACK: Incoming messages are processed immediately upon arrival via a + * callback function. + * + * - ONDEMAND: Incoming messages are queued and accessed explicitly through a + * `get_msg()` function, allowing controlled and deferred processing. + */ + enum AfferentProcessMode { CALLBACK, ONDEMAND }; /** * @brief Constructor for the Afferent class. * @param parent Shared pointer to the lifecycle node managing this instance. */ - explicit Afferent(rclcpp_lifecycle::LifecycleNode::SharedPtr parent); + explicit Afferent(const std::string &name, + rclcpp_lifecycle::LifecycleNode::SharedPtr parent); /** - * @brief Configures the afferent component; intended for subclass implementation. + * @brief Configures the afferent component; intended for subclass + * implementation. * @return True if configuration is successful. */ - virtual bool configure() = 0; + virtual bool configure(); /** * @brief Sets the processing mode and an optional callback function. * + * @param topic The topic to subscribe * @param mode Processing mode for handling messages. - * @param cb Optional callback function for handling serialized messages in CALLBACK mode. + * @param cb Optional callback function for handling serialized messages in + * CALLBACK mode. */ - void set_mode( - EfferentProcessMode mode, - std::function)> cb = nullptr); + void set_mode(const std::string &topic, AfferentProcessMode mode, + std::function)> + cb = nullptr); + /** + * @brief Sets the processing mode and an optional callback function. + * + * @param topic_idx The index of the topic input. + * @param mode Processing mode for handling messages. + * @param cb Optional callback function for handling serialized messages in + * CALLBACK mode. + */ + void set_mode(size_t topic_idx, AfferentProcessMode mode, + std::function)> + cb = nullptr); /** * @brief Gets the current processing mode. - * @return The current EfferentProcessMode. + * @return The current AfferentProcessMode. */ - EfferentProcessMode get_mode() {return mode_;} + AfferentProcessMode get_mode() { return mode_; } /** * @brief Sets the maximum queue size for storing messages. * @param size Maximum number of messages the queue can hold. */ - void set_max_queue_size(size_t size) {max_queue_size_ = size;} + void set_max_queue_size(size_t size) { max_queue_size_ = size; } /** * @brief Gets the maximum queue size. * @return The maximum queue size. */ - size_t get_max_queue_size() {return max_queue_size_;} + size_t get_max_queue_size() { return max_queue_size_; } /** - * @brief Converts a serialized message to a typed message. - * @tparam MessageT Type of the message to deserialize. - * @param msg Serialized message to convert. - * @return A unique pointer to the deserialized message. - */ - template std::unique_ptr get_msg( - std::unique_ptr msg) - { - auto typed_msg = std::make_unique(); + * @brief Converts a serialized message to a typed message. + * @tparam MessageT Type of the message to deserialize. + * @param msg Serialized message to convert. + * @return A shared pointer to the deserialized message. + */ + template + std::shared_ptr + get_msg(std::shared_ptr msg) { + auto typed_msg = std::make_shared(); rclcpp::Serialization serializer; serializer.deserialize_message(msg.get(), typed_msg.get()); - return std::move(typed_msg); + return typed_msg; } /** * @brief Retrieves the next message from the queue, if available. * @tparam MessageT Type of message to retrieve. - * @return A unique pointer to the next message, or nullptr if the queue is empty. + * @tparam topic The topic of the associated queue to retrieve the message. + * @return A unique pointer to the next message, or nullptr if the queue is + * empty or does not exists. */ - template std::unique_ptr get_msg() - { - if (msg_queue_.empty()) { - return {}; + template + std::shared_ptr get_msg(const std::string &topic) { + if (msg_queues_.find(topic) == msg_queues_.end() || + msg_queues_[topic].empty()) { + return nullptr; } - std::unique_ptr msg = std::move(msg_queue_.front()); - msg_queue_.pop(); + std::shared_ptr msg = msg_queues_[topic].front(); + msg_queues_[topic].pop(); - return get_msg(std::move(msg)); + return get_msg(msg); + } + + /** + * @brief Retrieves the next message from the queue, if available. + * @tparam MessageT Type of message to retrieve. + * @tparam topic_idx The index fn the topic input associated queue to retrieve + * the message. + * @return A unique pointer to the next message, or nullptr if the queue is + * empty or does not exists. + */ + template + std::shared_ptr get_msg(size_t topic_idx) { + if (topic_idx >= msg_queues_.size()) { + return nullptr; + } + + const std::string &topic = input_topic_names_[topic_idx]; + + if (msg_queues_.find(topic) == msg_queues_.end() || + msg_queues_[topic].empty()) { + return nullptr; + } + + std::shared_ptr msg = msg_queues_[topic].front(); + msg_queues_[topic].pop(); + + return get_msg(msg); } protected: /** Shared pointer to the parent node. */ rclcpp_lifecycle::LifecycleNode::SharedPtr parent_; - /** List of subscriptions. */ - std::vector> subs_; + std::string name_; - EfferentProcessMode mode_ {ONDEMAND}; /**< Current processing mode. */ + AfferentProcessMode mode_{ONDEMAND}; /**< Current processing mode. */ /** Default maximum queue size. */ const size_t MAX_DEFAULT_QUEUE_SIZE = 100; /** Maximum queue size. */ - size_t max_queue_size_ {MAX_DEFAULT_QUEUE_SIZE}; + size_t max_queue_size_{MAX_DEFAULT_QUEUE_SIZE}; + + /** List of subscriptions. */ + std::vector> subs_; /** Queue for serialized messages. */ - std::queue> msg_queue_; + std::map>> + msg_queues_; + /**< List of input topics to subscribe to for images. */ + std::vector input_topic_names_; + /**< List of input topics types. */ + std::vector input_topic_types_; /** Callback for serialized messages. */ - std::function)> callback_; - + std::map)>> + callbacks_; /** * @brief Creates a subscriber for a specific topic and message type. @@ -146,9 +203,9 @@ class Afferent * @param type Type of message for the subscription. * @return True if the subscriber was created successfully. */ - bool create_subscriber(const std::string & topic, const std::string & type); + bool create_subscriber(const std::string &topic, const std::string &type); }; -} // namespace cs4home_core +} // namespace cs4home_core -#endif // CS4HOME_CORE__AFFERENT_HPP_ +#endif // CS4HOME_CORE__AFFERENT_HPP_ diff --git a/cs4home_core/include/cs4home_core/CognitiveModule.hpp b/cs4home_core/include/cs4home_core/CognitiveModule.hpp index 57885cd..f4b1724 100644 --- a/cs4home_core/include/cs4home_core/CognitiveModule.hpp +++ b/cs4home_core/include/cs4home_core/CognitiveModule.hpp @@ -28,16 +28,17 @@ #include "rclcpp/macros.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp" namespace cs4home_core { +class CognitiveModule : public rclcpp_cascade_lifecycle::CascadeLifecycleNode /** * @class CognitiveModule - * @brief Extends the LifecycleNode to manage cognitive processing components in a ROS 2 lifecycle, + * @brief Extends the Cascade LifecycleNode to manage cognitive processing components in a ROS 2 lifecycle, * including afferent, efferent, core, meta, and coupling components. */ -class CognitiveModule : public rclcpp_lifecycle::LifecycleNode { public: RCLCPP_SMART_PTR_DEFINITIONS(CognitiveModule) diff --git a/cs4home_core/include/cs4home_core/Core.hpp b/cs4home_core/include/cs4home_core/Core.hpp index be7e701..a9b0630 100644 --- a/cs4home_core/include/cs4home_core/Core.hpp +++ b/cs4home_core/include/cs4home_core/Core.hpp @@ -16,6 +16,7 @@ #define CS4HOME_CORE__CORE_HPP_ #include +#include #include "cs4home_core/Afferent.hpp" #include "cs4home_core/Efferent.hpp" @@ -40,7 +41,8 @@ class Core * @brief Constructs a Core object associated with a parent lifecycle node. * @param parent Shared pointer to the lifecycle node managing this Core instance. */ - explicit Core(rclcpp_lifecycle::LifecycleNode::SharedPtr parent); + explicit Core( + const std::string & name, rclcpp_lifecycle::LifecycleNode::SharedPtr parent); /** * @brief Configures the Core component. @@ -75,6 +77,8 @@ class Core protected: /** Shared pointer to the parent lifecycle node. */ rclcpp_lifecycle::LifecycleNode::SharedPtr parent_; + + std::string name_; /** Shared pointer to the Afferent component. */ cs4home_core::Afferent::SharedPtr afferent_; /** Shared pointer to the Efferent component. */ diff --git a/cs4home_core/include/cs4home_core/Coupling.hpp b/cs4home_core/include/cs4home_core/Coupling.hpp index ffaa10d..96afcaa 100644 --- a/cs4home_core/include/cs4home_core/Coupling.hpp +++ b/cs4home_core/include/cs4home_core/Coupling.hpp @@ -15,6 +15,8 @@ #ifndef CS4HOME_CORE__COUPLING_HPP_ #define CS4HOME_CORE__COUPLING_HPP_ +#include + #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rclcpp/macros.hpp" @@ -35,7 +37,8 @@ class Coupling * @brief Constructs a Coupling object associated with a parent lifecycle node. * @param parent Shared pointer to the lifecycle node managing this Coupling instance. */ - explicit Coupling(rclcpp_lifecycle::LifecycleNode::SharedPtr parent); + explicit Coupling( + const std::string & name, rclcpp_lifecycle::LifecycleNode::SharedPtr parent); /** * @brief Configures the Coupling component. @@ -46,6 +49,7 @@ class Coupling protected: /**< Shared pointer to the parent lifecycle node. */ rclcpp_lifecycle::LifecycleNode::SharedPtr parent_; + std::string name_; }; } // namespace cs4home_core diff --git a/cs4home_core/include/cs4home_core/Efferent.hpp b/cs4home_core/include/cs4home_core/Efferent.hpp index 2dea9e3..195e19d 100644 --- a/cs4home_core/include/cs4home_core/Efferent.hpp +++ b/cs4home_core/include/cs4home_core/Efferent.hpp @@ -19,34 +19,36 @@ #include #include -#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/serialization.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" -namespace cs4home_core -{ +namespace cs4home_core { /** * @class Efferent - * @brief Manages efferent operations in the robotic system, including the configuration - * of publishers and message broadcasting. + * @brief Manages efferent operations in the robotic system, including the + * configuration of publishers and message broadcasting. */ -class Efferent -{ +class Efferent { public: RCLCPP_SMART_PTR_DEFINITIONS(Efferent) /** - * @brief Constructs an Efferent object associated with a parent lifecycle node. - * @param parent Shared pointer to the lifecycle node managing this Efferent instance. + * @brief Constructs an Efferent object associated with a parent lifecycle + * node. + * @param name name of the component. + * @param parent Shared pointer to the lifecycle node managing this Efferent + * instance. */ - explicit Efferent(rclcpp_lifecycle::LifecycleNode::SharedPtr parent); + explicit Efferent(const std::string &name, + rclcpp_lifecycle::LifecycleNode::SharedPtr parent); /** * @brief Configures the Efferent component. * @return True if configuration is successful. */ - virtual bool configure() = 0; + virtual bool configure(); /** * @brief Publishes a serialized message to all configured publishers. @@ -55,36 +57,46 @@ class Efferent * each publisher in the `pubs_` list. * * @tparam MessageT Type of the message to publish. - * @param msg Unique pointer to the message to broadcast. + * @param msg shared pointer to the message to broadcast. */ - template - void publish(std::unique_ptr msg) - { + template + void publish(size_t topic_index, std::shared_ptr msg) { + if (topic_index >= pubs_.size()) { + RCLCPP_WARN(parent_->get_logger(), + "[Efferent] Error publishing: topic index not valid"); + return; + } + rclcpp::Serialization serializer; auto untyped_msg = rclcpp::SerializedMessage(); serializer.serialize_message(msg.get(), &untyped_msg); - for (auto & pub : pubs_) { - pub->publish(untyped_msg); - } + pubs_[topic_index]->publish(untyped_msg); } protected: /**< Shared pointer to the parent lifecycle node. */ rclcpp_lifecycle::LifecycleNode::SharedPtr parent_; + std::string name_; + /**< List of generic publishers. */ std::vector> pubs_; + /**< List of output topics to publish images. */ + std::vector output_topic_names_; + /**< List of output topics types. */ + std::vector output_topic_types_; + /** * @brief Creates a publisher for a specified topic and message type. * @param topic The topic name to publish messages to. * @param type The type of messages to publish on the topic. * @return True if the publisher was created successfully. */ - bool create_publisher(const std::string & topic, const std::string & type); + bool create_publisher(const std::string &topic, const std::string &type); }; -} // namespace cs4home_core +} // namespace cs4home_core -#endif // CS4HOME_CORE__EFFERENT_HPP_ +#endif // CS4HOME_CORE__EFFERENT_HPP_ diff --git a/cs4home_core/include/cs4home_core/Flow.hpp b/cs4home_core/include/cs4home_core/Flow.hpp index 2fdc463..31771d2 100644 --- a/cs4home_core/include/cs4home_core/Flow.hpp +++ b/cs4home_core/include/cs4home_core/Flow.hpp @@ -22,6 +22,8 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp" namespace cs4home_core { @@ -31,7 +33,7 @@ namespace cs4home_core * @brief Represents a sequence of nodes within a robotic system, with utilities * to manage and print the node sequence. */ -class Flow +class Flow : public rclcpp_cascade_lifecycle::CascadeLifecycleNode { public: RCLCPP_SMART_PTR_DEFINITIONS(Flow) @@ -40,18 +42,22 @@ class Flow * @brief Constructs a Flow object with a specified sequence of nodes. * @param nodes A vector of node names to initialize the flow sequence. */ - explicit Flow(const std::vector & nodes); + explicit Flow(const std::string & name); + Flow(const std::string & name, const std::vector & nodes); /** * @brief Prints the sequence of nodes in the flow to the standard output. */ void print() const; - /** * @brief Retrieves the sequence of nodes in the flow. * @return A constant reference to the vector of node names. */ - const std::vector & get_nodes() const {return nodes_;} + const std::vector & get_flow() const {return nodes_;} + void set_flow(const std::vector & nodes); + + void activate(); + void deactivate(); private: std::vector nodes_; /**< Sequence of nodes in the flow. */ diff --git a/cs4home_core/include/cs4home_core/Master.hpp b/cs4home_core/include/cs4home_core/Master.hpp index c67cfd3..c249025 100644 --- a/cs4home_core/include/cs4home_core/Master.hpp +++ b/cs4home_core/include/cs4home_core/Master.hpp @@ -17,8 +17,10 @@ #include #include +#include #include "cs4home_core/CognitiveModule.hpp" +#include "cs4home_core/Flow.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rclcpp/rclcpp.hpp" @@ -39,10 +41,12 @@ class Master : public rclcpp_lifecycle::LifecycleNode using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; /** - * @brief Constructs a Master lifecycle node with the specified options. + * @brief Constructs a Master cascade lifecycle node with the specified options. * @param options Node options to configure the Master node. */ - explicit Master(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + explicit Master( + const std::string & name, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); /** * @brief Configures the Master node. @@ -86,9 +90,10 @@ class Master : public rclcpp_lifecycle::LifecycleNode */ CallbackReturnT on_error(const rclcpp_lifecycle::State & state); + const std::map & get_flows() const {return flows_;} + protected: - /** Map of cognitive modules managed by the Master node. */ - std::map cog_modules_; + std::map flows_; }; } // namespace cs4home_core diff --git a/cs4home_core/include/cs4home_core/Meta.hpp b/cs4home_core/include/cs4home_core/Meta.hpp index 8431ad2..fc2e6ae 100644 --- a/cs4home_core/include/cs4home_core/Meta.hpp +++ b/cs4home_core/include/cs4home_core/Meta.hpp @@ -15,6 +15,8 @@ #ifndef CS4HOME_CORE__META_HPP_ #define CS4HOME_CORE__META_HPP_ +#include + #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rclcpp/macros.hpp" @@ -35,7 +37,7 @@ class Meta * @brief Constructs a Meta object associated with a parent lifecycle node. * @param parent Shared pointer to the lifecycle node managing this Meta instance. */ - explicit Meta(rclcpp_lifecycle::LifecycleNode::SharedPtr parent); + explicit Meta(const std::string & name, rclcpp_lifecycle::LifecycleNode::SharedPtr parent); /** * @brief Configures the Meta component. @@ -46,6 +48,7 @@ class Meta protected: /// < Shared pointer to the parent lifecycle node. rclcpp_lifecycle::LifecycleNode::SharedPtr parent_; + std::string name_; }; } // namespace cs4home_core diff --git a/cs4home_core/package.xml b/cs4home_core/package.xml index bc5d309..8803647 100644 --- a/cs4home_core/package.xml +++ b/cs4home_core/package.xml @@ -10,6 +10,7 @@ rclcpp rclcpp_lifecycle + rclcpp_cascade_lifecycle rclcpp_components ament_lint_auto diff --git a/cs4home_core/src/cs4home_core/Afferent.cpp b/cs4home_core/src/cs4home_core/Afferent.cpp index 0ab5ab6..7ab96f9 100644 --- a/cs4home_core/src/cs4home_core/Afferent.cpp +++ b/cs4home_core/src/cs4home_core/Afferent.cpp @@ -12,21 +12,23 @@ // See the License for the specific language governing permissions and // limitations under the License. - #include "cs4home_core/Afferent.hpp" -#include "rclcpp/rclcpp.hpp" #include "rclcpp/create_generic_subscription.hpp" -namespace cs4home_core -{ +#include "rclcpp/rclcpp.hpp" +namespace cs4home_core { /** * @brief Constructor for the Afferent class. - * @param parent Shared pointer to the lifecycle node that owns this Afferent instance. + * @param parent Shared pointer to the lifecycle node that owns this Afferent + * instance. */ -Afferent::Afferent(rclcpp_lifecycle::LifecycleNode::SharedPtr parent) -: parent_(parent) -{ +Afferent::Afferent(const std::string &name, + rclcpp_lifecycle::LifecycleNode::SharedPtr parent) + : parent_(parent), name_(name) { + // Declares the parameter for input topics. and types + parent_->declare_parameter(name_ + ".topics", input_topic_names_); + parent_->declare_parameter(name_ + ".types", input_topic_types_); } /** @@ -35,26 +37,59 @@ Afferent::Afferent(rclcpp_lifecycle::LifecycleNode::SharedPtr parent) * This function allows configuring the Afferent object with a specific * processing mode and an optional callback to handle serialized messages. * + * @param topic The topic to subscribe * @param mode The processing mode for the Afferent object. - * @param cb A callback function to process serialized messages, used if the mode is CALLBACK. + * @param cb A callback function to process serialized messages, used if the + * mode is CALLBACK. */ -void -Afferent::set_mode( - EfferentProcessMode mode, - std::function)> cb) -{ +void Afferent::set_mode( + const std::string &topic, AfferentProcessMode mode, + std::function)> cb) { if (mode == CALLBACK) { if (cb) { - callback_ = cb; + callbacks_[topic] = cb; } else { - RCLCPP_WARN( - parent_->get_logger(), "[Afferent] Error setting callback: not function specified"); + RCLCPP_WARN(parent_->get_logger(), + "[Afferent] Error setting callback: not function specified"); return; } } mode_ = mode; } +/** + * @brief Sets the operation mode and an optional callback function. + * + * This function allows configuring the Afferent object with a specific + * processing mode and an optional callback to handle serialized messages. + * + * @param topic_idx The index of the topic input. + * @param mode The processing mode for the Afferent object. + * @param cb A callback function to process serialized messages, used if the + * mode is CALLBACK. + */ +void Afferent::set_mode( + size_t topic_idx, AfferentProcessMode mode, + std::function)> cb) { + if (topic_idx >= input_topic_names_.size()) { + RCLCPP_WARN(parent_->get_logger(), + "[Afferent] Error setting callback: topic index not valid"); + return; + } + + const std::string &topic = input_topic_names_[topic_idx]; + + if (mode == CALLBACK) { + if (cb) { + callbacks_[topic] = cb; + } else { + RCLCPP_WARN(parent_->get_logger(), + "[Afferent] Error setting callback: not function specified"); + return; + } + } + mode_ = mode; +} /** * @brief Creates a subscription to a specified topic and type. @@ -67,37 +102,83 @@ Afferent::set_mode( * @param type The type of messages expected on the topic. * @return True if the subscription was created successfully. */ -bool -Afferent::create_subscriber(const std::string & topic, const std::string & type) -{ - RCLCPP_DEBUG( - parent_->get_logger(), - "[Afferent] Creating subscription [%s, %s]", - topic.c_str(), type.c_str()); +bool Afferent::create_subscriber(const std::string &topic, + const std::string &type) { + RCLCPP_INFO(parent_->get_logger(), + "[Afferent] Creating subscription [%s, %s]", topic.c_str(), + type.c_str()); + + if (msg_queues_.find(topic) == msg_queues_.end()) { + msg_queues_[topic] = + std::queue>(); + } auto sub = rclcpp::create_generic_subscription( - parent_->get_node_topics_interface(), topic, type, 100, - [&](std::unique_ptr msg) - { - if (mode_ == CALLBACK) { - if (callback_) { - callback_(std::move(msg)); + parent_->get_node_topics_interface(), topic, type, rclcpp::QoS(100), + [&](std::shared_ptr msg) { + if (mode_ == CALLBACK) { + if (callbacks_[topic]) { + callbacks_[topic](msg); + } else { + msg_queues_[topic].push(msg); + if (msg_queues_[topic].size() > max_queue_size_) { + msg_queues_[topic].pop(); + } + } } else { - RCLCPP_WARN( - parent_->get_logger(), "[Afferent] Error calling callback: not function specified"); + msg_queues_[topic].push(msg); + if (msg_queues_[topic].size() > max_queue_size_) { + msg_queues_[topic].pop(); + } } - } else { - msg_queue_.push(std::move(msg)); - if (msg_queue_.size() > max_queue_size_) { - msg_queue_.pop(); - } - } - }); - + }); subs_.push_back(sub); return true; } -} // namespace cs4home_core +/** + * @brief Configures the Afferent by creating subscribers for each specified + * topic. + * + * This method retrieves the topic names from the parameter server and attempts + * to create a subscription for each topic to receive messages. + * + * @return True if all subscriptions are created successfully. + */ +bool Afferent::configure() { + parent_->get_parameter(name_ + ".topics", input_topic_names_); + parent_->get_parameter(name_ + ".types", input_topic_types_); + + if (input_topic_names_.size() != input_topic_types_.size()) { + RCLCPP_ERROR(parent_->get_logger(), + "Parameter sizes are not correct (%zu != %zu)", + input_topic_names_.size(), input_topic_types_.size()); + return false; + } + + RCLCPP_DEBUG(parent_->get_logger(), + "Creating %zu subscriptions: ", input_topic_names_.size()); + + for (size_t i = 0; i < input_topic_names_.size(); i++) { + RCLCPP_DEBUG(parent_->get_logger(), + "\tCreating subscription for topic [%s] (%s)", + input_topic_names_[i].c_str(), input_topic_types_[i].c_str()); + + if (create_subscriber(input_topic_names_[i], input_topic_types_[i])) { + RCLCPP_DEBUG(parent_->get_logger(), + "[SimpleImageInput] created subscription to [%s, %s]", + input_topic_names_[i].c_str(), + input_topic_types_[i].c_str()); + } else { + RCLCPP_WARN(parent_->get_logger(), + "[SimpleImageInput] Couldn't create subscription to [%s, %s]", + input_topic_names_[i].c_str(), input_topic_types_[i].c_str()); + } + } + + return true; +} + +} // namespace cs4home_core diff --git a/cs4home_core/src/cs4home_core/CognitiveModule.cpp b/cs4home_core/src/cs4home_core/CognitiveModule.cpp index 2c5bc01..475e71b 100644 --- a/cs4home_core/src/cs4home_core/CognitiveModule.cpp +++ b/cs4home_core/src/cs4home_core/CognitiveModule.cpp @@ -14,19 +14,15 @@ #include "cs4home_core/CognitiveModule.hpp" -namespace cs4home_core -{ - +namespace cs4home_core { /** * @brief Constructs a CognitiveModule and declares parameters. * @param options Node options to initialize the CognitiveModule instance. */ -CognitiveModule::CognitiveModule( - const std::string & name, - const rclcpp::NodeOptions & options) -: LifecycleNode(name, options) -{ +CognitiveModule::CognitiveModule(const std::string &name, + const rclcpp::NodeOptions &options) + : CascadeLifecycleNode(name, options) { declare_parameter("core", core_name_); declare_parameter("afferent", afferent_name_); declare_parameter("efferent", efferent_name_); @@ -34,74 +30,89 @@ CognitiveModule::CognitiveModule( declare_parameter("coupling", coupling_name_); } -using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +using CallbackReturnT = + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; /** * @brief Configures the CognitiveModule by loading and setting up components. * @param state Current lifecycle state. - * @return CallbackReturnT::SUCCESS if configuration is successful, FAILURE otherwise. + * @return CallbackReturnT::SUCCESS if configuration is successful, FAILURE + * otherwise. */ -CallbackReturnT CognitiveModule::on_configure(const rclcpp_lifecycle::State & state) -{ +CallbackReturnT +CognitiveModule::on_configure(const rclcpp_lifecycle::State &state) { (void)state; get_parameter("core", core_name_); std::string error_core; - std::tie(core_, error_core) = load_component(core_name_, shared_from_this()); + std::tie(core_, error_core) = + load_component(core_name_, shared_from_this()); if (core_ == nullptr || !core_->configure()) { - RCLCPP_ERROR( - get_logger(), "Error configuring core at %s with name %s: %s", - get_name(), core_name_.c_str(), error_core.c_str()); + RCLCPP_ERROR(get_logger(), "Error configuring core at %s with name %s: %s", + get_name(), core_name_.c_str(), error_core.c_str()); return CallbackReturnT::FAILURE; } - get_parameter("efferent", efferent_name_); - std::string error_efferent; - std::tie(efferent_, error_efferent) = load_component( - efferent_name_, shared_from_this()); - if (efferent_ == nullptr || !efferent_->configure()) { - RCLCPP_ERROR( - get_logger(), "Error configuring efferent at %s with name %s: %s", - get_name(), efferent_name_.c_str(), error_efferent.c_str()); - return CallbackReturnT::FAILURE; + if (efferent_name_.empty()) { + RCLCPP_INFO(get_logger(), "No efferent component specified."); + } else { + std::string error_efferent; + std::tie(efferent_, error_efferent) = + load_component(efferent_name_, shared_from_this()); + if (efferent_ == nullptr || !efferent_->configure()) { + RCLCPP_ERROR(get_logger(), + "Error configuring efferent at %s with name %s: %s", + get_name(), efferent_name_.c_str(), error_efferent.c_str()); + return CallbackReturnT::FAILURE; + } + core_->set_efferent(efferent_); } get_parameter("afferent", afferent_name_); - std::string error_afferent; - std::tie(afferent_, error_afferent) = load_component( - afferent_name_, shared_from_this()); - if (afferent_ == nullptr || !afferent_->configure()) { - RCLCPP_ERROR( - get_logger(), "Error configuring afferent at %s with name %s: %s", - get_name(), afferent_name_.c_str(), error_afferent.c_str()); - return CallbackReturnT::FAILURE; + if (afferent_name_.empty()) { + RCLCPP_INFO(get_logger(), "No afferent component specified."); + } else { + std::string error_afferent; + std::tie(afferent_, error_afferent) = + load_component(afferent_name_, shared_from_this()); + if (afferent_ == nullptr || !afferent_->configure()) { + RCLCPP_ERROR(get_logger(), + "Error configuring afferent at %s with name %s: %s", + get_name(), afferent_name_.c_str(), error_afferent.c_str()); + return CallbackReturnT::FAILURE; + } + core_->set_afferent(afferent_); } - - core_->set_afferent(afferent_); - core_->set_efferent(efferent_); - - get_parameter("meta", meta_name_); - std::string error_meta; - std::tie(meta_, error_meta) = load_component(meta_name_, shared_from_this()); - if (meta_ == nullptr || !meta_->configure()) { - RCLCPP_ERROR( - get_logger(), "Error configuring efferent at %s with name %s: %s", - get_name(), meta_name_.c_str(), error_meta.c_str()); - return CallbackReturnT::FAILURE; + if (meta_name_.empty()) { + RCLCPP_INFO(get_logger(), "No meta component specified."); + } else { + std::string error_meta; + std::tie(meta_, error_meta) = + load_component(meta_name_, shared_from_this()); + if (meta_ == nullptr || !meta_->configure()) { + RCLCPP_ERROR(get_logger(), + "Error configuring efferent at %s with name %s: %s", + get_name(), meta_name_.c_str(), error_meta.c_str()); + return CallbackReturnT::FAILURE; + } } get_parameter("coupling", coupling_name_); - std::string error_coupling; - std::tie(coupling_, error_coupling) = load_component( - coupling_name_, shared_from_this()); - if (coupling_ == nullptr || !coupling_->configure()) { - RCLCPP_ERROR( - get_logger(), "Error configuring efferent at %s with name %s: %s", - get_name(), coupling_name_.c_str(), error_coupling.c_str()); - return CallbackReturnT::FAILURE; + if (coupling_name_.empty()) { + RCLCPP_INFO(get_logger(), "No coupling component specified."); + } else { + std::string error_coupling; + std::tie(coupling_, error_coupling) = + load_component(coupling_name_, shared_from_this()); + if (coupling_ == nullptr || !coupling_->configure()) { + RCLCPP_ERROR(get_logger(), + "Error configuring efferent at %s with name %s: %s", + get_name(), coupling_name_.c_str(), error_coupling.c_str()); + return CallbackReturnT::FAILURE; + } } return CallbackReturnT::SUCCESS; @@ -110,10 +121,11 @@ CallbackReturnT CognitiveModule::on_configure(const rclcpp_lifecycle::State & st /** * @brief Activates the core component. * @param state Current lifecycle state. - * @return CallbackReturnT::SUCCESS if activation is successful, FAILURE otherwise. + * @return CallbackReturnT::SUCCESS if activation is successful, FAILURE + * otherwise. */ -CallbackReturnT CognitiveModule::on_activate(const rclcpp_lifecycle::State & state) -{ +CallbackReturnT +CognitiveModule::on_activate(const rclcpp_lifecycle::State &state) { (void)state; if (!core_->activate()) { @@ -127,10 +139,11 @@ CallbackReturnT CognitiveModule::on_activate(const rclcpp_lifecycle::State & sta /** * @brief Deactivates the core component. * @param state Current lifecycle state. - * @return CallbackReturnT::SUCCESS if deactivation is successful, FAILURE otherwise. + * @return CallbackReturnT::SUCCESS if deactivation is successful, FAILURE + * otherwise. */ -CallbackReturnT CognitiveModule::on_deactivate(const rclcpp_lifecycle::State & state) -{ +CallbackReturnT +CognitiveModule::on_deactivate(const rclcpp_lifecycle::State &state) { (void)state; if (!core_->deactivate()) { @@ -146,8 +159,8 @@ CallbackReturnT CognitiveModule::on_deactivate(const rclcpp_lifecycle::State & s * @param state Current lifecycle state. * @return CallbackReturnT::SUCCESS indicating cleanup is complete. */ -CallbackReturnT CognitiveModule::on_cleanup(const rclcpp_lifecycle::State & state) -{ +CallbackReturnT +CognitiveModule::on_cleanup(const rclcpp_lifecycle::State &state) { (void)state; return CallbackReturnT::SUCCESS; @@ -158,8 +171,8 @@ CallbackReturnT CognitiveModule::on_cleanup(const rclcpp_lifecycle::State & stat * @param state Current lifecycle state. * @return CallbackReturnT::SUCCESS indicating shutdown is complete. */ -CallbackReturnT CognitiveModule::on_shutdown(const rclcpp_lifecycle::State & state) -{ +CallbackReturnT +CognitiveModule::on_shutdown(const rclcpp_lifecycle::State &state) { (void)state; return CallbackReturnT::SUCCESS; @@ -170,8 +183,8 @@ CallbackReturnT CognitiveModule::on_shutdown(const rclcpp_lifecycle::State & sta * @param state Current lifecycle state. * @return CallbackReturnT::SUCCESS indicating error handling is complete. */ -CallbackReturnT CognitiveModule::on_error(const rclcpp_lifecycle::State & state) -{ +CallbackReturnT +CognitiveModule::on_error(const rclcpp_lifecycle::State &state) { (void)state; return CallbackReturnT::SUCCESS; @@ -185,25 +198,29 @@ CallbackReturnT CognitiveModule::on_error(const rclcpp_lifecycle::State & state) * @tparam T Type of the component to load. * @param name Name of the component. * @param parent Shared pointer to the parent lifecycle node. - * @return A tuple containing the shared pointer to the component and an error string (if any). + * @return A tuple containing the shared pointer to the component and an error + * string (if any). */ -template std::tuple -CognitiveModule::load_component( - const std::string & name, rclcpp_lifecycle::LifecycleNode::SharedPtr parent) -{ +template +std::tuple CognitiveModule::load_component( + const std::string &name, + rclcpp_lifecycle::LifecycleNode::SharedPtr parent) { std::string lib_name = "lib" + name + ".so"; - void * handle = dlopen(lib_name.c_str(), RTLD_LAZY); + void *handle = dlopen(lib_name.c_str(), RTLD_LAZY); if (!handle) { return {nullptr, "Cannot open library:" + lib_name}; } - using FactoryFunction = typename T::SharedPtr (*)(rclcpp_lifecycle::LifecycleNode::SharedPtr); - FactoryFunction create_instance = (FactoryFunction)dlsym(handle, "create_instance"); - const char * dlsym_error = dlerror(); + using FactoryFunction = + typename T::SharedPtr (*)(rclcpp_lifecycle::LifecycleNode::SharedPtr); + FactoryFunction create_instance = + (FactoryFunction)dlsym(handle, "create_instance"); + const char *dlsym_error = dlerror(); if (dlsym_error) { dlclose(handle); - return {nullptr, std::string("Cannot load symbol 'create_instance': ") + dlsym_error}; + return {nullptr, std::string("Cannot load symbol 'create_instance': ") + + dlsym_error}; } return {create_instance(parent), ""}; } -} // namespace cs4home_core +} // namespace cs4home_core diff --git a/cs4home_core/src/cs4home_core/Core.cpp b/cs4home_core/src/cs4home_core/Core.cpp index 4d4f591..2db2e75 100644 --- a/cs4home_core/src/cs4home_core/Core.cpp +++ b/cs4home_core/src/cs4home_core/Core.cpp @@ -21,8 +21,10 @@ namespace cs4home_core * @brief Constructs a Core object associated with a given lifecycle node. * @param parent Shared pointer to the lifecycle node managing this Core instance. */ -Core::Core(rclcpp_lifecycle::LifecycleNode::SharedPtr parent) -: parent_(parent) +Core::Core( + const std::string & name, rclcpp_lifecycle::LifecycleNode::SharedPtr parent) +: parent_(parent), + name_(name) { } diff --git a/cs4home_core/src/cs4home_core/Coupling.cpp b/cs4home_core/src/cs4home_core/Coupling.cpp index 7efe8ab..2019370 100644 --- a/cs4home_core/src/cs4home_core/Coupling.cpp +++ b/cs4home_core/src/cs4home_core/Coupling.cpp @@ -21,8 +21,9 @@ namespace cs4home_core * @brief Constructs a Coupling object and assigns the parent lifecycle node. * @param parent Shared pointer to the lifecycle node managing this Coupling instance. */ -Coupling::Coupling(rclcpp_lifecycle::LifecycleNode::SharedPtr parent) -: parent_(parent) +Coupling::Coupling(const std::string & name, rclcpp_lifecycle::LifecycleNode::SharedPtr parent) +: parent_(parent), + name_(name) { } diff --git a/cs4home_core/src/cs4home_core/Efferent.cpp b/cs4home_core/src/cs4home_core/Efferent.cpp index feebdaa..79ccaee 100644 --- a/cs4home_core/src/cs4home_core/Efferent.cpp +++ b/cs4home_core/src/cs4home_core/Efferent.cpp @@ -14,16 +14,47 @@ #include "cs4home_core/Efferent.hpp" -namespace cs4home_core -{ +namespace cs4home_core { /** * @brief Constructs an Efferent object and assigns the parent lifecycle node. - * @param parent Shared pointer to the lifecycle node managing this Efferent instance. + * @param parent Shared pointer to the lifecycle node managing this Efferent + * instance. */ -Efferent::Efferent(rclcpp_lifecycle::LifecycleNode::SharedPtr parent) -: parent_(parent) -{ +Efferent::Efferent(const std::string &name, + rclcpp_lifecycle::LifecycleNode::SharedPtr parent) + : parent_(parent), name_(name) { + // Declares the parameter for output topics. + parent_->declare_parameter(name_ + ".topics", output_topic_names_); + parent_->declare_parameter(name_ + ".types", output_topic_names_); +} + +/** + * @brief Configures the Efferent by creating publishers for each specified + * topic. + * + * This method retrieves the topic names from the parameter server and attempts + * to create publishers for each topic to send messages. + * + * @return True if all publishers are created successfully. + */ +bool Efferent::configure() { + parent_->get_parameter(name_ + ".topics", output_topic_names_); + parent_->get_parameter(name_ + ".types", output_topic_types_); + + for (size_t i = 0; i < output_topic_names_.size(); i++) { + if (create_publisher(output_topic_names_[i], output_topic_types_[i])) { + RCLCPP_INFO( + parent_->get_logger(), "[Efferent] created publisher to [%s, %s]", + output_topic_names_[i].c_str(), output_topic_types_[i].c_str()); + } else { + RCLCPP_WARN(parent_->get_logger(), + "[Efferent] Couldn't create publisher to [%s, %s]", + output_topic_names_[i].c_str(), + output_topic_types_[i].c_str()); + } + } + return true; } /** @@ -36,15 +67,14 @@ Efferent::Efferent(rclcpp_lifecycle::LifecycleNode::SharedPtr parent) * @param type The type of messages to publish on the topic. * @return True if the publisher was created successfully. */ -bool -Efferent::create_publisher(const std::string & topic, const std::string & type) -{ +bool Efferent::create_publisher(const std::string &topic, + const std::string &type) { auto pub = rclcpp::create_generic_publisher( - parent_->get_node_topics_interface(), topic, type, 100); + parent_->get_node_topics_interface(), topic, type, 100); pubs_.push_back(pub); return true; } -} // namespace cs4home_core +} // namespace cs4home_core diff --git a/cs4home_core/src/cs4home_core/Flow.cpp b/cs4home_core/src/cs4home_core/Flow.cpp index 817317c..29311e1 100644 --- a/cs4home_core/src/cs4home_core/Flow.cpp +++ b/cs4home_core/src/cs4home_core/Flow.cpp @@ -12,41 +12,61 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include -#include #include "cs4home_core/Flow.hpp" -#include "rclcpp/rclcpp.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/rclcpp.hpp" -namespace cs4home_core -{ +namespace cs4home_core { /** * @brief Constructs a Flow object with a list of nodes. * @param nodes A vector of node names to initialize the flow sequence. */ -Flow::Flow(const std::vector & nodes) -: nodes_(nodes) -{ +Flow::Flow(const std::string &name) : CascadeLifecycleNode(name) { + trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); +} + +Flow::Flow(const std::string &name, const std::vector &nodes) + : Flow(name) { + set_flow(nodes); +} + +void Flow::set_flow(const std::vector &nodes) { + clear_activation(); + + nodes_ = nodes; + + for (const auto &node : nodes) { + RCLCPP_INFO(this->get_logger(), "flow: %s", node.c_str()); + add_activation(node); + } +} + +void Flow::activate() { + trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); +} + +void Flow::deactivate() { + trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE); } /** * @brief Prints the sequence of nodes in the flow to the standard output. * - * This function outputs each node name in the flow, prefixed by an arrow (->) to - * represent the sequence visually. + * This function outputs each node name in the flow, prefixed by an arrow (->) + * to represent the sequence visually. */ -void -Flow::print() const -{ - for (const auto & node : nodes_) { +void Flow::print() const { + for (const auto &node : nodes_) { std::cout << " -> " << node; } std::cout << std::endl; } -} // namespace cs4home_core +} // namespace cs4home_core diff --git a/cs4home_core/src/cs4home_core/Master.cpp b/cs4home_core/src/cs4home_core/Master.cpp index 054f94c..44606ee 100644 --- a/cs4home_core/src/cs4home_core/Master.cpp +++ b/cs4home_core/src/cs4home_core/Master.cpp @@ -21,9 +21,11 @@ namespace cs4home_core * @brief Constructs a Master lifecycle node with the specified options. * @param options Node options to configure the Master node. */ -Master::Master(const rclcpp::NodeOptions & options) -: LifecycleNode("master", options) +Master::Master(const std::string & name, const rclcpp::NodeOptions & options) +: LifecycleNode(name, options) { + std::vector flows; + declare_parameter("flows", std::vector{}); } using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; @@ -38,6 +40,17 @@ Master::on_configure(const rclcpp_lifecycle::State & state) { (void)state; + std::vector flows; + get_parameter("flows", flows); + + for (const auto & flow : flows) { + std::vector flow_cms; + declare_parameter(flow, flow_cms); + get_parameter(flow, flow_cms); + + flows_[flow] = Flow::make_shared(flow, flow_cms); + } + return CallbackReturnT::SUCCESS; } diff --git a/cs4home_core/src/cs4home_core/Meta.cpp b/cs4home_core/src/cs4home_core/Meta.cpp index cb98f4e..f685264 100644 --- a/cs4home_core/src/cs4home_core/Meta.cpp +++ b/cs4home_core/src/cs4home_core/Meta.cpp @@ -21,8 +21,9 @@ namespace cs4home_core * @brief Constructs a Meta object and assigns the parent lifecycle node. * @param parent Shared pointer to the lifecycle node managing this Meta instance. */ -Meta::Meta(rclcpp_lifecycle::LifecycleNode::SharedPtr parent) -: parent_(parent) +Meta::Meta(const std::string & name, rclcpp_lifecycle::LifecycleNode::SharedPtr parent) +: parent_(parent), + name_(name) { } diff --git a/cs4home_core/test/DefaultCoupling.cpp b/cs4home_core/test/DefaultCoupling.cpp index c2be7f4..a99f716 100644 --- a/cs4home_core/test/DefaultCoupling.cpp +++ b/cs4home_core/test/DefaultCoupling.cpp @@ -35,7 +35,7 @@ class DefaultCoupling : public cs4home_core::Coupling * @param parent Shared pointer to the lifecycle node managing this DefaultCoupling instance. */ explicit DefaultCoupling(rclcpp_lifecycle::LifecycleNode::SharedPtr parent) - : Coupling(parent) + : Coupling("default_coupling", parent) { RCLCPP_DEBUG(parent_->get_logger(), "Coupling created: [DefaultCoupling]"); } diff --git a/cs4home_core/test/DefaultMeta.cpp b/cs4home_core/test/DefaultMeta.cpp index ea67afc..8c7b77a 100644 --- a/cs4home_core/test/DefaultMeta.cpp +++ b/cs4home_core/test/DefaultMeta.cpp @@ -35,7 +35,7 @@ class DefaultMeta : public cs4home_core::Meta * @param parent Shared pointer to the lifecycle node managing this DefaultMeta instance. */ explicit DefaultMeta(rclcpp_lifecycle::LifecycleNode::SharedPtr parent) - : Meta(parent) + : Meta("default_meta", parent) { RCLCPP_DEBUG(parent_->get_logger(), "Meta created: [DefaultMeta]"); } diff --git a/cs4home_core/test/ImageFilter.cpp b/cs4home_core/test/ImageFilter.cpp index a1c4e2c..cd6eb54 100644 --- a/cs4home_core/test/ImageFilter.cpp +++ b/cs4home_core/test/ImageFilter.cpp @@ -18,59 +18,59 @@ #include "sensor_msgs/msg/image.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "rclcpp/macros.hpp" using std::placeholders::_1; using namespace std::chrono_literals; /** * @class ImageFilter - * @brief Core component that filters incoming image messages by modifying their headers and - * republishing them on a timer. + * @brief Core component that filters incoming image messages by modifying their + * headers and republishing them on a timer. */ -class ImageFilter : public cs4home_core::Core -{ +class ImageFilter : public cs4home_core::Core { public: RCLCPP_SMART_PTR_DEFINITIONS(ImageFilter) - /** - * @brief Constructs an ImageFilter object and initializes the parent lifecycle node. - * @param parent Shared pointer to the lifecycle node managing this ImageFilter instance. + * @brief Constructs an ImageFilter object and initializes the parent + * lifecycle node. + * @param parent Shared pointer to the lifecycle node managing this + * ImageFilter instance. */ explicit ImageFilter(rclcpp_lifecycle::LifecycleNode::SharedPtr parent) - : Core(parent) - { + : Core("image_filter", parent) { RCLCPP_DEBUG(parent_->get_logger(), "Core created: [ImageFilter]"); } /** * @brief Processes an incoming image message by modifying its header. * - * The `frame_id` in the image header is converted to an integer, doubled, and set as the new - * `frame_id`. The modified message is then sent to the efferent component. + * The `frame_id` in the image header is converted to an integer, doubled, and + * set as the new `frame_id`. The modified message is then sent to the + * efferent component. * - * @param msg Unique pointer to the incoming image message of type `sensor_msgs::msg::Image`. + * @param msg shared pointer to the incoming image message of type + * `sensor_msgs::msg::Image`. */ - void process_in_image(sensor_msgs::msg::Image::UniquePtr msg) - { + void process_in_image(std::shared_ptr msg) { int counter = std::atoi(msg->header.frame_id.c_str()); counter = counter * 2; msg->header.frame_id = std::to_string(counter); - efferent_->publish(std::move(msg)); + efferent_->publish(0, msg); } /** - * @brief Timer callback function that retrieves an image message and processes it. + * @brief Timer callback function that retrieves an image message and + * processes it. * - * This function is called periodically and attempts to retrieve an image message from the - * afferent component. If a message is received, it is passed to `process_in_image`. + * This function is called periodically and attempts to retrieve an image + * message from the afferent component. If a message is received, it is passed + * to `process_in_image`. */ - void timer_callback() - { - auto msg = afferent_->get_msg(); + void timer_callback() { + auto msg = afferent_->get_msg(0); if (msg != nullptr) { process_in_image(std::move(msg)); } @@ -80,8 +80,7 @@ class ImageFilter : public cs4home_core::Core * @brief Configures the ImageFilter component. * @return True if configuration is successful. */ - bool configure() override - { + bool configure() override { RCLCPP_DEBUG(parent_->get_logger(), "Core configured"); return true; } @@ -93,10 +92,9 @@ class ImageFilter : public cs4home_core::Core * * @return True if activation is successful. */ - bool activate() override - { + bool activate() override { timer_ = parent_->create_wall_timer( - 50ms, std::bind(&ImageFilter::timer_callback, this)); + 50ms, std::bind(&ImageFilter::timer_callback, this)); return true; } @@ -107,14 +105,14 @@ class ImageFilter : public cs4home_core::Core * * @return True if deactivation is successful. */ - bool deactivate() override - { + bool deactivate() override { timer_ = nullptr; return true; } private: - rclcpp::TimerBase::SharedPtr timer_; /**< Timer for periodic execution of `timer_callback`. */ + rclcpp::TimerBase::SharedPtr + timer_; /**< Timer for periodic execution of `timer_callback`. */ }; /// Registers the ImageFilter component with the ROS 2 class loader diff --git a/cs4home_core/test/ImageFilterCB.cpp b/cs4home_core/test/ImageFilterCB.cpp index 07655af..4c99c23 100644 --- a/cs4home_core/test/ImageFilterCB.cpp +++ b/cs4home_core/test/ImageFilterCB.cpp @@ -17,66 +17,81 @@ #include "sensor_msgs/msg/image.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" using std::placeholders::_1; +using std::placeholders::_2; using namespace std::chrono_literals; /** * @class ImageFilterCB - * @brief Core component that filters incoming image messages by modifying their headers and - * republishing them. Uses ROS 2 lifecycle and callback functions for message processing. + * @brief Core component that filters incoming image messages by modifying their + * headers and republishing them. Uses ROS 2 lifecycle and callback functions + * for message processing. */ -class ImageFilterCB : public cs4home_core::Core -{ +class ImageFilterCB : public cs4home_core::Core { public: RCLCPP_SMART_PTR_DEFINITIONS(ImageFilterCB) /** - * @brief Constructs an ImageFilterCB object and initializes the parent lifecycle node. - * @param parent Shared pointer to the lifecycle node managing this ImageFilterCB instance. + * @brief Constructs an ImageFilterCB object and initializes the parent + * lifecycle node. + * @param parent Shared pointer to the lifecycle node managing this + * ImageFilterCB instance. */ explicit ImageFilterCB(rclcpp_lifecycle::LifecycleNode::SharedPtr parent) - : Core(parent) - { + : Core("image_filter_cb", parent) { RCLCPP_DEBUG(parent_->get_logger(), "Core created: [ImageFilterCB]"); } /** - * @brief Processes incoming serialized image messages, applies a simple transformation, and - * republishes them. + * @brief Processes incoming serialized image messages, applies a simple + * transformation, and republishes them. * - * The `frame_id` in the image header is converted to an integer, doubled, and set as the new - * `frame_id`. The modified message is then sent to the efferent component. + * The `frame_id` in the image header is converted to an integer, doubled, and + * set as the new `frame_id`. The modified message is then sent to the + * efferent component. * - * @param msg Unique pointer to the serialized incoming image message. + * @param msg shared pointer to the serialized incoming image message. */ - void process_in_image(std::unique_ptr msg) - { - auto image_msg = afferent_->get_msg(std::move(msg)); + void process_in_image(std::shared_ptr msg) { + auto image_msg = + afferent_->get_msg(std::move(msg)); int counter = std::atoi(image_msg->header.frame_id.c_str()); counter = counter * 2; image_msg->header.frame_id = std::to_string(counter); - efferent_->publish(std::move(image_msg)); + efferent_->publish(0, image_msg); + } + + /** + * @brief Processes incoming serialized camera info image messages. Do nothing + * @param msg Unique pointer to the serialized incoming image message. + */ + void process_in_camerainfo(std::shared_ptr msg) { + auto camerainfo_msgs = + afferent_->get_msg(std::move(msg)); + // Nothing to do } /** * @brief Configures the ImageFilterCB component. * - * Sets the afferent component mode to `CALLBACK`, binding the `process_in_image` method to - * handle incoming messages. + * Sets the afferent component mode to `CALLBACK`, binding the + * `process_in_image` method to handle incoming messages. * * @return True if configuration is successful. */ - bool configure() override - { + bool configure() override { RCLCPP_DEBUG(parent_->get_logger(), "Core configured"); + afferent_->set_mode(0, cs4home_core::Afferent::CALLBACK, + std::bind(&ImageFilterCB::process_in_image, this, _1)); afferent_->set_mode( - cs4home_core::Afferent::CALLBACK, std::bind(&ImageFilterCB::process_in_image, this, _1)); + 1, cs4home_core::Afferent::CALLBACK, + std::bind(&ImageFilterCB::process_in_camerainfo, this, _1)); return true; } @@ -85,10 +100,7 @@ class ImageFilterCB : public cs4home_core::Core * @brief Activates the ImageFilterCB component. * @return True if activation is successful. */ - bool activate() override - { - return true; - } + bool activate() override { return true; } /** * @brief Deactivates the ImageFilterCB component. @@ -97,14 +109,14 @@ class ImageFilterCB : public cs4home_core::Core * * @return True if deactivation is successful. */ - bool deactivate() override - { + bool deactivate() override { timer_ = nullptr; return true; } private: - rclcpp::TimerBase::SharedPtr timer_; /**< Timer used for periodic operations if needed. */ + rclcpp::TimerBase::SharedPtr + timer_; /**< Timer used for periodic operations if needed. */ }; /// Registers the ImageFilterCB component with the ROS 2 class loader diff --git a/cs4home_core/test/SimpleImageInput.cpp b/cs4home_core/test/SimpleImageInput.cpp index 4b43f3d..7e2a41f 100644 --- a/cs4home_core/test/SimpleImageInput.cpp +++ b/cs4home_core/test/SimpleImageInput.cpp @@ -35,51 +35,19 @@ class SimpleImageInput : public cs4home_core::Afferent * @param parent Shared pointer to the lifecycle node managing this SimpleImageInput instance. */ explicit SimpleImageInput(rclcpp_lifecycle::LifecycleNode::SharedPtr parent) - : Afferent(parent) + : Afferent("simple_image_input", parent) { RCLCPP_DEBUG(parent_->get_logger(), "Efferent created: [SimpleImageInput]"); - - // Declares the parameter for input topics. - parent_->declare_parameter("simple_image_input.topics", input_topic_names_); } /** * @brief Configures the SimpleImageInput by creating subscribers for each specified topic. - * - * This method retrieves the topic names from the parameter server and attempts to create - * a subscription for each topic to receive `sensor_msgs::msg::Image` messages. - * * @return True if all subscriptions are created successfully. */ bool configure() override { - std::string param_name = "simple_image_input.topics"; - parent_->get_parameter(param_name, input_topic_names_); - - RCLCPP_DEBUG( - parent_->get_logger(), "[SimpleImageInput] Configuring %zu inputs [%s]", - input_topic_names_.size(), param_name.c_str()); - - for (size_t i = 0; i < input_topic_names_.size(); i++) { - if (create_subscriber(input_topic_names_[i], "sensor_msgs/msg/Image")) { - RCLCPP_DEBUG( - parent_->get_logger(), - "[SimpleImageInput] created subscription to [%s, sensor_msgs/msg/Image]", - input_topic_names_[i].c_str()); - } else { - RCLCPP_WARN( - parent_->get_logger(), - "[SimpleImageInput] Couldn't create subscription to [%s, sensor_msgs/msg/Image]", - input_topic_names_[i].c_str()); - } - } - - return true; + return Afferent::configure(); } - -private: - /**< List of input topics to subscribe to for images. */ - std::vector input_topic_names_; }; /// Registers the SimpleImageInput component with the ROS 2 class loader diff --git a/cs4home_core/test/SimpleImageOutput.cpp b/cs4home_core/test/SimpleImageOutput.cpp index 28e3ec0..a5b9c97 100644 --- a/cs4home_core/test/SimpleImageOutput.cpp +++ b/cs4home_core/test/SimpleImageOutput.cpp @@ -17,71 +17,50 @@ #include "sensor_msgs/msg/image.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" /** * @class SimpleImageOutput * @brief Manages image output by creating publishers for specified topics and * providing a method to publish image messages. */ -class SimpleImageOutput : public cs4home_core::Efferent -{ +class SimpleImageOutput : public cs4home_core::Efferent { public: RCLCPP_SMART_PTR_DEFINITIONS(SimpleImageOutput) /** - * @brief Constructs a SimpleImageOutput object and declares necessary parameters. - * @param parent Shared pointer to the lifecycle node managing this SimpleImageOutput instance. + * @brief Constructs a SimpleImageOutput object and declares necessary + * parameters. + * @param parent Shared pointer to the lifecycle node managing this + * SimpleImageOutput instance. */ explicit SimpleImageOutput(rclcpp_lifecycle::LifecycleNode::SharedPtr parent) - : Efferent(parent) - { - RCLCPP_DEBUG(parent_->get_logger(), "Afferent created: [SimpleImageOutput]"); - - // Declares the parameter for output topics. - parent_->declare_parameter("simple_image_output.topics", output_topic_names_); + : Efferent("simple_image_output", parent) { + RCLCPP_DEBUG(parent_->get_logger(), + "Afferent created: [SimpleImageOutput]"); } /** - * @brief Configures the SimpleImageOutput by creating publishers for each specified topic. + * @brief Configures the SimpleImageOutput by creating publishers for each + * specified topic. * - * This method retrieves the topic names from the parameter server and attempts to create - * a publisher for each topic to publish `sensor_msgs::msg::Image` messages. + * This method retrieves the topic names from the parameter server and + * attempts to create a publisher for each topic to publish + * `sensor_msgs::msg::Image` messages. * * @return True if all publishers are created successfully. */ - bool configure() - { - parent_->get_parameter("simple_image_output.topics", output_topic_names_); - - for (size_t i = 0; i < output_topic_names_.size(); i++) { - if (create_publisher(output_topic_names_[i], "sensor_msgs/msg/Image")) { - RCLCPP_DEBUG( - parent_->get_logger(), - "[SimpleImageOutput] created publisher to [%s, sensor_msgs/msg/Image]", - output_topic_names_[i].c_str()); - } else { - RCLCPP_WARN( - parent_->get_logger(), - "[SimpleImageOutput] Couldn't create publisher to [%s, sensor_msgs/msg/Image]", - output_topic_names_[i].c_str()); - } - } - return true; - } + bool configure() { return Efferent::configure(); } /** * @brief Publishes an image message to all configured topics. - * @param msg Unique pointer to an image message of type `sensor_msgs::msg::Image`. + * @param msg shared pointer to an image message of type + * `sensor_msgs::msg::Image`. */ - void publish_image(sensor_msgs::msg::Image::UniquePtr msg) - { - publish(std::move(msg)); + void publish_image(sensor_msgs::msg::Image::SharedPtr msg) { + publish(0, msg); } - -private: - std::vector output_topic_names_; /**< List of output topics to publish images. */ }; /// Registers the SimpleImageOutput component with the ROS 2 class loader diff --git a/cs4home_core/test/cognitive_module_test.cpp b/cs4home_core/test/cognitive_module_test.cpp index 58a86c1..d013cfd 100644 --- a/cs4home_core/test/cognitive_module_test.cpp +++ b/cs4home_core/test/cognitive_module_test.cpp @@ -14,9 +14,9 @@ #include +#include "lifecycle_msgs/msg/state.hpp" #include "sensor_msgs/msg/image.hpp" #include "vision_msgs/msg/detection2_d_array.hpp" -#include "lifecycle_msgs/msg/state.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" @@ -31,21 +31,24 @@ * @param parent Lifecycle node parent for the component. * @return Tuple with the loaded component shared pointer and error message. */ -template std::tuple -load_component( - const std::string & name, rclcpp_lifecycle::LifecycleNode::SharedPtr parent) -{ +template +std::tuple +load_component(const std::string &name, + rclcpp_lifecycle::LifecycleNode::SharedPtr parent) { std::string lib_name = "lib" + name + ".so"; - void * handle = dlopen(lib_name.c_str(), RTLD_LAZY); + void *handle = dlopen(lib_name.c_str(), RTLD_LAZY); if (!handle) { return {nullptr, "Cannot open library:" + lib_name}; } - using FactoryFunction = typename T::SharedPtr (*)(rclcpp_lifecycle::LifecycleNode::SharedPtr); - FactoryFunction create_instance = (FactoryFunction)dlsym(handle, "create_instance"); - const char * dlsym_error = dlerror(); + using FactoryFunction = + typename T::SharedPtr (*)(rclcpp_lifecycle::LifecycleNode::SharedPtr); + FactoryFunction create_instance = + (FactoryFunction)dlsym(handle, "create_instance"); + const char *dlsym_error = dlerror(); if (dlsym_error) { dlclose(handle); - return {nullptr, std::string("Cannot load symbol 'create_instance': ") + dlsym_error}; + return {nullptr, std::string("Cannot load symbol 'create_instance': ") + + dlsym_error}; } return {create_instance(parent), ""}; } @@ -53,16 +56,17 @@ load_component( using namespace std::chrono_literals; /** - * @test Verifies the functionality of the afferent component in "on demand" mode. + * @test Verifies the functionality of the afferent component in "on demand" + * mode. * - * This test sets up an afferent component to operate in "on demand" mode, where it - * retrieves messages from a queue rather than processing them immediately via a callback. - * Messages are published to a topic, and the afferent component is configured to pull - * messages from this queue as needed. The test ensures that the messages are correctly - * retrieved in order and that once the queue is empty, further retrievals return `nullptr`. + * This test sets up an afferent component to operate in "on demand" mode, where + * it retrieves messages from a queue rather than processing them immediately + * via a callback. Messages are published to a topic, and the afferent component + * is configured to pull messages from this queue as needed. The test ensures + * that the messages are correctly retrieved in order and that once the queue is + * empty, further retrievals return `nullptr`. */ -TEST(cognitive_module_test, afferent_on_demand) -{ +TEST(cognitive_module_test, afferent_on_demand) { // Create main nodes for the test auto node = rclcpp_lifecycle::LifecycleNode::make_shared("test_lc_node"); auto pub_node = rclcpp::Node::make_shared("pub_node"); @@ -74,15 +78,17 @@ TEST(cognitive_module_test, afferent_on_demand) exe.add_node(pub_node); // Configure topics for afferent component - std::vector topics {"/image"}; + std::vector topics{"/image"}; + std::vector types{"sensor_msgs/msg/Image"}; // Load afferent component and verify successful loading - auto [afferent, error_afferent] = load_component( - "simple_image_input", node); + auto [afferent, error_afferent] = + load_component("simple_image_input", node); ASSERT_NE(afferent, nullptr); // Set the topics parameter and configure afferent component node->set_parameter(rclcpp::Parameter("simple_image_input.topics", topics)); + node->set_parameter(rclcpp::Parameter("simple_image_input.types", types)); ASSERT_TRUE(afferent->configure()); // Publish test messages to the afferent component's subscribed topic @@ -101,27 +107,27 @@ TEST(cognitive_module_test, afferent_on_demand) // Retrieve and verify messages from the afferent component's queue for (int i = 0; i < 10; i++) { - auto in_msg = afferent->get_msg(); + auto in_msg = afferent->get_msg("/image"); ASSERT_NE(in_msg, nullptr); ASSERT_EQ(i, std::atoi(in_msg->header.frame_id.c_str())); } - // Verify that further retrieval attempts return `nullptr` as the queue is now empty - auto in_msg = afferent->get_msg(); + // Verify that further retrieval attempts return `nullptr` as the queue is now + // empty + auto in_msg = afferent->get_msg("/image"); ASSERT_EQ(in_msg, nullptr); } - /** * @test Verifies the functionality of the afferent component in callback mode. * * This test sets up an afferent component to operate in callback mode, where it - * subscribes to a topic and processes incoming messages by storing them in a vector - * for verification. The component listens to `/image` messages and ensures that all - * received messages are processed and accessible through the callback mechanism. + * subscribes to a topic and processes incoming messages by storing them in a + * vector for verification. The component listens to `/image` messages and + * ensures that all received messages are processed and accessible through the + * callback mechanism. */ -TEST(cognitive_module_test, afferent_on_subscription) -{ +TEST(cognitive_module_test, afferent_on_subscription) { // Create main nodes for the test auto node = rclcpp_lifecycle::LifecycleNode::make_shared("test_lc_node"); auto pub_node = rclcpp::Node::make_shared("pub_node"); @@ -133,26 +139,27 @@ TEST(cognitive_module_test, afferent_on_subscription) exe.add_node(pub_node); // Setup topics and image storage - std::vector topics {"/image"}; - std::vector> images; + std::vector topics{"/image"}; + std::vector types{"sensor_msgs/msg/Image"}; + + std::vector> images; // Load afferent component and verify successful loading - auto [afferent, error_afferent] = load_component( - "simple_image_input", node); + auto [afferent, error_afferent] = + load_component("simple_image_input", node); ASSERT_NE(afferent, nullptr); // Set the topics parameter and configure afferent mode node->set_parameter(rclcpp::Parameter("simple_image_input.topics", topics)); - afferent->set_mode(cs4home_core::Afferent::CALLBACK); + node->set_parameter(rclcpp::Parameter("simple_image_input.types", types)); + afferent->set_mode("/image", cs4home_core::Afferent::CALLBACK); ASSERT_EQ(afferent->get_mode(), cs4home_core::Afferent::ONDEMAND); - afferent->set_mode( - cs4home_core::Afferent::CALLBACK, - [&images](std::unique_ptr msg) { - images.push_back(std::move(msg)); - } - ); + afferent->set_mode("/image", cs4home_core::Afferent::CALLBACK, + [&images](std::shared_ptr msg) { + images.push_back(std::move(msg)); + }); ASSERT_TRUE(afferent->configure()); // Publish test messages to the afferent component's subscribed topic @@ -176,23 +183,24 @@ TEST(cognitive_module_test, afferent_on_subscription) ASSERT_NE(in_msg, nullptr); rclcpp::Serialization serializer; - auto typed_msg = std::make_unique(); + auto typed_msg = std::make_shared(); serializer.deserialize_message(in_msg.get(), typed_msg.get()); ASSERT_EQ(i, std::atoi(typed_msg->header.frame_id.c_str())); } } /** - * @test Verifies the functionality of the efferent component in publishing messages. + * @test Verifies the functionality of the efferent component in publishing + * messages. * - * This test sets up an efferent component to publish messages on a specified topic. - * It publishes a series of messages with sequential `frame_id` values and verifies - * that these messages are received correctly by a subscriber on the same topic. - * The test ensures that the efferent component is able to correctly configure its - * publishers and transmit messages to external listeners. + * This test sets up an efferent component to publish messages on a specified + * topic. It publishes a series of messages with sequential `frame_id` values + * and verifies that these messages are received correctly by a subscriber on + * the same topic. The test ensures that the efferent component is able to + * correctly configure its publishers and transmit messages to external + * listeners. */ -TEST(cognitive_module_test, efferent) -{ +TEST(cognitive_module_test, efferent) { // Create main nodes for the test auto node = rclcpp_lifecycle::LifecycleNode::make_shared("test_lc_node"); auto sub_node = rclcpp::Node::make_shared("sub_node"); @@ -200,9 +208,8 @@ TEST(cognitive_module_test, efferent) // Storage for received messages std::vector images; auto sub = sub_node->create_subscription( - "/image", 100, [&images](sensor_msgs::msg::Image msg) { - images.push_back(msg); - }); + "/image", 100, + [&images](sensor_msgs::msg::Image msg) { images.push_back(msg); }); // Single-threaded executor to handle callbacks rclcpp::executors::SingleThreadedExecutor exe; @@ -210,22 +217,25 @@ TEST(cognitive_module_test, efferent) exe.add_node(sub_node); // Configure topics for efferent component - std::vector topics {"/image"}; + std::vector topics{"/image"}; + std::vector types{"sensor_msgs/msg/Image"}; // Load efferent component and verify successful loading - auto [efferent, error_efferent] = load_component( - "simple_image_output", node); + auto [efferent, error_efferent] = + load_component("simple_image_output", node); ASSERT_NE(efferent, nullptr); // Set the topics parameter and configure efferent component node->set_parameter(rclcpp::Parameter("simple_image_output.topics", topics)); + node->set_parameter(rclcpp::Parameter("simple_image_output.types", types)); + ASSERT_TRUE(efferent->configure()); // Publish test messages via the efferent component for (int i = 0; i < 10; i++) { - auto msg = std::make_unique(); + auto msg = std::make_shared(); msg->header.frame_id = std::to_string(i); - efferent->publish(std::move(msg)); + efferent->publish(0, msg); exe.spin_some(); } @@ -242,9 +252,9 @@ TEST(cognitive_module_test, efferent) } } - /** - * @test Verifies the core component's behavior when processing incoming messages. + * @test Verifies the core component's behavior when processing incoming + * messages. * * This test sets up an afferent component to receive messages, a core component * to process them by doubling the `frame_id` in each message header, and an @@ -252,20 +262,20 @@ TEST(cognitive_module_test, efferent) * series of messages, each with a sequential `frame_id`, and verifies that the * processed messages in the efferent component have doubled `frame_id` values. */ -TEST(cognitive_module_test, core) -{ +TEST(cognitive_module_test, core) { auto node = rclcpp_lifecycle::LifecycleNode::make_shared("test_lc_node"); auto pub_node = rclcpp::Node::make_shared("pub_node"); auto sub_node = rclcpp::Node::make_shared("sub_node"); - // Publisher on /in_image and subscription to processed messages on /out_image. - auto pub = pub_node->create_publisher("/in_image", 100); + // Publisher on /in_image and subscription to processed messages on + // /out_image. + auto pub = + pub_node->create_publisher("/in_image", 100); std::vector images; auto sub = sub_node->create_subscription( - "/out_image", 100, [&images](sensor_msgs::msg::Image msg) { - images.push_back(msg); - }); + "/out_image", 100, + [&images](sensor_msgs::msg::Image msg) { images.push_back(msg); }); rclcpp::executors::SingleThreadedExecutor exe; exe.add_node(node->get_node_base_interface()); @@ -273,23 +283,28 @@ TEST(cognitive_module_test, core) exe.add_node(sub_node); // Setup topics for afferent and efferent components - std::vector in_topics {"/in_image"}; - std::vector out_topics {"/out_image"}; + std::vector in_topics{"/in_image"}; + std::vector out_topics{"/out_image"}; + std::vector types{"sensor_msgs/msg/Image"}; // Load components - auto [afferent, error_afferent] = load_component( - "simple_image_input", node); + auto [afferent, error_afferent] = + load_component("simple_image_input", node); ASSERT_NE(afferent, nullptr); - auto [efferent, error_efferent] = load_component( - "simple_image_output", node); + auto [efferent, error_efferent] = + load_component("simple_image_output", node); ASSERT_NE(efferent, nullptr); - auto [core, error_core] = load_component( - "image_filter", node); + auto [core, error_core] = + load_component("image_filter", node); ASSERT_NE(core, nullptr); // Set parameters and configure components - node->set_parameter(rclcpp::Parameter("simple_image_input.topics", in_topics)); - node->set_parameter(rclcpp::Parameter("simple_image_output.topics", out_topics)); + node->set_parameter( + rclcpp::Parameter("simple_image_input.topics", in_topics)); + node->set_parameter(rclcpp::Parameter("simple_image_input.types", types)); + node->set_parameter( + rclcpp::Parameter("simple_image_output.topics", out_topics)); + node->set_parameter(rclcpp::Parameter("simple_image_output.types", types)); ASSERT_TRUE(afferent->configure()); ASSERT_TRUE(efferent->configure()); core->set_afferent(afferent); @@ -320,29 +335,30 @@ TEST(cognitive_module_test, core) } /** - * @test Verifies the core component with callback functionality (core_cb) processes incoming - * messages and republishes them with doubled `frame_id` values. + * @test Verifies the core component with callback functionality (core_cb) + * processes incoming messages and republishes them with doubled `frame_id` + * values. * - * This test sets up an afferent component in callback mode to receive messages, a core - * component with a callback (`core_cb`) to process the messages by doubling the `frame_id` - * in each message header, and an efferent component to republish the processed messages. - * The test publishes a sequence of messages and verifies that the processed messages have - * doubled `frame_id` values. + * This test sets up an afferent component in callback mode to receive messages, + * a core component with a callback (`core_cb`) to process the messages by + * doubling the `frame_id` in each message header, and an efferent component to + * republish the processed messages. The test publishes a sequence of messages + * and verifies that the processed messages have doubled `frame_id` values. */ -TEST(cognitive_module_test, core_cb) -{ +TEST(cognitive_module_test, core_cb) { auto node = rclcpp_lifecycle::LifecycleNode::make_shared("test_lc_node"); auto pub_node = rclcpp::Node::make_shared("pub_node"); auto sub_node = rclcpp::Node::make_shared("sub_node"); - // Publisher on /in_image and subscription to processed messages on /out_image. - auto pub = pub_node->create_publisher("/in_image", 100); + // Publisher on /in_image and subscription to processed messages on + // /out_image. + auto pub = + pub_node->create_publisher("/in_image", 100); std::vector images; auto sub = sub_node->create_subscription( - "/out_image", 100, [&images](sensor_msgs::msg::Image msg) { - images.push_back(msg); - }); + "/out_image", 100, + [&images](sensor_msgs::msg::Image msg) { images.push_back(msg); }); rclcpp::executors::SingleThreadedExecutor exe; exe.add_node(node->get_node_base_interface()); @@ -350,23 +366,28 @@ TEST(cognitive_module_test, core_cb) exe.add_node(sub_node); // Setup topics for afferent and efferent components - std::vector in_topics {"/in_image"}; - std::vector out_topics {"/out_image"}; + std::vector in_topics{"/in_image"}; + std::vector out_topics{"/out_image"}; + std::vector types{"sensor_msgs/msg/Image"}; // Load components - auto [afferent, error_afferent] = load_component( - "simple_image_input", node); + auto [afferent, error_afferent] = + load_component("simple_image_input", node); ASSERT_NE(afferent, nullptr); - auto [efferent, error_efferent] = load_component( - "simple_image_output", node); + auto [efferent, error_efferent] = + load_component("simple_image_output", node); ASSERT_NE(efferent, nullptr); - auto [core, error_core] = load_component( - "image_filter_cb", node); + auto [core, error_core] = + load_component("image_filter_cb", node); ASSERT_NE(core, nullptr); // Set parameters and configure components - node->set_parameter(rclcpp::Parameter("simple_image_input.topics", in_topics)); - node->set_parameter(rclcpp::Parameter("simple_image_output.topics", out_topics)); + node->set_parameter( + rclcpp::Parameter("simple_image_input.topics", in_topics)); + node->set_parameter(rclcpp::Parameter("simple_image_input.types", types)); + node->set_parameter( + rclcpp::Parameter("simple_image_output.topics", out_topics)); + node->set_parameter(rclcpp::Parameter("simple_image_output.types", types)); ASSERT_TRUE(afferent->configure()); ASSERT_TRUE(efferent->configure()); core->set_afferent(afferent); @@ -396,46 +417,46 @@ TEST(cognitive_module_test, core_cb) } } - /** - * @test Tests the initialization and basic functionality of the CognitiveModule using a configuration file. + * @test Tests the initialization and basic functionality of the CognitiveModule + * using a configuration file. * - * This test verifies that the CognitiveModule initializes correctly from a configuration file - * and transitions through the ROS 2 lifecycle states (configure, activate, deactivate). - * It sets up publishers and subscribers to check that messages are processed with expected modifications - * (doubling of `frame_id`) and are received correctly after processing. + * This test verifies that the CognitiveModule initializes correctly from a + * configuration file and transitions through the ROS 2 lifecycle states + * (configure, activate, deactivate). It sets up publishers and subscribers to + * check that messages are processed with expected modifications (doubling of + * `frame_id`) and are received correctly after processing. */ -TEST(cognitive_module_test, startup_simple) -{ +TEST(cognitive_module_test, startup_simple) { // Obtain the path to the configuration file - std::string pkgpath = ament_index_cpp::get_package_share_directory("cs4home_core"); + std::string pkgpath = + ament_index_cpp::get_package_share_directory("cs4home_core"); std::string config_file = pkgpath + "/config/startup_simple_1.yaml"; rclcpp::NodeOptions options; - options.arguments( - {"--ros-args", "--params-file", config_file}); - + options.arguments({"--ros-args", "--params-file", config_file}); // Instantiate the CognitiveModule using the specified configuration file - auto cm1 = cs4home_core::CognitiveModule::make_shared("cognitive_module_1", options); + auto cm1 = + cs4home_core::CognitiveModule::make_shared("cognitive_module_1", options); ASSERT_EQ(std::string(cm1->get_name()), "cognitive_module_1"); // Verify the number of parameters loaded from the configuration file auto params = cm1->list_parameters({}, 0); - ASSERT_EQ(params.names.size(), 7u); + ASSERT_EQ(params.names.size(), 8u); // Set up publisher and subscriber nodes for testing message processing auto pub_node = rclcpp::Node::make_shared("pub_node"); auto sub_node = rclcpp::Node::make_shared("sub_node"); - auto pub = pub_node->create_publisher("/image_raw", 100); + auto pub = + pub_node->create_publisher("/image_raw", 100); std::vector images; auto sub = sub_node->create_subscription( - "/detections", 100, [&images](sensor_msgs::msg::Image msg) { - images.push_back(msg); - }); + "/detections", 100, + [&images](sensor_msgs::msg::Image msg) { images.push_back(msg); }); rclcpp::executors::SingleThreadedExecutor exe; exe.add_node(cm1->get_node_base_interface()); @@ -443,11 +464,14 @@ TEST(cognitive_module_test, startup_simple) exe.add_node(sub_node); // Transition the CognitiveModule through the lifecycle states - cm1->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); - ASSERT_EQ(cm1->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + cm1->trigger_transition( + lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + ASSERT_EQ(cm1->get_current_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); cm1->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); - ASSERT_EQ(cm1->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ(cm1->get_current_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); // Publish a series of messages to test processing by the CognitiveModule sensor_msgs::msg::Image msg; @@ -463,18 +487,20 @@ TEST(cognitive_module_test, startup_simple) } // Transition the module back to inactive state - cm1->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE); - ASSERT_EQ(cm1->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + cm1->trigger_transition( + lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE); + ASSERT_EQ(cm1->get_current_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - // Verify that the messages were processed correctly with doubled frame_id values + // Verify that the messages were processed correctly with doubled frame_id + // values ASSERT_EQ(images.size(), 10); for (int i = 0; i < 10; i++) { ASSERT_EQ(i * 2, std::atoi(images[i].header.frame_id.c_str())); } } -int main(int argc, char ** argv) -{ +int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); rclcpp::init(argc, argv); diff --git a/cs4home_core/test/config/startup_simple_1.yaml b/cs4home_core/test/config/startup_simple_1.yaml index ff72d90..d927b14 100644 --- a/cs4home_core/test/config/startup_simple_1.yaml +++ b/cs4home_core/test/config/startup_simple_1.yaml @@ -1,11 +1,32 @@ +master_1: + ros__parameters: + flows: [flow_1] + flow_1: [cognitive_module_1, cognitive_module_2] + cognitive_module_1: ros__parameters: core: image_filter afferent: simple_image_input simple_image_input: - topics: ["/image_raw"] + topics: ["/image_raw", "/camera_info"] + types: ["sensor_msgs/msg/Image", "sensor_msgs/msg/CameraInfo"] efferent: simple_image_output simple_image_output: topics: ["/detections"] + types: ["sensor_msgs/msg/Image"] + meta: default_meta + coupling: default_coupling + +cognitive_module_2: + ros__parameters: + core: image_filter + afferent: simple_image_input + simple_image_input: + topics: ["/detections"] + types: ["sensor_msgs/msg/Image"] + efferent: simple_image_output + simple_image_output: + topics: ["/detections2"] + types: ["sensor_msgs/msg/Image"] meta: default_meta coupling: default_coupling diff --git a/cs4home_core/test/master_test.cpp b/cs4home_core/test/master_test.cpp index 53a3361..5e2d767 100644 --- a/cs4home_core/test/master_test.cpp +++ b/cs4home_core/test/master_test.cpp @@ -14,39 +14,151 @@ #include "ament_index_cpp/get_package_share_directory.hpp" -#include "cs4home_core/Flow.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "sensor_msgs/msg/image.hpp" + #include "cs4home_core/CognitiveModule.hpp" +#include "cs4home_core/Flow.hpp" +#include "cs4home_core/Master.hpp" #include "gtest/gtest.h" +using namespace std::chrono_literals; + +TEST(master_test, flow_creation) { + auto flow1 = cs4home_core::Flow::make_shared( + "flow1", std::vector({"A", "B", "C", "D"})); + auto flow2 = cs4home_core::Flow::make_shared( + "flow2", std::vector({"A", "B", "C", "E"})); + + ASSERT_EQ(flow1->get_flow(), std::vector({"A", "B", "C", "D"})); + ASSERT_EQ(flow2->get_flow(), std::vector({"A", "B", "C", "E"})); +} + +TEST(master_test, master_creation) { + std::string pkgpath = + ament_index_cpp::get_package_share_directory("cs4home_core"); + std::string config_file = pkgpath + "/config/startup_simple_1.yaml"; + + rclcpp::NodeOptions options; + options.arguments({"--ros-args", "--params-file", config_file}); + + auto master = cs4home_core::Master::make_shared("master_1", options); + auto cm1 = + cs4home_core::CognitiveModule::make_shared("cognitive_module_1", options); + auto cm2 = + cs4home_core::CognitiveModule::make_shared("cognitive_module_2", options); + + auto pub_node = rclcpp::Node::make_shared("pub_node"); + auto sub_node = rclcpp::Node::make_shared("sub_node"); + auto pub = pub_node->create_publisher("/image", 100); + std::vector images; + auto sub = sub_node->create_subscription( + "/detections2", 100, + [&images](sensor_msgs::msg::Image msg) { images.push_back(msg); }); + + rclcpp::executors::SingleThreadedExecutor exe; + exe.add_node(master->get_node_base_interface()); + exe.add_node(cm1->get_node_base_interface()); + exe.add_node(cm2->get_node_base_interface()); + exe.add_node(pub_node); + exe.add_node(sub_node); + + master->trigger_transition( + lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + cm1->trigger_transition( + lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + cm2->trigger_transition( + lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + ASSERT_EQ(master->get_current_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + ASSERT_EQ(cm1->get_current_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + ASSERT_EQ(cm2->get_current_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + + const auto &flows = master->get_flows(); + ASSERT_EQ(flows.size(), 1u); + + ASSERT_NE(flows.find("flow_1"), flows.end()); + const auto &flow_1 = flows.at("flow_1"); + const auto &flow_1_cms = flow_1->get_flow(); + + ASSERT_EQ(flow_1_cms, std::vector( + {"cognitive_module_1", "cognitive_module_2"})); + + master->trigger_transition( + lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); + ASSERT_EQ(master->get_current_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + { + auto start = master->now(); + while (cm1->now() - start < 1s) { + exe.spin_some(); + } + } + + // They should be activated by master, as it is part of a flow + ASSERT_EQ(cm1->get_current_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ(cm2->get_current_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + sensor_msgs::msg::Image msg; + for (int i = 0; i < 10; i++) { + msg.header.frame_id = std::to_string(i); + pub->publish(msg); + exe.spin_some(); + } + + auto start = cm1->now(); + while (cm1->now() - start < 1s) { + exe.spin_some(); + } + + master->trigger_transition( + lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE); + ASSERT_EQ(master->get_current_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + ASSERT_EQ(cm1->get_current_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + ASSERT_EQ(cm2->get_current_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + + ASSERT_EQ(images.size(), 10); + for (int i = 0; i < 10; i++) { + ASSERT_EQ(i * 4, std::atoi(images[i].header.frame_id.c_str())); + } +} + /** * @brief Unit test for verifying the creation and structure of Flow instances. * - * This test checks that `Flow` instances are created correctly with the expected - * sequence of nodes, ensuring that the get_nodes() function returns the correct - * vector of node names. + * This test checks that `Flow` instances are created correctly with the + * expected sequence of nodes, ensuring that the get_nodes() function returns + * the correct vector of node names. */ -TEST(flow_test, flow_creation) -{ - cs4home_core::Flow flow1({"A", "B", "C", "D"}); - cs4home_core::Flow flow2({"A", "B", "C", "E"}); +TEST(flow_test, flow_creation) { + cs4home_core::Flow flow1("flow1", {"A", "B", "C", "D"}); + cs4home_core::Flow flow2("flow2", {"A", "B", "C", "E"}); // Check that the nodes in flow1 match the expected sequence - ASSERT_EQ(flow1.get_nodes(), std::vector({"A", "B", "C", "D"})); + ASSERT_EQ(flow1.get_flow(), std::vector({"A", "B", "C", "D"})); // Check that the nodes in flow2 match the expected sequence - ASSERT_EQ(flow2.get_nodes(), std::vector({"A", "B", "C", "E"})); + ASSERT_EQ(flow2.get_flow(), std::vector({"A", "B", "C", "E"})); } /** * @brief Main function for running all GoogleTest unit tests. * - * Initializes GoogleTest and ROS 2, then runs all tests defined in the executable. + * Initializes GoogleTest and ROS 2, then runs all tests defined in the + * executable. * * @param argc Argument count * @param argv Argument vector * @return int Test run result (0 if all tests passed, non-zero otherwise) */ -int main(int argc, char ** argv) -{ +int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); rclcpp::init(argc, argv);