From 1567c13155c728378ac74f71dc121fffeb1ee732 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 15 Oct 2019 18:21:48 -0300 Subject: [PATCH] Refactor intra-process demo into components. Signed-off-by: Michel Hidalgo --- intra_process_demo/CMakeLists.txt | 33 +++++++-- .../image_pipeline/camera_node.hpp | 6 +- .../image_pipeline/common.hpp | 6 +- .../image_pipeline/image_view_node.hpp | 6 +- .../image_pipeline/watermark_node.hpp | 6 +- .../two_node_pipeline/consumer_component.hpp | 46 +++++++++++++ .../two_node_pipeline/producer_component.hpp | 47 +++++++++++++ .../intra_process_demo/visibility_control.h | 35 ++++++++++ .../launch/two_node_pipeline_launch.py | 45 ++++++++++++ intra_process_demo/package.xml | 5 +- .../src/image_pipeline/camera_node.cpp | 2 +- .../image_pipeline_all_in_one.cpp | 6 +- .../image_pipeline_with_two_image_view.cpp | 6 +- .../src/image_pipeline/image_view_node.cpp | 2 +- .../src/image_pipeline/watermark_node.cpp | 2 +- .../two_node_pipeline/consumer_component.cpp | 56 +++++++++++++++ .../two_node_pipeline/producer_component.cpp | 68 +++++++++++++++++++ .../two_node_pipeline/two_node_pipeline.cpp | 62 ++--------------- 18 files changed, 357 insertions(+), 82 deletions(-) rename intra_process_demo/include/{ => intra_process_demo}/image_pipeline/camera_node.hpp (95%) rename intra_process_demo/include/{ => intra_process_demo}/image_pipeline/common.hpp (92%) rename intra_process_demo/include/{ => intra_process_demo}/image_pipeline/image_view_node.hpp (92%) rename intra_process_demo/include/{ => intra_process_demo}/image_pipeline/watermark_node.hpp (92%) create mode 100644 intra_process_demo/include/intra_process_demo/two_node_pipeline/consumer_component.hpp create mode 100644 intra_process_demo/include/intra_process_demo/two_node_pipeline/producer_component.hpp create mode 100644 intra_process_demo/include/intra_process_demo/visibility_control.h create mode 100644 intra_process_demo/launch/two_node_pipeline_launch.py create mode 100644 intra_process_demo/src/two_node_pipeline/consumer_component.cpp create mode 100644 intra_process_demo/src/two_node_pipeline/producer_component.cpp diff --git a/intra_process_demo/CMakeLists.txt b/intra_process_demo/CMakeLists.txt index 192ae40c0..27a4009d4 100644 --- a/intra_process_demo/CMakeLists.txt +++ b/intra_process_demo/CMakeLists.txt @@ -13,6 +13,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(rmw REQUIRED) find_package(sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) @@ -27,13 +28,27 @@ ament_export_include_directories(include) ## # Simple example of using unique_ptr to minimize copies. + +add_library(two_node_pipeline_components SHARED + src/two_node_pipeline/producer_component.cpp + src/two_node_pipeline/consumer_component.cpp +) +target_compile_definitions(two_node_pipeline_components + PRIVATE "INTRA_PROCESS_DEMO_BUILDING_DLL") +ament_target_dependencies(two_node_pipeline_components + "rclcpp" "rclcpp_components" "rcutils" "std_msgs") + +rclcpp_components_register_nodes(two_node_pipeline_components + "intra_process_demo::two_node_pipeline::Producer" + "intra_process_demo::two_node_pipeline::Consumer") + add_executable(two_node_pipeline src/two_node_pipeline/two_node_pipeline.cpp) -ament_target_dependencies(two_node_pipeline - "rclcpp" - "std_msgs") +target_link_libraries(two_node_pipeline + two_node_pipeline_components) - # Simple example of a cyclic pipeline which uses no allocation while iterating. + +# Simple example of a cyclic pipeline which uses no allocation while iterating. add_executable(cyclic_pipeline src/cyclic_pipeline/cyclic_pipeline.cpp) ament_target_dependencies(cyclic_pipeline @@ -80,6 +95,11 @@ ament_target_dependencies(image_view_node "sensor_msgs" "OpenCV") +install(TARGETS + two_node_pipeline_components + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib) + install(TARGETS two_node_pipeline cyclic_pipeline @@ -95,6 +115,11 @@ install( DESTINATION include ) +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME} +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() diff --git a/intra_process_demo/include/image_pipeline/camera_node.hpp b/intra_process_demo/include/intra_process_demo/image_pipeline/camera_node.hpp similarity index 95% rename from intra_process_demo/include/image_pipeline/camera_node.hpp rename to intra_process_demo/include/intra_process_demo/image_pipeline/camera_node.hpp index 344def872..926adb6be 100644 --- a/intra_process_demo/include/image_pipeline/camera_node.hpp +++ b/intra_process_demo/include/intra_process_demo/image_pipeline/camera_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef IMAGE_PIPELINE__CAMERA_NODE_HPP_ -#define IMAGE_PIPELINE__CAMERA_NODE_HPP_ +#ifndef INTRA_PROCESS_DEMO__IMAGE_PIPELINE__CAMERA_NODE_HPP_ +#define INTRA_PROCESS_DEMO__IMAGE_PIPELINE__CAMERA_NODE_HPP_ #include #include @@ -110,4 +110,4 @@ class CameraNode : public rclcpp::Node cv::Mat frame_; }; -#endif // IMAGE_PIPELINE__CAMERA_NODE_HPP_ +#endif // INTRA_PROCESS_DEMO__IMAGE_PIPELINE__CAMERA_NODE_HPP_ diff --git a/intra_process_demo/include/image_pipeline/common.hpp b/intra_process_demo/include/intra_process_demo/image_pipeline/common.hpp similarity index 92% rename from intra_process_demo/include/image_pipeline/common.hpp rename to intra_process_demo/include/intra_process_demo/image_pipeline/common.hpp index f81e6e535..6b4e3d2a6 100644 --- a/intra_process_demo/include/image_pipeline/common.hpp +++ b/intra_process_demo/include/intra_process_demo/image_pipeline/common.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef IMAGE_PIPELINE__COMMON_HPP_ -#define IMAGE_PIPELINE__COMMON_HPP_ +#ifndef INTRA_PROCESS_DEMO__IMAGE_PIPELINE__COMMON_HPP_ +#define INTRA_PROCESS_DEMO__IMAGE_PIPELINE__COMMON_HPP_ #ifdef _WIN32 #include @@ -85,4 +85,4 @@ draw_on_image(cv::Mat & image, const std::string & text, int height) cv::Scalar(0, 255, 0)); } -#endif // IMAGE_PIPELINE__COMMON_HPP_ +#endif // INTRA_PROCESS_DEMO__IMAGE_PIPELINE__COMMON_HPP_ diff --git a/intra_process_demo/include/image_pipeline/image_view_node.hpp b/intra_process_demo/include/intra_process_demo/image_pipeline/image_view_node.hpp similarity index 92% rename from intra_process_demo/include/image_pipeline/image_view_node.hpp rename to intra_process_demo/include/intra_process_demo/image_pipeline/image_view_node.hpp index 6b340868c..60d632f01 100644 --- a/intra_process_demo/include/image_pipeline/image_view_node.hpp +++ b/intra_process_demo/include/intra_process_demo/image_pipeline/image_view_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef IMAGE_PIPELINE__IMAGE_VIEW_NODE_HPP_ -#define IMAGE_PIPELINE__IMAGE_VIEW_NODE_HPP_ +#ifndef INTRA_PROCESS_DEMO__IMAGE_PIPELINE__IMAGE_VIEW_NODE_HPP_ +#define INTRA_PROCESS_DEMO__IMAGE_PIPELINE__IMAGE_VIEW_NODE_HPP_ #include #include @@ -78,4 +78,4 @@ class ImageViewNode : public rclcpp::Node cv::Mat frame_; }; -#endif // IMAGE_PIPELINE__IMAGE_VIEW_NODE_HPP_ +#endif // INTRA_PROCESS_DEMO__IMAGE_PIPELINE__IMAGE_VIEW_NODE_HPP_ diff --git a/intra_process_demo/include/image_pipeline/watermark_node.hpp b/intra_process_demo/include/intra_process_demo/image_pipeline/watermark_node.hpp similarity index 92% rename from intra_process_demo/include/image_pipeline/watermark_node.hpp rename to intra_process_demo/include/intra_process_demo/image_pipeline/watermark_node.hpp index ddc2a04dc..51ba43544 100644 --- a/intra_process_demo/include/image_pipeline/watermark_node.hpp +++ b/intra_process_demo/include/intra_process_demo/image_pipeline/watermark_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef IMAGE_PIPELINE__WATERMARK_NODE_HPP_ -#define IMAGE_PIPELINE__WATERMARK_NODE_HPP_ +#ifndef INTRA_PROCESS_DEMO__IMAGE_PIPELINE__WATERMARK_NODE_HPP_ +#define INTRA_PROCESS_DEMO__IMAGE_PIPELINE__WATERMARK_NODE_HPP_ #include #include @@ -69,4 +69,4 @@ class WatermarkNode : public rclcpp::Node cv::Mat frame_; }; -#endif // IMAGE_PIPELINE__WATERMARK_NODE_HPP_ +#endif // INTRA_PROCESS_DEMO__IMAGE_PIPELINE__WATERMARK_NODE_HPP_ diff --git a/intra_process_demo/include/intra_process_demo/two_node_pipeline/consumer_component.hpp b/intra_process_demo/include/intra_process_demo/two_node_pipeline/consumer_component.hpp new file mode 100644 index 000000000..8f240b0ff --- /dev/null +++ b/intra_process_demo/include/intra_process_demo/two_node_pipeline/consumer_component.hpp @@ -0,0 +1,46 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// 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 INTRA_PROCESS_DEMO__TWO_NODE_PIPELINE__CONSUMER_COMPONENT_HPP_ +#define INTRA_PROCESS_DEMO__TWO_NODE_PIPELINE__CONSUMER_COMPONENT_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/int32.hpp" + +#include "intra_process_demo/visibility_control.h" + +namespace intra_process_demo +{ +namespace two_node_pipeline +{ + +class Consumer : public rclcpp::Node +{ +public: + INTRA_PROCESS_DEMO_PUBLIC + explicit Consumer(const rclcpp::NodeOptions & options); + + explicit Consumer( + const std::string & name, + const std::string & input, + const rclcpp::NodeOptions & options); + +private: + rclcpp::Subscription::SharedPtr sub_; +}; + +} // namespace two_node_pipeline +} // namespace intra_process_demo + +#endif // INTRA_PROCESS_DEMO__TWO_NODE_PIPELINE__CONSUMER_COMPONENT_HPP_ diff --git a/intra_process_demo/include/intra_process_demo/two_node_pipeline/producer_component.hpp b/intra_process_demo/include/intra_process_demo/two_node_pipeline/producer_component.hpp new file mode 100644 index 000000000..27aef7e48 --- /dev/null +++ b/intra_process_demo/include/intra_process_demo/two_node_pipeline/producer_component.hpp @@ -0,0 +1,47 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// 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 INTRA_PROCESS_DEMO__TWO_NODE_PIPELINE__PRODUCER_COMPONENT_HPP_ +#define INTRA_PROCESS_DEMO__TWO_NODE_PIPELINE__PRODUCER_COMPONENT_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/int32.hpp" + +#include "intra_process_demo/visibility_control.h" + +namespace intra_process_demo +{ +namespace two_node_pipeline +{ + +class Producer : public rclcpp::Node +{ +public: + INTRA_PROCESS_DEMO_PUBLIC + explicit Producer(const rclcpp::NodeOptions & options); + + explicit Producer( + const std::string & name, + const std::string & output, + const rclcpp::NodeOptions & options); + +private: + rclcpp::Publisher::SharedPtr pub_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +} // namespace two_node_pipeline +} // namespace intra_process_demo + +#endif // INTRA_PROCESS_DEMO__TWO_NODE_PIPELINE__PRODUCER_COMPONENT_HPP_ diff --git a/intra_process_demo/include/intra_process_demo/visibility_control.h b/intra_process_demo/include/intra_process_demo/visibility_control.h new file mode 100644 index 000000000..3ae182f05 --- /dev/null +++ b/intra_process_demo/include/intra_process_demo/visibility_control.h @@ -0,0 +1,35 @@ +#ifndef INTRA_PROCESS_DEMO__VISIBILITY_CONTROL_H_ +#define INTRA_PROCESS_DEMO__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define INTRA_PROCESS_DEMO_EXPORT __attribute__ ((dllexport)) + #define INTRA_PROCESS_DEMO_IMPORT __attribute__ ((dllimport)) + #else + #define INTRA_PROCESS_DEMO_EXPORT __declspec(dllexport) + #define INTRA_PROCESS_DEMO_IMPORT __declspec(dllimport) + #endif + #ifdef INTRA_PROCESS_DEMO_BUILDING_LIBRARY + #define INTRA_PROCESS_DEMO_PUBLIC INTRA_PROCESS_DEMO_EXPORT + #else + #define INTRA_PROCESS_DEMO_PUBLIC INTRA_PROCESS_DEMO_IMPORT + #endif + #define INTRA_PROCESS_DEMO_PUBLIC_TYPE INTRA_PROCESS_DEMO_PUBLIC + #define INTRA_PROCESS_DEMO_LOCAL +#else + #define INTRA_PROCESS_DEMO_EXPORT __attribute__ ((visibility("default"))) + #define INTRA_PROCESS_DEMO_IMPORT + #if __GNUC__ >= 4 + #define INTRA_PROCESS_DEMO_PUBLIC __attribute__ ((visibility("default"))) + #define INTRA_PROCESS_DEMO_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define INTRA_PROCESS_DEMO_PUBLIC + #define INTRA_PROCESS_DEMO_LOCAL + #endif + #define INTRA_PROCESS_DEMO_PUBLIC_TYPE +#endif + +#endif // INTRA_PROCESS_DEMO__VISIBILITY_CONTROL_H_ diff --git a/intra_process_demo/launch/two_node_pipeline_launch.py b/intra_process_demo/launch/two_node_pipeline_launch.py new file mode 100644 index 000000000..17d929ee0 --- /dev/null +++ b/intra_process_demo/launch/two_node_pipeline_launch.py @@ -0,0 +1,45 @@ +# Copyright 2019 Open Source Robotics Foundation, Inc. +# +# 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. + +"""Launch a producer and a consumer in a component container, enabling intraprocess comms.""" + +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + return LaunchDescription([ + ComposableNodeContainer( + node_namespace='/my_ns', + node_name='my_container', + package='rclcpp_components', + node_executable='component_container', + composable_node_descriptions=[ + ComposableNode( + package='intra_process_demo', + node_namespace='/my_ns', node_name='my_producer', + node_plugin='intra_process_demo::two_node_pipeline::Producer', + extra_arguments=[{'use_intra_process_comms': True}], + ), + ComposableNode( + package='intra_process_demo', + node_namespace='/my_ns', node_name='my_consumer', + node_plugin='intra_process_demo::two_node_pipeline::Consumer', + extra_arguments=[{'use_intra_process_comms': True}], + ) + ], + output='screen', + ) + ]) diff --git a/intra_process_demo/package.xml b/intra_process_demo/package.xml index 09652b3d4..8f9611788 100644 --- a/intra_process_demo/package.xml +++ b/intra_process_demo/package.xml @@ -11,17 +11,20 @@ libopencv-dev rclcpp + rclcpp_components sensor_msgs std_msgs + launch + launch_ros libopencv-dev rclcpp + rclcpp_components sensor_msgs ament_cmake_pytest ament_lint_auto ament_lint_common - launch launch_testing launch_testing_ament_cmake rmw_implementation_cmake diff --git a/intra_process_demo/src/image_pipeline/camera_node.cpp b/intra_process_demo/src/image_pipeline/camera_node.cpp index ceefbf071..0daae928c 100644 --- a/intra_process_demo/src/image_pipeline/camera_node.cpp +++ b/intra_process_demo/src/image_pipeline/camera_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "image_pipeline/camera_node.hpp" +#include "intra_process_demo/image_pipeline/camera_node.hpp" #include diff --git a/intra_process_demo/src/image_pipeline/image_pipeline_all_in_one.cpp b/intra_process_demo/src/image_pipeline/image_pipeline_all_in_one.cpp index 32f91590e..df39918b0 100644 --- a/intra_process_demo/src/image_pipeline/image_pipeline_all_in_one.cpp +++ b/intra_process_demo/src/image_pipeline/image_pipeline_all_in_one.cpp @@ -16,9 +16,9 @@ #include -#include "image_pipeline/camera_node.hpp" -#include "image_pipeline/image_view_node.hpp" -#include "image_pipeline/watermark_node.hpp" +#include "intra_process_demo/image_pipeline/camera_node.hpp" +#include "intra_process_demo/image_pipeline/image_view_node.hpp" +#include "intra_process_demo/image_pipeline/watermark_node.hpp" int main(int argc, char * argv[]) { diff --git a/intra_process_demo/src/image_pipeline/image_pipeline_with_two_image_view.cpp b/intra_process_demo/src/image_pipeline/image_pipeline_with_two_image_view.cpp index 255e97e2d..747b7bc76 100644 --- a/intra_process_demo/src/image_pipeline/image_pipeline_with_two_image_view.cpp +++ b/intra_process_demo/src/image_pipeline/image_pipeline_with_two_image_view.cpp @@ -16,9 +16,9 @@ #include -#include "image_pipeline/camera_node.hpp" -#include "image_pipeline/image_view_node.hpp" -#include "image_pipeline/watermark_node.hpp" +#include "intra_process_demo/image_pipeline/camera_node.hpp" +#include "intra_process_demo/image_pipeline/image_view_node.hpp" +#include "intra_process_demo/image_pipeline/watermark_node.hpp" int main(int argc, char * argv[]) { diff --git a/intra_process_demo/src/image_pipeline/image_view_node.cpp b/intra_process_demo/src/image_pipeline/image_view_node.cpp index 94e190269..5af8a927f 100644 --- a/intra_process_demo/src/image_pipeline/image_view_node.cpp +++ b/intra_process_demo/src/image_pipeline/image_view_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "image_pipeline/image_view_node.hpp" +#include "intra_process_demo/image_pipeline/image_view_node.hpp" #include diff --git a/intra_process_demo/src/image_pipeline/watermark_node.cpp b/intra_process_demo/src/image_pipeline/watermark_node.cpp index a162bca42..370e9850a 100644 --- a/intra_process_demo/src/image_pipeline/watermark_node.cpp +++ b/intra_process_demo/src/image_pipeline/watermark_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "image_pipeline/watermark_node.hpp" +#include "intra_process_demo/image_pipeline/watermark_node.hpp" #include diff --git a/intra_process_demo/src/two_node_pipeline/consumer_component.cpp b/intra_process_demo/src/two_node_pipeline/consumer_component.cpp new file mode 100644 index 000000000..2798fc3a4 --- /dev/null +++ b/intra_process_demo/src/two_node_pipeline/consumer_component.cpp @@ -0,0 +1,56 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_components/register_node_macro.hpp" +#include "std_msgs/msg/int32.hpp" + +#include "intra_process_demo/two_node_pipeline/consumer_component.hpp" +#include "intra_process_demo/visibility_control.h" + +namespace intra_process_demo +{ +namespace two_node_pipeline +{ + +Consumer::Consumer(const rclcpp::NodeOptions & options) +: Consumer("consumer", "number", options) {} + +Consumer::Consumer( + const std::string & name, + const std::string & input, + const rclcpp::NodeOptions & options) +: Node(name, options) +{ + // Create a subscription on the input topic which prints on receipt of new messages. + sub_ = this->create_subscription( + input, + 10, + [](std_msgs::msg::Int32::UniquePtr msg) { + printf( + " Received message with value: %d, and address: 0x%" PRIXPTR "\n", msg->data, + reinterpret_cast(msg.get())); + }); +} + +} // namespace two_node_pipeline +} // namespace intra_process_demo + +RCLCPP_COMPONENTS_REGISTER_NODE(intra_process_demo::two_node_pipeline::Consumer) diff --git a/intra_process_demo/src/two_node_pipeline/producer_component.cpp b/intra_process_demo/src/two_node_pipeline/producer_component.cpp new file mode 100644 index 000000000..5cf158801 --- /dev/null +++ b/intra_process_demo/src/two_node_pipeline/producer_component.cpp @@ -0,0 +1,68 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_components/register_node_macro.hpp" +#include "std_msgs/msg/int32.hpp" + +#include "intra_process_demo/two_node_pipeline/producer_component.hpp" +#include "intra_process_demo/visibility_control.h" + +using namespace std::chrono_literals; + +namespace intra_process_demo +{ +namespace two_node_pipeline +{ + +Producer::Producer(const rclcpp::NodeOptions & options) +: Producer("producer", "number", options) {} + +Producer::Producer( + const std::string & name, + const std::string & output, + const rclcpp::NodeOptions & options) +: Node(name, options) +{ + // Create a publisher on the output topic. + pub_ = this->create_publisher(output, 10); + std::weak_ptr::type> captured_pub = pub_; + // Create a timer which publishes on the output topic at ~1Hz. + auto callback = [captured_pub]() -> void { + auto pub_ptr = captured_pub.lock(); + if (!pub_ptr) { + return; + } + static int32_t count = 0; + std_msgs::msg::Int32::UniquePtr msg(new std_msgs::msg::Int32()); + msg->data = count++; + printf( + "Published message with value: %d, and address: 0x%" PRIXPTR "\n", msg->data, + reinterpret_cast(msg.get())); + pub_ptr->publish(std::move(msg)); + }; + timer_ = this->create_wall_timer(1s, callback); +} + +} // namespace two_node_pipeline +} // namespace intra_process_demo + +RCLCPP_COMPONENTS_REGISTER_NODE(intra_process_demo::two_node_pipeline::Producer) diff --git a/intra_process_demo/src/two_node_pipeline/two_node_pipeline.cpp b/intra_process_demo/src/two_node_pipeline/two_node_pipeline.cpp index 347c3ad2a..fb44126d3 100644 --- a/intra_process_demo/src/two_node_pipeline/two_node_pipeline.cpp +++ b/intra_process_demo/src/two_node_pipeline/two_node_pipeline.cpp @@ -12,67 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include #include -#include #include #include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/int32.hpp" -using namespace std::chrono_literals; - -// Node that produces messages. -struct Producer : public rclcpp::Node -{ - Producer(const std::string & name, const std::string & output) - : Node(name, rclcpp::NodeOptions().use_intra_process_comms(true)) - { - // Create a publisher on the output topic. - pub_ = this->create_publisher(output, 10); - std::weak_ptr::type> captured_pub = pub_; - // Create a timer which publishes on the output topic at ~1Hz. - auto callback = [captured_pub]() -> void { - auto pub_ptr = captured_pub.lock(); - if (!pub_ptr) { - return; - } - static int32_t count = 0; - std_msgs::msg::Int32::UniquePtr msg(new std_msgs::msg::Int32()); - msg->data = count++; - printf( - "Published message with value: %d, and address: 0x%" PRIXPTR "\n", msg->data, - reinterpret_cast(msg.get())); - pub_ptr->publish(std::move(msg)); - }; - timer_ = this->create_wall_timer(1s, callback); - } - - rclcpp::Publisher::SharedPtr pub_; - rclcpp::TimerBase::SharedPtr timer_; -}; - -// Node that consumes messages. -struct Consumer : public rclcpp::Node -{ - Consumer(const std::string & name, const std::string & input) - : Node(name, rclcpp::NodeOptions().use_intra_process_comms(true)) - { - // Create a subscription on the input topic which prints on receipt of new messages. - sub_ = this->create_subscription( - input, - 10, - [](std_msgs::msg::Int32::UniquePtr msg) { - printf( - " Received message with value: %d, and address: 0x%" PRIXPTR "\n", msg->data, - reinterpret_cast(msg.get())); - }); - } - - rclcpp::Subscription::SharedPtr sub_; -}; +#include "intra_process_demo/two_node_pipeline/consumer_component.hpp" +#include "intra_process_demo/two_node_pipeline/producer_component.hpp" int main(int argc, char * argv[]) { @@ -80,8 +28,10 @@ int main(int argc, char * argv[]) rclcpp::init(argc, argv); rclcpp::executors::SingleThreadedExecutor executor; - auto producer = std::make_shared("producer", "number"); - auto consumer = std::make_shared("consumer", "number"); + auto producer = std::make_shared( + "producer", "number", rclcpp::NodeOptions().use_intra_process_comms(true)); + auto consumer = std::make_shared( + "consumer", "number", rclcpp::NodeOptions().use_intra_process_comms(true)); executor.add_node(producer); executor.add_node(consumer);