diff --git a/demo_nodes_cpp/CMakeLists.txt b/demo_nodes_cpp/CMakeLists.txt index e4b572551..143c189de 100644 --- a/demo_nodes_cpp/CMakeLists.txt +++ b/demo_nodes_cpp/CMakeLists.txt @@ -72,6 +72,7 @@ add_library(topics_library SHARED src/topics/talker_loaned_message.cpp src/topics/talker_serialized_message.cpp src/topics/listener.cpp + src/topics/listener_loaned_message.cpp src/topics/listener_serialized_message.cpp src/topics/listener_best_effort.cpp) add_demo_dependencies(timers_library) @@ -121,6 +122,9 @@ rclcpp_components_register_node(topics_library rclcpp_components_register_node(topics_library PLUGIN "demo_nodes_cpp::Listener" EXECUTABLE listener) +rclcpp_components_register_node(topics_library + PLUGIN "demo_nodes_cpp::LoanedMessageListener" + EXECUTABLE listener_loaned_message) rclcpp_components_register_node(topics_library PLUGIN "demo_nodes_cpp::SerializedMessageListener" EXECUTABLE listener_serialized_message) diff --git a/demo_nodes_cpp/src/topics/listener_loaned_message.cpp b/demo_nodes_cpp/src/topics/listener_loaned_message.cpp new file mode 100644 index 000000000..f5665a801 --- /dev/null +++ b/demo_nodes_cpp/src/topics/listener_loaned_message.cpp @@ -0,0 +1,53 @@ +// Copyright 2021 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 "rclcpp/rclcpp.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +#include "std_msgs/msg/float64.hpp" +#include "demo_nodes_cpp/visibility_control.h" + +namespace demo_nodes_cpp +{ +// Create a Listener class that subclasses the generic rclcpp::Node base class. +// The main function below will instantiate the class as a ROS node. +class LoanedMessageListener : public rclcpp::Node +{ +public: + DEMO_NODES_CPP_PUBLIC + explicit LoanedMessageListener(const rclcpp::NodeOptions & options) + : Node("listener_loaned_message", options) + { + // Create a callback function for when messages are received. + // Variations of this function also exist using, for example UniquePtr for zero-copy transport. + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + auto callback = + [this](const std_msgs::msg::Float64::SharedPtr msg) -> void + { + RCLCPP_INFO(this->get_logger(), "I heard: [%f]", msg->data); + }; + // Create a subscription to the topic which can be matched with one or more compatible ROS + // publishers. + // Note that not all publishers on the same topic with the same type will be compatible: + // they must have compatible Quality of Service policies. + sub_ = create_subscription("chatter_pod", 10, callback); + } + +private: + rclcpp::Subscription::SharedPtr sub_; +}; + +} // namespace demo_nodes_cpp + +RCLCPP_COMPONENTS_REGISTER_NODE(demo_nodes_cpp::LoanedMessageListener)