非常抱歉,我在之前的回答中漏掉了ROS主循环(ros::spin())和ros::ok()的部分。确实,在使用ROS时需要包含这些代码以确保ROS节点正常运行。
下面是更新后的代码示例:
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <cv_bridge/cv_bridge.h>
// 全局变量定义
std::mutex m_mutex;
std::vector<std::vector<bev_data>> handleDatavec;
// 图像发布函数
void publishImage(cv::Mat image, double timestamp)
{
// 创建节点句柄
ros::NodeHandle nh;
// 创建一个图像发布者
ros::Publisher image_pub = nh.advertise<sensor_msgs::Image>("/image_topic", 10);
sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();
// 设置时间戳信息(可选)
msg->header.stamp = ros::Time(timestamp);
// 发布图像消息
image_pub.publish(msg);
}
int main(int argc, char** argv)
{
// 初始化ROS节点
ros::init(argc, argv, "image_publisher");
while (ros::ok()) {
std::unique_lock<std::mutex> lck(m_mutex);
for (auto it = receiveDataMap.begin(); it != receiveDataMap.end(); ) {
double tmp_timestamp = it->first;
std::vector<bev_data>& dataList = it->second;
if (dataList.size() == dataList.at(0).total_package)
{
handleDatavec.emplace_back(dataList);
it = receiveDataMap.erase(it);
} else {
++it;
}
}
// 显示和保存
for (auto & vec:handleDatavec) {
std::vector<unsigned char> imageData; // 存储图像二进制数据
std::sort(vec.begin(), vec.end(), compareData);
double timestamp = 0;
for (const auto& data : vec) {
imageData.insert(imageData.end(), data.strPara, data.strPara + data.data_size); // 将数据追加到图像二进制数据中
timestamp = data.timestamp;
}
cv::Mat receivedImage = cv::imdecode(imageData, cv::IMREAD_COLOR); // 解码图像
if (receivedImage.empty()) {
ROS_ERROR("解码图像失败");
} else {
publishImage(receivedImage, timestamp); // 发布图像消息
std::string file_path = path + std::to_string(timestamp) + ".jpg";
cv::imwrite(file_path, receivedImage); // 保存接收到的图像文件
cv::imshow("Received Image", receivedImage); // 展示接收到的图像
cv::waitKey(30);
ros::spinOnce(); // 处理回调函数
}
}
handleDatavec.clear();
}
return 0;
}
现在,我们将while循环封装在了ros::ok()条件内,并添加了ros::spinOnce()来处理ROS回调函数。这样可以确保ROS节点在正常运行期间不会退出。
希望这次的更新代码对你有所帮助!
内容由零声教学AI助手提供,问题来源于学员提问




