├── .gitignore ├── LICENSE ├── README.md ├── bag2image ├── bag2image.py └── config │ └── config.yaml ├── bag2kitti ├── bag2kitti.py └── config │ └── config.yaml ├── bag2mat ├── CMakeLists.txt ├── LICENSE ├── ReadMe.md ├── config │ ├── examples │ │ ├── bag2matpy_vio_euroc.yaml │ │ ├── bag2matpy_vio_neufr.yaml │ │ ├── nuance_car.yaml │ │ ├── optitrack_config.yaml │ │ ├── seabed_config.yaml │ │ ├── simplenav_config.yaml │ │ └── vio_config.yaml │ ├── msg_dict.yaml │ └── test_config.yaml ├── package.xml └── scripts │ └── bag2matpy ├── bag2pandas ├── .gitignore ├── LICENSE ├── README.md ├── bag2pandasdf.py └── config │ ├── bag2pddf_config.yaml │ ├── bag2pddf_dict.yaml │ ├── bag2pddf_seabed_config.yaml │ └── bag2pddf_simplenav_uas.yaml ├── bag_correction ├── bagTime2Timestamp.py └── config.yaml ├── bagmerge └── bag_merge.py ├── bagselector ├── bag_selector.py ├── config │ ├── config.yaml │ ├── config_ir.yaml │ ├── config_ir_old.yaml │ └── config_zed.yaml └── insert_camerainfo_topics.py ├── downsample_bags ├── config │ └── config.yaml └── downsample_bags.py ├── image2bag ├── config │ ├── config.yaml │ ├── config_ir.yaml │ └── config_single.yaml └── image2bag.py └── image_resize └── resize_image.py /.gitignore: -------------------------------------------------------------------------------- 1 | *.bag 2 | *.idea/ 3 | *.yaml 4 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 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 | # rosbag_toolkit 2 | -------------------------------------------------------------------------------- /bag2image/bag2image.py: -------------------------------------------------------------------------------- 1 | import rosbag 2 | import roslib 3 | import sys 4 | import rospy 5 | import cv2 6 | from std_msgs.msg import String, Header 7 | from sensor_msgs.msg import Image 8 | from cv_bridge import CvBridge, CvBridgeError 9 | 10 | 11 | ''' 12 | @author : Pushyami Kaveti 13 | Simple script to take a rosbag and write images topics as images 14 | ''' 15 | import rospy 16 | import cv2 17 | from std_msgs.msg import String 18 | from sensor_msgs.msg import Image 19 | from cv_bridge import CvBridge, CvBridgeError 20 | import message_filters 21 | import numpy as np 22 | import rosbag 23 | import os 24 | import argparse 25 | import yaml 26 | 27 | ''' 28 | @author: Pushyami Kaveti 29 | This is a tool to select only required images from a rosbag. 30 | The script goes through each image in the bag one by one and 31 | shows in a window, it has key bindings to select the image 32 | which will be written to another bag file. It uses a config 33 | file to specify various options for selection like topics, 34 | time range, frequency etc. 35 | ''' 36 | 37 | class Bag2Image: 38 | def __init__(self,read_bag_path, write_folder_path, write_bag_path, config): 39 | self.viz=False 40 | self.calib=False 41 | self.write_bag = rosbag.Bag(os.path.join(write_folder_path,write_bag_path), 'w') 42 | self.write_folder_path = write_folder_path 43 | self.bridge = CvBridge() 44 | self.topics = self.image_pub_topics = config['topics'] 45 | 46 | self.num_topics = len(self.topics) 47 | for i in range(self.num_topics): 48 | pathe = os.path.join(self.write_folder_path,"cam"+str(i)) 49 | if not os.path.exists(pathe): 50 | os.makedirs(pathe) 51 | rospy.init_node('Bag2Image', anonymous = True) 52 | self.read_bag = rosbag.Bag(read_bag_path) 53 | self.start = rospy.Time(config['start_time'] + self.read_bag.get_start_time()) 54 | self.end = rospy.Time(config['end_time'] + self.read_bag.get_start_time()) 55 | self.frequency = config['frequency'] 56 | self.convertBag() 57 | 58 | def sort_topics_msgs(self,topic_list, msg_list): 59 | np_topic_list = np.array(topic_list) 60 | sorted_inds = np.argsort(np_topic_list) 61 | print(topic_list) 62 | print(sorted_inds) 63 | sorted_topiclist= [] 64 | sorted_msglist=[] 65 | for ind in sorted_inds: 66 | sorted_msglist.append(msg_list[ind]) 67 | sorted_topiclist.append(topic_list[ind]) 68 | return sorted_topiclist, sorted_msglist 69 | 70 | 71 | def convertBag(self): 72 | oldt = 0; 73 | topiclist=[] 74 | msg_list=[] 75 | count = 0 76 | print(type(self.read_bag.get_start_time())) 77 | for topic, msg, t in self.read_bag.read_messages(topics=self.topics , start_time= self.start , end_time= self.end): 78 | if oldt == msg.header.stamp: 79 | topiclist.append(topic) 80 | msg_list.append(msg) 81 | #print(msg.header.frame_id) 82 | else: 83 | oldt = msg.header.stamp 84 | #sort the topic list and msgs 85 | topiclist, msg_list = self.sort_topics_msgs(topiclist, msg_list) 86 | if len(topiclist) == self.num_topics and topiclist==self.topics : 87 | if count % self.frequency == 0: 88 | #print("Writing to bag:") 89 | #print('count:' + str(count)) 90 | #print(t) 91 | cv_img_list=[] 92 | for im_msg in msg_list: 93 | cv_image = self.bridge.imgmsg_to_cv2(im_msg, "mono8") 94 | cv_img_list.append(cv_image) 95 | 96 | for i in range(self.num_topics): 97 | writepath = os.path.join(self.write_folder_path,"cam"+str(i),str(oldt.to_nsec())+".png") 98 | if not os.path.exists(writepath): 99 | print(writepath) 100 | cv2.imwrite(writepath, cv_img_list[i]) 101 | count = count + 1 102 | print("starting list\n") 103 | 104 | topiclist=[] 105 | msg_list=[] 106 | topiclist.append(topic) 107 | msg_list.append(msg) 108 | self.write_bag.close() 109 | 110 | 111 | if __name__=="__main__": 112 | parser = argparse.ArgumentParser(formatter_class=argparse.RawTextHelpFormatter, 113 | description='Selects specific messages from a ROSBAG and saves them to a bag file.\ 114 | In case of images we can save image sto a fodler as well\n\n') 115 | parser.add_argument('-i','--input_bagfile', help='Input rosbag file to input', required=True) 116 | parser.add_argument('-o','--output_dir', help='Output dir for output bag file', default='.') 117 | parser.add_argument('-of','--output_filename', help='Output file name for output bag file' , default = 'output.bag') 118 | parser.add_argument('-c','--config_file', help='Yaml file which specifies the topic names, frequency of selection and time range', 119 | default = 'config/config.yaml') 120 | 121 | args = parser.parse_args() 122 | 123 | with open(args.config_file, 'r') as f: 124 | config = yaml.safe_load(f) 125 | print(config) 126 | bs = Bag2Image(args.input_bagfile , args.output_dir , args.output_filename, config) 127 | #bs = BagSelector("2019_Aug6_indoor_rig_calib_selected.bag" , "/home/auv/testing_bagselector", "") 128 | #/media/auv/Untitled/2019-08-13-00-09-48.bag 129 | -------------------------------------------------------------------------------- /bag2image/config/config.yaml: -------------------------------------------------------------------------------- 1 | topics: 2 | - /camera_array/cam0/image_raw 3 | - /camera_array/cam1/image_raw 4 | - /camera_array/cam2/image_raw 5 | - /camera_array/cam3/image_raw 6 | - /camera_array/cam4/image_raw 7 | - /camera_array/cam5/image_raw 8 | - /camera_array/cam6/image_raw 9 | 10 | frequency: 1 11 | start_time: 0 # in seconds 12 | viz: false 13 | end_time: 100000 # in seconds 14 | 15 | 16 | -------------------------------------------------------------------------------- /bag2kitti/bag2kitti.py: -------------------------------------------------------------------------------- 1 | import rosbag 2 | import roslib 3 | import sys 4 | import rospy 5 | import cv2 6 | from std_msgs.msg import String, Header 7 | from sensor_msgs.msg import Image 8 | from cv_bridge import CvBridge, CvBridgeError 9 | 10 | 11 | ''' 12 | @author : Pushyami Kaveti 13 | Simple script to take a rosbag and write image topics to disk in kitti dataset format. 14 | It uses a config file to specify various options for selection like topics, 15 | time range, frequency etc. 16 | ''' 17 | import rospy 18 | import cv2 19 | from std_msgs.msg import String 20 | from sensor_msgs.msg import Image 21 | from cv_bridge import CvBridge, CvBridgeError 22 | import message_filters 23 | import numpy as np 24 | import rosbag 25 | import os 26 | import argparse 27 | import yaml 28 | 29 | 30 | class Bag2Kitti: 31 | def __init__(self,read_bag_path, write_folder_path, write_bag_path, config): 32 | self.viz=False 33 | self.calib=False 34 | self.write_bag = rosbag.Bag(os.path.join(write_folder_path,write_bag_path), 'w') 35 | self.write_folder_path = write_folder_path 36 | self.bridge = CvBridge() 37 | self.topics = self.image_pub_topics = config['topics'] 38 | self.num_frames = config['num_frames'] 39 | self.bag_name = os.path.basename(read_bag_path)[:-4] 40 | self.num_topics = len(self.topics) 41 | #for i in range(self.num_topics): 42 | # pathe = os.path.join(self.write_folder_path,"image_"+str(i),"data") 43 | # if not os.path.exists(pathe): 44 | # os.makedirs(pathe) 45 | #self.timestamp_file = open(os.path.join(self.write_folder_path,'timestamps.txt'), 'a') 46 | rospy.init_node('Bag2Image', anonymous = True) 47 | self.read_bag = rosbag.Bag(read_bag_path) 48 | self.start = rospy.Time(config['start_time'] + self.read_bag.get_start_time()) 49 | self.end = rospy.Time(config['end_time'] + self.read_bag.get_start_time()) 50 | self.frequency = config['frequency'] 51 | self.convertBag() 52 | 53 | 54 | def convertBag(self): 55 | oldt = 0; 56 | topiclist=[] 57 | msg_list=[] 58 | count = 0 59 | part_num=0 60 | print(type(self.read_bag.get_start_time())) 61 | for topic, msg, t in self.read_bag.read_messages(topics=self.topics , start_time= self.start , end_time= self.end): 62 | if oldt == msg.header.stamp: 63 | topiclist.append(topic) 64 | msg_list.append(msg) 65 | #print(msg.header.frame_id) 66 | else: 67 | oldt = msg.header.stamp 68 | if count % self.num_frames == 0: 69 | if count != 0: 70 | self.timestamp_file.close() 71 | count = 0 72 | part_num = part_num + 1 73 | for i in range(self.num_topics): 74 | pathe = os.path.join(self.write_folder_path,self.bag_name+"_"+str(part_num).zfill(4),"image_"+str(i),"data") 75 | if not os.path.exists(pathe): 76 | os.makedirs(pathe) 77 | self.timestamp_file = open(os.path.join(self.write_folder_path,self.bag_name+"_"+str(part_num).zfill(4),'timestamps.txt'), 'a') 78 | 79 | if len(topiclist) == self.num_topics and topiclist==self.topics : 80 | if count % self.frequency == 0: 81 | #print("Writing to bag:") 82 | #print('count:' + str(count)) 83 | #print(t) 84 | cv_img_list=[] 85 | for im_msg in msg_list: 86 | cv_image = self.bridge.imgmsg_to_cv2(im_msg, "bgr8") 87 | cv_img_list.append(cv_image) 88 | 89 | for i in range(self.num_topics): 90 | print(os.path.join(self.write_folder_path,self.bag_name+"_"+str(part_num).zfill(4),"image_"+str(i),"data",str(count).zfill(10)+".jpg")) 91 | cv2.imwrite(os.path.join(self.write_folder_path,self.bag_name+"_"+str(part_num).zfill(4),"image_"+str(i),"data",str(count).zfill(10)+".jpg"), cv_img_list[i]) 92 | self.timestamp_file.write(str(oldt.secs)+"."+str(oldt.nsecs)+"\n") 93 | count = count + 1 94 | print("starting list\n") 95 | 96 | topiclist=[] 97 | msg_list=[] 98 | topiclist.append(topic) 99 | msg_list.append(msg) 100 | self.write_bag.close() 101 | self.timestamp_file.close() 102 | 103 | 104 | if __name__=="__main__": 105 | parser = argparse.ArgumentParser(formatter_class=argparse.RawTextHelpFormatter, 106 | description='Selects specific messages from a ROSBAG and saves them to a bag file.\ 107 | In case of images we can save image sto a fodler as well\n\n') 108 | parser.add_argument('-i','--input_bagfile', help='Input rosbag file to input', required=True) 109 | parser.add_argument('-o','--output_dir', help='Output dir for output bag file', default='.') 110 | parser.add_argument('-of','--output_filename', help='Output file name for output bag file' , default = 'output.bag') 111 | parser.add_argument('-c','--config_file', help='Yaml file which specifies the topic names, frequency of selection and time range', 112 | default = 'config/config.yaml') 113 | 114 | args = parser.parse_args() 115 | 116 | with open(args.config_file, 'r') as f: 117 | config = yaml.safe_load(f) 118 | print(config) 119 | bs = Bag2Kitti(args.input_bagfile , args.output_dir , args.output_filename, config) 120 | -------------------------------------------------------------------------------- /bag2kitti/config/config.yaml: -------------------------------------------------------------------------------- 1 | topics: 2 | - /camera_array/cam0/image_raw 3 | - /camera_array/cam1/image_raw 4 | - /camera_array/cam2/image_raw 5 | - /camera_array/cam3/image_raw 6 | - /camera_array/cam4/image_raw 7 | frequency: 1 8 | 9 | start_time: 0 # in seconds 10 | end_time: 100000 # in seconds 11 | num_frames: 150 12 | 13 | -------------------------------------------------------------------------------- /bag2mat/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 | -------------------------------------------------------------------------------- /bag2mat/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 copies or substantial portions of the Software. 13 | 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | SOFTWARE. 21 | -------------------------------------------------------------------------------- /bag2mat/ReadMe.md: -------------------------------------------------------------------------------- 1 | # bag2mat 2 | 3 | * Takes a rosbag file and converts it to a matlab mat file based on settings specified in a Dictionary and a yaml based config file. 4 | - Dictionary: The dictionary file specifies the list of variables(of interest) from each message type to be moved to matlab array. Definitions for many standard ros msgs are already included in in the config/msg_dict.yaml. Append your custom msgs definitions to the same file. 5 | eg `sensor_msgs/NavSatFix: t.to_sec(), m.latitude, m.longitude, m.altitude` 6 | 7 | - Configuration: This config file is used to specify topic names that should be transferred to a mat file and their associated variable name. 8 | Format`-[topic name, message type, name for variable in matfile]` 9 | See config/test_config.yaml for example. 10 | * Dependencies: 11 | - rosbag 12 | - roslib 13 | - scipy 14 | - argparse 15 | - yaml 16 | - utm 17 | - logging 18 | 19 | * Clone this repo in the src folder of your ros workspace and run 20 | ``` 21 | catkin_make 22 | source devel/setup.bash 23 | usage: bag2matpy [-h] -i INPUT_BAGFILE [-o OUTPUT_DIR] [-c CONFIG_FILE] 24 | [-d DICTIONARY] [-s] [-iutm] 25 | ``` 26 | ``` 27 | Complete List of Arguments: 28 | 29 | -h, --help show this help message and exit 30 | 31 | -i INPUT_BAGFILE, --input_bagfile INPUT_BAGFILE 32 | Input a rosbag file 33 | -o OUTPUT_DIR, --output_dir OUTPUT_DIR 34 | Output dir for mat file 35 | -c CONFIG_FILE, --config_file CONFIG_FILE 36 | Yaml file specifying topic names to convert 37 | -d DICTIONARY, --dictionary DICTIONARY 38 | Dictionary file which specifies how to read the topic 39 | -s, --subtract_start_time 40 | Boolean flag to specify whether to include 41 | offset_time obtained by subtracting bag start_time from all timestamps 42 | -iutm, --ignore_ll2utm 43 | Boolean flag to specify whether to ignore converting lat long msg to utm, defaults to False 44 | ``` 45 | ``` 46 | 47 | ``` 48 | -------------------------------------------------------------------------------- /bag2mat/config/examples/bag2matpy_vio_euroc.yaml: -------------------------------------------------------------------------------- 1 | namespace: [''] 2 | topic_list: #- [topic_name, message_type(as per dictionary), variable_name in mat file 3 | - [/imu0, sensor_msgs/Imu,imu_data] 4 | 5 | -------------------------------------------------------------------------------- /bag2mat/config/examples/bag2matpy_vio_neufr.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 | -------------------------------------------------------------------------------- /bag2mat/config/examples/nuance_car.yaml: -------------------------------------------------------------------------------- 1 | namespace: [''] 2 | topic_list: 3 | #- [topic_name, message_type(as per dictionary), variable_name in mat file 4 | - [/imu/imu, sensor_msgs/Imu, vn100_imu] 5 | - [/vehicle/gps/fix, sensor_msgs/NavSatFix, gps] 6 | -------------------------------------------------------------------------------- /bag2mat/config/examples/optitrack_config.yaml: -------------------------------------------------------------------------------- 1 | namespace: [''] 2 | topic_list: #- [topic_name, message_type(as per dictionary), variable_name in mat file 3 | - [/mocap_node/Robot_1/pose, geometry_msgs/PoseStamped, gt_pose] 4 | 5 | -------------------------------------------------------------------------------- /bag2mat/config/examples/seabed_config.yaml: -------------------------------------------------------------------------------- 1 | #namespace: [seabed] 2 | topic_list: 3 | #- [topic_name, message_type(as per dictionary), variable_name in mat file 4 | - [/gps/odom, nav_msgs/Odometry, gps_odom] 5 | - [/dvl/odom, nav_msgs/Odometry, dvl_odom] 6 | - [/dvl/velocity_measurements, seabed_drivers/DvlMeasurements, dvl_velocity_measurements] 7 | - [/imu/data, sensor_msgs/Imu, imu_data] 8 | - [/imu/data_raw, sensor_msgs/Imu, imu_data_raw] 9 | - [/cpu/temperature, std_msgs/Int8, cpu_temp] 10 | - [/heading, std_msgs/Float64, heading] 11 | - [/target_heading, std_msgs/Float64, target_heading] 12 | - [/target_pitch, std_msgs/Float64, target_pitch] 13 | - [/target_depth, std_msgs/Float64, target_depth] 14 | - [/heading_kp, std_msgs/Float64, heading_kp] 15 | - [/heading_kd, std_msgs/Float64, heading_kd] 16 | - [/heading_ki, std_msgs/Float64, heading_ki] 17 | - [/pitch_kp, std_msgs/Float64, pitch_kp] 18 | - [/pitch_kd, std_msgs/Float64, pitch_kd] 19 | - [/pitch_ki, std_msgs/Float64, pitch_ki] 20 | - [/depth_kp, std_msgs/Float64, depth_kp] 21 | - [/depth_kd, std_msgs/Float64, depth_kd] 22 | - [/depth_ki, std_msgs/Float64, depth_ki] 23 | - [/roll, std_msgs/Float64, roll] 24 | - [/target_roll, std_msgs/Float64, target_roll] 25 | - [/pitch, std_msgs/Float64, pitch] 26 | - [/target_pitch, std_msgs/Float64, target_pitch] 27 | - [/battery/temperature, std_msgs/Float64, battery_temp] 28 | - [/ct/conductivity, std_msgs/Float64, conductivity] 29 | - [/ct/salinity, std_msgs/Float64, salinity] 30 | - [/ct/temperature, std_msgs/Float64, ct_temp] 31 | - [/fins/elevator_cmd, std_msgs/Float64, elevator_cmd] 32 | - [/fins/roll_cmd, std_msgs/Float64, roll_cmd] 33 | - [/fins/rudder_cmd, std_msgs/Float64, rudder_cmd] 34 | - [/thruster/cmd, std_msgs/Float64, thruster_cmd] 35 | - [/paro/depth, std_msgs/Float64, depth] 36 | - [/paro/pressure, std_msgs/Float64, pressure] 37 | - [/battery/time_to_empty, std_msgs/Duration, time_to_empty] 38 | - [/battery/status, sensor_msgs/BatteryState, battery_state] 39 | - [/thruster/data, seabed_drivers/ThrusterData, thruster_data] 40 | 41 | -------------------------------------------------------------------------------- /bag2mat/config/examples/simplenav_config.yaml: -------------------------------------------------------------------------------- 1 | namespace: [frl_uas7, uas2, uas3, uas4, ''] # list of namespaces to search 2 | topic_list: 3 | #- [topic_name, message_type(as per dictionary), variable_name in mat file 4 | # dji osdk ros 3.8 5 | - [/dji_sdk/flight_status, std_msgs/UInt8, dji_flight_status] 6 | - [/dji_sdk/gps_health, std_msgs/UInt8, dji_gps_quality] 7 | - [/dji_sdk/gps_position, sensor_msgs/NavSatFix, dji_gps_fix] 8 | - [/dji_sdk/imu, sensor_msgs/Imu, dji_imu] 9 | - [/dji_sdk/rc, sensor_msgs/Joy_DJI_RC, dji_rc] 10 | - [/dji_sdk/flight_control_setpoint_generic, sensor_msgs/Joy_UAS, dji_cmd_vel] 11 | - [/dji_sdk/velocity, geometry_msgs/Vector3Stamped, dji_velocity] 12 | - [/dji_sdk/height_above_takeoff, std_msgs/Float32, dji_fusedHeight] 13 | - [/dji_sdk/battery_state, sensor_msgs/BatteryState, dji_battery] 14 | # simplenav stack 15 | - [/simplenav/odom, nav_msgs/Odometry, uas_odom] 16 | - [/simplenav/takeoffAltitude, std_msgs/Float64, takeoffAltitude] 17 | - [/simplenav/homePose, geometry_msgs/PoseStamped, homePose] 18 | - [/simplenav/cmd_vel, geometry_msgs/TwistStamped, cmd_vel] 19 | - [/navigator/goal, simplenav/NavigatorActionGoal, wp_goal] 20 | - [/navigator/result, simplenav/NavigatorActionResult, wp_result] 21 | # lidar_lite 22 | - [/lidarlite/range/down, sensor_msgs/Range, lidarlite_range_z] 23 | # ppk fix 24 | - [/ppk_fix/fix, sensor_msgs/NavSatFix, ppk_fix] 25 | #- [/ppk_fix/utm, geometry_msgs/PoseWithCovarianceStamped, ppk_utm] 26 | # spinnaker_sdk_driver, boson_sdk_driver 27 | - [/camera_array/cam0/image_raw, sensor_msgs/Image, vs_cam_0] 28 | - [/boson_camera_array/cam_0/image_raw, sensor_msgs/Image, ir_cam_0] 29 | # vn 100 imu 30 | - [/imu/imu, sensor_msgs/Imu, vn100_imu] 31 | - [/imu/fluid_pressure, sensor_msgs/FluidPressure, vn100_fluid_pressure] 32 | - [/imu/temperature, sensor_msgs/Temperature, vn100_temperature] 33 | - [/imu/magnetic_field, sensor_msgs/MagneticField, vn100_magnetic_field] 34 | # zed sdk 35 | - [/zed_node/pose_with_covariance, geometry_msgs/PoseWithCovarianceStamped, zed_pose] 36 | - [/zed_node/odom, nav_msgs/Odometry, zed_odom] 37 | # optitrack 38 | - [/mocap_node/frl_uas7/pose, geometry_msgs/PoseStamped, gt_uas] #optitrack_pose_on_uas 39 | -------------------------------------------------------------------------------- /bag2mat/config/examples/vio_config.yaml: -------------------------------------------------------------------------------- 1 | namespace: [''] 2 | topic_list: 3 | #- [topic_name, message_type(as per dictionary), variable_name in mat file 4 | - [/imu/imu, sensor_msgs/Imu, vn100_imu] # vn_100 topic name 5 | - [/sensor/xsens/sensor/imu_raw, sensor_msgs/Imu, xsens_imu] # xsense imu 6 | - [/android/tango1/imu_fixed, sensor_msgs/Imu, tango_imu] # tango phone 7 | - [/imu0, sensor_msgs/Imu, euroc_imu] # euroc dataset imu topic 8 | -------------------------------------------------------------------------------- /bag2mat/config/msg_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 | bag_time: t.to_sec() 7 | msg_time: 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 | bag_time: t.to_sec() 17 | msg_time: 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 | bag_time: t.to_sec() 28 | msg_time: 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 | bag_time: t.to_sec() 40 | msg_time: m.header.stamp.to_sec() 41 | vector_x: m.vector.x 42 | vector_y: m.vector.y 43 | vector_z: m.vector.z 44 | 45 | nav_msgs/Odometry: 46 | bag_time: t.to_sec() 47 | msg_time: m.header.stamp.to_sec() 48 | position_x: m.pose.pose.position.x 49 | position_y: m.pose.pose.position.y 50 | position_z: m.pose.pose.position.z 51 | orientation_quat_x: m.pose.pose.orientation.x 52 | orientation_quat_y: m.pose.pose.orientation.y 53 | orientation_quat_z: m.pose.pose.orientation.z 54 | orientation_quat_w: m.pose.pose.orientation.w 55 | velocity_x: m.twist.twist.linear.x 56 | velocity_y: m.twist.twist.linear.y 57 | velocity_z: m.twist.twist.linear.z 58 | angular_velocity_x: m.twist.twist.angular.x 59 | angular_velocity_y: m.twist.twist.angular.y 60 | angular_velocity_z: m.twist.twist.angular.z 61 | 62 | sensor_msgs/BatteryState: 63 | bag_time: t.to_sec() 64 | msg_time: m.header.stamp.to_sec() 65 | voltage: m.voltage 66 | current: m.current 67 | charge: m.charge 68 | capacity: m.capacity 69 | design_capacity: m.design_capacity 70 | percentage: m.percentage 71 | 72 | sensor_msgs/Imu: 73 | bag_time: t.to_sec() 74 | msg_time: m.header.stamp.to_sec() 75 | orientation_quat_x: m.orientation.x 76 | orientation_quat_y: m.orientation.y 77 | orientation_quat_z: m.orientation.z 78 | orientation_quat_w: m.orientation.w 79 | acceleration_x: m.linear_acceleration.x 80 | acceleration_y: m.linear_acceleration.y 81 | acceleration_z: m.linear_acceleration.z 82 | angular_velocity_x: m.angular_velocity.x 83 | angular_velocity_y: m.angular_velocity.y 84 | angular_velocity_z: m.angular_velocity.z 85 | 86 | sensor_msgs/JointState: 87 | bag_time: t.to_sec() 88 | msg_time: m.header.stamp.to_sec() 89 | 90 | sensor_msgs/Joy_UAS: 91 | bag_time: t.to_sec() 92 | msg_time: m.header.stamp.to_sec() 93 | cmd_velocity_x: m.axes[0] 94 | cmd_velocity_y: m.axes[1] 95 | cmd_velocity_z: m.axes[2] 96 | cmd_yaw_rate: m.axes[3] 97 | config_code: m.axes[4] 98 | 99 | sensor_msgs/Joy_DJI_RC: 100 | bag_time: t.to_sec() 101 | msg_time: m.header.stamp.to_sec() 102 | cmd_roll: m.axes[0] 103 | cmd_pitch: m.axes[1] 104 | cmd_yaw_rate: m.axes[2] 105 | cmd_thrust: m.axes[3] 106 | cmd_mode: m.axes[4] 107 | cmd_landing_gear: m.axes[5] 108 | 109 | sensor_msgs/MagneticField: 110 | bag_time: t.to_sec() 111 | msg_time: m.header.stamp.to_sec() 112 | magnetic_field_x: m.magnetic_field.x 113 | magnetic_field_y: m.magnetic_field.y 114 | magnetic_field_z: m.magnetic_field.z 115 | 116 | sensor_msgs/NavSatFix: 117 | bag_time: t.to_sec() 118 | msg_time: m.header.stamp.to_sec() 119 | latitude: m.latitude 120 | longitude: m.longitude 121 | altitude: m.altitude 122 | fix_status: m.status.status 123 | 124 | sensor_msgs/Temperature: 125 | bag_time: t.to_sec() 126 | msg_time: m.header.stamp.to_sec() 127 | temperature: m.temperature 128 | variance: m.variance 129 | 130 | sensor_msgs/FluidPressure: 131 | bag_time: t.to_sec() 132 | msg_time: m.header.stamp.to_sec() 133 | fluid_pressure: m.fluid_pressure 134 | 135 | sensor_msgs/TimeReference: 136 | bag_time: t.to_sec() 137 | msg_time: m.header.stamp.to_sec() 138 | time_ref: m.time_ref.to_sec() 139 | 140 | sensor_msgs/Range: 141 | bag_time: t.to_sec() 142 | msg_time: m.header.stamp.to_sec() 143 | range: m.range 144 | 145 | sensor_msgs/Image: 146 | bag_time: t.to_sec() 147 | msg_time: m.header.stamp.to_sec() 148 | 149 | std_msgs/Duration: 150 | bag_time: t.to_sec() 151 | duration: m.data.to_sec() 152 | 153 | std_msgs/Float32: 154 | bag_time: t.to_sec() 155 | data: m.data 156 | 157 | std_msgs/Float64: 158 | bag_time: t.to_sec() 159 | data: m.data 160 | 161 | std_msgs/Float64MultiArray: 162 | bag_time: t.to_sec() 163 | 164 | std_msgs/Int8: 165 | bag_time: t.to_sec() 166 | data: m.data 167 | 168 | std_msgs/UInt8: 169 | bag_time: t.to_sec() 170 | data: m.data 171 | 172 | std_msgs/String: 173 | bag_time: t.to_sec() 174 | data: m.data 175 | 176 | tf2_msgs/TFMessage: 177 | bag_time: t.to_sec() 178 | 179 | ################## 180 | # Custom messages 181 | ################## 182 | seabed_drivers/DvlMeasurements: 183 | bag_time: t.to_sec() 184 | msg_time: m.header.stamp.to_sec() 185 | altitude: m.altitude 186 | ranges_0: m.ranges[0] 187 | ranges_1: m.ranges[1] 188 | rnages_2: m.ranges[2] 189 | ranges_3: m.ranges[3] 190 | btv_x: m.btv.x 191 | btv_y: m.btv.y 192 | btv_z: m.btv.z 193 | btv_e: m.btv_e 194 | orientation_quat_x: m.orientation.x 195 | orientation_quat_y: m.orientation.y 196 | orientation_quat_z: m.orientation.z 197 | orientation_quat_w: m.orientation.w 198 | temperature: m.temperature 199 | salinity: m.salinity 200 | depth: m.depth 201 | soundvel: m.soundvel 202 | 203 | seabed_drivers/ThrusterData: # t.to_sec(), m.prop_rpm, m.omega, m.voltage, m.current, m.temperature 204 | bag_time: t.to_sec() 205 | msg_time: m.stamp.to_sec() 206 | prop_rpm: m.prop_rpm 207 | omega: m.omega 208 | voltage: m.voltage 209 | current: m.current 210 | temperature: m.temperature 211 | 212 | # simplenav action msg 213 | 214 | simplenav/NavigatorActionGoal: 215 | bag_time: t.to_sec() 216 | msg_time: m.header.stamp.to_sec() 217 | goal_x: m.goal.goalPose.pose.position.x 218 | goal_y: m.goal.goalPose.pose.position.y 219 | goal_z: m.goal.goalPose.pose.position.z 220 | goal_quat_x: m.goal.goalPose.pose.orientation.x 221 | goal_quat_y: m.goal.goalPose.pose.orientation.y 222 | goal_quat_z: m.goal.goalPose.pose.orientation.z 223 | goal_quat_w: m.goal.goalPose.pose.orientation.w 224 | start_point.x: m.goal.refStartPoint.x 225 | start_point.y: m.goal.refStartPoint.y 226 | start_point.z: m.goal.refStartPoint.z 227 | use_start_point: m.goal.useStartPoint 228 | active_hovering: m.goal.activeHovering 229 | 230 | simplenav/NavigatorActionResult: 231 | bag_time: t.to_sec() 232 | msg_time: m.result.refPose.header.stamp.to_sec() 233 | reached_point_x: m.result.refPose.pose.position.x 234 | reached_point_y: m.result.refPose.pose.position.y 235 | reached_point_z: m.result.refPose.pose.position.z 236 | reached_point_quat_x: m.result.refPose.pose.orientation.x 237 | reached_point_quat_y: m.result.refPose.pose.orientation.y 238 | reached_point_quat_z: m.result.refPose.pose.orientation.z 239 | reached_point_quat_w: m.result.refPose.pose.orientation.w 240 | -------------------------------------------------------------------------------- /bag2mat/config/test_config.yaml: -------------------------------------------------------------------------------- 1 | #namespace: [frl_uas7, uas2, ''] # list of namespaces to search 2 | topic_list: 3 | #- [topic_name, message_type(as per dictionary), variable_name in mat file 4 | - [/gps/odom, nav_msgs/Odometry, gps_odom] 5 | - [/dvl/odom, nav_msgs/Odometry, dvl_odom] 6 | - [/imu/data, sensor_msgs/Imu, imu_data] 7 | - [/heading, std_msgs/Float64, heading] 8 | - [/roll, std_msgs/Float64, roll] 9 | - [/pitch, std_msgs/Float64, pitch] 10 | - [/imu/imu, sensor_msgs/Imu, imu_vn] 11 | - [/vehicle/gps/fix, sensor_msgs/NavSatFix, gps_car] 12 | -------------------------------------------------------------------------------- /bag2mat/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | bag2mat 4 | 1.0.0 5 | Convert rosbag to mat file 6 | Vikrant Shah 7 | Mithun Diddi 8 | Vikrant Shah 9 | MIT 10 | https://web.northeastern.edu/fieldrobotics 11 | catkin 12 | rosbag 13 | roslib 14 | nodelet 15 | rospkg 16 | 17 | 18 | -------------------------------------------------------------------------------- /bag2mat/scripts/bag2matpy: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | ''' 4 | MIT License 5 | 6 | Copyright (c) 2018 neufieldrobotics 7 | 8 | Permission is hereby granted, free of charge, to any person obtaining a copy 9 | of this software and associated documentation files (the "Software"), to deal 10 | in the Software without restriction, including without limitation the rights 11 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 12 | copies of the Software, and to permit persons to whom the Software is 13 | furnished to do so, subject to the following conditions: 14 | 15 | The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 16 | 17 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | SOFTWARE. 24 | 25 | @author: Vikrant Shah 26 | A simple script that can converts rosmessages to matlab mat files 27 | Define the translation of each message type in a dictionary file and use a 28 | config file to specify all the topics to be converted. 29 | ''' 30 | from __future__ import print_function 31 | 32 | import roslib 33 | import rospy 34 | import rostopic 35 | import rospkg 36 | import rosbag 37 | 38 | import argparse 39 | import importlib 40 | import sys 41 | import numpy as np 42 | import time 43 | from scipy.io import savemat 44 | import os 45 | import yaml 46 | import logging 47 | import copy 48 | import utm 49 | 50 | bglog = logging.getLogger("bag2matpy") 51 | logging.basicConfig(level=logging.INFO, format='%(levelname)8s %(message)s') 52 | 53 | 54 | def save_topic2dict(topic_name, msg_dict, bagfile, start_time,subtract_start_time, msg_type, convert_to_utm): 55 | bglog = logging.getLogger(__name__) 56 | expression = ",".join(msg_dict.values()) 57 | field_names = list(msg_dict.keys()) 58 | print(field_names) 59 | # append additional variables 60 | if subtract_start_time: 61 | field_names += ['offset_time'] 62 | if convert_to_utm and msg_type =='sensor_msgs/NavSatFix': 63 | field_names += ['easting', 'northing', 'zone', 'letter_int'] 64 | 65 | data_array = [] 66 | for topic, msg, ts in bagfile.read_messages(topics=[topic_name]): 67 | res = eval("{}".format(expression),{'np':np}, {'m': msg, 't':ts}) 68 | 69 | if subtract_start_time: 70 | offset_time = (ts.to_sec() - start_time,) # store_as_tuple 71 | res += offset_time 72 | # do utm conversion if gps 73 | if convert_to_utm and msg_type =='sensor_msgs/NavSatFix': 74 | utm_data = utm.from_latlon(msg.latitude, msg.longitude) 75 | # converting 'letter' in utm to its unicode value as fix to \ 76 | # scipy savemat's bug converting everything to char 77 | utm_data = utm_data[:-1] + (ord(utm_data[-1]),) 78 | res +=utm_data 79 | 80 | data_array.append(res) 81 | 82 | if data_array: 83 | data_dict = {} 84 | for field_name, column in zip(field_names, np.array(data_array).T): 85 | data_dict[field_name] = column[:,None] 86 | 87 | return data_dict 88 | else: 89 | bglog.warn("Notfound! Topic: "+topic_name) 90 | return None 91 | 92 | # get the file path for rospy_tutorials 93 | rospack = rospkg.RosPack() 94 | 95 | parser = argparse.ArgumentParser(formatter_class=argparse.RawTextHelpFormatter, 96 | description='Convert ROSBAG to mat file.\n\n' 97 | 'A simple script that can converts ros messages to matlab mat files.' 98 | '\nDefine required message type(s) in a dictionary file and ' 99 | 'specify list of topics to be converted in a config file.\n\n') 100 | 101 | parser.add_argument('-i','--input_bagfile', required=True, help='Input a rosbag file') 102 | parser.add_argument('-o','--output_dir', help='Output dir for mat file') 103 | parser.add_argument('-c','--config_file', help='Yaml file specifying topic names to convert', default ='../config/test_config.yaml') 104 | parser.add_argument('-d','--dictionary', help='Dictionary file which specifies how to read the topic', default ='../config/msg_dict.yaml') 105 | 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") 106 | parser.add_argument('-iutm','--ignore_ll2utm', default=False, action="store_true" , help="Boolean flag to specify whether to ignore converting lat long msg to utm, defaults to False") 107 | 108 | args = parser.parse_args() 109 | 110 | with open(args.dictionary, 'r') as f: 111 | exp_dic = yaml.safe_load(f) 112 | 113 | with open(args.config_file, 'r') as f: 114 | config = yaml.safe_load(f) 115 | 116 | if 'namespace' in config: 117 | ns_prefixes = config['namespace'] 118 | else: 119 | ns_prefixes = [''] 120 | 121 | subtract_start_time = args.subtract_start_time 122 | convert_to_utm = not args.ignore_ll2utm 123 | 124 | bag = rosbag.Bag(args.input_bagfile) 125 | 126 | output_mat_name = (os.path.basename(args.input_bagfile)).replace('.bag','.mat') 127 | 128 | if args.output_dir is None: 129 | output_mat_path = os.path.dirname(os.path.realpath(args.input_bagfile))+'/' 130 | else: 131 | output_mat_path = os.path.realpath(args.output_dir) +'/' 132 | 133 | output_mat = output_mat_path + output_mat_name 134 | 135 | if not os.path.exists(output_mat_path): 136 | os.makedirs(output_mat_path) 137 | bglog.warn("output_dir doesn't exists, creating required path "+output_mat) 138 | 139 | if os.path.isfile(output_mat): 140 | bglog.warn("File Already Exists, over-writing - "+output_mat) 141 | else: 142 | bglog.info("Writing to file - "+output_mat) 143 | 144 | with open(output_mat,'wb',0) as ofile: 145 | 146 | start_time = bag.get_start_time() 147 | savemat(ofile, {'start_time': start_time }) 148 | bag_topics = bag.get_type_and_topic_info().topics.keys() 149 | 150 | for ns_prefix in ns_prefixes: 151 | 152 | if any([topic.startswith('/'+ns_prefix) for topic in bag_topics]): 153 | ns_dict = {} 154 | for topic in config['topic_list']: 155 | if ns_prefix: 156 | topic_name = '/'+ns_prefix+topic[0] 157 | else: 158 | topic_name = topic[0] 159 | 160 | msg_type = topic[1] 161 | variable_name = topic[2] 162 | msg_dict = exp_dic[msg_type] 163 | 164 | data_dict = save_topic2dict(topic_name, msg_dict, bag, start_time, subtract_start_time, msg_type, convert_to_utm) 165 | 166 | if data_dict: 167 | ns_dict[variable_name] = copy.deepcopy(data_dict) 168 | 169 | if ns_dict: 170 | if ns_prefix: 171 | savemat(ofile, {ns_prefix: ns_dict}) 172 | else: 173 | savemat(ofile, {'robot': ns_dict}) 174 | bglog.info("Wrote namespace: "+ns_prefix+" \n") 175 | else: 176 | bglog.warn("Notfound! Namespace: "+ns_prefix+'\n') 177 | 178 | bag.close() 179 | -------------------------------------------------------------------------------- /bag2pandas/.gitignore: -------------------------------------------------------------------------------- 1 | 2 | sample_data/ 3 | 4 | # Byte-compiled / optimized / DLL files 5 | __pycache__/ 6 | *.py[cod] 7 | *$py.class 8 | 9 | # C extensions 10 | *.so 11 | 12 | # Distribution / packaging 13 | .Python 14 | build/ 15 | develop-eggs/ 16 | dist/ 17 | downloads/ 18 | eggs/ 19 | .eggs/ 20 | lib/ 21 | lib64/ 22 | parts/ 23 | sdist/ 24 | var/ 25 | wheels/ 26 | *.egg-info/ 27 | .installed.cfg 28 | *.egg 29 | MANIFEST 30 | 31 | # PyInstaller 32 | # Usually these files are written by a python script from a template 33 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 34 | *.manifest 35 | *.spec 36 | 37 | # Installer logs 38 | pip-log.txt 39 | pip-delete-this-directory.txt 40 | 41 | # Unit test / coverage reports 42 | htmlcov/ 43 | .tox/ 44 | .coverage 45 | .coverage.* 46 | .cache 47 | nosetests.xml 48 | coverage.xml 49 | *.cover 50 | .hypothesis/ 51 | .pytest_cache/ 52 | 53 | # Translations 54 | *.mo 55 | *.pot 56 | 57 | # Django stuff: 58 | *.log 59 | local_settings.py 60 | db.sqlite3 61 | 62 | # Flask stuff: 63 | instance/ 64 | .webassets-cache 65 | 66 | # Scrapy stuff: 67 | .scrapy 68 | 69 | # Sphinx documentation 70 | docs/_build/ 71 | 72 | # PyBuilder 73 | target/ 74 | 75 | # Jupyter Notebook 76 | .ipynb_checkpoints 77 | 78 | # pyenv 79 | .python-version 80 | 81 | # celery beat schedule file 82 | celerybeat-schedule 83 | 84 | # SageMath parsed files 85 | *.sage.py 86 | 87 | # Environments 88 | .env 89 | .venv 90 | env/ 91 | venv/ 92 | ENV/ 93 | env.bak/ 94 | venv.bak/ 95 | 96 | # Spyder project settings 97 | .spyderproject 98 | .spyproject 99 | 100 | # Rope project settings 101 | .ropeproject 102 | 103 | # mkdocs documentation 104 | /site 105 | 106 | # mypy 107 | .mypy_cache/ 108 | -------------------------------------------------------------------------------- /bag2pandas/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 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 | -------------------------------------------------------------------------------- /bag2pandas/README.md: -------------------------------------------------------------------------------- 1 | # bag2pandasdf 2 | Convert rosbags to pandas dataframes for easy data manipulation and plotting 3 | 4 | * Takes a rosbag file and converts it to a pndas dataframe pickle file based on settings specified in a Dictionary and a yaml config file. 5 | - Dictonary: The dicionary file specified which variables from each message type should be converted to a dataframe columns. 6 | eg `sensor_msgs/NavSatFix: t.to_sec(), m.latitude, m.longitude, m.altitude` 7 | See config/bag2mat_dic.yaml for full example. 8 | - Configuration: This file specified which topic names should be transferred to dataframes and under which variable name. 9 | A list of `-[topic name, message type, name for variable in matfile]` 10 | See config/bag2mat_config.yaml for full example. 11 | * Currently needs the following packages: 12 | - rosbag 13 | - pandas 14 | - numpy 15 | - pyyaml 16 | - tqdm 17 | 18 | * Usage 19 | ``` 20 | usage: bag2pandasdf.py [-h] -i INPUT_BAGFILE [-o OUTPUT_DIR] [-c CONFIG_FILE] 21 | [-d DICTIONARY] 22 | 23 | Convert ROSBAG to pandas dataframe pkl file. 24 | 25 | A simple script that can converts rosmessages in a rossbag to pandas dataframes in a .pkl file. 26 | Define the translation of each message type in a dictionary file and use a config file to specify all the topics to be converted.. 27 | 28 | arguments: 29 | -i INPUT_BAGFILE, --input_bagfile INPUT_BAGFILE 30 | Input rosbag file to input 31 | 32 | optional arguments: 33 | -h, --help show this help message and exit 34 | -o OUTPUT_DIR, --output_dir OUTPUT_DIR 35 | Output dir for pkl file 36 | -c CONFIG_FILE, --config_file CONFIG_FILE 37 | Yaml file which specifies topic names to convert 38 | -d DICTIONARY, --dictionary DICTIONARY 39 | Dictionary file which specifies how to read the topic 40 | ``` 41 | ## Output pkl file data structure 42 | The resulting output file while have a structure as shown below. This can be directly used with the [our_qtplot](https://github.com/neufieldrobotics/our_qt_plot) data plotting GUI. 43 | ![Alt text](https://g.gravizo.com/source/svg/input_ds?https%3A%2F%2Fraw.githubusercontent.com%2Fneufieldrobotics%2Fbag2pandasdf%2Fmaster%2FREADME.md) 44 |
45 | 46 | input_ds 47 | digraph G { 48 | "data_file.pkl" -> "full_data_dict"; 49 | "full_data_dict" -> "namespace1 dict" [color="orange"]; 50 | "full_data_dict" -> "namespace2 dict" [color="orange"]; 51 | n1t1 [label="topic1 pandas df"]; 52 | n1t2 [label="topic2 pandas df"]; 53 | n2t1 [label="topic1 pandas df"]; 54 | n2t2 [label="topic2 pandas df"]; 55 | "namespace1 dict" -> n1t1 [color="green"]; 56 | "namespace1 dict" -> n1t2 [color="green"]; 57 | "namespace2 dict" -> n2t1 [color="green"]; 58 | "namespace2 dict" -> n2t2 [color="green"]; 59 | n1t1t [label="Time\n(index)"]; 60 | n1t1f1 [label="Field1\ncolumn"]; 61 | n1t1f2 [label="Field2\ncolumn"]; 62 | n1t2t [label="Time\n(index)"]; 63 | n1t2f1 [label="Field1\ncolumn"]; 64 | n1t2f2 [label="Field2\ncolumn"]; 65 | n2t1t [label="Time\n(index)"]; 66 | n2t1f1 [label="Field1\ncolumn"]; 67 | n2t1f2 [label="Field2\ncolumn"]; 68 | n2t2t [label="Time\n(index)"]; 69 | n2t2f1 [label="Field1\ncolumn"]; 70 | n2t2f2 [label="Field2\ncolumn"]; 71 | n1t1 -> n1t1t [color="blue"]; 72 | n1t1 -> n1t1f1 [color="blue"]; 73 | n1t1 -> n1t1f2 [color="blue"]; 74 | n1t2 -> n1t2t [color="blue"]; 75 | n1t2 -> n1t2f1 [color="blue"]; 76 | n1t2 -> n1t2f2 [color="blue"]; 77 | n2t1 -> n2t1t [color="blue"]; 78 | n2t1 -> n2t1f1 [color="blue"]; 79 | n2t1 -> n2t1f2 [color="blue"]; 80 | n2t2 -> n2t2t [color="blue"]; 81 | n2t2 -> n2t2f1 [color="blue"]; 82 | n2t2 -> n2t2f2 [color="blue"]; 83 | } 84 | input_ds 85 |
86 | -------------------------------------------------------------------------------- /bag2pandas/bag2pandasdf.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | 4 | """ 5 | @author: Vikrant Shah 6 | A simple script that can converts rosmessages to pandas dataframe pkl 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 | 16 | import argparse 17 | #import importlib 18 | import sys 19 | import rospkg 20 | import rosbag 21 | import numpy as np 22 | #import time 23 | import sys 24 | import os 25 | import yaml 26 | #import logging 27 | import pandas as pd 28 | import pickle 29 | from tqdm import tqdm 30 | import time 31 | 32 | def save_topic2df(topic_name, msg_dict, bagfile): 33 | #bglog = logging.getLogger(__name__) 34 | expression = ",".join(msg_dict.values()) 35 | field_names = list(msg_dict.keys()) 36 | 37 | data_array = [] 38 | if bag.get_message_count(topic_name) > 0: 39 | for topic, msg, ts in tqdm(bagfile.read_messages(topics=[topic_name]), 40 | total = bag.get_message_count(topic_name), 41 | leave = False, miniters = 2500, unit = 'msgs', 42 | desc = topic_name, position = 0): 43 | res = eval("{}".format(expression),{'np':np}, {'m': msg, 't':ts}) 44 | data_array.append(res) 45 | 46 | if data_array: 47 | data_np = np.array(data_array) 48 | df = pd.DataFrame(data = data_np, columns = field_names) 49 | 50 | df['msg_time'] = pd.to_datetime(df['msg_time'], unit='s') 51 | 52 | if 'header_timestamp' in df.columns: 53 | df['header_timestamp'] = pd.to_datetime(df['header_timestamp'], unit='s') 54 | df.set_index('header_timestamp',inplace=True) 55 | 56 | else: 57 | df.set_index('msg_time',inplace=True) 58 | 59 | return df 60 | 61 | else: 62 | time.sleep(0.1) 63 | tqdm.write("Notfound! Topic: "+topic_name ) 64 | return None 65 | 66 | 67 | parser = argparse.ArgumentParser(formatter_class=argparse.RawTextHelpFormatter, 68 | description='Convert ROSBAG to pandas dataframe pkl file.\n\n' 69 | 'A simple script that can converts rosmessages in a rossbag to pandas dataframes in a .pkl file. \ 70 | \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' 71 | ) 72 | parser.add_argument('-i','--input_bagfile', help='Input rosbag file to input', required=True) 73 | parser.add_argument('-o','--output_dir', help='Output dir for pkl file') 74 | parser.add_argument('-c','--config_file', help='Yaml file which specifies topic names to convert', 75 | default = 'config/bag2pddf_config.yaml') 76 | parser.add_argument('-d','--dictionary', help='Dictionary file which specifies how to read the topic', 77 | default = 'config/bag2pddf_dict.yaml') 78 | 79 | args = parser.parse_args() 80 | 81 | with open(args.dictionary, 'r') as f: 82 | exp_dic = yaml.safe_load(f) 83 | 84 | with open(args.config_file, 'r') as f: 85 | config = yaml.safe_load(f) 86 | 87 | if 'namespace' in config: 88 | ns_prefixes = config['namespace'] 89 | else: 90 | ns_prefixes = [''] 91 | 92 | sys.stdout.write("Reading bagfile: "+args.input_bagfile+" ..") 93 | sys.stdout.flush() 94 | bag = rosbag.Bag(args.input_bagfile) 95 | print("... done") 96 | 97 | output_pkl_name = (os.path.basename(args.input_bagfile)).replace('.bag','.pkl') 98 | 99 | if args.output_dir is None: 100 | output_pkl_path = os.path.dirname(os.path.realpath(args.input_bagfile))+'/' 101 | else: 102 | output_pkl_path = os.path.realpath(args.output_dir) +'/' 103 | 104 | output_pkl = output_pkl_path + output_pkl_name 105 | 106 | if not os.path.exists(output_pkl_path): 107 | os.makedirs(output_pkl_path) 108 | tqdm.write("output_dir doesn't exists, creating required path "+output_pkl) 109 | 110 | 111 | if os.path.isfile(output_pkl): 112 | tqdm.write("File Already Exits, over-writing - "+output_pkl) 113 | else: 114 | tqdm.write("Writing to file - "+output_pkl) 115 | 116 | with open(output_pkl,'wb') as pickle_file: 117 | 118 | start_time = bag.get_start_time() 119 | bag_topics = bag.get_type_and_topic_info().topics.keys() 120 | for ns_prefix in ns_prefixes: 121 | 122 | if any([topic.startswith('/'+ns_prefix) for topic in bag_topics]): 123 | final_data_dict = {} 124 | ns_dict = {} 125 | for topic in tqdm(config['topic_list'], unit = 'topic', desc = ns_prefix, position = 1): 126 | if ns_prefix: 127 | topic_name = '/'+ns_prefix+topic[0] 128 | else: 129 | topic_name = topic[0] 130 | msg_type = topic[1] 131 | 132 | variable_name = topic[2] 133 | msg_dict = exp_dic[msg_type] 134 | 135 | data_df = save_topic2df(topic_name, msg_dict , bag) 136 | 137 | if data_df is not None: 138 | ns_dict[variable_name] = data_df 139 | 140 | if ns_dict: 141 | if ns_prefix: 142 | final_data_dict[ns_prefix] = ns_dict 143 | tqdm.write("Wrote namespace: "+ns_prefix+" ") 144 | 145 | else: 146 | final_data_dict['robot'] = ns_dict 147 | tqdm.write("Wrote namespace: robot ") 148 | else: 149 | tqdm.write("Notfound! Namespace: "+ns_prefix+'\t\t\t') 150 | 151 | sys.stdout.write("Writing picklefile: "+output_pkl+" ..") 152 | sys.stdout.flush() 153 | pickle.dump(final_data_dict, pickle_file) 154 | print("... done") 155 | 156 | bag.close() 157 | -------------------------------------------------------------------------------- /bag2pandas/config/bag2pddf_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 | -------------------------------------------------------------------------------- /bag2pandas/config/bag2pddf_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/Vector3Stamped: 27 | msg_time: t.to_sec() 28 | vector_x: m.vector.x 29 | vector_y: m.vector.y 30 | vector_z: m.vector.z 31 | 32 | nav_msgs/Odometry: 33 | msg_time: t.to_sec() 34 | position_x: m.pose.pose.position.x 35 | position_y: m.pose.pose.position.y 36 | position_z: m.pose.pose.position.z 37 | velocity_x: m.twist.twist.linear.x 38 | velocity_y: m.twist.twist.linear.y 39 | velocity_z: m.twist.twist.linear.z 40 | orientation_quat_x: m.pose.pose.orientation.x 41 | orientation_quat_y: m.pose.pose.orientation.y 42 | orientation_quat_z: m.pose.pose.orientation.z 43 | orientation_quat_w: m.pose.pose.orientation.w 44 | angular_velocity_x: m.twist.twist.angular.x 45 | angular_velocity_y: m.twist.twist.angular.y 46 | angular_velocity_z: m.twist.twist.angular.z 47 | 48 | sensor_msgs/BatteryState: 49 | msg_time: t.to_sec() 50 | voltage: m.voltage 51 | current: m.current 52 | charge: m.charge 53 | capacity: m.capacity 54 | design_capacity: m.design_capacity 55 | percentage: m.percentage 56 | 57 | sensor_msgs/Imu: 58 | msg_time: t.to_sec() 59 | header_timestamp: m.header.stamp.to_sec() 60 | orientation_quat_x: m.orientation.x 61 | orientation_quat_y: m.orientation.y 62 | orientation_quat_z: m.orientation.z 63 | orientation_quat_w: m.orientation.w 64 | acceleration_x: m.linear_acceleration.x 65 | acceleration_y: m.linear_acceleration.y 66 | acceleration_z: m.linear_acceleration.z 67 | angular_velocity_x: m.angular_velocity.x 68 | angular_velocity_y: m.angular_velocity.y 69 | angular_velocity_z: m.angular_velocity.z 70 | 71 | sensor_msgs/JointState: 72 | msg_time: t.to_sec() 73 | 74 | sensor_msgs/Joy_UAS: 75 | msg_time: t.to_sec() 76 | cmd_velocity_x: m.axes[0] 77 | cmd_velocity_y: m.axes[1] 78 | cmd_velocity_z: m.axes[2] 79 | cmd_yaw_rate: m.axes[3] 80 | config_code: m.axes[4] 81 | 82 | sensor_msgs/Joy_DJI_RC: 83 | msg_time: t.to_sec() 84 | cmd_roll: m.axes[0] 85 | cmd_pitch: m.axes[1] 86 | cmd_yaw_rate: m.axes[2] 87 | cmd_thrust: m.axes[3] 88 | cmd_mode: m.axes[4] 89 | cmd_landing_gear: m.axes[5] 90 | 91 | sensor_msgs/MagneticField: 92 | msg_time: t.to_sec() 93 | magnetic_field_x: m.magnetic_field.x 94 | magnetic_field_y: m.magnetic_field.y 95 | magnetic_field_z: m.magnetic_field.z 96 | 97 | sensor_msgs/NavSatFix: 98 | msg_time: t.to_sec() 99 | header_timestamp: m.header.stamp.to_sec() 100 | latitude: m.latitude 101 | longitude: m.longitude 102 | altitude: m.altitude 103 | 104 | sensor_msgs/Temperature: 105 | msg_time: t.to_sec() 106 | temperature: m.temperature 107 | 108 | sensor_msgs/TimeReference: 109 | msg_time: t.to_sec() 110 | header_timestamp: m.header.stamp.to_sec() 111 | time_ref: m.time_ref.to_sec() 112 | 113 | std_msgs/Duration: 114 | msg_time: t.to_sec() 115 | duration: m.data.to_sec() 116 | 117 | std_msgs/Float32: 118 | msg_time: t.to_sec() 119 | data: m.data 120 | 121 | std_msgs/Float64: 122 | msg_time: t.to_sec() 123 | data: m.data 124 | 125 | std_msgs/Float64MultiArray: 126 | msg_time: t.to_sec() 127 | 128 | std_msgs/Int8: 129 | msg_time: t.to_sec() 130 | data: m.data 131 | 132 | std_msgs/UInt8: 133 | msg_time: t.to_sec() 134 | data: m.data 135 | 136 | std_msgs/String: 137 | msg_time: t.to_sec() 138 | data: m.data 139 | 140 | tf2_msgs/TFMessage: 141 | msg_time: t.to_sec() 142 | 143 | ################## 144 | # Custom messages 145 | ################## 146 | seabed_drivers/DvlMeasurements: 147 | msg_time: t.to_sec() 148 | altitude: m.altitude 149 | ranges_0: m.ranges[0] 150 | ranges_1: m.ranges[1] 151 | rnages_2: m.ranges[2] 152 | ranges_3: m.ranges[3] 153 | btv_x: m.btv.x 154 | btv_y: m.btv.y 155 | btv_z: m.btv.z 156 | btv_e: m.btv_e 157 | orientation_quat_x: m.orientation.x 158 | orientation_quat_y: m.orientation.y 159 | orientation_quat_z: m.orientation.z 160 | orientation_quat_w: m.orientation.w 161 | temperature: m.temperature 162 | salinity: m.salinity 163 | depth: m.depth 164 | soundvel: m.soundvel 165 | 166 | seabed_drivers/ThrusterData: # t.to_sec(), m.prop_rpm, m.omega, m.voltage, m.current, m.temperature 167 | msg_time: t.to_sec() 168 | prop_rpm: m.prop_rpm 169 | omega: m.omega 170 | voltage: m.voltage 171 | current: m.current 172 | temperature: m.temperature 173 | -------------------------------------------------------------------------------- /bag2pandas/config/bag2pddf_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 | -------------------------------------------------------------------------------- /bag2pandas/config/bag2pddf_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 | -------------------------------------------------------------------------------- /bag_correction/bagTime2Timestamp.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Wed Jan 8 13:37:41 2020 5 | 6 | @author: Jagatpreet Singh 7 | This is a tool to write the actual time stamp in bag time 8 | """ 9 | 10 | import rospy 11 | from std_msgs.msg import String 12 | from sensor_msgs.msg import Image 13 | from sensor_msgs.msg import Imu 14 | import numpy as np 15 | import rosbag 16 | import os 17 | import argparse 18 | import yaml 19 | import time 20 | 21 | parser = argparse.ArgumentParser(formatter_class=argparse.RawTextHelpFormatter, 22 | description='Selects specific messages from a ROSBAG and saves them to a bag file.\ 23 | In case of images we can save image sto a fodler as well\n\n') 24 | parser.add_argument('-i','--input_bagfile', help='Input rosbag file to input', required=True) 25 | parser.add_argument('-o','--output_dir', help='Output dir for output bag file', default='.') 26 | parser.add_argument('-c','--config_file', help='Yaml file which specifies the topic names, frequency of selection and time range', 27 | default = 'config.yaml') 28 | 29 | args = parser.parse_args() 30 | 31 | with open(args.config_file, 'r') as f: 32 | config = yaml.safe_load(f) 33 | print(config) 34 | topics = config['topics'] 35 | read_bag_path = args.input_bagfile 36 | output_filename = config['output_bag_name'][0] 37 | write_folder_path = args.output_dir 38 | read_bag = rosbag.Bag(read_bag_path) 39 | start = rospy.Time(read_bag.get_start_time()) 40 | end = rospy.Time(read_bag.get_end_time()) 41 | 42 | # Create output rosbag 43 | filepath = os.path.join(write_folder_path,output_filename) 44 | output_bag = rosbag.Bag(filepath,'w') 45 | start_time = time.time() 46 | for topic, msg, t in read_bag.read_messages(topics=topics ,start_time= start , end_time= end): 47 | output_bag.write(topic,msg,t=msg.header.stamp) 48 | elapsed_time = time.time() - start_time 49 | print("elapsed time : %s" %(elapsed_time)) 50 | read_bag.close() 51 | output_bag.close() -------------------------------------------------------------------------------- /bag_correction/config.yaml: -------------------------------------------------------------------------------- 1 | topics: 2 | - /imu/imu 3 | output_bag_name: 4 | - imu_200_sync_20.bag 5 | start_time: 0 # in seconds 6 | end_time: 69 # in seconds 7 | 8 | -------------------------------------------------------------------------------- /bagmerge/bag_merge.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys 4 | import argparse 5 | from fnmatch import fnmatchcase 6 | 7 | from rosbag import Bag 8 | 9 | def main(): 10 | 11 | parser = argparse.ArgumentParser(description='Merge one or more bag files with the possibilities of filtering topics.') 12 | parser.add_argument('outputbag', 13 | help='output bag file with topics merged') 14 | parser.add_argument('inputbag', nargs='+', 15 | help='input bag files') 16 | parser.add_argument('-v', '--verbose', action="store_true", default=False, 17 | help='verbose output') 18 | parser.add_argument('-t', '--topics', default="*", 19 | help='string interpreted as a list of topics (wildcards \'*\' and \'?\' allowed) to include in the merged bag file') 20 | 21 | args = parser.parse_args() 22 | 23 | topics = args.topics.split(' ') 24 | 25 | total_included_count = 0 26 | total_skipped_count = 0 27 | 28 | if (args.verbose): 29 | print("Writing bag file: " + args.outputbag) 30 | print("Matching topics against patters: '%s'" % ' '.join(topics)) 31 | 32 | with Bag(args.outputbag, 'w') as o: 33 | for ifile in args.inputbag: 34 | matchedtopics = [] 35 | included_count = 0 36 | skipped_count = 0 37 | if (args.verbose): 38 | print("> Reading bag file: " + ifile) 39 | with Bag(ifile, 'r') as ib: 40 | for topic, msg, t in ib: 41 | if any(fnmatchcase(topic, pattern) for pattern in topics): 42 | if not topic in matchedtopics: 43 | matchedtopics.append(topic) 44 | if (args.verbose): 45 | print("Including matched topic '%s'" % topic) 46 | o.write(topic, msg, t) 47 | included_count += 1 48 | else: 49 | skipped_count += 1 50 | total_included_count += included_count 51 | total_skipped_count += skipped_count 52 | if (args.verbose): 53 | print("< Included %d messages and skipped %d" % (included_count, skipped_count)) 54 | 55 | if (args.verbose): 56 | print("Total: Included %d messages and skipped %d" % (total_included_count, total_skipped_count)) 57 | 58 | if __name__ == "__main__": 59 | main() 60 | -------------------------------------------------------------------------------- /bagselector/bag_selector.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | import cv2 3 | from std_msgs.msg import String 4 | from sensor_msgs.msg import Image 5 | from cv_bridge import CvBridge, CvBridgeError 6 | import message_filters 7 | import numpy as np 8 | import rosbag 9 | import os 10 | import argparse 11 | import yaml 12 | 13 | ''' 14 | @author: Pushyami Kaveti 15 | This is a tool to select only required images from a rosbag. 16 | The script goes through each image in the bag one by one and 17 | shows in a window, it has key bindings to select the image 18 | which will be written to another bag file. It uses a config 19 | file to specify various options for selection like topics, 20 | time range, frequency etc. 21 | ''' 22 | 23 | class BagSelector: 24 | def __init__(self,read_bag_path, write_folder_path, write_bag_path, save_imgs, config): 25 | self.viz=config['viz'] 26 | self.calib=config['calib'] 27 | self.write_bag = rosbag.Bag(os.path.join(write_folder_path,write_bag_path), 'w') 28 | self.write_folder_path = write_folder_path 29 | self.save_type = config['save_type'] 30 | self.bridge = CvBridge() 31 | self.topics = self.image_pub_topics = sorted(config['topics']) 32 | 33 | self.num_topics = len(self.topics) 34 | rospy.init_node('BagSelector', anonymous = True) 35 | self.image_pubs = [] 36 | for i in range(self.num_topics): 37 | pub = rospy.Publisher(self.image_pub_topics[i], Image, queue_size=1) 38 | self.image_pubs.append(pub) 39 | self.read_bag = rosbag.Bag(read_bag_path) 40 | self.start = rospy.Time(config['start_time'] + self.read_bag.get_start_time()) 41 | self.end = rospy.Time(config['end_time'] + self.read_bag.get_start_time()) 42 | self.frequency = config['frequency'] 43 | self.invert = config['invert'] 44 | self.gray = config['gray'] 45 | self.bag_to_bag() 46 | 47 | def reverse_contrast(self,input_img,option): 48 | 49 | #INPUT: 50 | # input_img : 8 bit raw_img in numpy format 51 | # option: 1 - using max 8 bit value 52 | # option: 2 - using bitwise_not(image) from opencv library 53 | # option: 3 - using ~ operator on the image 54 | 55 | #OUTPUT: 56 | # output_img: contrast reversed image: white becomes black and black becomes 57 | # white 58 | if option==1: 59 | output_img = (255-input_img) 60 | elif option==2: 61 | output_img = cv.bitwise_not(input_img) 62 | elif option==3: 63 | output_img = ~(input_img) 64 | return output_img 65 | 66 | def sort_topics_msgs(self,topic_list, msg_list): 67 | np_topic_list = np.array(topic_list) 68 | sorted_inds = np.argsort(np_topic_list) 69 | print(topic_list) 70 | print(sorted_inds) 71 | sorted_topiclist= [] 72 | sorted_msglist=[] 73 | for ind in sorted_inds: 74 | sorted_msglist.append(msg_list[ind]) 75 | sorted_topiclist.append(topic_list[ind]) 76 | return sorted_topiclist, sorted_msglist 77 | 78 | def bag_to_bag(self): 79 | oldt = 0; 80 | topiclist=[] 81 | msg_list=[] 82 | count = 0 83 | print(type(self.read_bag.get_start_time())) 84 | for topic, msg, t in self.read_bag.read_messages(topics=self.topics , start_time= self.start , end_time= self.end): 85 | if oldt == msg.header.stamp: 86 | topiclist.append(topic) 87 | msg_list.append(msg) 88 | #print(msg.header.frame_id) 89 | else: 90 | oldt = msg.header.stamp 91 | #sort the topic list and msgs 92 | topiclist, msg_list = self.sort_topics_msgs(topiclist, msg_list) 93 | if len(topiclist) == self.num_topics and topiclist==self.topics : 94 | if count % self.frequency == 0: 95 | print("Writing to bag:") 96 | print('count:' + str(count)) 97 | print(t) 98 | print(topiclist) 99 | cv_img_list=[] 100 | draw_img_list=[] 101 | gray_msg_list = [] 102 | for im_msg in msg_list: 103 | cv_image = self.bridge.imgmsg_to_cv2(im_msg, "bgr8") 104 | # Convert the image to gray scale 105 | if self.gray: 106 | cv_image = self.bridge.imgmsg_to_cv2(im_msg, "mono8") 107 | gray_msg = self.bridge.cv2_to_imgmsg(cv_image, encoding="mono8") 108 | gray_msg.header.stamp = im_msg.header.stamp 109 | gray_msg_list.append(gray_msg) 110 | # invert the image 111 | if self.invert: 112 | cv_image = self.reverse_contrast(cv_image, 1) 113 | draw_image = cv_image.copy() 114 | if self.calib: 115 | # Find the chess board corners 116 | ret, corners = cv2.findChessboardCorners(draw_image, (9,16),None) 117 | if ret == True: 118 | criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) 119 | corners2 = cv2.cornerSubPix(draw_image,corners,(11,11),(-1,-1),criteria) 120 | # Draw and display the corners 121 | draw_image = cv2.drawChessboardCorners(draw_image, (9,16), corners2,ret) 122 | draw_img_list.append(draw_image) 123 | cv_img_list.append(cv_image) 124 | if self.viz : 125 | vis = np.concatenate(draw_img_list, axis=1) 126 | height = 0 127 | width = 0 128 | if self.gray: 129 | height, width = vis.shape 130 | else: 131 | height, width,_ = vis.shape 132 | target_size = (int( width/1.5), int(height/1.5)) 133 | vis = cv2.resize(vis, target_size, interpolation =cv2.INTER_NEAREST) 134 | cv2.imshow("image", vis) 135 | key = cv2.waitKey(0) & 0xFF 136 | if key == ord('a') : 137 | for i in range(self.num_topics): 138 | if self.gray: 139 | self.write_bag.write(self.image_pub_topics[i], gray_msg_list[i], gray_msg_list[i].header.stamp) 140 | else: 141 | self.write_bag.write(self.image_pub_topics[i], msg_list[i], msg_list[i].header.stamp) 142 | elif key == ord('i') : 143 | for i in range(self.num_topics): 144 | print(os.path.join(self.write_folder_path,"cam"+str(i),str(oldt.to_nsec())+"."+self.save_type)) 145 | cv2.imwrite(os.path.join(self.write_folder_path,"cam"+str(i),str(oldt.to_nsec())+"."+self.save_type), cv_img_list[i]) 146 | elif key == 27: 147 | self.write_bag.close() 148 | return 149 | else: 150 | for i in range(self.num_topics): 151 | if self.gray: 152 | self.write_bag.write(self.image_pub_topics[i], gray_msg_list[i], gray_msg_list[i].header.stamp) 153 | else: 154 | self.write_bag.write(self.image_pub_topics[i], msg_list[i], msg_list[i].header.stamp) 155 | count = count + 1 156 | print("starting list\n") 157 | 158 | topiclist=[] 159 | msg_list=[] 160 | topiclist.append(topic) 161 | msg_list.append(msg) 162 | self.write_bag.close() 163 | 164 | 165 | if __name__=="__main__": 166 | parser = argparse.ArgumentParser(formatter_class=argparse.RawTextHelpFormatter, 167 | description='Selects specific messages from a ROSBAG and saves them to a bag file.\ 168 | In case of images we can save image sto a fodler as well\n\n') 169 | parser.add_argument('-i','--input_bagfile', help='Input rosbag file to input', required=True) 170 | parser.add_argument('-o','--output_dir', help='Output dir for output bag file', default='.') 171 | parser.add_argument('-of','--output_filename', help='Output file name for output bag file' , default = 'output.bag') 172 | parser.add_argument('-c','--config_file', help='Yaml file which specifies the topic names, frequency of selection and time range', 173 | default = 'config/config.yaml') 174 | parser.add_argument('-s','--saveimages', help='bool which specifies if images should be saved or rosbag', 175 | default = False) 176 | 177 | args = parser.parse_args() 178 | 179 | with open(args.config_file, 'r') as f: 180 | config = yaml.safe_load(f) 181 | print(config) 182 | bs = BagSelector(args.input_bagfile , args.output_dir , args.output_filename, args.saveimages, config) 183 | #bs = BagSelector("2019_Aug6_indoor_rig_calib_selected.bag" , "/home/auv/testing_bagselector", "") 184 | #/media/auv/Untitled/2019-08-13-00-09-48.bag 185 | -------------------------------------------------------------------------------- /bagselector/config/config.yaml: -------------------------------------------------------------------------------- 1 | topics: 2 | - /camera_array/cam5/image_raw 3 | frequency: 5 4 | viz : True 5 | start_time: 0 # in seconds 6 | end_time: 5000 # in seconds 7 | calib: True 8 | board_rows: 16 9 | board_cols: 9 10 | invert: False 11 | gray: True 12 | save_type: bmp 13 | 14 | -------------------------------------------------------------------------------- /bagselector/config/config_ir.yaml: -------------------------------------------------------------------------------- 1 | topics: 2 | - /boson_camera_array/cam0/image_raw 3 | - /boson_camera_array/cam1/image_raw 4 | - /boson_camera_array/cam2/image_raw 5 | - /boson_camera_array/cam3/image_raw 6 | - /boson_camera_array/cam4/image_raw 7 | frequency: 1 8 | viz : True 9 | invert: False 10 | start_time: 0 # in seconds 11 | end_time: 120 # in seconds 12 | 13 | -------------------------------------------------------------------------------- /bagselector/config/config_ir_old.yaml: -------------------------------------------------------------------------------- 1 | topics: 2 | - /boson_camera_array/cam_1/image_raw 3 | #- /boson_camera_array/cam_2/image_raw 4 | #- /boson_camera_array/cam_3/image_raw 5 | #- /boson_camera_array/cam_4/image_raw 6 | #- /boson_camera_array/cam_5/image_raw 7 | frequency: 4 8 | viz : True 9 | invert: False 10 | start_time: 0 # in seconds 11 | end_time: 120 # in seconds 12 | 13 | -------------------------------------------------------------------------------- /bagselector/config/config_zed.yaml: -------------------------------------------------------------------------------- 1 | topics: 2 | - /zed_node/depth/depth_registered_drop 3 | - /zed_node/left/image_rect_color_drop 4 | - /zed_node/right/image_rect_color_drop 5 | 6 | frequency: 3 7 | 8 | start_time: 0 # in seconds 9 | end_time: 10 # in seconds 10 | 11 | -------------------------------------------------------------------------------- /bagselector/insert_camerainfo_topics.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | import cv2 3 | from std_msgs.msg import String 4 | from sensor_msgs.msg import Image, CameraInfo 5 | from cv_bridge import CvBridge, CvBridgeError 6 | import message_filters 7 | import numpy as np 8 | import rosbag 9 | import os 10 | import argparse 11 | import yaml 12 | 13 | ''' 14 | @author: Pushyami Kaveti 15 | This is a tool to select only required images from a rosbag. 16 | The script goes through each image in the bag one by one and 17 | shows in a window, it has key bindings to select the image 18 | which will be written to another bag file. It uses a config 19 | file to specify various options for selection like topics, 20 | time range, frequency etc. 21 | ''' 22 | 23 | class BagSelector: 24 | def __init__(self,read_bag_path, write_folder_path, write_bag_path, save_imgs, config): 25 | self.write_bag = rosbag.Bag(os.path.join(write_folder_path,write_bag_path), 'w') 26 | self.write_folder_path = write_folder_path 27 | self.bridge = CvBridge() 28 | self.topics = self.image_pub_topics = config['topics'] 29 | self.info_topic_names = config['info_topics'] 30 | 31 | self.num_topics = len(self.topics) 32 | rospy.init_node('BagSelector', anonymous = True) 33 | self.read_bag = rosbag.Bag(read_bag_path) 34 | self.start = rospy.Time(config['start_time'] + self.read_bag.get_start_time()) 35 | self.end = rospy.Time(config['end_time'] + self.read_bag.get_start_time()) 36 | self.frequency = config['frequency'] 37 | self.bag_to_bag() 38 | 39 | 40 | def bag_to_bag(self): 41 | oldt = 0; 42 | topiclist=[] 43 | msg_list=[] 44 | info_msgs=[] 45 | count = 0 46 | print(type(self.read_bag.get_start_time())) 47 | for topic, msg, t in self.read_bag.read_messages(topics=self.topics , start_time= self.start , end_time= self.end): 48 | print(topic) 49 | print(msg.header.stamp) 50 | if oldt == msg.header.stamp: 51 | topiclist.append(topic) 52 | msg_list.append(msg) 53 | cam_info_msg = CameraInfo() 54 | cam_info_msg.header = msg.header 55 | info_msgs.append(cam_info_msg) 56 | #print(msg.header.frame_id) 57 | else: 58 | oldt = msg.header.stamp 59 | if len(topiclist) == self.num_topics and set(topiclist)==set(self.topics) : 60 | if count % self.frequency == 0: 61 | print("Writing to bag:") 62 | print('count:' + str(count)) 63 | print(t) 64 | for i in range(self.num_topics): 65 | self.write_bag.write(self.image_pub_topics[i], msg_list[i], msg_list[i].header.stamp) 66 | self.write_bag.write(self.info_topic_names[i], info_msgs[i], info_msgs[i].header.stamp) 67 | count = count + 1 68 | print("starting list\n") 69 | 70 | topiclist=[] 71 | msg_list=[] 72 | info_msgs=[] 73 | topiclist.append(topic) 74 | msg_list.append(msg) 75 | cam_info_msg = CameraInfo() 76 | cam_info_msg.header = msg.header 77 | info_msgs.append(cam_info_msg) 78 | self.write_bag.close() 79 | 80 | 81 | if __name__=="__main__": 82 | parser = argparse.ArgumentParser(formatter_class=argparse.RawTextHelpFormatter, 83 | description='Selects specific messages from a ROSBAG and saves them to a bag file.\ 84 | In case of images we can save image sto a fodler as well\n\n') 85 | parser.add_argument('-i','--input_bagfile', help='Input rosbag file to input', required=True) 86 | parser.add_argument('-o','--output_dir', help='Output dir for output bag file', default='.') 87 | parser.add_argument('-of','--output_filename', help='Output file name for output bag file' , default = 'output.bag') 88 | parser.add_argument('-c','--config_file', help='Yaml file which specifies the topic names, frequency of selection and time range', 89 | default = 'config/config.yaml') 90 | parser.add_argument('-s','--saveimages', help='bool which specifies if images should be saved or rosbag', 91 | default = False) 92 | 93 | args = parser.parse_args() 94 | 95 | with open(args.config_file, 'r') as f: 96 | config = yaml.safe_load(f) 97 | print(config) 98 | bs = BagSelector(args.input_bagfile , args.output_dir , args.output_filename, args.saveimages, config) 99 | #bs = BagSelector("2019_Aug6_indoor_rig_calib_selected.bag" , "/home/auv/testing_bagselector", "") 100 | #/media/auv/Untitled/2019-08-13-00-09-48.bag 101 | -------------------------------------------------------------------------------- /downsample_bags/config/config.yaml: -------------------------------------------------------------------------------- 1 | #todo: Currently the script does not use the topics from config.yaml. It is hardcoded. 2 | #camera_topics: 3 | topics: 4 | - /cam0/image_raw_bin1 5 | - /cam0/image_raw_bin2 6 | - /cam0/image_raw_bin4 7 | - /imu/imu 8 | #imu_topics: 9 | base_camera_frequency: 10 | - 160 11 | base_imu_frequency: 12 | - 400 13 | downsampled_camera_frequency: # in hz 14 | - 20 15 | - 40 16 | - 80 17 | downsampled_imu_frequency: 18 | - 400 19 | 20 | file_name: 21 | - vio_sync_exp_2000us_lab_3D 22 | 23 | # Enter start_time and end_time as 0 24 | # if complete bag is to be analysed 25 | 26 | #start_time: 0 # in seconds 27 | #end_time: 10 # in seconds 28 | 29 | -------------------------------------------------------------------------------- /downsample_bags/downsample_bags.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | from std_msgs.msg import String 3 | from sensor_msgs.msg import Image 4 | from sensor_msgs.msg import Imu 5 | import message_filters 6 | import numpy as np 7 | import rosbag 8 | import os 9 | import argparse 10 | import yaml 11 | import time 12 | 13 | ''' 14 | @author: Jagatpreet Singh 15 | This is a tool to downsample bags 16 | ''' 17 | 18 | class BagSelector: 19 | def __init__(self,read_bag_path, config): 20 | rospy.init_node('BagSelector', anonymous = True) 21 | self.topics = config['topics'] 22 | self.read_bag = rosbag.Bag(read_bag_path) 23 | self.base_imu_freq = config['base_imu_frequency'][0] 24 | self.base_camera_freq = config['base_camera_frequency'][0] 25 | #self.trim_time = config['trim_messages'][0]/self.base_imu_freq; 26 | self.start = rospy.Time(self.read_bag.get_start_time()) 27 | self.end = rospy.Time(self.read_bag.get_end_time()) 28 | 29 | 30 | def downsample_to(self,file_path,imu_rate,cam_rate): 31 | imu_msg_gap = self.base_imu_freq/imu_rate 32 | cam_msg_gap = self.base_camera_freq/cam_rate 33 | bag = rosbag.Bag(file_path,'w') 34 | 35 | print("Imu rate: %s" %(imu_rate)) 36 | print("Imu message gap: %s" %(imu_msg_gap)) 37 | 38 | print("Cam rate: %s" %(cam_rate)) 39 | print("cam message gap: %s" %(cam_msg_gap)) 40 | print('start time : %s' %(self.read_bag.get_start_time())) 41 | print('end time : %s' %(self.read_bag.get_end_time())) 42 | count = 0; 43 | curr_imu_num = 0; 44 | prev_imu_num = 0; 45 | curr_image_num = 0; 46 | prev_image_num = 0; 47 | start_time = time.time() 48 | #raw_input('Press enter') 49 | for topic, msg, t in self.read_bag.read_messages(topics=self.topics ,start_time= self.start , end_time= self.end): 50 | print(curr_imu_num) 51 | print(curr_image_num) 52 | if topic== '/imu/imu': 53 | if curr_imu_num == 0: 54 | bag.write(topic,msg,t=msg.header.stamp) 55 | if curr_imu_num - prev_imu_num == imu_msg_gap: 56 | bag.write(topic,msg,t=msg.header.stamp) 57 | prev_imu_num = curr_imu_num 58 | curr_imu_num += 1 59 | if topic =='/cam0/image_raw_bin1' or topic == '/cam0/image_raw_bin2' or topic=='/cam0/image_raw_bin4': 60 | if curr_image_num == 0: 61 | bag.write(topic,msg,t=msg.header.stamp) 62 | if curr_image_num - prev_image_num == cam_msg_gap: 63 | bag.write(topic,msg,t=msg.header.stamp) 64 | prev_image_num = curr_image_num 65 | curr_image_num += 1 66 | # print ('time : %s and end time: %s'%(t.to_sec(),self.start.to_sec())) 67 | count += 1 68 | bag.close() 69 | elapsed_time = time.time() - start_time 70 | print("elapsed time : %s" %(elapsed_time)) 71 | print("Total messsages: %s" %(count)) 72 | #raw_input('Press enter') 73 | 74 | 75 | if __name__=="__main__": 76 | parser = argparse.ArgumentParser(formatter_class=argparse.RawTextHelpFormatter, 77 | description='Selects specific messages from a ROSBAG and saves them to a bag file.\ 78 | In case of images we can save image sto a fodler as well\n\n') 79 | parser.add_argument('-i','--input_bagfile', help='Input rosbag file to input', required=True) 80 | parser.add_argument('-o','--output_dir', help='Output dir for output bag file', default='.') 81 | parser.add_argument('-c','--config_file', help='Yaml file which specifies the topic names, frequency of selection and time range', 82 | default = 'config/config.yaml') 83 | 84 | args = parser.parse_args() 85 | 86 | with open(args.config_file, 'r') as f: 87 | config = yaml.safe_load(f) 88 | print(config) 89 | bs = BagSelector(args.input_bagfile , config) 90 | write_folder_path = args.output_dir 91 | ds_cam_frequency = config['downsampled_camera_frequency'] 92 | ds_imu_frequency = config['downsampled_imu_frequency'] 93 | file_name = config['file_name'] 94 | for cam_freq in ds_cam_frequency: 95 | filename = '%s_imu_%s_cam_%s.bag'%(file_name[0],ds_imu_frequency[0],cam_freq) 96 | full_bag_pathname = os.path.join(write_folder_path,filename) 97 | bs.downsample_to(full_bag_pathname,ds_imu_frequency[0],cam_freq) 98 | bs.read_bag.close() 99 | -------------------------------------------------------------------------------- /image2bag/config/config.yaml: -------------------------------------------------------------------------------- 1 | topics: 2 | - /camera_array/cam0/image_raw 3 | - /camera_array/cam1/image_raw 4 | - /camera_array/cam2/image_raw 5 | - /camera_array/cam3/image_raw 6 | - /camera_array/cam4/image_raw 7 | 8 | info_topics: 9 | - /camera_array/cam0/camera_info 10 | - /camera_array/cam1/camera_info 11 | - /camera_array/cam2/camera_info 12 | - /camera_array/cam3/camera_info 13 | - /camera_array/cam4/camera_info 14 | 15 | img_type: bmp 16 | 17 | frequency: 4 18 | 19 | -------------------------------------------------------------------------------- /image2bag/config/config_ir.yaml: -------------------------------------------------------------------------------- 1 | topics: 2 | - /boson_camera_array/cam0/image_raw 3 | - /boson_camera_array/cam1/image_raw 4 | - /boson_camera_array/cam2/image_raw 5 | - /boson_camera_array/cam3/image_raw 6 | - /boson_camera_array/cam4/image_raw 7 | 8 | info_topics: 9 | - /boson_camera_array/cam0/camera_info 10 | - /boson_camera_array/cam1/camera_info 11 | - /boson_camera_array/cam2/camera_info 12 | - /boson_camera_array/cam3/camera_info 13 | - /boson_camera_array/cam4/camera_info 14 | 15 | img_type: jpg 16 | 17 | frequency: 4 18 | 19 | -------------------------------------------------------------------------------- /image2bag/config/config_single.yaml: -------------------------------------------------------------------------------- 1 | topics: 2 | - /camera_array/cam2/image_raw 3 | 4 | info_topics: 5 | - /camera_array/cam2/camera_info 6 | 7 | img_type: jpg 8 | 9 | frequency: 0 10 | 11 | -------------------------------------------------------------------------------- /image2bag/image2bag.py: -------------------------------------------------------------------------------- 1 | import rosbag 2 | import roslib 3 | import sys 4 | import rospy 5 | import cv2 6 | from std_msgs.msg import String 7 | from sensor_msgs.msg import Image, CameraInfo 8 | from cv_bridge import CvBridge, CvBridgeError 9 | import std_msgs 10 | import os 11 | import glob 12 | import numpy as np 13 | import argparse 14 | import yaml 15 | 16 | 17 | ''' 18 | @author: Pushyami Kaveti 19 | This is a tool to convert images into a bag files.Each directory representss a topic. 20 | It uses a config file to specify various options for selection like topics, 21 | time range, frequency etc. 22 | ''' 23 | 24 | class Image2Bag: 25 | def __init__(self,topic_n, info_topics, freq, op_file, data_p="/home/auv/calib" ,img_t="bmp"): 26 | rospy.init_node('image_converter', anonymous=True) 27 | self.data_path=data_p 28 | self.topic_names = topic_n 29 | self.info_topic_names = info_topics 30 | self.num_topics = len(topic_n) 31 | self.img_type = img_t 32 | self.pubs = [] 33 | self.cam_info_pubs = [] 34 | self.im_list = [] 35 | self.bridge = CvBridge() 36 | self.init_publishers() 37 | self.get_imgs() 38 | self.to_bag= True 39 | if self.to_bag : 40 | self.write_bag = rosbag.Bag(op_file, 'w') 41 | self.frequency = freq 42 | 43 | def init_publishers(self): 44 | for top_ind in range(self.num_topics): 45 | image_pub = rospy.Publisher(self.topic_names[top_ind], Image, queue_size=1) 46 | cam_info_pub = rospy.Publisher(self.info_topic_names[top_ind], CameraInfo, queue_size=1) 47 | self.pubs.append(image_pub) 48 | self.cam_info_pubs.append(cam_info_pub) 49 | 50 | def get_imgs(self): 51 | # check all cam directories has the same number f images 52 | # check the number of diretories is equal to number of topics publishing 53 | 54 | self.dirs = [ name for name in os.listdir(self.data_path) if os.path.isdir(os.path.join(self.data_path, name)) ] 55 | self.dirs.sort() 56 | print(self.dirs) 57 | if(len(self.dirs) != self.num_topics): 58 | print("ERROR: number of image directories is not equal to number of topics") 59 | exit() 60 | self.im_list = os.listdir(os.path.join(self.data_path,self.dirs[0])) #glob.glob(os.path.join(self.data_path ,"cam*/*."+self.img_type)) 61 | self.im_list.sort() 62 | self.num_imgs = len(self.im_list) 63 | #print(self.im_list) 64 | for i in range(len(self.dirs)): 65 | assert len(os.listdir(os.path.join(self.data_path,self.dirs[i]))) == self.num_imgs 66 | 67 | def run(self): 68 | if self.frequency == 0: 69 | print("Use the time stamp") 70 | delT = (int(self.im_list[1][:-4])-int(self.im_list[0][:-4])) *1e-9 71 | r = rospy.Rate(1/delT) # 10hz 72 | else: 73 | print("use the specified frequency to write the ros bag") 74 | r = rospy.Rate(self.frequency) # 10hz 75 | i=0 76 | print(self.num_imgs) 77 | while not rospy.is_shutdown(): 78 | if ( i < self.num_imgs): 79 | h = std_msgs.msg.Header() 80 | if self.frequency == 0: 81 | floatStamp = int(self.im_list[i][:-4])*1e-9 82 | h.stamp = rospy.Time.from_sec(floatStamp) 83 | else: 84 | h.stamp = rospy.Time.now() 85 | h.seq = i 86 | print("--------------------------------") 87 | imgs = [] 88 | info_msgs = [] 89 | for j in range(self.num_topics): 90 | img = cv2.imread(os.path.join(self.data_path,self.dirs[j],self.im_list[i]), cv2.IMREAD_COLOR) 91 | print(os.path.join(self.data_path,self.dirs[j],self.im_list[i])) 92 | ros_img = self.bridge.cv2_to_imgmsg(img, "bgr8") 93 | ros_img.header = h 94 | imgs.append(ros_img) 95 | cam_info_msg = CameraInfo() 96 | cam_info_msg.header = h 97 | info_msgs.append(cam_info_msg) 98 | for k in range(self.num_topics): 99 | if self.to_bag: 100 | print("writing"+str(i)) 101 | self.write_bag.write(self.topic_names[k], imgs[k], imgs[k].header.stamp) 102 | self.write_bag.write(self.info_topic_names[k], info_msgs[k], info_msgs[k].header.stamp) 103 | else: 104 | self.pubs[k].publish(imgs[k]) 105 | self.cam_info_pubs[k].publish(info_msgs[k]) 106 | i = i+1 107 | r.sleep() 108 | else: 109 | print(i) 110 | break 111 | r.sleep() 112 | if self.to_bag: 113 | self.write_bag.close() 114 | 115 | if __name__=="__main__": 116 | parser = argparse.ArgumentParser(formatter_class=argparse.RawTextHelpFormatter, 117 | description='Reads the images from directories in the path and \ 118 | saves them to individual topics in a bag file.\n\n') 119 | parser.add_argument('-i','--input_dir', help='Input directory path containing images', required=True) 120 | parser.add_argument('-o','--output_dir', help='Output dir for output bag file', default='.') 121 | parser.add_argument('-of','--output_filename', help='Output file name for output bag file' , default = 'output.bag') 122 | parser.add_argument('-c','--config_file', help='Yaml file which specifies the topic names, frequency of selection and time range', 123 | default = 'config/config.yaml') 124 | 125 | args = parser.parse_args() 126 | 127 | with open(args.config_file, 'r') as f: 128 | config = yaml.safe_load(f) 129 | op_file = os.path.join(args.output_dir, args.output_filename) 130 | bs = Image2Bag(config['topics'], config['info_topics'], config['frequency'], op_file, args.input_dir , config['img_type']) 131 | bs.run() 132 | -------------------------------------------------------------------------------- /image_resize/resize_image.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Wed Aug 12 11:01:22 2020 5 | 6 | @author: jagat 7 | """ 8 | 9 | import cv2 as cv 10 | import os 11 | 12 | sourcepath = '/home/jagat/datasets/vio_rosbags/vio_rig/vio_motion_data_cam17197547_ap_f3_exp_2000us_right_shelf_data_3D_1/cam0' 13 | 14 | outputpath = '/home/jagat/datasets/vio_rosbags/vio_rig/vio_motion_data_cam17197547_ap_f3_exp_2000us_right_shelf_data_3D_1/cam0_640x512' 15 | 16 | onlyfiles = [f for f in os.listdir(sourcepath) if os.path.isfile(os.path.join(sourcepath, f))] 17 | 18 | for filename in onlyfiles: 19 | filepath = os.path.join(sourcepath,filename) 20 | inputimg = cv.imread(filepath,0); 21 | outputimg = cv.resize(inputimg,(640,512)) 22 | cv.imwrite(os.path.join(outputpath,filename),outputimg) 23 | 24 | --------------------------------------------------------------------------------