目前在RDK X3板端利用零拷贝发布图像代码如下所示：
#include
#include
#include
#include <opencv2/opencv.hpp>
#include “rclcpp/rclcpp.hpp”
#include “hbm_img_msgs/msg/hbm_msg1080_p.hpp”
#include “builtin_interfaces/msg/time.hpp”
using namespace std::chrono_literals;
class HbmemCameraPublisher : public rclcpp::Node {
public:
HbmemCameraPublisher()
: Node(“hbmem_camera_publisher”) {
publisher_ = this->create_publisher_hbmem<hbm_img_msgs::msg::HbmMsg1080P>(
“/image_raw_hbmem”, rclcpp::SensorDataQoS());
cap_.open(8);
cap_.set(cv::CAP_PROP_FRAME_WIDTH, 1920);
cap_.set(cv::CAP_PROP_FRAME_HEIGHT, 1080);
if (!cap_.isOpened()) {
RCLCPP_ERROR(this->get_logger(), "无法打开摄像头");
throw std::runtime_error("打开摄像头失败");
}
thread_ = std::thread(&HbmemCameraPublisher::publish_loop, this);
}
~HbmemCameraPublisher() override {
running_ = false;
if (thread_.joinable()) thread_.join();
cap_.release();
}
private:
void publish_loop() {
const auto delay = 20ms;
int index = 0;
while (rclcpp::ok() && running_) {
cv::Mat frame;
cap_ >> frame;
if (frame.empty()) {
RCLCPP_WARN(this->get_logger(), "图像帧为空");
std::this_thread::sleep_for(delay);
continue;
}
// 获取 ROS2 时间戳
auto now = this->get_clock()->now();
rclcpp::Time ros_now(now);
size_t img_size = frame.total() * frame.elemSize(); // e.g., 1920*1080*3 = 6220800
if (img_size > 6220800) {
RCLCPP_WARN(this->get_logger(), "图像数据超过最大缓冲区");
continue;
}
auto loaned_msg = publisher_->borrow_loaned_message();
if (!loaned_msg.is_valid()) {
RCLCPP_WARN(this->get_logger(), "获取 loaned message 失败");
continue;
}
auto & msg = loaned_msg.get();
msg.index = index++;
msg.time_stamp.sec = ros_now.seconds(); // 返回 double，需要分解
msg.time_stamp.nanosec = static_cast<uint32_t>((ros_now.seconds() - msg.time_stamp.sec) * 1e9);
msg.height = frame.rows;
msg.width = frame.cols;
msg.data_size = static_cast<uint32_t>(img_size);
msg.step = static_cast<uint32_t>(frame.step);
// 设置编码为 "bgr8"
const std::string encoding_str = "bgr8";
std::memset(msg.encoding.data(), 0, msg.encoding.size());
std::memcpy(msg.encoding.data(), encoding_str.c_str(),
std::min(msg.encoding.size(), encoding_str.size()));
// 拷贝图像数据
std::memcpy(msg.data.data(), frame.data, img_size);
publisher_->publish(std::move(loaned_msg));
std::this_thread::sleep_for(delay);
}
}
rclcpp::PublisherHbmem<hbm_img_msgs::msg::HbmMsg1080P>::SharedPtr publisher_;
cv::VideoCapture cap_;
std::thread thread_;
bool running_ = true;
};
int main(int argc, char * argv) {
rclcpp::init(argc, argv);
auto node = std::make_shared();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
目前hb_mem_msgs包已经部署在工作空间中
当我的节点运行后，发布的图像话题如图所示：
利用官方的hobot_codec库也无法运行，运行的命令如下：
ros2 launch hobot_codec hobot_codec_encode.launch.py codec_in_mode:=shared_mem codec_out_mode:=ros codec_in_format:=bgr8 codec_out_format:=jpeg
codec_sub_topic:=/image_raw_hbmem0x21062611010001200216090803180318 codec_pub_topic:=/image2
