ZBLOG

不需要ros::ok么

非常抱歉,我在之前的回答中漏掉了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助手提供,问题来源于学员提问

本站部分文章来源于网络,版权归原作者所有,如有侵权请联系站长删除。
转载请注明出处:https://golang.0voice.com/?id=10830

分享:
扫描分享到社交APP
上一篇
下一篇
发表列表
游客 游客
此处应有掌声~
评论列表

还没有评论,快来说点什么吧~

联系我们

在线咨询: 点击这里给我发消息

微信号:3007537140

上班时间: 10:30-22:30

关注我们
x

注册

已经有帐号?