问题描述:
针对上面的问题解决的方法有:
方法1:换个读写速度更快的SSD硬盘
--------------------------亲测可用---------------------------
方法2:压缩原始图像大小
安装插件
sudo apt-get install ros-melodic-image-transport-plugins
创建catkin_ws工作空间下的功能包,名字 image_compressor
launch/start_compressor.launch
start_compressor.launch
<launch>
<node name="image_compressor_node" pkg="image_compressor" type="image_compressor_node" output="screen"/>
</launch>
src/image_compressor_node.cpp
image_compressor_node.cpp
#include <ros/ros.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/CompressedImage.h>
#include <opencv2/imgcodecs.hpp>
class ImageCompressorNode
{
public:
ImageCompressorNode()
{
// 订阅压缩图像话题
for (const auto& topic : compressed_input_topics_)
{
subscribers_.push_back(nh_.subscribe<sensor_msgs::CompressedImage>(topic, 1, &ImageCompressorNode::compressedImageCallback, this));
}
// 发布压缩后的图像话题
for (const auto& topic : compressed_output_topics_)
{
compressed_publishers_.push_back(nh_.advertise<sensor_msgs::CompressedImage>(topic, 1));
}
}
private:
void compressedImageCallback(const sensor_msgs::CompressedImageConstPtr& msg)
{
try
{
cv::Mat image = cv::imdecode(cv::Mat(msg->data), cv::IMREAD_COLOR);
if(image.empty()) {
ROS_ERROR("Failed to decode image");
return;
}
std::vector<uchar> buffer;
std::vector<int> compression_params;
compression_params.push_back(cv::IMWRITE_JPEG_QUALITY);
compression_params.push_back(95); // Quality from 0 to 100
cv::imencode(".jpg", image, buffer, compression_params);
sensor_msgs::CompressedImage compressed_msg;
compressed_msg.header = msg->header;
compressed_msg.format = "jpeg";
compressed_msg.data = buffer;
// 发布压缩后的图像
std::string topic = msg->header.frame_id;
auto it = std::find(compressed_input_topics_.begin(), compressed_input_topics_.end(), topic);
if (it != compressed_input_topics_.end())
{
size_t index = std::distance(compressed_input_topics_.begin(), it);
compressed_publishers_[index].publish(compressed_msg);
}
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("Could not convert to image: %s", e.what());
}
}
ros::NodeHandle nh_;
std::vector<std::string> compressed_input_topics_ = {
"/usb_cam1/image_raw/compressed",
"/usb_cam2/image_raw/compressed",
"/usb_cam3/image_raw/compressed",
"/usb_cam4/image_raw/compressed",
"/usb_cam5/image_raw/compressed"
};
std::vector<std::string> compressed_output_topics_ = {
"/usb_cam1/image_compressed",
"/usb_cam2/image_compressed",
"/usb_cam3/image_compressed",
"/usb_cam4/image_compressed",
"/usb_cam5/image_compressed"
};
std::vector<ros::Subscriber> subscribers_;
std::vector<ros::Publisher> compressed_publishers_;
};
int main(int argc, char **argv)
{
ros::init(argc, argv, "image_compressor_node");
ImageCompressorNode node;
ros::spin();
return 0;
}
CMakeLists.txt
cmake_minimum_required(VERSION 3.0.2)
project(image_compressor)
find_package(catkin REQUIRED COMPONENTS
roscpp
sensor_msgs
cv_bridge
image_transport
)
find_package(OpenCV REQUIRED)
catkin_package(
CATKIN_DEPENDS roscpp sensor_msgs cv_bridge image_transport
)
include_directories(
${catkin_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
)
add_executable(${PROJECT_NAME}_node src/image_compressor_node.cpp)
target_link_libraries(${PROJECT_NAME}_node
${catkin_LIBRARIES}
${OpenCV_LIBRARIES}
)
package.xml
<?xml version="1.0"?>
<package format="2">
<name>image_compressor</name>
<version>0.0.0</version>
<description>The image_compressor package</description>
<maintainer email="user@todo.todo">user</maintainer>
<license>TODO</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>cv_bridge</build_depend>
<build_depend>image_transport</build_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>cv_bridge</exec_depend>
<exec_depend>image_transport</exec_depend>
</package>
cd ~catkin_ws
catkin_make
source devel/setup.bash
roslaunch multi_cam multi_cam.launch
roslaunch image_compressor start_compressor.launch
查看话题如下:
录用包的时候将
rosbag record /usb_cam1/image_raw /usb_cam2/image_raw /usb_cam3/image_raw /usb_cam4/image_raw /usb_cam5/image_raw
换成
rosbag record /usb_cam1/image_raw/compressed /usb_cam2/image_raw/compressed /usb_cam3/image_raw/compressed /usb_cam4/image_raw/compressed /usb_cam5/image_raw/compressed
--------------------------亲测可用,但是画质有所损伤---------------------------
方法3: 提高rosbag的缓存空间
rosbag record -b 4096 /usb_cam1/image_raw /usb_cam2/image_raw /usb_cam3/image_raw /usb_cam4/image_raw /usb_cam5/image_raw
--------------------------此方法效果不太明显---------------------------