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

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 COMPONENTS
  roscpp
  rosbag
  )


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_slice
  ARCHIVE 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);
}