diff --git a/cs4home_core/CMakeLists.txt b/cs4home_core/CMakeLists.txt deleted file mode 100644 index 99c7057..0000000 --- a/cs4home_core/CMakeLists.txt +++ /dev/null @@ -1,59 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(cs4home_core) - -set(CMAKE_BUILD_TYPE debug) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(rclcpp_components REQUIRED) - -set(dependencies - rclcpp - rclcpp_lifecycle - rclcpp_components -) - -include_directories(include) - -add_library(${PROJECT_NAME} SHARED - src/cs4home_core/Master.cpp - src/cs4home_core/CognitiveModule.cpp - src/cs4home_core/Afferent.cpp - src/cs4home_core/Efferent.cpp - src/cs4home_core/Core.cpp - src/cs4home_core/Meta.cpp - src/cs4home_core/Coupling.cpp -) -ament_target_dependencies(${PROJECT_NAME} ${dependencies}) - -install(DIRECTORY include/ - DESTINATION include/ -) - -install(TARGETS - ${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - ament_lint_auto_find_test_dependencies() - - find_package(ament_cmake_gtest REQUIRED) - add_subdirectory(test) -endif() - -ament_export_include_directories(include) -ament_export_libraries(${PROJECT_NAME}) -ament_export_dependencies(${dependencies}) - -ament_package() diff --git a/cs4home_core/include/cs4home_core/Afferent.hpp b/cs4home_core/include/cs4home_core/Afferent.hpp deleted file mode 100644 index 4c05712..0000000 --- a/cs4home_core/include/cs4home_core/Afferent.hpp +++ /dev/null @@ -1,90 +0,0 @@ -// Copyright 2024 Intelligent Robotics Lab -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// 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 "rclcpp_lifecycle/lifecycle_node.hpp" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp/macros.hpp" - -namespace cs4home_core -{ - -class Afferent -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS(Afferent) - - enum EfferentProcessMode {CALLBACK, ONDEMAND}; - - explicit Afferent(rclcpp_lifecycle::LifecycleNode::SharedPtr parent); - - virtual bool configure() = 0; - - void set_mode( - EfferentProcessMode mode, - std::function)> cb = nullptr); - - EfferentProcessMode get_mode() {return mode_;} - void set_max_queue_size(size_t size) {max_queue_size_ = size;} - size_t get_max_queue_size() {return max_queue_size_;} - - template std::unique_ptr get_msg( - std::unique_ptr msg) - { - auto typed_msg = std::make_unique(); - rclcpp::Serialization serializer; - serializer.deserialize_message(msg.get(), typed_msg.get()); - - return std::move(typed_msg); - } - - template std::unique_ptr get_msg() - { - if (msg_queue_.empty()) { - return {}; - } - - std::unique_ptr msg = std::move(msg_queue_.front()); - msg_queue_.pop(); - - return get_msg(std::move(msg)); - } - -protected: - rclcpp_lifecycle::LifecycleNode::SharedPtr parent_; - std::vector> subs_; - - EfferentProcessMode mode_ {ONDEMAND}; - - const size_t MAX_DEFAULT_QUEUE_SIZE = 100; - size_t max_queue_size_ {MAX_DEFAULT_QUEUE_SIZE}; - std::queue> msg_queue_; - - std::function)> callback_; - - bool create_subscriber(const std::string & topic, const std::string & type); -}; - -} // namespace cs4home_core - -#endif // CS4HOME_CORE__AFFERENT_HPP_ diff --git a/cs4home_core/include/cs4home_core/CognitiveModule.hpp b/cs4home_core/include/cs4home_core/CognitiveModule.hpp deleted file mode 100644 index c54173d..0000000 --- a/cs4home_core/include/cs4home_core/CognitiveModule.hpp +++ /dev/null @@ -1,80 +0,0 @@ -// Copyright 2024 Intelligent Robotics Lab -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - - -#ifndef CS4HOME_CORE__COGNITIVEMODULE_HPP_ -#define CS4HOME_CORE__COGNITIVEMODULE_HPP_ - -#include - -#include -#include - -#include "cs4home_core/Afferent.hpp" -#include "cs4home_core/Core.hpp" -#include "cs4home_core/Coupling.hpp" -#include "cs4home_core/Efferent.hpp" -#include "cs4home_core/Meta.hpp" - -#include "rclcpp/macros.hpp" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" - -namespace cs4home_core -{ - -class CognitiveModule : public rclcpp_lifecycle::LifecycleNode -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS(CognitiveModule) - using CallbackReturnT = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - - explicit CognitiveModule(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); - - CallbackReturnT on_configure(const rclcpp_lifecycle::State & state); - CallbackReturnT on_activate(const rclcpp_lifecycle::State & state); - CallbackReturnT on_deactivate(const rclcpp_lifecycle::State & state); - CallbackReturnT on_cleanup(const rclcpp_lifecycle::State & state); - CallbackReturnT on_shutdown(const rclcpp_lifecycle::State & state); - CallbackReturnT on_error(const rclcpp_lifecycle::State & state); - -protected: - Afferent::SharedPtr afferent_; - Efferent::SharedPtr efferent_; - Core::SharedPtr core_; - Meta::SharedPtr meta_; - Coupling::SharedPtr coupling_; - - std::string core_name_; - std::string afferent_name_; - std::string efferent_name_; - std::string meta_name_; - std::string coupling_name_; - - template - std::tuple load_component( - const std::string & name, rclcpp_lifecycle::LifecycleNode::SharedPtr parent); -}; - -} // namespace cs4home_core - -#endif // CS4HOME_CORE__COGNITIVEMODULE_HPP_ - -// #include "rclcpp_components/register_node_macro.hpp" -// -// // Register the component with class_loader. -// // This acts as a sort of entry point, allowing the component to be discoverable when its library -// // is being loaded into a running process. -// RCLCPP_COMPONENTS_REGISTER_NODE(cs4home_core::CognitiveModule) diff --git a/cs4home_core/include/cs4home_core/Core.hpp b/cs4home_core/include/cs4home_core/Core.hpp deleted file mode 100644 index 3e23af1..0000000 --- a/cs4home_core/include/cs4home_core/Core.hpp +++ /dev/null @@ -1,53 +0,0 @@ -// Copyright 2024 Intelligent Robotics Lab -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - - -#ifndef CS4HOME_CORE__CORE_HPP_ -#define CS4HOME_CORE__CORE_HPP_ - -#include - - -#include "cs4home_core/Afferent.hpp" -#include "cs4home_core/Efferent.hpp" - -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "rclcpp/macros.hpp" - -namespace cs4home_core -{ - -class Core -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS(Core) - - explicit Core(rclcpp_lifecycle::LifecycleNode::SharedPtr parent); - - virtual bool configure() = 0; - virtual bool activate() = 0; - virtual bool deactivate() = 0; - - void set_afferent(cs4home_core::Afferent::SharedPtr afferent) {afferent_ = afferent;} - void set_efferent(cs4home_core::Efferent::SharedPtr efferent) {efferent_ = efferent;} - -protected: - rclcpp_lifecycle::LifecycleNode::SharedPtr parent_; - cs4home_core::Afferent::SharedPtr afferent_; - cs4home_core::Efferent::SharedPtr efferent_; -}; - -} // namespace cs4home_core - -#endif // CS4HOME_CORE__CORE_HPP_ diff --git a/cs4home_core/include/cs4home_core/Coupling.hpp b/cs4home_core/include/cs4home_core/Coupling.hpp deleted file mode 100644 index d6849ce..0000000 --- a/cs4home_core/include/cs4home_core/Coupling.hpp +++ /dev/null @@ -1,40 +0,0 @@ -// Copyright 2024 Intelligent Robotics Lab -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - - -#ifndef CS4HOME_CORE__COUPLING_HPP_ -#define CS4HOME_CORE__COUPLING_HPP_ - -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "rclcpp/macros.hpp" - -namespace cs4home_core -{ - -class Coupling -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS(Coupling) - - explicit Coupling(rclcpp_lifecycle::LifecycleNode::SharedPtr parent); - - bool configure(); - -protected: - rclcpp_lifecycle::LifecycleNode::SharedPtr parent_; -}; - -} // namespace cs4home_core - -#endif // CS4HOME_CORE__COUPLING_HPP_ diff --git a/cs4home_core/include/cs4home_core/Efferent.hpp b/cs4home_core/include/cs4home_core/Efferent.hpp deleted file mode 100644 index a8a966a..0000000 --- a/cs4home_core/include/cs4home_core/Efferent.hpp +++ /dev/null @@ -1,60 +0,0 @@ -// Copyright 2024 Intelligent Robotics Lab -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - - -#ifndef CS4HOME_CORE__EFFERENT_HPP_ -#define CS4HOME_CORE__EFFERENT_HPP_ - -#include -#include -#include - -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "rclcpp/macros.hpp" - -namespace cs4home_core -{ - -class Efferent -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS(Efferent) - - explicit Efferent(rclcpp_lifecycle::LifecycleNode::SharedPtr parent); - - virtual bool configure() = 0; - - template - void publish(std::unique_ptr msg) - { - rclcpp::Serialization serializer; - auto untyped_msg = rclcpp::SerializedMessage(); - - serializer.serialize_message(msg.get(), &untyped_msg); - - for (auto & pub : pubs_) { - pub->publish(untyped_msg); - } - } - -protected: - rclcpp_lifecycle::LifecycleNode::SharedPtr parent_; - std::vector> pubs_; - - bool create_publisher(const std::string & topic, const std::string & type); -}; - -} // namespace cs4home_core - -#endif // CS4HOME_CORE__EFFERENT_HPP_ diff --git a/cs4home_core/include/cs4home_core/Master.hpp b/cs4home_core/include/cs4home_core/Master.hpp deleted file mode 100644 index ef82b5d..0000000 --- a/cs4home_core/include/cs4home_core/Master.hpp +++ /dev/null @@ -1,60 +0,0 @@ -// Copyright 2024 Intelligent Robotics Lab -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - - -#ifndef CS4HOME_CORE__MASTER_HPP_ -#define CS4HOME_CORE__MASTER_HPP_ - -#include -#include - -#include "cs4home_core/CognitiveModule.hpp" - -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp/macros.hpp" - -namespace cs4home_core -{ - -class Master : public rclcpp_lifecycle::LifecycleNode -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS(Master) - using CallbackReturnT = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - - explicit Master(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); - - CallbackReturnT on_configure(const rclcpp_lifecycle::State & state); - CallbackReturnT on_activate(const rclcpp_lifecycle::State & state); - CallbackReturnT on_deactivate(const rclcpp_lifecycle::State & state); - CallbackReturnT on_cleanup(const rclcpp_lifecycle::State & state); - CallbackReturnT on_shutdown(const rclcpp_lifecycle::State & state); - CallbackReturnT on_error(const rclcpp_lifecycle::State & state); - -protected: - std::map cog_modules_; -}; - -} // namespace cs4home_core - -#endif // CS4HOME_CORE__MASTER_HPP_ - -// #include "rclcpp_components/register_node_macro.hpp" -// -// // Register the component with class_loader. -// // This acts as a sort of entry point, allowing the component to be discoverable when its library -// // is being loaded into a running process. -// RCLCPP_COMPONENTS_REGISTER_NODE(cs4home_core::Master) diff --git a/cs4home_core/include/cs4home_core/Meta.hpp b/cs4home_core/include/cs4home_core/Meta.hpp deleted file mode 100644 index 4267dd1..0000000 --- a/cs4home_core/include/cs4home_core/Meta.hpp +++ /dev/null @@ -1,40 +0,0 @@ -// Copyright 2024 Intelligent Robotics Lab -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - - -#ifndef CS4HOME_CORE__META_HPP_ -#define CS4HOME_CORE__META_HPP_ - -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "rclcpp/macros.hpp" - -namespace cs4home_core -{ - -class Meta -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS(Meta) - - explicit Meta(rclcpp_lifecycle::LifecycleNode::SharedPtr parent); - - bool configure(); - -protected: - rclcpp_lifecycle::LifecycleNode::SharedPtr parent_; -}; - -} // namespace cs4home_core - -#endif // CS4HOME_CORE__META_HPP_ diff --git a/cs4home_core/include/cs4home_core/macros.hpp b/cs4home_core/include/cs4home_core/macros.hpp deleted file mode 100644 index 8e54e19..0000000 --- a/cs4home_core/include/cs4home_core/macros.hpp +++ /dev/null @@ -1,35 +0,0 @@ -// Copyright 2024 Intelligent Robotics Lab -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - - -#ifndef CS4HOME_CORE__MACROS_HPP_ -#define CS4HOME_CORE__MACROS_HPP_ - -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp/macros.hpp" - -namespace cs4home_core -{ - -#define CS_REGISTER_COMPONENT(class_name) \ - extern "C" class_name::SharedPtr create_instance( \ - rclcpp_lifecycle::LifecycleNode::SharedPtr parent) \ - { \ - return class_name::make_shared(parent); \ - } - -} // namespace cs4home_core - -#endif // CS4HOME_CORE__MACROS_HPP_ diff --git a/cs4home_core/launch/launch_simple.launch.py b/cs4home_core/launch/launch_simple.launch.py deleted file mode 100644 index bf1b282..0000000 --- a/cs4home_core/launch/launch_simple.launch.py +++ /dev/null @@ -1,53 +0,0 @@ -# Copyright 2024 Intelligent Robotics Lab -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import os - -from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - - -def generate_launch_description(): - namespace = LaunchConfiguration('namespace') - pkg_dir = get_package_share_directory('cs4home_simple_project') - - declare_namespace_cmd = DeclareLaunchArgument( - 'namespace', - default_value='', - description='Namespace') - - # Specify the actions - cm1_cmd = Node( - package='cs4home_core', - executable='cognitive_module', - name='image_filter', - namespace=namespace, - output='screen', - parameters=[ - os.path.join(pkg_dir, 'config', 'params_simple.yaml') - ]) - - # Create the launch description and populate - ld = LaunchDescription() - - ld.add_action(declare_namespace_cmd) - - # Declare the launch options - ld.add_action(cm1_cmd) - - return ld diff --git a/cs4home_core/package.xml b/cs4home_core/package.xml deleted file mode 100644 index bc5d309..0000000 --- a/cs4home_core/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - cs4home_core - 0.0.0 - TODO: Package description - fmrico - Apache License, Version 2.0 - ament_cmake - - rclcpp - rclcpp_lifecycle - rclcpp_components - - ament_lint_auto - ament_lint_common - sensor_msgs - vision_msgs - - - ament_cmake - - diff --git a/cs4home_core/src/cs4home_core/Afferent.cpp b/cs4home_core/src/cs4home_core/Afferent.cpp deleted file mode 100644 index 45e4006..0000000 --- a/cs4home_core/src/cs4home_core/Afferent.cpp +++ /dev/null @@ -1,77 +0,0 @@ -// Copyright 2024 Intelligent Robotics Lab -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - - -#include "cs4home_core/Afferent.hpp" - -#include "rclcpp/rclcpp.hpp" - -namespace cs4home_core -{ - -Afferent::Afferent(rclcpp_lifecycle::LifecycleNode::SharedPtr parent) -: parent_(parent) -{ -} - -void -Afferent::set_mode( - EfferentProcessMode mode, - std::function)> cb) -{ - if (mode == CALLBACK) { - if (cb) { - callback_ = cb; - } else { - RCLCPP_WARN( - parent_->get_logger(), "[Afferent] Error setting callback: not function specified"); - return; - } - } - mode_ = mode; -} - -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()); - - 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)); - } else { - RCLCPP_WARN( - parent_->get_logger(), "[Afferent] Error calling callback: not function specified"); - } - } 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 diff --git a/cs4home_core/src/cs4home_core/CognitiveModule.cpp b/cs4home_core/src/cs4home_core/CognitiveModule.cpp deleted file mode 100644 index 21d7306..0000000 --- a/cs4home_core/src/cs4home_core/CognitiveModule.cpp +++ /dev/null @@ -1,160 +0,0 @@ -// Copyright 2024 Intelligent Robotics Lab -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "cs4home_core/CognitiveModule.hpp" - -namespace cs4home_core -{ - -CognitiveModule::CognitiveModule(const rclcpp::NodeOptions & options) -: LifecycleNode("cognitive_module", options) -{ - declare_parameter("core", core_name_); - declare_parameter("afferent", afferent_name_); - declare_parameter("efferent", efferent_name_); - declare_parameter("meta", meta_name_); - declare_parameter("coupling", coupling_name_); -} - -using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - -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()); - 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()); - 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; - } - - 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; - } - - 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; - } - - 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; - } - - return CallbackReturnT::SUCCESS; -} - -CallbackReturnT CognitiveModule::on_activate(const rclcpp_lifecycle::State & state) -{ - (void)state; - - if (!core_->activate()) { - RCLCPP_ERROR(get_logger(), "Unable to activate Core"); - return CallbackReturnT::FAILURE; - } - - return CallbackReturnT::SUCCESS; -} - -CallbackReturnT CognitiveModule::on_deactivate(const rclcpp_lifecycle::State & state) -{ - (void)state; - - if (!core_->deactivate()) { - RCLCPP_ERROR(get_logger(), "Unable to activate Core"); - return CallbackReturnT::FAILURE; - } - - return CallbackReturnT::SUCCESS; -} - -CallbackReturnT CognitiveModule::on_cleanup(const rclcpp_lifecycle::State & state) -{ - (void)state; - - return CallbackReturnT::SUCCESS; -} - -CallbackReturnT CognitiveModule::on_shutdown(const rclcpp_lifecycle::State & state) -{ - (void)state; - - return CallbackReturnT::SUCCESS; -} - -CallbackReturnT CognitiveModule::on_error(const rclcpp_lifecycle::State & state) -{ - (void)state; - - return CallbackReturnT::SUCCESS; -} - - -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); - 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(); - if (dlsym_error) { - dlclose(handle); - return {nullptr, std::string("Cannot load symbol 'create_instance': ") + dlsym_error}; - } - return {create_instance(parent), ""}; -} - -} // namespace cs4home_core diff --git a/cs4home_core/src/cs4home_core/Core.cpp b/cs4home_core/src/cs4home_core/Core.cpp deleted file mode 100644 index fd9e0a9..0000000 --- a/cs4home_core/src/cs4home_core/Core.cpp +++ /dev/null @@ -1,31 +0,0 @@ -// Copyright 2024 Intelligent Robotics Lab -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "cs4home_core/Core.hpp" - -namespace cs4home_core -{ - -Core::Core(rclcpp_lifecycle::LifecycleNode::SharedPtr parent) -: parent_(parent) -{ -} - -bool -Core::configure() -{ - return true; -} - -} // namespace cs4home_core diff --git a/cs4home_core/src/cs4home_core/Coupling.cpp b/cs4home_core/src/cs4home_core/Coupling.cpp deleted file mode 100644 index 09309fd..0000000 --- a/cs4home_core/src/cs4home_core/Coupling.cpp +++ /dev/null @@ -1,31 +0,0 @@ -// Copyright 2024 Intelligent Robotics Lab -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "cs4home_core/Coupling.hpp" - -namespace cs4home_core -{ - -Coupling::Coupling(rclcpp_lifecycle::LifecycleNode::SharedPtr parent) -: parent_(parent) -{ -} - -bool -Coupling::configure() -{ - return true; -} - -} // namespace cs4home_core diff --git a/cs4home_core/src/cs4home_core/Efferent.cpp b/cs4home_core/src/cs4home_core/Efferent.cpp deleted file mode 100644 index 72ce0b0..0000000 --- a/cs4home_core/src/cs4home_core/Efferent.cpp +++ /dev/null @@ -1,36 +0,0 @@ -// Copyright 2024 Intelligent Robotics Lab -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "cs4home_core/Efferent.hpp" - -namespace cs4home_core -{ - -Efferent::Efferent(rclcpp_lifecycle::LifecycleNode::SharedPtr parent) -: parent_(parent) -{ -} - -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); - - pubs_.push_back(pub); - - return true; -} - -} // namespace cs4home_core diff --git a/cs4home_core/src/cs4home_core/Master.cpp b/cs4home_core/src/cs4home_core/Master.cpp deleted file mode 100644 index 20cdcbb..0000000 --- a/cs4home_core/src/cs4home_core/Master.cpp +++ /dev/null @@ -1,77 +0,0 @@ -// Copyright 2024 Intelligent Robotics Lab -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - - -#include "cs4home_core/Master.hpp" - -namespace cs4home_core -{ - -Master::Master(const rclcpp::NodeOptions & options) -: LifecycleNode("master", options) -{ -} - -using CallbackReturnT = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - -CallbackReturnT -Master::on_configure(const rclcpp_lifecycle::State & state) -{ - (void)state; - - return CallbackReturnT::SUCCESS; -} - -CallbackReturnT -Master::on_activate(const rclcpp_lifecycle::State & state) -{ - (void)state; - - return CallbackReturnT::SUCCESS; -} - -CallbackReturnT -Master::on_deactivate(const rclcpp_lifecycle::State & state) -{ - (void)state; - - return CallbackReturnT::SUCCESS; -} - -CallbackReturnT -Master::on_cleanup(const rclcpp_lifecycle::State & state) -{ - (void)state; - - return CallbackReturnT::SUCCESS; -} - -CallbackReturnT -Master::on_shutdown(const rclcpp_lifecycle::State & state) -{ - (void)state; - - return CallbackReturnT::SUCCESS; -} - -CallbackReturnT -Master::on_error(const rclcpp_lifecycle::State & state) -{ - (void)state; - - return CallbackReturnT::SUCCESS; -} - -} // namespace cs4home_core diff --git a/cs4home_core/src/cs4home_core/Meta.cpp b/cs4home_core/src/cs4home_core/Meta.cpp deleted file mode 100644 index e0db705..0000000 --- a/cs4home_core/src/cs4home_core/Meta.cpp +++ /dev/null @@ -1,31 +0,0 @@ -// Copyright 2024 Intelligent Robotics Lab -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "cs4home_core/Meta.hpp" - -namespace cs4home_core -{ - -Meta::Meta(rclcpp_lifecycle::LifecycleNode::SharedPtr parent) -: parent_(parent) -{ -} - -bool -Meta::configure() -{ - return true; -} - -} // namespace cs4home_core diff --git a/cs4home_core/test/CMakeLists.txt b/cs4home_core/test/CMakeLists.txt deleted file mode 100644 index d8e03fb..0000000 --- a/cs4home_core/test/CMakeLists.txt +++ /dev/null @@ -1,55 +0,0 @@ -find_package(sensor_msgs REQUIRED) -find_package(vision_msgs REQUIRED) -find_package(ament_index_cpp REQUIRED) - -set(test_dependencies - sensor_msgs - vision_msgs - ament_index_cpp -) - -add_library(image_filter SHARED ImageFilter.cpp) -ament_target_dependencies(image_filter ${dependencies} ${test_dependencies}) -target_link_libraries(image_filter ${PROJECT_NAME}) - -add_library(image_filter_cb SHARED ImageFilterCB.cpp) -ament_target_dependencies(image_filter_cb ${dependencies} ${test_dependencies}) -target_link_libraries(image_filter_cb ${PROJECT_NAME}) - -add_library(simple_image_input SHARED SimpleImageInput.cpp) -ament_target_dependencies(simple_image_input ${dependencies} ${test_dependencies}) -target_link_libraries(simple_image_input ${PROJECT_NAME}) - -add_library(simple_image_output SHARED SimpleImageOutput.cpp) -ament_target_dependencies(simple_image_output ${dependencies} ${test_dependencies}) -target_link_libraries(simple_image_output ${PROJECT_NAME}) - -add_library(default_meta SHARED DefaultMeta.cpp) -ament_target_dependencies(default_meta ${dependencies} ${test_dependencies}) -target_link_libraries(default_meta ${PROJECT_NAME}) - -add_library(default_coupling SHARED DefaultCoupling.cpp) -ament_target_dependencies(default_coupling ${dependencies} ${test_dependencies}) -target_link_libraries(default_coupling ${PROJECT_NAME}) - - -install(TARGETS - image_filter - image_filter_cb - simple_image_input - simple_image_output - default_meta - default_coupling - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} -) - -ament_add_gtest(cognitive_module_test cognitive_module_test.cpp) -target_link_libraries(cognitive_module_test ${PROJECT_NAME}) -ament_target_dependencies(cognitive_module_test ${test_dependencies}) - -install(DIRECTORY - config - DESTINATION share/${PROJECT_NAME} -) diff --git a/cs4home_core/test/DefaultCoupling.cpp b/cs4home_core/test/DefaultCoupling.cpp deleted file mode 100644 index 8576aec..0000000 --- a/cs4home_core/test/DefaultCoupling.cpp +++ /dev/null @@ -1,41 +0,0 @@ -// Copyright 2024 Intelligent Robotics Lab -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - - -#include "cs4home_core/Coupling.hpp" -#include "cs4home_core/macros.hpp" - -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "rclcpp/macros.hpp" - -class DefaultCoupling : public cs4home_core::Coupling -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS(DefaultCoupling) - - explicit DefaultCoupling(rclcpp_lifecycle::LifecycleNode::SharedPtr parent) - : Coupling(parent) - { - RCLCPP_DEBUG(parent_->get_logger(), "Coupling created: [DefaultCoupling]"); - } - - - bool configure() - { - RCLCPP_DEBUG(parent_->get_logger(), "Coupling configured"); - return true; - } -}; - -CS_REGISTER_COMPONENT(DefaultCoupling) diff --git a/cs4home_core/test/DefaultMeta.cpp b/cs4home_core/test/DefaultMeta.cpp deleted file mode 100644 index 7db1663..0000000 --- a/cs4home_core/test/DefaultMeta.cpp +++ /dev/null @@ -1,41 +0,0 @@ -// Copyright 2024 Intelligent Robotics Lab -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - - -#include "cs4home_core/Meta.hpp" -#include "cs4home_core/macros.hpp" - -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "rclcpp/macros.hpp" - -class DefaultMeta : public cs4home_core::Meta -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS(DefaultMeta) - - explicit DefaultMeta(rclcpp_lifecycle::LifecycleNode::SharedPtr parent) - : Meta(parent) - { - RCLCPP_DEBUG(parent_->get_logger(), "Meta created: [DefaultMeta]"); - } - - - bool configure() - { - RCLCPP_DEBUG(parent_->get_logger(), "Meta configured"); - return true; - } -}; - -CS_REGISTER_COMPONENT(DefaultMeta) diff --git a/cs4home_core/test/ImageFilter.cpp b/cs4home_core/test/ImageFilter.cpp deleted file mode 100644 index 4d80ba9..0000000 --- a/cs4home_core/test/ImageFilter.cpp +++ /dev/null @@ -1,78 +0,0 @@ -// Copyright 2024 Intelligent Robotics Lab -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - - -#include "cs4home_core/Core.hpp" -#include "cs4home_core/macros.hpp" - -#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 : public cs4home_core::Core -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS(ImageFilter) - - explicit ImageFilter(rclcpp_lifecycle::LifecycleNode::SharedPtr parent) - : Core(parent) - { - RCLCPP_DEBUG(parent_->get_logger(), "Core created: [ImageFilter]"); - } - - void process_in_image(sensor_msgs::msg::Image::UniquePtr 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)); - } - - void timer_callback() - { - auto msg = afferent_->get_msg(); - if (msg != nullptr) { - process_in_image(std::move(msg)); - } - } - - bool configure() - { - RCLCPP_DEBUG(parent_->get_logger(), "Core configured"); - return true; - } - - bool activate() - { - timer_ = parent_->create_wall_timer( - 50ms, std::bind(&ImageFilter::timer_callback, this)); - return true; - } - - bool deactivate() - { - timer_ = nullptr; - return true; - } - -private: - rclcpp::TimerBase::SharedPtr timer_; -}; - -CS_REGISTER_COMPONENT(ImageFilter) diff --git a/cs4home_core/test/ImageFilterCB.cpp b/cs4home_core/test/ImageFilterCB.cpp deleted file mode 100644 index 20f0451..0000000 --- a/cs4home_core/test/ImageFilterCB.cpp +++ /dev/null @@ -1,74 +0,0 @@ -// Copyright 2024 Intelligent Robotics Lab -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - - -#include "cs4home_core/Core.hpp" -#include "cs4home_core/macros.hpp" - -#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 ImageFilterCB : public cs4home_core::Core -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS(ImageFilterCB) - - explicit ImageFilterCB(rclcpp_lifecycle::LifecycleNode::SharedPtr parent) - : Core(parent) - { - RCLCPP_DEBUG(parent_->get_logger(), "Core created: [ImageFilterCB]"); - } - - void process_in_image(std::unique_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)); - } - - bool configure() - { - RCLCPP_DEBUG(parent_->get_logger(), "Core configured"); - - afferent_->set_mode( - cs4home_core::Afferent::CALLBACK, std::bind(&ImageFilterCB::process_in_image, this, _1)); - - return true; - } - - bool activate() - { - return true; - } - - bool deactivate() - { - timer_ = nullptr; - return true; - } - -private: - rclcpp::TimerBase::SharedPtr timer_; -}; - -CS_REGISTER_COMPONENT(ImageFilterCB) diff --git a/cs4home_core/test/SimpleImageInput.cpp b/cs4home_core/test/SimpleImageInput.cpp deleted file mode 100644 index 80bec94..0000000 --- a/cs4home_core/test/SimpleImageInput.cpp +++ /dev/null @@ -1,67 +0,0 @@ -// Copyright 2024 Intelligent Robotics Lab -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - - -#include "cs4home_core/Afferent.hpp" -#include "cs4home_core/macros.hpp" - -#include "sensor_msgs/msg/image.hpp" - -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "rclcpp/macros.hpp" - -class SimpleImageInput : public cs4home_core::Afferent -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS(SimpleImageInput) - - explicit SimpleImageInput(rclcpp_lifecycle::LifecycleNode::SharedPtr parent) - : Afferent(parent) - { - RCLCPP_DEBUG(parent_->get_logger(), "Efferent created: [SimpleImageInput]"); - - parent_->declare_parameter("simple_image_input.topics", input_topic_names_); - } - - 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; - } - -private: - std::vector input_topic_names_; -}; - -CS_REGISTER_COMPONENT(SimpleImageInput) diff --git a/cs4home_core/test/SimpleImageOutput.cpp b/cs4home_core/test/SimpleImageOutput.cpp deleted file mode 100644 index bc6f89b..0000000 --- a/cs4home_core/test/SimpleImageOutput.cpp +++ /dev/null @@ -1,66 +0,0 @@ -// Copyright 2024 Intelligent Robotics Lab -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - - -#include "cs4home_core/Efferent.hpp" -#include "cs4home_core/macros.hpp" - -#include "sensor_msgs/msg/image.hpp" - -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "rclcpp/macros.hpp" - -class SimpleImageOutput : public cs4home_core::Efferent -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS(SimpleImageOutput) - - explicit SimpleImageOutput(rclcpp_lifecycle::LifecycleNode::SharedPtr parent) - : Efferent(parent) - { - RCLCPP_DEBUG(parent_->get_logger(), "Afferent created: [SimpleImageOutput]"); - - parent_->declare_parameter("simple_image_output.topics", output_topic_names_); - } - - 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; - } - - void publish_image(sensor_msgs::msg::Image::UniquePtr msg) - { - publish(std::move(msg)); - } - -private: - std::vector output_topic_names_; -}; - -CS_REGISTER_COMPONENT(SimpleImageOutput) diff --git a/cs4home_core/test/cognitive_module_test.cpp b/cs4home_core/test/cognitive_module_test.cpp deleted file mode 100644 index 9c918f7..0000000 --- a/cs4home_core/test/cognitive_module_test.cpp +++ /dev/null @@ -1,376 +0,0 @@ -// Copyright 2024 Intelligent Robotics Lab -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#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" - -#include "cs4home_core/Afferent.hpp" -#include "cs4home_core/CognitiveModule.hpp" -#include "gtest/gtest.h" - - -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); - 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(); - if (dlsym_error) { - dlclose(handle); - return {nullptr, std::string("Cannot load symbol 'create_instance': ") + dlsym_error}; - } - return {create_instance(parent), ""}; -} - -using namespace std::chrono_literals; - -TEST(cognitive_module_test, afferent_on_demand) -{ - auto node = rclcpp_lifecycle::LifecycleNode::make_shared("test_lc_node"); - auto pub_node = rclcpp::Node::make_shared("pub_node"); - auto pub = pub_node->create_publisher("/image", 100); - - rclcpp::executors::SingleThreadedExecutor exe; - exe.add_node(node->get_node_base_interface()); - exe.add_node(pub_node); - - std::vector topics {"/image"}; - - auto [afferent, error_afferent] = load_component( - "simple_image_input", node); - ASSERT_NE(afferent, nullptr); - - node->set_parameter(rclcpp::Parameter("simple_image_input.topics", topics)); - ASSERT_TRUE(afferent->configure()); - - 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 = node->now(); - while (node->now() - start < 1s) { - exe.spin_some(); - } - - for (int i = 0; i < 10; i++) { - auto in_msg = afferent->get_msg(); - ASSERT_NE(in_msg, nullptr); - ASSERT_EQ(i, std::atoi(in_msg->header.frame_id.c_str())); - } - - auto in_msg = afferent->get_msg(); - ASSERT_EQ(in_msg, nullptr); -} - - -TEST(cognitive_module_test, afferent_on_subscription) -{ - auto node = rclcpp_lifecycle::LifecycleNode::make_shared("test_lc_node"); - auto pub_node = rclcpp::Node::make_shared("pub_node"); - auto pub = pub_node->create_publisher("/image", 100); - - rclcpp::executors::SingleThreadedExecutor exe; - exe.add_node(node->get_node_base_interface()); - exe.add_node(pub_node); - - std::vector topics {"/image"}; - std::vector> images; - - auto [afferent, error_afferent] = load_component( - "simple_image_input", node); - ASSERT_NE(afferent, nullptr); - - node->set_parameter(rclcpp::Parameter("simple_image_input.topics", topics)); - afferent->set_mode(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)); - } - ); - ASSERT_EQ(afferent->get_mode(), cs4home_core::Afferent::CALLBACK); - ASSERT_TRUE(afferent->configure()); - - 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 = node->now(); - while (node->now() - start < 1s) { - exe.spin_some(); - } - - ASSERT_EQ(images.size(), 10); - for (int i = 0; i < 10; i++) { - std::unique_ptr in_msg = std::move(images[i]); - ASSERT_NE(in_msg, nullptr); - - rclcpp::Serialization serializer; - auto typed_msg = std::make_unique(); - serializer.deserialize_message(in_msg.get(), typed_msg.get()); - - ASSERT_EQ(i, std::atoi(typed_msg->header.frame_id.c_str())); - } -} - - -TEST(cognitive_module_test, efferent) -{ - auto node = rclcpp_lifecycle::LifecycleNode::make_shared("test_lc_node"); - auto sub_node = rclcpp::Node::make_shared("sub_node"); - - std::vector images; - auto sub = sub_node->create_subscription( - "/image", 100, [&images] (sensor_msgs::msg::Image msg) { - images.push_back(msg); - }); - - rclcpp::executors::SingleThreadedExecutor exe; - exe.add_node(node->get_node_base_interface()); - exe.add_node(sub_node); - - std::vector topics {"/image"}; - - auto [efferent, error_efferent] = load_component( - "simple_image_output", node); - ASSERT_NE(efferent, nullptr); - - node->set_parameter(rclcpp::Parameter("simple_image_output.topics", topics)); - ASSERT_TRUE(efferent->configure()); - - for (int i = 0; i < 10; i++) { - auto msg = std::make_unique(); - msg->header.frame_id = std::to_string(i); - efferent->publish(std::move(msg)); - exe.spin_some(); - } - - auto start = node->now(); - while (node->now() - start < 1s) { - exe.spin_some(); - } - - ASSERT_EQ(images.size(), 10); - for (int i = 0; i < 10; i++) { - ASSERT_EQ(i, std::atoi(images[i].header.frame_id.c_str())); - } -} - - -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"); - - 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); - }); - - rclcpp::executors::SingleThreadedExecutor exe; - exe.add_node(node->get_node_base_interface()); - exe.add_node(pub_node); - exe.add_node(sub_node); - - std::vector in_topics {"/in_image"}; - std::vector out_topics {"/out_image"}; - - auto [afferent, error_afferent] = load_component( - "simple_image_input", node); - ASSERT_NE(afferent, nullptr); - auto [efferent, error_efferent] = load_component( - "simple_image_output", node); - ASSERT_NE(efferent, nullptr); - auto [core, error_core] = load_component( - "image_filter", node); - ASSERT_NE(core, nullptr); - - node->set_parameter(rclcpp::Parameter("simple_image_input.topics", in_topics)); - node->set_parameter(rclcpp::Parameter("simple_image_output.topics", out_topics)); - ASSERT_TRUE(afferent->configure()); - ASSERT_TRUE(efferent->configure()); - core->set_afferent(afferent); - core->set_efferent(efferent); - ASSERT_TRUE(core->configure()); - ASSERT_TRUE(core->activate()); - - 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 = node->now(); - while (node->now() - start < 1s) { - exe.spin_some(); - } - - ASSERT_TRUE(core->deactivate()); - - 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())); - } -} - -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"); - - 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); - }); - - rclcpp::executors::SingleThreadedExecutor exe; - exe.add_node(node->get_node_base_interface()); - exe.add_node(pub_node); - exe.add_node(sub_node); - - std::vector in_topics {"/in_image"}; - std::vector out_topics {"/out_image"}; - - auto [afferent, error_afferent] = load_component( - "simple_image_input", node); - ASSERT_NE(afferent, nullptr); - auto [efferent, error_efferent] = load_component( - "simple_image_output", node); - ASSERT_NE(efferent, nullptr); - auto [core, error_core] = load_component( - "image_filter_cb", node); - ASSERT_NE(core, nullptr); - - node->set_parameter(rclcpp::Parameter("simple_image_input.topics", in_topics)); - node->set_parameter(rclcpp::Parameter("simple_image_output.topics", out_topics)); - ASSERT_TRUE(afferent->configure()); - ASSERT_TRUE(efferent->configure()); - core->set_afferent(afferent); - core->set_efferent(efferent); - ASSERT_TRUE(core->configure()); - ASSERT_TRUE(core->activate()); - - 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 = node->now(); - while (node->now() - start < 1s) { - exe.spin_some(); - } - - ASSERT_TRUE(core->deactivate()); - - 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())); - } -} - - -TEST(cognitive_module_test, startup_simple) -{ - 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", "-r", "__node:=cognitive_module_1", "--params-file", config_file}); - - auto cm1 = cs4home_core::CognitiveModule::make_shared(options); - ASSERT_EQ(std::string(cm1->get_name()), "cognitive_module_1"); - - auto params = cm1->list_parameters({}, 0); - ASSERT_EQ(params.names.size(), 7u); - - 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); - - std::vector images; - auto sub = sub_node->create_subscription( - "/detections", 100, [&images] (sensor_msgs::msg::Image msg) { - images.push_back(msg); - }); - - rclcpp::executors::SingleThreadedExecutor exe; - exe.add_node(cm1->get_node_base_interface()); - exe.add_node(pub_node); - exe.add_node(sub_node); - - 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); - - 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(); - } - - cm1->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE); - ASSERT_EQ(cm1->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 * 2, std::atoi(images[i].header.frame_id.c_str())); - } -} - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - rclcpp::init(argc, argv); - - return RUN_ALL_TESTS(); -} diff --git a/cs4home_core/test/config/startup_simple_1.yaml b/cs4home_core/test/config/startup_simple_1.yaml deleted file mode 100644 index ff72d90..0000000 --- a/cs4home_core/test/config/startup_simple_1.yaml +++ /dev/null @@ -1,11 +0,0 @@ -cognitive_module_1: - ros__parameters: - core: image_filter - afferent: simple_image_input - simple_image_input: - topics: ["/image_raw"] - efferent: simple_image_output - simple_image_output: - topics: ["/detections"] - meta: default_meta - coupling: default_coupling diff --git a/cs4home_core/test/params_simple.yaml b/cs4home_core/test/params_simple.yaml deleted file mode 100644 index 1403a55..0000000 --- a/cs4home_core/test/params_simple.yaml +++ /dev/null @@ -1,13 +0,0 @@ -image_filter: - ros__parameters: - core: image_filter - efferent: ['image_input'] - image_input: - topic: /image_raw - type: sensor_msgs/msg/Image - afferent: ['image_output'] - image_output: - topic: /image_raw_filtered - type: sensor_msgs/msg/Image - meta: default_meta - coupling: default_coupling