class ImagePublisher : public rclcpp::Node
{
public:
ImagePublisher()
: Node("image_publisher")
{
Mat img(1920, 1080, CV_8UC3);
randu(img, Scalar(0, 0, 0), Scalar(255, 255, 255));
imwrite("publish.jpg", img);
int devCount;
void *ptr = NULL;
CUresult rc;
CUdevice cuda_device;
CUcontext cuda_context;
CUdeviceptr cuda_devptr;
CUipcMemHandle ipc_memhandle;
rc = cuInit(0);
if (rc != CUDA_SUCCESS) Elog("failed on cuInit: %s", cudaErrorName(rc));
rc = cuDeviceGet(&cuda_device, 0);
if (rc != CUDA_SUCCESS) Elog("failed on cuDeviceGet: %s", cudaErrorName(rc));
rc = cuCtxCreate(&cuda_context, 0, cuda_device);
if (rc != CUDA_SUCCESS) Elog("failed on cuCxtCreate: %s", cudaErrorName(rc));
size_t count = img.total() * img.elemSize();
rc = cuMemAlloc(&cuda_devptr, count);
if (rc != CUDA_SUCCESS) Elog("failed on cuMemAlloc: %s", cudaErrorName(rc));
rc= cuIpcGetMemHandle(&ipc_memhandle, cuda_devptr);
if (rc != CUDA_SUCCESS) Elog("failed on cuMemGetMemHandle: %s", cudaErrorName(rc));
RCLCPP_INFO(this->get_logger(), "%d", count);
uint8_t* byte_array = reinterpret_cast<uint8_t*>(&ipc_memhandle);
rc = cuIpcOpenMemHandle(&cuda_devptr, ipc_memhandle, CU_IPC_MEM_LAZY_ENABLE_PEER_ACCESS);
if (rc != CUDA_SUCCESS) Elog("failed on cuIpcOpenMemHandle: %s", cudaErrorName(rc));
publisher_ = this->create_publisher<std_msgs::msg::UInt8MultiArray>("cuda_transport", 10);
auto callback = [&]() -> void {
std_msgs::msg::UInt8MultiArray msg;
msg.layout.dim.push_back(std_msgs::msg::MultiArrayDimension());
msg.layout.dim[0].size = sizeof(ipc_memhandle);
msg.data.resize(sizeof(ipc_memhandle));
memcpy(msg.data.data(), byte_array, sizeof(ipc_memhandle));
rosidl_generator_traits::to_yaml(msg, std::cout);
publisher_->publish(msg);
RCLCPP_INFO(this->get_logger(), "publish end!");
};
timer_ = this->create_wall_timer(1s, callback);
}
private:
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Publisher<std_msgs::msg::UInt8MultiArray>::SharedPtr publisher_;
};