1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79
| #include <chrono> #include <cinttypes> #include <cstdio> #include <memory> #include <string> #include <utility>
#include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/int32.hpp"
using namespace std::chrono_literals;
struct Producer : public rclcpp::Node { Producer(const std::string & name, const std::string & output) : Node(name, rclcpp::NodeOptions().use_intra_process_comms(true)) { pub_ = this->create_publisher<std_msgs::msg::Int32>(output, 10); std::weak_ptr<std::remove_pointer<decltype(pub_.get())>::type> captured_pub = pub_; 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<std::uintptr_t>(msg.get())); pub_ptr->publish(std::move(msg)); }; timer_ = this->create_wall_timer(1s, callback); }
rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr pub_; rclcpp::TimerBase::SharedPtr timer_; };
struct Consumer : public rclcpp::Node { Consumer(const std::string & name, const std::string & input) : Node(name, rclcpp::NodeOptions().use_intra_process_comms(true)) { sub_ = this->create_subscription<std_msgs::msg::Int32>( input, 10, [](std_msgs::msg::Int32::UniquePtr msg) { printf( " Received message with value: %d, and address: 0x%" PRIXPTR "\n", msg->data, reinterpret_cast<std::uintptr_t>(msg.get())); }); }
rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr sub_; };
int main(int argc, char * argv[]) { setvbuf(stdout, NULL, _IONBF, BUFSIZ); rclcpp::init(argc, argv); rclcpp::executors::SingleThreadedExecutor executor;
auto producer = std::make_shared<Producer>("producer", "number"); auto consumer = std::make_shared<Consumer>("consumer", "number");
executor.add_node(producer); executor.add_node(consumer); executor.spin();
rclcpp::shutdown();
return 0; }
|