├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── ReadMe.md ├── config ├── bag2mat_dict.yaml ├── bag2matpy_config.yaml ├── bag2matpy_google_tango_online_data.yaml ├── bag2matpy_seabed_config.yaml ├── bag2matpy_simplenav_uas.yaml ├── bag2matpy_vio.yaml └── bag2matpy_xsens_online_data.yaml ├── package.xml └── scripts └── bag2matpy /.gitignore: -------------------------------------------------------------------------------- 1 | ToDo.md -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | 3 | # Project name 4 | project(bag2mat) 5 | 6 | # Include our cmake files 7 | SET(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake/) 8 | 9 | # Find catkin (the ROS build system) 10 | find_package(catkin REQUIRED COMPONENTS rosbag roslib) 11 | 12 | # Describe catkin Project 13 | catkin_package( 14 | CATKIN_DEPENDS 15 | # INCLUDE_DIRS src 16 | ) 17 | 18 | # Try to compile with c++11 19 | # http://stackoverflow.com/a/25836953 20 | include(CheckCXXCompilerFlag) 21 | CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) 22 | CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) 23 | if(COMPILER_SUPPORTS_CXX11) 24 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 25 | elseif(COMPILER_SUPPORTS_CXX0X) 26 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 27 | else() 28 | message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") 29 | endif() 30 | 31 | # Enable compile optimizations 32 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops") 33 | 34 | # Enable debug flags (use if you want to debug in gdb) 35 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g3 -Wall") 36 | 37 | # Include our header files 38 | #include_directories( 39 | # src 40 | # ${catkin_INCLUDE_DIRS} 41 | #) 42 | 43 | # Set link libraries used by all binaries 44 | list(APPEND parser_libraries 45 | ${catkin_LIBRARIES} 46 | ) 47 | 48 | 49 | ################################################## 50 | # Make binary for the offline reader 51 | ################################################## 52 | #add_executable(bag2mat 53 | # src/bag2mat.cpp 54 | #) 55 | #target_link_libraries(bag2mat ${parser_libraries}) 56 | 57 | ############# 58 | ## Install ## 59 | ############# 60 | 61 | # all install targets should use catkin DESTINATION variables 62 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 63 | 64 | ## Mark executable scripts (Python etc.) for installation 65 | ## in contrast to setup.py, you can choose the destination 66 | # install(PROGRAMS 67 | # scripts/my_python_script 68 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 69 | # ) 70 | catkin_install_python(PROGRAMS 71 | scripts/bag2matpy 72 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 73 | ) 74 | 75 | install( 76 | DIRECTORY launch 77 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 78 | ) 79 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2018 neufieldrobotics 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 | -------------------------------------------------------------------------------- /ReadMe.md: -------------------------------------------------------------------------------- 1 | ### This pkg is now part of [rosbag_toolkit](https://github.com/neufieldrobotics/rosbag_toolkit) and is moved to rosbag_toolkit/bag2mat, Checkout new version at https://github.com/neufieldrobotics/rosbag_toolkit/tree/master/bag2mat 2 | 3 | # bag2mat 4 | 5 | * Takes a rosbag file and converts it to a matlab mat file based on settings specified in a Dictionary and a yaml config file. 6 | - Dictonary: The dicionary file specified which variables from each message type should be moved to the matlab array. 7 | eg `sensor_msgs/NavSatFix: t.to_sec(), m.latitude, m.longitude, m.altitude` 8 | See config/bag2mat_dic.yaml for full example. 9 | - Configuration: This file specified which topic names should be transferred to matlab and under which variable name. 10 | A list of `-[topic name, message type, name for variable in matfile]` 11 | See config/bag2mat_config.yaml for full example. 12 | * Currently needs the following packages: 13 | - rosbag 14 | - roslib 15 | - scipy 16 | 17 | * Put this repo in the src folder of ros workspace and run 18 | ``` 19 | catkin_make 20 | source devel/setup.bash 21 | usage: bag2matpy [-h] [-c CONFIG_FILE] [-d DICTIONARY] [-i input_bagfile] 22 | 23 | ``` 24 | ``` 25 | Complete List of Arguments: 26 | 27 | -h, --help show this help message and exit 28 | -i INPUT_BAGFILE, --input_bagfile INPUT_BAGFILE 29 | Input rosbag file to input 30 | -o OUTPUT_DIR, --output_dir OUTPUT_DIR 31 | Output dir for matfile 32 | -c CONFIG_FILE, --config_file CONFIG_FILE 33 | Yaml file which specifies topic names to convert 34 | -d DICTIONARY, --dictionary DICTIONARY 35 | Dictionary file which specifies how to read the topic 36 | -s, --subtract_start_time 37 | Boolean flag to specify whether to include offset_time obtained by subtracting bag start_time from all timestamps 38 | ``` 39 | -------------------------------------------------------------------------------- /config/bag2mat_dict.yaml: -------------------------------------------------------------------------------- 1 | # message_type: 2 | # matlab_struct_field_name 1: expression1 3 | # matlab_struct_field_name 2: expression2 4 | 5 | geometry_msgs/TwistStamped: 6 | msg_time: t.to_sec() 7 | header_timestamp: m.header.stamp.to_sec() 8 | velocity_x: m.twist.linear.x 9 | velocity_y: m.twist.linear.y 10 | velocity_z: m.twist.linear.z 11 | angular_velocity_x: m.twist.angular.x 12 | angular_velocity_y: m.twist.angular.y 13 | angular_velocity_z: m.twist.angular.z 14 | 15 | geometry_msgs/PoseStamped: 16 | msg_time: t.to_sec() 17 | header_timestamp: m.header.stamp.to_sec() 18 | position_x: m.pose.position.x 19 | position_y: m.pose.position.y 20 | position_z: m.pose.position.z 21 | orientation_quat_x: m.pose.orientation.x 22 | orientation_quat_y: m.pose.orientation.y 23 | orientation_quat_z: m.pose.orientation.z 24 | orientation_quat_w: m.pose.orientation.w 25 | 26 | geometry_msgs/PoseWithCovarianceStamped: 27 | msg_time: t.to_sec() 28 | header_timestamp: m.header.stamp.to_sec() 29 | position_x: m.pose.pose.position.x 30 | position_y: m.pose.pose.position.y 31 | position_z: m.pose.pose.position.z 32 | orientation_quat_x: m.pose.pose.orientation.x 33 | orientation_quat_y: m.pose.pose.orientation.y 34 | orientation_quat_z: m.pose.pose.orientation.z 35 | orientation_quat_w: m.pose.pose.orientation.w 36 | #covariance: m.pose.covariance 37 | 38 | geometry_msgs/Vector3Stamped: 39 | msg_time: t.to_sec() 40 | vector_x: m.vector.x 41 | vector_y: m.vector.y 42 | vector_z: m.vector.z 43 | 44 | nav_msgs/Odometry: 45 | msg_time: t.to_sec() 46 | position_x: m.pose.pose.position.x 47 | position_y: m.pose.pose.position.y 48 | position_z: m.pose.pose.position.z 49 | velocity_x: m.twist.twist.linear.x 50 | velocity_y: m.twist.twist.linear.y 51 | velocity_z: m.twist.twist.linear.z 52 | orientation_quat_x: m.pose.pose.orientation.x 53 | orientation_quat_y: m.pose.pose.orientation.y 54 | orientation_quat_z: m.pose.pose.orientation.z 55 | orientation_quat_w: m.pose.pose.orientation.w 56 | angular_velocity_x: m.twist.twist.angular.x 57 | angular_velocity_y: m.twist.twist.angular.y 58 | angular_velocity_z: m.twist.twist.angular.z 59 | 60 | sensor_msgs/BatteryState: 61 | msg_time: t.to_sec() 62 | voltage: m.voltage 63 | current: m.current 64 | charge: m.charge 65 | capacity: m.capacity 66 | design_capacity: m.design_capacity 67 | percentage: m.percentage 68 | 69 | sensor_msgs/Imu: 70 | msg_time: t.to_sec() 71 | header_timestamp: m.header.stamp.to_sec() 72 | orientation_quat_x: m.orientation.x 73 | orientation_quat_y: m.orientation.y 74 | orientation_quat_z: m.orientation.z 75 | orientation_quat_w: m.orientation.w 76 | acceleration_x: m.linear_acceleration.x 77 | acceleration_y: m.linear_acceleration.y 78 | acceleration_z: m.linear_acceleration.z 79 | angular_velocity_x: m.angular_velocity.x 80 | angular_velocity_y: m.angular_velocity.y 81 | angular_velocity_z: m.angular_velocity.z 82 | 83 | sensor_msgs/JointState: 84 | msg_time: t.to_sec() 85 | 86 | sensor_msgs/Joy_UAS: 87 | msg_time: t.to_sec() 88 | cmd_velocity_x: m.axes[0] 89 | cmd_velocity_y: m.axes[1] 90 | cmd_velocity_z: m.axes[2] 91 | cmd_yaw_rate: m.axes[3] 92 | config_code: m.axes[4] 93 | 94 | sensor_msgs/Joy_DJI_RC: 95 | msg_time: t.to_sec() 96 | cmd_roll: m.axes[0] 97 | cmd_pitch: m.axes[1] 98 | cmd_yaw_rate: m.axes[2] 99 | cmd_thrust: m.axes[3] 100 | cmd_mode: m.axes[4] 101 | cmd_landing_gear: m.axes[5] 102 | 103 | sensor_msgs/MagneticField: 104 | msg_time: t.to_sec() 105 | magnetic_field_x: m.magnetic_field.x 106 | magnetic_field_y: m.magnetic_field.y 107 | magnetic_field_z: m.magnetic_field.z 108 | 109 | sensor_msgs/NavSatFix: 110 | msg_time: t.to_sec() 111 | latitude: m.latitude 112 | longitude: m.longitude 113 | altitude: m.altitude 114 | 115 | sensor_msgs/Temperature: 116 | msg_time: t.to_sec() 117 | temperature: m.temperature 118 | 119 | sensor_msgs/TimeReference: 120 | msg_time: t.to_sec() 121 | header_timestamp: m.header.stamp.to_sec() 122 | time_ref: m.time_ref.to_sec() 123 | 124 | std_msgs/Duration: 125 | msg_time: t.to_sec() 126 | duration: m.data.to_sec() 127 | 128 | std_msgs/Float32: 129 | msg_time: t.to_sec() 130 | data: m.data 131 | 132 | std_msgs/Float64: 133 | msg_time: t.to_sec() 134 | data: m.data 135 | 136 | std_msgs/Float64MultiArray: 137 | msg_time: t.to_sec() 138 | 139 | std_msgs/Int8: 140 | msg_time: t.to_sec() 141 | data: m.data 142 | 143 | std_msgs/UInt8: 144 | msg_time: t.to_sec() 145 | data: m.data 146 | 147 | std_msgs/String: 148 | msg_time: t.to_sec() 149 | data: m.data 150 | 151 | tf2_msgs/TFMessage: 152 | msg_time: t.to_sec() 153 | 154 | ################## 155 | # Custom messages 156 | ################## 157 | seabed_drivers/DvlMeasurements: 158 | msg_time: t.to_sec() 159 | altitude: m.altitude 160 | ranges_0: m.ranges[0] 161 | ranges_1: m.ranges[1] 162 | rnages_2: m.ranges[2] 163 | ranges_3: m.ranges[3] 164 | btv_x: m.btv.x 165 | btv_y: m.btv.y 166 | btv_z: m.btv.z 167 | btv_e: m.btv_e 168 | orientation_quat_x: m.orientation.x 169 | orientation_quat_y: m.orientation.y 170 | orientation_quat_z: m.orientation.z 171 | orientation_quat_w: m.orientation.w 172 | temperature: m.temperature 173 | salinity: m.salinity 174 | depth: m.depth 175 | soundvel: m.soundvel 176 | 177 | seabed_drivers/ThrusterData: # t.to_sec(), m.prop_rpm, m.omega, m.voltage, m.current, m.temperature 178 | msg_time: t.to_sec() 179 | prop_rpm: m.prop_rpm 180 | omega: m.omega 181 | voltage: m.voltage 182 | current: m.current 183 | temperature: m.temperature 184 | -------------------------------------------------------------------------------- /config/bag2matpy_config.yaml: -------------------------------------------------------------------------------- 1 | #namespace: seabed 2 | topic_list: #- [topic_name, message_type(as per dictionary), variable_name in mat file 3 | - [/gps/odom, nav_msgs/Odometry, gps_odom] 4 | - [/dvl/odom, nav_msgs/Odometry, dvl_odom] 5 | - [/imu/data, sensor_msgs/Imu, imu_data] 6 | - [/heading, std_msgs/Float64, heading] 7 | - [/roll, std_msgs/Float64, roll] 8 | - [/pitch, std_msgs/Float64, pitch] 9 | 10 | -------------------------------------------------------------------------------- /config/bag2matpy_google_tango_online_data.yaml: -------------------------------------------------------------------------------- 1 | namespace: [''] 2 | topic_list: #- [topic_name, message_type(as per dictionary), variable_name in mat file 3 | - [/android/tango1/imu_fixed, sensor_msgs/Imu,imu_data] 4 | 5 | -------------------------------------------------------------------------------- /config/bag2matpy_seabed_config.yaml: -------------------------------------------------------------------------------- 1 | #namespace: seabed 2 | topic_list: #- [topic_name, message_type(as per dictionary), variable_name in mat file 3 | - [/gps/odom, nav_msgs/Odometry, gps_odom] 4 | - [/dvl/odom, nav_msgs/Odometry, dvl_odom] 5 | - [/dvl/velocity_measurements, seabed_drivers/DvlMeasurements, dvl_velocity_measurements] 6 | - [/imu/data, sensor_msgs/Imu, imu_data] 7 | - [/imu/data_raw, sensor_msgs/Imu, imu_data_raw] 8 | - [/cpu/temperature, std_msgs/Int8, cpu_temp] 9 | - [/heading, std_msgs/Float64, heading] 10 | - [/target_heading, std_msgs/Float64, target_heading] 11 | - [/target_pitch, std_msgs/Float64, target_pitch] 12 | - [/target_depth, std_msgs/Float64, target_depth] 13 | - [/heading_kp, std_msgs/Float64, heading_kp] 14 | - [/heading_kd, std_msgs/Float64, heading_kd] 15 | - [/heading_ki, std_msgs/Float64, heading_ki] 16 | - [/pitch_kp, std_msgs/Float64, pitch_kp] 17 | - [/pitch_kd, std_msgs/Float64, pitch_kd] 18 | - [/pitch_ki, std_msgs/Float64, pitch_ki] 19 | - [/depth_kp, std_msgs/Float64, depth_kp] 20 | - [/depth_kd, std_msgs/Float64, depth_kd] 21 | - [/depth_ki, std_msgs/Float64, depth_ki] 22 | - [/roll, std_msgs/Float64, roll] 23 | - [/target_roll, std_msgs/Float64, target_roll] 24 | - [/pitch, std_msgs/Float64, pitch] 25 | - [/target_pitch, std_msgs/Float64, target_pitch] 26 | - [/battery/temperature, std_msgs/Float64, battery_temp] 27 | - [/ct/conductivity, std_msgs/Float64, conductivity] 28 | - [/ct/salinity, std_msgs/Float64, salinity] 29 | - [/ct/temperature, std_msgs/Float64, ct_temp] 30 | - [/fins/elevator_cmd, std_msgs/Float64, elevator_cmd] 31 | - [/fins/roll_cmd, std_msgs/Float64, roll_cmd] 32 | - [/fins/rudder_cmd, std_msgs/Float64, rudder_cmd] 33 | - [/thruster/cmd, std_msgs/Float64, thruster_cmd] 34 | - [/paro/depth, std_msgs/Float64, depth] 35 | - [/paro/pressure, std_msgs/Float64, pressure] 36 | - [/battery/time_to_empty, std_msgs/Duration, time_to_empty] 37 | - [/battery/status, sensor_msgs/BatteryState, battery_state] 38 | - [/thruster/data, seabed_drivers/ThrusterData, thruster_data] 39 | 40 | -------------------------------------------------------------------------------- /config/bag2matpy_simplenav_uas.yaml: -------------------------------------------------------------------------------- 1 | namespace: [uas1, uas2, uas3, uas4,''] # list of namespaces to search 2 | topic_list: #- [topic_name, message_type(as per dictionary), variable_name in mat file 3 | - [/dji_sdk/flight_status, std_msgs/UInt8, flight_status] 4 | - [/dji_sdk/gps_position, sensor_msgs/NavSatFix, gps_fix] 5 | - [/dji_sdk/imu, sensor_msgs/Imu, imu_data] 6 | - [/dji_sdk/rc, sensor_msgs/Joy_DJI_RC, rc] 7 | - [/dji_sdk/flight_control_setpoint_generic, sensor_msgs/Joy_UAS, cmd_vel_joy] 8 | - [/dji_sdk/velocity, geometry_msgs/Vector3Stamped, velocity] 9 | - [/dji_sdk/height_above_takeoff, std_msgs/Float32, fusedHeight] 10 | - [/simplenav/odom, nav_msgs/Odometry, uas_odom] 11 | - [/simplenav/takeoffAltitude, std_msgs/Float64, takeoffAltitude] 12 | - [/simplenav/homePose, geometry_msgs/PoseStamped, homePose] 13 | - [/simplenav/cmd_vel, geometry_msgs/TwistStamped, cmd_vel] 14 | - [/zed_node/pose_with_covariance, geometry_msgs/PoseWithCovarianceStamped, zed_pose] 15 | - [/zed_node/odom, nav_msgs/Odometry, zed_odom] -------------------------------------------------------------------------------- /config/bag2matpy_vio.yaml: -------------------------------------------------------------------------------- 1 | namespace: [''] 2 | topic_list: #- [topic_name, message_type(as per dictionary), variable_name in mat file 3 | - [/imu/imu, sensor_msgs/Imu,imu_data] 4 | 5 | -------------------------------------------------------------------------------- /config/bag2matpy_xsens_online_data.yaml: -------------------------------------------------------------------------------- 1 | namespace: [''] 2 | topic_list: #- [topic_name, message_type(as per dictionary), variable_name in mat file 3 | - [/sensor/xsens/sensor/imu_raw, sensor_msgs/Imu,imu_data] 4 | 5 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | bag2mat 4 | 1.0.0 5 | 6 | This package provides dataset parsing of rosbags to matlab format. 7 | 8 | Patrick Geneva 9 | Patrick Geneva 10 | 11 | 12 | MIT 13 | 14 | 15 | catkin 16 | 17 | 18 | rosbag 19 | roslib 20 | rospkg 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /scripts/bag2matpy: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | """ 5 | @author: Vikrant Shah 6 | A simple script that can converts rosmessages to matlab mat files 7 | Define the translation of each message type in a dictionary file and use a 8 | config file to specify all the topics to be converted. 9 | """ 10 | from __future__ import print_function 11 | 12 | import roslib 13 | import rospy 14 | import rostopic 15 | import rospkg 16 | import rosbag 17 | 18 | import argparse 19 | import importlib 20 | import sys 21 | import numpy as np 22 | import time 23 | from scipy.io import savemat 24 | import os 25 | import yaml 26 | import logging 27 | import copy 28 | #import progressbar 29 | #progressbar.streams.wrap_stderr() 30 | 31 | bglog = logging.getLogger("bag2matpy") 32 | logging.basicConfig(level=logging.INFO, format='%(levelname)8s %(message)s') 33 | 34 | 35 | def save_topic2dict(topic_name, msg_dict, bagfile, start_time,subtract_start_time): 36 | bglog = logging.getLogger(__name__) 37 | expression = ",".join(msg_dict.values()) 38 | field_names = msg_dict.keys() 39 | print('Topic %s being saved in a dict'%(topic_name)) 40 | data_array = [] 41 | progress = 0 42 | for topic, msg, ts in bagfile.read_messages(topics=[topic_name]): 43 | res = eval("{}".format(expression),{'np':np}, {'m': msg, 't':ts}) 44 | 45 | if subtract_start_time: 46 | offset_time = ts.to_sec() - start_time 47 | res = np.hstack([offset_time,res]) 48 | 49 | data_array.append(res) 50 | progress = progress + 1 51 | if progress%100000 == 0: 52 | bglog.info("100000 messages from the topic are added to dictionary and continuing") 53 | if data_array: 54 | data_dict = {} 55 | for field_name, column in zip(field_names, np.array(data_array).T): 56 | data_dict[field_name] = column[:,None] 57 | 58 | return data_dict 59 | 60 | else: 61 | bglog.warn("Notfound! Topic: "+topic_name) 62 | return None 63 | 64 | # get the file path for rospy_tutorials 65 | rospack = rospkg.RosPack() 66 | 67 | parser = argparse.ArgumentParser( 68 | formatter_class=argparse.RawTextHelpFormatter, 69 | description='Convert ROSBAG to mat file.\n\n' 70 | 'A simple script that can converts rosmessages to matlab mat files. \ 71 | \nDefine the translation of each message type in a dictionary file and use a config file to specify all the topics to be converted..\n\n' 72 | ) 73 | parser.add_argument('-i','--input_bagfile', help='Input rosbag file to input') 74 | parser.add_argument('-o','--output_dir', help='Output dir for matfile') 75 | parser.add_argument('-c','--config_file', help='Yaml file which specifies topic names to convert', 76 | default =rospack.get_path('bag2mat')+'/config/bag2matpy_config.yaml') 77 | parser.add_argument('-d','--dictionary', help='Dictionary file which specifies how to read the topic', 78 | default =rospack.get_path('bag2mat')+'/config/bag2mat_dict.yaml') 79 | parser.add_argument('-s','--subtract_start_time', default=False, action="store_true" , help="Boolean flag to specify whether to include offset_time obtained by subtracting bag start_time from all timestamps") 80 | 81 | args = parser.parse_args() 82 | 83 | with open(args.dictionary, 'r') as f: 84 | exp_dic = yaml.safe_load(f) 85 | 86 | with open(args.config_file, 'r') as f: 87 | config = yaml.safe_load(f) 88 | 89 | if 'namespace' in config: 90 | ns_prefixes = config['namespace'] 91 | else: 92 | ns_prefixes = [''] 93 | 94 | subtract_start_time = args.subtract_start_time 95 | 96 | bag = rosbag.Bag(args.input_bagfile) 97 | 98 | output_mat_name = (os.path.basename(args.input_bagfile)).replace('.bag','.mat') 99 | 100 | if args.output_dir is None: 101 | output_mat_path = os.path.dirname(os.path.realpath(args.input_bagfile))+'/' 102 | else: 103 | output_mat_path = os.path.realpath(args.output_dir) +'/' 104 | 105 | output_mat = output_mat_path + output_mat_name 106 | 107 | if not os.path.exists(output_mat_path): 108 | os.makedirs(output_mat_path) 109 | bglog.warn("output_dir doesn't exists, creating required path "+output_mat) 110 | 111 | 112 | if os.path.isfile(output_mat): 113 | bglog.warn("File Already Exists, over-writing - "+output_mat) 114 | else: 115 | bglog.info("Writing to file - "+output_mat) 116 | 117 | with open(output_mat,'wb',0) as ofile: 118 | 119 | start_time = bag.get_start_time() 120 | savemat(ofile, {'start_time': start_time }) 121 | bag_topics = bag.get_type_and_topic_info().topics.keys() 122 | #for topic in progressbar.progressbar(config['topic_list']): 123 | for ns_prefix in ns_prefixes: 124 | 125 | if any([topic.startswith('/'+ns_prefix) for topic in bag_topics]): 126 | ns_dict = {} 127 | for topic in config['topic_list']: 128 | if ns_prefix: 129 | topic_name = '/'+ns_prefix+topic[0] 130 | else: 131 | topic_name = topic[0] 132 | print("Current topic written:%s"%(topic_name)) 133 | msg_type = topic[1] 134 | 135 | #if 'namespace' in config: 136 | # variable_name = topic[2] +'_'+ config['namespace'] 137 | #else: 138 | variable_name = topic[2] 139 | msg_dict = exp_dic[msg_type] 140 | 141 | data_dict = save_topic2dict(topic_name, msg_dict , bag, start_time, subtract_start_time) 142 | 143 | if data_dict: 144 | ns_dict[variable_name] = copy.deepcopy(data_dict) 145 | 146 | if ns_dict: 147 | bglog.info("Saving mat file") 148 | if ns_prefix: 149 | savemat(ofile, {ns_prefix: ns_dict}) 150 | else: 151 | savemat(ofile, {'robot': ns_dict}) 152 | bglog.info("Wrote namespace: "+ns_prefix+" \n") 153 | else: 154 | bglog.warn("Notfound! Namespace: "+ns_prefix+'\n') 155 | 156 | bag.close() 157 | --------------------------------------------------------------------------------