├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── definitions.py ├── examples ├── nclt_all_kml.png ├── nclt_all_png.png ├── nclt_tf_tree.png └── rviz_example.gif ├── launch ├── convert.launch ├── download.launch └── visualize.launch ├── nclt2ros ├── __init__.py ├── converter │ ├── __init__.py │ ├── base_convert.py │ └── to_rosbag.py ├── downloader │ ├── __init__.py │ └── download.py ├── extractor │ ├── __init__.py │ ├── base_raw_data.py │ ├── extract.py │ ├── read_ground_truth.py │ ├── read_ground_truth_covariance.py │ └── read_sensor_data.py ├── transformer │ ├── __init__.py │ ├── coordinate_frame.py │ ├── hokuyo_data.py │ ├── image_data.py │ ├── sensor_data.py │ ├── velodyne_data.py │ └── velodyne_sync_data.py └── visualizer │ ├── __init__.py │ ├── all.py │ ├── gps.py │ ├── gps_rtk.py │ ├── gt.py │ ├── plotter.py │ ├── visualize.py │ └── wheel_odom.py ├── package.xml ├── requirements.txt ├── rviz └── config.rviz ├── scripts ├── nclt2downloader ├── nclt2rosbag └── nclt2visualizer └── setup.py /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | *.egg-info/ 24 | .installed.cfg 25 | *.egg 26 | MANIFEST 27 | 28 | # PyInstaller 29 | # Usually these files are written by a python script from a template 30 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 31 | *.manifest 32 | *.spec 33 | 34 | # Installer logs 35 | pip-log.txt 36 | pip-delete-this-directory.txt 37 | 38 | # Unit test / coverage reports 39 | htmlcov/ 40 | .tox/ 41 | .coverage 42 | .coverage.* 43 | .cache 44 | nosetests.xml 45 | coverage.xml 46 | *.cover 47 | .hypothesis/ 48 | .pytest_cache/ 49 | 50 | # Translations 51 | *.mo 52 | *.pot 53 | 54 | # Django stuff: 55 | *.log 56 | local_settings.py 57 | db.sqlite3 58 | 59 | # Flask stuff: 60 | instance/ 61 | .webassets-cache 62 | 63 | # Scrapy stuff: 64 | .scrapy 65 | 66 | # Sphinx documentation 67 | docs/_build/ 68 | 69 | # PyBuilder 70 | target/ 71 | 72 | # Jupyter Notebook 73 | .ipynb_checkpoints 74 | 75 | # pyenv 76 | .python-version 77 | 78 | # celery beat schedule file 79 | celerybeat-schedule 80 | 81 | # SageMath parsed files 82 | *.sage.py 83 | 84 | # Environments 85 | .env 86 | .venv 87 | env/ 88 | venv/ 89 | ENV/ 90 | env.bak/ 91 | venv.bak/ 92 | 93 | # Spyder project settings 94 | .spyderproject 95 | .spyproject 96 | 97 | # Rope project settings 98 | .ropeproject 99 | 100 | # mkdocs documentation 101 | /site 102 | 103 | # mypy 104 | .mypy_cache/ 105 | 106 | #project 107 | .idea/ 108 | raw_data/ 109 | plots/ -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(nclt2ros) 3 | 4 | 5 | find_package(catkin REQUIRED COMPONENTS roscpp rospy) 6 | 7 | catkin_python_setup() 8 | 9 | catkin_package() 10 | 11 | install(PROGRAMS scripts/nclt2downloader scripts/nclt2rosbag scripts/nclt2visualizer 12 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 13 | 14 | install(DIRECTORY launch 15 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 16 | 17 | # install rviz directory -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 Bierschneider Christian 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 | 2 | # NCLT2ROS 3 | 4 | The [nclt2ros](https://github.com/bierschi/nclt2rosbag) package provides ROS nodes for 5 | 6 | - [downloading](https://github.com/bierschi/nclt2rosbag#downloadlaunch) 7 | - [extracting](https://github.com/bierschi/nclt2rosbag#downloadlaunch) 8 | - [visualizing](https://github.com/bierschi/nclt2rosbag#visualizelaunch) 9 | - [converting](https://github.com/bierschi/nclt2rosbag#convertlaunch) 10 | 11 | the data from [The University of Michigan North Campus Long-Term Vision and LIDAR Dataset.](http://robots.engin.umich.edu/nclt/) 12 | 13 | 14 | ![](examples/rviz_example.gif) 15 | 16 | 17 | #### Table of contents: 18 | 19 | - [Launch files](https://github.com/bierschi/nclt2ros#launch-files) 20 | - [Usage](https://github.com/bierschi/nclt2ros#usage) 21 | - [Examples](https://github.com/bierschi/nclt2ros#examples) 22 | - [Transformation tree](https://github.com/bierschi/nclt2ros#transformation-tree) 23 | 24 | 25 | 26 | 27 | ## Launch files 28 | 29 | ##### download.launch 30 | launch file for downloading the raw data 31 | 32 | ```xml 33 | 34 | # download 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | ``` 50 | 51 | downloads the `sen` (sensor data), `gt` (ground truth) and `gt_cov` (ground truth covariance) from date `2013-01-10` in folder `/home/christian/nclt2ros/raw_data` 52 | 53 | 54 | ##### visualize.launch 55 | launch file for visualizing specific data 56 | 57 | ```xml 58 | 59 | # visualize 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | ``` 81 | 82 | visualizes the `gt_png`, `gps_rtk_kml`, `gps_rtk_png` and `odom_png` from date `2013-01-10` with raw data in `/home/christian/nclt2ros/raw_data` and places the plots in `/home/christian/nclt2ros/plots` 83 | 84 | ##### convert.launch 85 | launch file for converting the raw data to rosbag files 86 | 87 | ```xml 88 | 89 | # download 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | # specify data to convert 99 | 100 | 101 | 102 | 103 | 104 | 105 | # topic names 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | # frame ids 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | ``` 136 | 137 | 138 | ## Usage 139 | 140 | 141 | #### building from source 142 | 143 | create a catkin workspace 144 | 

145 | mkdir -p ~/catkin_ws/src

146 | cd ~/catkin_ws/

147 | catkin_make 

148 | source devel/setup.bash
149 |
150 | 151 | download and build this repository 152 | 

153 | cd src 

154 | git clone https://github.com/bierschi/nclt2ros.git 

155 | cd ~/catkin_ws/ 

156 | catkin_make
157 |
158 | 159 | source the catkin workspace to access the package nclt2ros 160 | 

161 | source devel/setup.bash
162 |
163 | 164 | execute specific roslaunch file 165 | 

