├── rosbag_fancy_msgs ├── msg │ ├── PlayTopicStatus.msg │ ├── TopicStatus.msg │ ├── Status.msg │ └── PlayStatus.msg ├── package.xml ├── CMakeLists.txt └── CHANGELOG.rst ├── .editorconfig ├── rosbag_fancy ├── src │ ├── doctest.cpp │ ├── mem_str.h │ ├── cmd_test.cpp │ ├── topic_subscriber.h │ ├── tf2_scanner.h │ ├── message_queue.cpp │ ├── message_queue.h │ ├── topic_manager.cpp │ ├── main.cpp │ ├── ui.h │ ├── mem_str.cpp │ ├── bag_view.h │ ├── topic_subscriber.cpp │ ├── bag_writer.h │ ├── topic_manager.h │ ├── terminal.h │ ├── cmd_info.cpp │ ├── bag_reader.h │ ├── tf2_scanner.cpp │ ├── cmd_record.cpp │ ├── bag_view.cpp │ ├── cmd_play.cpp │ ├── bag_writer.cpp │ ├── terminal.cpp │ ├── ui.cpp │ └── bag_reader.cpp ├── scripts │ ├── record_gif.sh │ ├── play.tape │ └── record_svg.sh ├── package.xml ├── tools │ └── tf2_republisher.cpp ├── contrib │ └── doctest │ │ └── LICENSE.txt ├── env-hooks │ └── 50-rosbag_fancy.bash ├── CMakeLists.txt └── CHANGELOG.rst ├── rqt_rosbag_fancy ├── rqt_plugins.xml ├── package.xml ├── CHANGELOG.rst ├── CMakeLists.txt └── src │ ├── topic_model.h │ ├── fancy_gui.h │ ├── topic_model.cpp │ ├── fancy_gui.cpp │ └── fancy_gui.ui ├── .github └── workflows │ └── build.yml ├── README.md └── LICENSE /rosbag_fancy_msgs/msg/PlayTopicStatus.msg: -------------------------------------------------------------------------------- 1 | 2 | string name 3 | 4 | float32 rate 5 | float32 bandwidth 6 | -------------------------------------------------------------------------------- /.editorconfig: -------------------------------------------------------------------------------- 1 | 2 | [*] 3 | end_of_line = lf 4 | insert_final_newline = true 5 | charset = utf-8 6 | indent_style = tab 7 | indent_size = 4 8 | 9 | -------------------------------------------------------------------------------- /rosbag_fancy/src/doctest.cpp: -------------------------------------------------------------------------------- 1 | // Doctest implementation 2 | // Author: Max Schwarz 3 | 4 | #define DOCTEST_CONFIG_IMPLEMENT 5 | #include "doctest.h" 6 | -------------------------------------------------------------------------------- /rosbag_fancy_msgs/msg/TopicStatus.msg: -------------------------------------------------------------------------------- 1 | 2 | string name 3 | 4 | uint32 publishers 5 | uint64 messages 6 | uint64 messages_in_current_bag 7 | float32 rate 8 | uint64 bytes 9 | float32 bandwidth 10 | -------------------------------------------------------------------------------- /rosbag_fancy_msgs/msg/Status.msg: -------------------------------------------------------------------------------- 1 | 2 | uint8 STATUS_PAUSED = 0 3 | uint8 STATUS_RUNNING = 1 4 | 5 | 6 | Header header 7 | 8 | uint8 status 9 | string bagfile 10 | 11 | uint64 bytes 12 | uint64 free_bytes 13 | float32 bandwidth 14 | 15 | TopicStatus[] topics 16 | -------------------------------------------------------------------------------- /rosbag_fancy_msgs/msg/PlayStatus.msg: -------------------------------------------------------------------------------- 1 | 2 | uint8 STATUS_PAUSED = 0 3 | uint8 STATUS_RUNNING = 1 4 | 5 | 6 | Header header 7 | 8 | uint8 status 9 | duration duration 10 | duration current 11 | time currentTime 12 | time startTime 13 | time endTime 14 | string[] bagfiles 15 | 16 | PlayTopicStatus[] topics 17 | 18 | -------------------------------------------------------------------------------- /rosbag_fancy_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | rosbag_fancy_msgs 3 | Messages rosbag_fancy 4 | 1.1.0 5 | BSD 6 | 7 | Max Schwarz 8 | 9 | catkin 10 | message_generation 11 | std_msgs 12 | 13 | -------------------------------------------------------------------------------- /rosbag_fancy_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | cmake_minimum_required(VERSION 3.0) 3 | project(rosbag_fancy_msgs) 4 | 5 | find_package(catkin REQUIRED COMPONENTS 6 | message_generation 7 | std_msgs 8 | ) 9 | 10 | add_message_files(FILES 11 | Status.msg 12 | TopicStatus.msg 13 | PlayStatus.msg 14 | PlayTopicStatus.msg 15 | ) 16 | 17 | generate_messages(DEPENDENCIES 18 | std_msgs 19 | ) 20 | 21 | catkin_package() 22 | -------------------------------------------------------------------------------- /rqt_rosbag_fancy/rqt_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | rosbag_fancy GUI 5 | 6 | 7 | 8 | GUI for rosbag_fancy 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /.github/workflows/build.yml: -------------------------------------------------------------------------------- 1 | name: Build & test 2 | on: [push, pull_request] 3 | 4 | defaults: 5 | run: 6 | shell: bash 7 | 8 | jobs: 9 | build_and_test_ubuntu: 10 | runs-on: ubuntu-latest 11 | container: 12 | image: rostooling/setup-ros-docker:ubuntu-focal-ros-noetic-ros-base-latest 13 | steps: 14 | - uses: ros-tooling/action-ros-ci@0.3.0 15 | with: 16 | package-name: rosbag_fancy 17 | target-ros1-distro: noetic 18 | -------------------------------------------------------------------------------- /rosbag_fancy/src/mem_str.h: -------------------------------------------------------------------------------- 1 | // Conversions of memory amounts and strings 2 | // Author: Max Schwarz 3 | 4 | #ifndef ROSBAG_FANCY_MEM_STR_H 5 | #define ROSBAG_FANCY_MEM_STR_H 6 | 7 | #include 8 | #include 9 | 10 | namespace rosbag_fancy 11 | { 12 | namespace mem_str 13 | { 14 | 15 | std::string memoryToString(uint64_t memory); 16 | uint64_t stringToMemory(std::string humanSize); 17 | 18 | } 19 | } 20 | 21 | #endif 22 | -------------------------------------------------------------------------------- /rosbag_fancy/src/cmd_test.cpp: -------------------------------------------------------------------------------- 1 | // test command 2 | // Author: Max Schwarz 3 | 4 | #include "doctest.h" 5 | 6 | #include 7 | #include 8 | 9 | int test(const std::vector& options) 10 | { 11 | doctest::Context context; 12 | 13 | std::vector values; 14 | values.push_back("dummy"); 15 | for(auto& str : options) 16 | values.push_back(str.c_str()); 17 | 18 | context.applyCommandLine(values.size(), values.data()); 19 | 20 | return context.run(); 21 | } 22 | -------------------------------------------------------------------------------- /rosbag_fancy/scripts/record_gif.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # This script is used to record the GIFs shown on GitHub. 4 | 5 | set -e 6 | 7 | export ROS_MASTER_URI=http://localhost:11411 8 | 9 | # Kill child processes on exit 10 | trap "trap - SIGTERM && kill -- -$$" SIGINT SIGTERM EXIT 11 | 12 | roscore -p 11411 & 13 | 14 | sleep 1 15 | 16 | rostopic pub -r 20.0 /tf tf2_msgs/TFMessage "{ transforms: [] }" & 17 | rostopic pub -r 0.2 /test tf2_msgs/TFMessage "{ transforms: [] }" & 18 | 19 | wget -nc 'https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/b0-2014-07-11-10-58-16.bag' 20 | 21 | rm -f out.bag 22 | 23 | vhs < play.tape 24 | -------------------------------------------------------------------------------- /rosbag_fancy/package.xml: -------------------------------------------------------------------------------- 1 | 2 | rosbag_fancy 3 | rosbag with terminal UI 4 | 1.1.0 5 | BSD 6 | Max Schwarz 7 | 8 | catkin 9 | boost 10 | bzip2 11 | libncurses-dev 12 | rosbag_fancy_msgs 13 | rosbag_storage 14 | roscpp 15 | roslz4 16 | rosfmt 17 | std_srvs 18 | tf2_ros 19 | topic_tools 20 | 21 | -------------------------------------------------------------------------------- /rosbag_fancy_msgs/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package rosbag_fancy_msgs 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.1.0 (2024-03-19) 6 | ------------------ 7 | * Add --no-ui for play with status info and start/stop/pause services (PR #19) 8 | * Contributors: Jan Quenzel, Max Schwarz 9 | 10 | 1.0.1 (2023-07-11) 11 | ------------------ 12 | 13 | 1.0.0 (2023-07-10) 14 | ------------------ 15 | * split into separate packages (main, _msgs, and _gui) 16 | * Contributors: Max Schwarz 17 | 18 | 0.2.0 (2020-06-16) 19 | ------------------ 20 | 21 | 0.1.1 (2019-10-22) 22 | ------------------ 23 | 24 | 0.1.0 (2019-10-18) 25 | ------------------ 26 | -------------------------------------------------------------------------------- /rqt_rosbag_fancy/package.xml: -------------------------------------------------------------------------------- 1 | 2 | rqt_rosbag_fancy 3 | rqt GUI for rosbag_fancy 4 | 1.1.0 5 | BSD 6 | 7 | Christian Lenz 8 | Max Schwarz 9 | 10 | catkin 11 | rosbag_fancy_msgs 12 | rosfmt 13 | rqt_gui 14 | rqt_gui_cpp 15 | std_srvs 16 | 17 | qtbase5-dev 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /rqt_rosbag_fancy/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package rqt_rosbag_fancy 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.1.0 (2024-03-19) 6 | ------------------ 7 | 8 | 1.0.1 (2023-07-11) 9 | ------------------ 10 | * add missing std_srvs dependency 11 | * Contributors: Max Schwarz 12 | 13 | 1.0.0 (2023-07-10) 14 | ------------------ 15 | * GUI/TUI: display messages in bag, not total messages 16 | * Add rqt GUI (PR #14) 17 | * rqt_rosbag_fancy: rosfmt/full.h is not available on Melodic 18 | * rename rosbag_fancy_gui to rqt_rosbag_fancy 19 | * Contributors: Max Schwarz 20 | 21 | 0.2.0 (2020-06-16) 22 | ------------------ 23 | 24 | 0.1.1 (2019-10-22) 25 | ------------------ 26 | 27 | 0.1.0 (2019-10-18) 28 | ------------------ 29 | -------------------------------------------------------------------------------- /rosbag_fancy/src/topic_subscriber.h: -------------------------------------------------------------------------------- 1 | // Subcribes to input topics 2 | // Author: Max Schwarz 3 | 4 | #ifndef ROSBAG_FANCY_TOPIC_SUBSCRIBER_H 5 | #define ROSBAG_FANCY_TOPIC_SUBSCRIBER_H 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | namespace topic_tools { class ShapeShifter; } 12 | 13 | namespace rosbag_fancy 14 | { 15 | 16 | class TopicManager; 17 | class MessageQueue; 18 | struct Topic; 19 | 20 | class TopicSubscriber 21 | { 22 | public: 23 | explicit TopicSubscriber(TopicManager& topicManager, MessageQueue& queue); 24 | private: 25 | void handle(Topic& topic, const ros::MessageEvent& msg); 26 | void updateStats(); 27 | 28 | TopicManager& m_topicManager; 29 | MessageQueue& m_queue; 30 | 31 | std::vector m_subscribers; 32 | 33 | ros::SteadyTimer m_timer; 34 | }; 35 | 36 | } 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /rosbag_fancy/src/tf2_scanner.h: -------------------------------------------------------------------------------- 1 | // Scans multiple bag files for tf2 messages and aggregates them 2 | // Author: Max Schwarz 3 | 4 | #ifndef ROSBAG_FANCY_TF2_SCANNER 5 | #define ROSBAG_FANCY_TF2_SCANNER 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | namespace rosbag_fancy 13 | { 14 | 15 | class BagReader; 16 | 17 | class TF2Scanner 18 | { 19 | public: 20 | explicit TF2Scanner(const std::vector& readers); 21 | ~TF2Scanner(); 22 | 23 | /** 24 | * @brief Fetch next aggregated message 25 | * 26 | * Returns the next tf_static message that needs to be published. 27 | * If there has been no change since the last call, it will return nullptr. 28 | **/ 29 | const tf2_msgs::TFMessage* fetchUpdate(const ros::Time& time); 30 | 31 | private: 32 | friend class Cursor; 33 | class Private; 34 | std::unique_ptr m_d; 35 | }; 36 | 37 | } 38 | 39 | #endif 40 | -------------------------------------------------------------------------------- /rosbag_fancy/tools/tf2_republisher.cpp: -------------------------------------------------------------------------------- 1 | // Consolidate & republish static transforms from a bag file 2 | // Author: Max Schwarz 3 | 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | 10 | int main(int argc, char** argv) 11 | { 12 | ros::init(argc, argv, "tf2_republisher"); 13 | 14 | ros::NodeHandle nh{"~"}; 15 | 16 | tf2_ros::StaticTransformBroadcaster pub_tf; 17 | 18 | using Msg = tf2_msgs::TFMessage; 19 | using Event = ros::MessageEvent; 20 | 21 | ros::Subscriber sub = nh.subscribe("/tf_static", 100, boost::function( 22 | [&](const Event& event) 23 | { 24 | // Did we publish this message? 25 | if(event.getPublisherName() == ros::this_node::getName()) 26 | return; 27 | 28 | pub_tf.sendTransform(event.getMessage()->transforms); 29 | } 30 | )); 31 | 32 | ros::spin(); 33 | 34 | return 0; 35 | } 36 | -------------------------------------------------------------------------------- /rqt_rosbag_fancy/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | cmake_minimum_required(VERSION 3.0) 3 | project(rqt_rosbag_fancy) 4 | 5 | find_package(catkin REQUIRED COMPONENTS 6 | rosbag_fancy_msgs 7 | rosfmt 8 | rqt_gui 9 | rqt_gui_cpp 10 | std_srvs 11 | ) 12 | 13 | catkin_package() 14 | 15 | include_directories(${catkin_INCLUDE_DIRS}) 16 | 17 | find_package(Qt5Widgets REQUIRED) 18 | 19 | include_directories(${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}) 20 | 21 | qt5_wrap_ui(UI_HDRS 22 | src/fancy_gui.ui 23 | ) 24 | 25 | qt5_wrap_cpp(MOC_SRCS 26 | src/fancy_gui.h 27 | src/topic_model.h 28 | ) 29 | 30 | add_library(rqt_rosbag_fancy 31 | ${UI_HDRS} 32 | ${MOC_SRCS} 33 | src/fancy_gui.cpp 34 | src/topic_model.cpp 35 | ) 36 | target_link_libraries(rqt_rosbag_fancy 37 | ${catkin_LIBRARIES} 38 | Qt5::Widgets 39 | ) 40 | 41 | install(TARGETS rqt_rosbag_fancy 42 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 43 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 44 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 45 | ) 46 | install(FILES ${PROJECT_SOURCE_DIR}/rqt_plugins.xml 47 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 48 | ) 49 | -------------------------------------------------------------------------------- /rosbag_fancy/contrib/doctest/LICENSE.txt: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2016-2021 Viktor Kirilov 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /rosbag_fancy/src/message_queue.cpp: -------------------------------------------------------------------------------- 1 | // In-Memory message buffer 2 | // Author: Max Schwarz 3 | 4 | #include "message_queue.h" 5 | 6 | namespace rosbag_fancy 7 | { 8 | 9 | MessageQueue::MessageQueue(uint64_t byteLimit) 10 | : m_byteLimit{byteLimit} 11 | { 12 | } 13 | 14 | bool MessageQueue::push(const rosbag_fancy::MessageQueue::Message& msg) 15 | { 16 | uint64_t len = msg.size(); 17 | if(m_bytesInQueue + len > m_byteLimit) 18 | return false; 19 | 20 | { 21 | std::unique_lock lock(m_mutex); 22 | m_queue.push(msg); 23 | m_bytesInQueue += len; 24 | m_msgsInQueue++; 25 | 26 | m_cond.notify_all(); 27 | } 28 | 29 | return true; 30 | } 31 | 32 | boost::optional MessageQueue::pop() 33 | { 34 | std::unique_lock lock(m_mutex); 35 | 36 | if(m_shuttingDown) 37 | return {}; 38 | 39 | while(m_queue.empty()) 40 | { 41 | m_cond.wait(lock); 42 | if(m_shuttingDown) 43 | return {}; 44 | } 45 | 46 | auto msg = m_queue.front(); 47 | uint64_t len = msg.size(); 48 | 49 | m_bytesInQueue -= len; 50 | m_msgsInQueue--; 51 | m_queue.pop(); 52 | 53 | return msg; 54 | } 55 | 56 | void MessageQueue::shutdown() 57 | { 58 | m_shuttingDown = true; 59 | m_cond.notify_all(); 60 | } 61 | 62 | } 63 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | 2 | rosbag_fancy 3 | ============ 4 | 5 | 6 | 7 | `rosbag_fancy` is a fancy terminal UI frontend for the venerable [rosbag] 8 | tool. 9 | 10 | Recording 11 | --------- 12 | 13 | At the moment, the `record` command is `rosbag_fancy`'s main feature. 14 | It offers the following advantages over plain `rosbag record`: 15 | 16 | * Live display of statistics per topic, such as number of messages, bandwidth, 17 | dropped messages, etc. Never notice *after* recording that you misspelled a 18 | topic name! 19 | * Bash completion for topic names 20 | * Optional per-topic rate limiting 21 | 22 | Info 23 | ---- 24 | 25 | `rosbag_fancy` also offers an `info` command similar to `rosbag info`, which 26 | has the same features but is much faster. 27 | 28 | Playback 29 | -------- 30 | 31 | Using `rosbag_fancy play ` you can play bag files interactively. 32 | Similar to `info`, this is much faster than plain `rosbag`. 33 | 34 | As an additional feature, `rosbag_fancy play` aggregates the `tf_static` 35 | topic over time, so no matter how many tf publishers were active or how 36 | much you seek in the file, the static transforms will be kept up-to-date. 37 | 38 | [rosbag]: http://wiki.ros.org/rosbag 39 | -------------------------------------------------------------------------------- /rosbag_fancy/scripts/play.tape: -------------------------------------------------------------------------------- 1 | # Where should we write the GIF? 2 | Output play.gif 3 | 4 | Set TypingSpeed 50ms 5 | Set FontSize 24 6 | Set Width 1300 7 | Set Height 680 8 | Set Padding 10 9 | Set Framerate 24 10 | 11 | ################################### RECORD ########################################### 12 | 13 | Type "rosbag_fancy record -o out.bag /tf /test /misspelled" 14 | Sleep 500ms 15 | Enter 16 | 17 | Sleep 5s 18 | 19 | Ctrl+C 20 | 21 | Sleep 1s 22 | 23 | ################################### INFO ########################################### 24 | 25 | Type "rosbag_fancy info out.bag" 26 | Sleep 500ms 27 | Enter 28 | 29 | Sleep 5s 30 | 31 | Hide 32 | Type "clear" 33 | Enter 34 | Show 35 | 36 | ################################### PLAY ############################################# 37 | # Type a command in the terminal. 38 | Type "rosbag_fancy play b0-2014-07-11-10-58-16.bag" 39 | 40 | # Pause for dramatic effect... 41 | Sleep 500ms 42 | 43 | # Run the command by pressing enter. 44 | Enter 45 | 46 | # Admire the output for a bit. 47 | Sleep 5s 48 | 49 | # Skip forward 50 | Right 51 | Sleep 500ms 52 | Right 53 | Sleep 500ms 54 | Right 55 | 56 | Sleep 2s 57 | 58 | # Skip backward 59 | Left 60 | Sleep 500ms 61 | Left 62 | Sleep 500ms 63 | Left 64 | 65 | Sleep 2s 66 | 67 | Type " " 68 | 69 | Sleep 2s 70 | 71 | Type " " 72 | 73 | Sleep 2s 74 | 75 | Type "q" 76 | -------------------------------------------------------------------------------- /rosbag_fancy/src/message_queue.h: -------------------------------------------------------------------------------- 1 | // In-Memory message buffer 2 | // Author: Max Schwarz 3 | 4 | #ifndef ROSBAG_FANCY_MESSAGE_QUEUE_H 5 | #define ROSBAG_FANCY_MESSAGE_QUEUE_H 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | #include 15 | 16 | #include 17 | 18 | namespace rosbag_fancy 19 | { 20 | 21 | struct Topic; 22 | 23 | class MessageQueue 24 | { 25 | public: 26 | struct Message 27 | { 28 | std::string topic; 29 | ros::MessageEvent message; 30 | 31 | uint64_t size() const 32 | { return message.getConstMessage()->size(); } 33 | 34 | Topic* topicData; 35 | }; 36 | 37 | explicit MessageQueue(uint64_t byteLimit); 38 | 39 | bool push(const Message& msg); 40 | boost::optional pop(); 41 | 42 | void shutdown(); 43 | 44 | uint64_t bytesInQueue() const 45 | { return m_bytesInQueue; } 46 | 47 | uint64_t messagesInQueue() const 48 | { return m_msgsInQueue; } 49 | private: 50 | std::queue m_queue; 51 | std::mutex m_mutex; 52 | std::condition_variable m_cond; 53 | 54 | std::atomic m_bytesInQueue{0}; 55 | std::atomic m_msgsInQueue{0}; 56 | uint64_t m_byteLimit; 57 | 58 | bool m_shuttingDown{false}; 59 | }; 60 | 61 | } 62 | 63 | #endif 64 | -------------------------------------------------------------------------------- /rqt_rosbag_fancy/src/topic_model.h: -------------------------------------------------------------------------------- 1 | // Qt table model for rosbag_fancy messages 2 | // Author: Christian Lenz 3 | 4 | #ifndef TOPIC_MODEL_H 5 | #define TOPIC_MODEL_H 6 | 7 | #include 8 | 9 | #include 10 | 11 | class QTimer; 12 | 13 | namespace rqt_rosbag_fancy 14 | { 15 | 16 | class TopicModel : public QAbstractTableModel 17 | { 18 | Q_OBJECT 19 | public: 20 | enum class Column 21 | { 22 | Activity, 23 | Name, 24 | Publisher, 25 | Messages, 26 | Rate, 27 | Bytes, 28 | Bandwidth, 29 | 30 | ColumnCount 31 | }; 32 | 33 | explicit TopicModel(QObject* parent = 0); 34 | virtual ~TopicModel(); 35 | 36 | int columnCount(const QModelIndex& parent) const override; 37 | int rowCount(const QModelIndex& parent) const override; 38 | 39 | QVariant data(const QModelIndex& index, int role) const override; 40 | QVariant headerData(int section, Qt::Orientation orientation, int role) const override; 41 | 42 | public Q_SLOTS: 43 | void setState(const rosbag_fancy_msgs::StatusConstPtr& status); 44 | 45 | private Q_SLOTS: 46 | void clear(); 47 | 48 | private: 49 | rosbag_fancy_msgs::StatusConstPtr m_status; 50 | bool m_valid = false; 51 | QTimer* m_timer; 52 | 53 | QString rateToString(double rate) const; 54 | QString memoryToString(uint64_t memory) const; 55 | 56 | std::vector m_lastMsgCount; 57 | }; 58 | 59 | } 60 | 61 | #endif 62 | -------------------------------------------------------------------------------- /rosbag_fancy/env-hooks/50-rosbag_fancy.bash: -------------------------------------------------------------------------------- 1 | 2 | # If you change this file, please increment the version number in 3 | # CMakeLists.txt to trigger a CMake update. 4 | 5 | function _rosbag_fancy() { 6 | local cur="${COMP_WORDS[COMP_CWORD]}" 7 | local cmd="${COMP_WORDS[1]}" 8 | 9 | if [[ "$COMP_CWORD" == "1" ]]; then 10 | COMPREPLY=( $(compgen -W "record info play --help" -- $cur) ) 11 | return 12 | fi 13 | 14 | case "${cmd}" in 15 | record) 16 | local FLAGS=( --help ) 17 | local OPTS=( --topic --queue-size -o --output -p --prefix ) 18 | 19 | # Are we currently inside an option? 20 | if [[ " ${OPTS[@]} " =~ " ${COMP_WORDS[COMP_CWORD-1]} " ]]; then 21 | case "${COMP_WORDS[COMP_CWORD-1]}" in 22 | --topic) 23 | COMPREPLY=( $(compgen -o nospace -W "$(rostopic list 2>/dev/null)" -- $cur) ) 24 | compopt -o nospace 25 | ;; 26 | -o|--output|-p|--prefix) 27 | COMPREPLY=( $(compgen -f -- $cur) ) 28 | ;; 29 | *) 30 | COMPREPLY=() 31 | ;; 32 | esac 33 | return 34 | fi 35 | 36 | COMPREPLY=( $(compgen -o nospace -W "${FLAGS[*]} ${OPTS[*]} $(rostopic list 2>/dev/null)" -- $cur) ) 37 | compopt -o nospace 38 | ;; 39 | info) 40 | COMPREPLY=( $(compgen -f -- $cur ) ) 41 | compopt -o nospace 42 | ;; 43 | play) 44 | local FLAGS=( --help --clock ) 45 | 46 | COMPREPLY=( $(compgen -o nospace -f -W "${FLAGS[*]}" -- $cur) ) 47 | compopt -o nospace 48 | ;; 49 | esac 50 | } 51 | complete -F _rosbag_fancy rosbag_fancy 52 | 53 | -------------------------------------------------------------------------------- /rosbag_fancy/src/topic_manager.cpp: -------------------------------------------------------------------------------- 1 | // Contains the topic configuration & status 2 | // Author: Max Schwarz 3 | 4 | #include "topic_manager.h" 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | namespace rosbag_fancy 12 | { 13 | 14 | constexpr float STAT_TIME = 0.5; 15 | 16 | const ros::WallTime Topic::T0 = ros::WallTime::now(); 17 | 18 | TopicManager::TopicManager() 19 | { 20 | ros::NodeHandle nh; 21 | m_timer = nh.createSteadyTimer(ros::WallDuration(STAT_TIME), 22 | boost::bind(&TopicManager::updateStatistics, this) 23 | ); 24 | } 25 | 26 | void TopicManager::addTopic(const std::string& topic, float rateLimit, int flags) 27 | { 28 | std::string resolvedName = ros::names::resolve(topic); 29 | 30 | auto it = std::find_if(m_topics.begin(), m_topics.end(), [&](Topic& t){ 31 | return t.name == resolvedName; 32 | }); 33 | 34 | if(it != m_topics.end()) 35 | { 36 | ROSFMT_WARN( 37 | "You tried to record topic '{}' twice. I'll ignore that (and use the first rate limit given, if applicable)", 38 | resolvedName 39 | ); 40 | return; 41 | } 42 | 43 | m_topics.emplace_back(resolvedName, m_topics.size(), rateLimit, flags); 44 | } 45 | 46 | void TopicManager::updateStatistics() 47 | { 48 | for(auto& topic : m_topics) 49 | { 50 | topic.messageRate = topic.messagesInStatsPeriod / STAT_TIME; 51 | topic.messagesInStatsPeriod = 0; 52 | 53 | topic.bandwidth = topic.bytesInStatsPeriod / STAT_TIME; 54 | topic.bytesInStatsPeriod = 0; 55 | } 56 | } 57 | 58 | } 59 | -------------------------------------------------------------------------------- /rqt_rosbag_fancy/src/fancy_gui.h: -------------------------------------------------------------------------------- 1 | // rosbag_fancy rqt gui 2 | // Author: Christian Lenz 3 | 4 | #ifndef ROSBAGFANCY_GUI_H 5 | #define ROSBAGFANCY_GUI_H 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | #include 13 | 14 | #include "topic_model.h" 15 | #include "ui_fancy_gui.h" 16 | 17 | namespace rqt_rosbag_fancy 18 | { 19 | 20 | class FancyGui : public rqt_gui_cpp::Plugin 21 | { 22 | Q_OBJECT 23 | public: 24 | FancyGui(); 25 | virtual ~FancyGui(); 26 | 27 | virtual void initPlugin(qt_gui_cpp::PluginContext& context) override; 28 | virtual void shutdownPlugin() override; 29 | virtual void saveSettings(qt_gui_cpp::Settings& plugin_settings, qt_gui_cpp::Settings& instance_settings) const override; 30 | virtual void restoreSettings(const qt_gui_cpp::Settings& plugin_settings, const qt_gui_cpp::Settings& instance_settings) override; 31 | 32 | Q_SIGNALS: 33 | void receivedStatus(const rosbag_fancy_msgs::StatusConstPtr& msg); 34 | public Q_SLOTS: 35 | void refreshTopicList(); 36 | void subscribe(); 37 | void updateView(const rosbag_fancy_msgs::StatusConstPtr& msg); 38 | 39 | private Q_SLOTS: 40 | void start(); 41 | void stop(); 42 | 43 | private: 44 | ros::Subscriber m_sub_status; 45 | 46 | QWidget* m_w; 47 | Ui_FancyGui m_ui; 48 | 49 | TopicModel m_model; 50 | 51 | std::string m_prefix; 52 | 53 | QString rateToString(double rate) const; 54 | QString memoryToString(uint64_t memory) const; 55 | }; 56 | 57 | } 58 | 59 | #endif 60 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2019, Max Schwarz 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | -------------------------------------------------------------------------------- /rosbag_fancy/src/main.cpp: -------------------------------------------------------------------------------- 1 | // Fancy rosbag terminal UI 2 | // Author: Max Schwarz 3 | 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | 10 | // Implemented in cmd_*.cpp 11 | int record(const std::vector& options); 12 | int info(const std::vector& options); 13 | int play(const std::vector& options); 14 | int test(const std::vector& options); 15 | 16 | int main(int argc, char** argv) 17 | { 18 | ros::init(argc, argv, "rosbag_fancy", ros::init_options::AnonymousName); 19 | 20 | auto usage = [](std::FILE* f){ 21 | fmt::print(f, 22 | "Usage: rosbag_fancy [args]\n\n" 23 | "Available commands:\n" 24 | " record: Record a bagfile\n" 25 | " info: Display information about a bagfile\n" 26 | " play: Play bagfile\n" 27 | " test: Run internal unit-tests\n" 28 | "\n" 29 | "See rosbag_fancy --help for command-specific instructions.\n" 30 | "\n" 31 | ); 32 | }; 33 | 34 | if(argc < 2) 35 | { 36 | usage(stderr); 37 | return 1; 38 | } 39 | 40 | std::string cmd = std::string(argv[1]); 41 | std::vector arguments(argc - 2); 42 | std::copy(argv + 2, argv + argc, arguments.begin()); 43 | 44 | if(cmd == "-h" || cmd == "--help") 45 | { 46 | usage(stdout); 47 | return 0; 48 | } 49 | 50 | if(cmd == "record") 51 | return record(arguments); 52 | else if(cmd == "info") 53 | return info(arguments); 54 | else if(cmd == "play") 55 | return play(arguments); 56 | else if(cmd == "test") 57 | return test(arguments); 58 | else 59 | { 60 | fmt::print(stderr, "Unknown command {}, see --help\n", cmd); 61 | return 1; 62 | } 63 | 64 | return 0; 65 | } 66 | 67 | -------------------------------------------------------------------------------- /rosbag_fancy/scripts/record_svg.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # This script is used to record the SVGs shown on GitHub. 4 | 5 | set -e 6 | 7 | export ROS_MASTER_URI=http://localhost:11411 8 | 9 | # Kill child processes on exit 10 | trap "trap - SIGTERM && kill -- -$$" SIGINT SIGTERM EXIT 11 | 12 | roscore -p 11411 & 13 | 14 | sleep 1 15 | 16 | rostopic pub -r 20.0 /tf tf2_msgs/TFMessage "{ transforms: [] }" & 17 | rostopic pub -r 0.2 /test tf2_msgs/TFMessage "{ transforms: [] }" & 18 | 19 | 20 | 21 | type() { 22 | str="$1" 23 | for (( i=0; i<${#str}; i++ )); do 24 | echo -ne "${str:$i:1}" 25 | sleep 0.05 26 | done 27 | } 28 | 29 | wget -nc 'https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/b0-2014-07-11-10-58-16.bag' 30 | 31 | rm -f play.cast 32 | rm -f out.bag 33 | 34 | export PS1="max@machine:~$ " 35 | ( 36 | sleep 0.1 37 | 38 | type $'rosbag_fancy record -o out.bag /tf /test /misspelled' 39 | sleep 0.5 40 | type $'\n' 41 | 42 | sleep 5 43 | 44 | killall -SIGINT rosbag_fancy 45 | 46 | sleep 1 47 | 48 | ######################### INFO 49 | 50 | type $'rosbag_fancy info out.bag' 51 | sleep 0.5 52 | type $'\n' 53 | 54 | sleep 5 55 | 56 | echo 'clear' 57 | 58 | ######################## PLAY 59 | 60 | sleep 0.5 61 | type $'rosbag_fancy play b0-2014-07-11-10-58-16.bag\n' 62 | sleep 5.0 63 | 64 | # Right arrow 65 | type $'\e[C' 66 | sleep 0.5 67 | type $'\e[C' 68 | sleep 0.5 69 | type $'\e[C' 70 | 71 | sleep 2.0 72 | 73 | # Left arrow 74 | type $'\e[D' 75 | sleep 0.5 76 | type $'\e[D' 77 | sleep 0.5 78 | type $'\e[D' 79 | 80 | sleep 2.0 81 | 82 | # Pause 83 | type ' ' 84 | 85 | sleep 2.0 86 | 87 | # Resume 88 | type ' ' 89 | 90 | sleep 2.0 91 | 92 | # Exit 93 | type 'q' 94 | 95 | echo 'exit' 96 | ) | asciinema rec --stdin -c "bash --norc" play.cast 97 | -------------------------------------------------------------------------------- /rosbag_fancy/src/ui.h: -------------------------------------------------------------------------------- 1 | // Terminal UI 2 | // Author: Max Schwarz 3 | 4 | #ifndef ROSBAG_FANCY_UI_H 5 | #define ROSBAG_FANCY_UI_H 6 | 7 | #include "terminal.h" 8 | #include "topic_manager.h" 9 | #include "message_queue.h" 10 | 11 | #include 12 | 13 | #include 14 | 15 | namespace rosbag_fancy 16 | { 17 | 18 | class BagWriter; 19 | class BagReader; 20 | 21 | class UI 22 | { 23 | public: 24 | UI(TopicManager& config, MessageQueue& queue, BagWriter& writer); 25 | 26 | void draw(); 27 | 28 | private: 29 | template 30 | void printLine(unsigned int& lineCounter, const Args& ... args); 31 | 32 | TopicManager& m_topicManager; 33 | MessageQueue& m_queue; 34 | BagWriter& m_bagWriter; 35 | 36 | Terminal m_term; 37 | 38 | ros::SteadyTimer m_timer; 39 | ros::WallTime m_lastDrawTime; 40 | }; 41 | 42 | class PlaybackUI 43 | { 44 | public: 45 | explicit PlaybackUI(TopicManager& topics, const ros::Time& startTime, const ros::Time& endTime); 46 | 47 | void setPositionInBag(const ros::Time& stamp); 48 | void setPaused(bool paused); 49 | 50 | void draw(); 51 | 52 | void handleInput(); 53 | 54 | boost::signals2::signal seekForwardRequested; 55 | boost::signals2::signal seekBackwardRequested; 56 | boost::signals2::signal pauseRequested; 57 | boost::signals2::signal exitRequested; 58 | 59 | private: 60 | template 61 | void printLine(unsigned int& lineCounter, const Args& ... args); 62 | 63 | TopicManager& m_topicManager; 64 | 65 | ros::Time m_startTime; 66 | ros::Time m_endTime; 67 | 68 | Terminal m_term; 69 | 70 | ros::SteadyTimer m_timer; 71 | ros::WallTime m_lastDrawTime; 72 | 73 | ros::Time m_positionInBag; 74 | 75 | ros::SteadyTime m_lastSeekFwd; 76 | ros::SteadyTime m_lastSeekBwd; 77 | 78 | bool m_paused = false; 79 | }; 80 | 81 | } 82 | 83 | #endif 84 | -------------------------------------------------------------------------------- /rosbag_fancy/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | cmake_minimum_required(VERSION 3.0) 3 | project(rosbag_fancy) 4 | 5 | find_package(catkin REQUIRED COMPONENTS 6 | rosbag_fancy_msgs 7 | rosbag_storage 8 | roscpp 9 | rosfmt 10 | roslz4 11 | std_srvs 12 | topic_tools 13 | tf2_ros 14 | ) 15 | 16 | catkin_package() 17 | 18 | include_directories(${catkin_INCLUDE_DIRS}) 19 | 20 | find_package(Curses REQUIRED) 21 | include_directories(${CURSES_INCLUDE_DIRS}) 22 | 23 | find_package(BZip2 REQUIRED) 24 | 25 | include_directories(contrib/doctest) 26 | 27 | set(CMAKE_CXX_STANDARD 17) 28 | 29 | add_executable(rosbag_fancy 30 | src/bag_reader.cpp 31 | src/bag_view.cpp 32 | src/bag_writer.cpp 33 | src/cmd_info.cpp 34 | src/cmd_play.cpp 35 | src/cmd_record.cpp 36 | src/cmd_test.cpp 37 | src/doctest.cpp 38 | src/main.cpp 39 | src/mem_str.cpp 40 | src/message_queue.cpp 41 | src/terminal.cpp 42 | src/tf2_scanner.cpp 43 | src/topic_manager.cpp 44 | src/topic_subscriber.cpp 45 | src/ui.cpp 46 | ) 47 | target_link_libraries(rosbag_fancy 48 | ${catkin_LIBRARIES} 49 | ${CURSES_LIBRARIES} 50 | BZip2::BZip2 51 | ) 52 | 53 | # Tools 54 | add_executable(tf2_republisher 55 | tools/tf2_republisher.cpp 56 | ) 57 | target_link_libraries(tf2_republisher 58 | ${catkin_LIBRARIES} 59 | ) 60 | 61 | # Shell helper 62 | # Version 1.11 (increment this comment to trigger a CMake update) 63 | catkin_add_env_hooks(50-rosbag_fancy 64 | SHELLS bash 65 | DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/env-hooks 66 | ) 67 | 68 | # Link executable into bin/ in the devel space 69 | add_custom_target(symlink ALL 70 | COMMAND ${CMAKE_COMMAND} -E make_directory ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_BIN_DESTINATION} 71 | COMMAND ${CMAKE_COMMAND} -E create_symlink $ ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_BIN_DESTINATION}/rosbag_fancy 72 | ) 73 | 74 | 75 | install(TARGETS rosbag_fancy tf2_republisher 76 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 77 | ) 78 | install(TARGETS rosbag_fancy 79 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 80 | ) 81 | -------------------------------------------------------------------------------- /rosbag_fancy/src/mem_str.cpp: -------------------------------------------------------------------------------- 1 | // Conversions of memory amounts and strings 2 | // Author: Max Schwarz 3 | 4 | #include "mem_str.h" 5 | 6 | #include 7 | 8 | #include 9 | 10 | namespace rosbag_fancy 11 | { 12 | namespace mem_str 13 | { 14 | 15 | std::string memoryToString(uint64_t memory) 16 | { 17 | if(memory < static_cast(1<<10)) 18 | return fmt::format("{}.0 B", memory); 19 | else if(memory < static_cast(1<<20)) 20 | return fmt::format("{:.1f} KiB", static_cast(memory) / static_cast(1<<10)); 21 | else if(memory < static_cast(1<<30)) 22 | return fmt::format("{:.1f} MiB", static_cast(memory) / static_cast(1<<20)); 23 | else if(memory < static_cast(1ull<<40)) 24 | return fmt::format("{:.1f} GiB", static_cast(memory) / static_cast(1ull<<30)); 25 | else 26 | return fmt::format("{:.1f} TiB", static_cast(memory) / static_cast(1ull<<40)); 27 | } 28 | 29 | uint64_t stringToMemory(std::string humanSize) 30 | { 31 | // format should be "10MB" or "3 GB" or "10 B" or "10GiB" 32 | static std::regex memoryRegex{R"EOS((\d+)\s*(K|M|G|T|E|)i?B?)EOS"}; 33 | 34 | std::smatch match; 35 | if(!std::regex_match(humanSize, match, memoryRegex)) 36 | { 37 | fmt::print(stderr, "Could not parse memory string '{}' - it should be something like '120B' or '10GB'\n", 38 | humanSize 39 | ); 40 | std::exit(1); 41 | } 42 | 43 | std::string number = match[1].str(); 44 | std::string unit = match[2].str(); 45 | 46 | std::uint64_t multiplier = [&]() -> std::uint64_t { 47 | if(unit.empty()) 48 | return 1; 49 | 50 | switch(unit[0]) 51 | { 52 | case 'K': return 1ULL << 10; 53 | case 'M': return 1ULL << 20; 54 | case 'G': return 1ULL << 30; 55 | case 'T': return 1ULL << 40; 56 | case 'E': return 1ULL << 50; 57 | } 58 | 59 | throw std::logic_error{"I got regexes wrong :("}; 60 | }(); 61 | 62 | return std::stoull(humanSize) * multiplier; 63 | } 64 | 65 | } 66 | } 67 | -------------------------------------------------------------------------------- /rosbag_fancy/src/bag_view.h: -------------------------------------------------------------------------------- 1 | // Filtering view on (multiple) bag files 2 | // Author: Max Schwarz 3 | 4 | #ifndef ROSBAG_FANCY_BAG_VIEW_H 5 | #define ROSBAG_FANCY_BAG_VIEW_H 6 | 7 | #include "bag_reader.h" 8 | 9 | namespace rosbag_fancy 10 | { 11 | 12 | class BagView 13 | { 14 | public: 15 | class Iterator; 16 | 17 | class MultiBagMessage 18 | { 19 | public: 20 | const BagReader::Message* msg = {}; 21 | unsigned int bagIndex = 0; 22 | }; 23 | 24 | class Iterator 25 | { 26 | public: 27 | using iterator_category = std::input_iterator_tag; 28 | using value_type = BagReader::Message; 29 | using reference = const MultiBagMessage&; 30 | using pointer = const MultiBagMessage*; 31 | 32 | Iterator() {} 33 | ~Iterator(); 34 | 35 | Iterator(const Iterator&) = default; 36 | Iterator& operator=(const Iterator&) = default; 37 | 38 | reference operator*(); 39 | pointer operator->() { return &(**this); } 40 | 41 | Iterator& operator++(); 42 | Iterator operator++(int) { Iterator tmp = *this; ++(*this); return tmp; } 43 | 44 | friend bool operator== (const Iterator& a, const Iterator& b); 45 | friend bool operator!= (const Iterator& a, const Iterator& b); 46 | 47 | private: 48 | friend class BagView; 49 | 50 | explicit Iterator(const BagView* view); 51 | Iterator(const BagView* view, const ros::Time& time); 52 | 53 | class Private; 54 | std::shared_ptr m_d; 55 | }; 56 | 57 | BagView(); 58 | ~BagView(); 59 | 60 | BagView(const BagView&) = delete; 61 | BagView& operator=(const BagView&) = delete; 62 | 63 | void addBag(BagReader* reader); 64 | void addBag(BagReader* reader, const std::function& connectionPredicate); 65 | 66 | ros::Time startTime() const; 67 | ros::Time endTime() const; 68 | 69 | Iterator begin() const; 70 | Iterator end() const; 71 | Iterator findTime(const ros::Time& time) const; 72 | 73 | private: 74 | friend class Iterator::Private; 75 | class Private; 76 | std::unique_ptr m_d; 77 | }; 78 | 79 | } 80 | 81 | #endif 82 | -------------------------------------------------------------------------------- /rosbag_fancy/src/topic_subscriber.cpp: -------------------------------------------------------------------------------- 1 | // Subcribes to input topics 2 | // Author: Max Schwarz 3 | 4 | #include "topic_subscriber.h" 5 | #include "message_queue.h" 6 | #include "topic_manager.h" 7 | 8 | namespace rosbag_fancy 9 | { 10 | 11 | TopicSubscriber::TopicSubscriber(rosbag_fancy::TopicManager& topicManager, rosbag_fancy::MessageQueue& queue) 12 | : m_topicManager{topicManager} 13 | , m_queue{queue} 14 | { 15 | ros::NodeHandle nh; 16 | 17 | for(auto& topic : topicManager.topics()) 18 | { 19 | ros::TransportHints transportHints; 20 | 21 | if(topic.flags & static_cast(Topic::Flag::UDP)) 22 | transportHints = transportHints.udp(); 23 | 24 | boost::function&)> cb{ 25 | boost::bind(&TopicSubscriber::handle, this, boost::ref(topic), _1) 26 | }; 27 | m_subscribers.push_back(nh.subscribe( 28 | topic.name, 10, 29 | cb, {}, transportHints 30 | )); 31 | } 32 | 33 | m_timer = nh.createSteadyTimer(ros::WallDuration(1.0), 34 | boost::bind(&TopicSubscriber::updateStats, this) 35 | ); 36 | } 37 | 38 | void TopicSubscriber::handle(Topic& topic, const ros::MessageEvent& msg) 39 | { 40 | std::uint64_t bytes = msg.getConstMessage()->size(); 41 | 42 | if(topic.rateLimit != 0.0f) 43 | { 44 | ros::Time now = ros::Time::now(); 45 | if(topic.lastMessageReceivedROSTime == ros::Time(0)) 46 | topic.lastMessageReceivedROSTime = now; 47 | else 48 | { 49 | // Basic token bucket algorithm for rate limiting 50 | ros::Duration elapsed = now - topic.lastMessageReceivedROSTime; 51 | topic.lastMessageReceivedROSTime = now; 52 | 53 | topic.throttleAllowance = std::min(2.0f, 54 | topic.throttleAllowance + static_cast(elapsed.toSec()) * topic.rateLimit 55 | ); 56 | 57 | if(topic.throttleAllowance < 1.0f) 58 | return; 59 | 60 | topic.throttleAllowance -= 1.0f; 61 | } 62 | } 63 | 64 | topic.notifyMessage(bytes); 65 | 66 | if(!m_queue.push({topic.name, msg, &topic})) 67 | topic.dropCounter++; 68 | } 69 | 70 | void TopicSubscriber::updateStats() 71 | { 72 | for(std::size_t i = 0; i < m_topicManager.topics().size(); ++i) 73 | { 74 | auto& topic = m_topicManager.topics()[i]; 75 | auto& sub = m_subscribers[i]; 76 | 77 | topic.numPublishers = sub.getNumPublishers(); 78 | } 79 | } 80 | 81 | 82 | } 83 | -------------------------------------------------------------------------------- /rosbag_fancy/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package rosbag_fancy 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.1.0 (2024-03-19) 6 | ------------------ 7 | * play: refuse to play non-monotonic bags 8 | * Add --no-ui for play with status info and start/stop/pause services (PR #19) 9 | * cmd_play: basic topic information in status message 10 | * rosbag_fancy: link executable for devel space 11 | Fixes rosbag_fancy usage in catkin devel spaces (which is the default 12 | with catkin_make and catkin_tools). I mainly test & develop with colcon, 13 | so I did not notice so far that the command was not available properly. 14 | * Contributors: Jan Quenzel, Max Schwarz 15 | 16 | 1.0.1 (2023-07-11) 17 | ------------------ 18 | * add missing std_srvs dependency 19 | * Contributors: Max Schwarz 20 | 21 | 1.0.0 (2023-07-10) 22 | ------------------ 23 | * play: decompression support (PR #18) 24 | * BagView & TF2 scanner (PR #17) 25 | * CI & unit tests 26 | * play: support for multiple bag files 27 | * play: ui: show current date/time 28 | * play: support for --clock 29 | * ui: remove sub column 30 | * play: faster startup, don't crash if select() is interrupted 31 | * play command 32 | * record: wait for ros::Time to become valid 33 | This fixes recording with use_sim_time=true (Issue #16) 34 | * tui: fix count display 35 | * tui: display byte & message counts correctly 36 | * GUI/TUI: display messages in bag, not total messages 37 | * rosbag_fancy: properly initialize compression 38 | * split into separate packages (main, _msgs, and _gui) 39 | * Contributors: Max Schwarz 40 | 41 | 0.2.0 (2020-06-16) 42 | ------------------ 43 | * bag_writer: fix uninitialized read in run() during shutdown 44 | This happens in certain cases during shutdown, where msg is not set. 45 | * bag_writer: make sure we don't overwrite bag files with same stamp 46 | * ui: move paused/recording indicator to bottom 47 | * bag_writer: special handling for tf2 static transforms on bag reopen 48 | * add --paused flag to start in paused mode 49 | * UI improvements for start/stop 50 | * start/stop service calls 51 | * add tf2_republisher tool (helpful for playback) 52 | * topic_manager: compile fix for clang 53 | * Contributors: Max Schwarz 54 | 55 | 0.1.1 (2019-10-22) 56 | ------------------ 57 | * Compatibility with ROS Kinetic (PR: #7) 58 | also helps with Debian Stretch 59 | * Contributors: Davide Faconti, Max Schwarz 60 | 61 | 0.1.0 (2019-10-18) 62 | ------------------ 63 | * Initial release 64 | * Contributors: Jan Quenzel, Max Schwarz 65 | -------------------------------------------------------------------------------- /rosbag_fancy/src/bag_writer.h: -------------------------------------------------------------------------------- 1 | // Output thread 2 | // Author: Max Schwarz 3 | 4 | #ifndef ROSBAG_FANCY_BAG_WRITER_H 5 | #define ROSBAG_FANCY_BAG_WRITER_H 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | #include 17 | 18 | namespace rosbag_fancy 19 | { 20 | 21 | class MessageQueue; 22 | 23 | class BagWriter 24 | { 25 | public: 26 | enum class Naming 27 | { 28 | Verbatim, 29 | AppendTimestamp 30 | }; 31 | 32 | explicit BagWriter( 33 | MessageQueue& queue, const std::string& filename, Naming namingMode, 34 | std::uint64_t splitSizeInBytes, std::uint64_t deleteOldAtInBytes 35 | ); 36 | ~BagWriter(); 37 | 38 | void start(); 39 | void stop(); 40 | bool running() const 41 | { return m_running; } 42 | 43 | std::uint64_t sizeInBytes() const 44 | { return m_sizeInBytes; } 45 | 46 | std::uint64_t splitSizeInBytes() const 47 | { return m_splitSizeInBytes; } 48 | 49 | std::uint64_t directorySizeInBytes() const 50 | { return m_directorySizeInBytes; } 51 | 52 | std::uint64_t deleteOldAtInBytes() const 53 | { return m_deleteOldAtInBytes; } 54 | 55 | std::uint64_t freeSpace() const 56 | { return m_freeSpace; } 57 | 58 | std::string bagfileName() const 59 | { return m_expandedFilename; } 60 | 61 | const std::vector& messageCounts() const 62 | { return m_messageCounts; } 63 | 64 | const std::vector& byteCounts() const 65 | { return m_byteCounts; } 66 | 67 | rosbag::compression::CompressionType compression() const 68 | { return m_compressionType; } 69 | 70 | void setCompression(rosbag::compression::CompressionType type); 71 | private: 72 | void run(); 73 | void cleanupThread(); 74 | 75 | void checkFreeSpace(); 76 | 77 | MessageQueue& m_queue; 78 | 79 | std::string m_filename; 80 | Naming m_namingMode; 81 | 82 | std::string m_expandedFilename; 83 | 84 | bool m_isReopeningBag{false}; 85 | std::uint64_t m_splitSizeInBytes = 0; 86 | std::uint64_t m_deleteOldAtInBytes = 0; 87 | std::atomic m_directorySizeInBytes{0}; 88 | 89 | rosbag::Bag m_bag; 90 | bool m_bagOpen{false}; 91 | 92 | std::thread m_thread; 93 | std::thread m_cleanup_thread; 94 | 95 | bool m_shouldShutdown{false}; 96 | 97 | std::atomic m_sizeInBytes{0}; 98 | std::uint64_t m_freeSpace = 0; 99 | 100 | ros::SteadyTimer m_freeSpaceTimer; 101 | 102 | std::atomic m_running{false}; 103 | std::mutex m_mutex; 104 | 105 | std::mutex m_cleanupMutex; 106 | std::condition_variable m_cleanupCondition; 107 | 108 | tf2_ros::Buffer m_tf_buf; 109 | boost::shared_ptr> m_tf_header; 110 | 111 | std::vector m_messageCounts; 112 | std::vector m_byteCounts; 113 | 114 | rosbag::compression::CompressionType m_compressionType = rosbag::compression::Uncompressed; 115 | }; 116 | 117 | } 118 | 119 | #endif 120 | -------------------------------------------------------------------------------- /rosbag_fancy/src/topic_manager.h: -------------------------------------------------------------------------------- 1 | // Contains the topic configuration & status 2 | // Author: Max Schwarz 3 | 4 | #ifndef ROSBAG_FANCY_TOPIC_CONFIG_MANAGER_H 5 | #define ROSBAG_FANCY_TOPIC_CONFIG_MANAGER_H 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | namespace rosbag_fancy 13 | { 14 | 15 | struct Topic 16 | { 17 | enum class Flag 18 | { 19 | UDP = (1 << 0) 20 | }; 21 | 22 | explicit Topic(const std::string& name, std::size_t id, float rateLimit = 0.0f, int flags = 0) 23 | : name(name), rateLimit(rateLimit), flags(flags) 24 | , id(id) 25 | { 26 | } 27 | 28 | Topic(const Topic& other) = delete; 29 | Topic& operator=(const Topic& other) = delete; 30 | 31 | Topic(Topic&& other) = default; 32 | Topic& operator=(Topic&& other) = default; 33 | 34 | std::string name; 35 | float rateLimit; 36 | int flags; 37 | 38 | std::size_t id; 39 | 40 | // Status 41 | ros::WallTime lastMessageTime; 42 | ros::Time lastMessageROSTime; 43 | ros::Time lastMessageReceivedROSTime; 44 | std::uint64_t messagesInStatsPeriod = 0; 45 | std::uint64_t bytesInStatsPeriod = 0; 46 | 47 | float throttleAllowance = 0.0f; 48 | 49 | float messageRate = 0.0f; 50 | float bandwidth = 0.0f; 51 | 52 | std::uint64_t dropCounter = 0; 53 | 54 | std::uint64_t totalMessages = 0; 55 | std::uint64_t totalBytes = 0; 56 | 57 | unsigned int numPublishers = 0; 58 | 59 | // The smooth rate estimate is taken from here: 60 | // https://stackoverflow.com/a/23617678 61 | 62 | float lambdaLast = 0.0f; 63 | float lambdaSmoothLast = 0.0f; 64 | 65 | static constexpr float HALF_LIFE = 1.0f; 66 | static constexpr float LOG05 = -0.6931471805599453f; // std::log(0.5f) 67 | static constexpr float DECAY = -LOG05/HALF_LIFE; 68 | static const ros::WallTime T0; 69 | 70 | void notifyMessage(std::uint64_t bytes) 71 | { 72 | ros::WallTime time = ros::WallTime::now(); 73 | 74 | totalMessages++; 75 | totalBytes += bytes; 76 | bytesInStatsPeriod += bytes; 77 | 78 | float tDelta = (time - lastMessageTime).toSec(); 79 | 80 | float expL = std::exp(-DECAY * tDelta); 81 | 82 | lambdaSmoothLast = DECAY * tDelta * expL * lambdaLast + expL * lambdaSmoothLast; 83 | lambdaLast = DECAY + expL * lambdaLast; 84 | lastMessageTime = time; 85 | lastMessageROSTime = ros::Time::now(); 86 | } 87 | 88 | inline float messageRateAt(const ros::WallTime& time) 89 | { 90 | float tDelta = (time - lastMessageTime).toSec(); 91 | float expL = std::exp(-DECAY * tDelta); 92 | 93 | // Bias correction 94 | float t0Delta = (time - T0).toSec(); 95 | float S = (1.0f + DECAY * t0Delta) * std::exp(-DECAY * t0Delta); 96 | 97 | return (DECAY * tDelta * expL * lambdaLast + expL * lambdaSmoothLast) / (1.0f - S); 98 | } 99 | }; 100 | 101 | class TopicManager 102 | { 103 | public: 104 | TopicManager(); 105 | 106 | inline std::vector& topics() 107 | { return m_topics; } 108 | 109 | void addTopic(const std::string& topic, float rateLimit = 0.0f, int flags = 0); 110 | private: 111 | void updateStatistics(); 112 | 113 | std::vector m_topics; 114 | ros::SteadyTimer m_timer; 115 | }; 116 | 117 | } 118 | 119 | #endif 120 | -------------------------------------------------------------------------------- /rosbag_fancy/src/terminal.h: -------------------------------------------------------------------------------- 1 | // Encapsulates terminal control (colors, cursor, ...) 2 | // Author: Max Schwarz 3 | 4 | #ifndef TERMINAL_H 5 | #define TERMINAL_H 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace rosbag_fancy 13 | { 14 | 15 | /** 16 | * @brief Encapsulates terminal control 17 | * 18 | * This class enables low-level manipulation of the terminal. It uses the 19 | * ncurses/terminfo library internally to stay somewhat portable. 20 | **/ 21 | class Terminal 22 | { 23 | public: 24 | /** 25 | * @brief Simple colors 26 | * 27 | * These colors can be used with the setSimpleForeground(), 28 | * setSimpleBackground(), setSimplePair() methods. 29 | **/ 30 | enum SimpleColor 31 | { 32 | Black, 33 | Red, 34 | Green, 35 | Yellow, 36 | Blue, 37 | Magenta, 38 | Cyan, 39 | White, 40 | IntenseBlack, 41 | IntenseRed, 42 | IntenseGreen, 43 | IntenseYellow, 44 | IntenseBlue, 45 | IntenseMagenta, 46 | IntenseCyan, 47 | IntenseWhite, 48 | 49 | //! 24-step grayscale starts here 50 | Grayscale = 0xe8 51 | }; 52 | 53 | /** 54 | * @brief Terminal escape sequence parser 55 | * 56 | * This class allows the user to parse Linux escape sequences 57 | * (restricted to simple color sequences for now). 58 | **/ 59 | class Parser 60 | { 61 | public: 62 | Parser(); 63 | 64 | //! parse single character c 65 | void parse(char c); 66 | 67 | //! parse string 68 | void parse(const std::string& str); 69 | 70 | //! Apply the current internal state (colors) on the terminal 71 | void apply(Terminal* term); 72 | private: 73 | void parseSetAttributes(const std::string& str); 74 | 75 | enum State 76 | { 77 | STATE_ESCAPE, 78 | STATE_TYPE, 79 | STATE_CSI 80 | }; 81 | 82 | State m_state; 83 | std::string m_buf; 84 | 85 | int m_fgColor; 86 | int m_bgColor; 87 | bool m_bold; 88 | }; 89 | 90 | Terminal(); 91 | 92 | /** 93 | * @brief Set 24-bit foreground color 94 | * 95 | * This automatically falls back to 256 colors if the terminal does not 96 | * support true color. 97 | **/ 98 | void setForegroundColor(uint32_t color); 99 | 100 | /** 101 | * @brief Set 24-bit background color 102 | * 103 | * This automatically falls back to 256 colors if the terminal does not 104 | * support true color. 105 | **/ 106 | void setBackgroundColor(uint32_t color); 107 | 108 | //! hide cursor 109 | void setCursorInvisible(); 110 | 111 | //! restore cursor 112 | void setCursorVisible(); 113 | 114 | /** 115 | * Enable/disable automatic echo of keypresses 116 | **/ 117 | void setEcho(bool on); 118 | 119 | void setBold(bool on); 120 | 121 | //! @name Set indexed foreground/background color 122 | //@{ 123 | void setSimpleBackground(SimpleColor color); 124 | void setSimpleForeground(SimpleColor color); 125 | void setSimplePair(SimpleColor fg, SimpleColor bg); 126 | //@} 127 | 128 | //! Reset fg + bg to standard terminal colors 129 | void setStandardColors(); 130 | 131 | //! Clear characters from cursor to end of line 132 | void clearToEndOfLine(); 133 | 134 | //! Move cursor up by numLines 135 | void moveCursorUp(int numLines); 136 | 137 | //! Move cursor to start of the line 138 | void moveCursorToStartOfLine(); 139 | 140 | /** 141 | * @brief Get current window size 142 | * 143 | * @return true on success 144 | **/ 145 | bool getSize(int* columns, int* rows); 146 | 147 | /** 148 | * Returns whether the terminal supports 256 colors. If it does not suppport 149 | * 256 colors, you should not use the setForegroundColor() / 150 | * setBackgroundColor() functions. 151 | **/ 152 | bool has256Colors() const; 153 | 154 | /** 155 | * Terminal supports escape codes 156 | **/ 157 | bool interactive() const 158 | { return m_valid; } 159 | 160 | void setWindowTitle(const std::string& title); 161 | void clearWindowTitle(const std::string& backup); 162 | 163 | 164 | enum SpecialKey 165 | { 166 | SK_F1 = 0x100, SK_F2, SK_F3, SK_F4, 167 | SK_F5, SK_F6, SK_F7, SK_F8, 168 | SK_F9, SK_F10, SK_F11, SK_F12, 169 | SK_Left, SK_Right, SK_Up, SK_Down, SK_Backspace 170 | }; 171 | 172 | int readKey(); 173 | 174 | private: 175 | bool m_valid; 176 | bool m_256colors; 177 | bool m_truecolor; 178 | 179 | std::string m_bgColorStr; 180 | std::string m_fgColorStr; 181 | std::string m_opStr; 182 | std::string m_sgr0Str; 183 | std::string m_elStr; 184 | std::string m_upStr; 185 | std::string m_boldStr; 186 | 187 | std::map m_specialKeys; 188 | 189 | std::string m_currentEscapeStr; 190 | std::chrono::steady_clock::time_point m_escapeStartTime; 191 | bool m_currentEscapeAborted = false; 192 | unsigned int m_currentEscapeAbortIdx = 0; 193 | }; 194 | 195 | } 196 | 197 | #endif 198 | -------------------------------------------------------------------------------- /rosbag_fancy/src/cmd_info.cpp: -------------------------------------------------------------------------------- 1 | // Info command 2 | // Author: Max Schwarz 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | #include "bag_reader.h" 13 | #include "mem_str.h" 14 | #include "rosbag/stream.h" 15 | 16 | namespace po = boost::program_options; 17 | using namespace rosbag_fancy; 18 | 19 | int info(const std::vector& options) 20 | { 21 | po::variables_map vm; 22 | 23 | // Handle CLI arguments 24 | { 25 | po::options_description desc("Options"); 26 | desc.add_options() 27 | ("help", "Display this help message") 28 | ; 29 | 30 | po::options_description hidden("Hidden"); 31 | hidden.add_options() 32 | ("input", po::value()->required(), "Input file") 33 | ; 34 | 35 | po::options_description all("All"); 36 | all.add(desc).add(hidden); 37 | 38 | po::positional_options_description p; 39 | p.add("input", 1); 40 | 41 | auto usage = [&](){ 42 | std::cout << "Usage: rosbag_fancy info [options] \n\n"; 43 | std::cout << desc << "\n\n"; 44 | }; 45 | 46 | try 47 | { 48 | po::store( 49 | po::command_line_parser(options).options(all).positional(p).run(), 50 | vm 51 | ); 52 | 53 | if(vm.count("help")) 54 | { 55 | usage(); 56 | return 0; 57 | } 58 | 59 | po::notify(vm); 60 | } 61 | catch(po::error& e) 62 | { 63 | std::cerr << "Could not parse arguments: " << e.what() << "\n\n"; 64 | usage(); 65 | return 1; 66 | } 67 | } 68 | 69 | std::string filename = vm["input"].as(); 70 | 71 | BagReader reader(filename); 72 | 73 | std::map> connectionsForTopic; 74 | for(auto& c : reader.connections()) 75 | connectionsForTopic[c.second.topicInBag].push_back(c.second.id); 76 | 77 | // A lot of std::chrono magic to get local/UTC time 78 | std::chrono::time_point startTimeC(std::chrono::nanoseconds(reader.startTime().toNSec())); 79 | std::chrono::time_point endTimeC(std::chrono::nanoseconds(reader.endTime().toNSec())); 80 | 81 | std::chrono::seconds startTimeS = std::chrono::duration_cast(startTimeC.time_since_epoch()); 82 | std::time_t startTimeSC(startTimeS.count()); 83 | std::tm startTimeB; 84 | std::tm startTimeBUTC; 85 | localtime_r(&startTimeSC, &startTimeB); 86 | gmtime_r(&startTimeSC, &startTimeBUTC); 87 | 88 | std::chrono::seconds endTimeS = std::chrono::duration_cast(endTimeC.time_since_epoch()); 89 | std::time_t endTimeSC(endTimeS.count()); 90 | std::tm endTimeB; 91 | std::tm endTimeBUTC; 92 | localtime_r(&endTimeSC, &endTimeB); 93 | gmtime_r(&endTimeSC, &endTimeBUTC); 94 | 95 | std::chrono::nanoseconds duration{(reader.endTime() - reader.startTime()).toNSec()}; 96 | 97 | fmt::print("File: {}\n", filename); 98 | fmt::print("Start time: {:%Y-%m-%d %H:%M:%S} ({}) / {:%Y-%m-%d %H:%M:%S} (UTC)\n", startTimeB, daylight ? tzname[1] : tzname[0], startTimeBUTC); 99 | fmt::print("End time: {:%Y-%m-%d %H:%M:%S} ({}) / {:%Y-%m-%d %H:%M:%S} (UTC)\n", endTimeB, daylight ? tzname[1] : tzname[0], endTimeBUTC); 100 | fmt::print("Duration: {:%H:%M:%S} ({:.2f}s)\n", duration, (reader.endTime() - reader.startTime()).toSec()); 101 | fmt::print("Size: {}\n", mem_str::memoryToString(reader.size())); 102 | 103 | auto it = reader.begin(); 104 | if(it != reader.end()) 105 | { 106 | fmt::print("Compression: "); 107 | switch(it.currentChunkCompression()) 108 | { 109 | case rosbag::compression::Uncompressed: fmt::print("Uncompressed\n"); break; 110 | case rosbag::compression::BZ2: fmt::print("BZ2\n"); break; 111 | case rosbag::compression::LZ4: fmt::print("LZ4\n"); break; 112 | default: fmt::print("unknown\n"); 113 | } 114 | } 115 | 116 | fmt::print("Types:\n"); 117 | std::map> types; 118 | for(auto& con : reader.connections()) 119 | types[con.second.type].insert(con.second.md5sum); 120 | 121 | std::size_t maxTypeLength = 0; 122 | for(auto& t : types) 123 | maxTypeLength = std::max(maxTypeLength, t.first.size()); 124 | 125 | for(auto& t : types) 126 | fmt::print(" - {:{}} {}\n", t.first, maxTypeLength, t.second); 127 | 128 | fmt::print("Topics:\n"); 129 | std::size_t maxTopicLength = 0; 130 | for(auto& topicPair : connectionsForTopic) 131 | maxTopicLength = std::max(maxTopicLength, topicPair.first.size()); 132 | 133 | for(auto& topicPair : connectionsForTopic) 134 | { 135 | std::uint64_t count = 0; 136 | std::set types; 137 | for(auto& conID : topicPair.second) 138 | { 139 | auto it = reader.connections().find(conID); 140 | auto& conn = it->second; 141 | 142 | types.insert(conn.type); 143 | 144 | count += conn.totalCount; 145 | } 146 | 147 | fmt::print(" - {:{}} {:8d} msgs: ", 148 | topicPair.first, maxTopicLength, count 149 | ); 150 | 151 | if(types.size() == 1) 152 | fmt::print("{:10}", *types.begin()); 153 | else 154 | fmt::print("{:10}", types); 155 | 156 | if(topicPair.second.size() > 1) 157 | { 158 | fmt::print(" ({} connections)", 159 | topicPair.second.size() 160 | ); 161 | } 162 | 163 | fmt::print("\n"); 164 | } 165 | 166 | return 0; 167 | } 168 | -------------------------------------------------------------------------------- /rqt_rosbag_fancy/src/topic_model.cpp: -------------------------------------------------------------------------------- 1 | // Qt table model for rosbag_fancy messages 2 | // Author: Christian Lenz 3 | 4 | #include "topic_model.h" 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | namespace rqt_rosbag_fancy 11 | { 12 | 13 | TopicModel::TopicModel(QObject* parent) 14 | : QAbstractTableModel(parent) 15 | { 16 | m_timer = new QTimer(this); 17 | m_timer->setSingleShot(true); 18 | m_timer->setInterval(3000); 19 | 20 | connect(m_timer, &QTimer::timeout, this, &TopicModel::clear); 21 | } 22 | 23 | TopicModel::~TopicModel() 24 | { 25 | } 26 | 27 | void TopicModel::clear() 28 | { 29 | m_valid = false; 30 | 31 | dataChanged( 32 | index(0,0), 33 | index(m_status->topics.size()-1, static_cast(Column::ColumnCount)-1) 34 | ); 35 | } 36 | 37 | int TopicModel::columnCount(const QModelIndex& parent) const 38 | { 39 | if(parent.isValid()) 40 | return 0; 41 | 42 | return static_cast(Column::ColumnCount); 43 | } 44 | 45 | int TopicModel::rowCount(const QModelIndex& parent) const 46 | { 47 | if(parent.isValid()) 48 | return 0; 49 | 50 | if(!m_status) 51 | return 0; 52 | 53 | return m_status->topics.size(); 54 | } 55 | 56 | QVariant TopicModel::data(const QModelIndex& index, int role) const 57 | { 58 | if(!m_status) 59 | return 0; 60 | 61 | if(index.parent().isValid()) 62 | return QVariant(); 63 | 64 | int column = index.column(); 65 | if(column < 0 || column >= static_cast(Column::ColumnCount)) 66 | return QVariant(); 67 | 68 | Column col = static_cast(column); 69 | 70 | int row = index.row(); 71 | if(row < 0 || row >= (int)m_status->topics.size()) 72 | return QVariant(); 73 | 74 | const auto& topic = m_status->topics[row]; 75 | 76 | bool active = topic.messages > m_lastMsgCount[row]; 77 | 78 | switch(role) 79 | { 80 | case Qt::ForegroundRole: 81 | if(m_valid) 82 | { 83 | if(topic.publishers < 1 && col == Column::Publisher) 84 | return QColor(Qt::white); 85 | else 86 | return QVariant{}; 87 | } 88 | else 89 | return QVariant{}; 90 | 91 | break; 92 | 93 | case Qt::BackgroundRole: 94 | if(m_valid) 95 | { 96 | if(active && col == Column::Activity && m_status->status == rosbag_fancy_msgs::Status::STATUS_RUNNING) 97 | return QColor(Qt::green); 98 | else if(topic.publishers < 1 && col == Column::Publisher) 99 | return QColor(Qt::red); 100 | else 101 | return QVariant{}; 102 | } 103 | else 104 | return QColor(Qt::gray); 105 | 106 | break; 107 | 108 | case Qt::DisplayRole: 109 | switch(col) 110 | { 111 | case Column::Activity: 112 | return QString(""); 113 | case Column::Name: 114 | return QString::fromStdString(topic.name); 115 | case Column::Publisher: 116 | return QString::number(topic.publishers); 117 | case Column::Messages: 118 | return QString::number(topic.messages_in_current_bag); 119 | case Column::Rate: 120 | return rateToString(topic.rate); 121 | case Column::Bytes: 122 | return memoryToString(topic.bytes); 123 | case Column::Bandwidth: 124 | return memoryToString(topic.bandwidth) + "/s"; 125 | default: 126 | return QVariant(); 127 | } 128 | break; 129 | 130 | case Qt::TextAlignmentRole: 131 | switch(col) 132 | { 133 | case Column::Name: 134 | return int(Qt::AlignLeft | Qt::AlignVCenter); 135 | 136 | default: 137 | return int(Qt::AlignRight | Qt::AlignVCenter); 138 | } 139 | break; 140 | } 141 | 142 | return QVariant(); 143 | } 144 | 145 | QVariant TopicModel::headerData(int section, Qt::Orientation orientation, int role) const 146 | { 147 | if(role != Qt::DisplayRole || orientation != Qt::Horizontal) 148 | return QVariant(); 149 | 150 | if(section < 0 || section > static_cast(Column::ColumnCount)) 151 | return QVariant(); 152 | 153 | Column col = static_cast(section); 154 | 155 | switch(col) 156 | { 157 | case Column::Activity: 158 | return "Act"; 159 | case Column::Name: 160 | return "Name"; 161 | case Column::Publisher: 162 | return "Publisher"; 163 | case Column::Messages: 164 | return "Messages"; 165 | case Column::Rate: 166 | return "Rate"; 167 | case Column::Bytes: 168 | return "Bytes"; 169 | case Column::Bandwidth: 170 | return "Bandwidth"; 171 | default: 172 | return QVariant(); 173 | } 174 | } 175 | 176 | void TopicModel::setState(const rosbag_fancy_msgs::StatusConstPtr& state) 177 | { 178 | if(!m_status || m_status->topics.size() != state->topics.size()) 179 | { 180 | m_lastMsgCount.clear(); 181 | m_lastMsgCount.reserve(state->topics.size()); 182 | 183 | for(auto& t : state->topics) 184 | m_lastMsgCount.push_back(t.messages); 185 | 186 | beginResetModel(); 187 | m_status = state; 188 | endResetModel(); 189 | 190 | return; 191 | } 192 | 193 | for(unsigned int i=0;itopics.size();i++) 194 | m_lastMsgCount[i] = m_status->topics[i].messages; 195 | 196 | m_status = state; 197 | m_valid = true; 198 | dataChanged( 199 | index(0,0), 200 | index(m_status->topics.size()-1, static_cast(Column::ColumnCount)-1) 201 | ); 202 | 203 | m_timer->start(); 204 | } 205 | 206 | QString TopicModel::rateToString(double rate) const 207 | { 208 | std::string s; 209 | if(rate < 1000.0) 210 | s = fmt::format("{:.1f} Hz", rate); 211 | else if(rate < 1e6) 212 | s = fmt::format("{:.1f} kHz", rate / 1e3); 213 | else 214 | s = fmt::format("{:.1f} MHz", rate / 1e6); 215 | 216 | return QString::fromStdString(s); 217 | } 218 | 219 | QString TopicModel::memoryToString(uint64_t memory) const 220 | { 221 | std::string s; 222 | if(memory < static_cast(1<<10)) 223 | s = fmt::format("{}.0 B", memory); 224 | else if(memory < static_cast(1<<20)) 225 | s = fmt::format("{:.1f} KiB", static_cast(memory) / static_cast(1<<10)); 226 | else if(memory < static_cast(1<<30)) 227 | s = fmt::format("{:.1f} MiB", static_cast(memory) / static_cast(1<<20)); 228 | else if(memory < static_cast(1ull<<40)) 229 | s = fmt::format("{:.1f} GiB", static_cast(memory) / static_cast(1ull<<30)); 230 | else 231 | s = fmt::format("{:.1f} TiB", static_cast(memory) / static_cast(1ull<<40)); 232 | 233 | return QString::fromStdString(s); 234 | } 235 | 236 | } 237 | -------------------------------------------------------------------------------- /rqt_rosbag_fancy/src/fancy_gui.cpp: -------------------------------------------------------------------------------- 1 | // rosbag_fancy GUI 2 | // Author: Christian Lenz 3 | 4 | #include "fancy_gui.h" 5 | 6 | #include "topic_model.h" 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | #include 17 | 18 | Q_DECLARE_METATYPE(rosbag_fancy_msgs::StatusConstPtr) 19 | 20 | namespace rqt_rosbag_fancy 21 | { 22 | 23 | FancyGui::FancyGui() 24 | { 25 | } 26 | 27 | FancyGui::~FancyGui() 28 | { 29 | } 30 | 31 | void FancyGui::initPlugin(qt_gui_cpp::PluginContext& context) 32 | { 33 | m_w = new QWidget; 34 | m_ui.setupUi(m_w); 35 | 36 | qRegisterMetaType(); 37 | connect(this, &FancyGui::receivedStatus, &m_model, &TopicModel::setState, Qt::QueuedConnection); 38 | connect(this, &FancyGui::receivedStatus, this, &FancyGui::updateView, Qt::QueuedConnection); 39 | 40 | m_ui.tableView->setModel(&m_model); 41 | 42 | QHeaderView *verticalHeader = m_ui.tableView->verticalHeader(); 43 | verticalHeader->setDefaultSectionSize(verticalHeader->fontMetrics().height()+2); 44 | verticalHeader->hide(); 45 | 46 | connect(m_ui.prefixComboBox, QOverload::of(&QComboBox::activated), this, &FancyGui::subscribe); 47 | connect(m_ui.refreshButton, &QToolButton::clicked, this, &FancyGui::refreshTopicList); 48 | 49 | connect(m_ui.startButton, &QPushButton::clicked, this, &FancyGui::start); 50 | connect(m_ui.stopButton, &QPushButton::clicked, this, &FancyGui::stop); 51 | 52 | context.addWidget(m_w); 53 | } 54 | 55 | void FancyGui::shutdownPlugin() 56 | { 57 | m_sub_status.shutdown(); 58 | } 59 | 60 | void FancyGui::saveSettings(qt_gui_cpp::Settings& plugin_settings, qt_gui_cpp::Settings& instance_settings) const 61 | { 62 | qt_gui_cpp::Plugin::saveSettings(plugin_settings, instance_settings); 63 | 64 | instance_settings.setValue("prefix", QString::fromStdString(m_prefix)); 65 | instance_settings.setValue("columns", m_ui.tableView->horizontalHeader()->saveState()); 66 | } 67 | 68 | void FancyGui::restoreSettings(const qt_gui_cpp::Settings& plugin_settings, const qt_gui_cpp::Settings& instance_settings) 69 | { 70 | qt_gui_cpp::Plugin::restoreSettings(plugin_settings, instance_settings); 71 | 72 | m_prefix = instance_settings.value("prefix").toString().toStdString(); 73 | refreshTopicList(); 74 | 75 | m_ui.tableView->horizontalHeader()->restoreState(instance_settings.value("columns").toByteArray()); 76 | } 77 | 78 | void FancyGui::refreshTopicList() 79 | { 80 | ros::master::V_TopicInfo topics; 81 | ros::master::getTopics(topics); 82 | 83 | m_ui.prefixComboBox->clear(); 84 | 85 | int idx = 0; 86 | for(const ros::master::TopicInfo& topic : topics) 87 | { 88 | if(topic.datatype != "rosbag_fancy_msgs/Status") 89 | continue; 90 | 91 | QString name = QString::fromStdString(topic.name); 92 | 93 | // Strip /status suffix 94 | if(name.endsWith("/status")) 95 | name = name.left(name.length() - 7); 96 | else 97 | continue; 98 | 99 | m_ui.prefixComboBox->addItem(name); 100 | if(name.toStdString() == m_prefix) 101 | { 102 | m_ui.prefixComboBox->setCurrentIndex(idx); 103 | subscribe(); 104 | } 105 | ++idx; 106 | } 107 | 108 | } 109 | 110 | void FancyGui::subscribe() 111 | { 112 | ros::NodeHandle nh; 113 | std::string prefix = m_ui.prefixComboBox->currentText().toStdString(); 114 | m_prefix = prefix; 115 | 116 | m_sub_status = nh.subscribe(prefix + "/status", 1, &FancyGui::receivedStatus, this); 117 | 118 | m_w->setWindowTitle(QString::fromStdString(m_prefix)); 119 | } 120 | 121 | void FancyGui::updateView(const rosbag_fancy_msgs::StatusConstPtr& msg) 122 | { 123 | m_ui.file_name->setText(QString::fromStdString(msg->bagfile)); 124 | m_ui.bandwidth->setText(memoryToString(msg->bandwidth) + "/s"); 125 | m_ui.size->setText(memoryToString(msg->bytes)); 126 | m_ui.free_space->setText(memoryToString(msg->free_bytes)); 127 | 128 | int totalMsgs = 0; 129 | for(auto& t : msg->topics) 130 | totalMsgs += t.messages; 131 | 132 | m_ui.messages->setText(QString::number(totalMsgs)); 133 | 134 | switch(msg->status) 135 | { 136 | case rosbag_fancy_msgs::Status::STATUS_PAUSED: 137 | m_ui.status->setText("PAUSED"); 138 | m_ui.status->setStyleSheet("color: white; background-color: red;"); 139 | m_ui.startButton->setEnabled(true); 140 | m_ui.stopButton->setEnabled(false); 141 | break; 142 | case rosbag_fancy_msgs::Status::STATUS_RUNNING: 143 | m_ui.status->setText("RUNNING"); 144 | m_ui.status->setStyleSheet("color: white; background-color: green;"); 145 | m_ui.startButton->setEnabled(false); 146 | m_ui.stopButton->setEnabled(true); 147 | break; 148 | default: 149 | m_ui.status->setText("UNKNOWN"); 150 | m_ui.status->setStyleSheet(""); 151 | m_ui.startButton->setEnabled(false); 152 | m_ui.stopButton->setEnabled(false); 153 | break; 154 | } 155 | 156 | m_ui.tableView->resizeRowsToContents(); 157 | } 158 | 159 | QString FancyGui::rateToString(double rate) const 160 | { 161 | std::string s; 162 | if(rate < 1000.0) 163 | s = fmt::format("{:.1f} Hz", rate); 164 | else if(rate < 1e6) 165 | s = fmt::format("{:.1f} kHz", rate / 1e3); 166 | else 167 | s = fmt::format("{:.1f} MHz", rate / 1e6); 168 | 169 | return QString::fromStdString(s); 170 | } 171 | 172 | QString FancyGui::memoryToString(uint64_t memory) const 173 | { 174 | std::string s; 175 | if(memory < static_cast(1<<10)) 176 | s = fmt::format("{}.0 B", memory); 177 | else if(memory < static_cast(1<<20)) 178 | s = fmt::format("{:.1f} KiB", static_cast(memory) / static_cast(1<<10)); 179 | else if(memory < static_cast(1<<30)) 180 | s = fmt::format("{:.1f} MiB", static_cast(memory) / static_cast(1<<20)); 181 | else if(memory < static_cast(1ull<<40)) 182 | s = fmt::format("{:.1f} GiB", static_cast(memory) / static_cast(1ull<<30)); 183 | else 184 | s = fmt::format("{:.1f} TiB", static_cast(memory) / static_cast(1ull<<40)); 185 | 186 | return QString::fromStdString(s); 187 | } 188 | 189 | void FancyGui::start() 190 | { 191 | std_srvs::Trigger srv; 192 | if(!ros::service::call(m_prefix + "/start", srv)) 193 | QMessageBox::critical(m_w, "Error", "Could not call start service"); 194 | } 195 | 196 | void FancyGui::stop() 197 | { 198 | std_srvs::Trigger srv; 199 | if(!ros::service::call(m_prefix + "/stop", srv)) 200 | QMessageBox::critical(m_w, "Error", "Could not call stop service"); 201 | } 202 | 203 | } 204 | 205 | PLUGINLIB_EXPORT_CLASS(rqt_rosbag_fancy::FancyGui, rqt_gui_cpp::Plugin) 206 | -------------------------------------------------------------------------------- /rqt_rosbag_fancy/src/fancy_gui.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | FancyGui 4 | 5 | 6 | 7 | 0 8 | 0 9 | 1027 10 | 672 11 | 12 | 13 | 14 | rosbag_fancy 15 | 16 | 17 | 18 | 19 | 20 | 0 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | Qt::AlignCenter 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 0 37 | 0 38 | 39 | 40 | 41 | 42 | 43 | 44 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 45 | 46 | 47 | 48 | 49 | 50 | 51 | Size: 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 62 | 63 | 64 | 65 | 66 | 67 | 68 | Messages: 69 | 70 | 71 | 72 | 73 | 74 | 75 | Bandwidth: 76 | 77 | 78 | 79 | 80 | 81 | 82 | Free space: 83 | 84 | 85 | 86 | 87 | 88 | 89 | Status: 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 0 98 | 0 99 | 100 | 101 | 102 | 103 | 104 | 105 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 116 | 117 | 118 | 119 | 120 | 121 | 122 | Node 123 | 124 | 125 | 126 | 127 | 128 | 129 | File: 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 140 | 141 | 142 | 143 | 144 | 145 | 146 | Qt::Horizontal 147 | 148 | 149 | 150 | 40 151 | 20 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 0 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | ... 168 | 169 | 170 | 171 | .. 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | QAbstractItemView::NoSelection 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | Start 192 | 193 | 194 | 195 | 196 | 197 | 198 | Stop 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | -------------------------------------------------------------------------------- /rosbag_fancy/src/bag_reader.h: -------------------------------------------------------------------------------- 1 | // Fast native bag reader 2 | // Author: Max Schwarz 3 | 4 | #ifndef ROSBAG_FANCY_BAGREADER_H 5 | #define ROSBAG_FANCY_BAGREADER_H 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | #include 17 | 18 | namespace rosbag_fancy 19 | { 20 | 21 | class BagReader 22 | { 23 | public: 24 | class Exception : public std::runtime_error 25 | { 26 | public: 27 | explicit Exception(const std::string& what) 28 | : std::runtime_error{what} 29 | {} 30 | }; 31 | 32 | struct Connection 33 | { 34 | std::uint32_t id; 35 | std::string topicInBag; 36 | 37 | std::string topicAsPublished; 38 | std::string type; 39 | std::string md5sum; 40 | std::string msgDef; 41 | std::string callerID; 42 | bool latching = false; 43 | 44 | std::uint64_t totalCount = 0; 45 | }; 46 | 47 | using ConnectionMap = std::map; 48 | 49 | struct ConnectionInfo 50 | { 51 | std::uint32_t id; 52 | std::uint32_t msgCount; 53 | }; 54 | 55 | class ChunkIterator; 56 | class Iterator; 57 | 58 | struct Message 59 | { 60 | ros::Time stamp; 61 | const Connection* connection; 62 | 63 | template 64 | boost::shared_ptr instantiate() const 65 | { 66 | if(ros::message_traits::DataType::value() != connection->type) 67 | return {}; 68 | if(ros::message_traits::MD5Sum::value() != connection->md5sum) 69 | return {}; 70 | 71 | namespace ser = ros::serialization; 72 | ser::IStream stream(const_cast(m_data), m_size); 73 | 74 | auto ret = boost::make_shared(); 75 | ser::deserialize(stream, *ret); 76 | return ret; 77 | } 78 | 79 | const uint8_t* data() const 80 | { return m_data; } 81 | 82 | std::size_t size() const 83 | { return m_size; } 84 | 85 | private: 86 | friend class BagReader::ChunkIterator; 87 | friend struct ros::serialization::Serializer; 88 | 89 | const uint8_t* m_data = nullptr; 90 | std::size_t m_size; 91 | }; 92 | 93 | class ChunkIterator 94 | { 95 | public: 96 | using iterator_category = std::input_iterator_tag; 97 | using value_type = Message; 98 | using reference = const Message&; 99 | using pointer = const Message*; 100 | 101 | ChunkIterator(const ChunkIterator&) = default; 102 | ChunkIterator& operator=(const ChunkIterator&) = default; 103 | 104 | reference operator*() { return m_msg; } 105 | pointer operator->() { return &m_msg; } 106 | 107 | ChunkIterator& operator++(); 108 | ChunkIterator operator++(int) { ChunkIterator tmp = *this; ++(*this); return tmp; } 109 | 110 | friend bool operator== (const ChunkIterator& a, const ChunkIterator& b) { return a.m_d == b.m_d && a.m_idx == b.m_idx; }; 111 | friend bool operator!= (const ChunkIterator& a, const ChunkIterator& b) { return a.m_d != b.m_d || a.m_idx != b.m_idx; }; 112 | 113 | private: 114 | friend class BagReader::Iterator; 115 | 116 | ChunkIterator() {} 117 | explicit ChunkIterator(const BagReader* reader, int chunk); 118 | 119 | class Private; 120 | std::shared_ptr m_d; 121 | std::size_t m_idx = 0; 122 | Message m_msg; 123 | }; 124 | 125 | class Iterator 126 | { 127 | public: 128 | using iterator_category = std::input_iterator_tag; 129 | using value_type = Message; 130 | using reference = const Message&; 131 | using pointer = const Message*; 132 | 133 | Iterator() {} 134 | Iterator(const Iterator&) = default; 135 | Iterator& operator=(const Iterator&) = default; 136 | 137 | reference operator*() { return *m_it; } 138 | pointer operator->() { return &*m_it; } 139 | 140 | Iterator& operator++(); 141 | Iterator operator++(int) { Iterator tmp = *this; ++(*this); return tmp; } 142 | 143 | void advanceWithPredicates(const std::function& connPredicate, const std::function& messagePredicate); 144 | void findNextWithPredicates(const std::function& connPredicate, const std::function& messagePredicate); 145 | 146 | friend bool operator== (const Iterator& a, const Iterator& b) { return a.m_chunk == b.m_chunk && a.m_it == b.m_it; }; 147 | friend bool operator!= (const Iterator& a, const Iterator& b) { return a.m_chunk != b.m_chunk || a.m_it != b.m_it; }; 148 | 149 | std::vector& currentChunkConnections() const; 150 | rosbag::CompressionType currentChunkCompression() const; 151 | 152 | void skipChunk(); 153 | int chunk() const 154 | { return m_chunk; } 155 | 156 | private: 157 | friend class BagReader; 158 | 159 | explicit Iterator(const BagReader* reader, int chunk); 160 | 161 | const BagReader* m_reader; 162 | int m_chunk = -1; 163 | ChunkIterator m_it; 164 | }; 165 | 166 | explicit BagReader(const std::string& filename); 167 | ~BagReader(); 168 | 169 | BagReader(const BagReader&) = delete; 170 | BagReader& operator=(const BagReader&) = delete; 171 | 172 | BagReader(BagReader&& other); 173 | 174 | const ConnectionMap& connections() const; 175 | 176 | ros::Time startTime() const; 177 | ros::Time endTime() const; 178 | 179 | std::size_t size() const; 180 | 181 | Iterator begin() const; 182 | Iterator end() const; 183 | Iterator findTime(const ros::Time& time) const; 184 | 185 | std::size_t numChunks() const; 186 | 187 | int findChunk(const ros::Time& time) const; 188 | Iterator chunkBegin(int chunk) const; 189 | 190 | bool isMonotonic() const; 191 | private: 192 | class Private; 193 | std::unique_ptr m_d; 194 | }; 195 | 196 | } 197 | 198 | namespace ros 199 | { 200 | namespace message_traits 201 | { 202 | template<> struct IsMessage : TrueType { }; 203 | 204 | template<> 205 | struct MD5Sum 206 | { 207 | static const char* value(const rosbag_fancy::BagReader::Message& m) 208 | { 209 | return m.connection->md5sum.c_str(); 210 | } 211 | }; 212 | 213 | template<> 214 | struct DataType 215 | { 216 | static const char* value(const rosbag_fancy::BagReader::Message& m) 217 | { 218 | return m.connection->type.c_str(); 219 | } 220 | }; 221 | 222 | template<> 223 | struct Definition 224 | { 225 | static const char* value(const rosbag_fancy::BagReader::Message& m) 226 | { 227 | return m.connection->msgDef.c_str(); 228 | } 229 | }; 230 | } 231 | 232 | namespace serialization 233 | { 234 | template<> 235 | struct Serializer 236 | { 237 | template 238 | inline static void write(Stream& stream, const rosbag_fancy::BagReader::Message& t) 239 | { 240 | std::memcpy(stream.advance(t.m_size), t.m_data, t.m_size); 241 | } 242 | 243 | inline static uint32_t serializedLength(const rosbag_fancy::BagReader::Message& t) 244 | { 245 | return t.m_size; 246 | } 247 | }; 248 | } 249 | } 250 | 251 | #endif 252 | -------------------------------------------------------------------------------- /rosbag_fancy/src/tf2_scanner.cpp: -------------------------------------------------------------------------------- 1 | // Scans multiple bag files for tf2 messages and aggregates them 2 | // Author: Max Schwarz 3 | 4 | #include "tf2_scanner.h" 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | #include "bag_view.h" 15 | 16 | #include "doctest.h" 17 | 18 | namespace rosbag_fancy 19 | { 20 | 21 | namespace 22 | { 23 | bool tf2StaticPredicate(const BagReader::Connection& con) 24 | { 25 | return con.topicInBag == "/tf_static" && con.type == "tf2_msgs/TFMessage"; 26 | } 27 | } 28 | 29 | class TF2Scanner::Private 30 | { 31 | public: 32 | explicit Private(const std::vector& readers) 33 | { 34 | for(auto& reader : readers) 35 | m_view.addBag(reader, tf2StaticPredicate); 36 | 37 | m_thread = std::thread{[&](){ process(); }}; 38 | } 39 | 40 | ~Private() 41 | { 42 | m_shouldExit = true; 43 | m_thread.join(); 44 | } 45 | 46 | const tf2_msgs::TFMessage* fetchUpdate(const ros::Time& time) 47 | { 48 | std::unique_lock lock{m_mutex}; 49 | 50 | // Wait until the message list is populated until the point we are interested in 51 | m_cond.wait(lock, [&]() -> bool { 52 | return m_scanningFinished || (!m_msgs.empty() && m_scannedTime >= time); 53 | }); 54 | 55 | // No messages? 56 | if(m_msgs.empty()) 57 | return {}; 58 | 59 | // If we made a skip back in time, reset the cache idx 60 | if(m_msgs[m_cacheIdx].stamp > time) 61 | { 62 | m_cacheIdx = 0; 63 | m_sentIdx = -1; 64 | 65 | // If time is earlier than the first entry, we have no transforms. 66 | if(m_msgs[m_cacheIdx].stamp > time) 67 | return &m_emptyMessage; 68 | } 69 | 70 | // Advance 71 | while(m_cacheIdx < m_msgs.size()-1 && m_msgs[m_cacheIdx+1].stamp < time) 72 | { 73 | m_cacheIdx++; 74 | } 75 | 76 | if(static_cast(m_cacheIdx) != m_sentIdx) 77 | { 78 | m_sentIdx = m_cacheIdx; 79 | return &m_msgs[m_cacheIdx].msg; 80 | } 81 | else 82 | return {}; 83 | } 84 | 85 | private: 86 | struct Msg 87 | { 88 | ros::Time stamp; 89 | tf2_msgs::TFMessage msg; 90 | }; 91 | 92 | void process() 93 | { 94 | std::unordered_map transforms; 95 | 96 | for(auto& pmsg : m_view) 97 | { 98 | if(m_shouldExit) 99 | return; 100 | 101 | auto msg = pmsg.msg->instantiate(); 102 | if(!msg) 103 | continue; 104 | 105 | bool changed = false; 106 | for(auto& transform : msg->transforms) 107 | { 108 | auto [it, inserted] = transforms.try_emplace(transform.child_frame_id, transform); 109 | if(inserted) 110 | changed = true; 111 | else 112 | { 113 | if(it->second != transform) 114 | { 115 | it->second = transform; 116 | changed = true; 117 | } 118 | } 119 | } 120 | 121 | if(changed) 122 | { 123 | std::unique_lock lock{m_mutex}; 124 | 125 | auto& entry = m_msgs.emplace_back(); 126 | entry.stamp = pmsg.msg->stamp; 127 | entry.msg.transforms.reserve(transforms.size()); 128 | for(auto& [_, transform] : transforms) 129 | entry.msg.transforms.push_back(transform); 130 | 131 | m_scannedTime = pmsg.msg->stamp; 132 | 133 | m_cond.notify_all(); 134 | } 135 | } 136 | 137 | { 138 | std::unique_lock lock{m_mutex}; 139 | m_scannedTime = m_view.endTime(); 140 | m_scanningFinished = true; 141 | m_cond.notify_all(); 142 | } 143 | } 144 | 145 | BagView m_view; 146 | 147 | std::atomic_bool m_shouldExit = false; 148 | 149 | std::mutex m_mutex; 150 | std::vector m_msgs; 151 | ros::Time m_scannedTime; 152 | bool m_scanningFinished = false; 153 | std::condition_variable m_cond; 154 | 155 | std::thread m_thread; 156 | 157 | unsigned int m_cacheIdx = 0; 158 | int m_sentIdx = -1; 159 | 160 | tf2_msgs::TFMessage m_emptyMessage{}; 161 | }; 162 | 163 | TF2Scanner::TF2Scanner(const std::vector& readers) 164 | : m_d{std::make_unique(readers)} 165 | { 166 | } 167 | 168 | TF2Scanner::~TF2Scanner() 169 | { 170 | } 171 | 172 | const tf2_msgs::TFMessage* TF2Scanner::fetchUpdate(const ros::Time& time) 173 | { 174 | return m_d->fetchUpdate(time); 175 | } 176 | 177 | 178 | // TESTS 179 | 180 | TEST_CASE("TF2Scanner") 181 | { 182 | // Generate a bag file 183 | // NOTE: This generates a bag file which is slightly different, since 184 | // the latch=true header is not set for tf_static messages. 185 | // For the purpose of testing the above, this is enough, however. 186 | char bagfileName[] = "/tmp/rosbag_fancy_test_XXXXXX"; 187 | { 188 | int fd = mkstemp(bagfileName); 189 | REQUIRE(fd >= 0); 190 | close(fd); 191 | 192 | rosbag::Bag bag{bagfileName, rosbag::BagMode::Write}; 193 | 194 | { 195 | std_msgs::Header msg; 196 | msg.frame_id = "a"; 197 | bag.write("/topicA", ros::Time(1000, 0), msg); 198 | } 199 | { 200 | std_msgs::Header msg; 201 | msg.frame_id = "b"; 202 | bag.write("/topicB", ros::Time(1001, 0), msg); 203 | } 204 | { 205 | tf2_msgs::TFMessage msg; 206 | msg.transforms.resize(2); 207 | msg.transforms[0].header.frame_id = "base_link"; 208 | msg.transforms[0].child_frame_id = "arm_link"; 209 | msg.transforms[0].transform.translation.x = 1.0; 210 | msg.transforms[1].header.frame_id = "base_link"; 211 | msg.transforms[1].child_frame_id = "leg_link"; 212 | msg.transforms[1].transform.translation.x = 4.0; 213 | bag.write("/tf_static", ros::Time(1002, 0), msg); 214 | } 215 | { 216 | tf2_msgs::TFMessage msg; 217 | msg.transforms.resize(1); 218 | msg.transforms[0].header.frame_id = "base_link"; 219 | msg.transforms[0].child_frame_id = "arm_link"; 220 | msg.transforms[0].transform.translation.x = 2.0; 221 | bag.write("/tf_static", ros::Time(1010, 0), msg); 222 | } 223 | 224 | bag.close(); 225 | } 226 | 227 | BagReader reader{bagfileName}; 228 | 229 | TF2Scanner scanner{{&reader}}; 230 | 231 | { 232 | auto msg = scanner.fetchUpdate(ros::Time(0)); 233 | REQUIRE(msg); 234 | CAPTURE(*msg); 235 | CHECK(msg->transforms.size() == 0); 236 | } 237 | { 238 | auto msg = scanner.fetchUpdate(ros::Time(1005)); 239 | REQUIRE(msg); 240 | CAPTURE(*msg); 241 | REQUIRE(msg->transforms.size() == 2); 242 | 243 | auto it = std::find_if(msg->transforms.begin(), msg->transforms.end(), [&](auto& trans){ return trans.child_frame_id == "arm_link"; }); 244 | REQUIRE(it != msg->transforms.end()); 245 | CHECK(it->transform.translation.x == doctest::Approx(1.0)); 246 | } 247 | { 248 | auto msg = scanner.fetchUpdate(ros::Time(1006)); 249 | REQUIRE(!msg); 250 | } 251 | { 252 | auto msg = scanner.fetchUpdate(ros::Time(1012)); 253 | REQUIRE(msg); 254 | CAPTURE(*msg); 255 | REQUIRE(msg->transforms.size() == 2); 256 | 257 | auto it = std::find_if(msg->transforms.begin(), msg->transforms.end(), [&](auto& trans){ return trans.child_frame_id == "arm_link"; }); 258 | REQUIRE(it != msg->transforms.end()); 259 | CHECK(it->transform.translation.x == doctest::Approx(2.0)); 260 | } 261 | 262 | { 263 | auto msg = scanner.fetchUpdate(ros::Time(1015)); 264 | REQUIRE(!msg); 265 | } 266 | 267 | { 268 | auto msg = scanner.fetchUpdate(ros::Time(1005)); 269 | REQUIRE(msg); 270 | CAPTURE(*msg); 271 | REQUIRE(msg->transforms.size() == 2); 272 | 273 | auto it = std::find_if(msg->transforms.begin(), msg->transforms.end(), [&](auto& trans){ return trans.child_frame_id == "arm_link"; }); 274 | REQUIRE(it != msg->transforms.end()); 275 | CHECK(it->transform.translation.x == doctest::Approx(1.0)); 276 | } 277 | 278 | unlink(bagfileName); 279 | } 280 | 281 | } 282 | -------------------------------------------------------------------------------- /rosbag_fancy/src/cmd_record.cpp: -------------------------------------------------------------------------------- 1 | // record command 2 | // Author: Max Schwarz 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | #include 12 | 13 | #include 14 | 15 | #include "topic_manager.h" 16 | #include "topic_subscriber.h" 17 | #include "ui.h" 18 | #include "message_queue.h" 19 | #include "mem_str.h" 20 | #include "bag_writer.h" 21 | 22 | 23 | namespace po = boost::program_options; 24 | using namespace rosbag_fancy; 25 | using namespace rosbag_fancy_msgs; 26 | 27 | int record(const std::vector& options) 28 | { 29 | po::variables_map vm; 30 | 31 | // Handle CLI arguments 32 | { 33 | po::options_description desc("Options"); 34 | desc.add_options() 35 | ("help", "Display this help message") 36 | ("prefix,p", po::value()->default_value("bag"), "Prefix for output bag file. The prefix is extended with a timestamp.") 37 | ("output,o", po::value()->value_name("FILE"), "Output bag file (overrides --prefix)") 38 | ("topic", po::value>()->required(), "Topics to record") 39 | ("queue-size", po::value()->value_name("SIZE")->default_value("500MB"), "Queue size") 40 | ("delete-old-at", po::value()->value_name("SIZE"), "Delete old bags at given size, e.g. 100GB or 1TB") 41 | ("split-bag-size", po::value()->value_name("SIZE"), "Bag size for splitting, e.g. 1GB") 42 | ("paused", "Start paused") 43 | ("no-ui", "Disable terminal UI") 44 | ("udp", "Subscribe using UDP transport") 45 | ("bz2", "Enable BZ2 compression") 46 | ("lz4", "Enable LZ2 compression") 47 | ; 48 | 49 | po::positional_options_description p; 50 | p.add("topic", -1); 51 | 52 | auto usage = [&](){ 53 | std::cout << "Usage: rosbag_fancy record [options] -o \n\n"; 54 | std::cout << desc << "\n\n"; 55 | std::cout << "Topics may be annotated with a rate limit in Hz, e.g.:\n"; 56 | std::cout << " rosbag_fancy /camera/image_raw=10.0\n"; 57 | std::cout << "\n"; 58 | }; 59 | 60 | try 61 | { 62 | po::store( 63 | po::command_line_parser(options).options(desc).positional(p).run(), 64 | vm 65 | ); 66 | 67 | if(vm.count("help")) 68 | { 69 | usage(); 70 | return 0; 71 | } 72 | 73 | po::notify(vm); 74 | } 75 | catch(po::error& e) 76 | { 77 | std::cerr << "Could not parse arguments: " << e.what() << "\n\n"; 78 | usage(); 79 | return 1; 80 | } 81 | } 82 | 83 | ros::NodeHandle nh{"~"}; 84 | 85 | std::vector topics = vm["topic"].as>(); 86 | std::sort(topics.begin(), topics.end()); 87 | 88 | TopicManager topicManager; 89 | for(auto& topicSpec : topics) 90 | { 91 | std::string name = topicSpec; 92 | float rateLimit = 0.0f; 93 | 94 | auto sepIdx = topicSpec.find('='); 95 | 96 | if(sepIdx != std::string::npos) 97 | { 98 | name = topicSpec.substr(0, sepIdx); 99 | 100 | try 101 | { 102 | rateLimit = boost::lexical_cast(topicSpec.substr(sepIdx+1)); 103 | } 104 | catch(boost::bad_lexical_cast&) 105 | { 106 | std::cerr << "Bad topic spec: '" << topicSpec << "'\n"; 107 | return 1; 108 | } 109 | } 110 | 111 | int flags = 0; 112 | if(vm.count("udp")) 113 | flags |= static_cast(Topic::Flag::UDP); 114 | 115 | topicManager.addTopic(name, rateLimit, flags); 116 | } 117 | 118 | std::uint64_t queueSize = mem_str::stringToMemory(vm["queue-size"].as()); 119 | MessageQueue queue{queueSize}; 120 | 121 | // Figure out the output file name 122 | auto namingMode = BagWriter::Naming::Verbatim; 123 | std::string bagName = ""; 124 | if(vm.count("output")) 125 | { 126 | bagName = vm["output"].as(); 127 | namingMode = BagWriter::Naming::Verbatim; 128 | } 129 | else 130 | { 131 | bagName = vm["prefix"].as(); 132 | namingMode = BagWriter::Naming::AppendTimestamp; 133 | } 134 | 135 | std::uint64_t splitBagSizeInBytes = 0; 136 | if(vm.count("split-bag-size")) 137 | { 138 | splitBagSizeInBytes = mem_str::stringToMemory(vm["split-bag-size"].as()); 139 | } 140 | 141 | std::uint64_t deleteOldAtInBytes = 0; 142 | if(vm.count("delete-old-at")) 143 | { 144 | deleteOldAtInBytes = mem_str::stringToMemory(vm["delete-old-at"].as()); 145 | if(splitBagSizeInBytes != 0 && deleteOldAtInBytes < splitBagSizeInBytes) 146 | { 147 | ROSFMT_WARN("Chosen split-bag-size is larger than delete-old-at size!"); 148 | } 149 | } 150 | 151 | if(!ros::Time::isValid()) 152 | { 153 | ROSFMT_INFO("Waiting for ros::Time to become valid..."); 154 | ros::Time::waitForValid(); 155 | } 156 | 157 | BagWriter writer{queue, bagName, namingMode, splitBagSizeInBytes, deleteOldAtInBytes}; 158 | 159 | if(vm.count("bz2") && vm.count("lz4")) 160 | { 161 | fmt::print(stderr, "Options --bz2 and --lz4 are mutually exclusive\n"); 162 | return 1; 163 | } 164 | 165 | if(vm.count("bz2")) 166 | writer.setCompression(rosbag::compression::BZ2); 167 | if(vm.count("lz4")) 168 | writer.setCompression(rosbag::compression::LZ4); 169 | 170 | auto start = [&](){ 171 | try 172 | { 173 | writer.start(); 174 | } 175 | catch(rosbag::BagException& e) 176 | { 177 | ROSFMT_ERROR("Could not open output bag file: {}", e.what()); 178 | return false; 179 | } 180 | return true; 181 | }; 182 | 183 | // Start/Stop service calls 184 | ros::ServiceServer srv_start = nh.advertiseService("start", boost::function([&](auto&, auto& resp){ 185 | resp.success = start(); 186 | return true; 187 | })); 188 | ros::ServiceServer srv_stop = nh.advertiseService("stop", boost::function([&](auto&, auto& resp){ 189 | writer.stop(); 190 | resp.success = true; 191 | return true; 192 | })); 193 | 194 | // Status publisher 195 | ros::Publisher pub_status = nh.advertise("status", 1); 196 | ros::SteadyTimer timer_status = nh.createSteadyTimer(ros::WallDuration(0.1), boost::function([&](auto&){ 197 | ros::WallTime now = ros::WallTime::now(); 198 | 199 | StatusPtr msg = boost::make_shared(); 200 | msg->header.stamp = ros::Time::now(); 201 | 202 | msg->status = writer.running() ? Status::STATUS_RUNNING : Status::STATUS_PAUSED; 203 | 204 | msg->bagfile = writer.bagfileName(); 205 | 206 | msg->bytes = writer.sizeInBytes(); 207 | msg->free_bytes = writer.freeSpace(); 208 | msg->bandwidth = 0; 209 | 210 | auto& counts = writer.messageCounts(); 211 | 212 | for(auto& topic : topicManager.topics()) 213 | { 214 | msg->topics.emplace_back(); 215 | auto& topicMsg = msg->topics.back(); 216 | 217 | msg->bandwidth += topic.bandwidth; 218 | 219 | topicMsg.name = topic.name; 220 | topicMsg.publishers = topic.numPublishers; 221 | topicMsg.bandwidth = topic.bandwidth; 222 | topicMsg.bytes = topic.totalBytes; 223 | topicMsg.messages = topic.totalMessages; 224 | 225 | if(topic.id < counts.size()) 226 | topicMsg.messages_in_current_bag = counts[topic.id]; 227 | 228 | topicMsg.rate = topic.messageRateAt(now); 229 | } 230 | 231 | pub_status.publish(msg); 232 | })); 233 | 234 | // Start recording if --paused is not given 235 | if(vm.count("paused") == 0) 236 | { 237 | if(!start()) 238 | return 1; 239 | } 240 | 241 | TopicSubscriber subscriber{topicManager, queue}; 242 | 243 | std::unique_ptr ui; 244 | 245 | if(!vm.count("no-ui")) 246 | ui.reset(new UI{topicManager, queue, writer}); 247 | 248 | ros::spin(); 249 | 250 | return 0; 251 | } 252 | -------------------------------------------------------------------------------- /rosbag_fancy/src/bag_view.cpp: -------------------------------------------------------------------------------- 1 | // Filtering view on (multiple) bag files 2 | // Author: Max Schwarz 3 | 4 | #include "bag_view.h" 5 | 6 | #include 7 | #include 8 | #include 9 | #include "doctest.h" 10 | 11 | namespace rosbag_fancy 12 | { 13 | 14 | class BagView::Private 15 | { 16 | public: 17 | void addBag(BagReader* reader, const std::function& connectionPredicate) 18 | { 19 | auto& handle = m_bags.emplace_back(); 20 | handle.reader = reader; 21 | handle.filtering = true; 22 | 23 | for(auto& conn : reader->connections()) 24 | { 25 | if(handle.connectionIDs.size() <= conn.first) 26 | handle.connectionIDs.resize(conn.first+1, false); 27 | 28 | if(connectionPredicate(conn.second)) 29 | handle.connectionIDs[conn.first] = true; 30 | } 31 | } 32 | 33 | void addBag(BagReader* reader) 34 | { 35 | auto& handle = m_bags.emplace_back(); 36 | handle.reader = reader; 37 | handle.filtering = false; 38 | } 39 | 40 | ros::Time startTime() const 41 | { 42 | if(m_bags.empty()) 43 | return {}; 44 | 45 | ros::Time start = m_bags.front().reader->startTime(); 46 | for(auto& bag : m_bags) 47 | start = std::min(start, bag.reader->startTime()); 48 | 49 | return start; 50 | } 51 | 52 | ros::Time endTime() const 53 | { 54 | ros::Time end; 55 | for(auto& bag : m_bags) 56 | end = std::max(end, bag.reader->endTime()); 57 | 58 | return end; 59 | } 60 | 61 | private: 62 | friend class BagView::Iterator::Private; 63 | 64 | struct BagHandle 65 | { 66 | BagReader* reader; 67 | bool filtering; 68 | std::vector connectionIDs; 69 | }; 70 | std::vector m_bags; 71 | }; 72 | 73 | class BagView::Iterator::Private 74 | { 75 | public: 76 | Private(const BagView::Private* view) 77 | { 78 | for(auto& handle : view->m_bags) 79 | { 80 | auto& state = m_state.emplace_back(); 81 | state.handle = &handle; 82 | state.it = handle.reader->begin(); 83 | 84 | if(handle.filtering) 85 | state.skipToNext(false); 86 | } 87 | } 88 | 89 | Private(const BagView::Private* view, const ros::Time& time) 90 | { 91 | for(auto& handle : view->m_bags) 92 | { 93 | auto& state = m_state.emplace_back(); 94 | state.handle = &handle; 95 | state.it = handle.reader->findTime(time); 96 | 97 | if(handle.filtering) 98 | state.skipToNext(false); 99 | } 100 | } 101 | 102 | struct BagState 103 | { 104 | const BagView::Private::BagHandle* handle; 105 | int chunk = -1; 106 | BagReader::Iterator it; 107 | 108 | void skipToNext(bool advance) 109 | { 110 | // We need to skip to the next valid message in this bag. 111 | auto messageIsInteresting = [&](const BagReader::Message& msg){ 112 | return handle->connectionIDs[msg.connection->id]; 113 | }; 114 | auto connectionIsInteresting = [&](const BagReader::ConnectionInfo& conn){ 115 | return handle->connectionIDs[conn.id]; 116 | }; 117 | 118 | if(advance) 119 | it.advanceWithPredicates(connectionIsInteresting, messageIsInteresting); 120 | else 121 | it.findNextWithPredicates(connectionIsInteresting, messageIsInteresting); 122 | } 123 | }; 124 | 125 | std::vector m_state; 126 | BagState* m_nextBag{}; 127 | MultiBagMessage m_msg; 128 | }; 129 | 130 | BagView::Iterator::Iterator(const BagView* view) 131 | : m_d{std::make_shared(view->m_d.get())} 132 | { 133 | ++(*this); 134 | } 135 | 136 | BagView::Iterator::Iterator(const BagView* view, const ros::Time& time) 137 | : m_d{std::make_shared(view->m_d.get(), time)} 138 | { 139 | ++(*this); 140 | } 141 | 142 | BagView::Iterator::~Iterator() 143 | {} 144 | 145 | const BagView::MultiBagMessage& BagView::Iterator::operator*() 146 | { 147 | if(!m_d) 148 | throw std::logic_error{"Attempt to dereference invalid BagView::Iterator"}; 149 | 150 | return m_d->m_msg; 151 | } 152 | 153 | BagView::Iterator& BagView::Iterator::operator++() 154 | { 155 | if(!m_d) 156 | return *this; 157 | 158 | if(m_d->m_nextBag) 159 | { 160 | auto* bag = m_d->m_nextBag; 161 | 162 | // We need to skip to the next valid message in this bag. 163 | if(bag->handle->filtering) 164 | bag->skipToNext(true); 165 | else 166 | ++bag->it; 167 | } 168 | 169 | // Figure out the earliest available message from all the bags 170 | ros::Time earliestStamp; 171 | m_d->m_nextBag = nullptr; 172 | std::size_t bagIndex = 0; 173 | 174 | for(std::size_t i = 0; i < m_d->m_state.size(); ++i) 175 | { 176 | auto& state = m_d->m_state[i]; 177 | 178 | if(state.it == state.handle->reader->end()) 179 | continue; 180 | 181 | if(!m_d->m_nextBag || state.it->stamp < earliestStamp) 182 | { 183 | m_d->m_nextBag = &state; 184 | earliestStamp = state.it->stamp; 185 | bagIndex = i; 186 | } 187 | } 188 | 189 | if(!m_d->m_nextBag) 190 | { 191 | // End reached, invalidate 192 | m_d.reset(); 193 | return *this; 194 | } 195 | 196 | // Found a message! 197 | m_d->m_msg.msg = &*m_d->m_nextBag->it; 198 | m_d->m_msg.bagIndex = bagIndex; 199 | 200 | return *this; 201 | } 202 | 203 | bool operator==(const BagView::Iterator& a, const BagView::Iterator& b) 204 | { 205 | // NOTE: This way view.begin() != view.begin(), but I don't care. 206 | return a.m_d == b.m_d; 207 | } 208 | 209 | bool operator!=(const BagView::Iterator& a, const BagView::Iterator& b) 210 | { 211 | // NOTE: This way view.begin() != view.begin(), but I don't care. 212 | return a.m_d != b.m_d; 213 | } 214 | 215 | BagView::BagView() 216 | : m_d{std::make_unique()} 217 | {} 218 | 219 | BagView::~BagView() 220 | {} 221 | 222 | void BagView::addBag(BagReader* reader, const std::function& connectionPredicate) 223 | { 224 | m_d->addBag(reader, connectionPredicate); 225 | } 226 | 227 | void BagView::addBag(BagReader* reader) 228 | { 229 | m_d->addBag(reader); 230 | } 231 | 232 | ros::Time BagView::startTime() const 233 | { 234 | return m_d->startTime(); 235 | } 236 | 237 | ros::Time BagView::endTime() const 238 | { 239 | return m_d->endTime(); 240 | } 241 | 242 | BagView::Iterator BagView::begin() const 243 | { 244 | return BagView::Iterator{this}; 245 | } 246 | 247 | BagView::Iterator BagView::end() const 248 | { 249 | return {}; 250 | } 251 | 252 | BagView::Iterator BagView::findTime(const ros::Time& time) const 253 | { 254 | return BagView::Iterator{this, time}; 255 | } 256 | 257 | 258 | TEST_CASE("BagView: One file") 259 | { 260 | // Generate a bag file 261 | char bagfileName[] = "/tmp/rosbag_fancy_test_XXXXXX"; 262 | { 263 | int fd = mkstemp(bagfileName); 264 | REQUIRE(fd >= 0); 265 | close(fd); 266 | 267 | rosbag::Bag bag{bagfileName, rosbag::BagMode::Write}; 268 | 269 | { 270 | std_msgs::Header msg; 271 | msg.frame_id = "a"; 272 | bag.write("/topicA", ros::Time(1000, 0), msg); 273 | } 274 | { 275 | std_msgs::Header msg; 276 | msg.frame_id = "b"; 277 | bag.write("/topicB", ros::Time(1001, 0), msg); 278 | } 279 | { 280 | std_msgs::UInt8 msg; 281 | msg.data = 123; 282 | bag.write("/topicC", ros::Time(1002, 0), msg); 283 | } 284 | 285 | bag.close(); 286 | } 287 | 288 | // Open bagfile 289 | BagReader reader{bagfileName}; 290 | 291 | SUBCASE("No selection") 292 | { 293 | BagView view; 294 | view.addBag(&reader); 295 | 296 | auto it = view.begin(); 297 | 298 | REQUIRE(it != view.end()); 299 | CHECK(it->msg->connection->topicInBag == "/topicA"); 300 | 301 | ++it; REQUIRE(it != view.end()); 302 | CHECK(it->msg->connection->topicInBag == "/topicB"); 303 | 304 | ++it; REQUIRE(it != view.end()); 305 | CHECK(it->msg->connection->topicInBag == "/topicC"); 306 | 307 | ++it; CHECK(it == view.end()); 308 | } 309 | 310 | SUBCASE("select by topic") 311 | { 312 | BagView view; 313 | view.addBag(&reader, [&](const BagReader::Connection& con){ 314 | return con.topicInBag == "/topicB"; 315 | }); 316 | 317 | int num = 0; 318 | for(auto& pmsg : view) 319 | { 320 | CHECK(pmsg.msg->connection->topicInBag == "/topicB"); 321 | 322 | auto msg = pmsg.msg->instantiate(); 323 | REQUIRE(msg); 324 | CHECK(msg->frame_id == "b"); 325 | num++; 326 | } 327 | 328 | CHECK(num == 1); 329 | } 330 | 331 | SUBCASE("select by type") 332 | { 333 | BagView view; 334 | view.addBag(&reader, [&](const BagReader::Connection& con){ 335 | return con.type == "std_msgs/UInt8"; 336 | }); 337 | 338 | int num = 0; 339 | for(auto& pmsg : view) 340 | { 341 | CHECK(pmsg.msg->connection->topicInBag == "/topicC"); 342 | 343 | auto msg = pmsg.msg->instantiate(); 344 | REQUIRE(msg); 345 | CHECK(msg->data == 123); 346 | num++; 347 | } 348 | 349 | CHECK(num == 1); 350 | } 351 | 352 | // Remove temp file 353 | unlink(bagfileName); 354 | } 355 | 356 | } 357 | -------------------------------------------------------------------------------- /rosbag_fancy/src/cmd_play.cpp: -------------------------------------------------------------------------------- 1 | // Play command 2 | // Author: Max Schwarz 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | #include "bag_reader.h" 20 | #include "bag_view.h" 21 | #include "topic_manager.h" 22 | #include "ui.h" 23 | #include "tf2_scanner.h" 24 | 25 | namespace po = boost::program_options; 26 | using namespace rosbag_fancy; 27 | using namespace rosbag_fancy_msgs; 28 | 29 | int play(const std::vector& options) 30 | { 31 | po::variables_map vm; 32 | 33 | // Handle CLI arguments 34 | { 35 | po::options_description desc("Options"); 36 | desc.add_options() 37 | ("help", "Display this help message") 38 | ("clock", "Publish clock (requires use_sim_time)") 39 | ("no-ui", "Disable terminal UI") 40 | ; 41 | 42 | po::options_description hidden("Hidden"); 43 | hidden.add_options() 44 | ("input", po::value>()->required(), "Input file") 45 | ; 46 | 47 | po::options_description all("All"); 48 | all.add(desc).add(hidden); 49 | 50 | po::positional_options_description p; 51 | p.add("input", -1); 52 | 53 | auto usage = [&](){ 54 | std::cout << "Usage: rosbag_fancy play [options] \n\n"; 55 | std::cout << desc << "\n\n"; 56 | }; 57 | 58 | try 59 | { 60 | po::store( 61 | po::command_line_parser(options).options(all).positional(p).run(), 62 | vm 63 | ); 64 | 65 | if(vm.count("help")) 66 | { 67 | usage(); 68 | return 0; 69 | } 70 | 71 | po::notify(vm); 72 | } 73 | catch(po::error& e) 74 | { 75 | std::cerr << "Could not parse arguments: " << e.what() << "\n\n"; 76 | usage(); 77 | return 1; 78 | } 79 | } 80 | 81 | bool publishClock = vm.count("clock"); 82 | 83 | ros::Publisher pub_clock; 84 | ros::SteadyTime lastClockPublishTime; 85 | 86 | if(publishClock) 87 | pub_clock = ros::NodeHandle{}.advertise("/clock", 1, true); 88 | 89 | struct Bag 90 | { 91 | explicit Bag(const std::string& filename) 92 | : reader{filename}, filename_ ( filename ) 93 | {} 94 | 95 | BagReader reader; 96 | std::string filename_; 97 | std::unordered_map publishers; 98 | }; 99 | 100 | std::vector bags; 101 | 102 | TopicManager topicManager; 103 | std::map topicMap; 104 | 105 | ros::NodeHandle nh; 106 | 107 | ros::Time startTime; 108 | ros::Time endTime; 109 | 110 | for(auto& filename : vm["input"].as>()) 111 | { 112 | auto& bag = bags.emplace_back(filename); 113 | 114 | if(!bag.reader.isMonotonic()) 115 | { 116 | throw std::runtime_error{fmt::format( 117 | "Bag '{}' contains non-monotonic chunks. This is not supported by rosbag_fancy play at the moment. " 118 | "You can convert a non-monotonic bag to a monotonic one by running 'rosbag filter in.bag out.bag True'.", 119 | filename 120 | )}; 121 | } 122 | 123 | if(startTime == ros::Time(0) || bag.reader.startTime() < startTime) 124 | startTime = bag.reader.startTime(); 125 | 126 | if(endTime == ros::Time(0) || bag.reader.endTime() > endTime) 127 | endTime = bag.reader.endTime(); 128 | 129 | for(auto& [id, con] : bag.reader.connections()) 130 | { 131 | ros::AdvertiseOptions opts; 132 | opts.datatype = con.type; 133 | opts.md5sum = con.md5sum; 134 | opts.message_definition = con.msgDef; 135 | opts.topic = con.topicInBag; 136 | opts.latch = con.latching; 137 | opts.queue_size = 100; 138 | opts.has_header = false; // FIXME: Is this correct? 139 | 140 | bag.publishers[id] = nh.advertise(opts); 141 | 142 | auto it = topicMap.find(con.topicInBag); 143 | if(it == topicMap.end()) 144 | { 145 | topicMap[con.topicInBag] = topicManager.topics().size(); 146 | topicManager.addTopic(con.topicInBag); 147 | } 148 | } 149 | } 150 | 151 | std::vector bagReaders; 152 | for(auto& bag : bags) 153 | bagReaders.push_back(&bag.reader); 154 | 155 | TF2Scanner tf2Scanner{bagReaders}; 156 | 157 | ros::Publisher pub_tf_static = nh.advertise("/tf_static", 20, true); 158 | 159 | BagView view; 160 | auto topicPredicate = [&](const BagReader::Connection& connection){ 161 | return connection.topicInBag != "/tf_static"; 162 | }; 163 | 164 | for(auto& bag : bags) 165 | view.addBag(&bag.reader, topicPredicate); 166 | 167 | ros::WallDuration(0.2).sleep(); 168 | 169 | ros::Time bagRefTime = startTime; 170 | ros::SteadyTime wallRefTime = ros::SteadyTime::now(); 171 | ros::Time currentTime = bagRefTime; 172 | 173 | std::unique_ptr ui; 174 | 175 | bool paused = false; 176 | 177 | BagView::Iterator it = view.begin(); 178 | 179 | if(!vm.count("no-ui")) 180 | { 181 | ui.reset(new PlaybackUI{topicManager, startTime, endTime}); 182 | 183 | ui->seekForwardRequested.connect([&](){ 184 | currentTime += ros::Duration{5.0}; 185 | bagRefTime += ros::Duration{5.0}; 186 | 187 | it = view.findTime(currentTime); 188 | }); 189 | ui->seekBackwardRequested.connect([&](){ 190 | currentTime -= ros::Duration{5.0}; 191 | bagRefTime -= ros::Duration{5.0}; 192 | 193 | if(currentTime < startTime) 194 | { 195 | currentTime = startTime; 196 | bagRefTime = currentTime; 197 | wallRefTime = ros::SteadyTime::now(); 198 | } 199 | 200 | it = view.findTime(currentTime); 201 | }); 202 | ui->pauseRequested.connect([&](){ 203 | paused = !paused; 204 | bagRefTime = currentTime; 205 | wallRefTime = ros::SteadyTime::now(); 206 | }); 207 | ui->exitRequested.connect([&](){ 208 | ros::shutdown(); 209 | }); 210 | } 211 | 212 | // Start/Stop service calls 213 | ros::ServiceServer srv_start = nh.advertiseService("start", boost::function([&](auto&, auto& resp) { 214 | paused = false; 215 | resp.success = !paused; 216 | if(ui) 217 | ui->setPaused(paused); 218 | return true; 219 | })); 220 | ros::ServiceServer srv_pause = nh.advertiseService("pause", boost::function([&](auto&, auto& resp) { 221 | paused = true; 222 | resp.success = paused; 223 | if(ui) 224 | ui->setPaused(paused); 225 | return true; 226 | })); 227 | ros::ServiceServer srv_toggle = nh.advertiseService("toggle", boost::function([&](auto&, auto& resp) { 228 | paused = !paused; 229 | if(ui) 230 | ui->setPaused(paused); 231 | resp.success = true; 232 | return true; 233 | })); 234 | 235 | // Status publisher 236 | ros::Publisher pub_status = nh.advertise("status", 1); 237 | ros::SteadyTimer timer_status = nh.createSteadyTimer(ros::WallDuration(0.1), boost::function([&](auto&) { 238 | auto msg = boost::make_shared(); 239 | msg->header.stamp = ros::Time::now(); 240 | 241 | msg->status = paused ? PlayStatus::STATUS_PAUSED : PlayStatus::STATUS_RUNNING; 242 | 243 | msg->currentTime = currentTime; 244 | msg->startTime = startTime; 245 | msg->endTime = endTime; 246 | msg->current = currentTime-startTime; 247 | msg->duration = endTime-startTime; 248 | 249 | msg->bagfiles.reserve(bags.size()); 250 | for(const auto& bag : bags) 251 | msg->bagfiles.emplace_back(bag.filename_); 252 | 253 | msg->topics.reserve(topicManager.topics().size()); 254 | for(const auto& topic : topicManager.topics()) 255 | { 256 | msg->topics.emplace_back(); 257 | auto& topicMsg = msg->topics.back(); 258 | 259 | topicMsg.name = topic.name; 260 | topicMsg.bandwidth = topic.bandwidth; 261 | topicMsg.rate = topic.messageRate; 262 | } 263 | 264 | pub_status.publish(msg); 265 | })); 266 | 267 | while(ros::ok()) 268 | { 269 | if(paused) 270 | ros::WallDuration{0.1}.sleep(); 271 | else 272 | { 273 | if(it == view.end()) 274 | break; 275 | 276 | auto& msg = *it->msg; 277 | 278 | if(msg.stamp < bagRefTime) 279 | continue; 280 | 281 | ros::SteadyTime wallStamp = wallRefTime + ros::WallDuration().fromNSec((msg.stamp - bagRefTime).toNSec()); 282 | ros::SteadyTime::sleepUntil(wallStamp); 283 | 284 | currentTime = msg.stamp; 285 | if(ui) 286 | ui->setPositionInBag(msg.stamp); 287 | 288 | bags[it->bagIndex].publishers[msg.connection->id].publish(msg); 289 | 290 | auto& topic = topicManager.topics()[topicMap[msg.connection->topicInBag]]; 291 | topic.notifyMessage(msg.size()); 292 | 293 | if(publishClock && wallStamp - lastClockPublishTime > ros::WallDuration{0.001}) 294 | { 295 | rosgraph_msgs::Clock msg; 296 | msg.clock = currentTime; 297 | pub_clock.publish(msg); 298 | lastClockPublishTime = wallStamp; 299 | } 300 | 301 | // Publish matching tf_static 302 | if(auto tfmsg = tf2Scanner.fetchUpdate(currentTime)) 303 | pub_tf_static.publish(*tfmsg); 304 | 305 | ++it; 306 | } 307 | 308 | ros::spinOnce(); 309 | 310 | if(ui) 311 | { 312 | // Handle key input 313 | fd_set fds{}; 314 | FD_ZERO(&fds); 315 | FD_SET(STDIN_FILENO, &fds); 316 | timeval timeout{}; 317 | int ret = select(STDIN_FILENO+1, &fds, nullptr, nullptr, &timeout); 318 | if(ret < 0) 319 | { 320 | if(errno != EAGAIN && errno != EINTR) 321 | throw std::runtime_error{fmt::format("select() error: {}", strerror(errno))}; 322 | } 323 | else if(ret != 0) 324 | ui->handleInput(); 325 | 326 | ui->setPaused(paused); 327 | } 328 | } 329 | 330 | return 0; 331 | } 332 | -------------------------------------------------------------------------------- /rosbag_fancy/src/bag_writer.cpp: -------------------------------------------------------------------------------- 1 | // Output thread 2 | // Author: Max Schwarz 3 | 4 | #include "bag_writer.h" 5 | #include "message_queue.h" 6 | #include "topic_manager.h" 7 | 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | #include 17 | 18 | namespace fs = boost::filesystem; 19 | 20 | namespace rosbag_fancy 21 | { 22 | 23 | namespace 24 | { 25 | std::string timeToString(const ros::Time& cur_ros_t) 26 | { 27 | std::time_t cur_t = cur_ros_t.sec; 28 | std::stringstream ss; 29 | ss << std::put_time(std::localtime(&cur_t), "%Y-%m-%d-%H-%M-%S"); 30 | return ss.str(); 31 | } 32 | 33 | std::string getNewFilename(const std::string& old) 34 | { 35 | for(int i = 2; i < 10; ++i) 36 | { 37 | auto tst_filename = fmt::format("{}.{}", old, i); 38 | if(!fs::exists(tst_filename)) 39 | return tst_filename; 40 | } 41 | 42 | return {}; 43 | } 44 | 45 | std::vector getBagFilesInCurrentFolder(const std::string& filename) 46 | { 47 | std::vector bagFiles; 48 | auto path = fs::absolute(fs::path(filename)).parent_path(); 49 | 50 | for(auto& entry : boost::make_iterator_range(fs::directory_iterator(path), {})) 51 | { 52 | auto filePath = entry.path(); 53 | if(fs::extension(filePath) == ".bag") 54 | { 55 | bagFiles.emplace_back(filePath); 56 | } 57 | } 58 | 59 | return bagFiles; 60 | } 61 | 62 | std::vector sortFilesByTime(const std::vector& files) 63 | { 64 | std::multimap timeFiles; 65 | for(auto & entry : files) 66 | { 67 | timeFiles.emplace(fs::last_write_time(entry),entry); 68 | } 69 | 70 | std::vector timeSortedFiles; 71 | if(!timeFiles.empty()) 72 | std::transform(timeFiles.begin(), timeFiles.end(), std::back_inserter(timeSortedFiles), [](auto &kv){ return kv.second;}); 73 | 74 | return timeSortedFiles; 75 | } 76 | 77 | std::uint64_t getTotalSizeInBytes(const std::vector& files) 78 | { 79 | std::uint64_t totalSizeInBytes = 0; 80 | for(auto& entry : files) 81 | totalSizeInBytes += fs::file_size(entry); 82 | 83 | return totalSizeInBytes; 84 | } 85 | } 86 | 87 | BagWriter::BagWriter(rosbag_fancy::MessageQueue& queue, const std::string& filename, Naming namingMode, std::uint64_t splitSizeInBytes, std::uint64_t deleteOldAtInBytes) 88 | : m_queue{queue} 89 | , m_filename{filename} 90 | , m_namingMode{namingMode} 91 | , m_splitSizeInBytes{splitSizeInBytes} 92 | , m_deleteOldAtInBytes{deleteOldAtInBytes} 93 | { 94 | ros::NodeHandle nh; 95 | m_freeSpaceTimer = nh.createSteadyTimer(ros::WallDuration(5.0), 96 | boost::bind(&BagWriter::checkFreeSpace, this) 97 | ); 98 | checkFreeSpace(); 99 | 100 | m_tf_header = boost::make_shared>(); 101 | { 102 | auto& connectionHeader = *m_tf_header; 103 | connectionHeader["topic"] = "/tf_static"; 104 | connectionHeader["callerid"] = ros::this_node::getName(); 105 | connectionHeader["md5sum"] = ros::message_traits::MD5Sum::value(); 106 | connectionHeader["type"] = "tf2_msgs/TFMessage"; 107 | connectionHeader["message_definition"] = ros::message_traits::Definition::value(); 108 | connectionHeader["latching"] = "1"; 109 | } 110 | 111 | m_thread = std::thread{std::bind(&BagWriter::run, this)}; 112 | 113 | if(m_deleteOldAtInBytes != 0) 114 | m_cleanup_thread = std::thread{std::bind(&BagWriter::cleanupThread, this)}; 115 | } 116 | 117 | BagWriter::~BagWriter() 118 | { 119 | m_shouldShutdown = true; 120 | m_queue.shutdown(); 121 | m_thread.join(); 122 | 123 | if(m_deleteOldAtInBytes != 0) 124 | { 125 | m_cleanupCondition.notify_one(); 126 | m_cleanup_thread.join(); 127 | } 128 | } 129 | 130 | void BagWriter::run() 131 | { 132 | while(!m_shouldShutdown) 133 | { 134 | auto msg = m_queue.pop(); 135 | 136 | { 137 | std::unique_lock lock(m_mutex); 138 | if(m_running && msg) 139 | { 140 | m_bag.write(msg->topic, msg->message); 141 | m_sizeInBytes = m_bag.getSize(); 142 | 143 | if(msg->topicData->id >= m_messageCounts.size()) 144 | { 145 | m_messageCounts.resize(msg->topicData->id+1, 0); 146 | m_byteCounts.resize(msg->topicData->id+1, 0); 147 | } 148 | 149 | m_messageCounts[msg->topicData->id]++; 150 | m_byteCounts[msg->topicData->id] += msg->size(); 151 | } 152 | } 153 | 154 | if(msg && msg->topic == "/tf_static") 155 | { 156 | auto shifter = msg->message.getMessage(); 157 | auto tf_msg = shifter->instantiate(); 158 | if(tf_msg) 159 | { 160 | for(auto& transformMsg : tf_msg->transforms) 161 | m_tf_buf.setTransform(transformMsg, "bag", true); 162 | } 163 | } 164 | 165 | if(m_splitSizeInBytes != 0 && m_sizeInBytes >= m_splitSizeInBytes) 166 | { 167 | m_isReopeningBag = true; 168 | stop(); 169 | start(); 170 | m_isReopeningBag = false; 171 | } 172 | } 173 | } 174 | 175 | void BagWriter::start() 176 | { 177 | std::unique_lock lock(m_mutex); 178 | 179 | if(m_running) 180 | return; 181 | 182 | m_messageCounts.clear(); 183 | 184 | std::string filename; 185 | switch(m_namingMode) 186 | { 187 | case Naming::AppendTimestamp: 188 | filename = fmt::format("{}_{}.bag", m_filename, timeToString(ros::Time::now())); 189 | break; 190 | case Naming::Verbatim: 191 | filename = m_filename; 192 | break; 193 | } 194 | 195 | if(!m_bagOpen) 196 | { 197 | // Don't overwrite existing files 198 | if(fs::exists(filename)) 199 | { 200 | auto replacement = getNewFilename(filename); 201 | if(replacement.empty()) 202 | { 203 | ROSFMT_ERROR("Could not find a bag file name to write to (template '{}')", filename); 204 | ROSFMT_ERROR("Ignoring start request."); 205 | m_bagOpen = false; 206 | m_running = false; 207 | return; 208 | } 209 | 210 | filename = replacement; 211 | } 212 | 213 | // We need to hold the cleanup mutex here to make sure the cleanup thread 214 | // is aware of the current bag file name. 215 | std::unique_lock lock(m_cleanupMutex); 216 | 217 | ROSFMT_INFO("Opening bag file: {}", filename.c_str()); 218 | m_bag.open(filename, rosbag::bagmode::Write); 219 | m_bag.setCompression(m_compressionType); 220 | 221 | m_expandedFilename = filename; 222 | 223 | // Write all known transforms to /tf_static 224 | { 225 | auto tf_msg = boost::make_shared(); 226 | std::vector frames; 227 | m_tf_buf._getFrameStrings(frames); 228 | 229 | for(auto& frame : frames) 230 | { 231 | std::string parent; 232 | if(!m_tf_buf._getParent(frame, ros::Time(0), parent)) 233 | continue; 234 | 235 | geometry_msgs::TransformStamped transform = m_tf_buf.lookupTransform(parent, frame, ros::Time(0)); 236 | tf_msg->transforms.push_back(std::move(transform)); 237 | } 238 | 239 | ros::MessageEvent event{tf_msg, m_tf_header, ros::Time::now()}; 240 | 241 | m_bag.write("/tf_static", event); 242 | } 243 | } 244 | 245 | m_bagOpen = true; 246 | m_running = true; 247 | } 248 | 249 | void BagWriter::stop() 250 | { 251 | std::unique_lock lock(m_mutex); 252 | 253 | switch(m_namingMode) 254 | { 255 | case Naming::AppendTimestamp: 256 | m_bag.close(); 257 | m_bagOpen = false; 258 | break; 259 | case Naming::Verbatim: 260 | break; 261 | } 262 | 263 | m_running = false; 264 | 265 | ROSFMT_INFO("Recording stopped."); 266 | } 267 | 268 | void BagWriter::checkFreeSpace() 269 | { 270 | namespace fs = boost::filesystem; 271 | 272 | auto path = fs::absolute(fs::path(m_filename)).parent_path(); 273 | auto space = fs::space(path); 274 | 275 | m_freeSpace = space.available; 276 | } 277 | 278 | void BagWriter::cleanupThread() 279 | { 280 | namespace fs = boost::filesystem; 281 | 282 | using namespace std::literals; 283 | 284 | while(!m_shouldShutdown) 285 | { 286 | std::vector bagFiles = getBagFilesInCurrentFolder(m_filename); 287 | m_directorySizeInBytes = getTotalSizeInBytes(bagFiles); 288 | 289 | if(m_deleteOldAtInBytes != 0 && m_directorySizeInBytes > m_deleteOldAtInBytes) 290 | { 291 | std::unique_lock lock(m_cleanupMutex); 292 | 293 | // explicit new computation, since there might have been some changes in the mean time. 294 | // Here we are protected by the mutex, so filename change can happen in the meantime. 295 | std::vector bagFiles = getBagFilesInCurrentFolder(m_filename); 296 | std::uint64_t directorySizeInBytes = getTotalSizeInBytes(bagFiles); 297 | std::vector timeSortedBagFiles = sortFilesByTime(bagFiles); 298 | 299 | fs::path currentPath = fs::path(bagfileName()); 300 | 301 | for(auto& entry : timeSortedBagFiles) 302 | { 303 | // do not delete current one! 304 | if(fs::equivalent(entry, currentPath)) 305 | continue; 306 | 307 | std::uint64_t bagSize = fs::file_size(entry); 308 | 309 | ROSFMT_INFO("Bag directory requires too much space. Removing old bag file: {}", entry.c_str()); 310 | fs::remove(entry); 311 | 312 | directorySizeInBytes -= bagSize; 313 | 314 | // Already finished? 315 | if(directorySizeInBytes < m_deleteOldAtInBytes) 316 | break; 317 | } 318 | 319 | if(directorySizeInBytes > m_deleteOldAtInBytes) 320 | { 321 | ROSFMT_WARN("I could not remove enough bag files to make the bag file directory small enough."); 322 | } 323 | 324 | m_directorySizeInBytes = directorySizeInBytes; 325 | } 326 | 327 | // Sleep for defined time, but exit when requested 328 | std::unique_lock lock(m_cleanupMutex); 329 | m_cleanupCondition.wait_for(lock, 5s, [&]{ return m_shouldShutdown; }); 330 | 331 | if(m_shouldShutdown) 332 | break; 333 | } 334 | } 335 | 336 | void BagWriter::setCompression(rosbag::compression::CompressionType type) 337 | { 338 | std::unique_lock lock{m_mutex}; 339 | m_compressionType = type; 340 | m_bag.setCompression(type); 341 | } 342 | 343 | } 344 | -------------------------------------------------------------------------------- /rosbag_fancy/src/terminal.cpp: -------------------------------------------------------------------------------- 1 | // Encapsulates terminal control (colors, cursor, ...) 2 | // Author: Max Schwarz 3 | 4 | #include "terminal.h" 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | 23 | namespace rosbag_fancy 24 | { 25 | 26 | Terminal::Parser::Parser() 27 | : m_state(STATE_ESCAPE) 28 | , m_fgColor(-1) 29 | , m_bgColor(-1) 30 | , m_bold(false) 31 | { 32 | } 33 | 34 | void Terminal::Parser::parseSetAttributes(const std::string& str) 35 | { 36 | using Tokenizer = boost::tokenizer>; 37 | 38 | boost::char_separator sep(";"); 39 | Tokenizer tok(str, sep); 40 | 41 | for(Tokenizer::iterator it = tok.begin(); it != tok.end(); ++it) 42 | { 43 | errno = 0; 44 | auto endptr = const_cast(it->c_str()); 45 | int code = strtoul(it->c_str(), &endptr, 10); 46 | 47 | if(errno != 0 || *endptr != 0) 48 | { 49 | // Error in specification, break out of here 50 | m_fgColor = -1; 51 | m_bgColor = -1; 52 | return; 53 | } 54 | 55 | if(code == 0) 56 | { 57 | m_fgColor = -1; 58 | m_bgColor = -1; 59 | } 60 | else if(code >= 30 && code <= 37) 61 | m_fgColor = code - 30; 62 | else if(code >= 40 && code <= 47) 63 | m_bgColor = code - 40; 64 | else if(code == 1) 65 | m_bold = true; 66 | } 67 | } 68 | 69 | void Terminal::Parser::parse(char c) 70 | { 71 | switch(m_state) 72 | { 73 | case STATE_ESCAPE: 74 | if(c == '\033') 75 | m_state = STATE_TYPE; 76 | break; 77 | case STATE_TYPE: 78 | if(c == '[') 79 | { 80 | m_state = STATE_CSI; 81 | m_buf.clear(); 82 | } 83 | else 84 | m_state = STATE_ESCAPE; 85 | break; 86 | case STATE_CSI: 87 | if(c == 'm') 88 | { 89 | parseSetAttributes(m_buf); 90 | m_state = STATE_ESCAPE; 91 | } 92 | else 93 | { 94 | m_buf.push_back(c); 95 | if(m_buf.length() >= 16) 96 | m_state = STATE_ESCAPE; 97 | } 98 | break; 99 | } 100 | } 101 | 102 | void Terminal::Parser::parse(const std::string& str) 103 | { 104 | for(char c : str) 105 | parse(c); 106 | } 107 | 108 | void Terminal::Parser::apply(Terminal* term) 109 | { 110 | if(m_fgColor >= 0 && m_bgColor >= 0) 111 | term->setSimplePair((SimpleColor)m_fgColor, (SimpleColor)m_bgColor); 112 | else 113 | { 114 | term->setStandardColors(); 115 | if(m_fgColor >= 0) 116 | term->setSimpleForeground((SimpleColor)m_fgColor); 117 | else if(m_bgColor >= 0) 118 | term->setSimpleBackground((SimpleColor)m_fgColor); 119 | } 120 | } 121 | 122 | Terminal::Terminal() 123 | : m_valid(false) 124 | , m_256colors(false) 125 | , m_truecolor(false) 126 | { 127 | // Override using environment variable 128 | char* overrideMode = getenv("ROSBAG_FANCY_COLOR_MODE"); 129 | const char* termOverride = nullptr; 130 | if(overrideMode) 131 | { 132 | if(strcasecmp(overrideMode, "truecolor") == 0) 133 | { 134 | termOverride = "xterm-256color"; 135 | m_256colors = true; 136 | m_truecolor = true; 137 | } 138 | else if(strcasecmp(overrideMode, "256colors") == 0) 139 | { 140 | termOverride = "xterm-256color"; 141 | m_256colors = true; 142 | m_truecolor = false; 143 | } 144 | else if(strcasecmp(overrideMode, "ansi") == 0) 145 | { 146 | m_256colors = false; 147 | m_truecolor = false; 148 | } 149 | else 150 | { 151 | fmt::print(stderr, "Warning: Unknown ROSBAG_FANCY_COLOR_MODE value: '{}'\n", overrideMode); 152 | } 153 | } 154 | else 155 | { 156 | // Detect truecolor-capable terminals 157 | if(getenv("KONSOLE_DBUS_SESSION")) 158 | { 159 | // Sadly, there is no way to determine the Konsole version. Since 160 | // any reasonably recent version supports true colors, just assume 161 | // true color support 162 | m_truecolor = true; 163 | m_256colors = true; 164 | } 165 | 166 | char* vte_version = getenv("VTE_VERSION"); 167 | if(vte_version && boost::lexical_cast(vte_version) >= 3600) 168 | { 169 | m_256colors = true; 170 | m_truecolor = true; 171 | } 172 | } 173 | 174 | int ret; 175 | if(setupterm(termOverride, STDOUT_FILENO, &ret) != OK) 176 | { 177 | fmt::print("Could not setup the terminal. Disabling all colors...\n"); 178 | return; 179 | } 180 | 181 | m_valid = true; 182 | 183 | if(!m_256colors && !overrideMode) 184 | { 185 | // Detect 256 color terminals 186 | int num_colors = tigetnum("colors"); 187 | m_256colors = num_colors >= 256; 188 | } 189 | 190 | { 191 | const char* bgColor = tigetstr("setab"); 192 | if(bgColor && bgColor != (char*)-1) 193 | m_bgColorStr = bgColor; 194 | else 195 | fmt::print("Your terminal does not support ANSI background!\n"); 196 | } 197 | { 198 | const char* fgColor = tigetstr("setaf"); 199 | if(fgColor && fgColor != (char*)-1) 200 | m_fgColorStr = fgColor; 201 | else 202 | fmt::print("Your terminal does not support ANSI foreground!\n"); 203 | } 204 | 205 | m_opStr = tigetstr("op"); 206 | m_sgr0Str = tigetstr("sgr0"); 207 | m_elStr = tigetstr("el"); 208 | m_upStr = tigetstr("cuu"); 209 | 210 | m_boldStr = tigetstr("bold"); 211 | 212 | auto registerKey = [&](const char* name, SpecialKey key, const std::string& fallback = ""){ 213 | char* code = tigetstr(name); 214 | 215 | // Who comes up with these return codes? 216 | if(code && code != reinterpret_cast(-1)) 217 | m_specialKeys[code] = key; 218 | else if(!fallback.empty()) 219 | m_specialKeys[fallback] = key; 220 | }; 221 | 222 | // Map function keys 223 | for(int i = 0; i < 12; ++i) 224 | { 225 | registerKey( 226 | fmt::format("kf{}", i+1).c_str(), 227 | static_cast(SK_F1 + i) 228 | ); 229 | } 230 | 231 | // Backspace 232 | registerKey(key_backspace, SK_Backspace); 233 | 234 | // Arrow keys 235 | registerKey(key_up, SK_Up, "\033[A"); 236 | registerKey(key_down, SK_Down, "\033[B"); 237 | registerKey(key_right, SK_Right, "\033[C"); 238 | registerKey(key_left, SK_Left, "\033[D"); 239 | } 240 | 241 | bool Terminal::has256Colors() const 242 | { 243 | return m_256colors; 244 | } 245 | 246 | void Terminal::setCursorInvisible() 247 | { 248 | if(!m_valid) 249 | return; 250 | 251 | putp(tigetstr("civis")); 252 | } 253 | 254 | void Terminal::setCursorVisible() 255 | { 256 | if(!m_valid) 257 | return; 258 | 259 | putp(tigetstr("cnorm")); 260 | } 261 | 262 | static int ansiColor(uint32_t rgb) 263 | { 264 | int r = (rgb & 0xFF); 265 | int g = (rgb >> 8) & 0xFF; 266 | int b = (rgb >> 16) & 0xFF; 267 | 268 | r = r * 6 / 256; 269 | g = g * 6 / 256; 270 | b = b * 6 / 256; 271 | 272 | return 16 + 36 * r + 6 * g + b; 273 | } 274 | 275 | void Terminal::setBackgroundColor(uint32_t color) 276 | { 277 | if(!m_valid) 278 | return; 279 | 280 | if(m_truecolor) 281 | { 282 | char buf[256]; 283 | snprintf(buf, sizeof(buf), "\033[48;2;%d;%d;%dm", color & 0xFF, (color >> 8) & 0xFF, (color >> 16) & 0xFF); 284 | fputs(buf, stdout); 285 | } 286 | else 287 | { 288 | char* out = tiparm(m_bgColorStr.c_str(), ansiColor(color)); 289 | putp(out); 290 | } 291 | } 292 | 293 | void Terminal::setForegroundColor(uint32_t color) 294 | { 295 | if(!m_valid) 296 | return; 297 | 298 | if(m_truecolor) 299 | { 300 | char buf[256]; 301 | snprintf(buf, sizeof(buf), "\033[38;2;%d;%d;%dm", color & 0xFF, (color >> 8) & 0xFF, (color >> 16) & 0xFF); 302 | fputs(buf, stdout); 303 | } 304 | else 305 | { 306 | char* out = tiparm(m_fgColorStr.c_str(), ansiColor(color)); 307 | putp(out); 308 | } 309 | } 310 | 311 | void Terminal::setEcho(bool on) 312 | { 313 | // Switch character echo on 314 | termios ios; 315 | if(tcgetattr(STDIN_FILENO, &ios) == 0) 316 | { 317 | if(on) 318 | { 319 | ios.c_lflag |= ECHO; 320 | ios.c_lflag |= ICANON; 321 | } 322 | else 323 | { 324 | ios.c_lflag &= ~ECHO; 325 | ios.c_lflag &= ~ICANON; 326 | } 327 | 328 | tcsetattr(STDIN_FILENO, TCSANOW, &ios); 329 | } 330 | } 331 | 332 | void Terminal::setBold(bool on) 333 | { 334 | if(!m_valid) 335 | return; 336 | 337 | if(on) 338 | putp(m_boldStr.c_str()); 339 | } 340 | 341 | void Terminal::setSimpleForeground(SimpleColor color) 342 | { 343 | if(!m_valid) 344 | return; 345 | 346 | char* out = tiparm(m_fgColorStr.c_str(), color); 347 | putp(out); 348 | } 349 | 350 | void Terminal::setSimpleBackground(SimpleColor color) 351 | { 352 | if(!m_valid) 353 | return; 354 | 355 | char* out = tiparm(m_bgColorStr.c_str(), color); 356 | putp(out); 357 | } 358 | 359 | void Terminal::setSimplePair(SimpleColor fg, SimpleColor bg) 360 | { 361 | if(!m_valid) 362 | return; 363 | 364 | setSimpleForeground(fg); 365 | setSimpleBackground(bg); 366 | } 367 | 368 | void Terminal::setStandardColors() 369 | { 370 | if(!m_valid) 371 | return; 372 | 373 | putp(m_opStr.c_str()); 374 | putp(m_sgr0Str.c_str()); 375 | } 376 | 377 | void Terminal::clearToEndOfLine() 378 | { 379 | if(!m_valid) 380 | return; 381 | 382 | putp(m_elStr.c_str()); 383 | } 384 | 385 | void Terminal::moveCursorUp(int numLines) 386 | { 387 | if(!m_valid) 388 | return; 389 | 390 | putp(tparm(m_upStr.c_str(), numLines)); 391 | } 392 | 393 | void Terminal::moveCursorToStartOfLine() 394 | { 395 | putchar('\r'); 396 | } 397 | 398 | bool Terminal::getSize(int* outColumns, int* outRows) 399 | { 400 | struct winsize w; 401 | if(ioctl(STDIN_FILENO, TIOCGWINSZ, &w) == 0) 402 | { 403 | *outColumns = w.ws_col; 404 | *outRows = w.ws_row; 405 | return true; 406 | } 407 | else 408 | return false; 409 | } 410 | 411 | void Terminal::setWindowTitle(const std::string& title) 412 | { 413 | char buf[256]; 414 | 415 | // Konsole style 416 | snprintf(buf, sizeof(buf), "\033]30;%s\007", title.c_str()); 417 | fputs(buf, stdout); 418 | 419 | // screen/tmux style 420 | snprintf(buf, sizeof(buf), "\033k%s\033\\", title.c_str()); 421 | fputs(buf, stdout); 422 | } 423 | 424 | void Terminal::clearWindowTitle(const std::string& backup) 425 | { 426 | fputs("\033]30;%d : %n\007", stdout); 427 | 428 | // screen/tmux style 429 | fmt::print("\033k{}\033\\", backup); 430 | } 431 | 432 | int Terminal::readKey() 433 | { 434 | // Are we currently aborting an escape string that we did not recognize? 435 | if(m_currentEscapeAborted) 436 | { 437 | char c = m_currentEscapeStr[m_currentEscapeAbortIdx++]; 438 | if(m_currentEscapeAbortIdx >= m_currentEscapeStr.size()) 439 | { 440 | m_currentEscapeAborted = false; 441 | m_currentEscapeStr.clear(); 442 | } 443 | 444 | return c; 445 | } 446 | 447 | char c; 448 | if(read(STDIN_FILENO, &c, 1) != 1) 449 | return -1; 450 | 451 | if(m_currentEscapeStr.empty() && c == '\E') 452 | { 453 | m_currentEscapeStr.push_back(c); 454 | m_escapeStartTime = std::chrono::steady_clock::now(); 455 | return -1; 456 | } 457 | else if(!m_currentEscapeStr.empty()) 458 | { 459 | m_currentEscapeStr.push_back(c); 460 | m_escapeStartTime = std::chrono::steady_clock::now(); 461 | 462 | std::size_t matches = 0; 463 | int lastMatch = -1; 464 | bool completeMatch = false; 465 | 466 | for(auto& pair : m_specialKeys) 467 | { 468 | if(m_currentEscapeStr.length() > pair.first.length()) 469 | continue; 470 | 471 | if(pair.first.substr(0, m_currentEscapeStr.length()) == m_currentEscapeStr) 472 | { 473 | matches++; 474 | lastMatch = pair.second; 475 | 476 | if(m_currentEscapeStr.length() == pair.first.length()) 477 | { 478 | completeMatch = true; 479 | break; 480 | } 481 | } 482 | } 483 | 484 | if(matches == 0) 485 | { 486 | // We don't understand this code, just switch back to normal mode 487 | m_currentEscapeStr.clear(); 488 | } 489 | else if(completeMatch) 490 | { 491 | m_currentEscapeStr.clear(); 492 | return lastMatch; 493 | } 494 | else 495 | return -1; 496 | } 497 | 498 | if(c == 0x7f) // ASCII delete 499 | return SK_Backspace; 500 | 501 | return c; 502 | } 503 | 504 | } 505 | -------------------------------------------------------------------------------- /rosbag_fancy/src/ui.cpp: -------------------------------------------------------------------------------- 1 | // Terminal UI 2 | // Author: Max Schwarz 3 | 4 | #include "ui.h" 5 | #include "bag_writer.h" 6 | #include "bag_reader.h" 7 | 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | namespace rosbag_fancy 14 | { 15 | 16 | namespace 17 | { 18 | unsigned int g_statusLines = 2; 19 | std::string g_windowTitle; 20 | 21 | void cleanup() 22 | { 23 | for(unsigned int i = 0; i < g_statusLines+1; ++i) 24 | printf("\n"); 25 | 26 | Terminal term; 27 | 28 | // Switch cursor back on 29 | term.setCursorVisible(); 30 | 31 | // Switch character echo on 32 | term.setEcho(true); 33 | } 34 | 35 | std::string memoryToString(uint64_t memory) 36 | { 37 | if(memory < static_cast(1<<10)) 38 | return fmt::format("{}.0 B", memory); 39 | else if(memory < static_cast(1<<20)) 40 | return fmt::format("{:.1f} KiB", static_cast(memory) / static_cast(1<<10)); 41 | else if(memory < static_cast(1<<30)) 42 | return fmt::format("{:.1f} MiB", static_cast(memory) / static_cast(1<<20)); 43 | else if(memory < static_cast(1ull<<40)) 44 | return fmt::format("{:.1f} GiB", static_cast(memory) / static_cast(1ull<<30)); 45 | else 46 | return fmt::format("{:.1f} TiB", static_cast(memory) / static_cast(1ull<<40)); 47 | } 48 | 49 | std::string rateToString(double rate) 50 | { 51 | if(rate < 1000.0) 52 | return fmt::format("{:5.1f} Hz", rate); 53 | else if(rate < 1e6) 54 | return fmt::format("{:5.1f} kHz", rate / 1e3); 55 | else 56 | return fmt::format("{:5.1f} MHz", rate / 1e6); 57 | } 58 | 59 | template 60 | class TableWriter 61 | { 62 | public: 63 | struct Column 64 | { 65 | Column(const std::string& label, unsigned int width = 0, const std::string& format = {}) 66 | : label(label), width(width), format(format) 67 | { 68 | if(width == 0) 69 | this->width = label.size(); 70 | 71 | valueFormatString = fmt::format("{{:{}{}}}", this->width, format); 72 | } 73 | 74 | std::string label; 75 | unsigned int width; 76 | std::string format; 77 | 78 | std::string valueFormatString; 79 | }; 80 | 81 | TableWriter(Terminal& term, const std::array& columns) 82 | : m_term(term) 83 | , m_cols(columns) 84 | {} 85 | 86 | void printHeader() 87 | { 88 | { 89 | bool first = true; 90 | for(auto& col : m_cols) 91 | { 92 | if(first) 93 | first = false; 94 | else 95 | fmt::print(" │ "); 96 | 97 | std::string format = fmt::format("{{:^{}}}", col.width); 98 | fmt::print(format, col.label); 99 | } 100 | fmt::print("\n"); 101 | } 102 | 103 | printDash(); 104 | } 105 | 106 | void printDash() 107 | { 108 | bool first = true; 109 | for(auto& col : m_cols) 110 | { 111 | if(first) 112 | first = false; 113 | else 114 | fmt::print("─┼─"); 115 | 116 | for(unsigned int i = 0; i < col.width; ++i) 117 | fmt::print("─"); 118 | } 119 | fmt::print("\n"); 120 | } 121 | 122 | void startRow() 123 | { 124 | m_currentCol = 0; 125 | } 126 | 127 | void endRow() 128 | { 129 | fmt::print("\n"); 130 | } 131 | 132 | template 133 | void printColumn(const T& value, uint32_t color = 0) 134 | { 135 | if(m_currentCol != 0) 136 | fmt::print(" │ "); 137 | 138 | if(color != 0) 139 | m_term.setForegroundColor(color); 140 | 141 | fmt::print(m_cols[m_currentCol].valueFormatString, value); 142 | 143 | if(color != 0) 144 | m_term.setStandardColors(); 145 | 146 | m_currentCol++; 147 | } 148 | 149 | private: 150 | Terminal& m_term; 151 | std::array m_cols; 152 | 153 | unsigned int m_currentCol = 0; 154 | }; 155 | } 156 | 157 | UI::UI(TopicManager& config, MessageQueue& queue, BagWriter& writer) 158 | : m_topicManager{config} 159 | , m_queue{queue} 160 | , m_bagWriter{writer} 161 | { 162 | std::atexit(&cleanup); 163 | 164 | // Switch cursor off 165 | m_term.setCursorInvisible(); 166 | 167 | // Switch character echo off 168 | m_term.setEcho(false); 169 | 170 | g_statusLines = m_topicManager.topics().size(); 171 | 172 | ros::NodeHandle nh; 173 | m_timer = nh.createSteadyTimer(ros::WallDuration(0.1), boost::bind(&UI::draw, this)); 174 | } 175 | 176 | template 177 | void UI::printLine(unsigned int& lineCounter, const Args& ... args) 178 | { 179 | lineCounter++; 180 | m_term.clearToEndOfLine(); 181 | fmt::print(args...); 182 | putchar('\n'); 183 | } 184 | 185 | void UI::draw() 186 | { 187 | unsigned int cnt = 0; 188 | 189 | ros::WallTime now = ros::WallTime::now(); 190 | 191 | printLine(cnt, ""); 192 | printLine(cnt, ""); 193 | 194 | uint64_t totalMessages = 0; 195 | uint64_t totalBytes = 0; 196 | float totalRate = 0.0f; 197 | float totalBandwidth = 0.0f; 198 | bool totalActivity = false; 199 | unsigned int totalDrops = 0; 200 | 201 | unsigned int maxTopicWidth = 0; 202 | for(auto& topic : m_topicManager.topics()) 203 | maxTopicWidth = std::max(maxTopicWidth, topic.name.length()); 204 | 205 | TableWriter<6> writer{m_term, {{ 206 | {"Act"}, 207 | {"Topic", maxTopicWidth}, 208 | {"Pub"}, 209 | {"Messages", 22}, 210 | {"Bytes", 25}, 211 | {"Drops"} 212 | }}}; 213 | 214 | writer.printHeader(); 215 | cnt += 2; 216 | 217 | auto& counts = m_bagWriter.messageCounts(); 218 | auto& bytes = m_bagWriter.byteCounts(); 219 | 220 | for(auto& topic : m_topicManager.topics()) 221 | { 222 | float messageRate = topic.messageRateAt(now); 223 | bool activity = topic.lastMessageTime > m_lastDrawTime; 224 | 225 | writer.startRow(); 226 | 227 | if(activity) 228 | { 229 | totalActivity = true; 230 | writer.printColumn(" ▮ ", 0x00FF00); 231 | } 232 | else 233 | writer.printColumn(""); 234 | 235 | writer.printColumn(topic.name); 236 | writer.printColumn(topic.numPublishers, topic.numPublishers == 0 ? 0x0000FF : 0); 237 | 238 | uint32_t messageColor = (topic.totalMessages == 0) ? 0x0000FF : 0; 239 | 240 | uint64_t messageCount = topic.id < counts.size() ? counts[topic.id] : 0; 241 | uint64_t byteCount = topic.id < bytes.size() ? bytes[topic.id] : 0; 242 | 243 | writer.printColumn(fmt::format("{:10} ({:>8})", messageCount, rateToString(messageRate)), messageColor); 244 | writer.printColumn(fmt::format("{:>10} ({:>10}/s)", memoryToString(byteCount), memoryToString(topic.bandwidth))); 245 | 246 | writer.printColumn(topic.dropCounter, topic.dropCounter > 0 ? 0x0000FF : 0); 247 | 248 | writer.endRow(); 249 | cnt++; 250 | 251 | totalMessages += messageCount; 252 | totalBytes += byteCount; 253 | totalRate += messageRate; 254 | totalBandwidth += topic.bandwidth; 255 | totalDrops += topic.dropCounter; 256 | 257 | m_term.setStandardColors(); 258 | } 259 | writer.printDash(); 260 | cnt++; 261 | 262 | writer.startRow(); 263 | if(totalActivity) 264 | writer.printColumn(" ▮ ", 0x00FF00); 265 | else 266 | writer.printColumn(""); 267 | writer.printColumn("All"); 268 | writer.printColumn(""); 269 | writer.printColumn(fmt::format("{:10} ({:>8})", totalMessages, rateToString(totalRate))); 270 | writer.printColumn(fmt::format("{:>10} ({:>10}/s)", memoryToString(totalBytes), memoryToString(totalBandwidth))); 271 | writer.printColumn(totalDrops, totalDrops > 0 ? 0x0000FF : 0); 272 | 273 | writer.endRow(); 274 | cnt++; 275 | 276 | printLine(cnt, ""); 277 | if(m_bagWriter.running()) 278 | { 279 | m_term.setForegroundColor(0x00FF00); 280 | printLine(cnt, "Recording..."); 281 | m_term.setStandardColors(); 282 | } 283 | else 284 | { 285 | m_term.setForegroundColor(0x0000FF); 286 | printLine(cnt, "Paused."); 287 | m_term.setStandardColors(); 288 | } 289 | printLine(cnt, "Message queue: {:10} messages, {:>10}", m_queue.messagesInQueue(), memoryToString(m_queue.bytesInQueue())); 290 | 291 | printLine(cnt, "Compression: {:>10}", [&](){ 292 | switch(m_bagWriter.compression()) 293 | { 294 | case rosbag::compression::Uncompressed: return "None"; 295 | case rosbag::compression::BZ2: return "BZ2"; 296 | case rosbag::compression::LZ4: return "LZ4"; 297 | } 298 | 299 | return "Unknown"; 300 | }()); 301 | 302 | if(m_bagWriter.splitSizeInBytes() != 0) 303 | { 304 | printLine(cnt, "Bag size: {:>10} / {:>10} split size / {:>10} available", 305 | memoryToString(m_bagWriter.sizeInBytes()), 306 | memoryToString(m_bagWriter.splitSizeInBytes()), 307 | memoryToString(m_bagWriter.freeSpace()) 308 | ); 309 | } 310 | else 311 | { 312 | printLine(cnt, "Bag size: {:>10} / {:>10} available", 313 | memoryToString(m_bagWriter.sizeInBytes()), 314 | memoryToString(m_bagWriter.freeSpace()) 315 | ); 316 | } 317 | 318 | if(m_bagWriter.deleteOldAtInBytes() != 0) 319 | { 320 | printLine(cnt, "Directory size: {:>10} / {:>10}", 321 | memoryToString(m_bagWriter.directorySizeInBytes()), 322 | memoryToString(m_bagWriter.deleteOldAtInBytes()) 323 | ); 324 | } 325 | else 326 | { 327 | printLine(cnt, "Directory size: {:>10}", 328 | memoryToString(m_bagWriter.directorySizeInBytes()) 329 | ); 330 | } 331 | 332 | g_statusLines = cnt; 333 | 334 | // Move back 335 | m_term.clearToEndOfLine(); 336 | m_term.moveCursorUp(g_statusLines); 337 | m_term.moveCursorToStartOfLine(); 338 | fflush(stdout); 339 | 340 | m_lastDrawTime = now; 341 | } 342 | 343 | 344 | 345 | // Playback UI 346 | PlaybackUI::PlaybackUI(TopicManager& topics, const ros::Time& startTime, const ros::Time& endTime) 347 | : m_topicManager{topics} 348 | , m_startTime{startTime} 349 | , m_endTime{endTime} 350 | { 351 | std::atexit(&cleanup); 352 | 353 | // Switch cursor off 354 | m_term.setCursorInvisible(); 355 | 356 | // Switch character echo off 357 | m_term.setEcho(false); 358 | 359 | g_statusLines = m_topicManager.topics().size(); 360 | 361 | ros::NodeHandle nh; 362 | m_timer = nh.createSteadyTimer(ros::WallDuration(0.1), boost::bind(&PlaybackUI::draw, this)); 363 | } 364 | 365 | template 366 | void PlaybackUI::printLine(unsigned int& lineCounter, const Args& ... args) 367 | { 368 | lineCounter++; 369 | m_term.clearToEndOfLine(); 370 | fmt::print(args...); 371 | putchar('\n'); 372 | } 373 | 374 | void PlaybackUI::draw() 375 | { 376 | unsigned int cnt = 0; 377 | 378 | ros::WallTime now = ros::WallTime::now(); 379 | 380 | printLine(cnt, ""); 381 | printLine(cnt, ""); 382 | 383 | float totalRate = 0.0f; 384 | float totalBandwidth = 0.0f; 385 | bool totalActivity = false; 386 | unsigned int totalDrops = 0; 387 | 388 | unsigned int maxTopicWidth = 0; 389 | for(auto& topic : m_topicManager.topics()) 390 | maxTopicWidth = std::max(maxTopicWidth, topic.name.length()); 391 | 392 | TableWriter<4> writer{m_term, {{ 393 | {"Act"}, 394 | {"Topic", maxTopicWidth}, 395 | {"Messages", 22}, 396 | {"Bytes", 25} 397 | }}}; 398 | 399 | writer.printHeader(); 400 | cnt += 2; 401 | 402 | for(auto& topic : m_topicManager.topics()) 403 | { 404 | float messageRate = topic.messageRateAt(now); 405 | bool activity = topic.lastMessageTime > m_lastDrawTime; 406 | 407 | writer.startRow(); 408 | 409 | if(activity) 410 | { 411 | totalActivity = true; 412 | writer.printColumn(" ▮ ", 0x00FF00); 413 | } 414 | else 415 | writer.printColumn(""); 416 | 417 | writer.printColumn(topic.name); 418 | 419 | uint32_t messageColor = (topic.totalMessages == 0) ? 0x0000FF : 0; 420 | 421 | writer.printColumn(fmt::format("{:>8}", rateToString(messageRate)), messageColor); 422 | writer.printColumn(fmt::format("{:>10}/s", memoryToString(topic.bandwidth))); 423 | 424 | writer.endRow(); 425 | cnt++; 426 | 427 | totalRate += messageRate; 428 | totalBandwidth += topic.bandwidth; 429 | totalDrops += topic.dropCounter; 430 | 431 | m_term.setStandardColors(); 432 | } 433 | writer.printDash(); 434 | cnt++; 435 | 436 | writer.startRow(); 437 | if(totalActivity) 438 | writer.printColumn(" ▮ ", 0x00FF00); 439 | else 440 | writer.printColumn(""); 441 | writer.printColumn("All"); 442 | writer.printColumn(fmt::format("{:>8}", rateToString(totalRate))); 443 | writer.printColumn(fmt::format("{:>10}/s", memoryToString(totalBandwidth))); 444 | 445 | writer.endRow(); 446 | cnt++; 447 | 448 | printLine(cnt, ""); 449 | 450 | // Size info 451 | { 452 | // A lot of std::chrono magic to get local/UTC time 453 | std::chrono::time_point startTimeC(std::chrono::nanoseconds(m_startTime.toNSec())); 454 | std::chrono::time_point endTimeC(std::chrono::nanoseconds(m_endTime.toNSec())); 455 | std::chrono::time_point currentTimeC(std::chrono::nanoseconds(m_positionInBag.toNSec())); 456 | 457 | std::chrono::seconds startTimeS = std::chrono::duration_cast(startTimeC.time_since_epoch()); 458 | std::time_t startTimeSC(startTimeS.count()); 459 | std::tm startTimeB; 460 | std::tm startTimeBUTC; 461 | localtime_r(&startTimeSC, &startTimeB); 462 | gmtime_r(&startTimeSC, &startTimeBUTC); 463 | 464 | std::chrono::seconds endTimeS = std::chrono::duration_cast(endTimeC.time_since_epoch()); 465 | std::time_t endTimeSC(endTimeS.count()); 466 | std::tm endTimeB; 467 | std::tm endTimeBUTC; 468 | localtime_r(&endTimeSC, &endTimeB); 469 | gmtime_r(&endTimeSC, &endTimeBUTC); 470 | 471 | std::chrono::seconds currentTimeS = std::chrono::duration_cast(currentTimeC.time_since_epoch()); 472 | std::time_t currentTimeSC(currentTimeS.count()); 473 | std::tm currentTimeB; 474 | std::tm currentTimeBUTC; 475 | localtime_r(¤tTimeSC, ¤tTimeB); 476 | gmtime_r(¤tTimeSC, ¤tTimeBUTC); 477 | 478 | std::chrono::duration duration{(m_endTime - m_startTime).toNSec()}; 479 | std::chrono::duration positionInBag{(m_positionInBag - m_startTime).toNSec()}; 480 | 481 | printLine(cnt, "Start time: {:%Y-%m-%d %H:%M:%S} ({}) / {:%Y-%m-%d %H:%M:%S} (UTC)", startTimeB, daylight ? tzname[1] : tzname[0], startTimeBUTC); 482 | printLine(cnt, "End time: {:%Y-%m-%d %H:%M:%S} ({}) / {:%Y-%m-%d %H:%M:%S} (UTC)", endTimeB, daylight ? tzname[1] : tzname[0], endTimeBUTC); 483 | printLine(cnt, "Current time: {:%Y-%m-%d %H:%M:%S} ({}) / {:%Y-%m-%d %H:%M:%S} (UTC)", currentTimeB, daylight ? tzname[1] : tzname[0], currentTimeBUTC); 484 | printLine(cnt, "Duration: {:.2%H:%M:%S} ({:7.2f}s)", duration, (m_endTime - m_startTime).toSec()); 485 | printLine(cnt, "Position: {:.2%H:%M:%S} ({:7.2f}s)", positionInBag, (m_positionInBag - m_startTime).toSec()); 486 | printLine(cnt, ""); 487 | } 488 | 489 | // Progress bar 490 | { 491 | int w = 80, h; 492 | m_term.getSize(&w, &h); 493 | 494 | w -= 3; 495 | fmt::print("│"); 496 | 497 | int steps = w * 8; 498 | int pos = (m_positionInBag - m_startTime).toSec() / (m_endTime - m_startTime).toSec() * steps; 499 | 500 | for(int i = 0; i < pos / 8; ++i) 501 | fmt::print("█"); 502 | switch(pos % 8) 503 | { 504 | case 0: fmt::print(" "); break; 505 | case 1: fmt::print("▏"); break; 506 | case 2: fmt::print("▎"); break; 507 | case 3: fmt::print("▍"); break; 508 | case 4: fmt::print("▌"); break; 509 | case 5: fmt::print("▋"); break; 510 | case 6: fmt::print("▊"); break; 511 | case 7: fmt::print("▉"); break; 512 | } 513 | 514 | for(int i = 0; i < w - pos/8 - 1; ++i) 515 | putchar(' '); 516 | fmt::print("│\n"); 517 | 518 | cnt++; 519 | } 520 | 521 | printLine(cnt, ""); 522 | 523 | { 524 | ros::SteadyTime now = ros::SteadyTime::now(); 525 | 526 | m_term.clearToEndOfLine(); 527 | 528 | fmt::print("Hit "); 529 | 530 | if(m_paused) 531 | m_term.setSimplePair(Terminal::Black, Terminal::White); 532 | fmt::print("[space]"); 533 | m_term.setStandardColors(); 534 | 535 | fmt::print(" for pause, "); 536 | 537 | if(now - m_lastSeekBwd < ros::WallDuration(0.5)) 538 | m_term.setSimplePair(Terminal::Black, Terminal::White); 539 | fmt::print("[left]"); 540 | m_term.setStandardColors(); 541 | 542 | fmt::print("/"); 543 | 544 | if(now - m_lastSeekFwd < ros::WallDuration(0.5)) 545 | m_term.setSimplePair(Terminal::Black, Terminal::White); 546 | fmt::print("[right]"); 547 | m_term.setStandardColors(); 548 | 549 | fmt::print(" for seeking\n"); 550 | 551 | cnt++; 552 | } 553 | 554 | g_statusLines = cnt; 555 | 556 | // Move back 557 | m_term.clearToEndOfLine(); 558 | m_term.moveCursorUp(g_statusLines); 559 | m_term.moveCursorToStartOfLine(); 560 | fflush(stdout); 561 | 562 | m_lastDrawTime = now; 563 | } 564 | 565 | void PlaybackUI::setPositionInBag(const ros::Time& stamp) 566 | { 567 | m_positionInBag = stamp; 568 | } 569 | 570 | void PlaybackUI::setPaused(bool paused) 571 | { 572 | m_paused = paused; 573 | } 574 | 575 | void PlaybackUI::handleInput() 576 | { 577 | int c = m_term.readKey(); 578 | if(c < 0) 579 | return; 580 | 581 | switch(c) 582 | { 583 | case Terminal::SK_Left: 584 | seekBackwardRequested(); 585 | m_lastSeekBwd = ros::SteadyTime::now(); 586 | break; 587 | case Terminal::SK_Right: 588 | seekForwardRequested(); 589 | m_lastSeekFwd = ros::SteadyTime::now(); 590 | break; 591 | case ' ': 592 | pauseRequested(); 593 | break; 594 | case 'q': 595 | exitRequested(); 596 | break; 597 | } 598 | } 599 | 600 | } 601 | -------------------------------------------------------------------------------- /rosbag_fancy/src/bag_reader.cpp: -------------------------------------------------------------------------------- 1 | // Fast native bag reader 2 | // Author: Max Schwarz 3 | 4 | #include "bag_reader.h" 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | #include 13 | 14 | #include 15 | 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | #include "doctest.h" 22 | #include "rosbag/stream.h" 23 | 24 | namespace 25 | { 26 | using Exception = rosbag_fancy::BagReader::Exception; 27 | 28 | struct Span 29 | { 30 | uint8_t* start; 31 | std::size_t size; 32 | }; 33 | 34 | struct Record 35 | { 36 | uint8_t* headerBegin; 37 | uint32_t headerSize; 38 | 39 | uint8_t* dataBegin; 40 | uint32_t dataSize; 41 | 42 | uint8_t* end; 43 | 44 | std::map headers; 45 | 46 | uint8_t op; 47 | 48 | template 49 | T integralHeader(const std::string& name) 50 | { 51 | auto it = headers.find(name); 52 | if(it == headers.end()) 53 | throw Exception{fmt::format("Could not find header '{}' in record of type {}\n", name, op)}; 54 | 55 | if(it->second.size != sizeof(T)) 56 | throw Exception{fmt::format("Header '{}' has wrong size {} (expected {})\n", name, it->second.size, sizeof(T))}; 57 | 58 | T ret; 59 | std::memcpy(&ret, it->second.start, sizeof(T)); 60 | return ret; 61 | } 62 | 63 | std::string stringHeader(const std::string& name, const std::optional& defaultValue = {}) 64 | { 65 | auto it = headers.find(name); 66 | if(it == headers.end()) 67 | { 68 | if(defaultValue) 69 | return *defaultValue; 70 | else 71 | throw Exception{fmt::format("Could not find header '{}' in record of type {}\n", name, op)}; 72 | } 73 | 74 | return {reinterpret_cast(it->second.start), it->second.size}; 75 | } 76 | }; 77 | 78 | struct Chunk 79 | { 80 | static_assert(sizeof(rosbag_fancy::BagReader::ConnectionInfo) == 8, "ConnectionInfo should have size 8"); 81 | 82 | uint8_t* chunkStart; 83 | std::size_t chunkCompressedSize = 0; 84 | 85 | ros::Time startTime; 86 | ros::Time endTime; 87 | 88 | std::vector connectionInfos; 89 | }; 90 | 91 | std::map readHeader(uint8_t* base, std::size_t remainingSize) 92 | { 93 | std::map ret; 94 | 95 | while(remainingSize > 0) 96 | { 97 | if(remainingSize < 4) 98 | throw Exception{"Record too small"}; 99 | 100 | uint32_t entrySize; 101 | std::memcpy(&entrySize, base, 4); 102 | 103 | base += 4; 104 | remainingSize -= 4; 105 | 106 | // Find '=' character 107 | auto* equal = reinterpret_cast(std::memchr(base, '=', entrySize)); 108 | if(!equal) 109 | throw Exception{"Invalid header map entry"}; 110 | 111 | ret[std::string(reinterpret_cast(base), equal - base)] 112 | = Span{equal+1, static_cast(entrySize - 1 - (equal - base))}; 113 | 114 | base += entrySize; 115 | remainingSize -= entrySize; 116 | } 117 | 118 | return ret; 119 | } 120 | 121 | Record readRecord(uint8_t* base, std::size_t remainingSize) 122 | { 123 | Record record; 124 | 125 | if(remainingSize < 4) 126 | throw Exception{"Record too small"}; 127 | 128 | std::memcpy(&record.headerSize, base, 4); 129 | remainingSize -= 4; 130 | base += 4; 131 | 132 | if(remainingSize < record.headerSize) 133 | throw Exception{"Record too small"}; 134 | 135 | record.headerBegin = base; 136 | 137 | base += record.headerSize; 138 | remainingSize -= record.headerSize; 139 | 140 | if(remainingSize < 4) 141 | throw Exception{"Record too small"}; 142 | 143 | std::memcpy(&record.dataSize, base, 4); 144 | remainingSize -= 4; 145 | base += 4; 146 | 147 | if(remainingSize < record.dataSize) 148 | throw Exception{"Record too small"}; 149 | 150 | record.dataBegin = base; 151 | 152 | record.end = base + record.dataSize; 153 | 154 | // Parse header 155 | record.headers = readHeader(record.headerBegin, record.headerSize); 156 | 157 | auto it = record.headers.find("op"); 158 | if(it == record.headers.end()) 159 | throw Exception{"Record without op header"}; 160 | 161 | if(it->second.size != 1) 162 | throw Exception{fmt::format("op header has invalid size {}", it->second.size)}; 163 | 164 | record.op = it->second.start[0]; 165 | 166 | return record; 167 | } 168 | 169 | 170 | class MappedFile 171 | { 172 | public: 173 | explicit MappedFile(const std::string& file) 174 | { 175 | m_fd = open(file.c_str(), O_RDONLY); 176 | if(m_fd < 0) 177 | throw Exception{fmt::format("Could not open file: {}", strerror(errno))}; 178 | 179 | // Measure size 180 | m_size = lseek(m_fd, 0, SEEK_END); 181 | if(m_size < 0) 182 | { 183 | close(m_fd); 184 | throw Exception{fmt::format("Could not seek: {}", strerror(errno))}; 185 | } 186 | 187 | lseek(m_fd, 0, SEEK_SET); 188 | 189 | m_data = reinterpret_cast(mmap(nullptr, m_size, PROT_READ, MAP_PRIVATE, m_fd, 0)); 190 | if(m_data == MAP_FAILED) 191 | { 192 | close(m_fd); 193 | throw Exception{fmt::format("Could not mmap() bagfile: {}", strerror(errno))}; 194 | } 195 | } 196 | 197 | ~MappedFile() 198 | { 199 | munmap(m_data, m_size); 200 | close(m_fd); 201 | } 202 | 203 | off_t size() const 204 | { return m_size; } 205 | 206 | void* data() 207 | { return m_data; } 208 | 209 | private: 210 | int m_fd; 211 | void* m_data; 212 | off_t m_size; 213 | }; 214 | } 215 | 216 | template <> struct fmt::formatter: formatter 217 | { 218 | template 219 | auto format(Span c, FormatContext& ctx) 220 | { 221 | if(std::all_of(c.start, c.start + c.size, [](uint8_t c){return std::isprint(c);})) 222 | return formatter::format(string_view(reinterpret_cast(c.start), c.size), ctx); 223 | else 224 | { 225 | std::stringstream s; 226 | for(std::size_t i = 0; i < c.size; ++i) 227 | { 228 | if(i != 0) 229 | fmt::print(s, " "); 230 | 231 | fmt::print(s, "{:02X}", c.start[i]); 232 | } 233 | 234 | return formatter::format(string_view(s.str()), ctx); 235 | } 236 | } 237 | }; 238 | 239 | 240 | namespace rosbag_fancy 241 | { 242 | 243 | class BagReader::Private 244 | { 245 | public: 246 | explicit Private(const std::string& filename) 247 | : file{filename} 248 | {} 249 | 250 | MappedFile file; 251 | off_t size; 252 | uint8_t* data; 253 | 254 | std::vector chunks; 255 | std::map connections; 256 | std::map> connectionsForTopic; 257 | 258 | ros::Time startTime; 259 | ros::Time endTime; 260 | 261 | bool isMonotonic = true; 262 | }; 263 | 264 | class BagReader::ChunkIterator::Private 265 | { 266 | public: 267 | enum class Compression 268 | { 269 | None, 270 | BZ2, 271 | LZ4 272 | }; 273 | 274 | enum class ReadResult 275 | { 276 | OK, 277 | EndOfFile 278 | }; 279 | 280 | ~Private() 281 | { 282 | switch(m_compression) 283 | { 284 | case Compression::None: 285 | break; 286 | case Compression::BZ2: 287 | BZ2_bzDecompressEnd(&m_bzStream); 288 | break; 289 | case Compression::LZ4: 290 | break; 291 | } 292 | } 293 | 294 | void readData(std::size_t amount) 295 | { 296 | switch(m_compression) 297 | { 298 | case Private::Compression::None: 299 | case Private::Compression::LZ4: 300 | return; 301 | case Private::Compression::BZ2: 302 | { 303 | std::size_t offset = m_uncompressedBuffer.size(); 304 | m_uncompressedBuffer.resize(offset + amount); 305 | 306 | m_bzStream.next_out = reinterpret_cast(m_uncompressedBuffer.data() + offset); 307 | m_bzStream.avail_out = amount; 308 | m_bzStream.total_out_lo32 = 0; 309 | 310 | int ret = BZ2_bzDecompress(&m_bzStream); 311 | if(m_bzStream.total_out_lo32 != amount) 312 | throw Exception{"Compressed chunk data ends prematurely! (not enough output)"}; 313 | 314 | if(ret != BZ_STREAM_END && ret != BZ_OK) 315 | throw Exception{fmt::format("BZ2 decoding error ({})", ret)}; 316 | 317 | return; 318 | } 319 | } 320 | 321 | throw std::logic_error{"non-implemented compression mode"}; 322 | } 323 | 324 | Record readRecord() 325 | { 326 | switch(m_compression) 327 | { 328 | case Compression::None: 329 | case Compression::LZ4: 330 | { 331 | auto record = ::readRecord(m_dataPtr, m_remaining); 332 | m_remaining -= (record.end - m_dataPtr); 333 | m_dataPtr = record.end; 334 | 335 | return record; 336 | } 337 | 338 | case Compression::BZ2: 339 | { 340 | Record record; 341 | 342 | m_uncompressedBuffer.clear(); 343 | readData(4); 344 | 345 | std::memcpy(&record.headerSize, m_uncompressedBuffer.data(), 4); 346 | readData(record.headerSize + 4); 347 | 348 | std::memcpy(&record.dataSize, m_uncompressedBuffer.data() + m_uncompressedBuffer.size() - 4, 4); 349 | readData(record.dataSize); 350 | 351 | record.headerBegin = m_uncompressedBuffer.data() + 4; 352 | record.dataBegin = m_uncompressedBuffer.data() + 4 + record.headerSize + 4; 353 | 354 | record.headers = readHeader(record.headerBegin, record.headerSize); 355 | 356 | auto it = record.headers.find("op"); 357 | if(it == record.headers.end()) 358 | throw Exception{"Record without op header"}; 359 | 360 | if(it->second.size != 1) 361 | throw Exception{fmt::format("op header has invalid size {}", it->second.size)}; 362 | 363 | record.op = it->second.start[0]; 364 | 365 | m_remaining -= m_uncompressedBuffer.size(); 366 | 367 | return record; 368 | } 369 | } 370 | 371 | throw std::logic_error{"non-implemented compression mode"}; 372 | } 373 | 374 | const BagReader* m_reader{}; 375 | std::size_t m_chunk = 0; 376 | 377 | Compression m_compression; 378 | bz_stream m_bzStream{}; 379 | 380 | uint8_t* m_dataPtr = {}; 381 | std::size_t m_remaining = 0; 382 | 383 | std::vector m_uncompressedBuffer; 384 | }; 385 | 386 | BagReader::ChunkIterator::ChunkIterator(const BagReader* reader, int chunk) 387 | : m_d{std::make_unique()} 388 | { 389 | m_d->m_reader = reader; 390 | m_d->m_chunk = chunk; 391 | 392 | auto& chunkData = reader->m_d->chunks[chunk]; 393 | auto rec = readRecord(chunkData.chunkStart, chunkData.chunkCompressedSize); 394 | 395 | std::string compression = rec.stringHeader("compression"); 396 | if(compression == "none") 397 | m_d->m_compression = Private::Compression::None; 398 | else if(compression == "bz2") 399 | m_d->m_compression = Private::Compression::BZ2; 400 | else if(compression == "lz4") 401 | m_d->m_compression = Private::Compression::LZ4; 402 | else 403 | throw Exception{fmt::format("Unknown compression type '{}'", compression)}; 404 | 405 | switch(m_d->m_compression) 406 | { 407 | case Private::Compression::None: 408 | m_d->m_dataPtr = rec.dataBegin; 409 | m_d->m_remaining = rec.dataSize; 410 | break; 411 | 412 | case Private::Compression::BZ2: 413 | { 414 | int ret = BZ2_bzDecompressInit(&m_d->m_bzStream, 0, 0); 415 | if(ret != BZ_OK) 416 | throw Exception{fmt::format("Could not initialize BZ2 decompression ({})", ret)}; 417 | 418 | m_d->m_bzStream.next_in = reinterpret_cast(rec.dataBegin); 419 | m_d->m_bzStream.avail_in = rec.dataSize; 420 | 421 | m_d->m_remaining = rec.integralHeader("size"); 422 | 423 | break; 424 | } 425 | 426 | case Private::Compression::LZ4: 427 | { 428 | // roslz4's streaming capabilities are not flexible enough for the code path above. 429 | // So for now, we just decode the entire chunk on the fly. 430 | 431 | m_d->m_remaining = rec.integralHeader("size"); 432 | m_d->m_uncompressedBuffer.resize(m_d->m_remaining); 433 | 434 | unsigned int length = m_d->m_remaining; 435 | 436 | int ret = roslz4_buffToBuffDecompress( 437 | reinterpret_cast(rec.dataBegin), rec.dataSize, 438 | reinterpret_cast(m_d->m_uncompressedBuffer.data()), &length 439 | ); 440 | 441 | if(ret != ROSLZ4_OK) 442 | throw Exception{fmt::format("Could not decode LZ4 chunk: {}", ret)}; 443 | 444 | if(m_d->m_remaining != length) 445 | throw Exception{fmt::format("LZ4 size mismatch: got {}, expected {} bytes", length, m_d->m_remaining)}; 446 | 447 | m_d->m_dataPtr = m_d->m_uncompressedBuffer.data(); 448 | 449 | break; 450 | } 451 | } 452 | 453 | (*this)++; 454 | } 455 | 456 | BagReader::ChunkIterator& BagReader::ChunkIterator::operator++() 457 | { 458 | if(!m_d) 459 | return *this; 460 | 461 | while(true) 462 | { 463 | if(m_d->m_remaining == 0) 464 | { 465 | m_d.reset(); 466 | m_idx = 0; 467 | return *this; 468 | } 469 | 470 | auto rec = m_d->readRecord(); 471 | 472 | if(rec.op == 0x02) 473 | { 474 | m_msg.m_data = rec.dataBegin; 475 | m_msg.m_size = rec.dataSize; 476 | 477 | uint32_t connID = rec.integralHeader("conn"); 478 | if(connID > m_d->m_reader->connections().size()) 479 | throw Exception{fmt::format("Invalid connection ID {}", connID)}; 480 | 481 | m_msg.connection = &m_d->m_reader->connections().at(connID); 482 | 483 | uint64_t time = rec.integralHeader("time"); 484 | m_msg.stamp = ros::Time(time & 0xFFFFFFFF, time >> 32); 485 | 486 | break; 487 | } 488 | } 489 | 490 | return *this; 491 | } 492 | 493 | BagReader::Iterator::Iterator(const BagReader* reader, int chunk) 494 | : m_reader{reader} 495 | , m_chunk{chunk} 496 | , m_it{reader, chunk} 497 | { 498 | } 499 | 500 | BagReader::Iterator& BagReader::Iterator::operator++() 501 | { 502 | m_it++; 503 | if(m_it == ChunkIterator{}) 504 | { 505 | m_chunk++; 506 | if(m_chunk < static_cast(m_reader->m_d->chunks.size())) 507 | { 508 | m_it = ChunkIterator{m_reader, m_chunk}; 509 | } 510 | else 511 | { 512 | m_chunk = -1; 513 | m_it = {}; 514 | } 515 | } 516 | 517 | return *this; 518 | } 519 | 520 | void BagReader::Iterator::advanceWithPredicates(const std::function& connPredicate, const std::function& messagePredicate) 521 | { 522 | m_it++; 523 | 524 | if(m_it == ChunkIterator{}) 525 | { 526 | m_chunk++; 527 | 528 | if(m_chunk == static_cast(m_reader->m_d->chunks.size())) 529 | { 530 | m_chunk = -1; 531 | m_it = {}; 532 | return; 533 | } 534 | } 535 | 536 | findNextWithPredicates(connPredicate, messagePredicate); 537 | } 538 | 539 | void BagReader::Iterator::findNextWithPredicates(const std::function& connPredicate, const std::function& messagePredicate) 540 | { 541 | auto currentChunkIsInteresting = [&](){ 542 | auto& chunkData = m_reader->m_d->chunks[m_chunk]; 543 | return std::any_of(chunkData.connectionInfos.begin(), chunkData.connectionInfos.end(), connPredicate); 544 | }; 545 | 546 | while(true) 547 | { 548 | if(m_it == ChunkIterator{}) 549 | { 550 | if(m_chunk == -1 || m_chunk == static_cast(m_reader->m_d->chunks.size())) 551 | { 552 | m_chunk = -1; 553 | return; 554 | } 555 | 556 | // Find next matching chunk 557 | while(!currentChunkIsInteresting()) 558 | { 559 | m_chunk++; 560 | if(m_chunk >= static_cast(m_reader->m_d->chunks.size())) 561 | { 562 | m_chunk = -1; 563 | return; 564 | } 565 | } 566 | 567 | m_it = ChunkIterator{m_reader, m_chunk}; 568 | } 569 | 570 | // We are now in a chunk which contains interesting connections. 571 | 572 | // Iterate through the chunk 573 | while(m_it != ChunkIterator{}) 574 | { 575 | if(messagePredicate(*m_it)) 576 | return; // Found something! *this is pointing at the message. 577 | 578 | ++m_it; 579 | } 580 | 581 | // Next chunk 582 | m_chunk++; 583 | } 584 | } 585 | 586 | std::vector& BagReader::Iterator::currentChunkConnections() const 587 | { 588 | return m_reader->m_d->chunks[m_chunk].connectionInfos; 589 | } 590 | 591 | rosbag::CompressionType BagReader::Iterator::currentChunkCompression() const 592 | { 593 | switch(m_it.m_d->m_compression) 594 | { 595 | case ChunkIterator::Private::Compression::None: 596 | return rosbag::compression::Uncompressed; 597 | case ChunkIterator::Private::Compression::BZ2: 598 | return rosbag::compression::BZ2; 599 | case ChunkIterator::Private::Compression::LZ4: 600 | return rosbag::compression::LZ4; 601 | } 602 | 603 | throw std::logic_error{"unknown compression"}; 604 | } 605 | 606 | 607 | BagReader::BagReader(const std::string& filename) 608 | : m_d{std::make_unique(filename)} 609 | { 610 | m_d->data = reinterpret_cast(m_d->file.data()); 611 | m_d->size = m_d->file.size(); 612 | 613 | // Read version line 614 | std::string versionLine; 615 | uint8_t* bagHeaderBegin{}; 616 | { 617 | constexpr int MAX_VERSION_LENGTH = 20; 618 | auto* newline = reinterpret_cast(std::memchr( 619 | m_d->data, '\n', std::min(m_d->size, MAX_VERSION_LENGTH)) 620 | ); 621 | if(!newline) 622 | throw Exception{fmt::format("Could not read version line\n")}; 623 | 624 | versionLine = std::string(reinterpret_cast(m_d->data), newline - m_d->data); 625 | bagHeaderBegin = newline + 1; 626 | } 627 | 628 | if(versionLine != "#ROSBAG V2.0") 629 | throw Exception{"I can't read this file (I can only read ROSBAG V2.0"}; 630 | 631 | Record bagHeader = readRecord(bagHeaderBegin, m_d->size - (bagHeaderBegin - m_d->data)); 632 | 633 | if(bagHeader.op != 0x03) 634 | throw Exception{"First record is not a bag header\n"}; 635 | 636 | std::uint64_t indexPos = bagHeader.integralHeader("index_pos"); 637 | std::uint32_t connectionCount = bagHeader.integralHeader("conn_count"); 638 | std::uint32_t chunkCount = bagHeader.integralHeader("chunk_count"); 639 | 640 | if(indexPos >= static_cast(m_d->size)) 641 | throw Exception{fmt::format("Index position is too large: {} vs data size {}", indexPos, m_d->size)}; 642 | 643 | // Read all index records 644 | uint8_t* rptr = m_d->data + indexPos; 645 | std::size_t remaining = m_d->size - indexPos; 646 | 647 | // Connection records 648 | for(std::size_t i = 0; i < connectionCount; ++i) 649 | { 650 | Record rec = readRecord(rptr, remaining); 651 | if(rec.op != 7) 652 | throw Exception{"Expected connection record"}; 653 | 654 | Record recData; 655 | recData.headers = readHeader(rec.dataBegin, rec.dataSize); 656 | 657 | Connection con; 658 | con.id = rec.integralHeader("conn"); 659 | con.topicInBag = rec.stringHeader("topic"); 660 | con.topicAsPublished = recData.stringHeader("topic", std::string{}); 661 | con.type = recData.stringHeader("type"); 662 | con.md5sum = recData.stringHeader("md5sum"); 663 | con.msgDef = recData.stringHeader("message_definition"); 664 | 665 | { 666 | auto it = recData.headers.find("callerid"); 667 | if(it != recData.headers.end()) 668 | con.callerID = recData.stringHeader("callerid"); 669 | } 670 | { 671 | auto it = recData.headers.find("latching"); 672 | if(it != recData.headers.end()) 673 | { 674 | if(it->second.size != 1) 675 | throw Exception{"Invalid latching header"}; 676 | 677 | if(it->second.start[0] == '1') 678 | con.latching = true; 679 | } 680 | } 681 | 682 | m_d->connections[con.id] = con; 683 | m_d->connectionsForTopic[con.topicInBag].push_back(con.id); 684 | 685 | rptr = rec.end; 686 | remaining = m_d->size - (rptr - m_d->data); 687 | } 688 | 689 | // Chunk infos 690 | m_d->chunks.reserve(chunkCount); 691 | Chunk* lastChunk = {}; 692 | for(std::size_t i = 0; i < chunkCount; ++i) 693 | { 694 | Record rec = readRecord(rptr, remaining); 695 | if(rec.op != 6) 696 | throw Exception{"Expected chunk info record"}; 697 | 698 | auto& chunk = m_d->chunks.emplace_back(); 699 | 700 | auto version = rec.integralHeader("ver"); 701 | if(version != 1) 702 | throw Exception{fmt::format("Unsupported chunk info version {}", version)}; 703 | 704 | auto chunkPos = rec.integralHeader("chunk_pos"); 705 | if(chunkPos >= static_cast(m_d->size)) 706 | throw Exception{"chunk_pos points outside of valid range"}; 707 | 708 | chunk.chunkStart = m_d->data + chunkPos; 709 | if(lastChunk) 710 | lastChunk->chunkCompressedSize = chunk.chunkStart - lastChunk->chunkStart; 711 | 712 | std::uint64_t startTime = rec.integralHeader("start_time"); 713 | std::uint64_t endTime = rec.integralHeader("end_time"); 714 | 715 | chunk.startTime = ros::Time(startTime & 0xFFFFFFFF, startTime >> 32); 716 | chunk.endTime = ros::Time(endTime & 0xFFFFFFFF, endTime >> 32); 717 | 718 | if(lastChunk && lastChunk->endTime > chunk.startTime) 719 | m_d->isMonotonic = false; 720 | 721 | auto numConnections = rec.integralHeader("count"); 722 | 723 | if(rec.dataSize < numConnections * sizeof(ConnectionInfo)) 724 | throw Exception{"Chunk info is too small"}; 725 | 726 | chunk.connectionInfos.resize(numConnections); 727 | std::memcpy(chunk.connectionInfos.data(), rec.dataBegin, numConnections * sizeof(ConnectionInfo)); 728 | 729 | for(auto& connInfo : chunk.connectionInfos) 730 | { 731 | auto it = m_d->connections.find(connInfo.id); 732 | if(it == m_d->connections.end()) 733 | throw Exception{fmt::format("Could not find connection with id {}", connInfo.id)}; 734 | 735 | it->second.totalCount += connInfo.msgCount; 736 | } 737 | 738 | rptr = rec.end; 739 | remaining = m_d->size - (rptr - m_d->data); 740 | lastChunk = &chunk; 741 | } 742 | if(lastChunk) 743 | lastChunk->chunkCompressedSize = (m_d->data + m_d->size) - lastChunk->chunkStart; 744 | 745 | if(!m_d->chunks.empty()) 746 | m_d->startTime = m_d->chunks.front().startTime; 747 | 748 | for(auto& c : m_d->chunks) 749 | { 750 | m_d->startTime = std::min(c.startTime, m_d->startTime); 751 | m_d->endTime = std::max(c.endTime, m_d->endTime); 752 | } 753 | } 754 | 755 | BagReader::BagReader(BagReader&& other) 756 | : m_d{std::move(other.m_d)} 757 | { 758 | } 759 | 760 | BagReader::~BagReader() 761 | { 762 | } 763 | 764 | const BagReader::ConnectionMap& BagReader::connections() const 765 | { 766 | return m_d->connections; 767 | } 768 | 769 | ros::Time BagReader::startTime() const 770 | { 771 | return m_d->startTime; 772 | } 773 | 774 | ros::Time BagReader::endTime() const 775 | { 776 | return m_d->endTime; 777 | } 778 | 779 | std::size_t BagReader::size() const 780 | { 781 | return m_d->size; 782 | } 783 | 784 | BagReader::Iterator BagReader::begin() const 785 | { 786 | if(m_d->chunks.empty()) 787 | return {}; 788 | 789 | return Iterator{this, 0}; 790 | } 791 | 792 | BagReader::Iterator BagReader::end() const 793 | { 794 | return Iterator(); 795 | } 796 | 797 | BagReader::Iterator BagReader::findTime(const ros::Time& time) const 798 | { 799 | // Find chunk 800 | auto chunkIt = std::lower_bound(m_d->chunks.begin(), m_d->chunks.end(), time, [&](Chunk& chunk, const ros::Time& time){ 801 | return chunk.endTime < time; 802 | }); 803 | 804 | if(chunkIt == m_d->chunks.end()) 805 | return {}; 806 | 807 | int chunkIdx = chunkIt - m_d->chunks.begin(); 808 | 809 | // Find message in chunk 810 | auto it = Iterator{this, chunkIdx}; 811 | for(; it != end(); ++it) 812 | { 813 | if(it->stamp > time) 814 | return it; 815 | } 816 | 817 | return it; 818 | } 819 | 820 | BagReader::Iterator BagReader::chunkBegin(int chunk) const 821 | { 822 | if(chunk < 0 || chunk >= static_cast(m_d->chunks.size())) 823 | return {}; 824 | 825 | return Iterator{this, chunk}; 826 | } 827 | 828 | std::size_t BagReader::numChunks() const 829 | { 830 | return m_d->chunks.size(); 831 | } 832 | 833 | bool BagReader::isMonotonic() const 834 | { 835 | return m_d->isMonotonic; 836 | } 837 | 838 | 839 | // TESTS 840 | 841 | TEST_CASE("BagView: One file") 842 | { 843 | // Generate a bag file 844 | char bagfileName[] = "/tmp/rosbag_fancy_test_XXXXXX"; 845 | { 846 | int fd = mkstemp(bagfileName); 847 | REQUIRE(fd >= 0); 848 | close(fd); 849 | 850 | rosbag::Bag bag{bagfileName, rosbag::BagMode::Write}; 851 | 852 | SUBCASE("plain") 853 | { bag.setCompression(rosbag::CompressionType::Uncompressed); } 854 | SUBCASE("BZ2") 855 | { bag.setCompression(rosbag::CompressionType::BZ2); } 856 | SUBCASE("LZ4") 857 | { bag.setCompression(rosbag::CompressionType::LZ4); } 858 | 859 | { 860 | std_msgs::Header msg; 861 | msg.frame_id = "a"; 862 | bag.write("/topicA", ros::Time(1000, 0), msg); 863 | } 864 | { 865 | std_msgs::Header msg; 866 | msg.frame_id = "b"; 867 | bag.write("/topicB", ros::Time(1001, 0), msg); 868 | } 869 | { 870 | std_msgs::UInt8 msg; 871 | msg.data = 123; 872 | bag.write("/topicC", ros::Time(1002, 0), msg); 873 | } 874 | 875 | bag.close(); 876 | } 877 | 878 | // Open bagfile 879 | BagReader reader{bagfileName}; 880 | 881 | auto it = reader.begin(); 882 | 883 | REQUIRE(it != reader.end()); 884 | CHECK(it->connection->topicInBag == "/topicA"); 885 | 886 | ++it; REQUIRE(it != reader.end()); 887 | CHECK(it->connection->topicInBag == "/topicB"); 888 | 889 | ++it; REQUIRE(it != reader.end()); 890 | CHECK(it->connection->topicInBag == "/topicC"); 891 | 892 | ++it; CHECK(it == reader.end()); 893 | } 894 | 895 | 896 | } 897 | --------------------------------------------------------------------------------