#include <ros/ros.h>
#include <rosbag/bag.h>
#include <sensor_msgs/PointCloud2.h>
#include <deque>
#include <thread>
#include <mutex>
#include <chrono>
#include <cstdlib>
#include <fstream>
#include <sstream>
#include <sys/time.h>
#include <memory>
#include <atomic>
extern "C" {
#include <libavformat/avformat.h>
#include <libavcodec/avcodec.h>
#include <libavutil/opt.h>
#include <libavutil/time.h>
}
class FFmpegRTSPStreamer {
public:
FFmpegRTSPStreamer(int port = 8554, const std::string& stream_name = "mystream")
: port_(port), stream_name_(stream_name), context_(nullptr), running_(false) {
av_register_all();
avformat_network_init();
}
~FFmpegRTSPStreamer() {
stop();
}
bool start() {
if (running_) return true;
// 创建输出格式上下文 - 指定为RTSP服务器
avformat_alloc_output_context2(&context_, nullptr, "rtsp", nullptr);
if (!context_) {
ROS_ERROR("Could not create RTSP server context");
return false;
}
// 设置服务器选项
AVDictionary* options = nullptr;
char port_str[16];
snprintf(port_str, sizeof(port_str), "%d", port_);
av_dict_set(&options, "rtsp_port", port_str, 0);
av_dict_set(&options, "listen", "1", 0); // 设置为监听模式
// 创建输出流
AVStream* stream = avformat_new_stream(context_, nullptr);
if (!stream) {
ROS_ERROR("Could not create output stream");
return false;
}
// 设置输出URL
std::stringstream url_ss;
url_ss << "rtsp://0.0.0.0:" << port_ << "/" << stream_name_;
std::string url = url_ss.str();
// 打开输出
if (avio_open2(&context_->pb, url.c_str(), AVIO_FLAG_WRITE, nullptr, &options) < 0) {
ROS_ERROR("Could not open RTSP URL: %s", url.c_str());
av_dict_free(&options);
return false;
}
av_dict_free(&options);
// 写入头部信息
if (avformat_write_header(context_, nullptr) < 0) {
ROS_ERROR("Error writing RTSP header");
return false;
}
running_ = true;
ROS_INFO("FFmpeg RTSP server started at: %s", url.c_str());
return true;
}
void stream_frame(AVPacket* packet) {
if (!running_) return;
// 发送数据包到RTSP客户端
if (av_interleaved_write_frame(context_, packet) < 0) {
ROS_ERROR("Error writing frame to RTSP stream");
}
}
void stop() {
if (!running_) return;
// 写入尾部信息
av_write_trailer(context_);
// 关闭输出
if (context_->pb) {
avio_close(context_->pb);
}
// 释放资源
avformat_free_context(context_);
context_ = nullptr;
running_ = false;
ROS_INFO("FFmpeg RTSP server stopped");
}
bool is_running() const {
return running_;
}
private:
int port_;
std::string stream_name_;
AVFormatContext* context_;
bool running_;
};
class AVPointCloudSyncer {
public:
AVPointCloudSyncer(const std::string& bag_file,
const std::string& topic_name,
const std::string& video_path,
int rtsp_port = 8554,
const std::string& rtsp_stream_name = "mystream")
: bag_file_(bag_file),
topic_name_(topic_name),
video_path_(video_path),
rtsp_port_(rtsp_port),
rtsp_stream_name_(rtsp_stream_name),
sync_ratio_(1.0),
running_(false) {
// 初始化FFmpeg库
av_register_all();
avformat_network_init();
}
~AVPointCloudSyncer() {
cleanup();
}
// 设置RTSP服务器参数的方法
void set_rtsp_params(int port, const std::string& stream_name) {
rtsp_port_ = port;
rtsp_stream_name_ = stream_name;
}
bool initialize() {
// 创建ROS发布器
pub_ = nh_.advertise<sensor_msgs::PointCloud2>(topic_name_, 1000);
// 确保发布器已连接
ros::WallDuration(2.0).sleep();
// 设置AV流
if (!setup_av_streaming()) {
ROS_ERROR("Failed to setup AV streaming");
return false;
}
// 设置RTSP服务器
if (!setup_rtsp_server()) {
ROS_ERROR("Failed to setup RTSP server");
return false;
}
// 提取时间戳
if (!extract_video_timestamps()) {
ROS_ERROR("Failed to extract video timestamps");
return false;
}
if (!extract_pointcloud_timestamps()) {
ROS_ERROR("Failed to extract pointcloud timestamps");
return false;
}
return true;
}
void run() {
if (!initialize()) {
ROS_ERROR("Initialization failed. Exiting.");
return;
}
running_ = true;
// 启动视频流线程
std::thread video_thread(&AVPointCloudSyncer::stream_video_with_av, this);
// 在主线程中同步播放点云
replay_pointcloud_synced();
// 等待视频线程结束
if (video_thread.joinable()) {
video_thread.join();
}
running_ = false;
ROS_INFO("AVPointCloudSyncer finished");
}
void cleanup() {
running_ = false;
// 关闭FFmpeg资源
if (input_context_) {
avformat_close_input(&input_context_);
input_context_ = nullptr;
}
// 停止RTSP服务器
if (rtsp_streamer_) {
rtsp_streamer_->stop();
}
}
private:
// 设置RTSP服务器
bool setup_rtsp_server() {
ROS_INFO("Setting up RTSP server on port %d with stream name '%s'",
rtsp_port_, rtsp_stream_name_.c_str());
rtsp_streamer_ = std::make_unique<FFmpegRTSPStreamer>(rtsp_port_, rtsp_stream_name_);
return rtsp_streamer_->start();
}
// FFmpeg设置
bool setup_av_streaming() {
ROS_INFO("Setting up AV streaming...");
input_context_ = avformat_alloc_context();
if (!input_context_) {
ROS_ERROR("Failed to allocate AVFormatContext");
return false;
}
// 设置输入选项（针对文件的优化）
AVDictionary* options = nullptr;
av_dict_set(&options, "analyzeduration", "1000000", 0); // 1秒分析时长
av_dict_set(&options, "probesize", "500000", 0); // 500KB探测大小
// 打开输入视频
if (avformat_open_input(&input_context_, video_path_.c_str(), nullptr, &options) != 0) {
ROS_ERROR("Could not open input file: %s", video_path_.c_str());
av_dict_free(&options);
return false;
}
av_dict_free(&options);
// 查找流信息
if (avformat_find_stream_info(input_context_, nullptr) < 0) {
ROS_ERROR("Failed to find stream information");
return false;
}
// 查找视频流
video_stream_index_ = -1;
for (unsigned int i = 0; i < input_context_->nb_streams; i++) {
if (input_context_->streams[i]->codecpar->codec_type == AVMEDIA_TYPE_VIDEO) {
video_stream_index_ = i;
break;
}
}
if (video_stream_index_ == -1) {
ROS_ERROR("Could not find video stream");
return false;
}
// 获取视频流参数
AVStream* video_stream = input_context_->streams[video_stream_index_];
ROS_INFO("Video stream initialized: index %d, codec: %s, resolution: %dx%d",
video_stream_index_,
avcodec_get_name(video_stream->codecpar->codec_id),
video_stream->codecpar->width,
video_stream->codecpar->height);
return true;
}
// 视频流处理（集成RTSP输出）
void stream_video_with_av() {
ROS_INFO("Starting video streaming with FFmpeg RTSP server...");
if (!input_context_) {
ROS_ERROR("AVFormatContext not initialized");
return;
}
AVPacket packet;
av_init_packet(&packet);
packet.data = nullptr;
packet.size = 0;
int frame_count = 0;
auto start_time = std::chrono::steady_clock::now();
while (running_ && av_read_frame(input_context_, &packet) >= 0) {
if (packet.stream_index == video_stream_index_) {
// 如果RTSP服务器正在运行，转发视频帧
if (rtsp_streamer_ && rtsp_streamer_->is_running()) {
// 创建输出包副本
AVPacket output_packet;
if (av_packet_ref(&output_packet, &packet) < 0) {
ROS_ERROR("Failed to create packet reference");
} else {
// 发送到RTSP服务器
rtsp_streamer_->stream_frame(&output_packet);
av_packet_unref(&output_packet);
}
}
frame_count++;
if (frame_count % 30 == 0) {
auto elapsed = std::chrono::steady_clock::now() - start_time;
double fps = 30.0 / (std::chrono::duration<double>(elapsed).count());
ROS_INFO("Processed %d video frames (%.1f FPS)", frame_count, fps);
start_time = std::chrono::steady_clock::now();
}
}
av_packet_unref(&packet);
}
ROS_INFO("Video streaming completed: %d frames", frame_count);
}
// 提取视频时间戳
bool extract_video_timestamps() {
ROS_INFO("Extracting video timestamps...");
AVFormatContext* temp_context = avformat_alloc_context();
if (!temp_context) {
ROS_ERROR("Failed to allocate temporary AVFormatContext");
return false;
}
if (avformat_open_input(&temp_context, video_path_.c_str(), nullptr, nullptr) != 0) {
ROS_ERROR("Could not open input file for timestamp extraction");
avformat_free_context(temp_context);
return false;
}
if (avformat_find_stream_info(temp_context, nullptr) < 0) {
ROS_ERROR("Failed to find stream information for timestamp extraction");
avformat_close_input(&temp_context);
return false;
}
int video_stream_index = -1;
for (unsigned int i = 0; i < temp_context->nb_streams; i++) {
if (temp_context->streams[i]->codecpar->codec_type == AVMEDIA_TYPE_VIDEO) {
video_stream_index = i;
break;
}
}
if (video_stream_index == -1) {
ROS_ERROR("Could not find video stream for timestamp extraction");
avformat_close_input(&temp_context);
return false;
}
AVPacket packet;
av_init_packet(&packet);
packet.data = nullptr;
packet.size = 0;
int frame_count = 0;
while (av_read_frame(temp_context, &packet) >= 0) {
if (packet.stream_index == video_stream_index) {
// 计算时间戳（秒）
AVStream* stream = temp_context->streams[packet.stream_index];
double pts = packet.pts * av_q2d(stream->time_base);
video_frame_times_.push_back(pts);
frame_count++;
if (frame_count >= 5000) break; // 限制提取数量
}
av_packet_unref(&packet);
}
av_packet_unref(&packet);
avformat_close_input(&temp_context);
ROS_INFO("Extracted %lu video frame timestamps", video_frame_times_.size());
return !video_frame_times_.empty();
}
// 提取点云时间戳
bool extract_pointcloud_timestamps() {
ROS_INFO("Extracting pointcloud timestamps...");
try {
rosbag::Bag bag(bag_file_, rosbag::bagmode::Read);
rosbag::View view(bag, rosbag::TopicQuery(topic_name_));
int message_count = 0;
for (const rosbag::MessageInstance& m : view) {
sensor_msgs::PointCloud2::ConstPtr msg = m.instantiate<sensor_msgs::PointCloud2>();
if (msg != nullptr) {
pointcloud_times_.push_back(m.getTime().toSec());
message_count++;
if (message_count % 1000 == 0) {
ROS_INFO("Extracted %d pointcloud timestamps", message_count);
}
if (message_count >= 10000) break; // 限制提取数量
}
}
bag.close();
ROS_INFO("Total pointcloud messages extracted: %lu", pointcloud_times_.size());
return !pointcloud_times_.empty();
}
catch (rosbag::BagException& e) {
ROS_ERROR("Error opening rosbag: %s", e.what());
return false;
}
}
// 同步点云重放
void replay_pointcloud_synced() {
ROS_INFO("Starting synchronized pointcloud replay...");
if (pointcloud_times_.empty()) {
ROS_ERROR("No pointcloud timestamps extracted!");
return;
}
// 计算同步比例
if (video_frame_times_.size() > 1 && pointcloud_times_.size() > 1) {
double video_duration = video_frame_times_.back() - video_frame_times_.front();
double pointcloud_duration = pointcloud_times_.back() - pointcloud_times_.front();
if (pointcloud_duration > 0) {
sync_ratio_ = video_duration / pointcloud_duration;
ROS_INFO("Sync ratio: %.3f (Video: %.2fs, PointCloud: %.2fs)",
sync_ratio_, video_duration, pointcloud_duration);
}
}
try {
rosbag::Bag bag(bag_file_, rosbag::bagmode::Read);
rosbag::View view(bag, rosbag::TopicQuery(topic_name_));
auto start_time = std::chrono::steady_clock::now();
double bag_start_time = pointcloud_times_.front();
int message_count = 0;
for (const rosbag::MessageInstance& m : view) {
if (!running_ || !ros::ok()) {
break;
}
sensor_msgs::PointCloud2::ConstPtr msg = m.instantiate<sensor_msgs::PointCloud2>();
if (!msg) continue;
// 计算当前消息相对于开始的时间（应用同步比例）
double msg_elapsed = (m.getTime().toSec() - bag_start_time) * sync_ratio_;
auto current_time = std::chrono::steady_clock::now();
double current_elapsed = std::chrono::duration<double>(current_time - start_time).count();
// 等待直到正确的发布时间
if (current_elapsed < msg_elapsed) {
double sleep_time = msg_elapsed - current_elapsed;
ros::WallDuration(sleep_time).sleep();
}
// 发布点云消息
pub_.publish(msg);
message_count++;
if (message_count % 100 == 0) {
ROS_INFO("Published pointcloud message %d", message_count);
}
}
bag.close();
ROS_INFO("Finished replaying %d pointcloud messages", message_count);
}
catch (rosbag::BagException& e) {
ROS_ERROR("Error during pointcloud replay: %s", e.what());
}
}
private:
// ROS相关
ros::NodeHandle nh_;
ros::Publisher pub_;
// 配置参数
std::string bag_file_;
std::string topic_name_;
std::string video_path_;
int rtsp_port_;
std::string rtsp_stream_name_;
// 时间同步相关
std::deque<double> video_frame_times_;
std::deque<double> pointcloud_times_;
double sync_ratio_;
// FFmpeg相关
AVFormatContext* input_context_ = nullptr;
int video_stream_index_ = -1;
std::unique_ptr<FFmpegRTSPStreamer> rtsp_streamer_;
// 控制标志
std::atomic<bool> running_;
};
int main(int argc, char** argv) {
ros::init(argc, argv, "av_pointcloud_syncer");
// 配置参数 - 可从命令行或参数服务器读取
std::string bag_file = "/media/jxpy-chengb/Ubuntu 24.0/T06/TC1-20250626/_2025-06-26-10-15-10_669.bag";
std::请补充完整这个代码
当Ubuntu系统提示磁盘空间不足时，可以尝试进入recovery模式，通过删除无用文件来释放空间。具体步骤包括：重启并长按shift，选择recovery mode，进入root shell，输入密码后在命令行操作。
扫一扫
3796
到【灌水乐园】发言
