From fe8901742d0b288713fd7066ca7e4f10d8e5f132 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 28 Oct 2024 09:18:45 +0100 Subject: [PATCH] Initial version MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .github/workflows/rolling.yaml | 45 +++ cs4home_core/CMakeLists.txt | 60 +++ .../include/cs4home_core/Afferent.hpp | 90 +++++ .../include/cs4home_core/CognitiveModule.hpp | 80 ++++ cs4home_core/include/cs4home_core/Core.hpp | 53 +++ .../include/cs4home_core/Coupling.hpp | 40 ++ .../include/cs4home_core/Efferent.hpp | 60 +++ cs4home_core/include/cs4home_core/Flow.hpp | 54 +++ cs4home_core/include/cs4home_core/Master.hpp | 60 +++ cs4home_core/include/cs4home_core/Meta.hpp | 40 ++ cs4home_core/include/cs4home_core/macros.hpp | 35 ++ cs4home_core/launch/launch_simple.launch.py | 53 +++ cs4home_core/package.xml | 23 ++ cs4home_core/src/cs4home_core/Afferent.cpp | 77 ++++ .../src/cs4home_core/CognitiveModule.cpp | 160 ++++++++ cs4home_core/src/cs4home_core/Core.cpp | 31 ++ cs4home_core/src/cs4home_core/Coupling.cpp | 31 ++ cs4home_core/src/cs4home_core/Efferent.cpp | 36 ++ cs4home_core/src/cs4home_core/Flow.cpp | 43 ++ cs4home_core/src/cs4home_core/Master.cpp | 77 ++++ cs4home_core/src/cs4home_core/Meta.cpp | 31 ++ cs4home_core/test/CMakeLists.txt | 60 +++ cs4home_core/test/DefaultCoupling.cpp | 41 ++ cs4home_core/test/DefaultMeta.cpp | 41 ++ cs4home_core/test/ImageFilter.cpp | 78 ++++ cs4home_core/test/ImageFilterCB.cpp | 74 ++++ cs4home_core/test/SimpleImageInput.cpp | 67 ++++ cs4home_core/test/SimpleImageOutput.cpp | 66 +++ cs4home_core/test/cognitive_module_test.cpp | 376 ++++++++++++++++++ .../test/config/startup_simple_1.yaml | 11 + cs4home_core/test/master_test.cpp | 39 ++ cs4home_core/test/params_simple.yaml | 13 + 32 files changed, 2045 insertions(+) create mode 100644 .github/workflows/rolling.yaml create mode 100644 cs4home_core/CMakeLists.txt create mode 100644 cs4home_core/include/cs4home_core/Afferent.hpp create mode 100644 cs4home_core/include/cs4home_core/CognitiveModule.hpp create mode 100644 cs4home_core/include/cs4home_core/Core.hpp create mode 100644 cs4home_core/include/cs4home_core/Coupling.hpp create mode 100644 cs4home_core/include/cs4home_core/Efferent.hpp create mode 100644 cs4home_core/include/cs4home_core/Flow.hpp create mode 100644 cs4home_core/include/cs4home_core/Master.hpp create mode 100644 cs4home_core/include/cs4home_core/Meta.hpp create mode 100644 cs4home_core/include/cs4home_core/macros.hpp create mode 100644 cs4home_core/launch/launch_simple.launch.py create mode 100644 cs4home_core/package.xml create mode 100644 cs4home_core/src/cs4home_core/Afferent.cpp create mode 100644 cs4home_core/src/cs4home_core/CognitiveModule.cpp create mode 100644 cs4home_core/src/cs4home_core/Core.cpp create mode 100644 cs4home_core/src/cs4home_core/Coupling.cpp create mode 100644 cs4home_core/src/cs4home_core/Efferent.cpp create mode 100644 cs4home_core/src/cs4home_core/Flow.cpp create mode 100644 cs4home_core/src/cs4home_core/Master.cpp create mode 100644 cs4home_core/src/cs4home_core/Meta.cpp create mode 100644 cs4home_core/test/CMakeLists.txt create mode 100644 cs4home_core/test/DefaultCoupling.cpp create mode 100644 cs4home_core/test/DefaultMeta.cpp create mode 100644 cs4home_core/test/ImageFilter.cpp create mode 100644 cs4home_core/test/ImageFilterCB.cpp create mode 100644 cs4home_core/test/SimpleImageInput.cpp create mode 100644 cs4home_core/test/SimpleImageOutput.cpp create mode 100644 cs4home_core/test/cognitive_module_test.cpp create mode 100644 cs4home_core/test/config/startup_simple_1.yaml create mode 100644 cs4home_core/test/master_test.cpp create mode 100644 cs4home_core/test/params_simple.yaml diff --git a/.github/workflows/rolling.yaml b/.github/workflows/rolling.yaml new file mode 100644 index 0000000..d3cbb34 --- /dev/null +++ b/.github/workflows/rolling.yaml @@ -0,0 +1,45 @@ +name: rolling + +on: + pull_request: + branches: + - rolling + push: + branches: + - rolling + schedule: + - cron: '0 0 * * 6' +jobs: + build-and-test: + runs-on: ${{ matrix.os }} + strategy: + matrix: + os: [ubuntu-22.04] + fail-fast: false + steps: + - uses: actions/checkout@v2 + - name: Setup ROS 2 + uses: ros-tooling/setup-ros@0.7.1 + with: + required-ros-distributions: rolling + - name: build and test + uses: ros-tooling/action-ros-ci@0.3.6 + with: + package-name: cs4home_core + target-ros2-distro: rolling + colcon-defaults: | + { + "test": { + "parallel-workers" : 1 + } + } + colcon-mixin-name: coverage-gcc + colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml + - name: Codecov + uses: codecov/codecov-action@v1.2.1 + with: + file: ros_ws/lcov/total_coverage.info + flags: unittests + name: codecov-umbrella + # yml: ./codecov.yml + fail_ci_if_error: false diff --git a/cs4home_core/CMakeLists.txt b/cs4home_core/CMakeLists.txt new file mode 100644 index 0000000..774d5a3 --- /dev/null +++ b/cs4home_core/CMakeLists.txt @@ -0,0 +1,60 @@ +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/Flow.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 new file mode 100644 index 0000000..4c05712 --- /dev/null +++ b/cs4home_core/include/cs4home_core/Afferent.hpp @@ -0,0 +1,90 @@ +// 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 new file mode 100644 index 0000000..c54173d --- /dev/null +++ b/cs4home_core/include/cs4home_core/CognitiveModule.hpp @@ -0,0 +1,80 @@ +// 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 new file mode 100644 index 0000000..3e23af1 --- /dev/null +++ b/cs4home_core/include/cs4home_core/Core.hpp @@ -0,0 +1,53 @@ +// 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 new file mode 100644 index 0000000..d6849ce --- /dev/null +++ b/cs4home_core/include/cs4home_core/Coupling.hpp @@ -0,0 +1,40 @@ +// 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 new file mode 100644 index 0000000..a8a966a --- /dev/null +++ b/cs4home_core/include/cs4home_core/Efferent.hpp @@ -0,0 +1,60 @@ +// 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/Flow.hpp b/cs4home_core/include/cs4home_core/Flow.hpp new file mode 100644 index 0000000..54013c4 --- /dev/null +++ b/cs4home_core/include/cs4home_core/Flow.hpp @@ -0,0 +1,54 @@ +// 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__FLOW_HPP_ +#define CS4HOME_CORE__FLOW_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/macros.hpp" + +namespace cs4home_core +{ + +class Flow +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(Flow) + + explicit Flow(const std::vector & nodes); + + void print() const; + const std::vector & get_nodes() const {return nodes_;} + +private: + std::vector nodes_; +}; + + +} // namespace cs4home_core + +#endif // CS4HOME_CORE__FLOW_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::Flow) diff --git a/cs4home_core/include/cs4home_core/Master.hpp b/cs4home_core/include/cs4home_core/Master.hpp new file mode 100644 index 0000000..ef82b5d --- /dev/null +++ b/cs4home_core/include/cs4home_core/Master.hpp @@ -0,0 +1,60 @@ +// 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 new file mode 100644 index 0000000..4267dd1 --- /dev/null +++ b/cs4home_core/include/cs4home_core/Meta.hpp @@ -0,0 +1,40 @@ +// 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 new file mode 100644 index 0000000..8e54e19 --- /dev/null +++ b/cs4home_core/include/cs4home_core/macros.hpp @@ -0,0 +1,35 @@ +// 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 new file mode 100644 index 0000000..bf1b282 --- /dev/null +++ b/cs4home_core/launch/launch_simple.launch.py @@ -0,0 +1,53 @@ +# 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 new file mode 100644 index 0000000..bc5d309 --- /dev/null +++ b/cs4home_core/package.xml @@ -0,0 +1,23 @@ + + + + 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 new file mode 100644 index 0000000..45e4006 --- /dev/null +++ b/cs4home_core/src/cs4home_core/Afferent.cpp @@ -0,0 +1,77 @@ +// 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 new file mode 100644 index 0000000..21d7306 --- /dev/null +++ b/cs4home_core/src/cs4home_core/CognitiveModule.cpp @@ -0,0 +1,160 @@ +// 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 new file mode 100644 index 0000000..fd9e0a9 --- /dev/null +++ b/cs4home_core/src/cs4home_core/Core.cpp @@ -0,0 +1,31 @@ +// 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 new file mode 100644 index 0000000..09309fd --- /dev/null +++ b/cs4home_core/src/cs4home_core/Coupling.cpp @@ -0,0 +1,31 @@ +// 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 new file mode 100644 index 0000000..72ce0b0 --- /dev/null +++ b/cs4home_core/src/cs4home_core/Efferent.cpp @@ -0,0 +1,36 @@ +// 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/Flow.cpp b/cs4home_core/src/cs4home_core/Flow.cpp new file mode 100644 index 0000000..d044d82 --- /dev/null +++ b/cs4home_core/src/cs4home_core/Flow.cpp @@ -0,0 +1,43 @@ +// 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 +#include +#include + +#include "cs4home_core/Flow.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/macros.hpp" + +namespace cs4home_core +{ + +Flow::Flow(const std::vector & nodes) +: nodes_(nodes) +{ +} + +void +Flow::print() const +{ + for (const auto & node : nodes_) { + std::cout << " -> " << node; + } + std::cout << std::endl; +} + +} // namespace cs4home_core diff --git a/cs4home_core/src/cs4home_core/Master.cpp b/cs4home_core/src/cs4home_core/Master.cpp new file mode 100644 index 0000000..20cdcbb --- /dev/null +++ b/cs4home_core/src/cs4home_core/Master.cpp @@ -0,0 +1,77 @@ +// 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 new file mode 100644 index 0000000..e0db705 --- /dev/null +++ b/cs4home_core/src/cs4home_core/Meta.cpp @@ -0,0 +1,31 @@ +// 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 new file mode 100644 index 0000000..bdc9f58 --- /dev/null +++ b/cs4home_core/test/CMakeLists.txt @@ -0,0 +1,60 @@ +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}) + +ament_add_gtest(master_test master_test.cpp) +target_link_libraries(master_test ${PROJECT_NAME}) +ament_target_dependencies(master_test ${test_dependencies}) + + +install(DIRECTORY + config + DESTINATION share/${PROJECT_NAME} +) diff --git a/cs4home_core/test/DefaultCoupling.cpp b/cs4home_core/test/DefaultCoupling.cpp new file mode 100644 index 0000000..8576aec --- /dev/null +++ b/cs4home_core/test/DefaultCoupling.cpp @@ -0,0 +1,41 @@ +// 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 new file mode 100644 index 0000000..7db1663 --- /dev/null +++ b/cs4home_core/test/DefaultMeta.cpp @@ -0,0 +1,41 @@ +// 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 new file mode 100644 index 0000000..4d80ba9 --- /dev/null +++ b/cs4home_core/test/ImageFilter.cpp @@ -0,0 +1,78 @@ +// 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 new file mode 100644 index 0000000..20f0451 --- /dev/null +++ b/cs4home_core/test/ImageFilterCB.cpp @@ -0,0 +1,74 @@ +// 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 new file mode 100644 index 0000000..80bec94 --- /dev/null +++ b/cs4home_core/test/SimpleImageInput.cpp @@ -0,0 +1,67 @@ +// 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 new file mode 100644 index 0000000..bc6f89b --- /dev/null +++ b/cs4home_core/test/SimpleImageOutput.cpp @@ -0,0 +1,66 @@ +// 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 new file mode 100644 index 0000000..9c918f7 --- /dev/null +++ b/cs4home_core/test/cognitive_module_test.cpp @@ -0,0 +1,376 @@ +// 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 new file mode 100644 index 0000000..ff72d90 --- /dev/null +++ b/cs4home_core/test/config/startup_simple_1.yaml @@ -0,0 +1,11 @@ +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/master_test.cpp b/cs4home_core/test/master_test.cpp new file mode 100644 index 0000000..7b344a6 --- /dev/null +++ b/cs4home_core/test/master_test.cpp @@ -0,0 +1,39 @@ +// 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 "ament_index_cpp/get_package_share_directory.hpp" + +#include "cs4home_core/Flow.hpp" +#include "cs4home_core/CognitiveModule.hpp" +#include "gtest/gtest.h" + + +TEST(flow_test, flow_creation) +{ + Flow flow1({"A", "B", "C", "D"}); + Flow flow2({"A", "B", "C", "E"}); + + ASSERT_EQ(flow1.get_nodes(), std::vector({"A", "B", "C", "D"})); + ASSERT_EQ(flow2.get_nodes(), std::vector({"A", "B", "C", "E"})); +} + + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/cs4home_core/test/params_simple.yaml b/cs4home_core/test/params_simple.yaml new file mode 100644 index 0000000..1403a55 --- /dev/null +++ b/cs4home_core/test/params_simple.yaml @@ -0,0 +1,13 @@ +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