ROS笔记之rosbag的快速切片(C++实现)

—— 杭州 2023-12-21 夜

code review

正常该功能是ROS官方命令行：rosbag filter来实现，但速度太慢.

代码抄自大佬的Github：https://github.com/berndpfrommer/fast_rosbag_slice.git

1.运行效果

• input_bag的情况
  

• 运行，想得到后50s的bag

rosrun fast_rosbag_slice fast_rosbag_slice -i a.bag -o output.bag -s 1686903228.56 -e 1686903278.56

耗时8.68s(windows虚拟机环境)

• output_bag的情况
  

2.文件结构

3.fast_rosbag_slice.cpp

// -*-c++-*--------------------------------------------------------------------
// Copyright 2022 Bernd Pfrommer <bernd.pfrommer@gmail.com>
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <unistd.h>#include <chrono>
#include <iostream>
#include <limits>void usage()
{std::cout << "usage:" << std::endl;std::cout << "fast_rosbag_slice -i input_bag -o output_bag -s start_time -e stop_time "<< std::endl;
}static size_t process_bag(const std::string & inBagName, const std::string & outBagName, const double startTime,const double endTime)
{std::cout << "reading from bag: " << inBagName << std::endl;std::cout << "writing to bag: " << outBagName << std::endl;rosbag::Bag inBag;inBag.open(inBagName, rosbag::bagmode::Read);rosbag::Bag outBag;outBag.open(outBagName, rosbag::bagmode::Write);rosbag::View view(inBag);size_t numMessages(0);for (const rosbag::MessageInstance & m : view) {if (m.getTime().toSec() > endTime) {break;}if (m.getTime().toSec() >= startTime) {outBag.write(m.getTopic(), m.getTime(), m);numMessages++;}}inBag.close();outBag.close();return (numMessages);
}int main(int argc, char ** argv)
{int opt;ros::Time::init();std::string inBag;std::string outBag;double startTime(0);double endTime(std::numeric_limits<double>::max());while ((opt = getopt(argc, argv, "i:o:s:e:h")) != -1) {switch (opt) {case 'i':inBag = optarg;break;case 'o':outBag = optarg;break;case 's':startTime = atof(optarg);break;case 'e':endTime = atof(optarg);break;case 'h':usage();return (-1);default:std::cout << "unknown option: " << opt << std::endl;usage();return (-1);break;}}if (inBag.empty() || outBag.empty()) {std::cout << "missing input or output bag name!" << std::endl;usage();return (-1);}const auto start = std::chrono::high_resolution_clock::now();size_t numMsg = process_bag(inBag, outBag, startTime, endTime);const auto end = std::chrono::high_resolution_clock::now();auto total_duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start).count();std::cout << "total time: " << total_duration * 1e-6 << " s" << std::endl;std::cout << "message processing rate: " << numMsg * 1e6 / total_duration << " hz" << std::endl;return (0);
}

4.CMakeLists.txt

#
# Copyright 2022 Bernd Pfrommer <bernd.pfrommer@gmail.com>
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
cmake_minimum_required(VERSION 3.5)
project(fast_rosbag_slice)set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -Wall -Wextra -Wpedantic -Werror")
set (CMAKE_CXX_STANDARD 14)find_package(catkin REQUIRED COMPONENTSroscpprosbag)catkin_package()include_directories(${catkin_INCLUDE_DIRS}
)# --------- sync test
add_executable(fast_rosbag_slice src/fast_rosbag_slice.cpp)
target_link_libraries(fast_rosbag_slice ${catkin_LIBRARIES})
#
# volumetric tracking node and nodelet
#
install(TARGETS fast_rosbag_sliceARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
)

5.package.xml

<?xml version="1.0"?>
<package format="3"><name>fast_rosbag_slice</name><version>1.0.0</version><description>fast rosbag time slicer</description><maintainer email="bernd.pfrommer@gmail.com">Bernd Pfrommer</maintainer><license>Apache2</license><buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend><depend condition="$ROS_VERSION == 1">roscpp</depend><depend condition="$ROS_VERSION == 1">rosbag</depend><export><build_type condition="$ROS_VERSION == 1">catkin</build_type></export></package>

6.对fast_rosbag_slice.cpp进行函数封装

运行

代码

#include <chrono>
#include <iostream>
#include <limits>
#include <rosbag/bag.h>
#include <rosbag/view.h>static size_t process_bag(const std::string &inBagName, const std::string &outBagName, const double startTime,const double endTime) {std::cout << "reading from bag: " << inBagName << std::endl;std::cout << "writing to bag: " << outBagName << std::endl;rosbag::Bag inBag;inBag.open(inBagName, rosbag::bagmode::Read);rosbag::Bag outBag;outBag.open(outBagName, rosbag::bagmode::Write);rosbag::View view(inBag);size_t numMessages(0);for (const rosbag::MessageInstance &m : view) {if (m.getTime().toSec() > endTime) {break;}if (m.getTime().toSec() >= startTime) {outBag.write(m.getTopic(), m.getTime(), m);numMessages++;}}inBag.close();outBag.close();return (numMessages);
}int main() {std::string inBag = "/home/user/bag/a.bag";std::string outBag = "/home/user/bag/output.bag";double startTime = 1686903228.56;double endTime = 1686903278.56;const auto start = std::chrono::high_resolution_clock::now();size_t numMsg = process_bag(inBag, outBag, startTime, endTime);const auto end = std::chrono::high_resolution_clock::now();auto total_duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start).count();std::cout << "total time: " << total_duration * 1e-6 << " s" << std::endl;std::cout << "message processing rate: " << numMsg * 1e6 / total_duration << " hz" << std::endl;return (0);
}