From 9db2bc87b3cf20e8a16ecf2e71f8ebbb8cfc07da Mon Sep 17 00:00:00 2001 From: Juancams Date: Tue, 28 Nov 2023 09:57:12 +0100 Subject: [PATCH 01/26] Interfaces package created --- .../CMakeLists.txt | 26 +++++++++++++++++++ .../package.xml | 18 +++++++++++++ 2 files changed, 44 insertions(+) create mode 100644 coresense_instrumentation_interfaces/CMakeLists.txt create mode 100644 coresense_instrumentation_interfaces/package.xml diff --git a/coresense_instrumentation_interfaces/CMakeLists.txt b/coresense_instrumentation_interfaces/CMakeLists.txt new file mode 100644 index 0000000..71acc28 --- /dev/null +++ b/coresense_instrumentation_interfaces/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.8) +project(coresense_instrumentation_interfaces) + +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) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/coresense_instrumentation_interfaces/package.xml b/coresense_instrumentation_interfaces/package.xml new file mode 100644 index 0000000..f79fca9 --- /dev/null +++ b/coresense_instrumentation_interfaces/package.xml @@ -0,0 +1,18 @@ + + + + coresense_instrumentation_interfaces + 0.0.0 + TODO: Package description + juanca + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + From 3438e6a4804536fb615418a00e111fa5d273196c Mon Sep 17 00:00:00 2001 From: Juancams Date: Tue, 28 Nov 2023 09:57:41 +0100 Subject: [PATCH 02/26] Driver package created and initialized --- .../CMakeLists.txt | 58 +++++++++++++++++++ .../CoresenseInstrumentationDriver.hpp | 37 ++++++++++++ .../InstrumentationLifecycleNode.hpp | 31 ++++++++++ .../InstrumentationNode.hpp | 30 ++++++++++ coresense_instrumentation_driver/package.xml | 22 +++++++ .../CoresenseInstrumentationDriver.cpp | 33 +++++++++++ .../coresense_instrumentation_driver_node.cpp | 28 +++++++++ 7 files changed, 239 insertions(+) create mode 100644 coresense_instrumentation_driver/CMakeLists.txt create mode 100644 coresense_instrumentation_driver/include/coresense_instrumentation_driver/CoresenseInstrumentationDriver.hpp create mode 100644 coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/InstrumentationLifecycleNode.hpp create mode 100644 coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/InstrumentationNode.hpp create mode 100644 coresense_instrumentation_driver/package.xml create mode 100644 coresense_instrumentation_driver/src/coresense_instrumentation_driver/CoresenseInstrumentationDriver.cpp create mode 100644 coresense_instrumentation_driver/src/coresense_instrumentation_driver_node.cpp diff --git a/coresense_instrumentation_driver/CMakeLists.txt b/coresense_instrumentation_driver/CMakeLists.txt new file mode 100644 index 0000000..c587b2b --- /dev/null +++ b/coresense_instrumentation_driver/CMakeLists.txt @@ -0,0 +1,58 @@ +cmake_minimum_required(VERSION 3.5) +project(coresense_instrumentation_driver) + +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(std_msgs REQUIRED) +find_package(rclcpp_components REQUIRED) + +set(dependencies + rclcpp + std_msgs + rclcpp_components +) + +include_directories(include) + +add_library(${PROJECT_NAME}_lib SHARED + src/coresense_instrumentation_driver/CoresenseInstrumentationDriver.cpp +) +ament_target_dependencies(${PROJECT_NAME}_lib ${dependencies}) + +add_executable(coresense_instrumentation_driver_node src/coresense_instrumentation_driver_node.cpp) +ament_target_dependencies(coresense_instrumentation_driver_node ${dependencies}) +target_link_libraries(coresense_instrumentation_driver_node ${PROJECT_NAME}_lib) + +rclcpp_components_register_nodes(${PROJECT_NAME}_lib + PLUGIN "node_handler::ExampleNode" +) + +install(TARGETS + ${PROJECT_NAME}_lib + coresense_instrumentation_driver_node + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME}_lib +) + +install(DIRECTORY include/ + DESTINATION include +) + +ament_export_include_directories(include) +ament_export_dependencies(${dependencies}) +ament_export_libraries(${PROJECT_NAME}_lib) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/CoresenseInstrumentationDriver.hpp b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/CoresenseInstrumentationDriver.hpp new file mode 100644 index 0000000..02348a5 --- /dev/null +++ b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/CoresenseInstrumentationDriver.hpp @@ -0,0 +1,37 @@ +// Copyright 2023 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 CORESENSE_INSTRUMENTATION_DRIVER_HPP +#define CORESENSE_INSTRUMENTATION_DRIVER_HPP + +#include "rclcpp/rclcpp.hpp" + +namespace coresense_instrumentation_driver +{ + + +class CoresenseInstrumentationDriver : public rclcpp::Node +{ +public: + explicit CoresenseInstrumentationDriver( + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + virtual ~CoresenseInstrumentationDriver(); + +private: + +}; + +} // namespace coresense_instrumentation_driver + +#endif // CORESENSE_INSTRUMENTATION_DRIVER_HPP diff --git a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/InstrumentationLifecycleNode.hpp b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/InstrumentationLifecycleNode.hpp new file mode 100644 index 0000000..69bef07 --- /dev/null +++ b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/InstrumentationLifecycleNode.hpp @@ -0,0 +1,31 @@ +// Copyright 2023 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 "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +namespace coresense_instrumentation_driver +{ + +class InstrumentationLifecycleNode : public rclcpp_lifecycle::LifecycleNode +{ +public: + InstrumentationLifecycleNode( + const std::string & node_name, + const std::string & ns = "", + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + virtual ~LifecycleNode(); +}; + +} // namespace coresense_instrumentation_driver diff --git a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/InstrumentationNode.hpp b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/InstrumentationNode.hpp new file mode 100644 index 0000000..eb155b1 --- /dev/null +++ b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/InstrumentationNode.hpp @@ -0,0 +1,30 @@ +// Copyright 2023 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 "rclcpp/rclcpp.hpp" + +namespace coresense_instrumentation_driver +{ + +class InstrumentationNode : public rclcpp::Node +{ +public: + InstrumentationNode( + const std::string & node_name, + const std::string & ns = "", + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + virtual ~InstrumentationNode(); +}; + +} // namespace coresense_instrumentation_driver diff --git a/coresense_instrumentation_driver/package.xml b/coresense_instrumentation_driver/package.xml new file mode 100644 index 0000000..a2ef534 --- /dev/null +++ b/coresense_instrumentation_driver/package.xml @@ -0,0 +1,22 @@ + + + + coresense_instrumentation_driver + 0.0.0 + Coresense Instrumentation Driver + juanca + Apache-2.0 + + ament_cmake + + rclcpp + std_msgs + rclcpp_components + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/coresense_instrumentation_driver/src/coresense_instrumentation_driver/CoresenseInstrumentationDriver.cpp b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/CoresenseInstrumentationDriver.cpp new file mode 100644 index 0000000..e8b1561 --- /dev/null +++ b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/CoresenseInstrumentationDriver.cpp @@ -0,0 +1,33 @@ +// Copyright 2023 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 "coresense_instrumentation_driver/CoresenseInstrumentationDriver.hpp" + +namespace coresense_instrumentation_driver +{ + +CoresenseInstrumentationDriver::CoresenseInstrumentationDriver( + const rclcpp::NodeOptions & options) +: Node("coresense_instrumentation_driver_node", options) +{ +} + +CoresenseInstrumentationDriver::~CoresenseInstrumentationDriver() +{ +} + +} // namespace coresense_instrumentation_driver + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(coresense_instrumentation_driver::CoresenseInstrumentationDriver) diff --git a/coresense_instrumentation_driver/src/coresense_instrumentation_driver_node.cpp b/coresense_instrumentation_driver/src/coresense_instrumentation_driver_node.cpp new file mode 100644 index 0000000..74c2a92 --- /dev/null +++ b/coresense_instrumentation_driver/src/coresense_instrumentation_driver_node.cpp @@ -0,0 +1,28 @@ +// Copyright 2023 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 "coresense_instrumentation_driver/CoresenseInstrumentationDriver.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + auto node = std::make_shared(); + + rclcpp::spin(node); + + rclcpp::shutdown(); + return 0; +} From 4fc31b59da843a8e7b63c6280a817e552db21edf Mon Sep 17 00:00:00 2001 From: Juancams Date: Tue, 28 Nov 2023 09:58:21 +0100 Subject: [PATCH 03/26] CI --- .github/workflows/main.yaml | 36 ++++++++++++++++++++++++++++++++++++ 1 file changed, 36 insertions(+) create mode 100644 .github/workflows/main.yaml diff --git a/.github/workflows/main.yaml b/.github/workflows/main.yaml new file mode 100644 index 0000000..35a3992 --- /dev/null +++ b/.github/workflows/main.yaml @@ -0,0 +1,36 @@ +name: main + +on: + push: + branches: + - main + +jobs: + ROS2: + runs-on: ubuntu-22.04 + steps: + - name: Checkout + uses: actions/checkout@v2 + + - name: Setup ROS 2 environment + uses: ros-tooling/setup-ros@0.7.0 + with: + ros-distro: humble + + - name: Build ROS 2 package + uses: ros-tooling/action-ros-ci@v0.3 + with: + package-name: coresense_instrumentation_driver coresense_instrumentation_interfaces + target-ros2-distro: humble + colcon-defaults: | + { + "build": { + "mixin": ["coverage-gcc"] + } + } + colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/1ddb69bedfd1f04c2f000e95452f7c24a4d6176b/index.yaml + + - name: Upload coverage reports to Codecov + uses: codecov/codecov-action@v3 + env: + CODECOV_TOKEN: ${{ secrets.CODECOV_TOKEN }} \ No newline at end of file From 708b39ee281b68de5d2713a220f27d3977932c18 Mon Sep 17 00:00:00 2001 From: Juan Carlos Manzanares Serrano Date: Tue, 28 Nov 2023 10:07:37 +0100 Subject: [PATCH 04/26] Update README.md --- README.md | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 2870452..762ced9 100644 --- a/README.md +++ b/README.md @@ -1 +1,4 @@ -# coresense_instrumentation \ No newline at end of file +# Coresense Instrumentation + +[![main](https://github.com/Juancams/coresense_instrumentation/actions/workflows/main.yaml/badge.svg?branch=main)](https://github.com/Juancams/coresense_instrumentation/actions/workflows/main.yaml) +[![codecov](https://codecov.io/gh/Juancams/coresense_instrumentation/graph/badge.svg?token=EvUIoImzzh)](https://codecov.io/gh/Juancams/coresense_instrumentation) From 7d2523476a6b81533c825a69580272b1202aaee6 Mon Sep 17 00:00:00 2001 From: Juancams Date: Tue, 28 Nov 2023 18:41:37 +0100 Subject: [PATCH 05/26] Driver working --- .../CMakeLists.txt | 29 +++++--- .../config/params.yaml | 4 ++ .../CoresenseInstrumentationDriver.hpp | 9 ++- .../instrumentation_utils/ICell.hpp | 15 ++++ .../InstrumentationLifecycleNode.hpp | 72 ++++++++++++++++++- ...coresense_instrumentation_driver.launch.py | 49 +++++++++++++ coresense_instrumentation_driver/package.xml | 2 +- .../CoresenseInstrumentationDriver.cpp | 34 +++++++++ 8 files changed, 197 insertions(+), 17 deletions(-) create mode 100644 coresense_instrumentation_driver/config/params.yaml create mode 100644 coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/ICell.hpp create mode 100644 coresense_instrumentation_driver/launch/coresense_instrumentation_driver.launch.py diff --git a/coresense_instrumentation_driver/CMakeLists.txt b/coresense_instrumentation_driver/CMakeLists.txt index c587b2b..514449a 100644 --- a/coresense_instrumentation_driver/CMakeLists.txt +++ b/coresense_instrumentation_driver/CMakeLists.txt @@ -8,45 +8,52 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) -find_package(std_msgs REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) find_package(rclcpp_components REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) set(dependencies rclcpp - std_msgs rclcpp_components + rclcpp_lifecycle + std_msgs + sensor_msgs ) include_directories(include) -add_library(${PROJECT_NAME}_lib SHARED +add_library(${PROJECT_NAME} SHARED src/coresense_instrumentation_driver/CoresenseInstrumentationDriver.cpp ) -ament_target_dependencies(${PROJECT_NAME}_lib ${dependencies}) +ament_target_dependencies(${PROJECT_NAME} ${dependencies}) add_executable(coresense_instrumentation_driver_node src/coresense_instrumentation_driver_node.cpp) ament_target_dependencies(coresense_instrumentation_driver_node ${dependencies}) -target_link_libraries(coresense_instrumentation_driver_node ${PROJECT_NAME}_lib) +target_link_libraries(coresense_instrumentation_driver_node ${PROJECT_NAME}) -rclcpp_components_register_nodes(${PROJECT_NAME}_lib - PLUGIN "node_handler::ExampleNode" +rclcpp_components_register_nodes(${PROJECT_NAME} + PLUGIN "coresense_instrumentation_driver::CoresenseInstrumentationDriver" + EXECUTABLE coresense_instrumentation_driver_node ) install(TARGETS - ${PROJECT_NAME}_lib + ${PROJECT_NAME} coresense_instrumentation_driver_node ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME}_lib + RUNTIME DESTINATION lib/${PROJECT_NAME} ) install(DIRECTORY include/ DESTINATION include ) +install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}) + ament_export_include_directories(include) ament_export_dependencies(${dependencies}) -ament_export_libraries(${PROJECT_NAME}_lib) +ament_export_libraries(${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -55,4 +62,4 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() -ament_package() +ament_package() \ No newline at end of file diff --git a/coresense_instrumentation_driver/config/params.yaml b/coresense_instrumentation_driver/config/params.yaml new file mode 100644 index 0000000..e8dc2b4 --- /dev/null +++ b/coresense_instrumentation_driver/config/params.yaml @@ -0,0 +1,4 @@ +coresense_instrumentation_driver_node: + ros__parameters: + topics: ["scan", "image"] + topic_types: [sensor_msgs/msg/LaserScan, sensor_msgs/msg/Image] \ No newline at end of file diff --git a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/CoresenseInstrumentationDriver.hpp b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/CoresenseInstrumentationDriver.hpp index 02348a5..45666dd 100644 --- a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/CoresenseInstrumentationDriver.hpp +++ b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/CoresenseInstrumentationDriver.hpp @@ -16,11 +16,12 @@ #define CORESENSE_INSTRUMENTATION_DRIVER_HPP #include "rclcpp/rclcpp.hpp" +#include "coresense_instrumentation_driver/instrumentation_utils/InstrumentationLifecycleNode.hpp" + namespace coresense_instrumentation_driver { - class CoresenseInstrumentationDriver : public rclcpp::Node { public: @@ -29,7 +30,11 @@ class CoresenseInstrumentationDriver : public rclcpp::Node virtual ~CoresenseInstrumentationDriver(); private: - + rclcpp::executors::MultiThreadedExecutor executor_; + std::vector> nodes_; + std::vector topics_; + std::vector topic_types_; + std::unique_ptr thread_; }; } // namespace coresense_instrumentation_driver diff --git a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/ICell.hpp b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/ICell.hpp new file mode 100644 index 0000000..27773f1 --- /dev/null +++ b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/ICell.hpp @@ -0,0 +1,15 @@ +#ifndef ICELL_HPP +#define ICELL_HPP + +// Esta es una clase base abstracta que define la interfaz común de los nodos de instrumentación +class ICell +{ +public: + // Este es un método virtual puro que debe ser implementado por las clases derivadas + virtual void process() = 0; + + // Este es un destructor virtual para permitir el polimorfismo + virtual ~ICell() {} +}; + +#endif // ICELL_HPP diff --git a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/InstrumentationLifecycleNode.hpp b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/InstrumentationLifecycleNode.hpp index 69bef07..6e49c7c 100644 --- a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/InstrumentationLifecycleNode.hpp +++ b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/InstrumentationLifecycleNode.hpp @@ -12,20 +12,86 @@ // See the License for the specific language governing permissions and // limitations under the License. +#ifndef INSTRUMENTATION_LIFECYCLE_NODE_HPP +#define INSTRUMENTATION_LIFECYCLE_NODE_HPP + #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "coresense_instrumentation_driver/instrumentation_utils/ICell.hpp" +#include "std_msgs/msg/string.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" namespace coresense_instrumentation_driver { -class InstrumentationLifecycleNode : public rclcpp_lifecycle::LifecycleNode +template +class InstrumentationLifecycleNode : public rclcpp_lifecycle::LifecycleNode, public ICell { public: InstrumentationLifecycleNode( const std::string & node_name, + const std::string & topic, const std::string & ns = "", - const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); - virtual ~LifecycleNode(); + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : rclcpp_lifecycle::LifecycleNode(node_name, ns, options) + { + sub_ = this->create_subscription( + topic, 10, + [this](const typename TopicT::SharedPtr msg) { + if (pub_) { + + pub_->publish(std::make_unique(*msg)); + } + }); + + pub_ = this->create_publisher(ns + topic, 10); + + RCLCPP_INFO(get_logger(), "Creating InstrumentationLifecycleNode"); + } + + virtual ~InstrumentationLifecycleNode() { + RCLCPP_INFO(get_logger(), "Destroying InstrumentationLifecycleNode"); + } + +private: + using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + + CallbackReturnT + on_configure(const rclcpp_lifecycle::State &) override + { + return CallbackReturnT::SUCCESS; + } + + CallbackReturnT + on_activate(const rclcpp_lifecycle::State &) override + { + return CallbackReturnT::SUCCESS; + } + + CallbackReturnT + on_deactivate(const rclcpp_lifecycle::State &) override + { + return CallbackReturnT::SUCCESS; + } + + CallbackReturnT + on_cleanup(const rclcpp_lifecycle::State &) override + { + return CallbackReturnT::SUCCESS; + } + + CallbackReturnT + on_shutdown(const rclcpp_lifecycle::State &) override + { + return CallbackReturnT::SUCCESS; + } + + void process() override { } + + typename rclcpp::Subscription::SharedPtr sub_; + typename rclcpp::Publisher::SharedPtr pub_; }; } // namespace coresense_instrumentation_driver + +#endif // INSTRUMENTATION_LIFECYCLE_NODE_HPP \ No newline at end of file diff --git a/coresense_instrumentation_driver/launch/coresense_instrumentation_driver.launch.py b/coresense_instrumentation_driver/launch/coresense_instrumentation_driver.launch.py new file mode 100644 index 0000000..dc050e7 --- /dev/null +++ b/coresense_instrumentation_driver/launch/coresense_instrumentation_driver.launch.py @@ -0,0 +1,49 @@ +# Copyright 2023 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_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + + pkg_dir = get_package_share_directory('coresense_instrumentation_driver') + param_file = os.path.join(pkg_dir, 'config', 'params.yaml') + + coresense_node = ComposableNode( + package='coresense_instrumentation_driver', + plugin='coresense_instrumentation_driver::CoresenseInstrumentationDriver', + name='coresense_instrumentation_driver_node', + namespace='coresense', + parameters=[{"topics": ["/scan", "/chatter"], + "topic_types": ["sensor_msgs/msg/LaserScan", "std_msgs/msg/String"]}], + ) + + container = ComposableNodeContainer( + name='coresense_container', + namespace='coresense', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[coresense_node], + output='screen', + ) + + ld = LaunchDescription() + ld.add_action(container) + + return ld diff --git a/coresense_instrumentation_driver/package.xml b/coresense_instrumentation_driver/package.xml index a2ef534..6de6686 100644 --- a/coresense_instrumentation_driver/package.xml +++ b/coresense_instrumentation_driver/package.xml @@ -4,7 +4,7 @@ coresense_instrumentation_driver 0.0.0 Coresense Instrumentation Driver - juanca + Juan Carlos Manzanares Serrano Apache-2.0 ament_cmake diff --git a/coresense_instrumentation_driver/src/coresense_instrumentation_driver/CoresenseInstrumentationDriver.cpp b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/CoresenseInstrumentationDriver.cpp index e8b1561..20db0bf 100644 --- a/coresense_instrumentation_driver/src/coresense_instrumentation_driver/CoresenseInstrumentationDriver.cpp +++ b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/CoresenseInstrumentationDriver.cpp @@ -21,10 +21,44 @@ CoresenseInstrumentationDriver::CoresenseInstrumentationDriver( const rclcpp::NodeOptions & options) : Node("coresense_instrumentation_driver_node", options) { + declare_parameter("topics", std::vector()); + declare_parameter("topic_types", std::vector()); + + get_parameter("topics", topics_); + get_parameter("topic_types", topic_types_); + + for (size_t i = 0; i < topics_.size(); i++) { + + if (topic_types_[i] == "std_msgs/msg/String") { + auto node = std::make_shared>( + "instrumentation_" + std::to_string(i), topics_[i], get_namespace()); + + nodes_.push_back(node); + + executor_.add_node(node->get_node_base_interface()); + } else if (topic_types_[i] == "sensor_msgs/msg/LaserScan") { + auto node = std::make_shared>( + "instrumentation_" + std::to_string(i), topics_[i], get_namespace()); + + nodes_.push_back(node); + + executor_.add_node(node->get_node_base_interface()); + } else { + RCLCPP_ERROR(get_logger(), "Unknown topic type '%s'", topic_types_[i].c_str()); + exit(-1); + } + } + + thread_ = std::make_unique([&]() {executor_.spin();}); } CoresenseInstrumentationDriver::~CoresenseInstrumentationDriver() { + if (thread_ && thread_->joinable()) { + thread_->join(); + } + + RCLCPP_INFO(get_logger(), "Destroying CoresenseInstrumentationDriver"); } } // namespace coresense_instrumentation_driver From ec498c1e752bab8a4ebcc57bf581e31ff5e31896 Mon Sep 17 00:00:00 2001 From: Juancams Date: Tue, 28 Nov 2023 18:43:13 +0100 Subject: [PATCH 06/26] Deleted IntrumentationNode --- .../InstrumentationNode.hpp | 30 ------------------- 1 file changed, 30 deletions(-) delete mode 100644 coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/InstrumentationNode.hpp diff --git a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/InstrumentationNode.hpp b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/InstrumentationNode.hpp deleted file mode 100644 index eb155b1..0000000 --- a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/InstrumentationNode.hpp +++ /dev/null @@ -1,30 +0,0 @@ -// Copyright 2023 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 "rclcpp/rclcpp.hpp" - -namespace coresense_instrumentation_driver -{ - -class InstrumentationNode : public rclcpp::Node -{ -public: - InstrumentationNode( - const std::string & node_name, - const std::string & ns = "", - const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); - virtual ~InstrumentationNode(); -}; - -} // namespace coresense_instrumentation_driver From f4ffe9ed0b84e4230b68c8c2135589984ea27604 Mon Sep 17 00:00:00 2001 From: Juancams Date: Tue, 28 Nov 2023 19:55:05 +0100 Subject: [PATCH 07/26] Lint errors --- .../InstrumentationLifecycleNode.hpp | 39 ++++++++++--------- 1 file changed, 20 insertions(+), 19 deletions(-) diff --git a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/InstrumentationLifecycleNode.hpp b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/InstrumentationLifecycleNode.hpp index 6e49c7c..9dfc99e 100644 --- a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/InstrumentationLifecycleNode.hpp +++ b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/InstrumentationLifecycleNode.hpp @@ -33,23 +33,24 @@ class InstrumentationLifecycleNode : public rclcpp_lifecycle::LifecycleNode, pub const std::string & topic, const std::string & ns = "", const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) - : rclcpp_lifecycle::LifecycleNode(node_name, ns, options) - { - sub_ = this->create_subscription( - topic, 10, - [this](const typename TopicT::SharedPtr msg) { - if (pub_) { - - pub_->publish(std::make_unique(*msg)); - } - }); - - pub_ = this->create_publisher(ns + topic, 10); - - RCLCPP_INFO(get_logger(), "Creating InstrumentationLifecycleNode"); - } - - virtual ~InstrumentationLifecycleNode() { + : rclcpp_lifecycle::LifecycleNode(node_name, ns, options) + { + sub_ = this->create_subscription( + topic, 10, + [this](const typename TopicT::SharedPtr msg) { + if (pub_) { + + pub_->publish(std::make_unique(*msg)); + } + }); + + pub_ = this->create_publisher(ns + topic, 10); + + RCLCPP_INFO(get_logger(), "Creating InstrumentationLifecycleNode"); + } + + virtual ~InstrumentationLifecycleNode() + { RCLCPP_INFO(get_logger(), "Destroying InstrumentationLifecycleNode"); } @@ -86,7 +87,7 @@ class InstrumentationLifecycleNode : public rclcpp_lifecycle::LifecycleNode, pub return CallbackReturnT::SUCCESS; } - void process() override { } + void process() override {} typename rclcpp::Subscription::SharedPtr sub_; typename rclcpp::Publisher::SharedPtr pub_; @@ -94,4 +95,4 @@ class InstrumentationLifecycleNode : public rclcpp_lifecycle::LifecycleNode, pub } // namespace coresense_instrumentation_driver -#endif // INSTRUMENTATION_LIFECYCLE_NODE_HPP \ No newline at end of file +#endif // INSTRUMENTATION_LIFECYCLE_NODE_HPP From 168d5d387f04c59e0a2556bfc4df4047b5144e5a Mon Sep 17 00:00:00 2001 From: Juancams Date: Tue, 28 Nov 2023 19:55:20 +0100 Subject: [PATCH 08/26] Flake8 errors --- .../launch/coresense_instrumentation_driver.launch.py | 6 ------ 1 file changed, 6 deletions(-) diff --git a/coresense_instrumentation_driver/launch/coresense_instrumentation_driver.launch.py b/coresense_instrumentation_driver/launch/coresense_instrumentation_driver.launch.py index dc050e7..fcaa806 100644 --- a/coresense_instrumentation_driver/launch/coresense_instrumentation_driver.launch.py +++ b/coresense_instrumentation_driver/launch/coresense_instrumentation_driver.launch.py @@ -12,9 +12,6 @@ # 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_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode @@ -22,9 +19,6 @@ def generate_launch_description(): - pkg_dir = get_package_share_directory('coresense_instrumentation_driver') - param_file = os.path.join(pkg_dir, 'config', 'params.yaml') - coresense_node = ComposableNode( package='coresense_instrumentation_driver', plugin='coresense_instrumentation_driver::CoresenseInstrumentationDriver', From c11d2736776470cc7efad605723c65c6476fabbd Mon Sep 17 00:00:00 2001 From: Juancams Date: Tue, 28 Nov 2023 20:06:54 +0100 Subject: [PATCH 09/26] Update package.xml --- coresense_instrumentation_driver/package.xml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/coresense_instrumentation_driver/package.xml b/coresense_instrumentation_driver/package.xml index 6de6686..c3c99d7 100644 --- a/coresense_instrumentation_driver/package.xml +++ b/coresense_instrumentation_driver/package.xml @@ -10,8 +10,10 @@ ament_cmake rclcpp - std_msgs + rclcpp_lifecycle rclcpp_components + std_msgs + sensor_msgs ament_lint_auto ament_lint_common From 657a3d2c61692543ad0348e09cbf4217a45ce359 Mon Sep 17 00:00:00 2001 From: Juancams Date: Wed, 29 Nov 2023 00:40:44 +0100 Subject: [PATCH 10/26] Added new features and reformatted Signed-off-by: Juancams --- .../CoresenseInstrumentationDriver.hpp | 7 ++ .../instrumentation_utils/ICell.hpp | 39 ++++++++ .../InstrumentationLifecycleNode.hpp | 20 +++- .../CoresenseInstrumentationDriver.cpp | 91 +++++++++++++++---- 4 files changed, 137 insertions(+), 20 deletions(-) diff --git a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/CoresenseInstrumentationDriver.hpp b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/CoresenseInstrumentationDriver.hpp index 45666dd..881ed0b 100644 --- a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/CoresenseInstrumentationDriver.hpp +++ b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/CoresenseInstrumentationDriver.hpp @@ -29,7 +29,14 @@ class CoresenseInstrumentationDriver : public rclcpp::Node const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); virtual ~CoresenseInstrumentationDriver(); + int getNumberOfNodes(); + void newNode(const std::string & topic, const std::string & topic_type); + void removeNode(const std::string & topic); + private: + void initializeNodes(); + void addNode(const std::string & topic, const std::string & topic_type); + rclcpp::executors::MultiThreadedExecutor executor_; std::vector> nodes_; std::vector topics_; diff --git a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/ICell.hpp b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/ICell.hpp index 27773f1..f9ddfe4 100644 --- a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/ICell.hpp +++ b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/ICell.hpp @@ -1,6 +1,8 @@ #ifndef ICELL_HPP #define ICELL_HPP +#include "rclcpp/rclcpp.hpp" + // Esta es una clase base abstracta que define la interfaz común de los nodos de instrumentación class ICell { @@ -10,6 +12,43 @@ class ICell // Este es un destructor virtual para permitir el polimorfismo virtual ~ICell() {} + + void set_name(const std::string & name) + { + name_ = name; + } + + void set_topic(const std::string & topic) + { + topic_ = topic; + } + + void set_node_base_interface( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface) + { + node_base_interface_ = node_base_interface; + } + + std::string get_name() + { + return name_; + } + + std::string get_topic() + { + return topic_; + } + + + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_base_interface() + { + return node_base_interface_; + } + +private: + std::string name_; + std::string topic_; + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface_; }; #endif // ICELL_HPP diff --git a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/InstrumentationLifecycleNode.hpp b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/InstrumentationLifecycleNode.hpp index 9dfc99e..29d5d45 100644 --- a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/InstrumentationLifecycleNode.hpp +++ b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/InstrumentationLifecycleNode.hpp @@ -35,8 +35,13 @@ class InstrumentationLifecycleNode : public rclcpp_lifecycle::LifecycleNode, pub const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) : rclcpp_lifecycle::LifecycleNode(node_name, ns, options) { + set_name(node_name); + set_topic(topic); + set_node_base_interface(this->get_node_base_interface()); + topic_ = topic; + sub_ = this->create_subscription( - topic, 10, + topic_, 10, [this](const typename TopicT::SharedPtr msg) { if (pub_) { @@ -44,7 +49,7 @@ class InstrumentationLifecycleNode : public rclcpp_lifecycle::LifecycleNode, pub } }); - pub_ = this->create_publisher(ns + topic, 10); + pub_ = this->create_publisher(ns + topic_, 10); RCLCPP_INFO(get_logger(), "Creating InstrumentationLifecycleNode"); } @@ -54,6 +59,16 @@ class InstrumentationLifecycleNode : public rclcpp_lifecycle::LifecycleNode, pub RCLCPP_INFO(get_logger(), "Destroying InstrumentationLifecycleNode"); } + std::string get_name() + { + return this->get_name(); + } + + std::string get_topic() + { + return topic_; + } + private: using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; @@ -91,6 +106,7 @@ class InstrumentationLifecycleNode : public rclcpp_lifecycle::LifecycleNode, pub typename rclcpp::Subscription::SharedPtr sub_; typename rclcpp::Publisher::SharedPtr pub_; + std::string topic_; }; } // namespace coresense_instrumentation_driver diff --git a/coresense_instrumentation_driver/src/coresense_instrumentation_driver/CoresenseInstrumentationDriver.cpp b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/CoresenseInstrumentationDriver.cpp index 20db0bf..5b04289 100644 --- a/coresense_instrumentation_driver/src/coresense_instrumentation_driver/CoresenseInstrumentationDriver.cpp +++ b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/CoresenseInstrumentationDriver.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include "coresense_instrumentation_driver/CoresenseInstrumentationDriver.hpp" +#include namespace coresense_instrumentation_driver { @@ -27,38 +28,92 @@ CoresenseInstrumentationDriver::CoresenseInstrumentationDriver( get_parameter("topics", topics_); get_parameter("topic_types", topic_types_); + initializeNodes(); +} + +CoresenseInstrumentationDriver::~CoresenseInstrumentationDriver() +{ + executor_.cancel(); + if (thread_ && thread_->joinable()) { + thread_->join(); + } + + RCLCPP_INFO(get_logger(), "Destroying CoresenseInstrumentationDriver"); +} + +void +CoresenseInstrumentationDriver::initializeNodes() +{ for (size_t i = 0; i < topics_.size(); i++) { + addNode(topics_[i], topic_types_[i]); + } - if (topic_types_[i] == "std_msgs/msg/String") { - auto node = std::make_shared>( - "instrumentation_" + std::to_string(i), topics_[i], get_namespace()); + thread_ = std::make_unique([&]() {executor_.spin();}); +} - nodes_.push_back(node); +void +CoresenseInstrumentationDriver::addNode( + const std::string & new_topic, + const std::string & new_topic_type) +{ + if (new_topic_type == "std_msgs/msg/String") { + auto node = std::make_shared>( + "instrumentation_dynamic", new_topic, get_namespace()); - executor_.add_node(node->get_node_base_interface()); - } else if (topic_types_[i] == "sensor_msgs/msg/LaserScan") { - auto node = std::make_shared>( - "instrumentation_" + std::to_string(i), topics_[i], get_namespace()); + nodes_.push_back(node); - nodes_.push_back(node); + executor_.add_node(node->get_node_base_interface()); + } else if (new_topic_type == "sensor_msgs/msg/LaserScan") { + auto node = std::make_shared>( + "instrumentation_dynamic", new_topic, get_namespace()); - executor_.add_node(node->get_node_base_interface()); - } else { - RCLCPP_ERROR(get_logger(), "Unknown topic type '%s'", topic_types_[i].c_str()); - exit(-1); - } + nodes_.push_back(node); + + executor_.add_node(node->get_node_base_interface()); + } else { + RCLCPP_ERROR(get_logger(), "Unknown topic type '%s'", new_topic_type.c_str()); } +} + +void +CoresenseInstrumentationDriver::newNode( + const std::string & new_topic, + const std::string & new_topic_type) +{ + if (executor_.get_number_of_threads() > 0) { + executor_.cancel(); + thread_->join(); + } + + addNode(new_topic, new_topic_type); thread_ = std::make_unique([&]() {executor_.spin();}); } -CoresenseInstrumentationDriver::~CoresenseInstrumentationDriver() +void CoresenseInstrumentationDriver::removeNode(const std::string & topic) { - if (thread_ && thread_->joinable()) { - thread_->join(); + auto it = std::find_if( + nodes_.begin(), nodes_.end(), + [&](const auto & node) {return node->get_topic() == topic;}); + + if (it != nodes_.end()) { + if (executor_.get_number_of_threads() > 0) { + executor_.cancel(); + thread_->join(); + } + + executor_.remove_node(it->get()->get_base_interface()); + nodes_.erase(it); + + thread_ = std::make_unique([&]() {executor_.spin();}); + } else { + RCLCPP_WARN(get_logger(), "Node for topic '%s' not found", topic.c_str()); } +} - RCLCPP_INFO(get_logger(), "Destroying CoresenseInstrumentationDriver"); +int CoresenseInstrumentationDriver::getNumberOfNodes() +{ + return nodes_.size(); } } // namespace coresense_instrumentation_driver From 1d5cd0aa3643aba2e3ca6a99cd874dbba4b2889c Mon Sep 17 00:00:00 2001 From: Juancams Date: Wed, 29 Nov 2023 00:41:05 +0100 Subject: [PATCH 11/26] Added test --- .../CMakeLists.txt | 2 + .../test/CMakeLists.txt | 3 + .../test_coresense_instrumentation_driver.cpp | 60 +++++++++++++++++++ 3 files changed, 65 insertions(+) create mode 100644 coresense_instrumentation_driver/test/CMakeLists.txt create mode 100644 coresense_instrumentation_driver/test/test_coresense_instrumentation_driver.cpp diff --git a/coresense_instrumentation_driver/CMakeLists.txt b/coresense_instrumentation_driver/CMakeLists.txt index 514449a..ba618ed 100644 --- a/coresense_instrumentation_driver/CMakeLists.txt +++ b/coresense_instrumentation_driver/CMakeLists.txt @@ -59,7 +59,9 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) set(ament_cmake_copyright_FOUND TRUE) set(ament_cmake_cpplint_FOUND TRUE) + find_package(ament_cmake_gtest REQUIRED) ament_lint_auto_find_test_dependencies() + add_subdirectory(test) endif() ament_package() \ No newline at end of file diff --git a/coresense_instrumentation_driver/test/CMakeLists.txt b/coresense_instrumentation_driver/test/CMakeLists.txt new file mode 100644 index 0000000..5c65239 --- /dev/null +++ b/coresense_instrumentation_driver/test/CMakeLists.txt @@ -0,0 +1,3 @@ +ament_add_gtest(test_coresense_instrumentation_driver_gtest test_coresense_instrumentation_driver.cpp) +ament_target_dependencies(test_coresense_instrumentation_driver_gtest ${dependencies}) +target_link_libraries(test_coresense_instrumentation_driver_gtest ${PROJECT_NAME}) \ No newline at end of file diff --git a/coresense_instrumentation_driver/test/test_coresense_instrumentation_driver.cpp b/coresense_instrumentation_driver/test/test_coresense_instrumentation_driver.cpp new file mode 100644 index 0000000..0e28bff --- /dev/null +++ b/coresense_instrumentation_driver/test/test_coresense_instrumentation_driver.cpp @@ -0,0 +1,60 @@ +#include +#include "coresense_instrumentation_driver/CoresenseInstrumentationDriver.hpp" + +TEST(CoresenseInstrumentationDriverTest, Initialization) +{ + rclcpp::init(0, nullptr); + auto driver = + std::make_unique(); + ASSERT_EQ(driver->getNumberOfNodes(), 0); + rclcpp::shutdown(); +} + +TEST(CoresenseInstrumentationDriverTest, AddNodeString) +{ + rclcpp::init(0, nullptr); + auto driver = + std::make_unique(); + driver->newNode("test_topic", "std_msgs/msg/String"); + ASSERT_EQ(driver->getNumberOfNodes(), 1); + rclcpp::shutdown(); +} + +TEST(CoresenseInstrumentationDriverTest, AddNodeScan) +{ + rclcpp::init(0, nullptr); + auto driver = + std::make_unique(); + driver->newNode("test_topic", "sensor_msgs/msg/LaserScan"); + ASSERT_EQ(driver->getNumberOfNodes(), 1); + rclcpp::shutdown(); +} + +TEST(CoresenseInstrumentationDriverTest, AddNodeFail) +{ + rclcpp::init(0, nullptr); + auto driver = + std::make_unique(); + driver->newNode("test_topic_2", "std_msgs/msg/Float32"); + ASSERT_EQ(driver->getNumberOfNodes(), 0); + rclcpp::shutdown(); +} + +TEST(CoresenseInstrumentationDriverTest, RemoveNodeString) +{ + rclcpp::init(0, nullptr); + auto driver = + std::make_unique(); + driver->newNode("test_topic", "std_msgs/msg/String"); + ASSERT_EQ(driver->getNumberOfNodes(), 1); + + driver->removeNode("test_topic"); + ASSERT_EQ(driver->getNumberOfNodes(), 0); + rclcpp::shutdown(); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From 1ad8cfba0c4cd52ff791b822c9ad644b370ce263 Mon Sep 17 00:00:00 2001 From: Juancams Date: Wed, 29 Nov 2023 00:44:56 +0100 Subject: [PATCH 12/26] Flake8 errors solved --- .../launch/coresense_instrumentation_driver.launch.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/coresense_instrumentation_driver/launch/coresense_instrumentation_driver.launch.py b/coresense_instrumentation_driver/launch/coresense_instrumentation_driver.launch.py index fcaa806..4ffa438 100644 --- a/coresense_instrumentation_driver/launch/coresense_instrumentation_driver.launch.py +++ b/coresense_instrumentation_driver/launch/coresense_instrumentation_driver.launch.py @@ -24,8 +24,8 @@ def generate_launch_description(): plugin='coresense_instrumentation_driver::CoresenseInstrumentationDriver', name='coresense_instrumentation_driver_node', namespace='coresense', - parameters=[{"topics": ["/scan", "/chatter"], - "topic_types": ["sensor_msgs/msg/LaserScan", "std_msgs/msg/String"]}], + parameters=[{'topics': ['/scan', '/chatter'], + 'topic_types': ['sensor_msgs/msg/LaserScan', 'std_msgs/msg/String']}], ) container = ComposableNodeContainer( From 11e31ae53c0c729878a7adc4f27079cab5238481 Mon Sep 17 00:00:00 2001 From: Juan Carlos Manzanares Serrano Date: Wed, 29 Nov 2023 17:06:52 +0100 Subject: [PATCH 13/26] (#1) 0.1.0 * Reimplementation * LifecyclePublisher, topic parameter & deleted traces * New function: get_topic_type * Update CI * Added test * 0.1.0 --- .github/workflows/main.yaml | 2 +- coresense_instrumentation/CHANGELOG.rst | 8 + coresense_instrumentation/CMakeLists.txt | 4 + coresense_instrumentation/package.xml | 17 ++ .../CHANGELOG.rst | 8 + .../CMakeLists.txt | 28 ++-- .../CoresenseInstrumentationDriver.hpp | 49 ------ .../InstrumentationLifecycleNode.hpp | 55 +++++++ .../instrumentation_utils/ICell.hpp | 54 ------ .../InstrumentationLifecycleNode.hpp | 114 ------------- ...coresense_instrumentation_driver.launch.py | 25 ++- coresense_instrumentation_driver/package.xml | 2 +- .../CoresenseInstrumentationDriver.cpp | 122 -------------- .../InstrumentationLifecycleNode.cpp | 116 +++++++++++++ .../coresense_instrumentation_driver_node.cpp | 29 +++- .../test_coresense_instrumentation_driver.cpp | 154 +++++++++++++----- 16 files changed, 381 insertions(+), 406 deletions(-) create mode 100644 coresense_instrumentation/CHANGELOG.rst create mode 100644 coresense_instrumentation/CMakeLists.txt create mode 100644 coresense_instrumentation/package.xml create mode 100644 coresense_instrumentation_driver/CHANGELOG.rst delete mode 100644 coresense_instrumentation_driver/include/coresense_instrumentation_driver/CoresenseInstrumentationDriver.hpp create mode 100644 coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp delete mode 100644 coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/ICell.hpp delete mode 100644 coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/InstrumentationLifecycleNode.hpp delete mode 100644 coresense_instrumentation_driver/src/coresense_instrumentation_driver/CoresenseInstrumentationDriver.cpp create mode 100644 coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationLifecycleNode.cpp diff --git a/.github/workflows/main.yaml b/.github/workflows/main.yaml index 35a3992..6d48f52 100644 --- a/.github/workflows/main.yaml +++ b/.github/workflows/main.yaml @@ -3,7 +3,7 @@ name: main on: push: branches: - - main + - '*' jobs: ROS2: diff --git a/coresense_instrumentation/CHANGELOG.rst b/coresense_instrumentation/CHANGELOG.rst new file mode 100644 index 0000000..4f22ad4 --- /dev/null +++ b/coresense_instrumentation/CHANGELOG.rst @@ -0,0 +1,8 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package coresense_instrumentation +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.0 (2023-11-29) +------------------ +* Add website tag +* Contributors: Noel Jimenez \ No newline at end of file diff --git a/coresense_instrumentation/CMakeLists.txt b/coresense_instrumentation/CMakeLists.txt new file mode 100644 index 0000000..d709e0d --- /dev/null +++ b/coresense_instrumentation/CMakeLists.txt @@ -0,0 +1,4 @@ +cmake_minimum_required(VERSION 3.5) +project(coresense_instrumentation) +find_package(ament_cmake REQUIRED) +ament_package() \ No newline at end of file diff --git a/coresense_instrumentation/package.xml b/coresense_instrumentation/package.xml new file mode 100644 index 0000000..4d2db93 --- /dev/null +++ b/coresense_instrumentation/package.xml @@ -0,0 +1,17 @@ + + + + coresense_instrumentation + 0.1.0 + Coresense Instrumentation + Juan Carlos Manzanares Serrano + Apache-2.0 + + ament_cmake + + coresense_instrumentation_driver + + + ament_cmake + + \ No newline at end of file diff --git a/coresense_instrumentation_driver/CHANGELOG.rst b/coresense_instrumentation_driver/CHANGELOG.rst new file mode 100644 index 0000000..4f22ad4 --- /dev/null +++ b/coresense_instrumentation_driver/CHANGELOG.rst @@ -0,0 +1,8 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package coresense_instrumentation +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.0 (2023-11-29) +------------------ +* Add website tag +* Contributors: Noel Jimenez \ No newline at end of file diff --git a/coresense_instrumentation_driver/CMakeLists.txt b/coresense_instrumentation_driver/CMakeLists.txt index ba618ed..133779e 100644 --- a/coresense_instrumentation_driver/CMakeLists.txt +++ b/coresense_instrumentation_driver/CMakeLists.txt @@ -24,36 +24,38 @@ set(dependencies include_directories(include) add_library(${PROJECT_NAME} SHARED - src/coresense_instrumentation_driver/CoresenseInstrumentationDriver.cpp + src/coresense_instrumentation_driver/InstrumentationLifecycleNode.cpp # ) -ament_target_dependencies(${PROJECT_NAME} ${dependencies}) -add_executable(coresense_instrumentation_driver_node src/coresense_instrumentation_driver_node.cpp) -ament_target_dependencies(coresense_instrumentation_driver_node ${dependencies}) -target_link_libraries(coresense_instrumentation_driver_node ${PROJECT_NAME}) +ament_target_dependencies(${PROJECT_NAME} ${dependencies}) rclcpp_components_register_nodes(${PROJECT_NAME} - PLUGIN "coresense_instrumentation_driver::CoresenseInstrumentationDriver" - EXECUTABLE coresense_instrumentation_driver_node + "coresense_instrumentation_driver::InstrumentationLifecycleNode" + "coresense_instrumentation_driver::InstrumentationLifecycleNode" ) install(TARGETS ${PROJECT_NAME} - coresense_instrumentation_driver_node - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} + DESTINATION lib ) install(DIRECTORY include/ DESTINATION include ) +add_executable(coresense_instrumentation_driver_node src/coresense_instrumentation_driver_node.cpp) +ament_target_dependencies(coresense_instrumentation_driver_node ${dependencies}) +target_link_libraries(coresense_instrumentation_driver_node ${PROJECT_NAME}) + +install(TARGETS + coresense_instrumentation_driver_node + DESTINATION lib/${PROJECT_NAME} +) + install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}) ament_export_include_directories(include) ament_export_dependencies(${dependencies}) -ament_export_libraries(${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -64,4 +66,4 @@ if(BUILD_TESTING) add_subdirectory(test) endif() -ament_package() \ No newline at end of file +ament_package() diff --git a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/CoresenseInstrumentationDriver.hpp b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/CoresenseInstrumentationDriver.hpp deleted file mode 100644 index 881ed0b..0000000 --- a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/CoresenseInstrumentationDriver.hpp +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright 2023 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 CORESENSE_INSTRUMENTATION_DRIVER_HPP -#define CORESENSE_INSTRUMENTATION_DRIVER_HPP - -#include "rclcpp/rclcpp.hpp" -#include "coresense_instrumentation_driver/instrumentation_utils/InstrumentationLifecycleNode.hpp" - - -namespace coresense_instrumentation_driver -{ - -class CoresenseInstrumentationDriver : public rclcpp::Node -{ -public: - explicit CoresenseInstrumentationDriver( - const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); - virtual ~CoresenseInstrumentationDriver(); - - int getNumberOfNodes(); - void newNode(const std::string & topic, const std::string & topic_type); - void removeNode(const std::string & topic); - -private: - void initializeNodes(); - void addNode(const std::string & topic, const std::string & topic_type); - - rclcpp::executors::MultiThreadedExecutor executor_; - std::vector> nodes_; - std::vector topics_; - std::vector topic_types_; - std::unique_ptr thread_; -}; - -} // namespace coresense_instrumentation_driver - -#endif // CORESENSE_INSTRUMENTATION_DRIVER_HPP diff --git a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp new file mode 100644 index 0000000..0d1350c --- /dev/null +++ b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp @@ -0,0 +1,55 @@ +// Copyright 2023 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 INSTRUMENTATION_LIFECYCLE_NODE_HPP +#define INSTRUMENTATION_LIFECYCLE_NODE_HPP + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "std_msgs/msg/string.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" + +namespace coresense_instrumentation_driver +{ + +template +class InstrumentationLifecycleNode : public rclcpp_lifecycle::LifecycleNode +{ +public: + InstrumentationLifecycleNode( + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + + virtual ~InstrumentationLifecycleNode(); + + std::string get_topic(); + std::string get_topic_type(); + + using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + + CallbackReturnT on_configure(const rclcpp_lifecycle::State &) override; + CallbackReturnT on_activate(const rclcpp_lifecycle::State &) override; + CallbackReturnT on_deactivate(const rclcpp_lifecycle::State &) override; + CallbackReturnT on_cleanup(const rclcpp_lifecycle::State &) override; + CallbackReturnT on_shutdown(const rclcpp_lifecycle::State &) override; + +private: + typename rclcpp::Subscription::SharedPtr sub_; + typename rclcpp_lifecycle::LifecyclePublisher::SharedPtr pub_; + std::string topic_; + std::string topic_type_; +}; + +} // namespace coresense_instrumentation_driver + +#endif // INSTRUMENTATION_LIFECYCLE_NODE_HPP diff --git a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/ICell.hpp b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/ICell.hpp deleted file mode 100644 index f9ddfe4..0000000 --- a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/ICell.hpp +++ /dev/null @@ -1,54 +0,0 @@ -#ifndef ICELL_HPP -#define ICELL_HPP - -#include "rclcpp/rclcpp.hpp" - -// Esta es una clase base abstracta que define la interfaz común de los nodos de instrumentación -class ICell -{ -public: - // Este es un método virtual puro que debe ser implementado por las clases derivadas - virtual void process() = 0; - - // Este es un destructor virtual para permitir el polimorfismo - virtual ~ICell() {} - - void set_name(const std::string & name) - { - name_ = name; - } - - void set_topic(const std::string & topic) - { - topic_ = topic; - } - - void set_node_base_interface( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface) - { - node_base_interface_ = node_base_interface; - } - - std::string get_name() - { - return name_; - } - - std::string get_topic() - { - return topic_; - } - - - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_base_interface() - { - return node_base_interface_; - } - -private: - std::string name_; - std::string topic_; - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface_; -}; - -#endif // ICELL_HPP diff --git a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/InstrumentationLifecycleNode.hpp b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/InstrumentationLifecycleNode.hpp deleted file mode 100644 index 29d5d45..0000000 --- a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/instrumentation_utils/InstrumentationLifecycleNode.hpp +++ /dev/null @@ -1,114 +0,0 @@ -// Copyright 2023 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 INSTRUMENTATION_LIFECYCLE_NODE_HPP -#define INSTRUMENTATION_LIFECYCLE_NODE_HPP - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "coresense_instrumentation_driver/instrumentation_utils/ICell.hpp" -#include "std_msgs/msg/string.hpp" -#include "sensor_msgs/msg/laser_scan.hpp" - -namespace coresense_instrumentation_driver -{ - -template -class InstrumentationLifecycleNode : public rclcpp_lifecycle::LifecycleNode, public ICell -{ -public: - InstrumentationLifecycleNode( - const std::string & node_name, - const std::string & topic, - const std::string & ns = "", - const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) - : rclcpp_lifecycle::LifecycleNode(node_name, ns, options) - { - set_name(node_name); - set_topic(topic); - set_node_base_interface(this->get_node_base_interface()); - topic_ = topic; - - sub_ = this->create_subscription( - topic_, 10, - [this](const typename TopicT::SharedPtr msg) { - if (pub_) { - - pub_->publish(std::make_unique(*msg)); - } - }); - - pub_ = this->create_publisher(ns + topic_, 10); - - RCLCPP_INFO(get_logger(), "Creating InstrumentationLifecycleNode"); - } - - virtual ~InstrumentationLifecycleNode() - { - RCLCPP_INFO(get_logger(), "Destroying InstrumentationLifecycleNode"); - } - - std::string get_name() - { - return this->get_name(); - } - - std::string get_topic() - { - return topic_; - } - -private: - using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - - CallbackReturnT - on_configure(const rclcpp_lifecycle::State &) override - { - return CallbackReturnT::SUCCESS; - } - - CallbackReturnT - on_activate(const rclcpp_lifecycle::State &) override - { - return CallbackReturnT::SUCCESS; - } - - CallbackReturnT - on_deactivate(const rclcpp_lifecycle::State &) override - { - return CallbackReturnT::SUCCESS; - } - - CallbackReturnT - on_cleanup(const rclcpp_lifecycle::State &) override - { - return CallbackReturnT::SUCCESS; - } - - CallbackReturnT - on_shutdown(const rclcpp_lifecycle::State &) override - { - return CallbackReturnT::SUCCESS; - } - - void process() override {} - - typename rclcpp::Subscription::SharedPtr sub_; - typename rclcpp::Publisher::SharedPtr pub_; - std::string topic_; -}; - -} // namespace coresense_instrumentation_driver - -#endif // INSTRUMENTATION_LIFECYCLE_NODE_HPP diff --git a/coresense_instrumentation_driver/launch/coresense_instrumentation_driver.launch.py b/coresense_instrumentation_driver/launch/coresense_instrumentation_driver.launch.py index 4ffa438..c395a6a 100644 --- a/coresense_instrumentation_driver/launch/coresense_instrumentation_driver.launch.py +++ b/coresense_instrumentation_driver/launch/coresense_instrumentation_driver.launch.py @@ -19,21 +19,28 @@ def generate_launch_description(): - coresense_node = ComposableNode( - package='coresense_instrumentation_driver', - plugin='coresense_instrumentation_driver::CoresenseInstrumentationDriver', - name='coresense_instrumentation_driver_node', - namespace='coresense', - parameters=[{'topics': ['/scan', '/chatter'], - 'topic_types': ['sensor_msgs/msg/LaserScan', 'std_msgs/msg/String']}], - ) + names = ['scan', 'chatter'] + topics = ['/scan', '/chatter'] + types = ['sensor_msgs::msg::LaserScan', 'std_msgs::msg::String'] + + composable_nodes = [] + for topic, topic_type, name in zip(topics, types, names): + composable_node = ComposableNode( + package='coresense_instrumentation_driver', + plugin='coresense_instrumentation_driver::InstrumentationLifecycleNode<' + + topic_type + '>', + name=name + '_node', + namespace='coresense', + parameters=[{'topic': topic, 'topic_type': topic_type}], + ) + composable_nodes.append(composable_node) container = ComposableNodeContainer( name='coresense_container', namespace='coresense', package='rclcpp_components', executable='component_container', - composable_node_descriptions=[coresense_node], + composable_node_descriptions=composable_nodes, output='screen', ) diff --git a/coresense_instrumentation_driver/package.xml b/coresense_instrumentation_driver/package.xml index c3c99d7..6313f5a 100644 --- a/coresense_instrumentation_driver/package.xml +++ b/coresense_instrumentation_driver/package.xml @@ -2,7 +2,7 @@ coresense_instrumentation_driver - 0.0.0 + 0.1.0 Coresense Instrumentation Driver Juan Carlos Manzanares Serrano Apache-2.0 diff --git a/coresense_instrumentation_driver/src/coresense_instrumentation_driver/CoresenseInstrumentationDriver.cpp b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/CoresenseInstrumentationDriver.cpp deleted file mode 100644 index 5b04289..0000000 --- a/coresense_instrumentation_driver/src/coresense_instrumentation_driver/CoresenseInstrumentationDriver.cpp +++ /dev/null @@ -1,122 +0,0 @@ -// Copyright 2023 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 "coresense_instrumentation_driver/CoresenseInstrumentationDriver.hpp" -#include - -namespace coresense_instrumentation_driver -{ - -CoresenseInstrumentationDriver::CoresenseInstrumentationDriver( - const rclcpp::NodeOptions & options) -: Node("coresense_instrumentation_driver_node", options) -{ - declare_parameter("topics", std::vector()); - declare_parameter("topic_types", std::vector()); - - get_parameter("topics", topics_); - get_parameter("topic_types", topic_types_); - - initializeNodes(); -} - -CoresenseInstrumentationDriver::~CoresenseInstrumentationDriver() -{ - executor_.cancel(); - if (thread_ && thread_->joinable()) { - thread_->join(); - } - - RCLCPP_INFO(get_logger(), "Destroying CoresenseInstrumentationDriver"); -} - -void -CoresenseInstrumentationDriver::initializeNodes() -{ - for (size_t i = 0; i < topics_.size(); i++) { - addNode(topics_[i], topic_types_[i]); - } - - thread_ = std::make_unique([&]() {executor_.spin();}); -} - -void -CoresenseInstrumentationDriver::addNode( - const std::string & new_topic, - const std::string & new_topic_type) -{ - if (new_topic_type == "std_msgs/msg/String") { - auto node = std::make_shared>( - "instrumentation_dynamic", new_topic, get_namespace()); - - nodes_.push_back(node); - - executor_.add_node(node->get_node_base_interface()); - } else if (new_topic_type == "sensor_msgs/msg/LaserScan") { - auto node = std::make_shared>( - "instrumentation_dynamic", new_topic, get_namespace()); - - nodes_.push_back(node); - - executor_.add_node(node->get_node_base_interface()); - } else { - RCLCPP_ERROR(get_logger(), "Unknown topic type '%s'", new_topic_type.c_str()); - } -} - -void -CoresenseInstrumentationDriver::newNode( - const std::string & new_topic, - const std::string & new_topic_type) -{ - if (executor_.get_number_of_threads() > 0) { - executor_.cancel(); - thread_->join(); - } - - addNode(new_topic, new_topic_type); - - thread_ = std::make_unique([&]() {executor_.spin();}); -} - -void CoresenseInstrumentationDriver::removeNode(const std::string & topic) -{ - auto it = std::find_if( - nodes_.begin(), nodes_.end(), - [&](const auto & node) {return node->get_topic() == topic;}); - - if (it != nodes_.end()) { - if (executor_.get_number_of_threads() > 0) { - executor_.cancel(); - thread_->join(); - } - - executor_.remove_node(it->get()->get_base_interface()); - nodes_.erase(it); - - thread_ = std::make_unique([&]() {executor_.spin();}); - } else { - RCLCPP_WARN(get_logger(), "Node for topic '%s' not found", topic.c_str()); - } -} - -int CoresenseInstrumentationDriver::getNumberOfNodes() -{ - return nodes_.size(); -} - -} // namespace coresense_instrumentation_driver - -#include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(coresense_instrumentation_driver::CoresenseInstrumentationDriver) diff --git a/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationLifecycleNode.cpp b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationLifecycleNode.cpp new file mode 100644 index 0000000..9e82ad5 --- /dev/null +++ b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationLifecycleNode.cpp @@ -0,0 +1,116 @@ +// Copyright 2023 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 "coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp" + +template class coresense_instrumentation_driver::InstrumentationLifecycleNode; +template class coresense_instrumentation_driver::InstrumentationLifecycleNode; + +namespace coresense_instrumentation_driver +{ + +template +InstrumentationLifecycleNode::InstrumentationLifecycleNode( + const rclcpp::NodeOptions & options) +: rclcpp_lifecycle::LifecycleNode("InstrumentationLifecycleNode", "", options) +{ + declare_parameter("topic", std::string("")); + declare_parameter("topic_type", std::string("")); + + get_parameter("topic", topic_); + get_parameter("topic_type", topic_type_); + + RCLCPP_DEBUG(get_logger(), "Creating InstrumentationLifecycleNode"); +} + +template +InstrumentationLifecycleNode::~InstrumentationLifecycleNode() +{ + RCLCPP_DEBUG(get_logger(), "Destroying InstrumentationLifecycleNode"); +} + +template +std::string InstrumentationLifecycleNode::get_topic() +{ + return topic_; +} + +template +std::string InstrumentationLifecycleNode::get_topic_type() +{ + return topic_type_; +} + +template +typename rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +InstrumentationLifecycleNode::on_configure(const rclcpp_lifecycle::State &) +{ + sub_ = this->create_subscription( + topic_, 10, + [this](const typename TopicT::SharedPtr msg) { + if (pub_) { + pub_->publish(std::make_unique(*msg)); + } + }); + + pub_ = this->create_publisher("/coresense" + topic_, 10); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +template +typename rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +InstrumentationLifecycleNode::on_activate(const rclcpp_lifecycle::State &) +{ + pub_->on_activate(); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +template +typename rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +InstrumentationLifecycleNode::on_deactivate(const rclcpp_lifecycle::State &) +{ + pub_->on_deactivate(); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +template +typename rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +InstrumentationLifecycleNode::on_cleanup(const rclcpp_lifecycle::State &) +{ + pub_.reset(); + sub_.reset(); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +template +typename rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +InstrumentationLifecycleNode::on_shutdown(const rclcpp_lifecycle::State &) +{ + pub_.reset(); + sub_.reset(); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +} // namespace coresense_instrumentation_driver + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE( + coresense_instrumentation_driver::InstrumentationLifecycleNode) +RCLCPP_COMPONENTS_REGISTER_NODE( + coresense_instrumentation_driver::InstrumentationLifecycleNode) diff --git a/coresense_instrumentation_driver/src/coresense_instrumentation_driver_node.cpp b/coresense_instrumentation_driver/src/coresense_instrumentation_driver_node.cpp index 74c2a92..625be7b 100644 --- a/coresense_instrumentation_driver/src/coresense_instrumentation_driver_node.cpp +++ b/coresense_instrumentation_driver/src/coresense_instrumentation_driver_node.cpp @@ -13,15 +13,38 @@ // limitations under the License. #include -#include "coresense_instrumentation_driver/CoresenseInstrumentationDriver.hpp" +#include "coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp" int main(int argc, char * argv[]) { rclcpp::init(argc, argv); - auto node = std::make_shared(); + if (argc < 3) { + RCLCPP_INFO(rclcpp::get_logger("main"), "Usage: %s ", argv[0]); + return 1; + } - rclcpp::spin(node); + std::string topic = argv[1]; + std::string topic_type = argv[2]; + + auto executor = std::make_shared(); + + if (topic_type == "std_msgs::msg::String") { + auto node = + std::make_shared>(); + + executor->add_node(node->get_node_base_interface()); + executor->spin(); + } else if (topic_type == "sensor_msgs::msg::LaserScan") { + auto node = + std::make_shared>(); + + executor->add_node(node->get_node_base_interface()); + executor->spin(); + } else { + RCLCPP_INFO(rclcpp::get_logger("main"), "Usage: %s ", argv[0]); + return 1; + } rclcpp::shutdown(); return 0; diff --git a/coresense_instrumentation_driver/test/test_coresense_instrumentation_driver.cpp b/coresense_instrumentation_driver/test/test_coresense_instrumentation_driver.cpp index 0e28bff..5560416 100644 --- a/coresense_instrumentation_driver/test/test_coresense_instrumentation_driver.cpp +++ b/coresense_instrumentation_driver/test/test_coresense_instrumentation_driver.cpp @@ -1,60 +1,134 @@ #include -#include "coresense_instrumentation_driver/CoresenseInstrumentationDriver.hpp" +#include +#include +#include +#include +#include "coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp" -TEST(CoresenseInstrumentationDriverTest, Initialization) +class IntegrationTest : public ::testing::Test { - rclcpp::init(0, nullptr); - auto driver = - std::make_unique(); - ASSERT_EQ(driver->getNumberOfNodes(), 0); - rclcpp::shutdown(); -} +protected: + void SetUp() override + { + rclcpp::init(0, nullptr); + } + + void TearDown() override + { + rclcpp::shutdown(); + } +}; -TEST(CoresenseInstrumentationDriverTest, AddNodeString) +TEST_F(IntegrationTest, LaserScanNodeLifecycle) { - rclcpp::init(0, nullptr); - auto driver = - std::make_unique(); - driver->newNode("test_topic", "std_msgs/msg/String"); - ASSERT_EQ(driver->getNumberOfNodes(), 1); - rclcpp::shutdown(); + rclcpp::executors::SingleThreadedExecutor executor; + + auto node = + std::make_shared>( + rclcpp::NodeOptions().append_parameter_override( + "topic", + "/test_topic").append_parameter_override( + "topic_type", "sensor_msgs::msg::LaserScan")); + + executor.add_node(node->get_node_base_interface()); + + auto result = node->on_configure(rclcpp_lifecycle::State()); + ASSERT_EQ( + result, + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); + + result = node->on_activate(rclcpp_lifecycle::State()); + ASSERT_EQ( + result, + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); + + node->on_activate(rclcpp_lifecycle::State()); + result = node->on_deactivate(rclcpp_lifecycle::State()); + ASSERT_EQ( + result, + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); + + result = node->on_cleanup(rclcpp_lifecycle::State()); + ASSERT_EQ( + result, + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); + + result = node->on_shutdown(rclcpp_lifecycle::State()); + ASSERT_EQ( + result, + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); + + executor.spin_once(std::chrono::seconds(1)); } -TEST(CoresenseInstrumentationDriverTest, AddNodeScan) +TEST_F(IntegrationTest, StringNodeLifecycle) { - rclcpp::init(0, nullptr); - auto driver = - std::make_unique(); - driver->newNode("test_topic", "sensor_msgs/msg/LaserScan"); - ASSERT_EQ(driver->getNumberOfNodes(), 1); - rclcpp::shutdown(); + rclcpp::executors::SingleThreadedExecutor executor; + + auto node = + std::make_shared>( + rclcpp::NodeOptions().append_parameter_override( + "topic", + "/test_topic").append_parameter_override( + "topic_type", "std_msgs::msg::String")); + + executor.add_node(node->get_node_base_interface()); + + auto result = node->on_configure(rclcpp_lifecycle::State()); + ASSERT_EQ( + result, + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); + + result = node->on_activate(rclcpp_lifecycle::State()); + ASSERT_EQ( + result, + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); + + node->on_activate(rclcpp_lifecycle::State()); + result = node->on_deactivate(rclcpp_lifecycle::State()); + ASSERT_EQ( + result, + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); + + result = node->on_cleanup(rclcpp_lifecycle::State()); + ASSERT_EQ( + result, + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); + + result = node->on_shutdown(rclcpp_lifecycle::State()); + ASSERT_EQ( + result, + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); + + executor.spin_once(std::chrono::seconds(1)); } -TEST(CoresenseInstrumentationDriverTest, AddNodeFail) +TEST_F(IntegrationTest, GetTopic) { - rclcpp::init(0, nullptr); - auto driver = - std::make_unique(); - driver->newNode("test_topic_2", "std_msgs/msg/Float32"); - ASSERT_EQ(driver->getNumberOfNodes(), 0); - rclcpp::shutdown(); + auto node = + std::make_shared>( + rclcpp::NodeOptions().append_parameter_override( + "topic", + "/test_topic").append_parameter_override( + "topic_type", "std_msgs::msg::String")); + + ASSERT_EQ(node->get_topic(), "/test_topic"); } -TEST(CoresenseInstrumentationDriverTest, RemoveNodeString) +TEST_F(IntegrationTest, GetTopicType) { - rclcpp::init(0, nullptr); - auto driver = - std::make_unique(); - driver->newNode("test_topic", "std_msgs/msg/String"); - ASSERT_EQ(driver->getNumberOfNodes(), 1); - - driver->removeNode("test_topic"); - ASSERT_EQ(driver->getNumberOfNodes(), 0); - rclcpp::shutdown(); + auto node = + std::make_shared>( + rclcpp::NodeOptions().append_parameter_override( + "topic", + "/test_topic").append_parameter_override( + "topic_type", "std_msgs::msg::String")); + + ASSERT_EQ(node->get_topic_type(), "std_msgs::msg::String"); } int main(int argc, char ** argv) { - ::testing::InitGoogleTest(&argc, argv); + testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } From fed938cbfb6a5ed39cb5de1f5e57b82c3d836750 Mon Sep 17 00:00:00 2001 From: Juancams Date: Thu, 30 Nov 2023 09:26:15 +0100 Subject: [PATCH 14/26] Update --- coresense_instrumentation/CHANGELOG.rst | 8 -------- 1 file changed, 8 deletions(-) delete mode 100644 coresense_instrumentation/CHANGELOG.rst diff --git a/coresense_instrumentation/CHANGELOG.rst b/coresense_instrumentation/CHANGELOG.rst deleted file mode 100644 index 4f22ad4..0000000 --- a/coresense_instrumentation/CHANGELOG.rst +++ /dev/null @@ -1,8 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package coresense_instrumentation -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.1.0 (2023-11-29) ------------------- -* Add website tag -* Contributors: Noel Jimenez \ No newline at end of file From 7c6cf0d6bd9263c4c957aa8c6b5d8ecf5ce9a60a Mon Sep 17 00:00:00 2001 From: Juan Carlos Manzanares Serrano Date: Thu, 30 Nov 2023 09:30:05 +0100 Subject: [PATCH 15/26] Update CHANGELOG.rst --- coresense_instrumentation_driver/CHANGELOG.rst | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/coresense_instrumentation_driver/CHANGELOG.rst b/coresense_instrumentation_driver/CHANGELOG.rst index 4f22ad4..c5d6b7e 100644 --- a/coresense_instrumentation_driver/CHANGELOG.rst +++ b/coresense_instrumentation_driver/CHANGELOG.rst @@ -4,5 +4,4 @@ Changelog for package coresense_instrumentation 0.1.0 (2023-11-29) ------------------ -* Add website tag -* Contributors: Noel Jimenez \ No newline at end of file +* Driver works correctly From 9a23fd8f0600d91e9d1d3ee5f224bb46decde1ea Mon Sep 17 00:00:00 2001 From: Juan Carlos Manzanares Serrano Date: Thu, 30 Nov 2023 18:39:03 +0100 Subject: [PATCH 16/26] 0.2.0 * Added generic template * Added image template * Added image_transport * Added image option * Added image_transport pub & sub * Bug * Added new tests * 0.2.0 --- coresense_instrumentation/CHANGELOG.rst | 11 ++ coresense_instrumentation/package.xml | 2 +- .../CHANGELOG.rst | 6 +- .../CMakeLists.txt | 6 +- .../InstrumentationLifecycleNode.hpp | 34 ++++++ ...coresense_instrumentation_driver.launch.py | 6 +- coresense_instrumentation_driver/package.xml | 3 +- ...cleNode.cpp => InstrumentationGeneric.cpp} | 11 +- .../InstrumentationImage.cpp | 110 ++++++++++++++++++ .../coresense_instrumentation_driver_node.cpp | 15 ++- .../test_coresense_instrumentation_driver.cpp | 43 +++++++ 11 files changed, 229 insertions(+), 18 deletions(-) create mode 100644 coresense_instrumentation/CHANGELOG.rst rename coresense_instrumentation_driver/src/coresense_instrumentation_driver/{InstrumentationLifecycleNode.cpp => InstrumentationGeneric.cpp} (94%) create mode 100644 coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationImage.cpp diff --git a/coresense_instrumentation/CHANGELOG.rst b/coresense_instrumentation/CHANGELOG.rst new file mode 100644 index 0000000..bf4cd09 --- /dev/null +++ b/coresense_instrumentation/CHANGELOG.rst @@ -0,0 +1,11 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package coresense_instrumentation +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.2.0 (2023-11-30) +------------------ +* Added new type: sensor_msgs/msg/Image with specialiced template + +0.1.0 (2023-11-29) +------------------ +* Driver works correctly diff --git a/coresense_instrumentation/package.xml b/coresense_instrumentation/package.xml index 4d2db93..f185844 100644 --- a/coresense_instrumentation/package.xml +++ b/coresense_instrumentation/package.xml @@ -2,7 +2,7 @@ coresense_instrumentation - 0.1.0 + 0.2.0 Coresense Instrumentation Juan Carlos Manzanares Serrano Apache-2.0 diff --git a/coresense_instrumentation_driver/CHANGELOG.rst b/coresense_instrumentation_driver/CHANGELOG.rst index c5d6b7e..74ee18a 100644 --- a/coresense_instrumentation_driver/CHANGELOG.rst +++ b/coresense_instrumentation_driver/CHANGELOG.rst @@ -1,7 +1,11 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package coresense_instrumentation +Changelog for package coresense_instrumentation_drived ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.0 (2023-11-30) +------------------ +* Added new type: sensor_msgs/msg/Image with specialiced template + 0.1.0 (2023-11-29) ------------------ * Driver works correctly diff --git a/coresense_instrumentation_driver/CMakeLists.txt b/coresense_instrumentation_driver/CMakeLists.txt index 133779e..6df5c95 100644 --- a/coresense_instrumentation_driver/CMakeLists.txt +++ b/coresense_instrumentation_driver/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(rclcpp_lifecycle REQUIRED) find_package(rclcpp_components REQUIRED) find_package(std_msgs REQUIRED) find_package(sensor_msgs REQUIRED) +find_package(image_transport REQUIRED) set(dependencies rclcpp @@ -19,12 +20,14 @@ set(dependencies rclcpp_lifecycle std_msgs sensor_msgs + image_transport ) include_directories(include) add_library(${PROJECT_NAME} SHARED - src/coresense_instrumentation_driver/InstrumentationLifecycleNode.cpp # + src/coresense_instrumentation_driver/InstrumentationGeneric.cpp + src/coresense_instrumentation_driver/InstrumentationImage.cpp ) ament_target_dependencies(${PROJECT_NAME} ${dependencies}) @@ -32,6 +35,7 @@ ament_target_dependencies(${PROJECT_NAME} ${dependencies}) rclcpp_components_register_nodes(${PROJECT_NAME} "coresense_instrumentation_driver::InstrumentationLifecycleNode" "coresense_instrumentation_driver::InstrumentationLifecycleNode" + "coresense_instrumentation_driver::InstrumentationLifecycleNode" ) install(TARGETS diff --git a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp index 0d1350c..1756652 100644 --- a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp +++ b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp @@ -19,10 +19,14 @@ #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "std_msgs/msg/string.hpp" #include "sensor_msgs/msg/laser_scan.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "image_transport/image_transport.hpp" namespace coresense_instrumentation_driver { +using std::placeholders::_1; + template class InstrumentationLifecycleNode : public rclcpp_lifecycle::LifecycleNode { @@ -50,6 +54,36 @@ class InstrumentationLifecycleNode : public rclcpp_lifecycle::LifecycleNode std::string topic_type_; }; +template<> +class InstrumentationLifecycleNode: public rclcpp_lifecycle::LifecycleNode +{ +public: + InstrumentationLifecycleNode( + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + + virtual ~InstrumentationLifecycleNode(); + + using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + + CallbackReturnT on_configure(const rclcpp_lifecycle::State &) override; + CallbackReturnT on_activate(const rclcpp_lifecycle::State &) override; + CallbackReturnT on_deactivate(const rclcpp_lifecycle::State &) override; + CallbackReturnT on_cleanup(const rclcpp_lifecycle::State &) override; + CallbackReturnT on_shutdown(const rclcpp_lifecycle::State &) override; + + std::string get_topic(); + std::string get_topic_type(); + +private: + void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr & msg); + + rclcpp::Node::SharedPtr node_; + image_transport::Publisher pub_; + image_transport::Subscriber sub_; + std::string topic_; + std::string topic_type_; +}; + } // namespace coresense_instrumentation_driver #endif // INSTRUMENTATION_LIFECYCLE_NODE_HPP diff --git a/coresense_instrumentation_driver/launch/coresense_instrumentation_driver.launch.py b/coresense_instrumentation_driver/launch/coresense_instrumentation_driver.launch.py index c395a6a..34f33c6 100644 --- a/coresense_instrumentation_driver/launch/coresense_instrumentation_driver.launch.py +++ b/coresense_instrumentation_driver/launch/coresense_instrumentation_driver.launch.py @@ -19,9 +19,9 @@ def generate_launch_description(): - names = ['scan', 'chatter'] - topics = ['/scan', '/chatter'] - types = ['sensor_msgs::msg::LaserScan', 'std_msgs::msg::String'] + names = ['scan', 'chatter', 'image'] + topics = ['/scan', '/chatter', '/image'] + types = ['sensor_msgs::msg::LaserScan', 'std_msgs::msg::String', 'sensor_msgs::msg::Image'] composable_nodes = [] for topic, topic_type, name in zip(topics, types, names): diff --git a/coresense_instrumentation_driver/package.xml b/coresense_instrumentation_driver/package.xml index 6313f5a..9f45365 100644 --- a/coresense_instrumentation_driver/package.xml +++ b/coresense_instrumentation_driver/package.xml @@ -2,7 +2,7 @@ coresense_instrumentation_driver - 0.1.0 + 0.2.0 Coresense Instrumentation Driver Juan Carlos Manzanares Serrano Apache-2.0 @@ -14,6 +14,7 @@ rclcpp_components std_msgs sensor_msgs + image_transport ament_lint_auto ament_lint_common diff --git a/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationLifecycleNode.cpp b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationGeneric.cpp similarity index 94% rename from coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationLifecycleNode.cpp rename to coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationGeneric.cpp index 9e82ad5..6b52f36 100644 --- a/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationLifecycleNode.cpp +++ b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationGeneric.cpp @@ -14,16 +14,17 @@ #include "coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp" -template class coresense_instrumentation_driver::InstrumentationLifecycleNode; -template class coresense_instrumentation_driver::InstrumentationLifecycleNode; namespace coresense_instrumentation_driver { +template class coresense_instrumentation_driver::InstrumentationLifecycleNode; +template class coresense_instrumentation_driver::InstrumentationLifecycleNode; + template InstrumentationLifecycleNode::InstrumentationLifecycleNode( const rclcpp::NodeOptions & options) -: rclcpp_lifecycle::LifecycleNode("InstrumentationLifecycleNode", "", options) +: rclcpp_lifecycle::LifecycleNode("lifecycle_node", "", options) { declare_parameter("topic", std::string("")); declare_parameter("topic_type", std::string("")); @@ -31,13 +32,13 @@ InstrumentationLifecycleNode::InstrumentationLifecycleNode( get_parameter("topic", topic_); get_parameter("topic_type", topic_type_); - RCLCPP_DEBUG(get_logger(), "Creating InstrumentationLifecycleNode"); + RCLCPP_INFO(get_logger(), "Creating InstrumentationGeneric"); } template InstrumentationLifecycleNode::~InstrumentationLifecycleNode() { - RCLCPP_DEBUG(get_logger(), "Destroying InstrumentationLifecycleNode"); + RCLCPP_DEBUG(get_logger(), "Destroying InstrumentationGeneric"); } template diff --git a/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationImage.cpp b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationImage.cpp new file mode 100644 index 0000000..ded88de --- /dev/null +++ b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationImage.cpp @@ -0,0 +1,110 @@ +// Copyright 2023 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 "coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp" + +namespace coresense_instrumentation_driver +{ + +template class coresense_instrumentation_driver::InstrumentationLifecycleNode; + +InstrumentationLifecycleNode::InstrumentationLifecycleNode( + const rclcpp::NodeOptions & options) +: rclcpp_lifecycle::LifecycleNode("image_node", "", options) +{ + declare_parameter("topic", std::string("")); + declare_parameter("topic_type", std::string("")); + + get_parameter("topic", topic_); + get_parameter("topic_type", topic_type_); + + node_ = rclcpp::Node::make_shared("subnode"); + + RCLCPP_INFO(get_logger(), "Creating InstrumentationImage"); +} + +InstrumentationLifecycleNode::~InstrumentationLifecycleNode() +{ + RCLCPP_DEBUG(get_logger(), "Destroying InstrumentationImage"); +} + +void InstrumentationLifecycleNode::imageCallback( + const sensor_msgs::msg::Image::ConstSharedPtr & msg) +{ + if (pub_ && pub_.getNumSubscribers() > 0) { + pub_.publish(msg); + } +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +InstrumentationLifecycleNode::on_configure(const rclcpp_lifecycle::State &) +{ + auto subscription_options = rclcpp::SubscriptionOptions(); + + sub_ = image_transport::create_subscription( + node_.get(), + topic_, + std::bind( + &InstrumentationLifecycleNode::imageCallback, this, + std::placeholders::_1), + "raw", + rmw_qos_profile_sensor_data, + subscription_options); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +InstrumentationLifecycleNode::on_activate(const rclcpp_lifecycle::State &) +{ + image_transport::ImageTransport it(node_); + pub_ = it.advertise("/coresense" + topic_, 1); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +InstrumentationLifecycleNode::on_deactivate( + const rclcpp_lifecycle::State &) +{ + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +InstrumentationLifecycleNode::on_cleanup(const rclcpp_lifecycle::State &) +{ + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +InstrumentationLifecycleNode::on_shutdown(const rclcpp_lifecycle::State &) +{ + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +std::string InstrumentationLifecycleNode::get_topic() +{ + return topic_; +} + +std::string InstrumentationLifecycleNode::get_topic_type() +{ + return topic_type_; +} + +} // namespace coresense_instrumentation_driver + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE( + coresense_instrumentation_driver::InstrumentationLifecycleNode) diff --git a/coresense_instrumentation_driver/src/coresense_instrumentation_driver_node.cpp b/coresense_instrumentation_driver/src/coresense_instrumentation_driver_node.cpp index 625be7b..3dbc20a 100644 --- a/coresense_instrumentation_driver/src/coresense_instrumentation_driver_node.cpp +++ b/coresense_instrumentation_driver/src/coresense_instrumentation_driver_node.cpp @@ -29,20 +29,23 @@ int main(int argc, char * argv[]) auto executor = std::make_shared(); - if (topic_type == "std_msgs::msg::String") { + if (topic_type == "sensor_msgs/msg/LaserScan") { + auto node = + std::make_shared>(); + executor->add_node(node->get_node_base_interface()); + executor->spin(); + } else if (topic_type == "std_msgs/msg/String") { auto node = std::make_shared>(); - executor->add_node(node->get_node_base_interface()); executor->spin(); - } else if (topic_type == "sensor_msgs::msg::LaserScan") { + } else if (topic_type == "sensor_msgs/msg/Image") { auto node = - std::make_shared>(); - + std::make_shared>(); executor->add_node(node->get_node_base_interface()); executor->spin(); } else { - RCLCPP_INFO(rclcpp::get_logger("main"), "Usage: %s ", argv[0]); + RCLCPP_ERROR(rclcpp::get_logger("main"), "Unknown topic type: %s", topic_type.c_str()); return 1; } diff --git a/coresense_instrumentation_driver/test/test_coresense_instrumentation_driver.cpp b/coresense_instrumentation_driver/test/test_coresense_instrumentation_driver.cpp index 5560416..e5ad660 100644 --- a/coresense_instrumentation_driver/test/test_coresense_instrumentation_driver.cpp +++ b/coresense_instrumentation_driver/test/test_coresense_instrumentation_driver.cpp @@ -2,6 +2,7 @@ #include #include #include +#include #include #include "coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp" @@ -103,6 +104,48 @@ TEST_F(IntegrationTest, StringNodeLifecycle) executor.spin_once(std::chrono::seconds(1)); } +TEST_F(IntegrationTest, ImageNodeLifecycle) +{ + rclcpp::executors::SingleThreadedExecutor executor; + + auto node = + std::make_shared>( + rclcpp::NodeOptions().append_parameter_override( + "topic", + "/test_topic").append_parameter_override( + "topic_type", "sensor_msgs::msg::Image")); + + executor.add_node(node->get_node_base_interface()); + + auto result = node->on_configure(rclcpp_lifecycle::State()); + ASSERT_EQ( + result, + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); + + result = node->on_activate(rclcpp_lifecycle::State()); + ASSERT_EQ( + result, + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); + + node->on_activate(rclcpp_lifecycle::State()); + result = node->on_deactivate(rclcpp_lifecycle::State()); + ASSERT_EQ( + result, + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); + + result = node->on_cleanup(rclcpp_lifecycle::State()); + ASSERT_EQ( + result, + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); + + result = node->on_shutdown(rclcpp_lifecycle::State()); + ASSERT_EQ( + result, + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); + + executor.spin_once(std::chrono::seconds(1)); +} + TEST_F(IntegrationTest, GetTopic) { auto node = From dad507224ca1156d565cf83101487f5dcef76ac5 Mon Sep 17 00:00:00 2001 From: Juan Carlos Manzanares Serrano Date: Thu, 25 Jan 2024 19:43:47 +0100 Subject: [PATCH 17/26] Publisher subscriber services & producer / consumer (#4) * New services: Create & delete publisher * New dependencies * Created services * Uncrustify * Services working correctly * Added new srv * New msg * Update CMakeLists * Name changed * Added producer / consumer * Name changed * Updated launcher --- .../CMakeLists.txt | 25 +- .../InstrumentationConsumer.hpp | 89 ++++++ .../InstrumentationLifecycleNode.hpp | 89 ------ .../InstrumentationProducer.hpp | 138 +++++++++ ...coresense_instrumentation_driver.launch.py | 20 +- coresense_instrumentation_driver/package.xml | 1 + .../InstrumentationConsumerGeneric.cpp | 276 +++++++++++++++++ .../InstrumentationGeneric.cpp | 117 -------- .../InstrumentationImage.cpp | 110 ------- .../InstrumentationProducerGeneric.cpp | 277 ++++++++++++++++++ .../InstrumentationProducerImage.cpp | 211 +++++++++++++ .../coresense_instrumentation_driver_node.cpp | 54 ---- .../test_coresense_instrumentation_driver.cpp | 12 +- .../CMakeLists.txt | 22 +- .../msg/NodeInfo.msg | 5 + .../package.xml | 9 +- .../srv/CreatePublisher.srv | 4 + .../srv/CreateSubscriber.srv | 4 + .../srv/DeletePublisher.srv | 4 + .../srv/DeleteSubscriber.srv | 4 + 20 files changed, 1066 insertions(+), 405 deletions(-) create mode 100644 coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationConsumer.hpp delete mode 100644 coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp create mode 100644 coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationProducer.hpp create mode 100644 coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationConsumerGeneric.cpp delete mode 100644 coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationGeneric.cpp delete mode 100644 coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationImage.cpp create mode 100644 coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationProducerGeneric.cpp create mode 100644 coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationProducerImage.cpp delete mode 100644 coresense_instrumentation_driver/src/coresense_instrumentation_driver_node.cpp create mode 100644 coresense_instrumentation_interfaces/msg/NodeInfo.msg create mode 100644 coresense_instrumentation_interfaces/srv/CreatePublisher.srv create mode 100644 coresense_instrumentation_interfaces/srv/CreateSubscriber.srv create mode 100644 coresense_instrumentation_interfaces/srv/DeletePublisher.srv create mode 100644 coresense_instrumentation_interfaces/srv/DeleteSubscriber.srv diff --git a/coresense_instrumentation_driver/CMakeLists.txt b/coresense_instrumentation_driver/CMakeLists.txt index 6df5c95..03235a5 100644 --- a/coresense_instrumentation_driver/CMakeLists.txt +++ b/coresense_instrumentation_driver/CMakeLists.txt @@ -12,7 +12,9 @@ find_package(rclcpp_lifecycle REQUIRED) find_package(rclcpp_components REQUIRED) find_package(std_msgs REQUIRED) find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(image_transport REQUIRED) +find_package(coresense_instrumentation_interfaces REQUIRED) set(dependencies rclcpp @@ -20,22 +22,30 @@ set(dependencies rclcpp_lifecycle std_msgs sensor_msgs + geometry_msgs image_transport + coresense_instrumentation_interfaces ) include_directories(include) add_library(${PROJECT_NAME} SHARED - src/coresense_instrumentation_driver/InstrumentationGeneric.cpp - src/coresense_instrumentation_driver/InstrumentationImage.cpp + src/coresense_instrumentation_driver/InstrumentationProducerGeneric.cpp + src/coresense_instrumentation_driver/InstrumentationProducerImage.cpp + src/coresense_instrumentation_driver/InstrumentationConsumerGeneric.cpp ) ament_target_dependencies(${PROJECT_NAME} ${dependencies}) rclcpp_components_register_nodes(${PROJECT_NAME} - "coresense_instrumentation_driver::InstrumentationLifecycleNode" - "coresense_instrumentation_driver::InstrumentationLifecycleNode" - "coresense_instrumentation_driver::InstrumentationLifecycleNode" + "coresense_instrumentation_driver::InstrumentationProducer" + "coresense_instrumentation_driver::InstrumentationProducer" + "coresense_instrumentation_driver::InstrumentationProducer" + "coresense_instrumentation_driver::InstrumentationProducer" + "coresense_instrumentation_driver::InstrumentationConsumer" + "coresense_instrumentation_driver::InstrumentationConsumer" + "coresense_instrumentation_driver::InstrumentationConsumer" + "coresense_instrumentation_driver::InstrumentationConsumer" ) install(TARGETS @@ -47,12 +57,7 @@ install(DIRECTORY include/ DESTINATION include ) -add_executable(coresense_instrumentation_driver_node src/coresense_instrumentation_driver_node.cpp) -ament_target_dependencies(coresense_instrumentation_driver_node ${dependencies}) -target_link_libraries(coresense_instrumentation_driver_node ${PROJECT_NAME}) - install(TARGETS - coresense_instrumentation_driver_node DESTINATION lib/${PROJECT_NAME} ) diff --git a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationConsumer.hpp b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationConsumer.hpp new file mode 100644 index 0000000..4e3c3ac --- /dev/null +++ b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationConsumer.hpp @@ -0,0 +1,89 @@ +// Copyright 2023 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 INSTRUMENTATION_PRODUCER_HPP +#define INSTRUMENTATION_PRODUCER_HPP + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "std_msgs/msg/string.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "image_transport/image_transport.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "coresense_instrumentation_interfaces/srv/create_publisher.hpp" +#include "coresense_instrumentation_interfaces/srv/delete_publisher.hpp" +#include "coresense_instrumentation_interfaces/srv/create_subscriber.hpp" +#include "coresense_instrumentation_interfaces/srv/delete_subscriber.hpp" +#include "coresense_instrumentation_interfaces/msg/node_info.hpp" +#include + +namespace coresense_instrumentation_driver +{ + +using std::placeholders::_1; + +template +class InstrumentationConsumer : public rclcpp_lifecycle::LifecycleNode +{ +public: + InstrumentationConsumer( + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + + virtual ~InstrumentationConsumer(); + + std::string get_topic(); + std::string get_topic_type(); + + using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + + CallbackReturnT on_configure(const rclcpp_lifecycle::State &) override; + CallbackReturnT on_activate(const rclcpp_lifecycle::State &) override; + CallbackReturnT on_deactivate(const rclcpp_lifecycle::State &) override; + CallbackReturnT on_cleanup(const rclcpp_lifecycle::State &) override; + CallbackReturnT on_shutdown(const rclcpp_lifecycle::State &) override; + +private: + typename rclcpp_lifecycle::LifecyclePublisher::SharedPtr pub_; + rclcpp::Publisher::SharedPtr status_pub_; + rclcpp::TimerBase::SharedPtr status_timer_; + + void handleCreateSubscriberRequest( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void handleDeleteSubscriberRequest( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void publish_status(); + + rclcpp::Service::SharedPtr + create_subscriber_service_; + rclcpp::Service::SharedPtr + delete_subscriber_service_; + + std::unordered_map::SharedPtr> subscriptions_; + + std::string topic_; + std::string topic_type_; +}; + +} // namespace coresense_instrumentation_driver + +#endif // INSTRUMENTATION_PRODUCER_HPP diff --git a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp deleted file mode 100644 index 1756652..0000000 --- a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp +++ /dev/null @@ -1,89 +0,0 @@ -// Copyright 2023 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 INSTRUMENTATION_LIFECYCLE_NODE_HPP -#define INSTRUMENTATION_LIFECYCLE_NODE_HPP - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "std_msgs/msg/string.hpp" -#include "sensor_msgs/msg/laser_scan.hpp" -#include "sensor_msgs/msg/image.hpp" -#include "image_transport/image_transport.hpp" - -namespace coresense_instrumentation_driver -{ - -using std::placeholders::_1; - -template -class InstrumentationLifecycleNode : public rclcpp_lifecycle::LifecycleNode -{ -public: - InstrumentationLifecycleNode( - const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); - - virtual ~InstrumentationLifecycleNode(); - - std::string get_topic(); - std::string get_topic_type(); - - using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - - CallbackReturnT on_configure(const rclcpp_lifecycle::State &) override; - CallbackReturnT on_activate(const rclcpp_lifecycle::State &) override; - CallbackReturnT on_deactivate(const rclcpp_lifecycle::State &) override; - CallbackReturnT on_cleanup(const rclcpp_lifecycle::State &) override; - CallbackReturnT on_shutdown(const rclcpp_lifecycle::State &) override; - -private: - typename rclcpp::Subscription::SharedPtr sub_; - typename rclcpp_lifecycle::LifecyclePublisher::SharedPtr pub_; - std::string topic_; - std::string topic_type_; -}; - -template<> -class InstrumentationLifecycleNode: public rclcpp_lifecycle::LifecycleNode -{ -public: - InstrumentationLifecycleNode( - const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); - - virtual ~InstrumentationLifecycleNode(); - - using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - - CallbackReturnT on_configure(const rclcpp_lifecycle::State &) override; - CallbackReturnT on_activate(const rclcpp_lifecycle::State &) override; - CallbackReturnT on_deactivate(const rclcpp_lifecycle::State &) override; - CallbackReturnT on_cleanup(const rclcpp_lifecycle::State &) override; - CallbackReturnT on_shutdown(const rclcpp_lifecycle::State &) override; - - std::string get_topic(); - std::string get_topic_type(); - -private: - void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr & msg); - - rclcpp::Node::SharedPtr node_; - image_transport::Publisher pub_; - image_transport::Subscriber sub_; - std::string topic_; - std::string topic_type_; -}; - -} // namespace coresense_instrumentation_driver - -#endif // INSTRUMENTATION_LIFECYCLE_NODE_HPP diff --git a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationProducer.hpp b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationProducer.hpp new file mode 100644 index 0000000..cf03e01 --- /dev/null +++ b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationProducer.hpp @@ -0,0 +1,138 @@ +// Copyright 2023 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 INSTRUMENTATION_PRODUCER_HPP +#define INSTRUMENTATION_PRODUCER_HPP + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "std_msgs/msg/string.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "image_transport/image_transport.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "coresense_instrumentation_interfaces/srv/create_publisher.hpp" +#include "coresense_instrumentation_interfaces/srv/delete_publisher.hpp" +#include "coresense_instrumentation_interfaces/msg/node_info.hpp" +#include + +namespace coresense_instrumentation_driver +{ + +using std::placeholders::_1; + +template +class InstrumentationProducer : public rclcpp_lifecycle::LifecycleNode +{ +public: + InstrumentationProducer( + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + + virtual ~InstrumentationProducer(); + + std::string get_topic(); + std::string get_topic_type(); + + using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + + CallbackReturnT on_configure(const rclcpp_lifecycle::State &) override; + CallbackReturnT on_activate(const rclcpp_lifecycle::State &) override; + CallbackReturnT on_deactivate(const rclcpp_lifecycle::State &) override; + CallbackReturnT on_cleanup(const rclcpp_lifecycle::State &) override; + CallbackReturnT on_shutdown(const rclcpp_lifecycle::State &) override; + +private: + typename rclcpp::Subscription::SharedPtr sub_; + rclcpp::Publisher::SharedPtr status_pub_; + rclcpp::TimerBase::SharedPtr status_timer_; + + void handleCreatePublisherRequest( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void handleDeletePublisherRequest( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void publish_status(); + + rclcpp::Service::SharedPtr + create_publisher_service_; + rclcpp::Service::SharedPtr + delete_publisher_service_; + + std::unordered_map::SharedPtr> publishers_; + + std::string topic_; + std::string topic_type_; +}; + +template<> +class InstrumentationProducer: public rclcpp_lifecycle::LifecycleNode +{ +public: + InstrumentationProducer( + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + + virtual ~InstrumentationProducer(); + + using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + + CallbackReturnT on_configure(const rclcpp_lifecycle::State &) override; + CallbackReturnT on_activate(const rclcpp_lifecycle::State &) override; + CallbackReturnT on_deactivate(const rclcpp_lifecycle::State &) override; + CallbackReturnT on_cleanup(const rclcpp_lifecycle::State &) override; + CallbackReturnT on_shutdown(const rclcpp_lifecycle::State &) override; + + std::string get_topic(); + std::string get_topic_type(); + +private: + rclcpp::Publisher::SharedPtr status_pub_; + rclcpp::TimerBase::SharedPtr status_timer_; + + void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr & msg); + + void handleCreatePublisherRequest( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void handleDeletePublisherRequest( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void publish_status(); + + rclcpp::Service::SharedPtr + create_publisher_service_; + rclcpp::Service::SharedPtr + delete_publisher_service_; + + std::unordered_map publishers_; + + rclcpp::Node::SharedPtr node_; + image_transport::Subscriber sub_; + std::string topic_; + std::string topic_type_; +}; + +} // namespace coresense_instrumentation_driver + +#endif // INSTRUMENTATION_PRODUCER_HPP diff --git a/coresense_instrumentation_driver/launch/coresense_instrumentation_driver.launch.py b/coresense_instrumentation_driver/launch/coresense_instrumentation_driver.launch.py index 34f33c6..9e373aa 100644 --- a/coresense_instrumentation_driver/launch/coresense_instrumentation_driver.launch.py +++ b/coresense_instrumentation_driver/launch/coresense_instrumentation_driver.launch.py @@ -19,25 +19,27 @@ def generate_launch_description(): - names = ['scan', 'chatter', 'image'] - topics = ['/scan', '/chatter', '/image'] - types = ['sensor_msgs::msg::LaserScan', 'std_msgs::msg::String', 'sensor_msgs::msg::Image'] + names = ['scan_raw', 'nav_vel'] + topics = ['/scan_raw', '/nav_vel'] + msgs = ['sensor_msgs::msg::LaserScan', 'geometry_msgs::msg::Twist'] + node_types = ['Consumer', 'Consumer'] + ns = '' composable_nodes = [] - for topic, topic_type, name in zip(topics, types, names): + for topic, msg, name, type in zip(topics, msgs, names, node_types): composable_node = ComposableNode( package='coresense_instrumentation_driver', - plugin='coresense_instrumentation_driver::InstrumentationLifecycleNode<' - + topic_type + '>', + plugin='coresense_instrumentation_driver::Instrumentation' + type + '<' + + msg + '>', name=name + '_node', - namespace='coresense', - parameters=[{'topic': topic, 'topic_type': topic_type}], + namespace=ns, + parameters=[{'topic': topic, 'topic_type': msg}], ) composable_nodes.append(composable_node) container = ComposableNodeContainer( name='coresense_container', - namespace='coresense', + namespace=ns, package='rclcpp_components', executable='component_container', composable_node_descriptions=composable_nodes, diff --git a/coresense_instrumentation_driver/package.xml b/coresense_instrumentation_driver/package.xml index 9f45365..8f1103a 100644 --- a/coresense_instrumentation_driver/package.xml +++ b/coresense_instrumentation_driver/package.xml @@ -15,6 +15,7 @@ std_msgs sensor_msgs image_transport + coresense_instrumentation_interfaces ament_lint_auto ament_lint_common diff --git a/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationConsumerGeneric.cpp b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationConsumerGeneric.cpp new file mode 100644 index 0000000..b016b10 --- /dev/null +++ b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationConsumerGeneric.cpp @@ -0,0 +1,276 @@ +// Copyright 2023 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 "coresense_instrumentation_driver/InstrumentationConsumer.hpp" + + +namespace coresense_instrumentation_driver +{ + +template class coresense_instrumentation_driver::InstrumentationConsumer; +template class coresense_instrumentation_driver::InstrumentationConsumer; +template class coresense_instrumentation_driver::InstrumentationConsumer; + +template +InstrumentationConsumer::InstrumentationConsumer( + const rclcpp::NodeOptions & options) +: rclcpp_lifecycle::LifecycleNode("lifecycle_node", "", options) +{ + declare_parameter("topic", std::string("")); + declare_parameter("topic_type", std::string("")); + + get_parameter("topic", topic_); + get_parameter("topic_type", topic_type_); + + status_pub_ = this->create_publisher( + "/status", 10); + + status_timer_ = this->create_wall_timer( + std::chrono::seconds(1), + [this]() { + publish_status(); + }); + + RCLCPP_DEBUG(get_logger(), "Creating InstrumentationGeneric"); +} + +template +InstrumentationConsumer::~InstrumentationConsumer() +{ + RCLCPP_DEBUG(get_logger(), "Destroying InstrumentationGeneric"); +} + +template +std::string InstrumentationConsumer::get_topic() +{ + return topic_; +} + +template +std::string InstrumentationConsumer::get_topic_type() +{ + return topic_type_; +} + +template +void InstrumentationConsumer::publish_status() +{ + auto status_msg = std::make_unique(); + auto lifecycle_state = get_current_state(); + + if (std::string(this->get_namespace()) == "/") { + status_msg->node_name = this->get_name(); + } else { + status_msg->node_name = std::string(this->get_namespace()) + "/" + this->get_name(); + } + + status_msg->state = lifecycle_state.id(); + status_msg->stamp = this->now(); + + for (const auto & entry : subscriptions_) { + status_msg->topics.push_back(entry.first); + } + + int status; + char * demangled_name = abi::__cxa_demangle(typeid(TopicT).name(), nullptr, nullptr, &status); + std::string result(demangled_name); + + size_t pos = result.find('<'); + if (pos != std::string::npos) { + result = result.substr(0, pos); + } + + size_t last_underscore = result.rfind('_'); + if (last_underscore != std::string::npos) { + result = result.substr(0, last_underscore); + } + + std::free(demangled_name); + + status_msg->type = result; + + status_pub_->publish(std::move(status_msg)); +} + +template +typename rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +InstrumentationConsumer::on_configure(const rclcpp_lifecycle::State &) +{ + pub_ = this->create_publisher(topic_, 10); + + if (topic_[0] == '/') { + topic_ = topic_.substr(1); + } + + std::string topic; + + if (std::string(this->get_namespace()) == "/") { + topic = "/coresense/" + topic_; + } else { + topic = std::string(this->get_namespace()) + "/coresense/" + topic_; + } + + auto sub = this->create_subscription( + topic, 10, + [this](const typename TopicT::SharedPtr msg) { + if (pub_) { + pub_->publish(std::make_unique(*msg)); + } + }); + + subscriptions_.insert({topic, sub}); + + std::string create_service, delete_service; + + if (std::string(this->get_namespace()) == "/") { + create_service = std::string(this->get_name()) + "/create_subscriber"; + delete_service = std::string(this->get_name()) + "/delete_subscriber"; + } else { + create_service = std::string(this->get_namespace()) + "/" + this->get_name() + + "/create_subscriber"; + delete_service = std::string(this->get_namespace()) + "/" + this->get_name() + + "/delete_subscriber"; + } + + create_subscriber_service_ = + this->create_service( + create_service, + std::bind( + &InstrumentationConsumer::handleCreateSubscriberRequest, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + delete_subscriber_service_ = + this->create_service( + delete_service, + std::bind( + &InstrumentationConsumer::handleDeleteSubscriberRequest, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +template +typename rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +InstrumentationConsumer::on_activate(const rclcpp_lifecycle::State &) +{ + pub_->on_activate(); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +template +typename rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +InstrumentationConsumer::on_deactivate(const rclcpp_lifecycle::State &) +{ + pub_->on_deactivate(); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +template +typename rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +InstrumentationConsumer::on_cleanup(const rclcpp_lifecycle::State &) +{ + for (auto & sub : subscriptions_) { + sub.second.reset(); + } + + subscriptions_.clear(); + + pub_.reset(); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +template +typename rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +InstrumentationConsumer::on_shutdown(const rclcpp_lifecycle::State &) +{ + for (auto & sub : subscriptions_) { + sub.second.reset(); + } + + subscriptions_.clear(); + + pub_.reset(); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +template +void InstrumentationConsumer::handleCreateSubscriberRequest( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + + std::string new_topic = request->topic_name; + + if (new_topic[0] == '/') { + new_topic = new_topic.substr(1); + } + + for (auto & sub : subscriptions_) { + if (sub.first == new_topic) { + response->success = false; + return; + } + } + + auto new_sub = this->create_subscription( + new_topic, 10, + [this](const typename TopicT::SharedPtr msg) { + if (pub_) { + pub_->publish(std::make_unique(*msg)); + } + }); + + subscriptions_.insert({new_topic, new_sub}); + response->success = true; +} + +template +void InstrumentationConsumer::handleDeleteSubscriberRequest( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + + std::string remove_topic = request->topic_name; + + if (remove_topic[0] == '/') { + remove_topic = remove_topic.substr(1); + } + + for (auto & sub : subscriptions_) { + if (sub.first == remove_topic) { + sub.second.reset(); + } + } + + subscriptions_.erase(remove_topic); + response->success = true; +} + +} // namespace coresense_instrumentation_driver + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE( + coresense_instrumentation_driver::InstrumentationConsumer) +RCLCPP_COMPONENTS_REGISTER_NODE( + coresense_instrumentation_driver::InstrumentationConsumer) +RCLCPP_COMPONENTS_REGISTER_NODE( + coresense_instrumentation_driver::InstrumentationConsumer) diff --git a/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationGeneric.cpp b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationGeneric.cpp deleted file mode 100644 index 6b52f36..0000000 --- a/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationGeneric.cpp +++ /dev/null @@ -1,117 +0,0 @@ -// Copyright 2023 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 "coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp" - - -namespace coresense_instrumentation_driver -{ - -template class coresense_instrumentation_driver::InstrumentationLifecycleNode; -template class coresense_instrumentation_driver::InstrumentationLifecycleNode; - -template -InstrumentationLifecycleNode::InstrumentationLifecycleNode( - const rclcpp::NodeOptions & options) -: rclcpp_lifecycle::LifecycleNode("lifecycle_node", "", options) -{ - declare_parameter("topic", std::string("")); - declare_parameter("topic_type", std::string("")); - - get_parameter("topic", topic_); - get_parameter("topic_type", topic_type_); - - RCLCPP_INFO(get_logger(), "Creating InstrumentationGeneric"); -} - -template -InstrumentationLifecycleNode::~InstrumentationLifecycleNode() -{ - RCLCPP_DEBUG(get_logger(), "Destroying InstrumentationGeneric"); -} - -template -std::string InstrumentationLifecycleNode::get_topic() -{ - return topic_; -} - -template -std::string InstrumentationLifecycleNode::get_topic_type() -{ - return topic_type_; -} - -template -typename rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -InstrumentationLifecycleNode::on_configure(const rclcpp_lifecycle::State &) -{ - sub_ = this->create_subscription( - topic_, 10, - [this](const typename TopicT::SharedPtr msg) { - if (pub_) { - pub_->publish(std::make_unique(*msg)); - } - }); - - pub_ = this->create_publisher("/coresense" + topic_, 10); - - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; -} - -template -typename rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -InstrumentationLifecycleNode::on_activate(const rclcpp_lifecycle::State &) -{ - pub_->on_activate(); - - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; -} - -template -typename rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -InstrumentationLifecycleNode::on_deactivate(const rclcpp_lifecycle::State &) -{ - pub_->on_deactivate(); - - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; -} - -template -typename rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -InstrumentationLifecycleNode::on_cleanup(const rclcpp_lifecycle::State &) -{ - pub_.reset(); - sub_.reset(); - - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; -} - -template -typename rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -InstrumentationLifecycleNode::on_shutdown(const rclcpp_lifecycle::State &) -{ - pub_.reset(); - sub_.reset(); - - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; -} - -} // namespace coresense_instrumentation_driver - -#include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE( - coresense_instrumentation_driver::InstrumentationLifecycleNode) -RCLCPP_COMPONENTS_REGISTER_NODE( - coresense_instrumentation_driver::InstrumentationLifecycleNode) diff --git a/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationImage.cpp b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationImage.cpp deleted file mode 100644 index ded88de..0000000 --- a/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationImage.cpp +++ /dev/null @@ -1,110 +0,0 @@ -// Copyright 2023 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 "coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp" - -namespace coresense_instrumentation_driver -{ - -template class coresense_instrumentation_driver::InstrumentationLifecycleNode; - -InstrumentationLifecycleNode::InstrumentationLifecycleNode( - const rclcpp::NodeOptions & options) -: rclcpp_lifecycle::LifecycleNode("image_node", "", options) -{ - declare_parameter("topic", std::string("")); - declare_parameter("topic_type", std::string("")); - - get_parameter("topic", topic_); - get_parameter("topic_type", topic_type_); - - node_ = rclcpp::Node::make_shared("subnode"); - - RCLCPP_INFO(get_logger(), "Creating InstrumentationImage"); -} - -InstrumentationLifecycleNode::~InstrumentationLifecycleNode() -{ - RCLCPP_DEBUG(get_logger(), "Destroying InstrumentationImage"); -} - -void InstrumentationLifecycleNode::imageCallback( - const sensor_msgs::msg::Image::ConstSharedPtr & msg) -{ - if (pub_ && pub_.getNumSubscribers() > 0) { - pub_.publish(msg); - } -} - -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -InstrumentationLifecycleNode::on_configure(const rclcpp_lifecycle::State &) -{ - auto subscription_options = rclcpp::SubscriptionOptions(); - - sub_ = image_transport::create_subscription( - node_.get(), - topic_, - std::bind( - &InstrumentationLifecycleNode::imageCallback, this, - std::placeholders::_1), - "raw", - rmw_qos_profile_sensor_data, - subscription_options); - - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; -} - -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -InstrumentationLifecycleNode::on_activate(const rclcpp_lifecycle::State &) -{ - image_transport::ImageTransport it(node_); - pub_ = it.advertise("/coresense" + topic_, 1); - - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; -} - -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -InstrumentationLifecycleNode::on_deactivate( - const rclcpp_lifecycle::State &) -{ - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; -} - -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -InstrumentationLifecycleNode::on_cleanup(const rclcpp_lifecycle::State &) -{ - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; -} - -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -InstrumentationLifecycleNode::on_shutdown(const rclcpp_lifecycle::State &) -{ - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; -} - -std::string InstrumentationLifecycleNode::get_topic() -{ - return topic_; -} - -std::string InstrumentationLifecycleNode::get_topic_type() -{ - return topic_type_; -} - -} // namespace coresense_instrumentation_driver - -#include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE( - coresense_instrumentation_driver::InstrumentationLifecycleNode) diff --git a/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationProducerGeneric.cpp b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationProducerGeneric.cpp new file mode 100644 index 0000000..91b8e41 --- /dev/null +++ b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationProducerGeneric.cpp @@ -0,0 +1,277 @@ +// Copyright 2023 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 "coresense_instrumentation_driver/InstrumentationProducer.hpp" + + +namespace coresense_instrumentation_driver +{ + +template class coresense_instrumentation_driver::InstrumentationProducer; +template class coresense_instrumentation_driver::InstrumentationProducer; +template class coresense_instrumentation_driver::InstrumentationProducer; + +template +InstrumentationProducer::InstrumentationProducer( + const rclcpp::NodeOptions & options) +: rclcpp_lifecycle::LifecycleNode("lifecycle_node", "", options) +{ + declare_parameter("topic", std::string("")); + declare_parameter("topic_type", std::string("")); + + get_parameter("topic", topic_); + get_parameter("topic_type", topic_type_); + + status_pub_ = this->create_publisher( + "/status", 10); + + status_timer_ = this->create_wall_timer( + std::chrono::seconds(1), + [this]() { + publish_status(); + }); + + RCLCPP_DEBUG(get_logger(), "Creating InstrumentationGeneric"); +} + +template +InstrumentationProducer::~InstrumentationProducer() +{ + RCLCPP_DEBUG(get_logger(), "Destroying InstrumentationGeneric"); +} + +template +std::string InstrumentationProducer::get_topic() +{ + return topic_; +} + +template +std::string InstrumentationProducer::get_topic_type() +{ + return topic_type_; +} + +template +void InstrumentationProducer::publish_status() +{ + auto status_msg = std::make_unique(); + auto lifecycle_state = get_current_state(); + + if (std::string(this->get_namespace()) == "/") { + status_msg->node_name = this->get_name(); + } else { + status_msg->node_name = std::string(this->get_namespace()) + "/" + this->get_name(); + } + + status_msg->state = lifecycle_state.id(); + status_msg->stamp = this->now(); + + for (const auto & entry : publishers_) { + status_msg->topics.push_back(entry.first); + } + + int status; + char * demangled_name = abi::__cxa_demangle(typeid(TopicT).name(), nullptr, nullptr, &status); + std::string result(demangled_name); + + size_t pos = result.find('<'); + if (pos != std::string::npos) { + result = result.substr(0, pos); + } + + size_t last_underscore = result.rfind('_'); + if (last_underscore != std::string::npos) { + result = result.substr(0, last_underscore); + } + + std::free(demangled_name); + + status_msg->type = result; + + status_pub_->publish(std::move(status_msg)); +} + +template +typename rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +InstrumentationProducer::on_configure(const rclcpp_lifecycle::State &) +{ + sub_ = this->create_subscription( + topic_, 10, + [this](const typename TopicT::SharedPtr msg) { + for (auto & pub : publishers_) { + if (pub.second->get_subscription_count() > 0 && pub.second->is_activated()) { + pub.second->publish(std::make_unique(*msg)); + } + } + }); + + if (topic_[0] == '/') { + topic_ = topic_.substr(1); + } + + std::string topic; + + if (std::string(this->get_namespace()) == "/") { + topic = "/coresense/" + topic_; + } else { + topic = std::string(this->get_namespace()) + "/coresense/" + topic_; + } + + auto pub = this->create_publisher(topic, 10); + publishers_.insert({topic, pub}); + + std::string create_service, delete_service; + + if (std::string(this->get_namespace()) == "/") { + create_service = std::string(this->get_name()) + "/create_publisher"; + delete_service = std::string(this->get_name()) + "/delete_publisher"; + } else { + create_service = std::string(this->get_namespace()) + "/" + this->get_name() + + "/create_publisher"; + delete_service = std::string(this->get_namespace()) + "/" + this->get_name() + + "/delete_publisher"; + } + + create_publisher_service_ = + this->create_service( + create_service, + std::bind( + &InstrumentationProducer::handleCreatePublisherRequest, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + delete_publisher_service_ = + this->create_service( + delete_service, + std::bind( + &InstrumentationProducer::handleDeletePublisherRequest, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +template +typename rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +InstrumentationProducer::on_activate(const rclcpp_lifecycle::State &) +{ + for (auto & pub : publishers_) { + pub.second->on_activate(); + } + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +template +typename rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +InstrumentationProducer::on_deactivate(const rclcpp_lifecycle::State &) +{ + for (auto & pub : publishers_) { + pub.second->on_deactivate(); + } + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +template +typename rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +InstrumentationProducer::on_cleanup(const rclcpp_lifecycle::State &) +{ + for (auto & pub : publishers_) { + pub.second.reset(); + } + + publishers_.clear(); + + sub_.reset(); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +template +typename rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +InstrumentationProducer::on_shutdown(const rclcpp_lifecycle::State &) +{ + for (auto & pub : publishers_) { + pub.second.reset(); + } + + sub_.reset(); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +template +void InstrumentationProducer::handleCreatePublisherRequest( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + + std::string new_topic = request->topic_name; + + if (new_topic[0] == '/') { + new_topic = new_topic.substr(1); + } + + for (auto & pub : publishers_) { + if (pub.first == new_topic) { + response->success = false; + return; + } + } + + auto new_pub = this->create_publisher("/coresense/" + new_topic, 10); + + if (this->get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { + new_pub->on_activate(); + } + + publishers_.insert({new_topic, new_pub}); + response->success = true; +} + +template +void InstrumentationProducer::handleDeletePublisherRequest( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + + std::string remove_topic = request->topic_name; + + if (remove_topic[0] == '/') { + remove_topic = remove_topic.substr(1); + } + + for (auto & pub : publishers_) { + if (pub.first == remove_topic) { + pub.second.reset(); + } + } + + publishers_.erase(remove_topic); + response->success = true; +} + +} // namespace coresense_instrumentation_driver + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE( + coresense_instrumentation_driver::InstrumentationProducer) +RCLCPP_COMPONENTS_REGISTER_NODE( + coresense_instrumentation_driver::InstrumentationProducer) +RCLCPP_COMPONENTS_REGISTER_NODE( + coresense_instrumentation_driver::InstrumentationProducer) diff --git a/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationProducerImage.cpp b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationProducerImage.cpp new file mode 100644 index 0000000..7fa1461 --- /dev/null +++ b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationProducerImage.cpp @@ -0,0 +1,211 @@ +// Copyright 2023 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 "coresense_instrumentation_driver/InstrumentationProducer.hpp" + +namespace coresense_instrumentation_driver +{ + +template class coresense_instrumentation_driver::InstrumentationProducer; + +InstrumentationProducer::InstrumentationProducer( + const rclcpp::NodeOptions & options) +: rclcpp_lifecycle::LifecycleNode("image_node", "", options) +{ + declare_parameter("topic", std::string("")); + declare_parameter("topic_type", std::string("")); + + get_parameter("topic", topic_); + get_parameter("topic_type", topic_type_); + + node_ = rclcpp::Node::make_shared("subnode"); + + status_pub_ = this->create_publisher( + "/status", 10); + + status_timer_ = this->create_wall_timer( + std::chrono::seconds(1), + [this]() { + publish_status(); + }); + + RCLCPP_INFO(get_logger(), "Creating InstrumentationImage"); +} + +InstrumentationProducer::~InstrumentationProducer() +{ + RCLCPP_DEBUG(get_logger(), "Destroying InstrumentationImage"); +} + +void InstrumentationProducer::publish_status() +{ + auto status_msg = std::make_unique(); + auto lifecycle_state = get_current_state(); + + if (std::string(this->get_namespace()) == "/") { + status_msg->node_name = get_name(); + } else { + status_msg->node_name = std::string(this->get_namespace()) + "/" + get_name(); + } + + status_msg->state = lifecycle_state.id(); + status_msg->stamp = this->now(); + + status_pub_->publish(std::move(status_msg)); +} + +void InstrumentationProducer::imageCallback( + const sensor_msgs::msg::Image::ConstSharedPtr & msg) +{ + for (auto & pub : publishers_) { + if (pub.second.getNumSubscribers() > 0) { + pub.second.publish(msg); + } + } +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +InstrumentationProducer::on_configure(const rclcpp_lifecycle::State &) +{ + auto subscription_options = rclcpp::SubscriptionOptions(); + + sub_ = image_transport::create_subscription( + node_.get(), + topic_, + std::bind( + &InstrumentationProducer::imageCallback, this, + std::placeholders::_1), + "raw", + rmw_qos_profile_sensor_data, + subscription_options); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +InstrumentationProducer::on_activate(const rclcpp_lifecycle::State &) +{ + image_transport::ImageTransport it(node_); + std::string topic; + std::string create_service, delete_service; + + if (std::string(this->get_namespace()) == "/") { + create_service = std::string(this->get_name()) + "/create_publisher"; + delete_service = std::string(this->get_name()) + "/delete_publisher"; + } else { + create_service = std::string(this->get_namespace()) + "/" + this->get_name() + + "/create_publisher"; + delete_service = std::string(this->get_namespace()) + "/" + this->get_name() + + "/delete_publisher"; + } + + auto pub = it.advertise(topic, 1); + publishers_.insert({topic, pub}); + + create_publisher_service_ = + this->create_service( + create_service, + std::bind( + &InstrumentationProducer::handleCreatePublisherRequest, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + delete_publisher_service_ = + this->create_service( + delete_service, + std::bind( + &InstrumentationProducer::handleDeletePublisherRequest, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +InstrumentationProducer::on_deactivate( + const rclcpp_lifecycle::State &) +{ + publishers_.erase(topic_); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +InstrumentationProducer::on_cleanup(const rclcpp_lifecycle::State &) +{ + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +InstrumentationProducer::on_shutdown(const rclcpp_lifecycle::State &) +{ + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +std::string InstrumentationProducer::get_topic() +{ + return topic_; +} + +std::string InstrumentationProducer::get_topic_type() +{ + return topic_type_; +} + +void InstrumentationProducer::handleCreatePublisherRequest( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + + std::string new_topic = request->topic_name; + + if (new_topic[0] == '/') { + new_topic = new_topic.substr(1); + } + + for (auto & pub : publishers_) { + if (pub.first == new_topic) { + response->success = false; + return; + } + } + + image_transport::ImageTransport it(node_); + auto new_pub = it.advertise(std::string(this->get_namespace()) + "/coresense/" + new_topic, 1); + publishers_.insert({new_topic, new_pub}); + response->success = true; +} + +void InstrumentationProducer::handleDeletePublisherRequest( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + + std::string remove_topic = request->topic_name; + + if (remove_topic[0] == '/') { + remove_topic = remove_topic.substr(1); + } + + publishers_.erase(remove_topic); + response->success = true; +} + +} // namespace coresense_instrumentation_driver + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE( + coresense_instrumentation_driver::InstrumentationProducer) diff --git a/coresense_instrumentation_driver/src/coresense_instrumentation_driver_node.cpp b/coresense_instrumentation_driver/src/coresense_instrumentation_driver_node.cpp deleted file mode 100644 index 3dbc20a..0000000 --- a/coresense_instrumentation_driver/src/coresense_instrumentation_driver_node.cpp +++ /dev/null @@ -1,54 +0,0 @@ -// Copyright 2023 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 "coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp" - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - - if (argc < 3) { - RCLCPP_INFO(rclcpp::get_logger("main"), "Usage: %s ", argv[0]); - return 1; - } - - std::string topic = argv[1]; - std::string topic_type = argv[2]; - - auto executor = std::make_shared(); - - if (topic_type == "sensor_msgs/msg/LaserScan") { - auto node = - std::make_shared>(); - executor->add_node(node->get_node_base_interface()); - executor->spin(); - } else if (topic_type == "std_msgs/msg/String") { - auto node = - std::make_shared>(); - executor->add_node(node->get_node_base_interface()); - executor->spin(); - } else if (topic_type == "sensor_msgs/msg/Image") { - auto node = - std::make_shared>(); - executor->add_node(node->get_node_base_interface()); - executor->spin(); - } else { - RCLCPP_ERROR(rclcpp::get_logger("main"), "Unknown topic type: %s", topic_type.c_str()); - return 1; - } - - rclcpp::shutdown(); - return 0; -} diff --git a/coresense_instrumentation_driver/test/test_coresense_instrumentation_driver.cpp b/coresense_instrumentation_driver/test/test_coresense_instrumentation_driver.cpp index e5ad660..94ff05c 100644 --- a/coresense_instrumentation_driver/test/test_coresense_instrumentation_driver.cpp +++ b/coresense_instrumentation_driver/test/test_coresense_instrumentation_driver.cpp @@ -4,7 +4,7 @@ #include #include #include -#include "coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp" +#include "coresense_instrumentation_driver/InstrumentationProducer.hpp" class IntegrationTest : public ::testing::Test { @@ -25,7 +25,7 @@ TEST_F(IntegrationTest, LaserScanNodeLifecycle) rclcpp::executors::SingleThreadedExecutor executor; auto node = - std::make_shared>( + std::make_shared>( rclcpp::NodeOptions().append_parameter_override( "topic", "/test_topic").append_parameter_override( @@ -67,7 +67,7 @@ TEST_F(IntegrationTest, StringNodeLifecycle) rclcpp::executors::SingleThreadedExecutor executor; auto node = - std::make_shared>( + std::make_shared>( rclcpp::NodeOptions().append_parameter_override( "topic", "/test_topic").append_parameter_override( @@ -109,7 +109,7 @@ TEST_F(IntegrationTest, ImageNodeLifecycle) rclcpp::executors::SingleThreadedExecutor executor; auto node = - std::make_shared>( + std::make_shared>( rclcpp::NodeOptions().append_parameter_override( "topic", "/test_topic").append_parameter_override( @@ -149,7 +149,7 @@ TEST_F(IntegrationTest, ImageNodeLifecycle) TEST_F(IntegrationTest, GetTopic) { auto node = - std::make_shared>( + std::make_shared>( rclcpp::NodeOptions().append_parameter_override( "topic", "/test_topic").append_parameter_override( @@ -161,7 +161,7 @@ TEST_F(IntegrationTest, GetTopic) TEST_F(IntegrationTest, GetTopicType) { auto node = - std::make_shared>( + std::make_shared>( rclcpp::NodeOptions().append_parameter_override( "topic", "/test_topic").append_parameter_override( diff --git a/coresense_instrumentation_interfaces/CMakeLists.txt b/coresense_instrumentation_interfaces/CMakeLists.txt index 71acc28..36224c4 100644 --- a/coresense_instrumentation_interfaces/CMakeLists.txt +++ b/coresense_instrumentation_interfaces/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.8) +cmake_minimum_required(VERSION 3.5) project(coresense_instrumentation_interfaces) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") @@ -6,19 +6,23 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() # find dependencies +find_package(rosidl_default_generators REQUIRED) find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) +find_package(std_msgs REQUIRED) +find_package(lifecycle_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/NodeInfo.msg" + "srv/CreatePublisher.srv" + "srv/CreateSubscriber.srv" + "srv/DeletePublisher.srv" + "srv/DeleteSubscriber.srv" + DEPENDENCIES std_msgs lifecycle_msgs +) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() endif() diff --git a/coresense_instrumentation_interfaces/msg/NodeInfo.msg b/coresense_instrumentation_interfaces/msg/NodeInfo.msg new file mode 100644 index 0000000..b7c60cc --- /dev/null +++ b/coresense_instrumentation_interfaces/msg/NodeInfo.msg @@ -0,0 +1,5 @@ +string node_name +uint8 state +string type +builtin_interfaces/Time stamp +string[] topics \ No newline at end of file diff --git a/coresense_instrumentation_interfaces/package.xml b/coresense_instrumentation_interfaces/package.xml index f79fca9..44e6fc8 100644 --- a/coresense_instrumentation_interfaces/package.xml +++ b/coresense_instrumentation_interfaces/package.xml @@ -3,11 +3,18 @@ coresense_instrumentation_interfaces 0.0.0 - TODO: Package description + Coresense Instrumentation Interfaces juanca TODO: License declaration ament_cmake + rosidl_default_generators + + std_msgs + lifecycle_msgs + rosidl_default_generators + + rosidl_interface_packages ament_lint_auto ament_lint_common diff --git a/coresense_instrumentation_interfaces/srv/CreatePublisher.srv b/coresense_instrumentation_interfaces/srv/CreatePublisher.srv new file mode 100644 index 0000000..5ea1988 --- /dev/null +++ b/coresense_instrumentation_interfaces/srv/CreatePublisher.srv @@ -0,0 +1,4 @@ +string topic_name + +--- +bool success \ No newline at end of file diff --git a/coresense_instrumentation_interfaces/srv/CreateSubscriber.srv b/coresense_instrumentation_interfaces/srv/CreateSubscriber.srv new file mode 100644 index 0000000..5ea1988 --- /dev/null +++ b/coresense_instrumentation_interfaces/srv/CreateSubscriber.srv @@ -0,0 +1,4 @@ +string topic_name + +--- +bool success \ No newline at end of file diff --git a/coresense_instrumentation_interfaces/srv/DeletePublisher.srv b/coresense_instrumentation_interfaces/srv/DeletePublisher.srv new file mode 100644 index 0000000..5ea1988 --- /dev/null +++ b/coresense_instrumentation_interfaces/srv/DeletePublisher.srv @@ -0,0 +1,4 @@ +string topic_name + +--- +bool success \ No newline at end of file diff --git a/coresense_instrumentation_interfaces/srv/DeleteSubscriber.srv b/coresense_instrumentation_interfaces/srv/DeleteSubscriber.srv new file mode 100644 index 0000000..5ea1988 --- /dev/null +++ b/coresense_instrumentation_interfaces/srv/DeleteSubscriber.srv @@ -0,0 +1,4 @@ +string topic_name + +--- +bool success \ No newline at end of file From 2ca031b7143b0e2a5998f87168892045a9789157 Mon Sep 17 00:00:00 2001 From: Juan Carlos Manzanares Serrano Date: Tue, 30 Jan 2024 14:11:15 +0100 Subject: [PATCH 18/26] Rviz plugin (#5) * Updated NodeInfo.msg * Publish type * Coresense instrumentation panel * Add tab panel & control * Reformated --- .../InstrumentationConsumer.hpp | 1 + .../InstrumentationProducer.hpp | 2 + .../InstrumentationConsumerGeneric.cpp | 5 +- .../InstrumentationProducerGeneric.cpp | 5 +- .../InstrumentationProducerImage.cpp | 25 ++ .../msg/NodeInfo.msg | 1 + coresense_instrumentation_rviz/CMakeLists.txt | 117 ++++++ .../config/coresense_instrumentation.rviz | 192 ++++++++++ .../CoresenseInstrumentationPanel.hpp | 109 ++++++ .../coresense_intrsumentation_rviz.launch.py | 31 ++ coresense_instrumentation_rviz/package.xml | 39 ++ .../plugins_description.xml | 9 + .../src/CoresenseInstrumentationPanel.cpp | 332 ++++++++++++++++++ 13 files changed, 866 insertions(+), 2 deletions(-) create mode 100644 coresense_instrumentation_rviz/CMakeLists.txt create mode 100644 coresense_instrumentation_rviz/config/coresense_instrumentation.rviz create mode 100644 coresense_instrumentation_rviz/include/coresense_instrumentation_rviz/CoresenseInstrumentationPanel.hpp create mode 100644 coresense_instrumentation_rviz/launch/coresense_intrsumentation_rviz.launch.py create mode 100644 coresense_instrumentation_rviz/package.xml create mode 100644 coresense_instrumentation_rviz/plugins_description.xml create mode 100644 coresense_instrumentation_rviz/src/CoresenseInstrumentationPanel.cpp diff --git a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationConsumer.hpp b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationConsumer.hpp index 4e3c3ac..8d4b5af 100644 --- a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationConsumer.hpp +++ b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationConsumer.hpp @@ -82,6 +82,7 @@ class InstrumentationConsumer : public rclcpp_lifecycle::LifecycleNode std::string topic_; std::string topic_type_; + std::string type_; }; } // namespace coresense_instrumentation_driver diff --git a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationProducer.hpp b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationProducer.hpp index cf03e01..3e546a4 100644 --- a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationProducer.hpp +++ b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationProducer.hpp @@ -80,6 +80,7 @@ class InstrumentationProducer : public rclcpp_lifecycle::LifecycleNode std::string topic_; std::string topic_type_; + std::string type_; }; template<> @@ -131,6 +132,7 @@ class InstrumentationProducer: public rclcpp_lifecycle: image_transport::Subscriber sub_; std::string topic_; std::string topic_type_; + std::string type_; }; } // namespace coresense_instrumentation_driver diff --git a/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationConsumerGeneric.cpp b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationConsumerGeneric.cpp index b016b10..08b8401 100644 --- a/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationConsumerGeneric.cpp +++ b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationConsumerGeneric.cpp @@ -29,9 +29,11 @@ InstrumentationConsumer::InstrumentationConsumer( { declare_parameter("topic", std::string("")); declare_parameter("topic_type", std::string("")); + declare_parameter("type", std::string("")); get_parameter("topic", topic_); get_parameter("topic_type", topic_type_); + get_parameter("type", type_); status_pub_ = this->create_publisher( "/status", 10); @@ -98,7 +100,8 @@ void InstrumentationConsumer::publish_status() std::free(demangled_name); - status_msg->type = result; + status_msg->type_msg = result; + status_msg->type = type_; status_pub_->publish(std::move(status_msg)); } diff --git a/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationProducerGeneric.cpp b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationProducerGeneric.cpp index 91b8e41..f8da830 100644 --- a/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationProducerGeneric.cpp +++ b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationProducerGeneric.cpp @@ -29,9 +29,11 @@ InstrumentationProducer::InstrumentationProducer( { declare_parameter("topic", std::string("")); declare_parameter("topic_type", std::string("")); + declare_parameter("type", std::string("")); get_parameter("topic", topic_); get_parameter("topic_type", topic_type_); + get_parameter("type", type_); status_pub_ = this->create_publisher( "/status", 10); @@ -98,7 +100,8 @@ void InstrumentationProducer::publish_status() std::free(demangled_name); - status_msg->type = result; + status_msg->type_msg = result; + status_msg->type = type_; status_pub_->publish(std::move(status_msg)); } diff --git a/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationProducerImage.cpp b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationProducerImage.cpp index 7fa1461..81d1a76 100644 --- a/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationProducerImage.cpp +++ b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationProducerImage.cpp @@ -25,9 +25,11 @@ InstrumentationProducer::InstrumentationProducer( { declare_parameter("topic", std::string("")); declare_parameter("topic_type", std::string("")); + declare_parameter("type", std::string("")); get_parameter("topic", topic_); get_parameter("topic_type", topic_type_); + get_parameter("type", type_); node_ = rclcpp::Node::make_shared("subnode"); @@ -62,6 +64,29 @@ void InstrumentationProducer::publish_status() status_msg->state = lifecycle_state.id(); status_msg->stamp = this->now(); + for (const auto & entry : publishers_) { + status_msg->topics.push_back(entry.first); + } + + int status; + char * demangled_name = abi::__cxa_demangle(typeid(sensor_msgs::msg::Image).name(), nullptr, nullptr, &status); + std::string result(demangled_name); + + size_t pos = result.find('<'); + if (pos != std::string::npos) { + result = result.substr(0, pos); + } + + size_t last_underscore = result.rfind('_'); + if (last_underscore != std::string::npos) { + result = result.substr(0, last_underscore); + } + + std::free(demangled_name); + + status_msg->type_msg = result; + status_msg->type = type_; + status_pub_->publish(std::move(status_msg)); } diff --git a/coresense_instrumentation_interfaces/msg/NodeInfo.msg b/coresense_instrumentation_interfaces/msg/NodeInfo.msg index b7c60cc..db37fb0 100644 --- a/coresense_instrumentation_interfaces/msg/NodeInfo.msg +++ b/coresense_instrumentation_interfaces/msg/NodeInfo.msg @@ -1,5 +1,6 @@ string node_name uint8 state +string type_msg string type builtin_interfaces/Time stamp string[] topics \ No newline at end of file diff --git a/coresense_instrumentation_rviz/CMakeLists.txt b/coresense_instrumentation_rviz/CMakeLists.txt new file mode 100644 index 0000000..ac3a595 --- /dev/null +++ b/coresense_instrumentation_rviz/CMakeLists.txt @@ -0,0 +1,117 @@ +cmake_minimum_required(VERSION 3.5) +project(coresense_instrumentation_rviz) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +# Qt5 boilerplate options from http://doc.qt.io/qt-5/cmake-manual.html +set(CMAKE_INCLUDE_CURRENT_DIR ON) +set(CMAKE_AUTOMOC ON) + +find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(Qt5 REQUIRED COMPONENTS Core Gui Widgets Test Concurrent) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(rviz_common REQUIRED) +find_package(rviz_default_plugins REQUIRED) +find_package(rviz_ogre_vendor REQUIRED) +find_package(rviz_rendering REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(coresense_instrumentation_interfaces REQUIRED) +find_package(lifecycle_msgs REQUIRED) + +set(coresense_instrumentation_rviz_headers_to_moc + include/coresense_instrumentation_rviz/CoresenseInstrumentationPanel.hpp +) + +include_directories( + include +) + +set(library_name ${PROJECT_NAME}) + +add_library(${library_name} SHARED + src/CoresenseInstrumentationPanel.cpp + ${coresense_instrumentation_rviz_headers_to_moc} +) + +set(dependencies + geometry_msgs + pluginlib + Qt5 + rclcpp + rclcpp_lifecycle + rviz_common + rviz_default_plugins + rviz_ogre_vendor + rviz_rendering + std_msgs + tf2_geometry_msgs + coresense_instrumentation_interfaces + lifecycle_msgs +) + +ament_target_dependencies(${library_name} + ${dependencies} +) + +target_include_directories(${library_name} PUBLIC + ${Qt5Widgets_INCLUDE_DIRS} + ${OGRE_INCLUDE_DIRS} +) + +target_link_libraries(${library_name} + rviz_common::rviz_common +) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +# TODO: Make this specific to this project (not rviz default plugins) +target_compile_definitions(${library_name} PRIVATE "RVIZ_DEFAULT_PLUGINS_BUILDING_LIBRARY") + +pluginlib_export_plugin_description_file(rviz_common plugins_description.xml) + +install( + TARGETS ${library_name} + EXPORT ${library_name} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +install( + DIRECTORY include/ + DESTINATION include/ +) + +install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_include_directories(include) +ament_export_targets(${library_name} HAS_LIBRARY_TARGET) +ament_export_dependencies( + Qt5 + rviz_common + geometry_msgs + map_msgs + rclcpp + coresense_instrumentation_interfaces + lifecycle_msgs +) + +ament_package() diff --git a/coresense_instrumentation_rviz/config/coresense_instrumentation.rviz b/coresense_instrumentation_rviz/config/coresense_instrumentation.rviz new file mode 100644 index 0000000..1e8559d --- /dev/null +++ b/coresense_instrumentation_rviz/config/coresense_instrumentation.rviz @@ -0,0 +1,192 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 89 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + Splitter Ratio: 0.5 + Tree Height: 520 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" + - Class: coresense_instrumentation_rviz/Coresense Instrumentation Panel + Name: Coresense Instrumentation Panel +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /coresense/image_raw + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 1.5268104076385498 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.027353262528777122 + Y: 0.12003128975629807 + Z: 0.11110114306211472 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.38539788126945496 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.8103979825973511 + Saved: ~ +Window Geometry: + Coresense Instrumentation Panel: + collapsed: false + Displays: + collapsed: false + Height: 1050 + Hide Left Dock: false + Hide Right Dock: false + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001560000037cfc020000000bfb0000001200530065006c0065006300740069006f006e000000003d000000820000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000029e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000480043006f0072006500730065006e0073006500200049006e0073007400720075006d0065006e0074006100740069006f006e0020005200760069007a002000500061006e0065006c01000002de000000b50000000000000000fb0000001e0043006f0072006500730065006e00730065005f00500061006e0065006c0100000176000001180000000000000000fb0000000a0049006d00610067006501000002e1000000d80000002800ffffff000000010000015d0000037cfc0200000004fb0000003e0043006f0072006500730065006e0073006500200049006e0073007400720075006d0065006e0074006100740069006f006e002000500061006e0065006c010000003d0000037c0000008500fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000251000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004c10000037c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 1920 + Y: 0 diff --git a/coresense_instrumentation_rviz/include/coresense_instrumentation_rviz/CoresenseInstrumentationPanel.hpp b/coresense_instrumentation_rviz/include/coresense_instrumentation_rviz/CoresenseInstrumentationPanel.hpp new file mode 100644 index 0000000..3b43b4b --- /dev/null +++ b/coresense_instrumentation_rviz/include/coresense_instrumentation_rviz/CoresenseInstrumentationPanel.hpp @@ -0,0 +1,109 @@ +// 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 CORESENSE_INSTRUMENTATION_RVIZ__CORESENSE_INSTRUMENTATION_PANEL_HPP_ +#define CORESENSE_INSTRUMENTATION_RVIZ__CORESENSE_INSTRUMENTATION_PANEL_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#undef NO_ERROR + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rviz_common/panel.hpp" +#include "std_msgs/msg/string.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +#include "coresense_instrumentation_interfaces/msg/node_info.hpp" +#include "coresense_instrumentation_interfaces/srv/create_publisher.hpp" +#include "coresense_instrumentation_interfaces/srv/delete_publisher.hpp" +#include "coresense_instrumentation_interfaces/srv/create_subscriber.hpp" +#include "coresense_instrumentation_interfaces/srv/delete_subscriber.hpp" +#include "lifecycle_msgs/srv/get_state.hpp" +#include "lifecycle_msgs/srv/change_state.hpp" + +class QPushButton; + +namespace coresense_instrumentation_rviz +{ + +class CoresensePanel : public rviz_common::Panel +{ + Q_OBJECT + +public: + explicit CoresensePanel(QWidget * parent = 0); + virtual ~CoresensePanel(); + + void onInitialize() override; + +protected: + QVBoxLayout * layout_; + QTreeWidget * tree_widget_; + QLabel * label_info_; + QTabWidget * tab_widget_; + QPushButton * button_activate_; + QPushButton * button_deactivate_; + QPushButton * button_create_; + QPushButton * button_delete_; + QLineEdit * line_edit_topic_; + QTreeWidget * topic_box_; + QLabel * type_label_; + +private slots: + void show_info(QTreeWidgetItem * item); + +private: + void statusCallback(const coresense_instrumentation_interfaces::msg::NodeInfo::SharedPtr msg); + void activate_node(const std::string & node_name); + void deactivate_node(const std::string & node_name); + void change_state(std::string node_name, std::uint8_t transition); + void update_state(QTreeWidgetItem * item, const std::string & node_name, std::uint8_t state); + void check_alive(); + void create_publisher(const std::string & node_name, const std::string & topic_name); + void create_subscriber(const std::string & node_name, const std::string & topic_name); + void delete_publisher(const std::string & node_name, const std::string & topic_name); + void delete_subscriber(const std::string & node_name, const std::string & topic_name); + void create_node_layout(); + + rclcpp::Node::SharedPtr node_; + rclcpp::Subscription::SharedPtr status_sub_; + rclcpp::TimerBase::SharedPtr alive_timer_; + std::map state_map_; + std::map> topic_map_; + std::map time_map_; + std::vector nodes_; + std::thread spin_thread_; +}; + +} // namespace coresense_instrumentation_rviz + +#endif // CORESENSE_INSTRUMENTATION_RVIZ__CORESENSE_INSTRUMENTATION_PANEL_HPP_ diff --git a/coresense_instrumentation_rviz/launch/coresense_intrsumentation_rviz.launch.py b/coresense_instrumentation_rviz/launch/coresense_intrsumentation_rviz.launch.py new file mode 100644 index 0000000..3e8fcf7 --- /dev/null +++ b/coresense_instrumentation_rviz/launch/coresense_intrsumentation_rviz.launch.py @@ -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. + +import os + +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + + return LaunchDescription([ + Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', os.path.join(get_package_share_directory('coresense_instrumentation_rviz'), 'config', 'coresense_instrumentation.rviz')] + ) + ]) diff --git a/coresense_instrumentation_rviz/package.xml b/coresense_instrumentation_rviz/package.xml new file mode 100644 index 0000000..b3708a3 --- /dev/null +++ b/coresense_instrumentation_rviz/package.xml @@ -0,0 +1,39 @@ + + + + coresense_instrumentation_rviz + 0.0.1 + Coresense instrumentation plugin for rviz + Juan Carlos Manzanares Serrano + Apache-2.0 + + ament_cmake + qtbase5-dev + + geometry_msgs + pluginlib + rclcpp + rclcpp_lifecycle + resource_retriever + rviz_common + rviz_default_plugins + rviz_ogre_vendor + rviz_rendering + std_msgs + tf2_geometry_msgs + visualization_msgs + coresense_instrumentation_interfaces + lifecycle_msgs + + libqt5-core + libqt5-gui + libqt5-opengl + libqt5-widgets + + ament_lint_common + ament_lint_auto + + + ament_cmake + + diff --git a/coresense_instrumentation_rviz/plugins_description.xml b/coresense_instrumentation_rviz/plugins_description.xml new file mode 100644 index 0000000..1dc8243 --- /dev/null +++ b/coresense_instrumentation_rviz/plugins_description.xml @@ -0,0 +1,9 @@ + + + + The Nav2 rviz panel. + + + diff --git a/coresense_instrumentation_rviz/src/CoresenseInstrumentationPanel.cpp b/coresense_instrumentation_rviz/src/CoresenseInstrumentationPanel.cpp new file mode 100644 index 0000000..70f1d31 --- /dev/null +++ b/coresense_instrumentation_rviz/src/CoresenseInstrumentationPanel.cpp @@ -0,0 +1,332 @@ +// 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 "coresense_instrumentation_rviz/CoresenseInstrumentationPanel.hpp" + +#include +#include + +#include +#include +#include +#include +#include + +#include "rviz_common/display_context.hpp" +#include "ament_index_cpp/get_package_share_directory.hpp" + +using namespace std::chrono_literals; + +namespace coresense_instrumentation_rviz +{ + +CoresensePanel::CoresensePanel(QWidget * parent) +: Panel(parent) +{ + node_ = rclcpp::Node::make_shared("coresense_instrumentation_rviz_panel"); + + tab_widget_ = new QTabWidget(); + + QWidget * panel_tab = new QWidget(); + + QVBoxLayout * panel_layout = new QVBoxLayout(panel_tab); + + tree_widget_ = new QTreeWidget(); + tree_widget_->setColumnCount(3); + tree_widget_->setHeaderLabels({"Node", "State", "Type"}); + tree_widget_->header()->setSectionResizeMode(0, QHeaderView::ResizeToContents); + tree_widget_->header()->setSectionResizeMode(1, QHeaderView::ResizeToContents); + tree_widget_->header()->setSectionResizeMode(2, QHeaderView::ResizeToContents); + tree_widget_->clear(); + panel_layout->addWidget(tree_widget_); + + label_info_ = new QLabel(); + panel_layout->addWidget(label_info_); + + panel_tab->setLayout(panel_layout); + tab_widget_->addTab(panel_tab, "Panel"); + + layout_ = new QVBoxLayout(); + layout_->addWidget(tab_widget_); + + create_node_layout(); + + connect( + tree_widget_, SIGNAL(itemClicked(QTreeWidgetItem*,int)), this, + SLOT(show_info(QTreeWidgetItem*))); + + setLayout(layout_); + + status_sub_ = node_->create_subscription( + "/status", 10, std::bind(&CoresensePanel::statusCallback, this, std::placeholders::_1)); + + spin_thread_ = std::thread( + [this]() { + rclcpp::spin(node_); + }); +} + +CoresensePanel::~CoresensePanel() +{ + spin_thread_.join(); +} + +void +CoresensePanel::onInitialize() +{ +} + +void CoresensePanel::statusCallback( + const coresense_instrumentation_interfaces::msg::NodeInfo::SharedPtr msg) +{ + auto it = std::find(nodes_.begin(), nodes_.end(), msg->node_name); + time_map_[msg->node_name] = msg->stamp; + + if (it == nodes_.end()) { + nodes_.push_back(msg->node_name); + QTreeWidgetItem * item = new QTreeWidgetItem(tree_widget_); + item->setText(0, QString::fromStdString(msg->node_name)); + item->setText(2, QString::fromStdString(msg->type)); + item->setData(3, Qt::UserRole, QString::fromStdString(msg->type_msg)); + item->setBackground(0, QColor(QColorConstants::Svg::green)); + item->setBackground(1, QColor(QColorConstants::Svg::green)); + item->setBackground(2, QColor(QColorConstants::Svg::green)); + topic_map_[msg->node_name] = msg->topics; + update_state(item, msg->node_name, msg->state); + } else { + if (state_map_[msg->node_name] != msg->state) { + for (int i = 0; i < tree_widget_->topLevelItemCount(); ++i) { + QTreeWidgetItem * item = tree_widget_->topLevelItem(i); + + if (item->foreground(0) != QColor(QColorConstants::Svg::green)) { + item->setBackground(0, QColor(QColorConstants::Svg::green)); + item->setBackground(1, QColor(QColorConstants::Svg::green)); + item->setBackground(2, QColor(QColorConstants::Svg::green)); + } + + if (item->text(0).toStdString() == msg->node_name) { + update_state(item, msg->node_name, msg->state); + break; + } + } + } + + if (topic_map_[msg->node_name] != msg->topics) { + topic_map_[msg->node_name] = msg->topics; + QTreeWidgetItem * selectedItem = tree_widget_->currentItem(); + QTimer::singleShot( + 0, this, [this, selectedItem]() { + show_info(selectedItem); + }); + } + } +} + +void +CoresensePanel::create_node_layout() +{ + QVBoxLayout * layout = new QVBoxLayout(label_info_); + type_label_ = new QLabel(); + layout->addWidget(type_label_); + layout->setAlignment(type_label_, Qt::AlignVCenter); + QHBoxLayout * activateDeactivateLayout = new QHBoxLayout(); + button_activate_ = new QPushButton("Activate"); + button_deactivate_ = new QPushButton("Deactivate"); + activateDeactivateLayout->addWidget(button_activate_); + activateDeactivateLayout->addWidget(button_deactivate_); + layout->addLayout(activateDeactivateLayout); + + line_edit_topic_ = new QLineEdit(); + layout->addWidget(line_edit_topic_); + + QHBoxLayout * createDeleteLayout = new QHBoxLayout(); + button_create_ = new QPushButton("Create"); + button_delete_ = new QPushButton("Delete"); + createDeleteLayout->addWidget(button_create_); + createDeleteLayout->addWidget(button_delete_); + layout->addLayout(createDeleteLayout); + + topic_box_ = new QTreeWidget(); + topic_box_->setColumnCount(1); + topic_box_->setHeaderLabels({"Topic"}); + topic_box_->header()->setSectionResizeMode(0, QHeaderView::ResizeToContents); + + layout->addWidget(topic_box_); + + label_info_->setLayout(layout); +} + +void +CoresensePanel::show_info(QTreeWidgetItem * item) +{ + topic_box_->clear(); + disconnect(button_activate_, &QPushButton::clicked, nullptr, nullptr); + disconnect(button_deactivate_, &QPushButton::clicked, nullptr, nullptr); + disconnect(button_create_, &QPushButton::clicked, nullptr, nullptr); + disconnect(button_delete_, &QPushButton::clicked, nullptr, nullptr); + + std::string node_name = item->text(0).toStdString(); + std::string type = item->text(2).toStdString(); + + type_label_->setText(item->data(3, Qt::UserRole).toString()); + + connect(button_activate_, &QPushButton::clicked, [this, node_name]() {activate_node(node_name);}); + connect( + button_deactivate_, &QPushButton::clicked, [this, node_name]() { + deactivate_node(node_name); + }); + + if (type == "Producer") { + button_create_->setText("Create Publisher"); + button_delete_->setText("Delete Publisher"); + connect( + button_create_, &QPushButton::clicked, [this, node_name]() { + create_publisher(node_name, line_edit_topic_->text().toStdString()); + }); + + connect( + button_delete_, &QPushButton::clicked, [this, node_name]() { + delete_publisher(node_name, line_edit_topic_->text().toStdString()); + }); + } else { + button_create_->setText("Create Subscriber"); + button_delete_->setText("Delete Subscriber"); + connect( + button_create_, &QPushButton::clicked, [this, node_name]() { + create_subscriber(node_name, line_edit_topic_->text().toStdString()); + }); + + connect( + button_delete_, &QPushButton::clicked, [this, node_name]() { + delete_subscriber(node_name, line_edit_topic_->text().toStdString()); + }); + } + + for (const auto & topic : topic_map_[node_name]) { + QTreeWidgetItem * item = new QTreeWidgetItem(topic_box_); + item->setText(0, QString::fromStdString(topic)); + } +} + +void CoresensePanel::update_state( + QTreeWidgetItem * item, const std::string & node_name, + std::uint8_t state) +{ + switch (state) { + case lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN: + item->setText(1, "Unknown"); + break; + case lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED: + item->setText(1, "Unconfigured"); + break; + case lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE: + item->setText(1, "Inactive"); + break; + case lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE: + item->setText(1, "Active"); + break; + case lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED: + item->setText(1, "Finalized"); + break; + default: + item->setText(1, "Unknown State"); + } + + state_map_[node_name] = state; +} + +void CoresensePanel::create_publisher(const std::string & node_name, const std::string & topic_name) +{ + std::string service_name = node_name + "/create_publisher"; + + auto client = node_->create_client( + service_name); + auto request = + std::make_shared(); + request->topic_name = topic_name; + + auto result = client->async_send_request(request); +} + +void CoresensePanel::create_subscriber( + const std::string & node_name, + const std::string & topic_name) +{ + std::string service_name = node_name + "/create_subscriber"; + + auto client = node_->create_client( + service_name); + auto request = + std::make_shared(); + request->topic_name = topic_name; + + auto result = client->async_send_request(request); +} + +void CoresensePanel::delete_publisher(const std::string & node_name, const std::string & topic_name) +{ + std::string service_name = node_name + "/delete_publisher"; + + auto client = node_->create_client( + service_name); + auto request = + std::make_shared(); + request->topic_name = topic_name; + + auto result = client->async_send_request(request); +} + +void CoresensePanel::delete_subscriber( + const std::string & node_name, + const std::string & topic_name) +{ + std::string service_name = node_name + "/delete_subscriber"; + + auto client = node_->create_client( + service_name); + auto request = + std::make_shared(); + request->topic_name = topic_name; + + auto result = client->async_send_request(request); +} + +void CoresensePanel::change_state(std::string node_name, std::uint8_t transition) +{ + auto client = node_->create_client(node_name + "/change_state"); + auto request = std::make_shared(); + request->transition.id = transition; + + if (client->wait_for_service(std::chrono::seconds(1))) { + auto result = client->async_send_request(request); + } +} + +void CoresensePanel::activate_node(const std::string & node_name) +{ + RCLCPP_INFO(node_->get_logger(), "Activating node %s", node_name.c_str()); + change_state(node_name, lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + change_state(node_name, lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); +} + +void CoresensePanel::deactivate_node(const std::string & node_name) +{ + change_state(node_name, lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE); + change_state(node_name, lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP); +} + +} // namespace coresense_instrumentation_rviz + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(coresense_instrumentation_rviz::CoresensePanel, rviz_common::Panel) From 3ca7fc42cc4cd2758dac562dccd84ade87dd4588 Mon Sep 17 00:00:00 2001 From: Juancams Date: Tue, 2 Apr 2024 15:31:09 +0200 Subject: [PATCH 19/26] Solved some bugs --- coresense_instrumentation/package.xml | 2 + ...coresense_instrumentation_driver.launch.py | 10 ++-- .../InstrumentationConsumerGeneric.cpp | 10 +++- .../InstrumentationProducerGeneric.cpp | 12 +++- .../InstrumentationProducerImage.cpp | 25 ++++++-- .../src/CoresenseInstrumentationPanel.cpp | 58 +++++++++++++++++++ 6 files changed, 102 insertions(+), 15 deletions(-) diff --git a/coresense_instrumentation/package.xml b/coresense_instrumentation/package.xml index f185844..bc4a4ca 100644 --- a/coresense_instrumentation/package.xml +++ b/coresense_instrumentation/package.xml @@ -9,7 +9,9 @@ ament_cmake + coresense_instrumentation_interfaces coresense_instrumentation_driver + coresense_instrumentation_rviz ament_cmake diff --git a/coresense_instrumentation_driver/launch/coresense_instrumentation_driver.launch.py b/coresense_instrumentation_driver/launch/coresense_instrumentation_driver.launch.py index 9e373aa..6e8a85b 100644 --- a/coresense_instrumentation_driver/launch/coresense_instrumentation_driver.launch.py +++ b/coresense_instrumentation_driver/launch/coresense_instrumentation_driver.launch.py @@ -19,10 +19,10 @@ def generate_launch_description(): - names = ['scan_raw', 'nav_vel'] - topics = ['/scan_raw', '/nav_vel'] - msgs = ['sensor_msgs::msg::LaserScan', 'geometry_msgs::msg::Twist'] - node_types = ['Consumer', 'Consumer'] + names = ['scan_raw', 'nav_vel', 'image_raw'] + topics = ['/scan_raw', '/nav_vel', '/head_front_camera/rgb/image_raw'] + msgs = ['sensor_msgs::msg::LaserScan', 'geometry_msgs::msg::Twist', 'sensor_msgs::msg::Image'] + node_types = ['Producer', 'Consumer', 'Producer'] ns = '' composable_nodes = [] @@ -33,7 +33,7 @@ def generate_launch_description(): + msg + '>', name=name + '_node', namespace=ns, - parameters=[{'topic': topic, 'topic_type': msg}], + parameters=[{'topic': topic, 'topic_type': msg, 'type': type}], ) composable_nodes.append(composable_node) diff --git a/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationConsumerGeneric.cpp b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationConsumerGeneric.cpp index 08b8401..405edc4 100644 --- a/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationConsumerGeneric.cpp +++ b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationConsumerGeneric.cpp @@ -225,6 +225,12 @@ void InstrumentationConsumer::handleCreateSubscriberRequest( new_topic = new_topic.substr(1); } + if (std::string(this->get_namespace()) == "/") { + new_topic = std::string("/coresense/" + new_topic); + } else { + new_topic = std::string(this->get_namespace()) + "/coresense/" + new_topic; + } + for (auto & sub : subscriptions_) { if (sub.first == new_topic) { response->success = false; @@ -254,8 +260,8 @@ void InstrumentationConsumer::handleDeleteSubscriberRequest( std::string remove_topic = request->topic_name; - if (remove_topic[0] == '/') { - remove_topic = remove_topic.substr(1); + if (remove_topic[0] != '/') { + remove_topic = "/" + remove_topic; } for (auto & sub : subscriptions_) { diff --git a/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationProducerGeneric.cpp b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationProducerGeneric.cpp index f8da830..2b8084f 100644 --- a/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationProducerGeneric.cpp +++ b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationProducerGeneric.cpp @@ -228,6 +228,12 @@ void InstrumentationProducer::handleCreatePublisherRequest( new_topic = new_topic.substr(1); } + if (std::string(this->get_namespace()) == "/") { + new_topic = std::string("/coresense/" + new_topic); + } else { + new_topic = std::string(this->get_namespace()) + "/coresense/" + new_topic; + } + for (auto & pub : publishers_) { if (pub.first == new_topic) { response->success = false; @@ -235,7 +241,7 @@ void InstrumentationProducer::handleCreatePublisherRequest( } } - auto new_pub = this->create_publisher("/coresense/" + new_topic, 10); + auto new_pub = this->create_publisher(new_topic, 10); if (this->get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { new_pub->on_activate(); @@ -255,8 +261,8 @@ void InstrumentationProducer::handleDeletePublisherRequest( std::string remove_topic = request->topic_name; - if (remove_topic[0] == '/') { - remove_topic = remove_topic.substr(1); + if (remove_topic[0] != '/') { + remove_topic = "/" + remove_topic; } for (auto & pub : publishers_) { diff --git a/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationProducerImage.cpp b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationProducerImage.cpp index 81d1a76..d2abe4e 100644 --- a/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationProducerImage.cpp +++ b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationProducerImage.cpp @@ -42,7 +42,7 @@ InstrumentationProducer::InstrumentationProducer( publish_status(); }); - RCLCPP_INFO(get_logger(), "Creating InstrumentationImage"); + RCLCPP_DEBUG(get_logger(), "Creating InstrumentationImage"); } InstrumentationProducer::~InstrumentationProducer() @@ -54,6 +54,7 @@ void InstrumentationProducer::publish_status() { auto status_msg = std::make_unique(); auto lifecycle_state = get_current_state(); + rclcpp::spin_some(node_); if (std::string(this->get_namespace()) == "/") { status_msg->node_name = get_name(); @@ -69,7 +70,8 @@ void InstrumentationProducer::publish_status() } int status; - char * demangled_name = abi::__cxa_demangle(typeid(sensor_msgs::msg::Image).name(), nullptr, nullptr, &status); + char * demangled_name = abi::__cxa_demangle( + typeid(sensor_msgs::msg::Image).name(), nullptr, nullptr, &status); std::string result(demangled_name); size_t pos = result.find('<'); @@ -125,14 +127,20 @@ InstrumentationProducer::on_activate(const rclcpp_lifec std::string topic; std::string create_service, delete_service; + if (topic_[0] == '/') { + topic_ = topic_.substr(1); + } + if (std::string(this->get_namespace()) == "/") { create_service = std::string(this->get_name()) + "/create_publisher"; delete_service = std::string(this->get_name()) + "/delete_publisher"; + topic = "/coresense/" + topic_; } else { create_service = std::string(this->get_namespace()) + "/" + this->get_name() + "/create_publisher"; delete_service = std::string(this->get_namespace()) + "/" + this->get_name() + "/delete_publisher"; + topic = std::string(this->get_namespace()) + "/coresense/" + topic_; } auto pub = it.advertise(topic, 1); @@ -199,6 +207,12 @@ void InstrumentationProducer::handleCreatePublisherRequ new_topic = new_topic.substr(1); } + if (std::string(this->get_namespace()) == "/") { + new_topic = std::string("/coresense/" + new_topic); + } else { + new_topic = std::string(this->get_namespace()) + "/coresense/" + new_topic; + } + for (auto & pub : publishers_) { if (pub.first == new_topic) { response->success = false; @@ -207,7 +221,8 @@ void InstrumentationProducer::handleCreatePublisherRequ } image_transport::ImageTransport it(node_); - auto new_pub = it.advertise(std::string(this->get_namespace()) + "/coresense/" + new_topic, 1); + + auto new_pub = it.advertise(new_topic, 1); publishers_.insert({new_topic, new_pub}); response->success = true; } @@ -221,8 +236,8 @@ void InstrumentationProducer::handleDeletePublisherRequ std::string remove_topic = request->topic_name; - if (remove_topic[0] == '/') { - remove_topic = remove_topic.substr(1); + if (remove_topic[0] != '/') { + remove_topic = "/" + remove_topic; } publishers_.erase(remove_topic); diff --git a/coresense_instrumentation_rviz/src/CoresenseInstrumentationPanel.cpp b/coresense_instrumentation_rviz/src/CoresenseInstrumentationPanel.cpp index 70f1d31..e79fb22 100644 --- a/coresense_instrumentation_rviz/src/CoresenseInstrumentationPanel.cpp +++ b/coresense_instrumentation_rviz/src/CoresenseInstrumentationPanel.cpp @@ -71,6 +71,11 @@ CoresensePanel::CoresensePanel(QWidget * parent) status_sub_ = node_->create_subscription( "/status", 10, std::bind(&CoresensePanel::statusCallback, this, std::placeholders::_1)); + alive_timer_ = + node_->create_wall_timer( + std::chrono::milliseconds(500), + std::bind(&CoresensePanel::check_alive, this)); + spin_thread_ = std::thread( [this]() { rclcpp::spin(node_); @@ -326,6 +331,59 @@ void CoresensePanel::deactivate_node(const std::string & node_name) change_state(node_name, lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP); } +void CoresensePanel::check_alive() +{ + auto current_time = node_->now(); + + for (const auto & entry : time_map_) { + const auto & node_name = entry.first; + const auto & last_update_time = entry.second; + + auto time_difference = current_time - last_update_time; + + if (time_difference.seconds() > 2) { + auto it = std::find(nodes_.begin(), nodes_.end(), node_name); + + if (it != nodes_.end()) { + for (int i = 0; i < tree_widget_->topLevelItemCount(); ++i) { + QTreeWidgetItem * item = tree_widget_->topLevelItem(i); + + if (item->text(0).toStdString() == node_name) { + if (item->foreground(0) != QColor(QColorConstants::Svg::orange)) { + item->setBackground(0, QColor(QColorConstants::Svg::orange)); + item->setBackground(1, QColor(QColorConstants::Svg::orange)); + item->setBackground(2, QColor(QColorConstants::Svg::orange)); + } + + break; + } + } + } + } + + if (time_difference.seconds() > 5) { + auto it = std::find(nodes_.begin(), nodes_.end(), node_name); + + if (it != nodes_.end()) { + for (int i = 0; i < tree_widget_->topLevelItemCount(); ++i) { + QTreeWidgetItem * item = tree_widget_->topLevelItem(i); + if (item->text(0).toStdString() == node_name) { + update_state(item, node_name, lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN); + + if (item->foreground(0) != QColor(QColorConstants::Svg::red)) { + item->setBackground(0, QColor(QColorConstants::Svg::red)); + item->setBackground(1, QColor(QColorConstants::Svg::red)); + item->setBackground(2, QColor(QColorConstants::Svg::red)); + } + + break; + } + } + } + } + } +} + } // namespace coresense_instrumentation_rviz #include // NOLINT From 91596ea2c3e2ac4807418b1ccd0e7192ec33383c Mon Sep 17 00:00:00 2001 From: Juancams Date: Tue, 2 Apr 2024 15:32:36 +0200 Subject: [PATCH 20/26] 1.0.0 Signed-off-by: Juancams --- coresense_instrumentation/package.xml | 2 +- coresense_instrumentation_driver/package.xml | 2 +- coresense_instrumentation_interfaces/package.xml | 2 +- coresense_instrumentation_rviz/package.xml | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/coresense_instrumentation/package.xml b/coresense_instrumentation/package.xml index bc4a4ca..8c96d1c 100644 --- a/coresense_instrumentation/package.xml +++ b/coresense_instrumentation/package.xml @@ -2,7 +2,7 @@ coresense_instrumentation - 0.2.0 + 1.0.0 Coresense Instrumentation Juan Carlos Manzanares Serrano Apache-2.0 diff --git a/coresense_instrumentation_driver/package.xml b/coresense_instrumentation_driver/package.xml index 8f1103a..eaab1e6 100644 --- a/coresense_instrumentation_driver/package.xml +++ b/coresense_instrumentation_driver/package.xml @@ -2,7 +2,7 @@ coresense_instrumentation_driver - 0.2.0 + 1.0.0 Coresense Instrumentation Driver Juan Carlos Manzanares Serrano Apache-2.0 diff --git a/coresense_instrumentation_interfaces/package.xml b/coresense_instrumentation_interfaces/package.xml index 44e6fc8..e04d458 100644 --- a/coresense_instrumentation_interfaces/package.xml +++ b/coresense_instrumentation_interfaces/package.xml @@ -2,7 +2,7 @@ coresense_instrumentation_interfaces - 0.0.0 + 1.0.0 Coresense Instrumentation Interfaces juanca TODO: License declaration diff --git a/coresense_instrumentation_rviz/package.xml b/coresense_instrumentation_rviz/package.xml index b3708a3..d91db05 100644 --- a/coresense_instrumentation_rviz/package.xml +++ b/coresense_instrumentation_rviz/package.xml @@ -2,7 +2,7 @@ coresense_instrumentation_rviz - 0.0.1 + 1.0.0 Coresense instrumentation plugin for rviz Juan Carlos Manzanares Serrano Apache-2.0 From 54f851097bb361199b1f5c81146a737ba0ab28d0 Mon Sep 17 00:00:00 2001 From: Juancams Date: Tue, 2 Apr 2024 19:07:34 +0200 Subject: [PATCH 21/26] Flake8 --- ...coresense_instrumentation_driver.launch.py | 30 ++++++++++++++----- 1 file changed, 22 insertions(+), 8 deletions(-) diff --git a/coresense_instrumentation_driver/launch/coresense_instrumentation_driver.launch.py b/coresense_instrumentation_driver/launch/coresense_instrumentation_driver.launch.py index 6e8a85b..d4faf56 100644 --- a/coresense_instrumentation_driver/launch/coresense_instrumentation_driver.launch.py +++ b/coresense_instrumentation_driver/launch/coresense_instrumentation_driver.launch.py @@ -19,21 +19,35 @@ def generate_launch_description(): - names = ['scan_raw', 'nav_vel', 'image_raw'] - topics = ['/scan_raw', '/nav_vel', '/head_front_camera/rgb/image_raw'] - msgs = ['sensor_msgs::msg::LaserScan', 'geometry_msgs::msg::Twist', 'sensor_msgs::msg::Image'] - node_types = ['Producer', 'Consumer', 'Producer'] + names = ['scan_raw', + 'nav_vel', + 'image_raw'] + + topics = ['/scan_raw', + '/nav_vel', + '/head_front_camera/rgb/image_raw'] + + msgs = ['sensor_msgs::msg::LaserScan', + 'geometry_msgs::msg::Twist', + 'sensor_msgs::msg::Image'] + + node_types = ['Producer', + 'Consumer', + 'Producer'] + ns = '' composable_nodes = [] - for topic, msg, name, type in zip(topics, msgs, names, node_types): + for topic, msg, name, node_type in zip(topics, msgs, names, node_types): composable_node = ComposableNode( package='coresense_instrumentation_driver', - plugin='coresense_instrumentation_driver::Instrumentation' + type + '<' - + msg + '>', + plugin='coresense_instrumentation_driver::Instrumentation' + + node_type + '<' + msg + '>', name=name + '_node', namespace=ns, - parameters=[{'topic': topic, 'topic_type': msg, 'type': type}], + parameters=[{'topic': topic, + 'topic_type': msg, + 'type': node_type}], ) composable_nodes.append(composable_node) From 50afda472c41869872934fe509ac1ad533cc4938 Mon Sep 17 00:00:00 2001 From: Juancams Date: Tue, 2 Apr 2024 19:17:34 +0200 Subject: [PATCH 22/26] xmllint --- coresense_instrumentation_interfaces/package.xml | 3 --- 1 file changed, 3 deletions(-) diff --git a/coresense_instrumentation_interfaces/package.xml b/coresense_instrumentation_interfaces/package.xml index e04d458..b6c91f8 100644 --- a/coresense_instrumentation_interfaces/package.xml +++ b/coresense_instrumentation_interfaces/package.xml @@ -16,9 +16,6 @@ rosidl_interface_packages - ament_lint_auto - ament_lint_common - ament_cmake From 06f7d9591b5e63211fcf3906fbb4071c019397ba Mon Sep 17 00:00:00 2001 From: Juancams Date: Tue, 2 Apr 2024 19:17:46 +0200 Subject: [PATCH 23/26] Flake8 --- .../launch/coresense_intrsumentation_rviz.launch.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/coresense_instrumentation_rviz/launch/coresense_intrsumentation_rviz.launch.py b/coresense_instrumentation_rviz/launch/coresense_intrsumentation_rviz.launch.py index 3e8fcf7..10f4791 100644 --- a/coresense_instrumentation_rviz/launch/coresense_intrsumentation_rviz.launch.py +++ b/coresense_instrumentation_rviz/launch/coresense_intrsumentation_rviz.launch.py @@ -14,9 +14,10 @@ import os +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory + def generate_launch_description(): @@ -26,6 +27,8 @@ def generate_launch_description(): executable='rviz2', name='rviz2', output='screen', - arguments=['-d', os.path.join(get_package_share_directory('coresense_instrumentation_rviz'), 'config', 'coresense_instrumentation.rviz')] + arguments=['-d', + os.path.join(get_package_share_directory('coresense_instrumentation_rviz'), + 'config', 'coresense_instrumentation.rviz')] ) ]) From 20c51cf1a9d307f566ab2b6ca153680111b41483 Mon Sep 17 00:00:00 2001 From: Juancams Date: Tue, 2 Apr 2024 21:22:43 +0200 Subject: [PATCH 24/26] Bug --- .../InstrumentationConsumer.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationConsumer.hpp b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationConsumer.hpp index 8d4b5af..17c79de 100644 --- a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationConsumer.hpp +++ b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationConsumer.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef INSTRUMENTATION_PRODUCER_HPP -#define INSTRUMENTATION_PRODUCER_HPP +#ifndef INSTRUMENTATION_CONSUMER_HPP +#define INSTRUMENTATION_CONSUMER_HPP #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" @@ -87,4 +87,4 @@ class InstrumentationConsumer : public rclcpp_lifecycle::LifecycleNode } // namespace coresense_instrumentation_driver -#endif // INSTRUMENTATION_PRODUCER_HPP +#endif // INSTRUMENTATION_CONSUMER_HPP From 18995c8b8ad8d8770b2026c183ec4ac12c0ce87f Mon Sep 17 00:00:00 2001 From: Juancams Date: Tue, 2 Apr 2024 21:22:54 +0200 Subject: [PATCH 25/26] New tests --- .../test_coresense_instrumentation_driver.cpp | 322 +++++++++++++++++- 1 file changed, 321 insertions(+), 1 deletion(-) diff --git a/coresense_instrumentation_driver/test/test_coresense_instrumentation_driver.cpp b/coresense_instrumentation_driver/test/test_coresense_instrumentation_driver.cpp index 94ff05c..ce23f52 100644 --- a/coresense_instrumentation_driver/test/test_coresense_instrumentation_driver.cpp +++ b/coresense_instrumentation_driver/test/test_coresense_instrumentation_driver.cpp @@ -3,8 +3,13 @@ #include #include #include +#include #include #include "coresense_instrumentation_driver/InstrumentationProducer.hpp" +#include "coresense_instrumentation_driver/InstrumentationConsumer.hpp" +#include "coresense_instrumentation_interfaces/srv/create_publisher.hpp" +#include "coresense_instrumentation_interfaces/srv/delete_publisher.hpp" +#include "coresense_instrumentation_interfaces/msg/node_info.hpp" class IntegrationTest : public ::testing::Test { @@ -104,12 +109,54 @@ TEST_F(IntegrationTest, StringNodeLifecycle) executor.spin_once(std::chrono::seconds(1)); } +TEST_F(IntegrationTest, TwistNodeLifecycle) +{ + rclcpp::executors::SingleThreadedExecutor executor; + + auto node = + std::make_shared>( + rclcpp::NodeOptions().append_parameter_override( + "topic", + "/test_topic").append_parameter_override( + "topic_type", "geometry_msgs::msg::Twist")); + + executor.add_node(node->get_node_base_interface()); + + auto result = node->on_configure(rclcpp_lifecycle::State()); + ASSERT_EQ( + result, + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); + + result = node->on_activate(rclcpp_lifecycle::State()); + ASSERT_EQ( + result, + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); + + node->on_activate(rclcpp_lifecycle::State()); + result = node->on_deactivate(rclcpp_lifecycle::State()); + ASSERT_EQ( + result, + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); + + result = node->on_cleanup(rclcpp_lifecycle::State()); + ASSERT_EQ( + result, + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); + + result = node->on_shutdown(rclcpp_lifecycle::State()); + ASSERT_EQ( + result, + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); + + executor.spin_once(std::chrono::seconds(1)); +} + TEST_F(IntegrationTest, ImageNodeLifecycle) { rclcpp::executors::SingleThreadedExecutor executor; auto node = - std::make_shared>( + std::make_shared>( rclcpp::NodeOptions().append_parameter_override( "topic", "/test_topic").append_parameter_override( @@ -146,6 +193,279 @@ TEST_F(IntegrationTest, ImageNodeLifecycle) executor.spin_once(std::chrono::seconds(1)); } +TEST_F(IntegrationTest, CreateStringPublisher) +{ + rclcpp::executors::SingleThreadedExecutor executor; + + auto node = + std::make_shared>( + rclcpp::NodeOptions().append_parameter_override( + "topic", + "/test_topic").append_parameter_override( + "topic_type", "std_msgs::msg::String")); + + auto node_client = rclcpp::Node::make_shared("node_client"); + + executor.add_node(node->get_node_base_interface()); + executor.add_node(node_client->get_node_base_interface()); + + node->on_configure(rclcpp_lifecycle::State()); + node->on_activate(rclcpp_lifecycle::State()); + + auto client = + node_client->create_client( + "/lifecycle_node/create_publisher"); + client->wait_for_service(); + + auto request = + std::make_shared(); + auto response = + std::make_shared(); + request->topic_name = "/new_test_topic"; + + client->async_send_request( + request, + [&response](rclcpp::Client:: + SharedFuture future) { + if (future.get()) { + response = future.get(); + } + }); + + for (int i = 0; i < 10; i++) { + executor.spin_once(std::chrono::milliseconds(100)); + } + + ASSERT_EQ(response->success, true); +} + +TEST_F(IntegrationTest, DeleteStringPublisher) +{ + rclcpp::executors::SingleThreadedExecutor executor; + + auto node = + std::make_shared>( + rclcpp::NodeOptions().append_parameter_override( + "topic", + "/test_topic").append_parameter_override( + "topic_type", "std_msgs::msg::String")); + + auto node_client = rclcpp::Node::make_shared("node_client"); + + executor.add_node(node->get_node_base_interface()); + executor.add_node(node_client->get_node_base_interface()); + + node->on_configure(rclcpp_lifecycle::State()); + node->on_activate(rclcpp_lifecycle::State()); + + auto client_create = + node_client->create_client( + "/lifecycle_node/create_publisher"); + client_create->wait_for_service(); + + auto request_create = + std::make_shared(); + auto response_create = + std::make_shared(); + request_create->topic_name = "/new_test_topic"; + + client_create->async_send_request( + request_create, + [&response_create](rclcpp::Client:: + SharedFuture future) { + if (future.get()) { + response_create = future.get(); + } + }); + + for (int i = 0; i < 10; i++) { + executor.spin_once(std::chrono::milliseconds(100)); + } + + ASSERT_EQ(response_create->success, true); + + auto request_delete = + std::make_shared(); + auto response_delete = + std::make_shared(); + + auto client_delete = + node_client->create_client( + "/lifecycle_node/delete_publisher"); + client_delete->wait_for_service(); + + request_delete->topic_name = "/coresense/new_test_topic"; + + client_delete->async_send_request( + request_delete, + [&response_delete](rclcpp::Client:: + SharedFuture future) { + if (future.get()) { + response_delete = future.get(); + } + }); + + for (int i = 0; i < 10; i++) { + executor.spin_once(std::chrono::milliseconds(100)); + } + + ASSERT_EQ(response_delete->success, true); +} + +TEST_F(IntegrationTest, CreateTwistSubscription) +{ + rclcpp::executors::SingleThreadedExecutor executor; + + auto node = + std::make_shared>( + rclcpp::NodeOptions().append_parameter_override( + "topic", + "/test_topic").append_parameter_override( + "topic_type", "geometry_msgs::msg::Twist")); + + auto node_client = rclcpp::Node::make_shared("node_client"); + + executor.add_node(node->get_node_base_interface()); + executor.add_node(node_client->get_node_base_interface()); + + node->on_configure(rclcpp_lifecycle::State()); + node->on_activate(rclcpp_lifecycle::State()); + + auto client = + node_client->create_client( + "/lifecycle_node/create_subscriber"); + client->wait_for_service(); + + auto request = + std::make_shared(); + auto response = + std::make_shared(); + request->topic_name = "/new_test_topic"; + + client->async_send_request( + request, + [&response](rclcpp::Client:: + SharedFuture future) { + if (future.get()) { + response = future.get(); + } + }); + + for (int i = 0; i < 10; i++) { + executor.spin_once(std::chrono::milliseconds(100)); + } + + ASSERT_EQ(response->success, true); +} + +TEST_F(IntegrationTest, DeleteTwistSubscription) +{ + rclcpp::executors::SingleThreadedExecutor executor; + + auto node = + std::make_shared>( + rclcpp::NodeOptions().append_parameter_override( + "topic", + "/test_topic").append_parameter_override( + "topic_type", "geometry_msgs::msg::Twist")); + + auto node_client = rclcpp::Node::make_shared("node_client"); + + executor.add_node(node->get_node_base_interface()); + executor.add_node(node_client->get_node_base_interface()); + + node->on_configure(rclcpp_lifecycle::State()); + node->on_activate(rclcpp_lifecycle::State()); + + auto client_create = + node_client->create_client( + "/lifecycle_node/create_subscriber"); + client_create->wait_for_service(); + + auto request_create = + std::make_shared(); + auto response_create = + std::make_shared(); + request_create->topic_name = "/new_test_topic"; + + client_create->async_send_request( + request_create, + [&response_create](rclcpp::Client:: + SharedFuture future) { + if (future.get()) { + response_create = future.get(); + } + }); + + for (int i = 0; i < 10; i++) { + executor.spin_once(std::chrono::milliseconds(100)); + } + + ASSERT_EQ(response_create->success, true); + + auto request_delete = + std::make_shared(); + auto response_delete = + std::make_shared(); + + auto client_delete = + node_client->create_client( + "/lifecycle_node/delete_subscriber"); + client_delete->wait_for_service(); + + request_delete->topic_name = "/coresense/new_test_topic"; + + client_delete->async_send_request( + request_delete, + [&response_delete](rclcpp::Client:: + SharedFuture future) { + if (future.get()) { + response_delete = future.get(); + } + }); + + for (int i = 0; i < 10; i++) { + executor.spin_once(std::chrono::milliseconds(100)); + } + + ASSERT_EQ(response_delete->success, true); +} + +TEST_F(IntegrationTest, GetStatus) +{ + rclcpp::executors::SingleThreadedExecutor executor; + + auto node = + std::make_shared>( + rclcpp::NodeOptions().append_parameter_override( + "topic", + "/test_topic").append_parameter_override( + "topic_type", "std_msgs::msg::String"). + append_parameter_override("type", "Producer")); + + auto node_sub = rclcpp::Node::make_shared("node_sub"); + + executor.add_node(node->get_node_base_interface()); + executor.add_node(node_sub->get_node_base_interface()); + + auto node_info = std::make_shared(); + + auto sub = node_sub->create_subscription( + "/status", 10, + [&node_info](const coresense_instrumentation_interfaces::msg::NodeInfo::SharedPtr msg) { + node_info = std::move(msg); + }); + + for (int i = 0; i < 30; i++) { + executor.spin_once(std::chrono::milliseconds(100)); + } + + ASSERT_EQ(node_info->node_name, "lifecycle_node"); + ASSERT_EQ(node_info->type, "Producer"); + ASSERT_EQ(node_info->state, 1); + ASSERT_EQ(node_info->type_msg, "std_msgs::msg::String"); +} + TEST_F(IntegrationTest, GetTopic) { auto node = From 08fcce299ba8d403fb507266e92da32259ce354f Mon Sep 17 00:00:00 2001 From: Juan Carlos Manzanares Serrano Date: Tue, 2 Apr 2024 21:51:21 +0200 Subject: [PATCH 26/26] Update README.md --- README.md | 86 +++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 86 insertions(+) diff --git a/README.md b/README.md index 762ced9..6c3886b 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,90 @@ # Coresense Instrumentation +![distro](https://img.shields.io/badge/Ubuntu%2022-Jammy%20Jellyfish-green) +![distro](https://img.shields.io/badge/ROS2-Humble-blue) [![main](https://github.com/Juancams/coresense_instrumentation/actions/workflows/main.yaml/badge.svg?branch=main)](https://github.com/Juancams/coresense_instrumentation/actions/workflows/main.yaml) [![codecov](https://codecov.io/gh/Juancams/coresense_instrumentation/graph/badge.svg?token=EvUIoImzzh)](https://codecov.io/gh/Juancams/coresense_instrumentation) + +## Installation + +```bash +cd ~/ros2_ws/src +git clone https://github.com/CoreSenseEU/coresense_instrumentation +cd ~/ros2_ws +colcon build --symlink-install --packages-up-to coresense_instrumention +``` + +## Usage +### Coresense Instrumentation Driver +
+Click to expand + +```python +def generate_launch_description(): + + names = ['scan_raw', + 'nav_vel', + 'image_raw'] + + topics = ['/scan_raw', + '/nav_vel', + '/head_front_camera/rgb/image_raw'] + + msgs = ['sensor_msgs::msg::LaserScan', + 'geometry_msgs::msg::Twist', + 'sensor_msgs::msg::Image'] + + node_types = ['Producer', + 'Consumer', + 'Producer'] + + ns = '' + + composable_nodes = [] + for topic, msg, name, node_type in zip(topics, msgs, names, node_types): + composable_node = ComposableNode( + package='coresense_instrumentation_driver', + plugin='coresense_instrumentation_driver::Instrumentation' + + node_type + '<' + msg + '>', + name=name + '_node', + namespace=ns, + parameters=[{'topic': topic, + 'topic_type': msg, + 'type': node_type}], + ) + composable_nodes.append(composable_node) + + container = ComposableNodeContainer( + name='coresense_container', + namespace=ns, + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=composable_nodes, + output='screen', + ) +``` +You have to indicate the name you will give to your nodes, the topic you subscribe to, the type of message and whether you are going to subscribe or publish to the topic. + +
+ +```shell +ros2 launch coresense_instrumentation_driver coresense_instrumentation_driver.launch.py +``` + +### Coresense Instrumentation Rviz +```shell +ros2 launch coresense_instrumentation_rviz coresense_instrumentation_rviz.launch.py +``` + +
+Click to expand + + +![image](https://github.com/Juancams/coresense_instrumentation/assets/44479765/fc2403ed-f39d-4b4d-ac86-57477d56342e) +![image](https://github.com/Juancams/coresense_instrumentation/assets/44479765/186e220d-ec4b-4071-9ecd-bc408d6e9725) + +When you launch rviz, you will see on your right a panel like this, but with your nodes. Through the following buttons, you can activate/deactivate your nodes and create/delete your publishers/subscribers +
+ +## Demo +[instrumentation_driver_demo](https://github.com/Juancams/coresense_instrumentation/assets/44479765/e6cada5c-5071-4a41-b226-5dc5c18a37aa)