├── .gitignore ├── LICENSE ├── README.md ├── bagInfo.m ├── bagReader.m ├── combineTimes.m ├── flattenTable.m ├── matlab_bag_helper.py ├── py2Matlab.m ├── rosImageToMatlab.m └── setupEnv.m /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | *.m~ 3 | *.asv 4 | .idea 5 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 2, June 1991 3 | 4 | Copyright (C) 1989, 1991 Free Software Foundation, Inc., 5 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 6 | Everyone is permitted to copy and distribute verbatim copies 7 | of this license document, but changing it is not allowed. 8 | 9 | Preamble 10 | 11 | The licenses for most software are designed to take away your 12 | freedom to share and change it. By contrast, the GNU General Public 13 | License is intended to guarantee your freedom to share and change free 14 | software--to make sure the software is free for all its users. This 15 | General Public License applies to most of the Free Software 16 | Foundation's software and to any other program whose authors commit to 17 | using it. (Some other Free Software Foundation software is covered by 18 | the GNU Lesser General Public License instead.) You can apply it to 19 | your programs, too. 20 | 21 | When we speak of free software, we are referring to freedom, not 22 | price. Our General Public Licenses are designed to make sure that you 23 | have the freedom to distribute copies of free software (and charge for 24 | this service if you wish), that you receive source code or can get it 25 | if you want it, that you can change the software or use pieces of it 26 | in new free programs; and that you know you can do these things. 27 | 28 | To protect your rights, we need to make restrictions that forbid 29 | anyone to deny you these rights or to ask you to surrender the rights. 30 | These restrictions translate to certain responsibilities for you if you 31 | distribute copies of the software, or if you modify it. 32 | 33 | For example, if you distribute copies of such a program, whether 34 | gratis or for a fee, you must give the recipients all the rights that 35 | you have. You must make sure that they, too, receive or can get the 36 | source code. And you must show them these terms so they know their 37 | rights. 38 | 39 | We protect your rights with two steps: (1) copyright the software, and 40 | (2) offer you this license which gives you legal permission to copy, 41 | distribute and/or modify the software. 42 | 43 | Also, for each author's protection and ours, we want to make certain 44 | that everyone understands that there is no warranty for this free 45 | software. If the software is modified by someone else and passed on, we 46 | want its recipients to know that what they have is not the original, so 47 | that any problems introduced by others will not reflect on the original 48 | authors' reputations. 49 | 50 | Finally, any free program is threatened constantly by software 51 | patents. We wish to avoid the danger that redistributors of a free 52 | program will individually obtain patent licenses, in effect making the 53 | program proprietary. To prevent this, we have made it clear that any 54 | patent must be licensed for everyone's free use or not licensed at all. 55 | 56 | The precise terms and conditions for copying, distribution and 57 | modification follow. 58 | 59 | GNU GENERAL PUBLIC LICENSE 60 | TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION 61 | 62 | 0. This License applies to any program or other work which contains 63 | a notice placed by the copyright holder saying it may be distributed 64 | under the terms of this General Public License. The "Program", below, 65 | refers to any such program or work, and a "work based on the Program" 66 | means either the Program or any derivative work under copyright law: 67 | that is to say, a work containing the Program or a portion of it, 68 | either verbatim or with modifications and/or translated into another 69 | language. (Hereinafter, translation is included without limitation in 70 | the term "modification".) Each licensee is addressed as "you". 71 | 72 | Activities other than copying, distribution and modification are not 73 | covered by this License; they are outside its scope. The act of 74 | running the Program is not restricted, and the output from the Program 75 | is covered only if its contents constitute a work based on the 76 | Program (independent of having been made by running the Program). 77 | Whether that is true depends on what the Program does. 78 | 79 | 1. You may copy and distribute verbatim copies of the Program's 80 | source code as you receive it, in any medium, provided that you 81 | conspicuously and appropriately publish on each copy an appropriate 82 | copyright notice and disclaimer of warranty; keep intact all the 83 | notices that refer to this License and to the absence of any warranty; 84 | and give any other recipients of the Program a copy of this License 85 | along with the Program. 86 | 87 | You may charge a fee for the physical act of transferring a copy, and 88 | you may at your option offer warranty protection in exchange for a fee. 89 | 90 | 2. You may modify your copy or copies of the Program or any portion 91 | of it, thus forming a work based on the Program, and copy and 92 | distribute such modifications or work under the terms of Section 1 93 | above, provided that you also meet all of these conditions: 94 | 95 | a) You must cause the modified files to carry prominent notices 96 | stating that you changed the files and the date of any change. 97 | 98 | b) You must cause any work that you distribute or publish, that in 99 | whole or in part contains or is derived from the Program or any 100 | part thereof, to be licensed as a whole at no charge to all third 101 | parties under the terms of this License. 102 | 103 | c) If the modified program normally reads commands interactively 104 | when run, you must cause it, when started running for such 105 | interactive use in the most ordinary way, to print or display an 106 | announcement including an appropriate copyright notice and a 107 | notice that there is no warranty (or else, saying that you provide 108 | a warranty) and that users may redistribute the program under 109 | these conditions, and telling the user how to view a copy of this 110 | License. (Exception: if the Program itself is interactive but 111 | does not normally print such an announcement, your work based on 112 | the Program is not required to print an announcement.) 113 | 114 | These requirements apply to the modified work as a whole. If 115 | identifiable sections of that work are not derived from the Program, 116 | and can be reasonably considered independent and separate works in 117 | themselves, then this License, and its terms, do not apply to those 118 | sections when you distribute them as separate works. But when you 119 | distribute the same sections as part of a whole which is a work based 120 | on the Program, the distribution of the whole must be on the terms of 121 | this License, whose permissions for other licensees extend to the 122 | entire whole, and thus to each and every part regardless of who wrote it. 123 | 124 | Thus, it is not the intent of this section to claim rights or contest 125 | your rights to work written entirely by you; rather, the intent is to 126 | exercise the right to control the distribution of derivative or 127 | collective works based on the Program. 128 | 129 | In addition, mere aggregation of another work not based on the Program 130 | with the Program (or with a work based on the Program) on a volume of 131 | a storage or distribution medium does not bring the other work under 132 | the scope of this License. 133 | 134 | 3. You may copy and distribute the Program (or a work based on it, 135 | under Section 2) in object code or executable form under the terms of 136 | Sections 1 and 2 above provided that you also do one of the following: 137 | 138 | a) Accompany it with the complete corresponding machine-readable 139 | source code, which must be distributed under the terms of Sections 140 | 1 and 2 above on a medium customarily used for software interchange; or, 141 | 142 | b) Accompany it with a written offer, valid for at least three 143 | years, to give any third party, for a charge no more than your 144 | cost of physically performing source distribution, a complete 145 | machine-readable copy of the corresponding source code, to be 146 | distributed under the terms of Sections 1 and 2 above on a medium 147 | customarily used for software interchange; or, 148 | 149 | c) Accompany it with the information you received as to the offer 150 | to distribute corresponding source code. (This alternative is 151 | allowed only for noncommercial distribution and only if you 152 | received the program in object code or executable form with such 153 | an offer, in accord with Subsection b above.) 154 | 155 | The source code for a work means the preferred form of the work for 156 | making modifications to it. For an executable work, complete source 157 | code means all the source code for all modules it contains, plus any 158 | associated interface definition files, plus the scripts used to 159 | control compilation and installation of the executable. However, as a 160 | special exception, the source code distributed need not include 161 | anything that is normally distributed (in either source or binary 162 | form) with the major components (compiler, kernel, and so on) of the 163 | operating system on which the executable runs, unless that component 164 | itself accompanies the executable. 165 | 166 | If distribution of executable or object code is made by offering 167 | access to copy from a designated place, then offering equivalent 168 | access to copy the source code from the same place counts as 169 | distribution of the source code, even though third parties are not 170 | compelled to copy the source along with the object code. 171 | 172 | 4. You may not copy, modify, sublicense, or distribute the Program 173 | except as expressly provided under this License. Any attempt 174 | otherwise to copy, modify, sublicense or distribute the Program is 175 | void, and will automatically terminate your rights under this License. 176 | However, parties who have received copies, or rights, from you under 177 | this License will not have their licenses terminated so long as such 178 | parties remain in full compliance. 179 | 180 | 5. You are not required to accept this License, since you have not 181 | signed it. However, nothing else grants you permission to modify or 182 | distribute the Program or its derivative works. These actions are 183 | prohibited by law if you do not accept this License. Therefore, by 184 | modifying or distributing the Program (or any work based on the 185 | Program), you indicate your acceptance of this License to do so, and 186 | all its terms and conditions for copying, distributing or modifying 187 | the Program or works based on it. 188 | 189 | 6. Each time you redistribute the Program (or any work based on the 190 | Program), the recipient automatically receives a license from the 191 | original licensor to copy, distribute or modify the Program subject to 192 | these terms and conditions. You may not impose any further 193 | restrictions on the recipients' exercise of the rights granted herein. 194 | You are not responsible for enforcing compliance by third parties to 195 | this License. 196 | 197 | 7. If, as a consequence of a court judgment or allegation of patent 198 | infringement or for any other reason (not limited to patent issues), 199 | conditions are imposed on you (whether by court order, agreement or 200 | otherwise) that contradict the conditions of this License, they do not 201 | excuse you from the conditions of this License. If you cannot 202 | distribute so as to satisfy simultaneously your obligations under this 203 | License and any other pertinent obligations, then as a consequence you 204 | may not distribute the Program at all. For example, if a patent 205 | license would not permit royalty-free redistribution of the Program by 206 | all those who receive copies directly or indirectly through you, then 207 | the only way you could satisfy both it and this License would be to 208 | refrain entirely from distribution of the Program. 209 | 210 | If any portion of this section is held invalid or unenforceable under 211 | any particular circumstance, the balance of the section is intended to 212 | apply and the section as a whole is intended to apply in other 213 | circumstances. 214 | 215 | It is not the purpose of this section to induce you to infringe any 216 | patents or other property right claims or to contest validity of any 217 | such claims; this section has the sole purpose of protecting the 218 | integrity of the free software distribution system, which is 219 | implemented by public license practices. Many people have made 220 | generous contributions to the wide range of software distributed 221 | through that system in reliance on consistent application of that 222 | system; it is up to the author/donor to decide if he or she is willing 223 | to distribute software through any other system and a licensee cannot 224 | impose that choice. 225 | 226 | This section is intended to make thoroughly clear what is believed to 227 | be a consequence of the rest of this License. 228 | 229 | 8. If the distribution and/or use of the Program is restricted in 230 | certain countries either by patents or by copyrighted interfaces, the 231 | original copyright holder who places the Program under this License 232 | may add an explicit geographical distribution limitation excluding 233 | those countries, so that distribution is permitted only in or among 234 | countries not thus excluded. In such case, this License incorporates 235 | the limitation as if written in the body of this License. 236 | 237 | 9. The Free Software Foundation may publish revised and/or new versions 238 | of the General Public License from time to time. Such new versions will 239 | be similar in spirit to the present version, but may differ in detail to 240 | address new problems or concerns. 241 | 242 | Each version is given a distinguishing version number. If the Program 243 | specifies a version number of this License which applies to it and "any 244 | later version", you have the option of following the terms and conditions 245 | either of that version or of any later version published by the Free 246 | Software Foundation. If the Program does not specify a version number of 247 | this License, you may choose any version ever published by the Free Software 248 | Foundation. 249 | 250 | 10. If you wish to incorporate parts of the Program into other free 251 | programs whose distribution conditions are different, write to the author 252 | to ask for permission. For software which is copyrighted by the Free 253 | Software Foundation, write to the Free Software Foundation; we sometimes 254 | make exceptions for this. Our decision will be guided by the two goals 255 | of preserving the free status of all derivatives of our free software and 256 | of promoting the sharing and reuse of software generally. 257 | 258 | NO WARRANTY 259 | 260 | 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY 261 | FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN 262 | OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES 263 | PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED 264 | OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 265 | MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS 266 | TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE 267 | PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, 268 | REPAIR OR CORRECTION. 269 | 270 | 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 271 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR 272 | REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, 273 | INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING 274 | OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED 275 | TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY 276 | YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER 277 | PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE 278 | POSSIBILITY OF SUCH DAMAGES. 279 | 280 | END OF TERMS AND CONDITIONS 281 | 282 | How to Apply These Terms to Your New Programs 283 | 284 | If you develop a new program, and you want it to be of the greatest 285 | possible use to the public, the best way to achieve this is to make it 286 | free software which everyone can redistribute and change under these terms. 287 | 288 | To do so, attach the following notices to the program. It is safest 289 | to attach them to the start of each source file to most effectively 290 | convey the exclusion of warranty; and each file should have at least 291 | the "copyright" line and a pointer to where the full notice is found. 292 | 293 | {description} 294 | Copyright (C) {year} {fullname} 295 | 296 | This program is free software; you can redistribute it and/or modify 297 | it under the terms of the GNU General Public License as published by 298 | the Free Software Foundation; either version 2 of the License, or 299 | (at your option) any later version. 300 | 301 | This program is distributed in the hope that it will be useful, 302 | but WITHOUT ANY WARRANTY; without even the implied warranty of 303 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 304 | GNU General Public License for more details. 305 | 306 | You should have received a copy of the GNU General Public License along 307 | with this program; if not, write to the Free Software Foundation, Inc., 308 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 309 | 310 | Also add information on how to contact you by electronic and paper mail. 311 | 312 | If the program is interactive, make it output a short notice like this 313 | when it starts in an interactive mode: 314 | 315 | Gnomovision version 69, Copyright (C) year name of author 316 | Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 317 | This is free software, and you are welcome to redistribute it 318 | under certain conditions; type `show c' for details. 319 | 320 | The hypothetical commands `show w' and `show c' should show the appropriate 321 | parts of the General Public License. Of course, the commands you use may 322 | be called something other than `show w' and `show c'; they could even be 323 | mouse-clicks or menu items--whatever suits your program. 324 | 325 | You should also get your employer (if you work as a programmer) or your 326 | school, if any, to sign a "copyright disclaimer" for the program, if 327 | necessary. Here is a sample; alter the names: 328 | 329 | Yoyodyne, Inc., hereby disclaims all copyright interest in the program 330 | `Gnomovision' (which makes passes at compilers) written by James Hacker. 331 | 332 | {signature of Ty Coon}, 1 April 1989 333 | Ty Coon, President of Vice 334 | 335 | This General Public License does not permit incorporating your program into 336 | proprietary programs. If your program is a subroutine library, you may 337 | consider it more useful to permit linking proprietary applications with the 338 | library. If this is what you want to do, use the GNU Lesser General 339 | Public License instead of this License. 340 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # bag2matlab 2 | This repo has Matlab and Python code for reading data from a ROS bag file directly into a Matlab workspace. See the documentation for bagReader.m and bagInfo.m for information on how to use the code. This requires Matlab and ROS to run. Tested with ROS Kinetic Kame, Matlab R2015b, and R2016b. 3 | 4 | The bagReader() Matlab script reads all data from a bag for a given topic. Simply call bagReader.m with the bag name and the topic to read to get all of the data from that topic as a table. For example, bagReader('flight.bag', '/uav/pose') will read all of the data published on the '/uav/pose' topic that is stored within 'flight.bag'. 5 | 6 | bagInfo() returns the names and types of all the topics in a bag. For example: bagInfo('flight.bag') will return two cell arrays, one with the topic names, and the other with the corresponding types. 7 | 8 | For convenience, there is also a Matlab function for converting ROS images from sensor_msgs/Image messages to a representation Matlab can understand. This functionality is in rosImageToMatlab.m. Currently, only mono8 and bgr8 conversions are supported. 9 | -------------------------------------------------------------------------------- /bagInfo.m: -------------------------------------------------------------------------------- 1 | function [topic_names, topic_types] = bagInfo(bag_file, varargin) 2 | % BAGINFO Reads the topic names and types in a ROS bag file 3 | % Usage: 4 | % [topic_names, topic_types= BAGINFO(bag_file) returns the two 1 x n cell 5 | % arrays containing the topic names and types of messages in the bag 6 | % file. 7 | % BAGREADER(bag_file, topic_name, ...) Enables optional name/value pairs. 8 | % Possible options are as follows 9 | % 10 | % 'ros_root' -- A string specifying the location of the ROS distribution 11 | % so the function can find the Python packages it depends on. This is 12 | % useful if ROS is built from source and not installed in a standard 13 | % location. If this pair is not specified, the function will check if 14 | % the PYTHONPATH environment variable is set. If it is not, then it 15 | % will scan /opt/ros and look for distribution folders there. 16 | % 17 | % Example: [names, types] = bagInfo('flight.bag'); 18 | % Example: [names, types] = bagInfo('flight.bag', 'ros_root', '~/ros_catkin_ws/devel'); 19 | 20 | % Copyright (c) 2016 David Anthony 21 | % 22 | % This program is free software; you can redistribute it and/or modify 23 | % it under the terms of the GNU General Public License as published by 24 | % the Free Software Foundation; either version 2 of the License, or 25 | % (at your option) any later version. 26 | % 27 | % This program is distributed in the hope that it will be useful, 28 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 29 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 30 | % GNU General Public License for more details. 31 | % 32 | % You should have received a copy of the GNU General Public License 33 | % along with this program; if not, write to the Free Software 34 | % Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 35 | 36 | %% Input parsing and validation 37 | % Build an argument parser for reading the optional arguments 38 | input_parser = inputParser; 39 | % Set default values 40 | default_ros_root = ''; 41 | % Tell the parser how the possible inputs 42 | addRequired(input_parser, 'bag_file', @ischar); 43 | addParameter(input_parser, 'ros_root', default_ros_root, @ischar); 44 | 45 | % Parse the inputs 46 | parse(input_parser, bag_file, varargin{:}); 47 | 48 | % Check to make sure any directories the user passes in exist 49 | assert(isempty(input_parser.Results.ros_root) || ... 50 | (exist(input_parser.Results.ros_root, 'dir') == 7), ... 51 | 'Requested ROS root directory does not exist'); 52 | 53 | %% Python setup 54 | % We need to import the helper function, which also depends on the 55 | % underlying ROS Python modules working as well. Doing so depends on the 56 | % environment being correctly configured. If Matlab launches from the 57 | % terminal, it will pick up any ROS environment variables that were 58 | % present, and everything should just work. However, many shortcuts to 59 | % launching Matlab will not have the terminal environment, and thus Matlab 60 | % will not know where the ROS distribution modules are at. In that case, we 61 | % will attempt to find them for the user if the initial import fails. 62 | setupEnv(input_parser.Results.ros_root); 63 | 64 | %% Bag interaction 65 | % Call into the Python interface to get the names and types 66 | bag_data = py.matlab_bag_helper.extract_topic_names_types(bag_file); 67 | % Convert Python list types to Matlab cell arrays 68 | topic_names = py2Matlab(bag_data{1}); 69 | topic_types = py2Matlab(bag_data{2}); -------------------------------------------------------------------------------- /bagReader.m: -------------------------------------------------------------------------------- 1 | function [bag_data, topics_read] = bagReader(bag_file, varargin) 2 | % BAGREADER Reads messages from a ROS bag file 3 | % Usage: 4 | % [bag_data, topics_read] = BAGREADER(bag_file) returns a cell array of tables 5 | % of all the data in the bag 'bag_file'. Each cell item should be a 6 | % Matlab table, unless the data could not be converted to a table. In 7 | % that case, the data well be in a cell array. topics_read is a cell 8 | % array containing the names of the topics which correspond to the 9 | % entries in bag_data. 10 | % 11 | % bag_file is the path to the bag file to analyze 12 | % 13 | % [bag_data, topics_read] = BAGREADER(bag_file, topic_name) returns a 14 | % single table with only the data from 'topic_name' in it. 'topic_name' 15 | % is a string specifying a topic name. 16 | % 17 | % [bag_data, topics_read] = BAGREADER(bag_file, topic_names) returns 18 | % tables corresponding to all of the topics in the topic_names cell array 19 | % 20 | % BAGREADER(bag_file, topic_names, ...) Enables optional name/value pairs. 21 | % Possible options are as follows 22 | % 23 | % 'ros_root' -- A string specifying the location of the ROS distribution 24 | % so the function can find the Python packages it depends on. This is 25 | % useful if ROS is built from source and not installed in a standard 26 | % location. If this pair is not specified, the function will check if 27 | % the PYTHONPATH environment variable is set. If it is not, then it 28 | % will scan /opt/ros and look for distribution folders there. 29 | % 30 | % 'combine_times' -- A logical value which if true makes the function 31 | % search the message field names for a standard header. If one is 32 | % found, combine the second and nanosecond fields in the header to make 33 | % a single combined time and add that to the results. Default value is 34 | % true. 35 | % 36 | % 'min_idx' -- A positive integer or array that defines the 1-based indices to 37 | % start reading messages from. Defaults to 1. Setting a larger values skips 38 | % over messages at the beginning of a bag file. The number of elements 39 | % in this argument must match the number of topics read from the bag 40 | % file. Pass in a NAN for an element to read all messages for that 41 | % topic. 42 | % 43 | % 'max_idx' -- A positive integer that is greater than min_idx. The 44 | % reader stops reading the bag file at this message index. min_idx and 45 | % max_idx are inclusive values, so specifying both of them results in 46 | % reading messages with indices between [min_idx, max_idx]. The number 47 | % of elements must match the number of topics read. Pass in a nan for an 48 | % element to read all messages for that topic. 49 | % 50 | % Example: Read all messages in flight.bag 51 | % [data, names] = bagReader('flight.bag'); 52 | % Example: Read all messages published on /uav/pose in flight.bag 53 | % pose = bagReader('flight.bag', '/uav/pose'); 54 | % Example: Read all messages published on /uav/pose and /uav/gps in flight.bag 55 | % [data, names] = bagReader('flight.bag', {'/uav/pose', '/uav/gps'}); 56 | % Example: Read all messages from bag file if ROS is built in ~/ros_catkin-ws/devel 57 | % pose = bagReader('flight.bag', '/uav/pose', 'ros_root', '~/ros_catkin_ws/devel'); 58 | % Example: Do not combine the seconds and nanoseconds fields in header messages 59 | % pose = bagReader('flight.bag', '/uav/pose', 'combine_times', false); 60 | % Example: Read the second 100 pose messages from the bag file 61 | % pose = bagReader('flight.bag', '/uav/pose', 'min_idx', 100, 'max_idx', 200); 62 | % Example: Read the second 100 pose messages from the bag file, and the 63 | % first 100 GPS messages 64 | % [data, names] = bagReader('flight.bag', {'/uav/pose', '/uav/gps'}, 65 | % 'min_idx', [100, 1], 'max_idx', [200, 100]); 66 | 67 | % Copyright (c) 2016 David Anthony 68 | % 69 | % This program is free software; you can redistribute it and/or modify 70 | % it under the terms of the GNU General Public License as published by 71 | % the Free Software Foundation; either version 2 of the License, or 72 | % (at your option) any later version. 73 | % 74 | % This program is distributed in the hope that it will be useful, 75 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 76 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 77 | % GNU General Public License for more details. 78 | % 79 | % You should have received a copy of the GNU General Public License 80 | % along with this program; if not, write to the Free Software 81 | % Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 82 | 83 | %% Input parsing and validation 84 | % Build an argument parser for reading the optional arguments 85 | input_parser = inputParser; 86 | % Set default values 87 | default_ros_root = ''; % Do not guess where ROS is by default 88 | default_time_combine = true; % Combine header time fields by default 89 | default_min_msg_idx = []; % Do not bound the reading 90 | default_max_msg_idx = []; % Do not bound the reading range 91 | % Tell the parser about the possible inputs 92 | addRequired(input_parser, 'bag_file', @ischar); 93 | addOptional(input_parser, 'topic_names', {}, @(x)(ischar(x) || (iscell(x) && isvector(x)) || isempty(x))); 94 | addParameter(input_parser, 'ros_root', default_ros_root, @ischar); 95 | addParameter(input_parser, 'combine_times', default_time_combine, @islogical); 96 | addParameter(input_parser, 'min_idx', default_min_msg_idx, @(x)(isempty(x) || (isnumeric(x) && (isvector(x) || isscalar(x))))); 97 | addParameter(input_parser, 'max_idx', default_max_msg_idx, @(x)(isempty(x) || (isnumeric(x) && (isvector(x) || isscalar(x))))); 98 | 99 | % Parse the inputs 100 | parse(input_parser, bag_file, varargin{:}); 101 | topic_names = input_parser.Results.topic_names; 102 | if(~isempty(topic_names)) 103 | topics_read = topic_names; 104 | end 105 | 106 | % Check to make sure any directories the user passes in exist 107 | assert(isempty(input_parser.Results.ros_root) || ... 108 | (exist(input_parser.Results.ros_root, 'dir') == 7), ... 109 | 'Requested ROS root directory does not exist'); 110 | 111 | if(exist(input_parser.Results.bag_file, 'file') ~= 2) 112 | error('%s does not exist', input_parser.Results.bag_file); 113 | end 114 | 115 | % Check that we have the same number of min and max indices 116 | assert(numel(input_parser.Results.min_idx) == numel(input_parser.Results.max_idx)); 117 | 118 | % If no topics were specified then we read all topics from the bag file. If 119 | % multiple topics are specified, make sure they are all strings. If a 120 | % single topic was specified, convert it to a cell array to make the future 121 | % process the same for all options. 122 | if(isempty(topic_names)) 123 | topic_names = bagInfo(bag_file); 124 | topics_read = topic_names; 125 | elseif(iscell(topic_names)) 126 | for idx = 1:numel(topic_names) 127 | assert(ischar(topic_names{idx})); 128 | end 129 | else 130 | topic_names = {topic_names}; 131 | end 132 | 133 | % If minimum/maximum message indices are defined, they must be defined for 134 | % all the topics to read from 135 | assert(isempty(input_parser.Results.min_idx) || (numel(input_parser.Results.min_idx) == numel(topic_names))); 136 | 137 | % Now check to make sure all min and max index pairs are valid. There are 138 | % three conditions to check: 139 | % 1. Either both are nans, or neither is a nan. nans indicate the ranges 140 | % are not checked when parsing the bag file, so either we don't check the 141 | % ranges, or we have a well defined interval to read from 142 | % 2. If defined, the minimum index must be more than 0, because only 143 | % positive, one based indices are valid. 144 | % 3. If defined, the maximum index must be greater than or equal to the 145 | % minimum index so that we have a well defined interval to read from. 146 | 147 | for idx = 1:numel(input_parser.Results.min_idx) 148 | assert((isnan(input_parser.Results.min_idx(idx)) && isnan(input_parser.Results.max_idx(idx))) || ... 149 | (~isnan(input_parser.Results.min_idx(idx)) && ~isnan(input_parser.Results.max_idx(idx)))); 150 | if(~isnan(input_parser.Results.min_idx(idx))) 151 | assert(input_parser.Results.min_idx(idx) > 0, ... 152 | 'Minimum message index must be greater than 0'); 153 | assert(input_parser.Results.max_idx(idx) >= input_parser.Results.min_idx(idx), ... 154 | 'Maximum message index must be greater than or equal to the minimum index'); 155 | end 156 | end 157 | 158 | min_indices = input_parser.Results.min_idx; 159 | max_indices = input_parser.Results.max_idx; 160 | 161 | % If the min and max reading ranges are not defined by the user, set them 162 | % to 1 and the maximum 64 bit integer number so that all messages in the 163 | % bag file are read. This assumes there are less than 2^64 - 2 messages on 164 | % any topic type in the bag file because of how we handle the conversion to 165 | % a 0 based index 166 | if(isempty(min_indices)) 167 | min_indices = ones(numel(topic_names), 1); 168 | max_indices = ones(numel(topic_names), 1); 169 | max_indices(1:end) = intmax('int64'); 170 | else 171 | min_indices(isnan(min_indices)) = 1; 172 | max_indices(isnan(max_indices)) = intmax('int64'); 173 | end 174 | 175 | % One last sanity check about having bounds for every topic we read from 176 | assert(numel(topic_names) == numel(max_indices)); 177 | assert(numel(topic_names) == numel(min_indices)); 178 | 179 | % Initialize the output 180 | bag_data = cell(numel(topic_names), 1); 181 | for idx = 1:numel(topic_names) 182 | bag_data{idx} = table(); 183 | end 184 | 185 | %% Python setup 186 | % We need to import the helper function, which also depends on the 187 | % underlying ROS Python modules working as well. Doing so depends on the 188 | % environment being correctly configured. If Matlab launches from the 189 | % terminal, it will pick up any ROS environment variables that were 190 | % present, and everything should just work. However, many shortcuts to 191 | % launching Matlab will not have the terminal environment, and thus Matlab 192 | % will not know where the ROS distribution modules are at. In that case, we 193 | % will attempt to find them for the user if the initial import fails. 194 | setupEnv(input_parser.Results.ros_root); 195 | 196 | %% Bag reading 197 | % Read the data in the bag file. Convert the min/max indices to 0-indexed 198 | % representation 199 | for idx = 1:numel(topic_names) 200 | fprintf('Processing topic: %s\n', topic_names{idx}); 201 | bag_data{idx} = py.matlab_bag_helper.read_bag(... 202 | bag_file, ... 203 | topic_names{idx}, ... 204 | min_indices(idx) - 1, ... 205 | max_indices(idx) - 1); 206 | % Convert the Python data to an array of structures 207 | bag_data{idx} = py2Matlab(bag_data{idx}); 208 | % Check to see if we found any messages containing data 209 | if(~isempty(bag_data{idx})) 210 | % Occasionally this conversion fails because the ROS messages may have 211 | % field names that are Matlab keywords, and Matlab does not like having 212 | % structure fields that are keyword names. Wrap this in a try/catch 213 | % block to perform the conversion if possible. Otherwise, just keep the 214 | % data as a cell array. 215 | try 216 | % Now make the Matlab structures a table 217 | bag_data{idx} = struct2table(cell2mat(bag_data{idx})); 218 | % Flatten the table 219 | bag_data{idx} = flattenTable(bag_data{idx}); 220 | % Now produce the combined time field if requested 221 | if(input_parser.Results.combine_times) 222 | bag_data{idx} = combineTimes(bag_data{idx}); 223 | end 224 | 225 | % Store the topic name in the user data of the table 226 | bag_data{idx}.Properties.UserData = topic_names{idx}; 227 | catch ME 228 | warning('Could not convert data on topic %s to table', topic_names{idx}); 229 | end 230 | % Did not find any messages. Return an empty table 231 | else 232 | warning('Did not find any messages on topic %s in the bag file', topic_names{idx}); 233 | bag_data{idx} = table(); 234 | end 235 | end 236 | 237 | % Return a table instead of a one element cell array if only one topic was 238 | % processed to make the output a little nicer 239 | if(numel(bag_data) == 1) 240 | bag_data = bag_data{1}; 241 | end 242 | end -------------------------------------------------------------------------------- /combineTimes.m: -------------------------------------------------------------------------------- 1 | function [modified_table] = combineTimes(original_table) 2 | % COMBINETIMES Condenses results from standard ROS header into single time 3 | % Usage: 4 | % modified = COMBINETIMES(data) Scans data for fields matching standard 5 | % header values. If they are found, append a column to the table which 6 | % is the combined seconds and nanoseconds field 7 | 8 | % Copyright (c) 2016 David Anthony 9 | % 10 | % This program is free software; you can redistribute it and/or modify 11 | % it under the terms of the GNU General Public License as published by 12 | % the Free Software Foundation; either version 2 of the License, or 13 | % (at your option) any later version. 14 | % 15 | % This program is distributed in the hope that it will be useful, 16 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | % GNU General Public License for more details. 19 | % 20 | % You should have received a copy of the GNU General Public License 21 | % along with this program; if not, write to the Free Software 22 | % Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 23 | 24 | % Copy the original data we will add to 25 | modified_table = original_table; 26 | 27 | % Field names matching the time fields in a standard ROS header 28 | NSECS_FIELD_NAME = 'header_stamp_nsecs'; 29 | SECS_FIELD_NAME = 'header_stamp_secs'; 30 | 31 | % Find any columns that match the nanosecond field 32 | nsec_fields = strncmp(... 33 | NSECS_FIELD_NAME, ... 34 | modified_table.Properties.VariableNames, ... 35 | length(NSECS_FIELD_NAME)); 36 | 37 | % Find any columns that match the seconds field 38 | sec_fields = strncmp(... 39 | SECS_FIELD_NAME, ... 40 | modified_table.Properties.VariableNames, ... 41 | length(SECS_FIELD_NAME)); 42 | 43 | % Either we do not have a header, and the matches fail, or we should find 44 | % exactly one column matching both our time fields. 45 | assert((sum(nsec_fields) == 0) || (sum(nsec_fields) == 1)); 46 | assert((sum(sec_fields) == 0) || ... 47 | ((sum(sec_fields) == 1) && (sum(nsec_fields) == 1))); 48 | 49 | % If we found the time fields in a standard header, combine them into one 50 | % column 51 | if(any(nsec_fields) && any(sec_fields)) 52 | % Scale nanoseconds to seconds 53 | nsecs = double(table2array(modified_table(:, nsec_fields))) / 1e9; 54 | secs = double(table2array(modified_table(:, sec_fields))); 55 | % Store the output as seconds 56 | modified_table.header_times = secs + nsecs; 57 | end 58 | 59 | end -------------------------------------------------------------------------------- /flattenTable.m: -------------------------------------------------------------------------------- 1 | function [flat_table] = flattenTable(data) 2 | % FLATTENTABLE Converts table with structs as columns to a flat 3 | % representation where all struct fields are columns in the table 4 | % 5 | % Usage: 6 | % flat_data = flattenTable(original_data) flattens the original data and 7 | % returns the resulting table. The flattening operation recursively 8 | % converts all columns of structs into tables, where each table column 9 | % is a member of the struct. 10 | 11 | % Copyright (c) 2016 David Anthony 12 | % 13 | % This program is free software; you can redistribute it and/or modify 14 | % it under the terms of the GNU General Public License as published by 15 | % the Free Software Foundation; either version 2 of the License, or 16 | % (at your option) any later version. 17 | % 18 | % This program is distributed in the hope that it will be useful, 19 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | % GNU General Public License for more details. 22 | % 23 | % You should have received a copy of the GNU General Public License 24 | % along with this program; if not, write to the Free Software 25 | % Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 26 | 27 | % Determine which columns are structs 28 | col_types = varfun(@isstruct, data, 'output', 'uniform'); 29 | % Initialize the output with all of the original columns that are not 30 | % structs, because we will not mbe modifying them. 31 | flat_table = data(:, ~col_types); 32 | 33 | % Iterate over the input table and convert all columns with structs into a 34 | % table representation 35 | for col_idx = 1:size(data, 2) 36 | % Check if the column contains structs 37 | if(col_types(col_idx) == 1) 38 | % Converting a column of structs is a three part process. First convert 39 | % the column into an array of structs, then convert that array back to 40 | % a table. This 'unnests' the struct members as columns of table 41 | col = table2array(data(:, col_idx)); 42 | col = struct2table(col); 43 | % Now we have a table where whose columns are members of the struct in 44 | % the original table. Recursively flatten these columns, in case we 45 | % have structs of structs. 46 | col = flattenTable(col); 47 | 48 | % Rename the column names with a prefix indicating the original column 49 | % name. This should guarantee all the column names are unique 50 | for new_idx = 1:numel(col.Properties.VariableNames) 51 | col.Properties.VariableNames{new_idx} = strcat(data.Properties.VariableNames{col_idx}, '_', col.Properties.VariableNames{new_idx}); 52 | end 53 | 54 | % Concatentate the flattened table to our output 55 | flat_table = [flat_table, col]; 56 | end 57 | end 58 | 59 | end -------------------------------------------------------------------------------- /matlab_bag_helper.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2016 David Anthony 4 | # 5 | # This program is free software; you can redistribute it and/or modify 6 | # it under the terms of the GNU General Public License as published by 7 | # the Free Software Foundation; either version 2 of the License, or 8 | # (at your option) any later version. 9 | # 10 | # This program is distributed in the hope that it will be useful, 11 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | # GNU General Public License for more details. 14 | # 15 | # You should have received a copy of the GNU General Public License 16 | # along with this program; if not, write to the Free Software 17 | # Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | 19 | from __future__ import division 20 | import os 21 | import rosbag 22 | import unittest 23 | 24 | 25 | class TestBagReader(unittest.TestCase): 26 | """ Put test cases here. These rely on paths to bag files on my local hard drive, so I have not committed them """ 27 | pass 28 | 29 | 30 | def read_bag(bag_file, topic_name, min_idx, max_idx): 31 | """ Reads all messages in a topic from a bag file. 32 | 33 | Opens up a bag file and reads all the messages in that bag file that match a topic name. Minimal error handling, 34 | so if the bag does not exist or the topic is not in the bag there will be strange behavior. 35 | 36 | Args: 37 | bag_file: A path to the bag file 38 | topic_name: A string containing the topic name to read out 39 | min_idx: An integer containing the first message index to return 40 | max_idx: An integer containing the last message index to return 41 | 42 | Returns: 43 | A list of dictionaries. Each dictionary is a single message in the bag. The list is each message as it was 44 | encountered in the bag file. 45 | """ 46 | 47 | # Check that the index bounds are reasonable 48 | assert min_idx >= 0 49 | assert max_idx >= min_idx 50 | 51 | # Open the bag file 52 | file_data = rosbag.Bag(os.path.abspath(os.path.expanduser(bag_file))) 53 | 54 | # Initialize the output 55 | extracted_data = [] 56 | 57 | # Initialize the counter for keeping track of whether or not we are within the minimum/maximum index 58 | msg_idx = 0 59 | 60 | # Iterate over every message in the bag that matches our topic name 61 | for _, msg, t in file_data.read_messages(topics=topic_name): 62 | # Check if we are past the start of the location in the bag file to read from 63 | if msg_idx >= min_idx: 64 | # Add the extracted data to our output list 65 | data = extract_topic_data(msg, t) 66 | # Convert the time the message the message was recorded in the bag file to a single scalar value and add it 67 | # to the new data 68 | data['rosbag_recv_time'] = t.secs + (t.nsecs / 1e9) 69 | # Add the new data to the set we return to the caller 70 | extracted_data.append(data) 71 | # Increment the index and bail from the loop early if we have advanced past the last message of interest. This 72 | # early exit can yield significant performance gains 73 | msg_idx += 1 74 | if msg_idx > max_idx: 75 | break 76 | 77 | # Clean up after ourselves 78 | file_data.close() 79 | 80 | # Return the extracted messages 81 | return extracted_data 82 | 83 | 84 | def extract_topic_names_types(bag_file): 85 | """ Gets the topic names and types of messages in a bag file. 86 | 87 | Opens up the bag file and reads the topic names and types of the messages in that bag file. Uses code from the 88 | ROS bag cookbook. 89 | 90 | Args: 91 | bag_file: path to the bag file 92 | 93 | Returns: 94 | A tuple of lists. The first list is the topic names, and the second list is the topic types. 95 | """ 96 | bag = rosbag.Bag(os.path.abspath(os.path.expanduser(bag_file))) 97 | topics = bag.get_type_and_topic_info()[1].keys() 98 | types = [] 99 | for i in range(0, len(bag.get_type_and_topic_info()[1].values())): 100 | types.append(bag.get_type_and_topic_info()[1].values()[i][0]) 101 | return topics, types 102 | 103 | 104 | def extract_topic_data(msg, t): 105 | """ Reads all data in a message 106 | 107 | This is a recursive function. Given a message, extract all of the data in the message to a dictionary. The keys 108 | of the dictionary are the field names within the message, and the values are the values of the fields in the 109 | message. Recursively call this function on a message to build up a dictionary of dictionaries representing the 110 | ROS message. 111 | 112 | Args: 113 | msg: A ROS message 114 | t: Time the message was recorded in the bag file 115 | 116 | Returns: 117 | A dictionary containing all information found in the message. 118 | """ 119 | # Initialize the information found for this message 120 | data = {} 121 | # If the message has slots, we have a non-primitive type, and need to extract all of the information from this 122 | # message by recursively calling this function on the data in that slot. For example, we may have a message with a 123 | # geometry_msgs/Vector3 as a field. Call this function on that field to get the x, y, and z components 124 | if hasattr(msg, '__slots__'): 125 | # Extract all information on a non-primitive type 126 | for slot in msg.__slots__: 127 | data[slot] = extract_topic_data(getattr(msg, slot), t) 128 | else: 129 | # We encountered a primitive type, like a double. Just return it so it gets put into the output dictionary 130 | return msg 131 | 132 | # Return the dictionary representing all of the fields and their information in this message 133 | return data 134 | 135 | 136 | if __name__ == "__main__": 137 | unittest.main() 138 | -------------------------------------------------------------------------------- /py2Matlab.m: -------------------------------------------------------------------------------- 1 | function [converted_data] = py2Matlab(original_data) 2 | % py2Matlab Convert Python objects to their Matlab equivalent 3 | % Usage: py2Matlab(python_data) Converts data from a 4 | % Python object to native Matlab representations. 5 | 6 | % Copyright (c) 2016 David Anthony 7 | % 8 | % This program is free software; you can redistribute it and/or modify 9 | % it under the terms of the GNU General Public License as published by 10 | % the Free Software Foundation; either version 2 of the License, or 11 | % (at your option) any later version. 12 | % 13 | % This program is distributed in the hope that it will be useful, 14 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | % GNU General Public License for more details. 17 | % 18 | % You should have received a copy of the GNU General Public License 19 | % along with this program; if not, write to the Free Software 20 | % Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | 22 | % Call the appropriate conversion routine for the given Python or Matlab 23 | % data type 24 | switch(class(original_data)) 25 | case 'py.list' 26 | % Recursively call this function on all data in a list to convert 27 | % all Python objects in the list to Matlab types 28 | converted_data = cell(original_data); 29 | 30 | % If we were passed an empty list just return an empty array. 31 | % The conversion function below does not work on empty data and will 32 | % crash if it operates on an empty cell array. 33 | if(~isempty(converted_data)) 34 | % We must call structfun on objects with non-scalar data types with 35 | % the UniformOutput argument set to false, or they don't convert 36 | % correctly. 37 | uniform_output = false; 38 | if(strcmp(class(converted_data{1}), 'py.int') || ... 39 | strcmp(class(converted_data{1}), 'py.long') || ... 40 | strcmp(class(converted_data{1}), 'py.array.array')) 41 | uniform_output = true; 42 | end 43 | converted_data = cellfun(@py2Matlab, converted_data, 'UniformOutput', uniform_output); 44 | else 45 | converted_data = []; 46 | end 47 | 48 | case 'py.dict' 49 | % Dictionaries can have Python data types in them, so recursively 50 | % convert them from Python types to Matlab types 51 | converted_data = struct(original_data); 52 | converted_data = structfun(@py2Matlab, converted_data, 'UniformOutput', false); 53 | 54 | case 'py.str' 55 | % Matlab has a bug where converting Python strings directly to Matlab 56 | % char arrays fails for ASCII values greater than 191. Casting to 57 | % uint8 first is a workaround to this problem. 58 | converted_data = char(uint8(original_data)); 59 | % This assertion is a sanity check to make sure the workaround above 60 | % is still working and all of the data is converted. 61 | assert(numel(converted_data) == py.len(original_data)); 62 | 63 | case 'py.tuple' 64 | converted_data = cell(original_data); 65 | 66 | case 'py.bytes' 67 | converted_data = uint8(original_data); 68 | 69 | case 'py.unicode' 70 | converted_data = char(original_data); 71 | 72 | case 'py.int' 73 | converted_data = int64(original_data); 74 | 75 | case 'py.long' 76 | converted_data = double(original_data); 77 | 78 | case 'py.array.array' 79 | converted_data = double(original_data); 80 | 81 | case 'double' 82 | converted_data = double(original_data); 83 | 84 | case 'single' 85 | converted_data = single(original_data); 86 | 87 | case 'logical' 88 | converted_data = logical(original_data); 89 | 90 | case 'char' 91 | converted_data = char(original_data); 92 | 93 | case 'int8' 94 | converted_data = int8(original_data); 95 | 96 | case 'uint8' 97 | converted_data = uint8(original_data); 98 | 99 | case 'int16' 100 | converted_data = int16(original_data); 101 | 102 | case 'uint16' 103 | converted_data = uint16(original_data); 104 | 105 | case 'int32' 106 | converted_data = int32(original_data); 107 | 108 | case 'uint32' 109 | converted_data = uint32(original_data); 110 | 111 | case 'int64' 112 | converted_data = int64(original_data); 113 | 114 | case 'uint64' 115 | converted_data = uint64(original_data); 116 | 117 | otherwise 118 | % Encountered some data that we do not have an explicit handler for. 119 | % This may not be a problem, and indicates we hit some kind of 120 | % fundamental ROS data type. Trying converting each one of the object 121 | % members to a Matlab representation 122 | try 123 | % Iterate over every member of the object and try to convert it to 124 | % a Matlab data type. 125 | p = properties(original_data); 126 | converted_data = struct(); 127 | 128 | % Initialize the struct field names 129 | for idx = 1:numel(p) 130 | converted_data.(p{idx}) = []; 131 | end 132 | 133 | % Use recursion to assign values to the fields 134 | for idx = 1:numel(p) 135 | converted_data.(p{idx}) = py2Matlab(original_data.(p{idx})); 136 | end 137 | catch 138 | error('Could not convert data of type: %s', class(original_data)); 139 | end 140 | end 141 | end -------------------------------------------------------------------------------- /rosImageToMatlab.m: -------------------------------------------------------------------------------- 1 | function [matlab_image] = rosImageToMatlab(ros_image, encoding, height, width) 2 | % rosImageToMatlab Convert ROS image representation to Matlab format 3 | % Usage: rosImageToMatlab(ros_image, encoding, height, width) Converts the 4 | % data in ros_image to a format usable in Matlab. ros_image is a 5 | % 1D vector, such as that defined in sensor_msgs/Image. encoding 6 | % is the ROS encoding string that is in sensor_msgs/Image. height 7 | % and width are the height and width of the image as defined in a 8 | % ROS image message. 9 | % 10 | % Please note that not all image encodings are currently supported. More 11 | % will be added as needed. 12 | 13 | % Copyright (c) 2016 David Anthony 14 | % 15 | % This program is free software; you can redistribute it and/or modify 16 | % it under the terms of the GNU General Public License as published by 17 | % the Free Software Foundation; either version 2 of the License, or 18 | % (at your option) any later version. 19 | % 20 | % This program is distributed in the hope that it will be useful, 21 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 22 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 23 | % GNU General Public License for more details. 24 | % 25 | % You should have received a copy of the GNU General Public License 26 | % along with this program; if not, write to the Free Software 27 | % Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 28 | 29 | switch encoding 30 | case 'mono8' 31 | convertMono8(); 32 | case 'bgr8' 33 | convertBGR8(); 34 | otherwise 35 | error('Unsupported encoding'); 36 | end 37 | 38 | function [] = convertMono8() 39 | matlab_image = reshape(uint8(ros_image), width, height)'; 40 | end 41 | 42 | function [] = convertBGR8() 43 | % Converts ROS BGR8 image representation to a Matlab representation 44 | 45 | % Preallocate output 46 | matlab_image = zeros(height, width, 3); 47 | read_idx = 1; 48 | 49 | % BGR8 ends up being a height x width x 3 array. The ROS image is 50 | % repeated tuples of blue, green, and red. Read 3 bytes at a time and 51 | % place them into the Matlab format. 52 | for row_idx = 1:height 53 | for col_idx = 1:width 54 | matlab_image(row_idx, col_idx, 1) = ros_image(read_idx + 2); 55 | matlab_image(row_idx, col_idx, 2) = ros_image(read_idx + 1); 56 | matlab_image(row_idx, col_idx, 3) = ros_image(read_idx); 57 | read_idx = read_idx + 3; 58 | end 59 | end 60 | 61 | matlab_image = uint8(matlab_image); 62 | end 63 | end 64 | -------------------------------------------------------------------------------- /setupEnv.m: -------------------------------------------------------------------------------- 1 | function [] = setupEnv(ros_root) 2 | % SETUPENV Set up Python environment for accessing ROS Python libraries 3 | % from Matlab. If the ROS_DISTRO environment variable exists, assume that 4 | % the environment is already correctly configured and do not modify the 5 | % Python path. Load the helper module once the environment is configured. 6 | % 7 | % Usage: 8 | % SETUPENV('') Search /opt/ros for ROS installations and add the Python 9 | % modules in those directories. 10 | % 11 | % SETUPENV(path) Search the specified path for Python modules 12 | 13 | % Copyright (c) 2016 David Anthony 14 | % 15 | % This program is free software; you can redistribute it and/or modify 16 | % it under the terms of the GNU General Public License as published by 17 | % the Free Software Foundation; either version 2 of the License, or 18 | % (at your option) any later version. 19 | % 20 | % This program is distributed in the hope that it will be useful, 21 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 22 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 23 | % GNU General Public License for more details. 24 | % 25 | % You should have received a copy of the GNU General Public License 26 | % along with this program; if not, write to the Free Software 27 | % Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 28 | 29 | % Keep track of whether or not we have managed to load the helper module 30 | persistent IMPORT_SUCCESS; 31 | 32 | % Indicate that we have not loaded the helper module if this is the first time 33 | % we have called this function 34 | if(isempty(IMPORT_SUCCESS)) 35 | IMPORT_SUCCESS = false; 36 | end 37 | 38 | % IF we have already import our Python module and the user is not 39 | % overriding the root 40 | if(IMPORT_SUCCESS && isempty(ros_root)) 41 | return; 42 | end 43 | 44 | % Check to see if the user has specified where to search for the Python 45 | % packages. We will skip all our autodetection and use their path if this 46 | % is the case 47 | if(~isempty(ros_root)) 48 | override_root = true; 49 | else 50 | override_root = false; 51 | end 52 | 53 | % If the user has not specified the ROS Python modules are located, we will 54 | % use the following code to try to find the packages if we have not managed 55 | % to previously find them. If we have already loaded the module, do not 56 | % repeat this step 57 | if(~override_root && ~IMPORT_SUCCESS) 58 | % If ROS_DISTRO is already an environment variable we assume that the 59 | % rest of the system is already configured and do not do anything 60 | if(isempty(getenv('ROS_DISTRO'))) 61 | % Look in the default ROS installation location for ROS distributions 62 | % and use the latest as the ROS source directory 63 | dir_names = dir('/opt/ros'); 64 | dir_names = dir_names([dir_names.isdir]); 65 | ros_root = ''; 66 | python_version = 2; 67 | 68 | % Iterate through the found directories and add the latest ROS 69 | % distribution as the ROS root location. By placing the switch clauses 70 | % in alphabetical order we should grab the latest distribution. 71 | for dir_idx = 1:numel(dir_names) 72 | switch(dir_names(dir_idx).name) 73 | case 'indigo' 74 | ros_root = '/opt/ros/indigo'; 75 | case 'jade' 76 | ros_root = '/opt/ros/jade'; 77 | case 'kinetic' 78 | ros_root = '/opt/ros/kinetic'; 79 | case 'noetic' 80 | ros_root = '/opt/ros/noetic'; 81 | python_version = 3; 82 | otherwise 83 | end 84 | end 85 | 86 | % We did not find a known ROS distribution. Just try using the last 87 | % directory in /opt/ros and warn the user things may be bad. 88 | if(isempty(ros_root)) 89 | ros_root = fullfile('/opt/ros', dir_names(end).name); 90 | warning('Could not find a known ROS distribution, assuming your ROS distribution is rooted in %s', ros_root); 91 | end 92 | 93 | % Assume that all of ROS's Python modules are nested in the following 94 | % subdirectories, depending on the OS 95 | os = computer; 96 | if(strcmpi(os, 'MACI64')) 97 | if(python_version == 2) 98 | ros_root = fullfile(ros_root, 'lib', 'python2.7', 'site-packages'); 99 | else 100 | ros_root = fullfile(ros_root, 'lib', 'python3', 'site-packages'); 101 | end 102 | else 103 | if(python_version == 2) 104 | ros_root = fullfile(ros_root, 'lib', 'python2.7', 'dist-packages'); 105 | else 106 | ros_root = fullfile(ros_root, 'lib', 'python3', 'dist-packages'); 107 | end 108 | end 109 | end 110 | end 111 | 112 | % Now modify our path to include to ROS modules if the environment variable was 113 | % not preconfigured from the shell and we have not already loaded the module, 114 | % or if the user has set a new ROS location 115 | if((isempty(getenv('ROS_DISTRO')) && ~IMPORT_SUCCESS) || override_root) 116 | % Check if the target directory exists 117 | if(exist(ros_root, 'dir') == 7) 118 | modifyPath(ros_root); 119 | else 120 | error('Could not find location of ROS Python packages'); 121 | IMPORT_SUCCESS = false; 122 | end 123 | end 124 | 125 | if(~IMPORT_SUCCESS || override_root) 126 | % Need to be in the directory where the helper Python script is located 127 | % to import it. Save our current directory, switch to that location, and 128 | % then restore the original directory at the end 129 | [function_path, ~, ~] = fileparts(which('setupEnv')); 130 | initial_path = pwd; 131 | cd(function_path); 132 | try 133 | py.importlib.import_module('matlab_bag_helper'); 134 | catch 135 | cd(initial_path); 136 | error('Could not import python helper function'); 137 | end 138 | 139 | % Set flag indicating we have imported our helper function, and return to the 140 | % calling directory 141 | IMPORT_SUCCESS = true; 142 | cd(initial_path); 143 | end 144 | 145 | function [] = modifyPath(module_location) 146 | % Helper function for adding the ROS Python modules to the path 147 | % Get the current modules Matlab knows about 148 | P = py.sys.path; 149 | % See if where we think the ROS modules are is already on the path 150 | if(~strncmp(... 151 | module_location, ... 152 | cellfun(@char, cell(P), 'UniformOutput', false), ... 153 | length(module_location))) 154 | % Modify the Python path with the location of the ROS modules 155 | insert(P, int32(0), module_location); 156 | end 157 | end 158 | end 159 | --------------------------------------------------------------------------------