diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index b1468bceeb..72d14d9f73 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/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(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 &serialized_buffer) { + rcl_serialized_message_t serialized_message = { 0 }; + serialized_message.buffer = reinterpret_cast(const_cast(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 &)"); + } + } + + void + publish(const char *serialized_msg, size_t size) { + rcl_serialized_message_t serialized_message = { 0 }; + serialized_message.buffer = const_cast(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(const_cast(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 get_allocator() const { return message_allocator_; diff --git a/rclcpp/test/test_publish_serialized.cc b/rclcpp/test/test_publish_serialized.cc new file mode 100644 index 0000000000..549865901d --- /dev/null +++ b/rclcpp/test/test_publish_serialized.cc @@ -0,0 +1,62 @@ +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +// 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("talker"); + + auto publisher = node->create_publisher(topic, 10); + + int i = 0; + while (!stop) { + std::string msg = ""; + publisher->publish(msg); + usleep(50000); + + std::vector 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; +}