make raw nodes classes

demo_nodes_cpp/src/topics/listener_raw.cpp

@@ -31,33 +31,65 @@ void print_usage()
   printf("-t topic_name : Specify the topic on which to subscribe. Defaults to chatter.\n");
 }
 
-void chatterCallback_raw(const std::shared_ptr<rcl_message_raw_t> msg)
+// 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 RawListener : public rclcpp::Node
 {
-  std::cout << "I heard: [" << msg->buffer_length << "]" << std::endl;
-  for (size_t i = 0; i < msg->buffer_length; ++i) {
-    fprintf(stderr, "%02x ", msg->buffer[i]);
+public:
+  explicit RawListener(const std::string & topic_name)
+  : Node("raw_listener")
+  {
+    // Create a callback function for when messages are received.
+    // Variations of this function also exist using, for example UniquePtr for zero-copy transport.
+    auto callback =
+      [this](const std::shared_ptr<rcl_message_raw_t> msg) -> void
+      {
+        std::cout << "I heard: [" << msg->buffer_length << "]" << std::endl;
+        for (size_t i = 0; i < msg->buffer_length; ++i) {
+          printf("%02x ", msg->buffer[i]);
+        }
+        printf("\n");
+      };
+
+    // 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<std_msgs::msg::String>(topic_name, callback);
   }
-  fprintf(stderr, "\n");
-}
+
+private:
+  rclcpp::Subscription<rmw_message_raw_t>::SharedPtr sub_;
+};
 
 int main(int argc, char * argv[])
 {
-  rclcpp::init(argc, argv);
-  auto node = rclcpp::Node::make_shared("listener");
+  // Force flush of the stdout buffer.
+  setvbuf(stdout, NULL, _IONBF, BUFSIZ);
 
   if (rcutils_cli_option_exist(argv, argv + argc, "-h")) {
     print_usage();
     return 0;
   }
 
+  // Initialize any global resources needed by the middleware and the client library.
+  // You must call this before using any other part of the ROS system.
+  // This should be called once per process.
+  rclcpp::init(argc, argv);
+
+  // Parse the command line options.
   auto topic = std::string("chatter");
   if (rcutils_cli_option_exist(argv, argv + argc, "-t")) {
     topic = std::string(rcutils_cli_get_option(argv, argv + argc, "-t"));
   }
-  auto sub = node->create_subscription<std_msgs::msg::String>(
-    topic, chatterCallback_raw);
 
+  // Create a node.
+  auto node = std::make_shared<RawListener>(topic);
+
+  // spin will block until work comes in, execute work as it becomes available, and keep blocking.
+  // It will only be interrupted by Ctrl-C.
   rclcpp::spin(node);
 
+  rclcpp::shutdown();
   return 0;
 }

demo_nodes_cpp/src/topics/talker_raw.cpp

@@ -22,6 +22,8 @@
 
 #include "std_msgs/msg/string.hpp"
 
+using namespace std::chrono_literals;
+
 void print_usage()
 {
   printf("Usage for talker app:\n");
@@ -31,53 +33,79 @@ void print_usage()
   printf("-t topic_name : Specify the topic on which to publish. Defaults to chatter.\n");
 }
 
-int main(int argc, char * argv[])
+class RawTalker : public rclcpp::Node
 {
-  rclcpp::init(argc, argv);
+public:
+  explicit RawTalker(const std::string & topic_name)
+  : Node("raw_talker")
+  {
+    raw_msg_.buffer_length = 24;
+    raw_msg_.buffer = new char[raw_msg_.buffer_length];
+
+    // Create a function for when messages are to be sent.
+    auto publish_message =
+      [this]() -> void
+      {
+        rcutils_snprintf(raw_msg_.buffer, raw_msg_.buffer_length, "%c%c%c%c%c%c%c%c%s %zu",
+          0x00, 0x01, 0x00, 0x00, 0x0f, 0x00, 0x00, 0x00, "Hello World:", (count_++));
+        RCLCPP_INFO(this->get_logger(), "Publishing: '%s: %zu'", "Hello World", count_)
+
+        printf("Raw message:\n");
+        for (unsigned int i = 0; i < raw_msg_.buffer_length; ++i) {
+          printf("%02x ", raw_msg_.buffer[i]);
+        }
+        printf("\n");
 
-  auto node = rclcpp::Node::make_shared("talker");
+        // Put the message into a queue to be processed by the middleware.
+        // This call is non-blocking.
+        pub_->publish(&raw_msg_);
+      };
 
-  rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_default;
-  custom_qos_profile.depth = 7;
+    rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_default;
+    custom_qos_profile.depth = 7;
+    pub_ = this->create_publisher<std_msgs::msg::String>(topic_name, custom_qos_profile);
+
+    // Use a timer to schedule periodic message publishing.
+    timer_ = this->create_wall_timer(1s, publish_message);
+  }
+
+private:
+  size_t count_ = 1;
+  rcl_message_raw_t raw_msg_;
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_;
+  rclcpp::TimerBase::SharedPtr timer_;
+};
+
+int main(int argc, char * argv[])
+{
+  // Force flush of the stdout buffer.
+  // This ensures a correct sync of all prints
+  // even when executed simultaneously within the launch file.
+  setvbuf(stdout, NULL, _IONBF, BUFSIZ);
 
   if (rcutils_cli_option_exist(argv, argv + argc, "-h")) {
     print_usage();
     return 0;
   }
 
+  // Initialize any global resources needed by the middleware and the client library.
+  // You must call this before using any other part of the ROS system.
+  // This should be called once per process.
+  rclcpp::init(argc, argv);
+
+  // Parse the command line options.
   auto topic = std::string("chatter");
   if (rcutils_cli_option_exist(argv, argv + argc, "-t")) {
     topic = std::string(rcutils_cli_get_option(argv, argv + argc, "-t"));
   }
-  auto chatter_pub = node->create_publisher<std_msgs::msg::String>(topic, custom_qos_profile);
 
-  rclcpp::WallRate loop_rate(2);
+  // Create a node.
+  auto node = std::make_shared<RawTalker>(topic);
 
-  int i = 1;
-
-  rcl_message_raw_t raw_msg;
-  raw_msg.buffer_length = 23;
-  raw_msg.buffer = new char[raw_msg.buffer_length];
-  rcutils_snprintf(raw_msg.buffer, raw_msg.buffer_length, "%c%c%c%c%c%c%c%c%s %d",
-    0x00, 0x01, 0x00, 0x00, 0x0f, 0x00, 0x00, 0x00, "Hello World:", (++i));
-  //snprintf(raw_msg.buffer, raw_msg.buffer_length, "%s",
-  //  "000100000f00000048656c6c6f20576f726c643a203100");
-  //auto msg = std::make_shared<test_msgs::msg::Nested>();
-
-  for (unsigned int i = 0; i < raw_msg.buffer_length; ++i) {
-    fprintf(stderr, "%02x ", raw_msg.buffer[i]);
-  }
-  fprintf(stderr, "\n");
-
-  while (rclcpp::ok()) {
-    printf("Publishing: '%s'\n", raw_msg.buffer);
-    chatter_pub->publish(&raw_msg);
-    //msg->primitive_values.int32_value = i;
-    //printf("Publishing: '%d'\n", i);
-    //chatter_pub->publish(msg);
-    rclcpp::spin_some(node);
-    loop_rate.sleep();
-  }
+  // spin will block until work comes in, execute work as it becomes available, and keep blocking.
+  // It will only be interrupted by Ctrl-C.
+  rclcpp::spin(node);
 
+  rclcpp::shutdown();
   return 0;
 }