166 | roslaunch nclt2ros download.launch
167 |
168 | 169 | 170 | 171 | ## Examples 172 | 173 | #### data from date 2013-01-10 174 | visualizing as a png file 175 | 176 |
177 |
178 | example 179 |
180 | 181 |
182 | visualizing as a kml file: 183 | 184 |
185 |
186 | example 187 |
188 | 189 | #### data from date 2012-01-08 190 | 191 | 192 | #### rosbag file 193 | 194 | 195 | ## Transformation Tree 196 | for self defined transformation tree, change `frame_id` and `topic_name` in launch file `convert.launch` 197 | 198 |
199 |
200 | example 201 |
-------------------------------------------------------------------------------- /definitions.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | ROOT_DIR = os.path.dirname(os.path.abspath(__file__)) 4 | -------------------------------------------------------------------------------- /examples/nclt_all_kml.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bierschi/nclt2ros/77b30ca6750d4b0cd82ccb6660f2fd0946581091/examples/nclt_all_kml.png -------------------------------------------------------------------------------- /examples/nclt_all_png.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bierschi/nclt2ros/77b30ca6750d4b0cd82ccb6660f2fd0946581091/examples/nclt_all_png.png -------------------------------------------------------------------------------- /examples/nclt_tf_tree.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bierschi/nclt2ros/77b30ca6750d4b0cd82ccb6660f2fd0946581091/examples/nclt_tf_tree.png -------------------------------------------------------------------------------- /examples/rviz_example.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bierschi/nclt2ros/77b30ca6750d4b0cd82ccb6660f2fd0946581091/examples/rviz_example.gif -------------------------------------------------------------------------------- /launch/convert.launch: -------------------------------------------------------------------------------- 1 | 2 | # convert 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | # specify data to convert 12 | 13 | 14 | 15 | 16 | 17 | 18 | # topic names 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | # frame ids 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /launch/download.launch: -------------------------------------------------------------------------------- 1 | 2 | # download 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /launch/visualize.launch: -------------------------------------------------------------------------------- 1 | 2 | # visualize 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /nclt2ros/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bierschi/nclt2ros/77b30ca6750d4b0cd82ccb6660f2fd0946581091/nclt2ros/__init__.py -------------------------------------------------------------------------------- /nclt2ros/converter/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bierschi/nclt2ros/77b30ca6750d4b0cd82ccb6660f2fd0946581091/nclt2ros/converter/__init__.py -------------------------------------------------------------------------------- /nclt2ros/converter/base_convert.py: -------------------------------------------------------------------------------- 1 | import os 2 | import rospy 3 | from definitions import ROOT_DIR 4 | 5 | 6 | class BaseConvert: 7 | """Base Class for converting 8 | 9 | USAGE: 10 | BaseConvert(date='2013-01-10') 11 | 12 | """ 13 | def __init__(self, date): 14 | 15 | self.date = date 16 | 17 | # create rosbag directory 18 | ROSBAG_PATH_DFLT = ROOT_DIR + '/nclt2ros/rosbags/' 19 | self.rosbag_path = rospy.get_param('~rosbag_output_path', ROSBAG_PATH_DFLT) 20 | 21 | if self.rosbag_path.endswith('/'): 22 | self.rosbag_dir = self.rosbag_path + str(self.date) 23 | else: 24 | self.rosbag_dir = self.rosbag_path + '/' + str(self.date) 25 | 26 | if not os.path.exists(self.rosbag_dir): 27 | os.makedirs(self.rosbag_dir) 28 | 29 | # create camera folder settings 30 | self.num_cameras = 6 31 | 32 | # init topic names 33 | self.gps_fix_topic = rospy.get_param('~gps_fix', '/navsat/fix') 34 | self.gps_track_topic = rospy.get_param('~gps_track', '/navsat/track') 35 | self.gps_speed_topic = rospy.get_param('~gps_speed', '/navsat/speed') 36 | self.gps_rtk_fix_topic = rospy.get_param('~gps_rtk_fix', '/navsat/rtk/fix') 37 | self.gps_rtk_track_topic = rospy.get_param('~gps_rtk_track', '/navsat/rtk/track') 38 | self.gps_rtk_speed_topic = rospy.get_param('~gps_rtk_speed', '/navsat/rtk/speed') 39 | self.imu_data_topic = rospy.get_param('~ms25_imu_data', '/imu/data') 40 | self.imu_mag_topic = rospy.get_param('~ms25_imu_mag', '/imu/mag') 41 | self.odom_topic = rospy.get_param('~wheel_odometry_topic', '/odom') 42 | self.hokuyo_utm_topic = rospy.get_param('~hokuyo_utm_topic', '/hokuyo_30m') 43 | self.hokuyo_urg_topic = rospy.get_param('~hokuyo_urg_topic', '/hokuyo_4m') 44 | self.velodyne_topic = rospy.get_param('~velodyne_topic', '/velodyne_points') 45 | self.ladybug_topic = rospy.get_param('~ladybug_topic', '/images/raw') 46 | self.ground_truth_topic = rospy.get_param('~ground_truth_topic', '/ground_truth') 47 | 48 | # init frame ids 49 | self.gps_frame = rospy.get_param('~gps_sensor', 'gps_link') 50 | self.gps_rtk_frame = rospy.get_param('~gps_rtk_sensor', 'gps_rtk_link') 51 | self.imu_frame = rospy.get_param('~imu_sensor', 'imu_link') 52 | self.odom_frame = rospy.get_param('~wheel_odometry', 'odom_link') 53 | self.hok_utm_frame = rospy.get_param('~hokuyo_utm_lidar', 'laser_utm') 54 | self.hok_urg_frame = rospy.get_param('~hokuyo_urg_lidar', 'laser_urg') 55 | self.velodyne_frame = rospy.get_param('~velodyne_lidar', 'velodyne') 56 | self.ladybug_frame = rospy.get_param('~ladybug_sensor', 'camera') 57 | self.ground_truth_frame = rospy.get_param('~ground_truth', 'ground_truth_link') 58 | self.body_frame = rospy.get_param('~body', 'base_link') 59 | -------------------------------------------------------------------------------- /nclt2ros/converter/to_rosbag.py: -------------------------------------------------------------------------------- 1 | import rosbag 2 | import os 3 | import subprocess 4 | import sys 5 | import rospy 6 | 7 | from nclt2ros.extractor.read_ground_truth import ReadGroundTruth 8 | from nclt2ros.extractor.read_ground_truth_covariance import ReadGroundTruthCovariance 9 | from nclt2ros.extractor.read_sensor_data import ReadSensorData 10 | from nclt2ros.transformer.hokuyo_data import HokuyoData 11 | from nclt2ros.transformer.velodyne_sync_data import VelodyneSyncData 12 | from nclt2ros.transformer.image_data import ImageData 13 | from nclt2ros.extractor.base_raw_data import BaseRawData 14 | from nclt2ros.transformer.sensor_data import RosSensorMsg 15 | from nclt2ros.converter.base_convert import BaseConvert 16 | 17 | 18 | class ToRosbag(BaseRawData, BaseConvert): 19 | """Class to convert the NCLT Dataset into a rosbag file 20 | 21 | USAGE: 22 | ToRosbag('2013-01-10', 'example.bag', cam_folder=None) 23 | 24 | """ 25 | def __init__(self, date, bag_name, cam_folder, gt, sen, hokuyo, vel, lb3): 26 | 27 | if isinstance(date, str): 28 | self.date = date 29 | else: 30 | raise TypeError('"date" must be of type string') 31 | 32 | self.gt = gt 33 | self.sen = sen 34 | self.hokuyo = hokuyo 35 | self.vel = vel 36 | self.lb3 = lb3 37 | 38 | # init base classes 39 | BaseRawData.__init__(self, date=self.date) 40 | BaseConvert.__init__(self, date=self.date) 41 | 42 | # load class instances, if available 43 | if self.ground_truth_flag and self.gt: 44 | self.gt = ReadGroundTruth(self.date) 45 | if self.ground_truth_covariance_flag: 46 | self.gt_cov = ReadGroundTruthCovariance(self.date) 47 | else: 48 | raise TypeError('No ground_truth_covariance directory available, please check!') 49 | 50 | if self.sensor_data_flag and self.sen: 51 | self.raw_data = ReadSensorData(self.date) 52 | 53 | if self.hokuyo_data_flag and self.hokuyo: 54 | self.hokuyo_data = HokuyoData(self.date) 55 | 56 | if self.velodyne_data_flag and self.vel: 57 | self.velodyne_sync_data = VelodyneSyncData(self.date) 58 | 59 | if self.images_flag and self.lb3: 60 | 61 | # create instance 62 | self.image_data = ImageData(self.date) 63 | 64 | # check cam_folder argument 65 | if cam_folder is None: 66 | self.cam_folder = 5 67 | else: 68 | self.cam_folder = cam_folder 69 | else: 70 | self.cam_folder = None 71 | 72 | # load ros msg converter class 73 | self.ros_sensor_msg = RosSensorMsg(self.date) 74 | 75 | # create rosbag file and check name 76 | self.bag_name = str(bag_name) 77 | 78 | if not self.bag_name.endswith('.bag'): 79 | self.bag_name = self.bag_name + '.bag' 80 | 81 | os.chdir(self.rosbag_dir) 82 | # check if file already exists 83 | if os.path.isfile(self.rosbag_dir + '/' + self.bag_name): 84 | rospy.loginfo("rosbag file %s already exists" % self.bag_name) 85 | self.bag_name = self.bag_name.split('.')[0] + '_2' + '.bag' 86 | 87 | # create rosbag file 88 | self.bag = rosbag.Bag(self.bag_name, 'w') 89 | 90 | def __del__(self): 91 | """destructor 92 | """ 93 | rospy.loginfo("close rosbag file") 94 | self.bag.close() 95 | 96 | def process(self): 97 | """loads and converts the data into a rosbag file 98 | 99 | """ 100 | # init counter with 0 101 | i_gt = 0 102 | i_gps = 0 103 | i_gps_rtk = 0 104 | i_ms25 = 0 105 | i_odom = 0 106 | i_vel = 0 107 | i_img = 0 108 | 109 | rospy.loginfo("loading data ...") 110 | 111 | # load ground_truth data 112 | if self.gt: 113 | gt_list = self.gt.read_gt_csv(all_in_one=True) 114 | gt_cov_list = self.gt_cov.read_gt_cov_csv(all_in_one=True) 115 | 116 | # load sensor data 117 | if self.sen: 118 | gps_list = self.raw_data.read_gps_csv(all_in_one=True) 119 | gps_rtk_list = self.raw_data.read_gps_rtk_csv(all_in_one=True) 120 | ms25_list = self.raw_data.read_ms25_csv(all_in_one=True) 121 | odom_list = self.raw_data.read_odometry_mu_100hz_csv(all_in_one=True) 122 | odom_cov_list = self.raw_data.read_odometry_cov_100hz_csv(all_in_one=True) 123 | wheels_list = self.raw_data.read_wheels_csv(all_in_one=True) 124 | kvh_list = self.raw_data.read_kvh_csv(all_in_one=True) 125 | 126 | # load hokuyo data 127 | if self.hokuyo: 128 | utime_hok4, data_hok4 = self.hokuyo_data.read_next_hokuyo_4m_packet() 129 | utime_hok30, data_hok30 = self.hokuyo_data.read_next_hokuyo_30m_packet() 130 | 131 | # load velodyne sync data 132 | if self.vel: 133 | vel_sync_timestamps_microsec, vel_sync_bin_files = self.velodyne_sync_data.get_velodyne_sync_timestamps_and_files() 134 | 135 | # load image data 136 | if self.cam_folder is not None and self.lb3: 137 | images_timestamps_microsec = self.image_data.get_image_timestamps() 138 | 139 | rospy.loginfo("data loaded, writing to rosbag file %s" % self.bag_name) 140 | 141 | max_num_messages = 1e20 142 | num_messages = 0 143 | 144 | while not rospy.is_shutdown(): 145 | next_packet = "done" 146 | next_utime = -1 147 | 148 | if self.gt: 149 | if i_gt < len(gt_list) and (gt_list[i_gt, 0] < next_utime or next_utime < 0): 150 | next_utime = gt_list[i_gt, 0] 151 | next_packet = "gt" 152 | 153 | if self.sen: 154 | if i_gps < len(gps_list) and (gps_list[i_gps, 0] < next_utime or next_utime < 0): 155 | next_utime = gps_list[i_gps, 0] 156 | next_packet = "gps" 157 | 158 | if i_gps_rtk < len(gps_rtk_list) and (gps_rtk_list[i_gps_rtk, 0] < next_utime or next_utime < 0): 159 | next_utime = gps_rtk_list[i_gps_rtk, 0] 160 | next_packet = "gps_rtk" 161 | 162 | if i_ms25 < len(ms25_list) and (ms25_list[i_ms25, 0] < next_utime or next_utime < 0): 163 | next_utime = ms25_list[i_ms25, 0] 164 | next_packet = "ms25" 165 | 166 | if i_odom < len(odom_list) and (odom_list[i_odom, 0] < next_utime or next_utime < 0): 167 | next_utime = odom_list[i_odom, 0] 168 | next_packet = "odom" 169 | 170 | if self.hokuyo: 171 | if utime_hok4 > 0 and (utime_hok4 < next_utime or next_utime < 0): 172 | next_packet = "hok4" 173 | 174 | if utime_hok30 > 0 and (utime_hok30 < next_utime or next_utime < 0): 175 | next_packet = "hok30" 176 | 177 | if self.vel: 178 | if i_vel < len(vel_sync_timestamps_microsec) and (vel_sync_timestamps_microsec[i_vel] < next_utime or next_utime < 0): 179 | next_utime = vel_sync_timestamps_microsec[i_vel] 180 | next_packet = "vel_sync" 181 | 182 | if self.cam_folder is not None and self.lb3: 183 | if i_img < len(images_timestamps_microsec) and (images_timestamps_microsec[i_img] < next_utime or next_utime < 0): 184 | next_utime = images_timestamps_microsec[i_img] 185 | next_packet = "img" 186 | 187 | if next_packet == "done": 188 | break 189 | 190 | elif next_packet == "gps": 191 | # print("write gps") 192 | navsat, track, speed, timestamp, tf_static_msg = self.ros_sensor_msg.gps_to_navsat(gps_list=gps_list, i=i_gps) 193 | self.bag.write(self.gps_fix_topic, navsat, timestamp) 194 | self.bag.write(self.gps_track_topic, track, timestamp) 195 | self.bag.write(self.gps_speed_topic, speed, timestamp) 196 | self.bag.write('/tf_static', tf_static_msg, t=timestamp) 197 | i_gps += 1 198 | 199 | elif next_packet == "gps_rtk": 200 | # print("write gps_rtk") 201 | navsat, track, speed, timestamp, tf_static_msg = self.ros_sensor_msg.gps_rtk_to_navsat(gps_rtk_list=gps_rtk_list, i=i_gps_rtk) 202 | self.bag.write(self.gps_rtk_fix_topic, navsat, t=timestamp) 203 | self.bag.write(self.gps_rtk_track_topic, track, t=timestamp) 204 | self.bag.write(self.gps_rtk_speed_topic, speed, t=timestamp) 205 | self.bag.write('/tf_static', tf_static_msg, t=timestamp) 206 | i_gps_rtk += 1 207 | 208 | elif next_packet == "ms25": 209 | # print("write ms25") 210 | imu, mag, timestamp, tf_static_msg = self.ros_sensor_msg.ms25_to_imu(imu_list=ms25_list, i=i_ms25) 211 | self.bag.write(self.imu_data_topic, imu, t=timestamp) 212 | self.bag.write(self.imu_mag_topic, mag, t=timestamp) 213 | self.bag.write('/tf_static', tf_static_msg, t=timestamp) 214 | i_ms25 += 1 215 | 216 | elif next_packet == "odom": 217 | # print("write odom") 218 | odom, timestamp, tf_msg, tf_static_msg = self.ros_sensor_msg.wheel_odom_to_odometry(odom_list=odom_list, odom_cov_list=odom_cov_list, wheels_list=wheels_list, kvh_list=kvh_list, i=i_odom) 219 | self.bag.write(self.odom_topic, odom, t=timestamp) 220 | self.bag.write('/tf', tf_msg, t=timestamp) 221 | self.bag.write('/tf_static', tf_static_msg, t=timestamp) 222 | i_odom += 1 223 | 224 | elif next_packet == "gt": 225 | # print("write gt") 226 | gt, timestamp, tf_static_msg = self.ros_sensor_msg.gt_to_odometry(gt_list=gt_list, gt_cov_list=gt_cov_list, i=i_gt) 227 | self.bag.write(self.ground_truth_topic, gt, t=timestamp) 228 | self.bag.write('/tf_static', tf_static_msg, t=timestamp) 229 | i_gt += 1 230 | 231 | elif next_packet == "hok4": 232 | # print("write hok4") 233 | timestamp, scan, tf_static_msg = self.hokuyo_data.write_hokuyo_4m_to_laserscan(utime_hok4, data_hok4) 234 | self.bag.write(self.hokuyo_urg_topic, scan, t=timestamp) 235 | self.bag.write('/tf_static', tf_static_msg, t=timestamp) 236 | utime_hok4, data_hok4 = self.hokuyo_data.read_next_hokuyo_4m_packet() 237 | 238 | elif next_packet == "hok30": 239 | # print("write hok30") 240 | timestamp, scan, tf_static_msg = self.hokuyo_data.write_hokuyo_30m_to_laserscan(utime_hok30, data_hok30) 241 | self.bag.write(self.hokuyo_utm_topic, scan, t=timestamp) 242 | self.bag.write('/tf_static', tf_static_msg, t=timestamp) 243 | utime_hok30, data_hok30 = self.hokuyo_data.read_next_hokuyo_30m_packet() 244 | 245 | elif next_packet == "vel_sync": 246 | # print("vel_sync") 247 | try: 248 | hits = self.velodyne_sync_data.read_next_velodyne_sync_packet(vel_sync_bin_files[i_vel]) 249 | except ValueError: 250 | rospy.logerr("error in sync velodyne packet") 251 | 252 | timestamp, pc2_msg, tf_static_msg = self.velodyne_sync_data.xyzil_array_to_pointcloud2(utime=vel_sync_timestamps_microsec[i_vel], hits=hits) 253 | self.bag.write(self.velodyne_topic, pc2_msg, t=timestamp) 254 | self.bag.write('/tf_static', tf_static_msg, t=timestamp) 255 | i_vel += 1 256 | 257 | elif next_packet == "img": 258 | if self.cam_folder is 'all': 259 | for camera_id in range(self.num_cameras): 260 | cam_file = os.path.join(self.images_lb3_dir, 'Cam' + str(camera_id), str(next_utime) + '.tiff') 261 | timestamp, image_msg, tf_static_msg = self.image_data.write_images(utime=next_utime, cam_file=cam_file) 262 | self.bag.write(self.ladybug_topic + str(camera_id), image_msg, t=timestamp) 263 | self.bag.write('/tf_static', tf_static_msg, t=timestamp) 264 | else: 265 | cam_file = os.path.join(self.images_lb3_dir, 'Cam' + str(self.cam_folder), str(next_utime) + '.tiff') 266 | timestamp, image_msg, tf_static_msg = self.image_data.write_images(utime=next_utime, cam_file=cam_file) 267 | self.bag.write(self.ladybug_topic + str(self.cam_folder), image_msg, t=timestamp) 268 | self.bag.write('/tf_static', tf_static_msg, t=timestamp) 269 | i_img += 1 270 | else: 271 | rospy.logerr("unknown packet type") 272 | 273 | num_messages += 1 274 | if (num_messages % 5000) == 0: 275 | rospy.loginfo("number messages written: %d" % num_messages) 276 | 277 | if num_messages >= max_num_messages: 278 | break 279 | 280 | if not rospy.is_shutdown(): 281 | self.bag.close() 282 | rospy.loginfo("successfully finished converting!") 283 | 284 | #self.compress_bag() 285 | 286 | def compress_bag(self): 287 | 288 | # TODO compress bag if file not to big 289 | files = os.listdir(self.rosbag_dir) 290 | try: 291 | 292 | if self.bag_name in files: 293 | 294 | os.chdir(self.rosbag_dir) 295 | reindex_cmd = "rosbag reindex " + self.bag_name 296 | compress_cmd = "rosbag compress --lz4 " + self.bag_name 297 | 298 | #reindex_proc = subprocess.Popen(reindex_cmd, stdin=subprocess.PIPE, shell=True, executable='/bin/bash') 299 | #output, error = reindex_proc.communicate() 300 | #if error: 301 | # print("error occured while running rosbag reindex") 302 | # sys.exit(0) 303 | 304 | #reindex_proc.wait() 305 | 306 | compress_proc = subprocess.Popen(compress_cmd, stdin=subprocess.PIPE, shell=True, executable='/bin/bash') 307 | 308 | output, error = compress_proc.communicate() 309 | 310 | if error: 311 | print("error occured while running rosbag compress") 312 | sys.exit(0) 313 | 314 | compress_proc.wait() 315 | 316 | # remove orig file 317 | orig_bag_str = self.bag_name[:-4] + '.orig.bag' 318 | all_files = os.listdir(self.rosbag_dir) 319 | for f in all_files: 320 | if f == orig_bag_str: 321 | os.remove(f) 322 | 323 | except Exception as e: 324 | print(e) 325 | -------------------------------------------------------------------------------- /nclt2ros/downloader/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bierschi/nclt2ros/77b30ca6750d4b0cd82ccb6660f2fd0946581091/nclt2ros/downloader/__init__.py -------------------------------------------------------------------------------- /nclt2ros/downloader/download.py: -------------------------------------------------------------------------------- 1 | import os 2 | import rospy 3 | import subprocess 4 | 5 | 6 | class Download: 7 | """Class to download the NCLT Dataset from http://robots.engin.umich.edu/nclt/ 8 | 9 | USAGE: 10 | Download(date='2013-01-10', raw_data_path='/home/christian/nclt2ros/raw_data', gt=True) 11 | """ 12 | 13 | def __init__(self, date, raw_data_path, lb3=False, sen=False, hokuyo=False, vel=False, gt=False, gt_cov=False): 14 | 15 | self.download_url_dir = 'http://robots.engin.umich.edu/nclt' 16 | self.dates = ['2012-01-08', '2012-01-15', '2012-01-22', '2012-02-02', '2012-02-04', '2012-02-05', '2012-02-12', 17 | '2012-02-18', '2012-02-19', '2012-03-17', '2012-03-25', '2012-03-31', '2012-04-29', '2012-05-11', 18 | '2012-05-26', '2012-06-15', '2012-08-04', '2012-08-20', '2012-09-28', '2012-10-28', '2012-11-04', 19 | '2012-11-16', '2012-11-17', '2012-12-01', '2013-01-10', '2013-02-23', '2013-04-05'] 20 | 21 | self.date = date 22 | self.raw_data_path = str(raw_data_path) 23 | 24 | self.lb3 = lb3 25 | self.sen = sen 26 | self.hokuyo = hokuyo 27 | self.vel = vel 28 | self.gt = gt 29 | self.gt_cov = gt_cov 30 | 31 | if self.raw_data_path.endswith('/'): 32 | self.raw_data_dir = raw_data_path + str(self.date) 33 | else: 34 | self.raw_data_dir = raw_data_path + '/' + str(self.date) 35 | 36 | if not os.path.exists(self.raw_data_dir): 37 | os.makedirs(self.raw_data_dir) 38 | 39 | self.saved_path = None 40 | 41 | if self.check_date(self.date): 42 | self.print_log() 43 | self.load_dataset() 44 | else: 45 | raise ValueError("Given 'Date' is not in dataset") 46 | 47 | def print_log(self): 48 | """method for logging purposes 49 | """ 50 | 51 | rospy.loginfo("Download: ") 52 | 53 | if self.lb3: 54 | rospy.loginfo("- Images") 55 | if self.sen: 56 | rospy.loginfo("- Sensors") 57 | if self.hokuyo: 58 | rospy.loginfo("- Hokuyo") 59 | if self.vel: 60 | rospy.loginfo("- Velodyne") 61 | if self.gt: 62 | rospy.loginfo("- Ground Truth Pose") 63 | if self.gt_cov: 64 | rospy.loginfo("- Ground Truth Covariance") 65 | 66 | rospy.loginfo("from date %s " % self.date) 67 | 68 | def load_dataset(self): 69 | """load data depending on the arguments specified in the launch file 70 | """ 71 | 72 | self.saved_path = os.getcwd() 73 | os.chdir(self.raw_data_dir) 74 | 75 | if self.lb3: 76 | self.get_images() 77 | 78 | if self.sen: 79 | self.get_sensors() 80 | 81 | if self.vel: 82 | self.get_velodyne() 83 | 84 | if self.hokuyo: 85 | self.get_hokuyo() 86 | 87 | if self.gt: 88 | self.get_ground_truth_pose() 89 | 90 | if self.gt_cov: 91 | self.get_ground_truth_cov() 92 | 93 | os.chdir(self.saved_path) 94 | 95 | def check_date(self, date): 96 | """checks if date is in self.dates 97 | 98 | :param date: String 99 | :return: Bool, True if date is in self.dates 100 | else False 101 | """ 102 | 103 | if date in self.dates: 104 | return True 105 | else: 106 | return False 107 | 108 | def get_images(self): 109 | """fetching the raw image data with wget and the continue flag for later continuation 110 | """ 111 | 112 | cmd = ['wget', '--continue', '%s/images/%s_lb3.tar.gz' % (self.download_url_dir, self.date), '-P', 'images'] 113 | cmd_str = ' '.join(cmd) 114 | rospy.loginfo("Calling: %s" % cmd_str) 115 | subprocess.call(cmd) 116 | 117 | def get_sensors(self): 118 | """fetching the raw sensors data with wget and the continue flag for later continuation 119 | """ 120 | 121 | cmd = ['wget', '--continue', '%s/sensor_data/%s_sen.tar.gz' % (self.download_url_dir, self.date), '-P', 'sensor_data'] 122 | cmd_str = ' '.join(cmd) 123 | rospy.loginfo("Calling: %s" % cmd_str) 124 | subprocess.call(cmd) 125 | 126 | def get_velodyne(self): 127 | """fetching the raw velodyne data with wget and the continue flag for later continuation 128 | """ 129 | 130 | cmd = ['wget', '--continue', '%s/velodyne_data/%s_vel.tar.gz' % (self.download_url_dir, self.date), '-P', 'velodyne_data'] 131 | cmd_str = ' '.join(cmd) 132 | rospy.loginfo("Calling: %s" % cmd_str) 133 | subprocess.call(cmd) 134 | 135 | def get_hokuyo(self): 136 | """fetching the raw hokuyo data with wget and the continue flag for later continuation 137 | """ 138 | 139 | cmd = ['wget', '--continue', '%s/hokuyo_data/%s_hokuyo.tar.gz' % (self.download_url_dir, self.date), '-P', 'hokuyo_data'] 140 | cmd_str = ' '.join(cmd) 141 | rospy.loginfo("Calling: %s" % cmd_str) 142 | subprocess.call(cmd) 143 | 144 | def get_ground_truth_pose(self): 145 | """fetching the raw ground truth pose data with wget and the continue flag for later continuation 146 | """ 147 | 148 | cmd = ['wget', '--continue', '%s/ground_truth/groundtruth_%s.csv' % (self.download_url_dir, self.date), '-P', 'ground_truth'] 149 | cmd_str = ' '.join(cmd) 150 | rospy.loginfo("Calling: %s" % cmd_str) 151 | subprocess.call(cmd) 152 | 153 | def get_ground_truth_cov(self): 154 | """fetching the raw ground truth covariance data with wget and the continue flag for later continuation 155 | """ 156 | 157 | cmd = ['wget', '--continue', '%s/covariance/cov_%s.csv' % (self.download_url_dir, self.date), '-P', 'ground_truth_covariance'] 158 | cmd_str = ' '.join(cmd) 159 | rospy.loginfo("Calling: %s" % cmd_str) 160 | subprocess.call(cmd) 161 | -------------------------------------------------------------------------------- /nclt2ros/extractor/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bierschi/nclt2ros/77b30ca6750d4b0cd82ccb6660f2fd0946581091/nclt2ros/extractor/__init__.py -------------------------------------------------------------------------------- /nclt2ros/extractor/base_raw_data.py: -------------------------------------------------------------------------------- 1 | import os 2 | import rospy 3 | from definitions import ROOT_DIR 4 | 5 | 6 | class BaseRawData: 7 | """Base class to initialize the directories for the raw data 8 | 9 | USAGE: 10 | BaseRawData('2013-01-10') 11 | 12 | """ 13 | def __init__(self, date): 14 | 15 | self.date = date 16 | 17 | RAW_DATA_PATH_DFLT = ROOT_DIR + '/nclt2ros/raw_data/' 18 | RAW_DATA_PATH_DFLT = '/home/christian/nclt2ros/raw_data/' # TODO remove, only for debugging 19 | self.raw_data_path = rospy.get_param('~raw_data_path', RAW_DATA_PATH_DFLT) 20 | 21 | # init raw directory 22 | if self.raw_data_path.endswith('/'): 23 | self.raw_data_dir = self.raw_data_path + str(self.date) 24 | else: 25 | self.raw_data_dir = self.raw_data_path + '/' + str(self.date) 26 | 27 | # check if data exists 28 | if os.path.exists(self.raw_data_dir): 29 | 30 | self.ground_truth_dir = self.raw_data_dir + '/ground_truth' 31 | if os.path.exists(self.ground_truth_dir): 32 | self.ground_truth_flag = True 33 | else: 34 | self.ground_truth_flag = False 35 | 36 | self.ground_truth_covariance_dir = self.raw_data_dir + '/ground_truth_covariance' 37 | if os.path.exists(self.ground_truth_covariance_dir): 38 | self.ground_truth_covariance_flag = True 39 | else: 40 | self.ground_truth_covariance_flag = False 41 | 42 | self.hokuyo_data_dir = self.raw_data_dir + '/hokuyo_data' 43 | if os.path.exists(self.hokuyo_data_dir): 44 | self.hokuyo_data_flag = True 45 | else: 46 | self.hokuyo_data_flag = False 47 | 48 | self.sensor_data_dir = self.raw_data_dir + '/sensor_data' 49 | if os.path.exists(self.sensor_data_dir): 50 | self.sensor_data_flag = True 51 | else: 52 | self.sensor_data_flag = False 53 | 54 | self.velodyne_data_dir = self.raw_data_dir + '/velodyne_data' 55 | if os.path.exists(self.velodyne_data_dir): 56 | self.velodyne_data_flag = True 57 | self.velodyne_sync_data_dir = self.raw_data_dir + '/velodyne_data/' + '%s' % self.date + '/velodyne_sync/' 58 | else: 59 | self.velodyne_data_flag = False 60 | 61 | self.images_dir = self.raw_data_dir + '/images' 62 | if os.path.exists(self.images_dir): 63 | self.images_flag = True 64 | self.images_lb3_dir = self.raw_data_dir + '/images/' + '%s' % self.date + '/lb3/' 65 | else: 66 | self.images_flag = False 67 | 68 | else: 69 | raise ValueError("raw_data directory not exists") 70 | 71 | -------------------------------------------------------------------------------- /nclt2ros/extractor/extract.py: -------------------------------------------------------------------------------- 1 | import os 2 | import tarfile 3 | import rospy 4 | from base_raw_data import BaseRawData 5 | 6 | 7 | class Extract(BaseRawData): 8 | """Class to extract the tarballs in param 'raw_data_path' 9 | 10 | USAGE: 11 | ExtractRawData('2013-01-10') 12 | 13 | """ 14 | 15 | def __init__(self, date, lb3=False, sen=False, vel=False, hokuyo=False): 16 | 17 | if isinstance(date, str): 18 | self.date = date 19 | else: 20 | raise TypeError("'date' must be type of string") 21 | 22 | # init base class 23 | BaseRawData.__init__(self, date=date) 24 | 25 | self.lb3 = lb3 26 | self.sen = sen 27 | self.vel = vel 28 | self.hokuyo = hokuyo 29 | 30 | if self.lb3 or self.sen or self.vel or self.hokuyo: 31 | rospy.loginfo("Extracting data from %s" % self.date) 32 | self.extract_data() 33 | else: 34 | rospy.loginfo("Nothing to extract") 35 | 36 | def extract_data(self): 37 | """extracts the data from tar.gz format 38 | """ 39 | 40 | # check hokuyo_data 41 | if self.hokuyo: 42 | if os.path.exists(self.hokuyo_data_dir): 43 | os.chdir(self.hokuyo_data_dir) 44 | files = os.listdir(self.hokuyo_data_dir) 45 | if not (self.date in files): 46 | for file in files: 47 | if file.endswith('tar.gz'): 48 | rospy.loginfo("unpacking {}".format(file)) 49 | tar = tarfile.open(file, 'r:gz') 50 | tar.extractall() 51 | tar.close() 52 | # remove tar.gz file 53 | os.remove(file) 54 | else: 55 | rospy.loginfo("hokuyo_data already exists") 56 | 57 | # check sensor_data 58 | if self.sen: 59 | if os.path.exists(self.sensor_data_dir): 60 | os.chdir(self.sensor_data_dir) 61 | files = os.listdir(self.sensor_data_dir) 62 | if not (self.date in files): 63 | for file in files: 64 | if file.endswith('tar.gz'): 65 | rospy.loginfo("unpacking {}".format(file)) 66 | tar = tarfile.open(file, 'r:gz') 67 | tar.extractall() 68 | tar.close() 69 | # remove tar.gz file 70 | os.remove(file) 71 | 72 | else: 73 | rospy.loginfo("sensor_data already exists") 74 | 75 | # check velodyne_data 76 | if self.vel: 77 | if os.path.exists(self.velodyne_data_dir): 78 | os.chdir(self.velodyne_data_dir) 79 | files = os.listdir(self.velodyne_data_dir) 80 | if not (self.date in files): 81 | for file in files: 82 | if file.endswith('tar.gz'): 83 | rospy.loginfo("unpacking {}".format(file)) 84 | tar = tarfile.open(file, 'r:gz') 85 | tar.extractall() 86 | tar.close() 87 | # remove tar.gz file 88 | os.remove(file) 89 | else: 90 | rospy.loginfo("velodyne_data already exists") 91 | 92 | # check image_data 93 | if self.lb3: 94 | if os.path.exists(self.images_dir): 95 | os.chdir(self.images_dir) 96 | files = os.listdir(self.images_dir) 97 | if not (self.date in files): 98 | for file in files: 99 | if file.endswith('tar.gz'): 100 | rospy.loginfo("unpacking {}".format(file)) 101 | tar = tarfile.open(file, 'r:gz') 102 | tar.extractall() 103 | tar.close() 104 | # remove tar.gz file 105 | os.remove(file) 106 | else: 107 | rospy.loginfo("image_data already exists") 108 | -------------------------------------------------------------------------------- /nclt2ros/extractor/read_ground_truth.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from nclt2ros.extractor.base_raw_data import BaseRawData 3 | 4 | 5 | class ReadGroundTruth(BaseRawData): 6 | """Class to read the ground_truth directory 7 | 8 | USAGE: 9 | ReadGroundTruth('2013-01-10') 10 | 11 | """ 12 | def __init__(self, date): 13 | BaseRawData.__init__(self, date=date) 14 | 15 | def read_gt_csv(self, all_in_one=None): 16 | """reads the data in the ground_truth csv file 17 | 18 | :param all_in_one: Boolean 19 | :return: ndarray: containing all data, if all_in_one is True 20 | else 21 | each column seperated as utimes, x, y, z, roll_rad, pitch_rad, yaw_rad 22 | """ 23 | 24 | if self.ground_truth_flag: 25 | 26 | gt = np.loadtxt(self.ground_truth_dir + '/groundtruth_%s.csv' % self.date, delimiter=",") 27 | 28 | if all_in_one is True: 29 | 30 | return gt 31 | 32 | else: 33 | utimes = gt[:, 0] 34 | x = gt[:, 1] 35 | y = gt[:, 2] 36 | z = gt[:, 3] 37 | roll_rad = gt[:, 4] 38 | pitch_rad = gt[:, 5] 39 | yaw_rad = gt[:, 6] 40 | 41 | return utimes, x, y, z, roll_rad, pitch_rad, yaw_rad 42 | else: 43 | raise ValueError('no ground_truth directory available') 44 | -------------------------------------------------------------------------------- /nclt2ros/extractor/read_ground_truth_covariance.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from nclt2ros.extractor.base_raw_data import BaseRawData 3 | 4 | 5 | class ReadGroundTruthCovariance(BaseRawData): 6 | """Class to read the ground_truth_covariance directory 7 | 8 | USAGE: 9 | ReadGroundTruthCovariance('2013-01-10') 10 | 11 | """ 12 | def __init__(self, date): 13 | BaseRawData.__init__(self, date=date) 14 | 15 | def read_gt_cov_csv(self, all_in_one=None): 16 | """reads the data in the ground_truth_covariance csv file 17 | 18 | :param all_in_one: Boolean 19 | :return: ndarray: containing all data, if all_in_one is True 20 | else 21 | each column seperated as utimes and covariance matrice ... 22 | """ 23 | 24 | if self.ground_truth_covariance_flag: 25 | 26 | gt_cov = np.loadtxt(self.ground_truth_covariance_dir + '/cov_%s.csv' % self.date, delimiter=",") 27 | 28 | if all_in_one is True: 29 | 30 | return gt_cov 31 | 32 | else: 33 | utimes = gt_cov[:, 0] 34 | xx = gt_cov[:, 1]; xy = gt_cov[:, 2]; xz = gt_cov[:, 3]; xr = gt_cov[:, 4]; xp = gt_cov[:, 5]; xh = gt_cov[:, 6] 35 | yy = gt_cov[:, 7]; yz = gt_cov[:, 8]; yr = gt_cov[:, 9]; yp = gt_cov[:, 10]; yh = gt_cov[:, 11] 36 | zz = gt_cov[:, 12]; zr = gt_cov[:, 13]; zp = gt_cov[:, 14]; zh = gt_cov[:, 15] 37 | rr = gt_cov[:, 16]; rp = gt_cov[:, 17]; rh = gt_cov[:, 18] 38 | pp = gt_cov[:, 19]; ph = gt_cov[:, 20] 39 | hh = gt_cov[:, 21] 40 | 41 | return utimes, xx, xy, xz, xr, xp, xh, yy, yz, yr, yp, yh, zz, zr, zp, zh, rr, rp, rh, pp, ph, hh 42 | 43 | else: 44 | raise ValueError('no ground_truth_covariance directory available') 45 | -------------------------------------------------------------------------------- /nclt2ros/extractor/read_sensor_data.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from nclt2ros.extractor.base_raw_data import BaseRawData 3 | 4 | 5 | class ReadSensorData(BaseRawData): 6 | """Class to read the sensor_data directory 7 | 8 | USAGE: 9 | ReadSensorData('2013-01-10') 10 | 11 | """ 12 | def __init__(self, date): 13 | BaseRawData.__init__(self, date=date) 14 | 15 | def read_gps_csv(self, all_in_one=None): 16 | """reads the data in the gps csv file 17 | 18 | :param all_in_one: Boolean 19 | :return: ndarray: containing all data, if all_in_one is True 20 | else 21 | each column seperated as utimes, modes, num_satss, lat, lng, alts, tracks, speeds 22 | """ 23 | if self.sensor_data_flag: 24 | 25 | gps = np.loadtxt(self.sensor_data_dir + '/%s/gps.csv' % self.date, delimiter=",") 26 | 27 | if all_in_one is True: 28 | 29 | return gps 30 | 31 | else: 32 | 33 | utimes = gps[:, 0] 34 | modes = gps[:, 1] 35 | num_satss = gps[:, 2] 36 | lat = gps[:, 3] 37 | lng = gps[:, 4] 38 | alts = gps[:, 5] 39 | tracks = gps[:, 6] 40 | speeds = gps[:, 7] 41 | 42 | return utimes, modes, num_satss, lat, lng, alts, tracks, speeds 43 | else: 44 | raise ValueError('no sensor_data directory available') 45 | 46 | def read_gps_rtk_csv(self, all_in_one=None): 47 | """reads the data in the gps_rtk csv file 48 | 49 | :param all_in_one: Boolean 50 | :return: ndarray: containing all data, if all_in_one is True 51 | else 52 | each column seperated as utimes, modes, num_satss, lat, lng, alts, tracks, speeds 53 | """ 54 | if self.sensor_data_flag: 55 | 56 | gps_rtk = np.loadtxt(self.sensor_data_dir + '/%s/gps_rtk.csv' % self.date, delimiter=",") 57 | 58 | if all_in_one is True: 59 | 60 | return gps_rtk 61 | 62 | else: 63 | 64 | utimes = gps_rtk[:, 0] 65 | modes = gps_rtk[:, 1] 66 | num_satss = gps_rtk[:, 2] 67 | lat = gps_rtk[:, 3] 68 | lng = gps_rtk[:, 4] 69 | alts = gps_rtk[:, 5] 70 | tracks = gps_rtk[:, 6] 71 | speeds = gps_rtk[:, 7] 72 | 73 | return utimes, modes, num_satss, lat, lng, alts, tracks, speeds 74 | 75 | else: 76 | raise ValueError('no sensor_data directory available') 77 | 78 | def read_gps_rtk_err_csv(self, all_in_one=None): 79 | """reads the data in the gps_rtk_err csv file 80 | 81 | :param all_in_one: Boolean 82 | :return: ndarray: containing all data, if all_in_one is True 83 | else 84 | each column seperated as utimes, error in meters 85 | """ 86 | if self.sensor_data_flag: 87 | 88 | gps_rtk_err = np.loadtxt(self.sensor_data_dir + '/%s/gps_rtk_err.csv' % self.date, delimiter=",") 89 | 90 | if all_in_one is True: 91 | 92 | return gps_rtk_err 93 | 94 | else: 95 | 96 | utimes = gps_rtk_err[:, 0] 97 | error = gps_rtk_err[:, 1] 98 | 99 | return utimes, error 100 | else: 101 | raise ValueError('no sensor_data directory available') 102 | 103 | def read_kvh_csv(self, all_in_one=None): 104 | """reads the data in the kvh csv file 105 | 106 | :param all_in_one: Boolean 107 | :return: ndarray: containing all data, if all_in_one is True 108 | else 109 | each column seperated as utimes, heading in rad 110 | """ 111 | if self.sensor_data_flag: 112 | 113 | kvh = np.loadtxt(self.sensor_data_dir + '/%s/kvh.csv' % self.date, delimiter=",") 114 | 115 | if all_in_one is True: 116 | 117 | return kvh 118 | 119 | else: 120 | 121 | utimes = kvh[:, 0] 122 | heading = kvh[:, 1] 123 | 124 | return utimes, heading 125 | else: 126 | raise ValueError('no sensor_data directory available') 127 | 128 | def read_ms25_csv(self, all_in_one=None): 129 | """reads the data in the ms25 csv file 130 | 131 | :param all_in_one: Boolean 132 | :return: ndarray: containing all data, if all_in_one is True 133 | else 134 | each column seperated as utimes, mag_xs, mag_ys, mag_zs, accel_xs, accel_ys, accel_zs, rot_rs, rot_ps, rot_hs 135 | """ 136 | if self.sensor_data_flag: 137 | 138 | ms25 = np.loadtxt(self.sensor_data_dir + '/%s/ms25.csv' % self.date, delimiter=",") 139 | 140 | if all_in_one is True: 141 | 142 | return ms25 143 | 144 | else: 145 | 146 | utimes = ms25[:, 0] 147 | 148 | mag_xs = ms25[:, 1] 149 | mag_ys = ms25[:, 2] 150 | mag_zs = ms25[:, 3] 151 | 152 | accel_xs = ms25[:, 4] 153 | accel_ys = ms25[:, 5] 154 | accel_zs = ms25[:, 6] 155 | 156 | rot_rs = ms25[:, 7] 157 | rot_ps = ms25[:, 8] 158 | rot_hs = ms25[:, 9] 159 | 160 | return utimes, mag_xs, mag_ys, mag_zs, accel_xs, accel_ys, accel_zs, rot_rs, rot_ps, rot_hs 161 | 162 | else: 163 | raise ValueError('no sensor_data directory available') 164 | 165 | def read_ms25_euler_csv(self, all_in_one=None): 166 | """reads the data in the ms25_euler csv file 167 | 168 | :param all_in_one: Boolean 169 | :return: ndarray: containing all data, if all_in_one is True 170 | else 171 | each column seperated as utimes, roll, pitch, heading 172 | """ 173 | if self.sensor_data_flag: 174 | 175 | ms25_euler = np.loadtxt(self.sensor_data_dir + '/%s/ms25.csv' % self.date, delimiter=",") 176 | 177 | if all_in_one is True: 178 | 179 | return ms25_euler 180 | 181 | else: 182 | 183 | utimes = ms25_euler[:, 0] 184 | roll = ms25_euler[:, 1] 185 | pitch = ms25_euler[:, 2] 186 | heading = ms25_euler[:, 3] 187 | 188 | return utimes, roll, pitch, heading 189 | 190 | else: 191 | raise ValueError('no sensor_data directory available') 192 | 193 | def read_odometry_cov_csv(self, all_in_one=None): 194 | """reads the data in the odometry_cov csv file 195 | 196 | :param all_in_one: Boolean 197 | :return: ndarray: containing all data, if all_in_one is True 198 | else 199 | each column seperated as utimes and the upper diagonal of the covariance matrix (row major) 200 | """ 201 | if self.sensor_data_flag: 202 | 203 | odom_cov = np.loadtxt(self.sensor_data_dir + '/%s/odometry_cov.csv' % self.date, delimiter=",") 204 | 205 | if all_in_one is True: 206 | 207 | return odom_cov 208 | 209 | else: 210 | utimes = odom_cov[:, 0] 211 | 212 | xx = odom_cov[:, 1]; xy = odom_cov[:, 2]; xz = odom_cov[:, 3]; xr = odom_cov[:, 4]; xp = odom_cov[:, 5]; xh = odom_cov[:, 6] 213 | yy = odom_cov[:, 7]; yz = odom_cov[:, 8]; yr = odom_cov[:, 9]; yp = odom_cov[:, 10]; yh = odom_cov[:, 11] 214 | zz = odom_cov[:, 12]; zr = odom_cov[:, 13]; zp = odom_cov[:, 14]; zh = odom_cov[:, 15] 215 | rr = odom_cov[:, 16]; rp = odom_cov[:, 17]; rh = odom_cov[:, 18] 216 | pp = odom_cov[:, 19]; ph = odom_cov[:, 20] 217 | hh = odom_cov[:, 21] 218 | 219 | return utimes, xx, xy, xz, xr, xp, xh, yy, yz, yr, yp, yh, zz, zr, zp, zh, rr, rp, rh, pp, ph, hh 220 | else: 221 | raise ValueError('no sensor_data directory available') 222 | 223 | def read_odometry_cov_100hz_csv(self, all_in_one=None): 224 | """reads the data in the odometry_cov_100hz csv file 225 | 226 | :param all_in_one: Boolean 227 | :return: ndarray: containing all data, if all_in_one is True 228 | else 229 | each column seperated as utimes and the upper diagonal of the covariance matrix (row major) 230 | """ 231 | if self.sensor_data_flag: 232 | 233 | odom_cov = np.loadtxt(self.sensor_data_dir + '/%s/odometry_cov_100hz.csv' % self.date, delimiter=",") 234 | 235 | if all_in_one is True: 236 | 237 | return odom_cov 238 | 239 | else: 240 | 241 | utimes = odom_cov[:, 0] 242 | 243 | xx = odom_cov[:, 1]; xy = odom_cov[:, 2]; xz = odom_cov[:, 3]; xr = odom_cov[:, 4]; xp = odom_cov[:, 5]; xh = odom_cov[:, 6] 244 | yy = odom_cov[:, 7]; yz = odom_cov[:, 8]; yr = odom_cov[:, 9]; yp = odom_cov[:, 10]; yh = odom_cov[:, 11] 245 | zz = odom_cov[:, 12]; zr = odom_cov[:, 13]; zp = odom_cov[:, 14]; zh = odom_cov[:, 15] 246 | rr = odom_cov[:, 16]; rp = odom_cov[:, 17]; rh = odom_cov[:, 18] 247 | pp = odom_cov[:, 19]; ph = odom_cov[:, 20] 248 | hh = odom_cov[:, 21] 249 | 250 | return utimes, xx, xy, xz, xr, xp, xh, yy, yz, yr, yp, yh, zz, zr, zp, zh, rr, rp, rh, pp, ph, hh 251 | 252 | else: 253 | raise ValueError('no sensor_data directory available') 254 | 255 | def read_odometry_mu_csv(self, all_in_one=None): 256 | """reads the data in the odometry_mu_100hz csv file 257 | 258 | :param all_in_one: Boolean 259 | :return: ndarray: containing all data, if all_in_one is True 260 | else 261 | each column seperated as utimes, x, y, z, roll, pitch, heading 262 | """ 263 | if self.sensor_data_flag: 264 | 265 | odom = np.loadtxt(self.sensor_data_dir + '/%s/odometry_mu.csv' % self.date, delimiter=",") 266 | 267 | if all_in_one is True: 268 | 269 | return odom 270 | 271 | else: 272 | 273 | utimes = odom[:, 0] 274 | x = odom[:, 1] 275 | y = odom[:, 2] 276 | z = odom[:, 3] 277 | roll = odom[:, 4] 278 | pitch = odom[:, 5] 279 | heading = odom[:, 6] 280 | 281 | return utimes, x, y, z, roll, pitch, heading 282 | else: 283 | raise ValueError('no sensor_data directory available') 284 | 285 | def read_odometry_mu_100hz_csv(self, all_in_one=None): 286 | """reads the data in the odometry_mu_100hz csv file 287 | 288 | :param all_in_one: Boolean 289 | :return: ndarray: containing all data, if all_in_one is True 290 | else 291 | each column seperated as utimes, x, y, z, roll, pitch, heading 292 | """ 293 | if self.sensor_data_flag: 294 | 295 | odom = np.loadtxt(self.sensor_data_dir + '/%s/odometry_mu_100hz.csv' % self.date, delimiter=",") 296 | 297 | if all_in_one is True: 298 | 299 | return odom 300 | 301 | else: 302 | 303 | utimes = odom[:, 0] 304 | x = odom[:, 1] 305 | y = odom[:, 2] 306 | z = odom[:, 3] 307 | roll = odom[:, 4] 308 | pitch = odom[:, 5] 309 | heading = odom[:, 6] 310 | 311 | return utimes, x, y, z, roll, pitch, heading 312 | else: 313 | raise ValueError('no sensor_data directory available') 314 | 315 | def read_wheels_csv(self, all_in_one=None): 316 | """reads the data in the wheels csv file 317 | 318 | :param all_in_one: Boolean 319 | :return: ndarray: containing all data, if all_in_one is True 320 | else 321 | each column seperated as utimes, vl, vr 322 | """ 323 | if self.sensor_data_flag: 324 | 325 | wheels = np.loadtxt(self.sensor_data_dir + '/%s/wheels.csv' % self.date, delimiter=",") 326 | 327 | if all_in_one is True: 328 | 329 | return wheels 330 | 331 | else: 332 | 333 | utimes = wheels[:, 0] 334 | vl = wheels[:, 1] 335 | vr = wheels[:, 2] 336 | 337 | return utimes, vl, vr 338 | 339 | else: 340 | raise ValueError('no sensor_data directory available') 341 | 342 | -------------------------------------------------------------------------------- /nclt2ros/transformer/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bierschi/nclt2ros/77b30ca6750d4b0cd82ccb6660f2fd0946581091/nclt2ros/transformer/__init__.py -------------------------------------------------------------------------------- /nclt2ros/transformer/coordinate_frame.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | 4 | 5 | class CoordinateFrame: 6 | """Class to transform between the odom and ground truth coordinate frame 7 | 8 | USAGE: 9 | CoordinateFrame(origin='odom') 10 | CoordinateFrame(origin='gt') 11 | """ 12 | def __init__(self, origin='odom'): 13 | 14 | if isinstance(origin, str): 15 | if origin is 'gt': 16 | self.lat_origin = 42.293227 # degree 17 | self.lon_origin = -83.709657 # degree 18 | if origin is 'odom': 19 | self.lat_origin = 42.2939096617 # degree 20 | self.lon_origin = -83.7083507601 # degree 21 | else: 22 | raise TypeError("origin must be type of str") 23 | 24 | self.alt_origin = 270 # meter 25 | self.earth_equat_radius = 6378135 # meter 26 | self.earth_polar_radius = 6356750 # meter 27 | 28 | def calc_radius_ns(self): 29 | """calculates the approximation of the earths radius in north-south direction 30 | 31 | :return: radius_ns: value of radius in meter 32 | """ 33 | 34 | numerator = math.pow(self.earth_equat_radius*self.earth_polar_radius, 2) 35 | val1 = math.pow(self.earth_equat_radius * math.cos(np.deg2rad(self.lat_origin)), 2) 36 | val2 = math.pow(self.earth_polar_radius * math.sin(np.deg2rad(self.lat_origin)), 2) 37 | denominator = math.pow(val1 + val2, 1.5) 38 | radius_ns = numerator / denominator 39 | 40 | return radius_ns 41 | 42 | def calc_radius_ew(self): 43 | """calculates the approximation of the earths radius in east-west direction 44 | 45 | :return: radius_ew: value of radius in meter 46 | """ 47 | numerator = math.pow(self.earth_equat_radius, 2) 48 | val1 = math.pow(self.earth_equat_radius * math.cos(np.deg2rad(self.lat_origin)), 2) 49 | val2 = math.pow(self.earth_polar_radius * math.sin(np.deg2rad(self.lat_origin)), 2) 50 | denominator = math.sqrt(val1 + val2) 51 | radius_ew = numerator / denominator 52 | 53 | return radius_ew 54 | 55 | def get_lat(self, x): 56 | """get the latitude value from x coordinate 57 | 58 | :param x: x coordinate in meters 59 | :return: lat: latitude in degree 60 | """ 61 | 62 | tmp = x / self.calc_radius_ns() 63 | lat = np.rad2deg(math.asin(tmp)) + self.lat_origin 64 | 65 | return lat 66 | 67 | def get_lon(self, y): 68 | """get the longitude value from y coordinate 69 | 70 | :param y: y coordinate in meters 71 | :return: lon: longitude in degree 72 | """ 73 | 74 | tmp = y / (self.calc_radius_ew() * math.cos(np.deg2rad(self.lat_origin))) 75 | lon = np.rad2deg(math.asin(tmp)) + self.lon_origin 76 | 77 | return lon 78 | 79 | def get_alt(self, z): 80 | """get the altitude value from z coordinate 81 | 82 | :param z: z coordinate in meters 83 | :return: altitude in meters 84 | """ 85 | return self.alt_origin - z 86 | 87 | def get_x(self, lat): 88 | """get the x coordinate from the latitude 89 | 90 | :param lat: latitude in degree 91 | :return: x coordinate in meters 92 | """ 93 | 94 | return math.sin(np.deg2rad(lat-self.lat_origin)) * self.calc_radius_ns() 95 | 96 | def get_y(self, lon): 97 | """get the y coordinate from the longitude 98 | 99 | :param lon: longitude in degree 100 | :return: y coordinate in meters 101 | """ 102 | 103 | return math.sin(np.deg2rad(lon - self.lon_origin)) * self.calc_radius_ew() * math.cos(np.deg2rad(self.lat_origin)) 104 | 105 | def get_z(self, alt): 106 | """get the z coordinate from the altitude 107 | 108 | :param alt: altitude in meters 109 | :return: z coordinate in meters 110 | """ 111 | 112 | return self.alt_origin - alt 113 | -------------------------------------------------------------------------------- /nclt2ros/transformer/hokuyo_data.py: -------------------------------------------------------------------------------- 1 | import struct 2 | import numpy as np 3 | import rospy 4 | import geometry_msgs.msg 5 | import tf2_msgs.msg 6 | import tf.transformations 7 | 8 | from sensor_msgs.msg import LaserScan 9 | from nclt2ros.extractor.base_raw_data import BaseRawData 10 | from nclt2ros.converter.base_convert import BaseConvert 11 | 12 | 13 | class HokuyoData(BaseRawData, BaseConvert): 14 | """Class to transform the hokuyo binary data to ROS LaserScan messages 15 | 16 | USAGE: 17 | HokuyoData('2013-01-10') 18 | 19 | """ 20 | def __init__(self, date): 21 | 22 | # init base classes 23 | BaseRawData.__init__(self, date=date) 24 | BaseConvert.__init__(self, date=date) 25 | 26 | # load binary files 27 | if self.hokuyo_data_flag: 28 | self.f_bin_hokuyo_4m = open(self.hokuyo_data_dir + '/%s/hokuyo_4m.bin' % self.date, 'r') 29 | self.f_bin_hokuyo_30m = open(self.hokuyo_data_dir + '/%s/hokuyo_30m.bin' % self.date, 'r') 30 | else: 31 | raise ValueError('hokuyo_data directory not exists') 32 | 33 | def __del__(self): 34 | """destructor""" 35 | 36 | self.f_bin_hokuyo_4m.close() 37 | self.f_bin_hokuyo_30m.close() 38 | 39 | def convert_hokuyo(self, x_s): 40 | """converts the hokuyo binary data to correct values, check out the paper http://robots.engin.umich.edu/nclt/nclt.pdf 41 | 42 | :param x_s: x value from binary file 43 | 44 | :return: corrected x value 45 | """ 46 | 47 | scaling = 0.005 48 | offset = -100.0 49 | 50 | x = x_s * scaling + offset 51 | 52 | return x 53 | 54 | def read_next_hokuyo_4m_packet(self): 55 | """reads the hokuyo 4m packets from binary file 56 | 57 | :return: utime: time in microseconds 58 | r: np array containing one observation 59 | """ 60 | 61 | try: 62 | num_hits = 726 63 | 64 | utime_str = self.f_bin_hokuyo_4m.read(8) 65 | if utime_str == '': # EOF reached 66 | return -1, None 67 | 68 | utime = struct.unpack(' utime_wheel: 302 | speed = (vl + vr) / 2.0 303 | 304 | odom.twist.twist.linear.x = speed 305 | 306 | self.last_twist = odom.twist.twist.linear.x 307 | self.i_wheel += 1 308 | 309 | elif self.init_twist: 310 | odom.twist.twist.linear.x = self.last_twist 311 | 312 | else: 313 | odom.twist.twist.linear.x = 0.0 314 | 315 | self.init_twist = True 316 | self.last_twist = odom.twist.twist.linear.x 317 | 318 | if utime > utime_kvh: 319 | odom.twist.twist.angular.z = kvh_heading 320 | self.last_twist_heading = odom.twist.twist.angular.z 321 | self.i_kvh += 1 322 | elif self.init_twist_heading: 323 | odom.twist.twist.angular.z = self.last_twist_heading 324 | 325 | else: 326 | odom.twist.twist.angular.z = 0.0 327 | self.init_twist_heading = True 328 | self.last_twist_heading = odom.twist.twist.angular.z 329 | 330 | odom.twist.twist.linear.y = 0.0 331 | odom.twist.twist.linear.z = 0.0 332 | odom.twist.twist.angular.x = 0.0 333 | odom.twist.twist.angular.y = 0.0 334 | 335 | # just an example 336 | odom.twist.covariance[0] = 0.001 337 | odom.twist.covariance[7] = 0.001 338 | odom.twist.covariance[14] = 99999.9 339 | odom.twist.covariance[21] = 99999.9 340 | odom.twist.covariance[28] = 99999.9 341 | odom.twist.covariance[35] = 0.001 342 | 343 | # broadcast odom base_link transform 344 | geo_msg = geometry_msgs.msg.TransformStamped() 345 | geo_msg.header.stamp = timestamp 346 | geo_msg.header.frame_id = wheel_odom_link 347 | geo_msg.child_frame_id = base_link 348 | geo_msg.transform.translation.x = x 349 | geo_msg.transform.translation.y = -y 350 | geo_msg.transform.translation.z = -z 351 | geo_msg.transform.rotation.x = quaternion[0] 352 | geo_msg.transform.rotation.y = quaternion[1] 353 | geo_msg.transform.rotation.z = quaternion[2] 354 | geo_msg.transform.rotation.w = quaternion[3] 355 | 356 | tf_msg = tf2_msgs.msg.TFMessage([geo_msg]) 357 | 358 | # create world odom static transformer 359 | odom_static_transform_stamped = geometry_msgs.msg.TransformStamped() 360 | odom_static_transform_stamped.header.stamp = timestamp 361 | odom_static_transform_stamped.header.frame_id = "world" 362 | odom_static_transform_stamped.child_frame_id = wheel_odom_link 363 | 364 | odom_static_transform_stamped.transform.translation.x = 0 365 | odom_static_transform_stamped.transform.translation.y = 0 366 | odom_static_transform_stamped.transform.translation.z = 0 367 | 368 | quat = tf.transformations.quaternion_from_euler(0, 0, 1.57) 369 | odom_static_transform_stamped.transform.rotation.x = quat[0] 370 | odom_static_transform_stamped.transform.rotation.y = quat[1] 371 | odom_static_transform_stamped.transform.rotation.z = quat[2] 372 | odom_static_transform_stamped.transform.rotation.w = quat[3] 373 | 374 | # publish static transform 375 | tf_static_msg = tf2_msgs.msg.TFMessage([odom_static_transform_stamped]) 376 | 377 | return odom, timestamp, tf_msg, tf_static_msg 378 | 379 | def ms25_to_imu(self, imu_list, i): 380 | """converts ms25 data to ROS imu messages 381 | 382 | :param imu_list: list containing the imu data from sensor ms25 383 | :param i: list row counter 384 | 385 | :return: fill bag with imu, mag, timestamp, tf_static_msg 386 | """ 387 | 388 | # load data from list 389 | utime = imu_list[i, 0] 390 | mag_xs = imu_list[i, 1] 391 | mag_ys = imu_list[i, 2] 392 | mag_zs = imu_list[i, 3] 393 | accel_xs = imu_list[i, 4] 394 | accel_ys = imu_list[i, 5] 395 | accel_zs = imu_list[i, 6] 396 | rot_rs = imu_list[i, 7] 397 | rot_ps = imu_list[i, 8] 398 | rot_hs = imu_list[i, 9] 399 | 400 | # create ros timestamp 401 | timestamp = rospy.Time.from_sec(utime / 1e6) 402 | 403 | # get imu and base link 404 | imu_link = self.imu_frame 405 | base_link = self.body_frame 406 | 407 | # create ros imu message 408 | imu = Imu() 409 | imu.header.stamp = timestamp 410 | imu.header.frame_id = imu_link 411 | 412 | # swap x,y due to the enu frame, negate z 413 | quaternion = tf.transformations.quaternion_from_euler( 414 | rot_ps, rot_rs, -rot_hs 415 | ) 416 | 417 | # alternative 418 | #quaternion = tf.transformations.quaternion_from_euler( 419 | # rot_rs, -rot_ps, -rot_hs 420 | #) 421 | 422 | imu.orientation.x = quaternion[0] 423 | imu.orientation.y = quaternion[1] 424 | imu.orientation.z = quaternion[2] 425 | imu.orientation.w = quaternion[3] 426 | 427 | # swap x,y due to the enu frame, negate z 428 | imu.angular_velocity.x = rot_ps # rot_rs 429 | imu.angular_velocity.y = rot_rs # -rot_ps 430 | imu.angular_velocity.z = -rot_hs 431 | imu.angular_velocity_covariance = IMU_VEL_COVAR 432 | 433 | # swap x,y due to the enu frame, negate z 434 | imu.linear_acceleration.x = accel_ys # accel_xs 435 | imu.linear_acceleration.y = accel_xs # -accel_ys 436 | imu.linear_acceleration.z = -accel_zs 437 | imu.linear_acceleration_covariance = IMU_ACCEL_COVAR 438 | 439 | # enu frame 440 | mag = MagneticField() 441 | mag.header.stamp = timestamp 442 | mag.magnetic_field.x = mag_ys 443 | mag.magnetic_field.y = mag_xs 444 | mag.magnetic_field.z = -mag_zs 445 | 446 | # create base_link imu static transformer 447 | imu_static_transform_stamped = geometry_msgs.msg.TransformStamped() 448 | imu_static_transform_stamped.header.stamp = timestamp 449 | imu_static_transform_stamped.header.frame_id = base_link 450 | imu_static_transform_stamped.child_frame_id = imu_link 451 | 452 | imu_static_transform_stamped.transform.translation.x = -0.11 453 | imu_static_transform_stamped.transform.translation.y = 0.18 454 | imu_static_transform_stamped.transform.translation.z = 0.71 455 | 456 | quat = tf.transformations.quaternion_from_euler(0, 0, 0) 457 | imu_static_transform_stamped.transform.rotation.x = quat[0] 458 | imu_static_transform_stamped.transform.rotation.y = quat[1] 459 | imu_static_transform_stamped.transform.rotation.z = quat[2] 460 | imu_static_transform_stamped.transform.rotation.w = quat[3] 461 | 462 | # publish static transform 463 | tf_static_msg = tf2_msgs.msg.TFMessage([imu_static_transform_stamped]) 464 | 465 | return imu, mag, timestamp, tf_static_msg 466 | 467 | def gt_to_odometry(self, gt_list, gt_cov_list, i): 468 | """converts ground_truth odometry to ROS odometry messages 469 | 470 | :param gt_list: list containing the ground truth data 471 | :param i: row counter 472 | 473 | :return: fill bag with gt, timestamp, tf_static_msg 474 | """ 475 | 476 | # load data from list 477 | utime = gt_list[i, 0] 478 | x = gt_list[i, 1] 479 | y = gt_list[i, 2] 480 | z = gt_list[i, 3] 481 | roll_rad = gt_list[i, 4] 482 | pitch_rad = gt_list[i, 5] 483 | yaw_rad = gt_list[i, 6] 484 | 485 | # get upper diagonal of the covariance matrix 486 | xx = gt_cov_list[self.i_gt, 1]; xy = gt_cov_list[self.i_gt, 2]; xz = gt_cov_list[self.i_gt, 3]; xr = gt_cov_list[self.i_gt, 4] 487 | xp = gt_cov_list[self.i_gt, 5]; xh = gt_cov_list[self.i_gt, 6]; yy = gt_cov_list[self.i_gt, 7]; yz = gt_cov_list[self.i_gt, 8] 488 | yr = gt_cov_list[self.i_gt, 9]; yp = gt_cov_list[self.i_gt, 10]; yh = gt_cov_list[self.i_gt, 11]; zz = gt_cov_list[self.i_gt, 12] 489 | zr = gt_cov_list[self.i_gt, 13]; zp = gt_cov_list[self.i_gt, 14]; zh = gt_cov_list[self.i_gt, 15]; rr = gt_cov_list[self.i_gt, 16] 490 | rp = gt_cov_list[self.i_gt, 17]; rh = gt_cov_list[self.i_gt, 18]; pp = gt_cov_list[self.i_gt, 19]; ph = gt_cov_list[self.i_gt, 20] 491 | hh = gt_cov_list[self.i_gt, 21] 492 | 493 | # load utime from gt_cov list 494 | utime_gt_cov = gt_cov_list[self.i_gt, 0] 495 | 496 | # create ros timestamp 497 | timestamp = rospy.Time.from_sec(utime / 1e6) 498 | 499 | # get ground truth link 500 | gt_link = str(self.ground_truth_frame) 501 | 502 | # create odometry message for ground truth 503 | gt = Odometry() 504 | gt.header.stamp = timestamp 505 | gt.header.frame_id = gt_link 506 | # odom.child_frame_id = 'base_link' 507 | 508 | # change due to the ENU frame 509 | gt.pose.pose.position.x = y - 107.724666286 # x # TODO not hardcoding 510 | gt.pose.pose.position.y = x - 75.829339527800 # y 511 | gt.pose.pose.position.z = -z + 3.272894625813646 # z 512 | 513 | # create quaternion from euler angles 514 | quaternion = tf.transformations.quaternion_from_euler(roll_rad, pitch_rad, yaw_rad) 515 | gt.pose.pose.orientation.x = quaternion[0] 516 | gt.pose.pose.orientation.y = quaternion[1] 517 | gt.pose.pose.orientation.z = quaternion[2] 518 | gt.pose.pose.orientation.w = quaternion[3] 519 | 520 | # fill covariance matrices 521 | if utime > utime_gt_cov: 522 | gt.pose.covariance[0] = xx; gt.pose.covariance[1] = xy; gt.pose.covariance[2] = xz; gt.pose.covariance[3] = xr 523 | gt.pose.covariance[4] = xp; gt.pose.covariance[5] = xh 524 | 525 | gt.pose.covariance[7] = yy; gt.pose.covariance[8] = yz; gt.pose.covariance[9] = yr; gt.pose.covariance[10] = yp 526 | gt.pose.covariance[11] = yh 527 | 528 | gt.pose.covariance[14] = zz; gt.pose.covariance[15] = zr; gt.pose.covariance[16] = zp; gt.pose.covariance[17] = zh 529 | 530 | gt.pose.covariance[21] = rr; gt.pose.covariance[22] = rp; gt.pose.covariance[23] = rh 531 | 532 | gt.pose.covariance[28] = pp; gt.pose.covariance[29] = ph 533 | 534 | gt.pose.covariance[35] = hh 535 | 536 | self.last_gt_cov = gt.pose.covariance 537 | self.i_gt += 1 538 | 539 | elif self.init_gt_cov: 540 | gt.pose.covariance = self.last_gt_cov 541 | 542 | else: 543 | gt.pose.covariance = POSE_COVAR 544 | 545 | self.init_gt_cov = True 546 | self.last_gt_cov = gt.pose.covariance 547 | 548 | # TODO fill TWIST 549 | gt.twist.twist.linear.x = 0.0 550 | gt.twist.twist.linear.y = 0.0 551 | gt.twist.twist.linear.z = 0.0 552 | gt.twist.twist.angular.x = 0.0 553 | gt.twist.twist.angular.y = 0.0 554 | gt.twist.twist.angular.z = 0.0 555 | 556 | # create world ground truth static transformer 557 | gt_static_transform_stamped = geometry_msgs.msg.TransformStamped() 558 | gt_static_transform_stamped.header.stamp = timestamp 559 | gt_static_transform_stamped.header.frame_id = "world" 560 | gt_static_transform_stamped.child_frame_id = gt_link 561 | 562 | gt_static_transform_stamped.transform.translation.x = 0 563 | gt_static_transform_stamped.transform.translation.y = 0 564 | gt_static_transform_stamped.transform.translation.z = 0 565 | 566 | quat = tf.transformations.quaternion_from_euler(0, 0, 0) 567 | gt_static_transform_stamped.transform.rotation.x = quat[0] 568 | gt_static_transform_stamped.transform.rotation.y = quat[1] 569 | gt_static_transform_stamped.transform.rotation.z = quat[2] 570 | gt_static_transform_stamped.transform.rotation.w = quat[3] 571 | 572 | # publish static transform 573 | tf_static_msg = tf2_msgs.msg.TFMessage([gt_static_transform_stamped]) 574 | 575 | return gt, timestamp, tf_static_msg 576 | -------------------------------------------------------------------------------- /nclt2ros/transformer/velodyne_data.py: -------------------------------------------------------------------------------- 1 | import struct 2 | import numpy as np 3 | import rospy 4 | 5 | from sensor_msgs.msg import PointCloud2, PointField 6 | from nclt2ros.extractor.base_raw_data import BaseRawData 7 | from nclt2ros.converter.base_convert import BaseConvert 8 | 9 | 10 | class VelodyneData(BaseRawData, BaseConvert): 11 | """Class to convert the velodyne binary file to ROS PointCloud2 messages 12 | 13 | USAGE: 14 | VelodyneData('2013-01-10') 15 | 16 | """ 17 | def __init__(self, date): 18 | 19 | # init base class 20 | BaseRawData.__init__(self, date=date) 21 | BaseConvert.__init__(self, date=date) 22 | 23 | # load velodyne_binary file 24 | if self.velodyne_data_flag: 25 | self.f_bin_velodyne = open(self.velodyne_data_dir + '/%s/velodyne_hits.bin' % self.date, 'r') 26 | else: 27 | raise ValueError('velodyne_data directory not exists') 28 | 29 | def verify_magic(self, s): 30 | """verifies the binary data 31 | 32 | :param s: 33 | :return: True, if data is correct 34 | False 35 | """ 36 | 37 | magic = 44444 38 | m = struct.unpack('= 3) and (m[0] == magic) and (m[1] == magic) and (m[2] == magic) and (m[3] == magic) 41 | 42 | def convert_velodyne(self, x_s, y_s, z_s): 43 | """converts the velodyne binary data to corrected values, check out the paper http://robots.engin.umich.edu/nclt/nclt.pdf 44 | 45 | :param x_s: x value from binary file 46 | :param y_s: y value from binary file 47 | :param z_s: z value from binary file 48 | 49 | :return: converted x, y, z values 50 | """ 51 | scaling = 0.005 52 | offset = -100.0 53 | 54 | x = x_s * scaling + offset 55 | y = y_s * scaling + offset 56 | z = z_s * scaling + offset 57 | 58 | return x, y, z 59 | 60 | def read_next_velodyne_packet(self): 61 | """reads the velodyne binary file 62 | 63 | :return: utime: 64 | data: 65 | num_hits: 66 | """ 67 | try: 68 | 69 | magic = self.f_bin_velodyne.read(8) 70 | if magic == '': # EOF reached 71 | return -1, None 72 | 73 | if not self.verify_magic(magic): 74 | print "Could not verify magic" 75 | return -1, None 76 | 77 | num_hits = struct.unpack(' 0) 117 | 118 | NUM_FIELDS = 5 119 | assert(np.mod(num_values, NUM_FIELDS) == 0) 120 | 121 | num_points = num_values / NUM_FIELDS 122 | 123 | assert(len(points.shape) == 1) 124 | pc2_msg.height = 1 125 | 126 | FLOAT_SIZE_BYTES = 4 127 | pc2_msg.width = num_values * FLOAT_SIZE_BYTES 128 | 129 | pc2_msg.fields = [ 130 | PointField('x', 0, PointField.FLOAT32, 1), 131 | PointField('y', 4, PointField.FLOAT32, 1), 132 | PointField('z', 8, PointField.FLOAT32, 1), 133 | PointField('i', 12, PointField.FLOAT32, 1), 134 | PointField('l', 16, PointField.FLOAT32, 1) 135 | ] 136 | 137 | pc2_msg.is_bigendian = False 138 | pc2_msg.point_step = NUM_FIELDS * FLOAT_SIZE_BYTES 139 | 140 | pc2_msg.row_step = pc2_msg.point_step * num_points 141 | pc2_msg.is_dense = False 142 | 143 | pc2_msg.width = num_points 144 | pc2_msg.data = np.asarray(points, np.float32).tostring() 145 | 146 | return timestamp, pc2_msg -------------------------------------------------------------------------------- /nclt2ros/transformer/velodyne_sync_data.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | import struct 3 | import os 4 | import numpy as np 5 | import geometry_msgs.msg 6 | import tf2_msgs.msg 7 | import tf.transformations 8 | 9 | from sensor_msgs.msg import PointCloud2, PointField 10 | from nclt2ros.extractor.base_raw_data import BaseRawData 11 | from nclt2ros.converter.base_convert import BaseConvert 12 | 13 | 14 | class VelodyneSyncData(BaseRawData, BaseConvert): 15 | """Class to transform the velodyne_sync binary files to ROS PointCloud2 messages 16 | 17 | USAGE: 18 | VelodyneSyncData('2013-01-10') 19 | 20 | """ 21 | def __init__(self, date): 22 | 23 | # init base classes 24 | BaseRawData.__init__(self, date=date) 25 | BaseConvert.__init__(self, date=date) 26 | 27 | def get_velodyne_sync_timestamps_and_files(self): 28 | """returns the timestamps and binary files in a sorted manner 29 | 30 | :return: timestamps_microsec: List, containing the sorted timestamps 31 | bin_files: List, containing the sorted binary files 32 | """ 33 | if self.velodyne_data_flag: 34 | files = os.listdir(self.velodyne_sync_data_dir) 35 | else: 36 | raise ValueError('velodyne_data not exists') 37 | 38 | timestamps_microsec = sorted([long(os.path.splitext(f)[0]) for f in files if f.endswith('.bin')]) 39 | bin_files = sorted([f for f in files if f.endswith('.bin')]) 40 | 41 | return timestamps_microsec, bin_files 42 | 43 | def convert_velodyne(self, x_s, y_s, z_s): 44 | """converts the velodyne binary data to corrected values, check out the paper http://robots.engin.umich.edu/nclt/nclt.pdf 45 | 46 | :param x_s: x value from binary file 47 | :param y_s: y value from binary file 48 | :param z_s: z value from binary file 49 | 50 | :return: converted x, y, z values 51 | """ 52 | 53 | scaling = 0.005 54 | offset = -100.0 55 | 56 | x = x_s * scaling + offset 57 | y = y_s * scaling + offset 58 | z = z_s * scaling + offset 59 | 60 | return x, y, z 61 | 62 | def read_next_velodyne_sync_packet(self, file): 63 | """reads one velodyne sync binary file, equals one revolution of the velodyne sensor 64 | 65 | :param file: one binary file from the velodyne_sync directory 66 | 67 | :return: hits: list, containing the x, y, z, intensity, laser_id of the pointcloud 68 | """ 69 | 70 | try: 71 | os.chdir(self.velodyne_sync_data_dir) 72 | f_bin = open(file, "r") 73 | 74 | hits = [] 75 | while True: 76 | 77 | x_str = f_bin.read(2) 78 | if x_str == '': # eof 79 | break 80 | 81 | x = struct.unpack(' 0) 124 | 125 | NUM_FIELDS = 5 126 | assert(np.mod(num_values, NUM_FIELDS) == 0) 127 | 128 | num_points = num_values / NUM_FIELDS 129 | 130 | assert(len(points.shape) == 1) 131 | pc2_msg.height = 1 132 | 133 | FLOAT_SIZE_BYTES = 4 134 | pc2_msg.width = num_values * FLOAT_SIZE_BYTES 135 | 136 | pc2_msg.fields = [ 137 | PointField('x', 0, PointField.FLOAT32, 1), 138 | PointField('y', 4, PointField.FLOAT32, 1), 139 | PointField('z', 8, PointField.FLOAT32, 1), 140 | PointField('i', 12, PointField.FLOAT32, 1), 141 | PointField('l', 16, PointField.FLOAT32, 1) 142 | ] 143 | 144 | pc2_msg.is_bigendian = False 145 | pc2_msg.point_step = NUM_FIELDS * FLOAT_SIZE_BYTES 146 | 147 | pc2_msg.row_step = pc2_msg.point_step * num_points 148 | pc2_msg.is_dense = False 149 | 150 | pc2_msg.width = num_points 151 | pc2_msg.data = np.asarray(points, np.float32).tostring() 152 | 153 | # create base_link velodyne_link static transformer 154 | vel_static_transform_stamped = geometry_msgs.msg.TransformStamped() 155 | vel_static_transform_stamped.header.stamp = timestamp 156 | vel_static_transform_stamped.header.frame_id = base_link 157 | vel_static_transform_stamped.child_frame_id = velodyne_link 158 | 159 | vel_static_transform_stamped.transform.translation.x = 0.002 160 | vel_static_transform_stamped.transform.translation.y = 0.004 161 | vel_static_transform_stamped.transform.translation.z = 0.957 162 | 163 | quat = tf.transformations.quaternion_from_euler(0, 0, 0) 164 | vel_static_transform_stamped.transform.rotation.x = quat[0] 165 | vel_static_transform_stamped.transform.rotation.y = quat[1] 166 | vel_static_transform_stamped.transform.rotation.z = quat[2] 167 | vel_static_transform_stamped.transform.rotation.w = quat[3] 168 | 169 | # publish static transform 170 | tf_static_msg = tf2_msgs.msg.TFMessage([vel_static_transform_stamped]) 171 | 172 | return timestamp, pc2_msg, tf_static_msg 173 | -------------------------------------------------------------------------------- /nclt2ros/visualizer/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bierschi/nclt2ros/77b30ca6750d4b0cd82ccb6660f2fd0946581091/nclt2ros/visualizer/__init__.py -------------------------------------------------------------------------------- /nclt2ros/visualizer/all.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | from nclt2ros.visualizer.plotter import Plotter 3 | from nclt2ros.visualizer.gt import GroundTruth 4 | from nclt2ros.visualizer.gps_rtk import GPS_RTK 5 | from nclt2ros.visualizer.gps import GPS 6 | from nclt2ros.visualizer.wheel_odom import WheelOdom 7 | 8 | 9 | class AllSensors(Plotter, GroundTruth, GPS_RTK, GPS, WheelOdom): 10 | """Class to visualize all sensor data in one plot 11 | 12 | USAGE: 13 | AllSensors('2013-01-10', plt_show=True) 14 | 15 | """ 16 | def __init__(self, date, plt_show=True): 17 | 18 | self.date = date 19 | 20 | # init base classes 21 | Plotter.__init__(self, date=self.date) 22 | GroundTruth.__init__(self, date=self.date) 23 | GPS_RTK.__init__(self, date=self.date) 24 | GPS.__init__(self, date=self.date) 25 | WheelOdom.__init__(self, date=self.date) 26 | 27 | self.plt_show = plt_show 28 | 29 | def plot(self): 30 | """visualize all data in one plot 31 | """ 32 | 33 | gt_x, gt_y = self.get_gt_data() 34 | gps_rtk_x, gps_rtk_y = self.get_gps_rtk_data() 35 | gps_x, gps_y = self.get_gps_data() 36 | wheel_odom_x, wheel_odom_y = self.get_wheel_odom_data() 37 | print(self.plt_show) 38 | gt = plt.plot(gt_y, gt_x, color="lime", label='ground truth') # swap x,y coordinate axes! 39 | gps_rtk = plt.plot(gps_rtk_y, gps_rtk_x, 'r-', label='gps rtk') # swap x,y coordinate axes! 40 | gps = plt.plot(gps_y, gps_x, 'y-', label='gps') # swap x,y coordinate axes! 41 | wheel_odom = plt.plot(wheel_odom_y, wheel_odom_x, 'm-', label='wheel odom') # swap x,y coordinate axes! 42 | 43 | plt.title('All Sensors') 44 | plt.xlabel('x in meter') 45 | plt.ylabel('y in meter') 46 | plt.legend(loc='upper left') 47 | 48 | plt.grid() 49 | plt.savefig(self.visualization_png_all_dir + 'raw_data_all.png') 50 | 51 | if self.plt_show: 52 | plt.show() 53 | 54 | def get_png_all_dir(self): 55 | """get the png all sensors directory 56 | 57 | :return: path to png all sensors directory 58 | """ 59 | return self.visualization_png_all_dir 60 | 61 | 62 | if __name__ == '__main__': 63 | all =AllSensors('2012-01-15', plt_show=False) 64 | all.plot() 65 | -------------------------------------------------------------------------------- /nclt2ros/visualizer/gps.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | 5 | from nclt2ros.visualizer.plotter import Plotter 6 | from nclt2ros.transformer.coordinate_frame import CoordinateFrame 7 | 8 | 9 | class GPS(Plotter): 10 | """Class to visualize the GPS data as a kml and png file 11 | 12 | USAGE: 13 | GPS(date='2013-01-10', output_file='gps', plt_show=True) 14 | 15 | """ 16 | def __init__(self, date, output_file='gps', plt_show=True): 17 | 18 | if isinstance(output_file, str): 19 | self.output_file = output_file 20 | else: 21 | raise TypeError("'output_file' must be type of string") 22 | 23 | self.date = date 24 | self.plt_show = plt_show 25 | 26 | # init base class 27 | Plotter.__init__(self, date=self.date) 28 | 29 | # transformer coordinate frame into 'odom' or 'gt' 30 | if self.date == '2013-01-10': 31 | self.gps_converter = CoordinateFrame(origin='odom') 32 | else: 33 | self.gps_converter = CoordinateFrame(origin='gt') 34 | 35 | # load data 36 | self.gps = self.reader.read_gps_csv(all_in_one=True) 37 | 38 | def save_kml_line(self): 39 | """visualize the gps data as a kml file 40 | """ 41 | 42 | lat = self.gps[:, 3] 43 | lng = self.gps[:, 4] 44 | gps_list = list() 45 | 46 | for (i, j) in zip(lat, lng): 47 | 48 | if not math.isnan(i) and not math.isnan(j): 49 | tup = (np.rad2deg(j), np.rad2deg(i)) # swap and convert lat long to deg 50 | gps_list.append(tup) 51 | 52 | ls = self.kml.newlinestring(name="gps", coords=gps_list, description="latitude and longitude from gps") 53 | ls.style.linestyle.width = 1 54 | ls.style.linestyle.color = self.yellow 55 | 56 | self.kml.save(self.visualization_kml_dir + self.output_file + '.kml') 57 | 58 | def get_gps_data(self): 59 | """get gps data for visualization 60 | 61 | :return: list for x coordinates, list for y coordinates 62 | """ 63 | lat = self.gps[:, 3] 64 | lng = self.gps[:, 4] 65 | x_list = list() 66 | y_list = list() 67 | 68 | for (i, j) in zip(lat, lng): 69 | 70 | if not math.isnan(i) and not math.isnan(j): 71 | x = self.gps_converter.get_x(lat=np.rad2deg(i)) 72 | y = self.gps_converter.get_y(lon=np.rad2deg(j)) 73 | x_list.append(x) 74 | y_list.append(y) 75 | 76 | return x_list, y_list 77 | 78 | def save_gps_png(self): 79 | """visualize the gps data as a kml file 80 | """ 81 | 82 | x_list, y_list = self.get_gps_data() 83 | 84 | plt.plot(y_list, x_list, 'y-', label='gps') 85 | plt.title('GPS') 86 | plt.xlabel('x in meter') 87 | plt.ylabel('y in meter') 88 | plt.legend(loc='upper left') 89 | 90 | plt.grid() 91 | plt.savefig(self.visualization_png_gps_dir + 'gps.png') 92 | if self.plt_show: 93 | plt.show() 94 | 95 | def get_png_gps_dir(self): 96 | """get the png gps directory 97 | 98 | :return: path to png gps directory 99 | """ 100 | return self.visualization_png_gps_dir 101 | 102 | 103 | if __name__ == '__main__': 104 | gps = GPS(date='2012-01-08') 105 | gps.save_kml_line() 106 | 107 | -------------------------------------------------------------------------------- /nclt2ros/visualizer/gps_rtk.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | 5 | from nclt2ros.visualizer.plotter import Plotter 6 | from nclt2ros.transformer.coordinate_frame import CoordinateFrame 7 | 8 | 9 | class GPS_RTK(Plotter): 10 | """Class to visualize the GPS RTK data as a kml and png file 11 | 12 | USAGE: 13 | GPS_RTK(date='2013-01-10', output_file='gps_rtk', plt_show=True) 14 | 15 | """ 16 | def __init__(self, date, output_file='gps_rtk', plt_show=True): 17 | 18 | if isinstance(output_file, str): 19 | self.output_file = output_file 20 | else: 21 | raise TypeError("'output_file' must be type of string") 22 | 23 | self.date = date 24 | self.plt_show = plt_show 25 | 26 | # init base class 27 | Plotter.__init__(self, date=self.date) 28 | 29 | # transform coordinate frame into 'odom' or 'gt' 30 | if self.date == '2013-01-10': 31 | self.gps_rtk_converter = CoordinateFrame(origin='odom') 32 | else: 33 | self.gps_rtk_converter = CoordinateFrame(origin='gt') 34 | 35 | # load data 36 | self.gps_rtk = self.reader.read_gps_rtk_csv(all_in_one=True) 37 | 38 | def save_kml_line(self): 39 | """visualize the gps rtk data as a kml file 40 | """ 41 | 42 | lat = self.gps_rtk[:, 3] 43 | lng = self.gps_rtk[:, 4] 44 | gps_rtk_list = list() 45 | 46 | for (i_lat, j_lng) in zip(lat, lng): 47 | 48 | if not math.isnan(i_lat) and not math.isnan(j_lng): 49 | tup = (np.rad2deg(j_lng), np.rad2deg(i_lat)) # swap and convert lat long to deg 50 | gps_rtk_list.append(tup) 51 | 52 | ls = self.kml.newlinestring(name="gps rtk", coords=gps_rtk_list, description="latitude and longitude from gps rtk") 53 | ls.style.linestyle.width = 1 54 | ls.style.linestyle.color = self.red 55 | 56 | self.kml.save(self.visualization_kml_dir + self.output_file + '.kml') 57 | 58 | def get_gps_rtk_data(self): 59 | """get gps rtk data for visualization 60 | 61 | :return: list for x coordinates, list for y coordinates 62 | """ 63 | lat = self.gps_rtk[:, 3] 64 | lng = self.gps_rtk[:, 4] 65 | x_list = list() 66 | y_list = list() 67 | 68 | for (i_lat, j_lng) in zip(lat, lng): 69 | 70 | if not math.isnan(i_lat) and not math.isnan(j_lng): 71 | x = self.gps_rtk_converter.get_x(lat=np.rad2deg(i_lat)) 72 | y = self.gps_rtk_converter.get_y(lon=np.rad2deg(j_lng)) 73 | x_list.append(x) 74 | y_list.append(y) 75 | 76 | return x_list, y_list 77 | 78 | def save_gps_rtk_png(self): 79 | """visualize the gps rtk data as a png file 80 | 81 | """ 82 | 83 | x_list, y_list = self.get_gps_rtk_data() 84 | 85 | plt.plot(y_list, x_list, 'r-', label='gps rtk') 86 | plt.title('GPS RTK') 87 | plt.xlabel('x in meter') 88 | plt.ylabel('y in meter') 89 | plt.legend(loc='upper left') 90 | 91 | plt.grid() 92 | plt.savefig(self.visualization_png_gps_rtk_dir + 'gps_rtk.png') 93 | 94 | if self.plt_show: 95 | plt.show() 96 | 97 | def get_png_gps_rtk_dir(self): 98 | """get the png gps rtk directory 99 | 100 | :return: path to png gps rtk directory 101 | """ 102 | return self.visualization_png_gps_rtk_dir 103 | 104 | 105 | if __name__ == '__main__': 106 | gps = GPS_RTK(date='2012-01-08') 107 | gps.save_gps_rtk_png() -------------------------------------------------------------------------------- /nclt2ros/visualizer/gt.py: -------------------------------------------------------------------------------- 1 | import math 2 | import matplotlib.pyplot as plt 3 | 4 | from nclt2ros.visualizer.plotter import Plotter 5 | from nclt2ros.transformer.coordinate_frame import CoordinateFrame 6 | 7 | 8 | class GroundTruth(Plotter): 9 | """Class to visualize the ground truth data as a kml and png file 10 | 11 | USAGE: 12 | GroundTruth(date='2013-01-10', output_file='ground_truth', plt_show=True) 13 | 14 | """ 15 | def __init__(self, date, output_file='ground_truth', plt_show=True): 16 | 17 | if isinstance(output_file, str): 18 | self.output_file = output_file 19 | else: 20 | raise TypeError("'output_file' must be type of string") 21 | 22 | self.date = date 23 | self.plt_show = plt_show 24 | 25 | # init base class 26 | Plotter.__init__(self, date=self.date) 27 | 28 | # transformer coordinate frame into 'gt' 29 | self.gt_converter = CoordinateFrame(origin='gt') 30 | 31 | # load gt data 32 | self.gt = self.reader_gt.read_gt_csv(all_in_one=True) 33 | 34 | def save_kml_line(self): 35 | """visualize the ground truth as a kml file 36 | """ 37 | 38 | gt_x = self.gt[:, 1] 39 | gt_y = self.gt[:, 2] 40 | gt_list = list() 41 | 42 | if len(gt_x) > 600000: 43 | divider = 35 44 | elif len(gt_x) > 200000: 45 | divider = 15 46 | else: 47 | divider = 5 48 | 49 | for row, (x_i, y_j) in enumerate(zip(gt_x, gt_y)): 50 | 51 | if not math.isnan(x_i) and not math.isnan(y_j): 52 | lat = self.gt_converter.get_lat(x=x_i) 53 | lng = self.gt_converter.get_lon(y=y_j) 54 | 55 | tup = (lng, lat) 56 | 57 | # minimize the elements in the kml output file 58 | if (row % divider) == 0: 59 | gt_list.append(tup) 60 | 61 | # create line string 62 | ls = self.kml.newlinestring(name="ground truth", coords=gt_list, description="latitude and longitude from ground truth",) 63 | ls.style.linestyle.width = 1 64 | ls.style.linestyle.color = self.green 65 | 66 | # save kml file in visualization directory 67 | self.kml.save(self.visualization_kml_dir + self.output_file + '.kml') 68 | 69 | def get_gt_data(self): 70 | """get ground truth data for visualization 71 | 72 | :return: list for x coordinates, list for y coordinates 73 | """ 74 | gt_x = self.gt[:, 1] 75 | gt_y = self.gt[:, 2] 76 | first_x_coord = gt_x[0] 77 | first_y_coord = gt_y[0] 78 | x_new = list() 79 | y_new = list() 80 | 81 | for row, (x_i, y_j) in enumerate(zip(gt_x, gt_y)): 82 | 83 | if not math.isnan(x_i) and not math.isnan(y_j): 84 | # eliminate offset in this dataset 85 | if self.date == '2013-01-10': 86 | x_i = x_i - first_x_coord 87 | y_j = y_j - first_y_coord 88 | y_new.append(y_j) 89 | x_new.append(x_i) 90 | else: 91 | y_new.append(y_j) 92 | x_new.append(x_i) 93 | 94 | return x_new, y_new 95 | 96 | def save_gt_png(self): 97 | """visualize the ground truth as a png file 98 | 99 | :param offset: Boolean, True if eliminate the offset between odom and ground truth coordinate frame 100 | """ 101 | 102 | x_new, y_new = self.get_gt_data() 103 | 104 | plt.plot(y_new, x_new, color="lime", label='ground truth') 105 | 106 | plt.xlabel('x in meter') 107 | plt.ylabel('y in meter') 108 | plt.legend(loc='upper left') 109 | 110 | plt.grid() 111 | 112 | plt.title('Ground Truth') 113 | plt.savefig(self.visualization_png_gt_dir + self.output_file + '.png') 114 | 115 | if self.plt_show: 116 | plt.show() 117 | 118 | def save_roll_png(self): 119 | """visualize the roll angle as a png file 120 | """ 121 | 122 | utimes = self.gt[:, 0] 123 | roll_rad = self.gt[:, 4] 124 | plt.plot(utimes, roll_rad, color="blue", label='roll angle') 125 | 126 | plt.xlabel('time in sec') 127 | plt.ylabel('roll in rad') 128 | plt.legend(loc='upper left') 129 | plt.grid() 130 | 131 | plt.title('Roll angle from Ground Truth') 132 | plt.savefig(self.visualization_png_gt_dir + self.output_file + '_roll.png') 133 | plt.show() 134 | 135 | def save_pitch_png(self): 136 | """visualize the pitch angle as a png file 137 | """ 138 | utimes = self.gt[:, 0] 139 | pitch_rad = self.gt[:, 5] 140 | 141 | plt.plot(utimes, pitch_rad, color="blue", label='pitch angle') 142 | 143 | plt.xlabel('time in sec') 144 | plt.ylabel('pitch in rad') 145 | plt.legend(loc='upper left') 146 | plt.grid() 147 | 148 | plt.title('Pitch angle from Ground Truth') 149 | plt.savefig(self.visualization_png_gt_dir + self.output_file + '_pitch.png') 150 | plt.show() 151 | 152 | def save_yaw_png(self): 153 | """visualize the yaw angle as a png file 154 | """ 155 | utimes = self.gt[:, 0] 156 | yaw_rad = self.gt[:, 6] 157 | 158 | plt.plot(utimes, yaw_rad, color="blue", label='yaw angle') 159 | 160 | plt.xlabel('time in sec') 161 | plt.ylabel('yaw in rad') 162 | plt.legend(loc='upper left') 163 | plt.grid() 164 | 165 | plt.title('Yaw angle from Ground Truth') 166 | plt.savefig(self.visualization_png_gt_dir + self.output_file + '_yaw.png') 167 | plt.show() 168 | 169 | def get_png_gt_dir(self): 170 | """get the png ground truth directory 171 | 172 | :return: path to png ground truth directory 173 | """ 174 | return self.visualization_png_gt_dir 175 | 176 | 177 | if __name__ == '__main__': 178 | gt = GroundTruth(date='2012-01-15') 179 | gt.save_gt_png() 180 | -------------------------------------------------------------------------------- /nclt2ros/visualizer/plotter.py: -------------------------------------------------------------------------------- 1 | import os 2 | import simplekml 3 | import rospy 4 | from definitions import ROOT_DIR 5 | from nclt2ros.extractor.read_sensor_data import ReadSensorData 6 | from nclt2ros.extractor.read_ground_truth import ReadGroundTruth 7 | 8 | 9 | class Plotter: 10 | """Base class for visualizing 11 | 12 | USAGE: 13 | Plotter('2013-01-10') 14 | 15 | """ 16 | def __init__(self, date): 17 | 18 | if isinstance(date, str): 19 | self.date = date 20 | self.reader = ReadSensorData(date=date) 21 | self.reader_gt = ReadGroundTruth(date=date) 22 | else: 23 | raise TypeError('"date" must be of type string') 24 | 25 | PLOT_PATH_DFLT = ROOT_DIR + '/nclt2ros/plots/' 26 | PLOT_PATH_DFLT = '/home/christian/nclt2ros/plots/' # TODO remove, only for debugging 27 | self.plot_path = rospy.get_param('~plot_path', PLOT_PATH_DFLT) 28 | 29 | if self.plot_path.endswith('/'): 30 | self.plot_dir = self.plot_path 31 | else: 32 | self.plot_dir = self.plot_path + '/' 33 | 34 | # define directories 35 | self.visualization_kml_dir = self.plot_dir + '%s/kml/' % self.date 36 | self.visualization_png_gt_dir = self.plot_dir + '%s/png/gt/' % self.date 37 | self.visualization_png_gps_rtk_dir = self.plot_dir + '%s/png/gps_rtk/' % self.date 38 | self.visualization_png_gps_dir = self.plot_dir + '%s/png/gps/' % self.date 39 | self.visualization_png_wheel_odom_dir = self.plot_dir + '%s/png/wheel_odom/' % self.date 40 | self.visualization_png_all_dir = self.plot_dir + '%s/png/all/' % self.date 41 | 42 | # check directories 43 | if not os.path.exists(self.visualization_kml_dir): 44 | os.makedirs(self.visualization_kml_dir) 45 | 46 | if not os.path.exists(self.visualization_png_gt_dir): 47 | os.makedirs(self.visualization_png_gt_dir) 48 | 49 | if not os.path.exists(self.visualization_png_gps_rtk_dir): 50 | os.makedirs(self.visualization_png_gps_rtk_dir) 51 | 52 | if not os.path.exists(self.visualization_png_gps_dir): 53 | os.makedirs(self.visualization_png_gps_dir) 54 | 55 | if not os.path.exists(self.visualization_png_wheel_odom_dir): 56 | os.makedirs(self.visualization_png_wheel_odom_dir) 57 | 58 | if not os.path.exists(self.visualization_png_all_dir): 59 | os.makedirs(self.visualization_png_all_dir) 60 | 61 | # kml settings 62 | self.kml = simplekml.Kml() 63 | self.green = simplekml.Color.lime 64 | self.red = simplekml.Color.red 65 | self.yellow = simplekml.Color.yellow 66 | self.magenta = simplekml.Color.magenta 67 | 68 | def get_kml_dir(self): 69 | """get the kml directory 70 | 71 | :return: path to kml directory 72 | """ 73 | return self.visualization_kml_dir -------------------------------------------------------------------------------- /nclt2ros/visualizer/visualize.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | from nclt2ros.visualizer.gt import GroundTruth 3 | from nclt2ros.visualizer.gps import GPS 4 | from nclt2ros.visualizer.gps_rtk import GPS_RTK 5 | from nclt2ros.visualizer.wheel_odom import WheelOdom 6 | from nclt2ros.visualizer.all import AllSensors 7 | 8 | 9 | class Visualize: 10 | """Class to visualize the raw data depending on the parameter specified in the launch file 11 | 12 | USAGE: 13 | Visualize(date='2013-01-10') 14 | 15 | """ 16 | def __init__(self, date, show_plot, **kwargs): 17 | 18 | self.date = date 19 | 20 | if isinstance(show_plot, bool): 21 | self.show_plot = show_plot 22 | else: 23 | raise TypeError('show plot must be type of boolean') 24 | 25 | self.v_gt_kml = None 26 | self.v_gt_png = None 27 | self.v_gps_kml = None 28 | self.v_gps_png = None 29 | self.v_gps_rtk_kml = None 30 | self.v_gps_rtk_png = None 31 | self.v_odom_kml = None 32 | self.v_odom_png = None 33 | self.v_all = None 34 | 35 | for (key, value) in kwargs.iteritems(): 36 | if hasattr(self, key): 37 | setattr(self, key, value) 38 | 39 | # TODO expand with more commands 40 | 41 | if self.v_gt_kml: 42 | rospy.loginfo("visualize ground truth kml from date %s" % self.date) 43 | self.gt = GroundTruth(date=self.date, plt_show=show_plot) 44 | self.gt.save_kml_line() 45 | kml_dir = self.gt.get_kml_dir() 46 | rospy.loginfo("successfully created ground truth kml file in %s" % kml_dir) 47 | 48 | if self.v_gt_png: 49 | rospy.loginfo("visualize ground truth png from date %s" % self.date) 50 | self.gt = GroundTruth(date=self.date, plt_show=show_plot) 51 | self.gt.save_gt_png() 52 | png_gt_dir = self.gt.get_png_gt_dir() 53 | rospy.loginfo("successfully created ground truth png file in %s" % png_gt_dir) 54 | 55 | if self.v_gps_kml: 56 | rospy.loginfo("visualize gps kml from date %s" % self.date) 57 | self.gps = GPS(date=self.date, plt_show=show_plot) 58 | self.gps.save_kml_line() 59 | kml_dir = self.gps.get_kml_dir() 60 | rospy.loginfo("successfully created gps kml file in %s" % kml_dir) 61 | 62 | if self.v_gps_png: 63 | rospy.loginfo("visualize gps png from %s" % self.date) 64 | self.gps = GPS(date=self.date, plt_show=show_plot) 65 | self.gps.save_gps_png() 66 | png_gps_dir = self.gps.get_png_gps_dir() 67 | rospy.loginfo("successfully created gps png file in %s" % png_gps_dir) 68 | 69 | if self.v_gps_rtk_kml: 70 | rospy.loginfo("visualize gps_rtk kml from date %s" % self.date) 71 | self.gps_rtk = GPS_RTK(date=self.date, plt_show=show_plot) 72 | self.gps_rtk.save_kml_line() 73 | kml_dir = self.gps_rtk.get_kml_dir() 74 | rospy.loginfo("successfully created gps_rtk kml file in %s" % kml_dir) 75 | 76 | if self.v_gps_rtk_png: 77 | rospy.loginfo("visualize gps_rtk png from date %s" % self.date) 78 | self.gps_rtk = GPS_RTK(date=self.date, plt_show=show_plot) 79 | self.gps_rtk.save_gps_rtk_png() 80 | png_gps_rtk_dir = self.gps_rtk.get_png_gps_rtk_dir() 81 | rospy.loginfo("successfully created gps_rtk png file in %s" % png_gps_rtk_dir) 82 | 83 | if self.v_odom_kml: 84 | rospy.loginfo("visualize wheel odometry kml from date %s" % self.date) 85 | self.wheel_odom = WheelOdom(date=self.date, plt_show=show_plot) 86 | self.wheel_odom.save_kml_line() 87 | kml_dir = self.wheel_odom.get_kml_dir() 88 | rospy.loginfo("successfully created wheel odometry kml file in %s" % kml_dir) 89 | 90 | if self.v_odom_png: 91 | rospy.loginfo("visualize wheel odometry png from date %s" % self.date) 92 | self.wheel_odom = WheelOdom(date=self.date, plt_show=show_plot) 93 | self.wheel_odom.save_wheel_odom_png() 94 | png_odom_dir = self.wheel_odom.get_png_odom_dir() 95 | rospy.loginfo("successfully created wheel odometry png file in %s/" % png_odom_dir) 96 | 97 | if self.v_all: 98 | rospy.loginfo("visualizes all data from date %s" % self.date) 99 | self.all = AllSensors(date=self.date, plt_show=show_plot) 100 | self.all.plot() 101 | png_all_dir = self.all.get_png_all_dir() 102 | rospy.loginfo("successfully created raw_data_all png file in %s" % png_all_dir) 103 | 104 | else: 105 | rospy.loginfo("no matching parameters were found") 106 | 107 | -------------------------------------------------------------------------------- /nclt2ros/visualizer/wheel_odom.py: -------------------------------------------------------------------------------- 1 | import math 2 | import matplotlib.pyplot as plt 3 | from nclt2ros.visualizer.plotter import Plotter 4 | from nclt2ros.transformer.coordinate_frame import CoordinateFrame 5 | 6 | 7 | class WheelOdom(Plotter): 8 | """Class to visualize the wheel odometry data as a kml and png file 9 | 10 | USAGE: 11 | WheelOdom(date='2013-01-10', output_file='wheel_odom', plt_show=True) 12 | 13 | """ 14 | def __init__(self, date, output_file='wheel_odom', plt_show=True): 15 | 16 | if isinstance(output_file, str): 17 | self.output_file = output_file 18 | else: 19 | raise TypeError("'output_file' must be type of string") 20 | 21 | self.date = date 22 | self.plt_show = plt_show 23 | 24 | # init base class 25 | Plotter.__init__(self, date=self.date) 26 | 27 | # transformer coordinate frame into 'odom' or 'gt' 28 | if self.date == '2013-01-10' or self.date == '2012-01-15': 29 | self.odom_converter = CoordinateFrame(origin='odom') 30 | else: 31 | self.odom_converter = CoordinateFrame(origin='gt') 32 | 33 | # load data 34 | self.wheel_odom = self.reader.read_odometry_mu_100hz_csv(all_in_one=True) 35 | 36 | def save_kml_line(self): 37 | """visualize wheel odometry data as a kml file 38 | """ 39 | 40 | wheel_odom_x = self.wheel_odom[:, 1] 41 | wheel_odom_y = self.wheel_odom[:, 2] 42 | odom_list = list() 43 | 44 | if len(wheel_odom_x) > 200000: 45 | divider = 15 46 | else: 47 | divider = 5 48 | 49 | for row, (x_i, y_j) in enumerate(zip(wheel_odom_x, wheel_odom_y)): 50 | 51 | if not math.isnan(x_i) and not math.isnan(y_j): 52 | lat = self.odom_converter.get_lat(x=x_i) 53 | lng = self.odom_converter.get_lon(y=y_j) 54 | tup = (lng, lat) 55 | 56 | # minimize the elements in the kml output file 57 | if (row % divider) == 0: 58 | odom_list.append(tup) 59 | 60 | ls = self.kml.newlinestring(name="wheel odometry", coords=odom_list, description="latitute and longitude from wheel odometry") 61 | ls.style.linestyle.width = 1 62 | ls.style.linestyle.color = self.magenta 63 | 64 | self.kml.save(self.visualization_kml_dir + self.output_file + '.kml') 65 | 66 | def get_wheel_odom_data(self): 67 | """get wheel odometry data for visualization 68 | 69 | :return: list for x coordinates, list for y coordinates 70 | """ 71 | wheel_odom_x = self.wheel_odom[:, 1] 72 | wheel_odom_y = self.wheel_odom[:, 2] 73 | 74 | return wheel_odom_x, wheel_odom_y 75 | 76 | def save_wheel_odom_png(self): 77 | """visualize wheel odometry data as a png file 78 | """ 79 | wheel_odom_x, wheel_odom_y = self.get_wheel_odom_data() 80 | 81 | plt.plot(wheel_odom_y, wheel_odom_x, 'm-', label="wheel odom") 82 | plt.title('Wheel Odometry') 83 | plt.xlabel('x in meter') 84 | plt.ylabel('y in meter') 85 | plt.legend(loc='upper left') 86 | 87 | plt.grid() 88 | plt.savefig(self.visualization_png_wheel_odom_dir + 'wheel_odom.png') 89 | 90 | if self.plt_show: 91 | plt.show() 92 | 93 | def get_png_odom_dir(self): 94 | """get the png odom directory 95 | 96 | :return: path to png odom directory 97 | """ 98 | return self.visualization_png_wheel_odom_dir 99 | 100 | 101 | if __name__ == '__main__': 102 | odom = WheelOdom(date='2012-01-15') 103 | odom.save_wheel_odom_png() -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | nclt2ros 4 | 1.0.0 5 | nclt2ros package provides ROS nodes for downloading, extracting, converting and visualizing the NCLT Dataset 6 | 7 | christian bierschneider 8 | christian bierschneider 9 | 10 | MIT 11 | 12 | 13 | catkin 14 | 15 | roscpp 16 | rospy 17 | roslaunch 18 | 19 | roscpp 20 | rospy 21 | roslaunch 22 | 23 | roscpp 24 | rospy 25 | python-numpy 26 | python-matplotlib 27 | std_msgs 28 | sensor_msgs 29 | nav_msgs 30 | geometry_msgs 31 | tf 32 | 33 | simplekml 34 | 35 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | simplekml 2 | numpy 3 | matplotlib 4 | -------------------------------------------------------------------------------- /rviz/config.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 116 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Odometry2/Shape1 10 | - /Image1 11 | Splitter Ratio: 0.5 12 | Tree Height: 632 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.588679016 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: PointCloud2 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Alpha: 0.5 36 | Cell Size: 1 37 | Class: rviz/Grid 38 | Color: 160; 160; 164 39 | Enabled: true 40 | Line Style: 41 | Line Width: 0.0299999993 42 | Value: Lines 43 | Name: Grid 44 | Normal Cell Count: 0 45 | Offset: 46 | X: 0 47 | Y: 0 48 | Z: 0 49 | Plane: XY 50 | Plane Cell Count: 10 51 | Reference Frame: 52 | Value: true 53 | - Class: rviz/TF 54 | Enabled: true 55 | Frame Timeout: 15 56 | Frames: 57 | All Enabled: true 58 | base_link: 59 | Value: true 60 | camera: 61 | Value: true 62 | gps_link: 63 | Value: true 64 | gps_rtk_link: 65 | Value: true 66 | ground_truth_link: 67 | Value: true 68 | imu_link: 69 | Value: true 70 | laser_urg: 71 | Value: true 72 | laser_utm: 73 | Value: true 74 | odom: 75 | Value: true 76 | velodyne: 77 | Value: true 78 | world: 79 | Value: true 80 | Marker Scale: 1 81 | Name: TF 82 | Show Arrows: true 83 | Show Axes: true 84 | Show Names: true 85 | Tree: 86 | world: 87 | ground_truth_link: 88 | {} 89 | odom: 90 | base_link: 91 | camera: 92 | {} 93 | gps_link: 94 | {} 95 | gps_rtk_link: 96 | {} 97 | imu_link: 98 | {} 99 | laser_urg: 100 | {} 101 | laser_utm: 102 | {} 103 | velodyne: 104 | {} 105 | Update Interval: 0 106 | Value: true 107 | - Alpha: 1 108 | Autocompute Intensity Bounds: true 109 | Autocompute Value Bounds: 110 | Max Value: 17.5682011 111 | Min Value: -2.36935019 112 | Value: true 113 | Axis: Z 114 | Channel Name: intensity 115 | Class: rviz/PointCloud2 116 | Color: 255; 255; 255 117 | Color Transformer: AxisColor 118 | Decay Time: 0 119 | Enabled: true 120 | Invert Rainbow: false 121 | Max Color: 255; 255; 255 122 | Max Intensity: 4096 123 | Min Color: 0; 0; 0 124 | Min Intensity: 0 125 | Name: PointCloud2 126 | Position Transformer: XYZ 127 | Queue Size: 10 128 | Selectable: true 129 | Size (Pixels): 3 130 | Size (m): 0.0199999996 131 | Style: Flat Squares 132 | Topic: /velodyne_points 133 | Unreliable: false 134 | Use Fixed Frame: true 135 | Use rainbow: true 136 | Value: true 137 | - Alpha: 1 138 | Autocompute Intensity Bounds: true 139 | Autocompute Value Bounds: 140 | Max Value: 10 141 | Min Value: -10 142 | Value: true 143 | Axis: Z 144 | Channel Name: intensity 145 | Class: rviz/LaserScan 146 | Color: 255; 255; 255 147 | Color Transformer: Intensity 148 | Decay Time: 0 149 | Enabled: true 150 | Invert Rainbow: false 151 | Max Color: 255; 255; 255 152 | Max Intensity: 4096 153 | Min Color: 0; 0; 0 154 | Min Intensity: 0 155 | Name: LaserScan 156 | Position Transformer: XYZ 157 | Queue Size: 10 158 | Selectable: true 159 | Size (Pixels): 3 160 | Size (m): 0.0199999996 161 | Style: Flat Squares 162 | Topic: /hokuyo_30m 163 | Unreliable: false 164 | Use Fixed Frame: true 165 | Use rainbow: true 166 | Value: true 167 | - Alpha: 1 168 | Autocompute Intensity Bounds: true 169 | Autocompute Value Bounds: 170 | Max Value: 0.493303388 171 | Min Value: 0.304097891 172 | Value: true 173 | Axis: Z 174 | Channel Name: intensity 175 | Class: rviz/LaserScan 176 | Color: 255; 255; 255 177 | Color Transformer: AxisColor 178 | Decay Time: 0 179 | Enabled: true 180 | Invert Rainbow: false 181 | Max Color: 255; 255; 255 182 | Max Intensity: 4096 183 | Min Color: 0; 0; 0 184 | Min Intensity: 0 185 | Name: LaserScan 186 | Position Transformer: XYZ 187 | Queue Size: 10 188 | Selectable: true 189 | Size (Pixels): 3 190 | Size (m): 0.0199999996 191 | Style: Flat Squares 192 | Topic: /hokuyo_4m 193 | Unreliable: false 194 | Use Fixed Frame: true 195 | Use rainbow: true 196 | Value: true 197 | - Angle Tolerance: 0.100000001 198 | Class: rviz/Odometry 199 | Covariance: 200 | Orientation: 201 | Alpha: 0.5 202 | Color: 255; 255; 127 203 | Color Style: Unique 204 | Frame: Local 205 | Offset: 1 206 | Scale: 1 207 | Value: true 208 | Position: 209 | Alpha: 0.300000012 210 | Color: 204; 51; 204 211 | Scale: 1 212 | Value: true 213 | Value: false 214 | Enabled: true 215 | Keep: 0 216 | Name: Odometry 217 | Position Tolerance: 0.100000001 218 | Shape: 219 | Alpha: 1 220 | Axes Length: 1 221 | Axes Radius: 0.100000001 222 | Color: 255; 25; 0 223 | Head Length: 0.300000012 224 | Head Radius: 0.100000001 225 | Shaft Length: 1 226 | Shaft Radius: 0.0500000007 227 | Value: Arrow 228 | Topic: /odom 229 | Unreliable: false 230 | Value: true 231 | - Angle Tolerance: 0.100000001 232 | Class: rviz/Odometry 233 | Covariance: 234 | Orientation: 235 | Alpha: 0.5 236 | Color: 255; 255; 127 237 | Color Style: Unique 238 | Frame: Local 239 | Offset: 1 240 | Scale: 1 241 | Value: true 242 | Position: 243 | Alpha: 0.300000012 244 | Color: 204; 51; 204 245 | Scale: 1 246 | Value: true 247 | Value: false 248 | Enabled: true 249 | Keep: 0 250 | Name: Odometry 251 | Position Tolerance: 0.100000001 252 | Shape: 253 | Alpha: 1 254 | Axes Length: 1 255 | Axes Radius: 0.100000001 256 | Color: 0; 255; 0 257 | Head Length: 0.300000012 258 | Head Radius: 0.100000001 259 | Shaft Length: 1 260 | Shaft Radius: 0.0500000007 261 | Value: Arrow 262 | Topic: /ground_truth 263 | Unreliable: false 264 | Value: true 265 | - Class: rviz/Image 266 | Enabled: true 267 | Image Topic: /images/raw5 268 | Max Value: 1 269 | Median window: 5 270 | Min Value: 0 271 | Name: Image 272 | Normalize Range: true 273 | Queue Size: 2 274 | Transport Hint: raw 275 | Unreliable: false 276 | Value: true 277 | Enabled: true 278 | Global Options: 279 | Background Color: 48; 48; 48 280 | Default Light: true 281 | Fixed Frame: odom 282 | Frame Rate: 30 283 | Name: root 284 | Tools: 285 | - Class: rviz/Interact 286 | Hide Inactive Objects: true 287 | - Class: rviz/MoveCamera 288 | - Class: rviz/Select 289 | - Class: rviz/FocusCamera 290 | - Class: rviz/Measure 291 | - Class: rviz/SetInitialPose 292 | Topic: /initialpose 293 | - Class: rviz/SetGoal 294 | Topic: /move_base_simple/goal 295 | - Class: rviz/PublishPoint 296 | Single click: true 297 | Topic: /clicked_point 298 | Value: true 299 | Views: 300 | Current: 301 | Class: rviz/Orbit 302 | Distance: 41.4196281 303 | Enable Stereo Rendering: 304 | Stereo Eye Separation: 0.0599999987 305 | Stereo Focal Distance: 1 306 | Swap Stereo Eyes: false 307 | Value: false 308 | Focal Point: 309 | X: 0 310 | Y: 0 311 | Z: 0 312 | Focal Shape Fixed Size: false 313 | Focal Shape Size: 0.0500000007 314 | Invert Z Axis: false 315 | Name: Current View 316 | Near Clip Distance: 0.00999999978 317 | Pitch: 0.690397322 318 | Target Frame: 319 | Value: Orbit (rviz) 320 | Yaw: 3.44359231 321 | Saved: ~ 322 | Window Geometry: 323 | Camera: 324 | collapsed: false 325 | Displays: 326 | collapsed: false 327 | Height: 951 328 | Hide Left Dock: false 329 | Hide Right Dock: false 330 | Image: 331 | collapsed: false 332 | QMainWindow State: 000000ff00000000fd00000004000000000000016a0000032dfc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc000000280000032d000000d700fffffffa000000040100000005fb0000000a0049006d0061006700650000000000ffffffff0000000000000000fb0000000c00430061006d0065007200610000000000ffffffff0000006700000067fb0000000c00430061006d0065007200610000000000ffffffff0000006700000067fb0000000a0049006d0061006700650000000000ffffffff0000000000000000fb000000100044006900730070006c00610079007301000000000000016a0000016a00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006503000001c3000001e40000024f00000186000000010000010f0000032dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007301000000280000032d000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004ba0000032d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 333 | Selection: 334 | collapsed: false 335 | Time: 336 | collapsed: false 337 | Tool Properties: 338 | collapsed: false 339 | Views: 340 | collapsed: false 341 | Width: 1855 342 | X: 65 343 | Y: 24 344 | -------------------------------------------------------------------------------- /scripts/nclt2downloader: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from nclt2ros.downloader.download import Download 5 | from nclt2ros.extractor.extract import Extract 6 | from definitions import ROOT_DIR 7 | 8 | 9 | def main(): 10 | 11 | rospy.init_node('nclt2downloader') 12 | rospy.loginfo("running node 'nclt2downloader'") 13 | 14 | date = rospy.get_param('~date', '2013-01-10') 15 | 16 | RAW_DATA_PATH_DFLT = ROOT_DIR + '/nclt2ros/raw_data/' 17 | raw_data_path = rospy.get_param('~raw_data_path', RAW_DATA_PATH_DFLT) 18 | 19 | lb3 = rospy.get_param('~lb3', False) 20 | sen = rospy.get_param('~sen', False) 21 | vel = rospy.get_param('~vel', False) 22 | hokuyo = rospy.get_param('~hokuyo', False) 23 | gt = rospy.get_param('~gt', False) 24 | gt_cov = rospy.get_param('~gt_cov', False) 25 | 26 | loader = Download(date=date, raw_data_path=raw_data_path, lb3=lb3, sen=sen, vel=vel, hokuyo=hokuyo, gt=gt, gt_cov=gt_cov) 27 | 28 | extractor = Extract(date=date, lb3=lb3, sen=sen, vel=vel, hokuyo=hokuyo) 29 | 30 | rospy.loginfo("terminating node 'nclt2downloader'") 31 | 32 | 33 | if __name__ == '__main__': 34 | try: 35 | main() 36 | except rospy.ROSInterruptException: 37 | pass 38 | -------------------------------------------------------------------------------- /scripts/nclt2rosbag: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from nclt2ros.converter.to_rosbag import ToRosbag 5 | 6 | 7 | def main(): 8 | 9 | rospy.init_node('nclt2ros') 10 | 11 | date = rospy.get_param('~date', '2013-01-10') 12 | 13 | bag_name = rospy.get_param('~bag_name', 'nclt') 14 | cam_folder = rospy.get_param('~cam_folder', 5) 15 | lb3 = rospy.get_param('~lb3', False) 16 | sen = rospy.get_param('~sen', False) 17 | hokuyo = rospy.get_param('~hokuyo', False) 18 | vel = rospy.get_param('~vel', False) 19 | gt = rospy.get_param('~gt', False) 20 | 21 | converter = ToRosbag(date=date, bag_name=bag_name, cam_folder=cam_folder, gt=gt, sen=sen, hokuyo=hokuyo, vel=vel, lb3=lb3) 22 | converter.process() 23 | 24 | 25 | if __name__ == '__main__': 26 | try: 27 | main() 28 | except rospy.ROSInterruptException: 29 | pass 30 | -------------------------------------------------------------------------------- /scripts/nclt2visualizer: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from nclt2ros.visualizer.visualize import Visualize 5 | 6 | 7 | def main(): 8 | 9 | rospy.init_node('nclt2visualizer') 10 | rospy.loginfo("running node 'nclt2visualizer'") 11 | 12 | date = rospy.get_param('~date', '2013-01-10') 13 | show_plot = rospy.get_param('~show_plot', True) 14 | 15 | v_gt_kml = rospy.get_param('~gt_kml', False) 16 | v_gt_png = rospy.get_param('~gt_png', False) 17 | v_gps_kml = rospy.get_param('~gps_kml', False) 18 | v_gps_png = rospy.get_param('~gps_png', False) 19 | v_gps_rtk_kml = rospy.get_param('~gps_rtk_kml', False) 20 | v_gps_rtk_png = rospy.get_param('~gps_rtk_png', False) 21 | v_odom_kml = rospy.get_param('~odom_kml', False) 22 | v_odom_png = rospy.get_param('~odom_png', False) 23 | v_all = rospy.get_param('~all', False) 24 | 25 | visualizer = Visualize(date=date, show_plot=show_plot, 26 | v_gt_kml=v_gt_kml, v_gt_png=v_gt_png, v_gps_kml=v_gps_kml, v_gps_png=v_gps_png, 27 | v_gps_rtk_kml=v_gps_rtk_kml, v_gps_rtk_png=v_gps_rtk_png, v_odom_kml=v_odom_kml, 28 | v_odom_png=v_odom_png, v_all=v_all) 29 | 30 | rospy.loginfo("terminating node 'nclt2visualizer'") 31 | 32 | 33 | if __name__ == '__main__': 34 | try: 35 | main() 36 | except rospy.ROSInterruptException: 37 | pass -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from distutils.core import setup 2 | from catkin_pkg.python_setup import generate_distutils_setup 3 | 4 | setup(**generate_distutils_setup( 5 | packages=['nclt2ros'], 6 | package_dir={'': ''} 7 | )) 8 | --------------------------------------------------------------------------------