.. _program_listing_file__tmp_ws_src_demos_intra_process_demo_include_image_pipeline_watermark_node.hpp: Program Listing for File watermark_node.hpp =========================================== |exhale_lsh| :ref:`Return to documentation for file ` (``/tmp/ws/src/demos/intra_process_demo/include/image_pipeline/watermark_node.hpp``) .. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS .. code-block:: cpp // 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. #ifndef IMAGE_PIPELINE__WATERMARK_NODE_HPP_ #define IMAGE_PIPELINE__WATERMARK_NODE_HPP_ #include #include #include #include #include "opencv2/opencv.hpp" #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/image.hpp" #include "common.hpp" class WatermarkNode final : public rclcpp::Node { public: explicit WatermarkNode( const std::string & input, const std::string & output, const std::string & text, const std::string & node_name = "watermark_node") : Node(node_name, rclcpp::NodeOptions().use_intra_process_comms(true)) { rclcpp::SensorDataQoS qos; // Create a publisher on the input topic. pub_ = this->create_publisher(output, qos); std::weak_ptr::type> captured_pub = pub_; // Create a subscription on the output topic. sub_ = this->create_subscription( input, qos, [captured_pub, text](sensor_msgs::msg::Image::UniquePtr msg) { auto pub_ptr = captured_pub.lock(); if (!pub_ptr) { return; } // Create a cv::Mat from the image message (without copying). cv::Mat cv_mat( msg->height, msg->width, encoding2mat_type(msg->encoding), msg->data.data()); // Annotate the image with the pid, pointer address, and the watermark text. std::stringstream ss; ss << "pid: " << GETPID() << ", ptr: " << msg.get() << " " << text; draw_on_image(cv_mat, ss.str(), 40); pub_ptr->publish(std::move(msg)); // Publish it along. }); } private: rclcpp::Publisher::SharedPtr pub_; rclcpp::Subscription::SharedPtr sub_; cv::VideoCapture cap_; cv::Mat frame_; }; #endif // IMAGE_PIPELINE__WATERMARK_NODE_HPP_