|
| 1 | +// License: Apache 2.0. See LICENSE file in root directory. |
| 2 | +#include "ros2_reader.h" |
| 3 | +#include <rsutils/string/from.h> |
| 4 | +#include <algorithm> |
| 5 | +#include <cstring> |
| 6 | +#include <src/source.h> // for frame_source & stream_to_frame_types |
| 7 | + |
| 8 | +namespace librealsense { |
| 9 | +using namespace device_serializer; |
| 10 | + |
| 11 | +static std::string to_string_buffer( const rosbag2_storage::SerializedBagMessage & msg ) |
| 12 | +{ |
| 13 | + if( ! msg.serialized_data || ! msg.serialized_data->buffer ) return {}; |
| 14 | + return std::string( reinterpret_cast< char * >( msg.serialized_data->buffer ), msg.serialized_data->buffer_length ); |
| 15 | +} |
| 16 | + |
| 17 | +ros2_reader::ros2_reader( std::shared_ptr< rosbag2_storage::storage_interfaces::ReadOnlyInterface > storage ) |
| 18 | + : _storage( std::move( storage ) ) |
| 19 | +{ |
| 20 | + if( _storage ) |
| 21 | + _file = _storage->get_relative_file_path(); |
| 22 | + reset(); |
| 23 | +} |
| 24 | + |
| 25 | +device_snapshot ros2_reader::query_device_description( const nanoseconds & ) |
| 26 | +{ |
| 27 | + // Minimal empty snapshot; advanced device description reconstruction can be added later |
| 28 | + return device_snapshot(); |
| 29 | +} |
| 30 | + |
| 31 | +void ros2_reader::reset() |
| 32 | +{ |
| 33 | + _messages.clear(); |
| 34 | + _cursor = 0; |
| 35 | + if( ! _storage ) return; |
| 36 | + while( _storage->has_next() ) |
| 37 | + { |
| 38 | + auto m = _storage->read_next(); |
| 39 | + _messages.push_back( *m ); |
| 40 | + } |
| 41 | + if( ! _messages.empty() ) |
| 42 | + { |
| 43 | + auto first_ts = _messages.front().time_stamp; |
| 44 | + auto last_ts = _messages.back().time_stamp; |
| 45 | + if( last_ts > first_ts ) |
| 46 | + _duration = nanoseconds( static_cast< uint64_t >( last_ts - first_ts ) ); |
| 47 | + } |
| 48 | + _initialized = true; |
| 49 | +} |
| 50 | + |
| 51 | +void ros2_reader::seek_to_time( const nanoseconds & t ) |
| 52 | +{ |
| 53 | + if( _messages.empty() ) return; |
| 54 | + int64_t target = static_cast< int64_t >( t.count() ); |
| 55 | + _cursor = 0; |
| 56 | + while( _cursor < _messages.size() && _messages[ _cursor ].time_stamp < target ) |
| 57 | + ++_cursor; |
| 58 | +} |
| 59 | + |
| 60 | +std::shared_ptr< serialized_data > ros2_reader::parse_frame( std::string const & topic, const rosbag2_storage::SerializedBagMessage & msg ) |
| 61 | +{ |
| 62 | + auto sid = ros_topic::get_stream_identifier( topic ); |
| 63 | + frame_additional_data add{}; |
| 64 | + add.timestamp = double( msg.time_stamp ) / 1e6; // ns -> ms |
| 65 | + add.frame_number = 0; // will try to parse later from metadata message if present |
| 66 | + add.fisheye_ae_mode = false; |
| 67 | + |
| 68 | + auto size = msg.serialized_data->buffer_length; |
| 69 | + // allocate frame |
| 70 | + frame_source fs( 32 ); |
| 71 | + fs.init( nullptr ); |
| 72 | + frame_interface * fi = fs.alloc_frame( { sid.stream_type, sid.stream_index, librealsense::frame_source::stream_to_frame_types( sid.stream_type ) }, size, std::move( add ), true ); |
| 73 | + if( ! fi ) |
| 74 | + return std::make_shared< serialized_invalid_frame >( nanoseconds( msg.time_stamp ), sid ); |
| 75 | + std::memcpy( const_cast< uint8_t * >( fi->get_frame_data() ), msg.serialized_data->buffer, size ); |
| 76 | + frame_holder fh{ fi }; |
| 77 | + return std::make_shared< serialized_frame >( nanoseconds( msg.time_stamp ), sid, std::move( fh ) ); |
| 78 | +} |
| 79 | + |
| 80 | +std::shared_ptr< serialized_data > ros2_reader::parse_option( std::string const & topic, const rosbag2_storage::SerializedBagMessage & msg ) |
| 81 | +{ |
| 82 | + auto sid = ros_topic::get_sensor_identifier( topic ); |
| 83 | + std::string payload = to_string_buffer( msg ); |
| 84 | + float value = 0.f; |
| 85 | + try { value = std::stof( payload ); } catch(...) {} |
| 86 | + // option id from topic name |
| 87 | + rs2_option opt_id = RS2_OPTION_COUNT; |
| 88 | + try |
| 89 | + { |
| 90 | + std::string opt_name = ros_topic::get_option_name( topic ); |
| 91 | + std::replace( opt_name.begin(), opt_name.end(), '_', ' ' ); |
| 92 | + if( ! try_parse( opt_name, opt_id ) ) |
| 93 | + return nullptr; |
| 94 | + } |
| 95 | + catch(...) { return nullptr; } |
| 96 | + |
| 97 | + auto option = std::make_shared< const_value_option >( "Recorded option", value ); |
| 98 | + return std::make_shared< serialized_option >( nanoseconds( msg.time_stamp ), sid, opt_id, option ); |
| 99 | +} |
| 100 | + |
| 101 | +std::shared_ptr< serialized_data > ros2_reader::parse_notification( std::string const & topic, const rosbag2_storage::SerializedBagMessage & msg ) |
| 102 | +{ |
| 103 | + auto sid = ros_topic::get_sensor_identifier( topic ); |
| 104 | + std::string payload = to_string_buffer( msg ); |
| 105 | + // very simple parse key=value; pairs (only category/severity/description/timestamp extracted) |
| 106 | + rs2_notification_category category = RS2_NOTIFICATION_CATEGORY_UNKNOWN_ERROR; |
| 107 | + rs2_log_severity severity = RS2_LOG_SEVERITY_INFO; |
| 108 | + std::string description; |
| 109 | + uint64_t ts_val = msg.time_stamp; |
| 110 | + size_t pos = 0; |
| 111 | + while( pos < payload.size() ) |
| 112 | + { |
| 113 | + auto semi = payload.find( ';', pos ); |
| 114 | + std::string part = payload.substr( pos, semi == std::string::npos ? std::string::npos : semi - pos ); |
| 115 | + auto eq = part.find( '=' ); |
| 116 | + if( eq != std::string::npos ) |
| 117 | + { |
| 118 | + std::string key = part.substr( 0, eq ); |
| 119 | + std::string val = part.substr( eq + 1 ); |
| 120 | + if( key == "category" ) try_parse( val, category ); |
| 121 | + else if( key == "severity" ) try_parse( val, severity ); |
| 122 | + else if( key == "description" ) description = val; |
| 123 | + else if( key == "timestamp" ) { try { ts_val = std::stoull( val ); } catch(...) {} } |
| 124 | + } |
| 125 | + if( semi == std::string::npos ) break; |
| 126 | + pos = semi + 1; |
| 127 | + } |
| 128 | + notification n( category, 0, severity, description ); |
| 129 | + n.timestamp = ts_val; |
| 130 | + return std::make_shared< serialized_notification >( nanoseconds( msg.time_stamp ), sid, n ); |
| 131 | +} |
| 132 | + |
| 133 | +std::shared_ptr< serialized_data > ros2_reader::parse_message( const rosbag2_storage::SerializedBagMessage & msg ) |
| 134 | +{ |
| 135 | + std::string topic = msg.topic_name; |
| 136 | + if( topic.find( "/option/" ) != std::string::npos && topic.find( "/value" ) != std::string::npos ) |
| 137 | + return parse_option( topic, msg ); |
| 138 | + if( topic.find( "/notification/" ) != std::string::npos ) |
| 139 | + return parse_notification( topic, msg ); |
| 140 | + |
| 141 | + // frame data topics end with /data and contain raw bytes |
| 142 | + if( topic.find( "/data" ) != std::string::npos && topic.find( "/tf" ) == std::string::npos && topic.find( "/metadata" ) == std::string::npos ) |
| 143 | + return parse_frame( topic, msg ); |
| 144 | + |
| 145 | + return nullptr; // ignore others |
| 146 | +} |
| 147 | + |
| 148 | +std::shared_ptr< serialized_data > ros2_reader::read_next_data() |
| 149 | +{ |
| 150 | + if( _cursor >= _messages.size() ) |
| 151 | + return std::make_shared< serialized_end_of_file >(); |
| 152 | + while( _cursor < _messages.size() ) |
| 153 | + { |
| 154 | + auto & m = _messages[ _cursor++ ]; |
| 155 | + auto data = parse_message( m ); |
| 156 | + if( data ) return data; |
| 157 | + } |
| 158 | + return std::make_shared< serialized_end_of_file >(); |
| 159 | +} |
| 160 | + |
| 161 | +} // namespace librealsense |
0 commit comments