ZORO-786:Added:Add publish serialized data interface

rclcpp/include/rclcpp/publisher.hpp

@@ -168,6 +168,62 @@ class Publisher : public PublisherBase
     }
   }
 
+  void
+  publish(const std::string &serialized_buffer) {
+    rcl_serialized_message_t serialized_message = { 0 };
+    serialized_message.buffer = const_cast<char*>(serialized_buffer.data());
+    serialized_message.buffer_length = serialized_buffer.size();
+    serialized_message.buffer_capacity = serialized_buffer.size();
+
+    auto status = rcl_publish_serialized_message(&publisher_handle_, &serialized_message);
+    if (RCL_RET_OK != status) {
+      rclcpp::exceptions::throw_from_rcl_error(status,
+              "failed to publish serialized message, publish(const std::string &)");
+    }
+  }
+
+  void
+  publish(const std::vector<uint8_t> &serialized_buffer) {
+    rcl_serialized_message_t serialized_message = { 0 };
+    serialized_message.buffer = reinterpret_cast<char*>(const_cast<uint8_t*>(serialized_buffer.data()));
+    serialized_message.buffer_length = serialized_buffer.size();
+    serialized_message.buffer_capacity = serialized_buffer.size();
+
+    auto status = rcl_publish_serialized_message(&publisher_handle_, &serialized_message);
+    if (RCL_RET_OK != status) {
+      rclcpp::exceptions::throw_from_rcl_error(status,
+              "failed to publish serialized message, publish(const std::vector<uint8_t> &)");
+    }
+  }
+
+  void
+  publish(const char *serialized_msg, size_t size) {
+    rcl_serialized_message_t serialized_message = { 0 };
+    serialized_message.buffer = const_cast<char*>(serialized_msg);
+    serialized_message.buffer_length = size;
+    serialized_message.buffer_capacity = size;
+
+    auto status = rcl_publish_serialized_message(&publisher_handle_, &serialized_message);
+    if (RCL_RET_OK != status) {
+      rclcpp::exceptions::throw_from_rcl_error(status,
+              "failed to publish serialized message, publish(const char *, size_t)");
+    }
+  }
+
+  void
+  publish(const uint8_t *serialized_msg, size_t size) {
+    rcl_serialized_message_t serialized_message = { 0 };
+    serialized_message.buffer = reinterpret_cast<char*>(const_cast<uint8_t*>(serialized_msg));
+    serialized_message.buffer_length = size;
+    serialized_message.buffer_capacity = size;
+
+    auto status = rcl_publish_serialized_message(&publisher_handle_, &serialized_message);
+    if (RCL_RET_OK != status) {
+      rclcpp::exceptions::throw_from_rcl_error(status,
+              "failed to publish serialized message, publish(const uint8_t *, size_t)");
+    }
+  }
+
   std::shared_ptr<MessageAlloc> get_allocator() const
   {
     return message_allocator_;

rclcpp/test/test_publish_serialized.cc

@@ -0,0 +1,62 @@
+#include <chrono>
+#include <cstdio>
+#include <memory>
+#include <string>
+
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/image.hpp>
+
+#include <unistd.h>
+#include <atomic>
+#include <vector>
+
+// compile commandline
+// g++ ../test/test_publisher_serialized.cc -std=c++14 -I./ -lrclcpp -lrcl
+// -lrcutils
+// -lsensor_msgs__rosidl_typesupport_cpp -lrmw
+
+bool stop = false;
+void sigint(int signo) {
+  if (signo == 2) stop = true;
+}
+
+using Type = sensor_msgs::msg::Image;
+
+int main(int argc, char *argv[]) {
+  signal(SIGINT, sigint);
+
+  rclcpp::init(argc, argv);
+
+  auto topic = std::string("chatter");
+
+  auto node = std::make_shared<rclcpp::Node>("talker");
+
+  auto publisher = node->create_publisher<Type>(topic, 10);
+
+  int i = 0;
+  while (!stop) {
+    std::string msg = "";
+    publisher->publish(msg);
+    usleep(50000);
+
+    std::vector<uint8_t> msg1 = {};
+    publisher->publish(msg1);
+    usleep(50000);
+
+    const char *ptr = "I am a const char * pointer";
+    size_t size = strlen(ptr);
+    publisher->publish(ptr, size);
+    usleep(50000);
+
+    // const uint8_t *ptr1 = (uint8_t*)(ptr);
+    const uint8_t *ptr1 = (uint8_t *)"I am a const uint8_t * pinter";
+    publisher->publish(ptr1, size);
+    usleep(50000);
+
+    printf("publish %d\n", ++i);
+  }
+
+  rclcpp::shutdown();
+
+  return 0;
+}