chunked_file.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of Willow Garage, Inc. nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 ********************************************************************/
34 
35 #include "rosbag/chunked_file.h"
36 
37 #include <iostream>
38 
39 #include "boost/format.hpp"
40 
41 //#include <ros/ros.h>
42 #ifdef _WIN32
43 # ifdef __MINGW32__
44 # define fseeko fseeko64
45 # define ftello ftello64
46 // not sure if we need a ftruncate here yet or not
47 # else
48 # include <io.h>
49 # define fseeko _fseeki64
50 # define ftello _ftelli64
51 # define fileno _fileno
52 # define ftruncate _chsize_s //Intel Realsense Change, Was: #define ftruncate _chsize
53 # endif
54 #endif
55 
56 using std::string;
57 using boost::format;
58 using std::shared_ptr;
60 
61 namespace rosbag {
62 
64  file_(NULL),
65  offset_(0),
66  compressed_in_(0),
67  unused_(NULL),
68  nUnused_(0)
69 {
70  stream_factory_ = std::make_shared<StreamFactory>(this);
71 }
72 
74  close();
75 }
76 
77 void ChunkedFile::openReadWrite(string const& filename) { open(filename, "r+b"); }
78 void ChunkedFile::openWrite (string const& filename) { open(filename, "w+b"); }
79 void ChunkedFile::openRead (string const& filename) { open(filename, "rb"); }
80 
81 void ChunkedFile::open(string const& filename, string const& mode) {
82  // Check if file is already open
83  if (file_)
84  throw BagIOException((format("File already open: %1%") % filename_.c_str()).str());
85 
86  // Open the file
87  if (mode == "r+b") {
88  // check if file already exists
89  #if defined(_MSC_VER) && (_MSC_VER >= 1400 )
90  fopen_s( &file_, filename.c_str(), "r" );
91  #else
92  file_ = fopen(filename.c_str(), "r");
93  #endif
94  if (file_ == NULL)
95  // create an empty file and open it for update
96  #if defined(_MSC_VER) && (_MSC_VER >= 1400 )
97  fopen_s( &file_, filename.c_str(), "w+b" );
98  #else
99  file_ = fopen(filename.c_str(), "w+b");
100  #endif
101  else {
102  fclose(file_);
103  // open existing file for update
104  #if defined(_MSC_VER) && (_MSC_VER >= 1400 )
105  fopen_s( &file_, filename.c_str(), "r+b" );
106  #else
107  file_ = fopen(filename.c_str(), "r+b");
108  #endif
109  }
110  }
111  else
112  #if defined(_MSC_VER) && (_MSC_VER >= 1400 )
113  fopen_s( &file_, filename.c_str(), mode.c_str() );
114  #else
115  file_ = fopen(filename.c_str(), mode.c_str());
116  #endif
117 
118  if (!file_)
119  throw BagIOException((format("Error opening file: %1%") % filename.c_str()).str());
120 
121  read_stream_ = std::make_shared<UncompressedStream>(this);
122  write_stream_ = std::make_shared<UncompressedStream>(this);
124  offset_ = ftello(file_);
125 }
126 
127 bool ChunkedFile::good() const {
128  return feof(file_) == 0 && ferror(file_) == 0;
129 }
130 
131 bool ChunkedFile::isOpen() const { return file_ != NULL; }
132 string ChunkedFile::getFileName() const { return filename_; }
133 
135  if (!file_)
136  return;
137 
138  // Close any compressed stream by changing to uncompressed mode
140 
141  // Close the file
142  int success = fclose(file_);
143  if (success != 0)
144  throw BagIOException((format("Error closing file: %1%") % filename_.c_str()).str());
145 
146  file_ = NULL;
147  filename_.clear();
148 
149  clearUnused();
150 }
151 
152 // Read/write modes
153 
155  if (!file_)
156  throw BagIOException("Can't set compression mode before opening a file");
157 
158  if (type != write_stream_->getCompressionType()) {
159  write_stream_->stopWrite();
160  shared_ptr<Stream> stream = stream_factory_->getStream(type);
161  stream->startWrite();
162  write_stream_ = stream;
163  }
164 }
165 
167  if (!file_)
168  throw BagIOException("Can't set compression mode before opening a file");
169 
170  if (type != read_stream_->getCompressionType()) {
171  read_stream_->stopRead();
172  shared_ptr<Stream> stream = stream_factory_->getStream(type);
173  stream->startRead();
174  read_stream_ = stream;
175  }
176 }
177 
178 void ChunkedFile::seek(uint64_t offset, int origin) {
179  if (!file_)
180  throw BagIOException("Can't seek - file not open");
181 
183 
184  int success = fseeko(file_, offset, origin);
185  if (success != 0)
186  throw BagIOException("Error seeking");
187 
188  offset_ = ftello(file_);
189 }
190 
193 
194 void ChunkedFile::write(string const& s) { write((void*) s.c_str(), s.size()); }
195 void ChunkedFile::write(void* ptr, size_t size) { write_stream_->write(ptr, size); }
196 void ChunkedFile::read(void* ptr, size_t size) { read_stream_->read(ptr, size); }
197 
199  int fd = fileno(file_);
200  return ftruncate(fd, length) == 0;
201 }
202 
205  char buffer[1024];
206  if(fgets(buffer, 1024, file_))
207  {
208  string s(buffer);
209  offset_ += s.size();
210  return s;
211  }
212  else
213  return string("");
214 }
215 
216 void ChunkedFile::decompress(CompressionType compression, uint8_t* dest, unsigned int dest_len, uint8_t* source, unsigned int source_len) {
217  stream_factory_->getStream(compression)->decompress(dest, dest_len, source, source_len);
218 }
219 
221  unused_ = NULL;
222  nUnused_ = 0;
223 }
224 
225 } // namespace rosbag
void openRead(std::string const &filename)
open file for reading
std::shared_ptr< StreamFactory > stream_factory_
Definition: chunked_file.h:93
GLsizei GLsizei GLchar * source
void setReadMode(CompressionType type)
std::shared_ptr< Stream > read_stream_
Definition: chunked_file.h:95
void read(void *ptr, size_t size)
read size bytes from the file into ptr
void open(std::string const &filename, std::string const &mode)
GLdouble s
bool isOpen() const
return true if file is open for reading or writing
GLsizei const GLchar *const * string
void write(std::string const &s)
bool truncate(uint64_t length)
void seek(uint64_t offset, int origin=std::ios_base::beg)
seek to given offset from origin
unsigned char uint8_t
Definition: stdint.h:78
void close()
close the file
bool good() const
return true if hasn&#39;t reached end-of-file and no error
GLenum GLfloat * buffer
std::string getFileName() const
return path of currently open file
uint64_t offset_
current position in the file
Definition: chunked_file.h:88
uint64_t getOffset() const
return current offset from the beginning of the file
void setWriteMode(CompressionType type)
uint32_t getCompressedBytesIn() const
return the number of bytes written to current compressed stream
GLenum mode
GLsizeiptr size
int nUnused_
number of bytes of extra data read by compressed stream
Definition: chunked_file.h:91
Base class for all exceptions thrown by ROS.
Definition: exception.h:39
unsigned int uint32_t
Definition: stdint.h:80
Exception thrown when on IO problems.
Definition: exceptions.h:50
GLint GLint GLsizei GLint GLenum format
unsigned __int64 uint64_t
Definition: stdint.h:90
std::string getline()
std::shared_ptr< Stream > write_stream_
Definition: chunked_file.h:96
uint64_t compressed_in_
number of bytes written to current compressed stream
Definition: chunked_file.h:89
std::string filename_
path to file
Definition: chunked_file.h:86
basic_format< char > format
Definition: format_fwd.hpp:25
void openWrite(std::string const &filename)
open file for writing
Definition: bag.h:66
GLenum type
FILE * file_
file pointer
Definition: chunked_file.h:87
void decompress(CompressionType compression, uint8_t *dest, unsigned int dest_len, uint8_t *source, unsigned int source_len)
#define NULL
Definition: tinycthread.c:47
GLenum GLuint GLenum GLsizei length
void openReadWrite(std::string const &filename)
open file for reading & writing
char * unused_
extra data read by compressed stream
Definition: chunked_file.h:90
GLintptr offset
char * dest
Definition: lz4.h:697


librealsense2
Author(s): LibRealSense ROS Team
autogenerated on Thu Dec 22 2022 03:43:16