diff --git a/.gitignore b/.gitignore index f7846dc..5361ad6 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,6 @@ .vscode *.pyc -*__pycache__* \ No newline at end of file +*__pycache__* +*.egg-info +.devcontainer/ +.env \ No newline at end of file diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 0000000..f570712 --- /dev/null +++ b/Dockerfile @@ -0,0 +1,39 @@ +FROM osrf/ros:kinetic-desktop-full + +LABEL author="Olivier Lamarre" +LABEL description="Run ENAV dataset utilities" + +# Dependencies +RUN apt-get update \ + && apt-get install -y \ + cython \ + git \ + libeigen3-dev \ + libgdal-dev \ + python-pip \ + python-tk \ + ros-kinetic-grid-map \ + ros-kinetic-interactive-marker-twist-server \ + ros-kinetic-moveit-ros-visualization \ + ros-kinetic-robot-localization \ + tmux \ + && rm -rf /var/lib/apt/lists/* + +# Catkin workspace & custom package +RUN mkdir -p /root/catkin_ws/src +COPY ./enav_ros /root/catkin_ws/src/enav_ros + +RUN git clone https://github.com/MHarbi/bagedit.git /root/catkin_ws/src/bagedit +RUN cd /root/catkin_ws/src/bagedit && git checkout f97b21050826f65757040750cecc165abd67c0d2 + +RUN . /opt/ros/kinetic/setup.sh && cd /root/catkin_ws && catkin_make + +RUN echo "source /opt/ros/kinetic/setup.bash" >> /root/.bashrc +RUN echo "source /root/catkin_ws/devel/setup.bash" >> /root/.bashrc + +# Python data fetching script +COPY ./enav_utilities /root/enav_utilities +RUN cd /root/enav_utilities && pip install -e . + +# Disable default entrypoint from parent ROS image +ENTRYPOINT [] \ No newline at end of file diff --git a/README.md b/README.md index 8dee9f5..de6a5e9 100644 --- a/README.md +++ b/README.md @@ -20,61 +20,71 @@ This dataset is described in details in our paper [*The Canadian Planetary Emula ## Overview -The data can be downloaded from the dataset's official [web page](http://www.starslab.ca/enav-planetary-dataset/), which contains the data both in human-readable and rosbag format. This repository provides tools to interact with the rosbag files. +The dataset is further described in the project's official [web page](http://www.starslab.ca/enav-planetary-dataset/) and can be downloaded from a dedicated [IEEE DataPort page](https://ieee-dataport.org/open-access/canadian-planetary-emulation-terrain-energy-aware-rover-navigation-dataset) (requires creating a free account). The dataset is available in both rosbag and human-readable formats. This repository provides tools to interact with the rosbag files; we let users interested in the human-readable-formatted data develop their own utilities. -Data fetching and plotting alone in generic Python formats can be accomplished using our custom fetching script (which wraps the rosbag module). Advanced data visualization and interaction is enabled using our lightweight ROS package (tested with ROS Kinetic on Ubuntu 16.04): +We provide a Docker container in which rosbags can be played and visualized using our custom ROS package, or parsed using our python utility scripts: ![enav_ros](https://media.giphy.com/media/YlBFpSSD9qxbCCCpyp/giphy.gif) -Lastly, this dataset also includes four different aerial maps of the test environment at a resolution of 0.2 meters per pixel: color, elevation, slope magnitude and slope orientation maps. Every map is georeferenced and is available in a `.tif` format. Tools to load them in Python or import them in ROS as a single [grid_map](https://github.com/ANYbotics/grid_map) message are also included: +Lastly, this dataset also includes four different aerial maps of the test environment at a resolution of 0.2 meters per pixel: color, elevation, slope magnitude and slope orientation maps. Every map is georeferenced and is available in a `.tif` format. Tools to broadcast them as ROS topics (as [grid_map](https://github.com/ANYbotics/grid_map) messages) or simply load them using Python are also included: ![enav_maps](https://media.giphy.com/media/j4w8J6OvbReBvQyqn0/giphy.gif) +## Setup -## Pre-requisites +As the dataset was originally collected with ROS Kinetic with a Ubuntu 16.04 machine, we provide a Docker container to play rosbags and/or extract specific data streams with Python. -1. ROS installation ([ROS Kinetic](http://wiki.ros.org/kinetic/Installation/Ubuntu) tested); -2. Python 3 (version 3.7 tested). We recommend using a Python environment manager like [pyenv](https://github.com/pyenv/pyenv); +1. Store downloaded rosbags in a single directory and the georeferenced maps in a dedicated subdirectory. -## Installation +```sh +tree /path/to/dataset/dir -1. Clone this repository (including the submodules) in the `src` directory of your catkin workspace: +├── run1_base_new.bag +├── run2_base_new.bag +├── run3_base_new.bag +├── run4_base_new.bag +├── ... +├── maps +    ├── aspect_utm_20cm.tif +    ├── dem_utm_20cm.tif +    ├── mosaic_utm_20cm.tif +    └── slope_utm_20cm.tif +``` + +> Note: obviously, you don't need to download all the rosbags - just the ones you need. - ```sh - cd ~/catkin_ws/src - git clone --recurse-submodules https://github.com/utiasSTARS/enav-planetary-dataset.git - ``` +2. Clone the current repository and store the location of the downloaded dataset in a `.env` file in the project's root directory: -2. In a Python 3 environment, the required Python 3 modules can be installed using pip and our [requirements file](#): - `pip install -r requirements.txt` +```sh +git clone https://github.com/utiasSTARS/enav-planetary-dataset.git +cd enav-planetary-dataset + +echo "ENAV_DATASET_DIR='/path/to/enav_dataset/dir'" > .env +``` -3. Install the required dependencies ([husky_msgs](http://wiki.ros.org/husky_msgs), [robot_localization](http://wiki.ros.org/robot_localization and [grid_map](https://github.com/ANYbotics/grid_map)) ROS packages): +3. If not already done, [install Docker](https://docs.docker.com/engine/install/). Then, set up X server permissions: - ```sh - sudo apt-get update - sudo apt-get install ros-kinetic-husky-msgs ros-kinetic-robot-localization ros-kinetic-grid-map - ``` +```sh +xhost +local:root +``` -4. Build the enav_ros package: +Run a container and enter it: - ```sh - cd ~/catkin_ws +```sh +docker compose run --rm enav +``` - # If you are using catkin build: - catkin build enav_ros - source ~/catkin_ws/setup.bash +Note that the dataset directory is now mounted at `/enav_dataset` in the container. - # Alternatively, using catkin_make: - catkin_make --pkg enav_ros - source ~/catkin_ws/setup.bash - ``` +Rosbag interactions are done from inside the container: -## Documentation +- Playing rosbags and launching ROS visualization interfaces is documented in the `enav_ros` package's [README file](enav_ros/README.md). -Documentation on how to fetch & plot the data collected by the rover or how to show our aerial maps (along with sample Python scripts) can be found in the [enav_utilities](enav_utilities) subdirectory. Similarly, instructions related to data visualization using our lightweight ROS package can be found in the [enav_ros](enav_ros) subdirectory. +- Extracting data from rosbags with Python is documented in the `enav_utilities` package's [README file](enav_utilities/README.md). ## Citation -``` + +```txt @article{lamarre2020canadian, author = {Lamarre, Olivier and Limoyo, Oliver and Mari{\'c}, Filip and Kelly, Jonathan}, title = {{The Canadian Planetary Emulation Terrain Energy-Aware Rover Navigation Dataset}}, diff --git a/dev/Dockerfile.dev b/dev/Dockerfile.dev new file mode 100644 index 0000000..eb0be76 --- /dev/null +++ b/dev/Dockerfile.dev @@ -0,0 +1,44 @@ +# Docker file dor development purposes with the ENAV project. +# Mounts the repository in a running container, as opposed to copying its +# contents (which is what the deployed version does) + +FROM osrf/ros:kinetic-desktop-full + +LABEL author="Olivier Lamarre" +LABEL description="Development for ENAV dataset utilities" + +# Dependencies +RUN apt-get update \ + && apt-get install -y \ + build-essential \ + curl \ + cython \ + git \ + libeigen3-dev \ + libgdal-dev \ + python-pip \ + python-tk \ + ros-kinetic-grid-map \ + ros-kinetic-interactive-marker-twist-server \ + ros-kinetic-moveit-ros-visualization \ + ros-kinetic-robot-localization \ + tmux \ + vim \ + && rm -rf /var/lib/apt/lists/* + +# Catkin workspace & custom ROS package +RUN mkdir -p /root/catkin_ws/src +# COPY ./enav_ros /root/catkin_ws/src/enav_ros + +RUN git clone https://github.com/MHarbi/bagedit.git /root/catkin_ws/src/bagedit +RUN cd /root/catkin_ws/src/bagedit && git checkout f97b21050826f65757040750cecc165abd67c0d2 + +RUN . /opt/ros/kinetic/setup.sh && cd /root/catkin_ws && catkin_make + +RUN echo "source /opt/ros/kinetic/setup.bash" >> /root/.bashrc +RUN echo "source /root/catkin_ws/devel/setup.bash" >> /root/.bashrc + +WORKDIR /workspace + +# Disable default entrypoint from parent ROS image +ENTRYPOINT [] \ No newline at end of file diff --git a/dev/docker-compose.dev.yml b/dev/docker-compose.dev.yml new file mode 100644 index 0000000..6a6aac8 --- /dev/null +++ b/dev/docker-compose.dev.yml @@ -0,0 +1,52 @@ +services: + enav: + build: + context: .. # Project root is the build context + dockerfile: dev/Dockerfile.dev + + # X server setup, inspired by https://wiki.ros.org/docker/Tutorials/GUI + environment: + - DISPLAY + - QT_X11_NO_MITSHM=1 + - DEBUG=true + + stdin_open: true # --interactive (-i) flag + tty: true # --tty (-t) flag + + volumes: + - ..:/workspace + - /tmp/.X11-unix:/tmp/.X11-unix:rw # X server + - ${ENAV_DATASET_DIR}:/enav_dataset + + # Build & install mounted project volume (bash keeps the container running) + command: > + bash -c " + ln -s /workspace/enav_ros /root/catkin_ws/src && + cd /root/catkin_ws && source /opt/ros/kinetic/setup.sh && catkin_make && + cd /workspace/enav_utilities && + pip install -e . && + echo 'ENAV utilities mounted & installed, container up & running' && + bash" + + + # ports: + # - "8000:8000" + # entrypoint: ["bash"] + + # image: olamarre/enav:latest + # container_name: enav + + + # # X server setup, inspired by https://wiki.ros.org/docker/Tutorials/GUI + # environment: + # - DISPLAY + # - QT_X11_NO_MITSHM=1 + + # volumes: + # - /tmp/.X11-unix:/tmp/.X11-unix:rw # X server + # - ${ENAV_DATASET_DIR}:/enav_dataset + # - ./:/dev/enav-planetary-dataset + + # # ports: + # # - 8080:8080 + # command: bash diff --git a/dev/readme.md b/dev/readme.md new file mode 100644 index 0000000..65da85f --- /dev/null +++ b/dev/readme.md @@ -0,0 +1,77 @@ +# dev workflow + +From repo's root directory: + +1. Create .env file with path to dataset: + +```sh +echo "ENAV_DATASET_DIR='/path/to/dataset/dir'" > .env +``` + +2. Create dev image & start container (-d runs it in the background): + +```sh +docker compose -f dev/docker-compose.dev.yml --env-file .env up -d +``` + +3. Open container in vscode: Bottom left: `Open a Remove Window` > `Attach to a running container` + +Useful container commands: + +```sh +# Check running container name +docker container ls + +# Open the running container in a separate shell +docker container exec -it [name] bash + +# Stop a running container +docker container stop [name] + +# Remove a container +docker container rm [name] +``` + +Useful image commands: + +```sh +# Check image name +docker images + +# Remove image +docker image rm [name] +``` + +## Re-build & push new main image + +1. Remove older images, if any: + +```sh +docker image rm enav +``` + +2. Regenerate a new image: + +```sh +docker build -t enav --no-cache . +``` + +3. Update image name & tag to point to olamarre's dockerhub repo: + +```sh +docker tag enav:latest olamarre/enav:latest +``` + +4. Push changes to dockerhub + +```sh +docker push olamarre/enav +``` + +5. Erase local images & test installation with docker-compose file: + +```sh +docker image rm enav olamarre/enav + +docker compose run --rm enav +``` \ No newline at end of file diff --git a/docker-compose.yml b/docker-compose.yml new file mode 100644 index 0000000..4e29a27 --- /dev/null +++ b/docker-compose.yml @@ -0,0 +1,18 @@ +services: + enav: + image: olamarre/enav:latest + container_name: enav + + stdin_open: true # --interactive (-i) flag + tty: true # --tty (-t) flag + + # X server setup, inspired by https://wiki.ros.org/docker/Tutorials/GUI + environment: + - DISPLAY + - QT_X11_NO_MITSHM=1 + + volumes: + - /tmp/.X11-unix:/tmp/.X11-unix:rw # X server + - ${ENAV_DATASET_DIR}:/enav_dataset + + command: bash # keep the container session open diff --git a/enav_ros/README.md b/enav_ros/README.md index a2576e4..0b2d3c3 100644 --- a/enav_ros/README.md +++ b/enav_ros/README.md @@ -1,20 +1,23 @@ # enav_ros package -Lightweight ROS package to interact and visualize data from the energy-aware planetary navigation dataset. First make sure you read the project's main [README.md](https://github.com/utiasSTARS/enav-planetary-dataset/blob/master/README.md) located in the root directory of this repository for preliminary instructions. +Lightweight ROS package to interact and visualize data in the energy-aware planetary navigation dataset. -## Visualization of rover's reference pose estimates +First, read the project's main [README.md](https://github.com/utiasSTARS/enav-planetary-dataset/blob/master/README.md) and set up / run a Docker container. The following commands need to run inside the container. -To visualize the pose estimates generated through the combination of VINS-Fusion (stereo imagery + GPS data), IMU data and reference elevation data contained in a rosbag (`run1_base.bag` for example), start the main launch file the following way: +## Visualization of rover's reference pose estimates +To visualize the pose estimates generated through the combination of VINS-Fusion (stereo imagery + GPS data), IMU data and reference elevation data contained in a rosbag (`run1_base_new.bag` for example), start the main launch file the following way: ```sh -roslaunch enav_ros main.launch bag:=/complete/path/to/run1_base.bag map_dir:/path/to/map/directory start_time:=55.0 +roslaunch enav_ros main.launch bag:=/enav_dataset/run1_base_new.bag map_dir:=/enav_dataset/maps start_time:=55.0 # Then, when everything loaded, hit spacebar to play the rosbag ``` +The above command will load a pre-configured RViz windows with a model of the rover and corresponding TF tree. Note that at first, the default view will be fixed on the origin of the terrain ("csa_origin"). To play the bag, hit spacebar in the same terminal window. + Arguments: -* `bag`: the complete path to the base .bag file desired +* `bag`: the complete path to the base .bag file desired (recall: if you followed the provided Docker instructions, the dataset directory should be mounted in the container at `/enav_dataset`) * `map_dir` (optional, default is `false`): the complete path to the directory containing the map (.tif) files. If unspecified, no map will be loaded. * `start_time` (optional, default is 0): the start time (seconds) into the rosbag. This is particularly useful when visualizing the reference pose estimates, since it allows the user to skip the initialization phase of VINS-Fusion. For convenience, here are the `start_time` values to skip the VINS-Fusion initialization for each run: @@ -22,29 +25,23 @@ Run 1 | Run 2 | Run 3 | Run 4 | Run 5 | Run 6 --- | --- | --- | --- | --- | --- 55.0 | 20.0 | 25.0 | 25.0 | 20.0 | 38.0 -The above command will load a pre-configured RViz windows with a model of the rover and corresponding TF tree. Note that at first, the default view will be fixed on the origin of the terrain ("csa_origin"). To play the bag, hit spacebar in the same terminal window. - ## Merging all the data (including point clouds) into one rosbag -As mentioned on the dataset's web page, the point clouds of each run were saved in a separate rosbag for convenience. While it is possible to manually start both bags (`base` and `clouds_only`) more or less simultaneously in separate terminal windows, another option is to merge them into a "master" rosbag file using the [bagedit package](https://github.com/MHarbi/bagedit). Here's a sample usage of the package's `bagmerge.py` script: +As mentioned on the dataset's web page, the point clouds of each run were saved in a separate rosbag. While it is possible to manually start both bags (`base` and `clouds_only`) more or less simultaneously in separate terminal windows, another option is to merge them into a combined rosbag file using the [bagedit package](https://github.com/MHarbi/bagedit) (already installed in the image): ```sh -# Terminal 1 +# Terminal 1 inside container roscore -# Terminal 2 -rosrun bagedit bagmerge.py -o run1_master.bag run1_base.bag run1_clouds_only.bag +# Terminal 2 inside container (open with: `docker exec -it enav bash` on host computer) +rosrun bagedit bagmerge.py -o /enav_dataset/run1_combined.bag /enav_dataset/run1_base_new.bag /enav_dataset/run1_clouds_only.bag ``` -Notes: - -- Make sure to have enough free space on your device for the merged rosbag (equivalent to the combined size of the `base` and `clouds_only` bags). - -- The bagedit package works with Python2.7. An alternative to installing a Python2.7 environment is to fix the `bagmerge.py` script so that it runs properly in Python 3.7. This can be accomplished by fixing (or commenting out) its `print` functions (i.e. change every `print "text"` to `print("text")`). +Notes: leave enough free space on your device for the merged rosbag (equivalent to the combined size of the `base` and `clouds_only` bags). The merged rosbag data can then be visualized following a similar procedure to that described above: ```sh -roslaunch enav_ros main.launch bag:=/complete/path/to/run1_base.bag map_dir:/path/to/map/directory start_time:=55.0 +roslaunch enav_ros main.launch bag:=/enav_dataset/run1_combined.bag map_dir:/enav_dataset/maps start_time:=55.0 # Then, when everything loaded, hit spacebar to play the rosbag -``` \ No newline at end of file +``` diff --git a/enav_utilities/README.md b/enav_utilities/README.md index 3510d3e..334ea8d 100644 --- a/enav_utilities/README.md +++ b/enav_utilities/README.md @@ -1,8 +1,12 @@ # enav_utilities -Two main Python fetching classes were created to convert rosbag & aerial map data to generic formats for visualization and/or further processing. +A lightweight python package for rosbag data extraction. Sample plotting scripts are included. + +First, read the project's main [README.md](https://github.com/utiasSTARS/enav-planetary-dataset/blob/master/README.md) and set up / run a Docker container of the ENAV image. The data loading instances are globally importable; scripts mentioned below show usage examples and are located in the container at `/root/enav_utilities/scripts`. ## Table of contents + +- [Package setup](#package-setup) - [Aerial maps visualization](#aerial-maps-visualization) - [Rosbag data fetching](#rosbag-data-fetching) - [GPS data (& spatial plotting)](#gps-data--spatial-plotting) @@ -16,84 +20,70 @@ Two main Python fetching classes were created to convert rosbag & aerial map dat ## Aerial maps visualization -To load any aerial map, a CSASite instance from [csa_raster_load.py](csa_raster_load.py) needs to be initialized using the path to the directory containing the mapping data. An example showing how to properly use this Python class is shown in the [show_raster.py](sample_scripts/show_raster.py) example: +A [ENAVMapsLoader](src/enav_utilities/maps_loader.py) instance handles georeferenced maps loading & related metadata. Plotting example script: ```sh -python show_raster.py -d /path/to/raster/directory +python plot_rasters.py -d /enav_dataset/maps ``` -![aerial_maps](sample_outputs/aerial_maps_sample.png) - ---- +![](images/aerial_maps_sample.png) ## Rosbag data fetching -To load data from a rosbag, a FetchEnergyDataset instance from [rosbag_data_load.py](rosbag_data_load.py) needs to be initialized using the path to the said rosbag. Several methods within this class are available to fetch specific data streams. +A [ENAVRosbagLoader](src/enav_utilities/rosbag_loader.py) instance is initialized with a specific rosbag path and provides methods to access every data stream. Some usage examples are provided below. ### GPS data (& spatial plotting) -The complete GPS history is accessed through the `load_gps_data` method documented [here](rosbag_data_load.py#L605). The following sample spatial plot was generated using the [plot_gps_data.py](sample_scripts/plot_gps_data.py) sample script: - ```sh -python plot_gps_data.py -b /path/to/file.bag -d /path/to/raster/directory +python plot_gps_data.py -b /path/to/file.bag -d /path/to/maps ``` -![gps](sample_outputs/gps_data_sample.png) +![](images/gps_data_sample.png) ### Images -Images are accessed through the `load_image_data` method documented [here](rosbag_data_load.py#L486). The following sample plot was generated using the [plot_images.py](sample_scripts/plot_images.py) sample script: - ```sh python plot_images.py -b /path/to/file.bag ``` -![image](sample_outputs/image_sample.png) +![](images/image_sample.png) ### Point clouds -Similar to images, point clouds can be fetched as numpy arrays by using the `load_pointcloud_data` method documented [here](rosbag_data_load.py#L413). Note that point clouds are saved in a separate rosbag for each run, so a rosbag different from the one needed for all other data types needs to be loaded. +Refer to the `load_pointcloud_data` method. Note that point clouds are saved in a separate rosbag for each run, so a rosbag different from the one needed for all other data types needs to be loaded. ### Energy consumption -The complete power and energy consumption history of the left and right motor drivers onboard the Husky rover base is accessed through the `load_energy_data` method documented [here](rosbag_data_load.py#L270). The following sample plot was generated using the [plot_energy.py](sample_scripts/plot_energy.py) sample script: - ```sh python plot_energy.py -b /path/to/file.bag ``` -![energy_power](sample_outputs/energy_power_sample.png) +![](images/energy_power_sample.png) ### Solar irradiance -The complete solar irradiance history on top of the rover's top plane is accessed through the `load_irradiance_data` method documented [here](rosbag_data_load.py#L222). The following sample plot was generated using the [plot_irradiance.py](sample_scripts/plot_irradiance.py) sample script: - ```sh python plot_irradiance.py -b /path/to/file.bag ``` -![irradiance](sample_outputs/irradiance_sample.png) +![](images/irradiance_sample.png) ### IMU data -The complete IMU history is accessed through the `load_imu_data` method documented [here](rosbag_data_load.py#L347). The following sample plot was generated using the [plot_imu_data.py](sample_scripts/plot_imu_data.py) sample script: - ```sh python plot_imu_data.py -b /path/to/file.bag ``` -![imu_data](sample_outputs/imu_data_sample.png) +![](images/imu_data_sample.png) ### Encoder data -The complete encoder history (angular positions and velocities) is accessed through the `load_encoder_data` method documented [here](rosbag_data_load.py#L167). +Refer to the `load_encoder_data` method. ### Commanded and estimated velocities -Target/commanded velocities were recorded and can be accessed via the `load_cmd_vel_data` method documented [here](rosbag_data_load.py#L53). Planar velocity estimates of the mobile base were computed from encoder velocities. Those can be accessed through the `load_est_vel_data` method documented [here](rosbag_data_load.py#L110). The following sample plot was generated using the [plot_cmd_est_velocity_data.py](sample_scripts/plot_cmd_est_velocity_data.py) sample script: - ```sh python plot_cmd_est_velocity_data.py -b /path/to/file.bag ``` -![cmd_est_vel_data](sample_outputs/cmd_est_vel_data.png) +![](images/cmd_est_vel_data.png) diff --git a/enav_utilities/csa_raster_load.py b/enav_utilities/csa_raster_load.py deleted file mode 100644 index c957313..0000000 --- a/enav_utilities/csa_raster_load.py +++ /dev/null @@ -1,75 +0,0 @@ -#!/usr/bin/env python - -""" csa_raster_load.py - - Load raster data of the CSA MET - - Author: Olivier Lamarre - - Affl.: Space and Terrestrial Autonomous Robotic Systems Laboratory - University of Toronto - Date: February 10, 2019 - """ - -import os -import utm -import numpy as np -import rasterio -import rasterio.plot - - -class CSASite(): - - _CSA_ORIGIN_LATLON = (45.518206644445, -73.393904468182) - _CSA_ORIGIN_UTM = utm.from_latlon(*_CSA_ORIGIN_LATLON)[0:2] - _CSA_THETA_NORTH_RAD = np.radians(0.5) - - def __init__(self, dirname): - self.dirname = dirname - - # Load site settings - try: - self.load_data_from_directory(self.dirname) - except Exception as e: - raise IOError("Could not load maps from {}: {}".format(self.dirname, e)) - - def load_data_from_directory(self, dirname): - - # GeoTIFF maps - self.gtif = { - 'mosaic': rasterio.open(os.path.join(dirname, "mosaic_utm_20cm.tif")), - 'dem': rasterio.open(os.path.join(dirname, 'dem_utm_20cm.tif')), - 'slope': rasterio.open(os.path.join(dirname, 'slopemod_utm_20cm.tif')), - 'aspect': rasterio.open(os.path.join(dirname, 'aspect_utm_20cm.tif')), - } - - for datatype in self.gtif: - setattr(self, datatype, self.get_raster(datatype)) - - # Generate rgb mosaic - mosaic_rgb = np.dstack(tuple(self.gtif['mosaic'].read(i) for i in [1,2,3])) - setattr(self, 'mosaic_rgb', mosaic_rgb) - - # Adjustment of slope & aspect - self.slope[self.slope < 0] = 0 - self.aspect[self.aspect < 0] = 0 - self.slope = np.deg2rad(self.slope) - self.aspect = np.deg2rad(self.aspect) - - def get_raster(self, key): - return self.gtif[key].read(1) - - @property - def extent_utm(self): - """Return Matplotlib plotting extent in UTM coords""" - - return rasterio.plot.plotting_extent(self.gtif['mosaic']) - - @property - def zone_utm(self): - """Return the UTM zone code of the CSA MET""" - return "18T" - - -if __name__=="__main__": - pass \ No newline at end of file diff --git a/enav_utilities/images/aerial_maps_sample.png b/enav_utilities/images/aerial_maps_sample.png new file mode 100755 index 0000000..c334a5c Binary files /dev/null and b/enav_utilities/images/aerial_maps_sample.png differ diff --git a/enav_utilities/images/cmd_est_vel_data.png b/enav_utilities/images/cmd_est_vel_data.png new file mode 100755 index 0000000..7378ca9 Binary files /dev/null and b/enav_utilities/images/cmd_est_vel_data.png differ diff --git a/enav_utilities/images/energy_power_sample.png b/enav_utilities/images/energy_power_sample.png new file mode 100755 index 0000000..8e3d0bd Binary files /dev/null and b/enav_utilities/images/energy_power_sample.png differ diff --git a/enav_utilities/images/gps_data_sample.png b/enav_utilities/images/gps_data_sample.png new file mode 100755 index 0000000..e2373f8 Binary files /dev/null and b/enav_utilities/images/gps_data_sample.png differ diff --git a/enav_utilities/images/image_sample.png b/enav_utilities/images/image_sample.png new file mode 100755 index 0000000..d50ddb8 Binary files /dev/null and b/enav_utilities/images/image_sample.png differ diff --git a/enav_utilities/images/imu_data_sample.png b/enav_utilities/images/imu_data_sample.png new file mode 100755 index 0000000..5ff3334 Binary files /dev/null and b/enav_utilities/images/imu_data_sample.png differ diff --git a/enav_utilities/images/irradiance_sample.png b/enav_utilities/images/irradiance_sample.png new file mode 100755 index 0000000..6e2e6e8 Binary files /dev/null and b/enav_utilities/images/irradiance_sample.png differ diff --git a/enav_utilities/old_images/aerial_maps_sample.png b/enav_utilities/old_images/aerial_maps_sample.png new file mode 100644 index 0000000..ebbd810 Binary files /dev/null and b/enav_utilities/old_images/aerial_maps_sample.png differ diff --git a/enav_utilities/sample_outputs/cmd_est_vel_data.png b/enav_utilities/old_images/cmd_est_vel_data.png similarity index 100% rename from enav_utilities/sample_outputs/cmd_est_vel_data.png rename to enav_utilities/old_images/cmd_est_vel_data.png diff --git a/enav_utilities/sample_outputs/energy_power_sample.png b/enav_utilities/old_images/energy_power_sample.png similarity index 100% rename from enav_utilities/sample_outputs/energy_power_sample.png rename to enav_utilities/old_images/energy_power_sample.png diff --git a/enav_utilities/sample_outputs/gps_data_sample.png b/enav_utilities/old_images/gps_data_sample.png similarity index 100% rename from enav_utilities/sample_outputs/gps_data_sample.png rename to enav_utilities/old_images/gps_data_sample.png diff --git a/enav_utilities/sample_outputs/image_sample.png b/enav_utilities/old_images/image_sample.png similarity index 100% rename from enav_utilities/sample_outputs/image_sample.png rename to enav_utilities/old_images/image_sample.png diff --git a/enav_utilities/sample_outputs/imu_data_sample.png b/enav_utilities/old_images/imu_data_sample.png similarity index 100% rename from enav_utilities/sample_outputs/imu_data_sample.png rename to enav_utilities/old_images/imu_data_sample.png diff --git a/enav_utilities/sample_outputs/irradiance_sample.png b/enav_utilities/old_images/irradiance_sample.png similarity index 100% rename from enav_utilities/sample_outputs/irradiance_sample.png rename to enav_utilities/old_images/irradiance_sample.png diff --git a/enav_utilities/sample_outputs/aerial_maps_sample.png b/enav_utilities/sample_outputs/aerial_maps_sample.png deleted file mode 100644 index acb86b3..0000000 Binary files a/enav_utilities/sample_outputs/aerial_maps_sample.png and /dev/null differ diff --git a/enav_utilities/sample_scripts/plot_cmd_est_velocity_data.py b/enav_utilities/scripts/plot_cmd_est_velocity_data.py similarity index 90% rename from enav_utilities/sample_scripts/plot_cmd_est_velocity_data.py rename to enav_utilities/scripts/plot_cmd_est_velocity_data.py index 9171f42..c9cd4e7 100644 --- a/enav_utilities/sample_scripts/plot_cmd_est_velocity_data.py +++ b/enav_utilities/scripts/plot_cmd_est_velocity_data.py @@ -5,20 +5,15 @@ Plot commanded and estimated velocities of the enav dataset Author: Olivier Lamarre - Affl.: Space and Terrestrial Autonomous Robotic Systems Laboratory University of Toronto - Date: February 10, 2019 """ -import sys import argparse import matplotlib.pyplot as plt -sys.path.append("..") -from rosbag_data_load import FetchEnergyDataset - +from enav_utilities.rosbag_loader import ENAVRosbagLoader # Parse bag file name parser = argparse.ArgumentParser() @@ -29,12 +24,12 @@ if __name__ == "__main__": # Fetch data - bag_obj = FetchEnergyDataset(args.bag_file) + bag_obj = ENAVRosbagLoader(args.bag_file) cmd_vel_data = bag_obj.load_cmd_vel_data(rel_time=True) est_vel_data = bag_obj.load_est_vel_data(rel_time=True) # Plot data - plt.figure(1) + plt.figure(1, dpi=100) ax1 = plt.subplot(221) ax1.plot(cmd_vel_data[:,0], cmd_vel_data[:,1], label="x axis") ax1.plot(cmd_vel_data[:,0], cmd_vel_data[:,2], label="y axis") @@ -48,7 +43,7 @@ ax2.plot(cmd_vel_data[:,0], cmd_vel_data[:,5], label="y axis") ax2.plot(cmd_vel_data[:,0], cmd_vel_data[:,6], label="z axis") ax2.set_ylabel('Angular velocities [rad/s]') - ax2.set_xlabel('Time [s]') + ax2.set_xlabel('Elapsed time [s]') ax2.legend(loc='upper right') ax3 = plt.subplot(222) @@ -64,7 +59,8 @@ ax4.plot(est_vel_data[:,0], est_vel_data[:,5], label="y axis") ax4.plot(est_vel_data[:,0], est_vel_data[:,6], label="z axis") ax4.set_ylabel('Angular velocities [rad/s]') - ax4.set_xlabel('Time [s]') + ax4.set_xlabel('Elapsed time [s]') ax4.legend(loc='upper right') + plt.tight_layout() plt.show() diff --git a/enav_utilities/sample_scripts/plot_energy.py b/enav_utilities/scripts/plot_energy.py similarity index 79% rename from enav_utilities/sample_scripts/plot_energy.py rename to enav_utilities/scripts/plot_energy.py index a1c1822..8f2d30c 100644 --- a/enav_utilities/sample_scripts/plot_energy.py +++ b/enav_utilities/scripts/plot_energy.py @@ -2,22 +2,18 @@ """ plot_energy.py - Plot power & energy consumption of the enav dataset + Plot power & energy consumption Author: Olivier Lamarre - Affl.: Space and Terrestrial Autonomous Robotic Systems Laboratory University of Toronto - Date: February 10, 2019 """ -import sys import argparse import matplotlib.pyplot as plt -sys.path.append("..") -from rosbag_data_load import FetchEnergyDataset +from enav_utilities.rosbag_loader import ENAVRosbagLoader # Parse bag file name @@ -29,16 +25,18 @@ if __name__ == "__main__": # Fetch data - bag_obj = FetchEnergyDataset(args.bag_file) + bag_obj = ENAVRosbagLoader(args.bag_file) power_data = bag_obj.load_energy_data(rel_time=True) # Plot data - plt.figure(1) + plt.figure(1, dpi=100) ax1 = plt.subplot(411) plt.plot(power_data[:,0], power_data[:,1], label="Left driver") plt.plot(power_data[:,0], power_data[:,3], label="Right driver") plt.ylabel('Voltage (V)') - plt.title("Left and right motor drivers voltages, currents, \n power and total energy spent vs time") + plt.title( + "Left and right motor drivers voltages, currents, " + "\n power and cumulative energy spent vs time") plt.legend(loc='upper right') plt.subplot(412, sharex=ax1) @@ -56,8 +54,9 @@ plt.subplot(414, sharex=ax1) plt.plot(power_data[:,0], power_data[:,7]/3600, label="Left driver") plt.plot(power_data[:,0], power_data[:,8]/3600, label="Right driver") - plt.xlabel('Time (s)') - plt.ylabel('Energy (Wh)') + plt.xlabel('Elapsed time (s)') + plt.ylabel('Cumulative\nEnergy (Wh)') plt.legend(loc='upper right') + plt.tight_layout() plt.show() diff --git a/enav_utilities/sample_scripts/plot_gps_data.py b/enav_utilities/scripts/plot_gps_data.py similarity index 63% rename from enav_utilities/sample_scripts/plot_gps_data.py rename to enav_utilities/scripts/plot_gps_data.py index a404c7e..213b7d9 100644 --- a/enav_utilities/sample_scripts/plot_gps_data.py +++ b/enav_utilities/scripts/plot_gps_data.py @@ -5,20 +5,16 @@ Spatially plot GPS data of the enav dataset Author: Olivier Lamarre - Affl.: Space and Terrestrial Autonomous Robotic Systems Laboratory University of Toronto - Date: February 10, 2019 """ -import sys import argparse import matplotlib.pyplot as plt -sys.path.append("..") -from rosbag_data_load import FetchEnergyDataset -from csa_raster_load import CSASite +from enav_utilities.rosbag_loader import ENAVRosbagLoader +from enav_utilities.maps_loader import ENAVMapsLoader # Parse bag file name @@ -30,21 +26,27 @@ if __name__ == "__main__": # Fetch data - bag_obj = FetchEnergyDataset(args.bag_file) + bag_obj = ENAVRosbagLoader(args.bag_file) gps_data = bag_obj.load_gps_data(ret_utm=True, rel_time=True) - site = CSASite(args.directory) - + site = ENAVMapsLoader(args.directory) + # Plot data - fig = plt.figure(figsize=(15, 10)) + fig = plt.figure(dpi=100) plt.imshow(site.mosaic_rgb, extent=site.extent_utm) plt.scatter(gps_data[:,1], gps_data[:,2], s=1, color='b') - + # Start & end points - plt.scatter(gps_data[0,1], gps_data[0,2], s=250, facecolors='none', marker='o', linewidths=3, edgecolors='b') - plt.scatter(gps_data[-1,1], gps_data[-1,2], s=250, facecolors='b', marker='o', edgecolors='b') + plt.scatter( + gps_data[0,1], gps_data[0,2], s=250, facecolors='none', + marker='o', linewidths=3, edgecolors='b', label='Start' + ) + plt.scatter( + gps_data[-1,1], gps_data[-1,2], s=250, facecolors='b', + marker='o', edgecolors='b', label='End' + ) plt.xlabel('Easting [m]', fontsize=13) plt.ylabel('Northing [m]', fontsize=13) + plt.legend(scatterpoints = 1) plt.show() - diff --git a/enav_utilities/sample_scripts/plot_images.py b/enav_utilities/scripts/plot_images.py similarity index 52% rename from enav_utilities/sample_scripts/plot_images.py rename to enav_utilities/scripts/plot_images.py index e5c5141..2613bf9 100644 --- a/enav_utilities/sample_scripts/plot_images.py +++ b/enav_utilities/scripts/plot_images.py @@ -2,47 +2,48 @@ """ plot_images.py - Plot images of the enav dataset + Plot images of the ENAV dataset Author: Olivier Lamarre - Affl.: Space and Terrestrial Autonomous Robotic Systems Laboratory University of Toronto - Date: February 10, 2019 """ -import sys import argparse import matplotlib.pyplot as plt import matplotlib.gridspec as gridspec -sys.path.append("..") -from rosbag_data_load import FetchEnergyDataset +from enav_utilities.rosbag_loader import ENAVRosbagLoader # Parse bag file name parser = argparse.ArgumentParser() -parser.add_argument("-b", "--bag_file", help="path to rosbag file") +parser.add_argument("-b", "--bag", help="Path to rosbag file") args = parser.parse_args() if __name__ == "__main__": - # Fetch data - bag_obj = FetchEnergyDataset(args.bag_file) - - mono_times, mono_img_data = bag_obj.load_image_data(img_source="mono_image", time_range=(1536096176, 1536096177)) - omni2_times, omni2_img_data = bag_obj.load_image_data(img_source="omni_image2", time_range=(1536096176, 1536096177)) - omni4_times, omni4_img_data = bag_obj.load_image_data(img_source="omni_image4", time_range=(1536096176, 1536096177)) - pan_times, pan_img_data = bag_obj.load_image_data(img_source="omni_stitched_image", time_range=(1536096176, 1536096177)) - pandisp_times, pandisp_img_data = bag_obj.load_image_data(img_source="omni_stitched_disparity", time_range=(1536096176, 1536096177)) - - # Plot data - fig = plt.figure() + # Fetch images between 60 and 62 seconds after the rosbag start time + bag_obj = ENAVRosbagLoader(args.bag) + + mono_times, mono_img_data = bag_obj.load_image_data( + img_source="mono_image", time_range=(60, 62), rel_time=True) + omni2_times, omni2_img_data = bag_obj.load_image_data( + img_source="omni_image2", time_range=(60, 62), rel_time=True) + omni4_times, omni4_img_data = bag_obj.load_image_data( + img_source="omni_image4", time_range=(60, 62), rel_time=True) + pan_times, pan_img_data = bag_obj.load_image_data( + img_source="omni_stitched_image", time_range=(60, 62), rel_time=True) + pandisp_times, pandisp_img_data = bag_obj.load_image_data( + img_source="omni_stitched_disparity", time_range=(60, 62), rel_time=True) + + # Plot first image of requested time range (60 seconds after bag start) + fig = plt.figure(dpi=100) gs = gridspec.GridSpec(3,3) - + ax = fig.add_subplot(gs[0,0]) ax.imshow(mono_img_data[0,...], cmap='gray') ax.set_title('Monocular camera raw image') @@ -68,4 +69,5 @@ ax.set_title('Panoramic disparity image') plt.axis('off') + plt.suptitle("Images 60 seconds from rosbag start time") plt.show() diff --git a/enav_utilities/sample_scripts/plot_imu_data.py b/enav_utilities/scripts/plot_imu_data.py similarity index 85% rename from enav_utilities/sample_scripts/plot_imu_data.py rename to enav_utilities/scripts/plot_imu_data.py index e04dc2d..1be0d14 100644 --- a/enav_utilities/sample_scripts/plot_imu_data.py +++ b/enav_utilities/scripts/plot_imu_data.py @@ -5,19 +5,15 @@ Plot IMU data of the enav dataset Author: Olivier Lamarre - Affl.: Space and Terrestrial Autonomous Robotic Systems Laboratory University of Toronto - Date: February 10, 2019 """ -import sys import argparse import matplotlib.pyplot as plt -sys.path.append("..") -from rosbag_data_load import FetchEnergyDataset +from enav_utilities.rosbag_loader import ENAVRosbagLoader # Parse bag file name @@ -29,12 +25,12 @@ if __name__ == "__main__": # Fetch data - bag_obj = FetchEnergyDataset(args.bag_file) + bag_obj = ENAVRosbagLoader(args.bag_file) imu_data = bag_obj.load_imu_data(rel_time=True) # Plot data - plt.figure(1) - ax1 = plt.subplot(411) + plt.figure(1, dpi=100) + ax1 = plt.subplot(311) plt.plot(imu_data[:,0], imu_data[:,1], label="x axis") plt.plot(imu_data[:,0], imu_data[:,2], label="y axis") plt.plot(imu_data[:,0], imu_data[:,3], label="z axis") @@ -42,20 +38,20 @@ plt.title("IMU linear accelerations, angular velocities, \n and orientations vs time") plt.legend(loc='upper right') - plt.subplot(412, sharex=ax1) + plt.subplot(312, sharex=ax1) plt.plot(imu_data[:,0], imu_data[:,4], label="x axis") plt.plot(imu_data[:,0], imu_data[:,5], label="y axis") plt.plot(imu_data[:,0], imu_data[:,6], label="z axis") plt.ylabel('Angular velocities [rad/s]') plt.legend(loc='upper right') - plt.subplot(413, sharex=ax1) + plt.subplot(313, sharex=ax1) plt.plot(imu_data[:,0], imu_data[:,7], label="q_x") plt.plot(imu_data[:,0], imu_data[:,8], label="q_y") plt.plot(imu_data[:,0], imu_data[:,9], label="q_z") plt.plot(imu_data[:,0], imu_data[:,10], label="q_w") plt.ylabel('Orientations [quaternion]') - plt.xlabel('Time [s]') + plt.xlabel('Elapsed time [s]') plt.legend(loc='upper right') plt.show() diff --git a/enav_utilities/sample_scripts/plot_irradiance.py b/enav_utilities/scripts/plot_irradiance.py similarity index 82% rename from enav_utilities/sample_scripts/plot_irradiance.py rename to enav_utilities/scripts/plot_irradiance.py index c81815d..e6bfa77 100644 --- a/enav_utilities/sample_scripts/plot_irradiance.py +++ b/enav_utilities/scripts/plot_irradiance.py @@ -5,19 +5,15 @@ Plot solar irradiance data of the enav dataset Author: Olivier Lamarre - Affl.: Space and Terrestrial Autonomous Robotic Systems Laboratory University of Toronto - Date: February 10, 2019 """ -import sys import argparse import matplotlib.pyplot as plt -sys.path.append("..") -from rosbag_data_load import FetchEnergyDataset +from enav_utilities.rosbag_loader import ENAVRosbagLoader # Parse bag file name @@ -29,13 +25,13 @@ if __name__ == "__main__": # Fetch data - bag_obj = FetchEnergyDataset(args.bag_file) + bag_obj = ENAVRosbagLoader(args.bag_file) irradiance_data = bag_obj.load_irradiance_data(rel_time=True) # Plot data - plt.figure() + plt.figure(dpi=100) plt.plot(irradiance_data[:,0], irradiance_data[:,1]) - plt.xlabel('Time [s]') + plt.xlabel('Elapsed time [s]') plt.ylabel('Irradiance [W/m^2]') plt.title("Solar irradiance received by the rover's top plane vs time") plt.show() diff --git a/enav_utilities/sample_scripts/show_raster.py b/enav_utilities/scripts/plot_rasters.py similarity index 52% rename from enav_utilities/sample_scripts/show_raster.py rename to enav_utilities/scripts/plot_rasters.py index 32dd0c0..73d9555 100644 --- a/enav_utilities/sample_scripts/show_raster.py +++ b/enav_utilities/scripts/plot_rasters.py @@ -1,72 +1,63 @@ #!/usr/bin/env python -""" show_plot.py +""" plot_rasters.py - Plot raster data of the CSA MET for demonstration/visualization purposes + Plot georeferenced maps of the test environment Author: Olivier Lamarre - Affl.: Space and Terrestrial Autonomous Robotic Systems Laboratory University of Toronto - Date: February 10, 2019 """ -import sys - import argparse +import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.axes_grid1 import make_axes_locatable -sys.path.append("..") -from csa_raster_load import CSASite +from enav_utilities.maps_loader import ENAVMapsLoader -# Parse directory name parser = argparse.ArgumentParser() -parser.add_argument("-d", "--directory", - help="Path to CSA raster data directory") +parser.add_argument("-d", "--directory", help="Path to maps directory") args = parser.parse_args() if __name__=="__main__": - site = CSASite(args.directory) - site_width = site.extent_utm[1] - site.extent_utm[0] - site_height = site.extent_utm[3] - site.extent_utm[2] + + site = ENAVMapsLoader(args.directory) # Show site info - print("Site width [m]: ", site_width) - print("Site height [m]: ", site_height) - print("Site mosaic res y [m/px]: ", site_height/site.mosaic_rgb.shape[0]) - print("Site mosaic res x [m/px]: ", site_width/site.mosaic_rgb.shape[1]) - print("Site extent: ", site.extent_utm) + print("Width [m]: {0:.2f}".format(site.width)) + print("Height [m]: {0:.2f}".format(site.height)) + print("Easting spatial resolution [m/px]: {0:.3f}".format(site.res_easting)) + print("Northing spatial resolution [m/px]: {0:.3f}".format(site.res_northing)) + print("Map extent in UTM coords (left, right, bottom, top): {}".format(site.extent_utm)) - # Plot all maps - fig, axes = plt.subplots(2, 2, figsize=(15,10)) + # Plot all rasters + fig, axes = plt.subplots(2, 2, dpi=100)# figsize=(15,10)) im00 = axes[0,0].imshow(site.mosaic_rgb, extent=site.extent_utm) axes[0,0].set_title("RGB mosaic") - axes[0,0].set_xlabel('Easting [m]') - axes[0,0].set_ylabel('Northing [m]') im01 = axes[0,1].imshow(site.dem, extent=site.extent_utm, cmap='gray') axes[0,1].set_title("Elevation above origin (NW corner) [meters]") - axes[0,1].set_xlabel('Easting [m]') - axes[0,1].set_ylabel('Northing [m]') + axes[0,1].scatter(*site.origin_utm, c='r', s=50, label="Terrain origin") + axes[0,1].legend(scatterpoints = 1) im10 = axes[1,0].imshow(site.slope, extent=site.extent_utm, cmap='jet') axes[1,0].set_title("Slope map [radians]") - axes[1,0].set_xlabel('Easting [m]') - axes[1,0].set_ylabel('Northing [m]') im11 = axes[1,1].imshow(site.aspect, extent=site.extent_utm, cmap='jet') axes[1,1].set_title("Aspect map [radians]") - axes[1,1].set_xlabel('Easting [m]') - axes[1,1].set_ylabel('Northing [m]') - plt.suptitle("All georeferenced maps in UTM coordinates (UTM zone {})".format(site.zone_utm)) + for elem in np.nditer(axes, flags=['refs_ok']): + elem.item().set_xlabel('Easting [m]') + elem.item().set_ylabel('Northing [m]') + + plt.suptitle("Georeferenced maps in UTM coordinates (UTM zone {})".format(site.zone_utm)) - # Show colorbars + # Colorbars div01 = make_axes_locatable(axes[0,1]) cax01 = div01.append_axes("right", size="5%", pad=0.1) plt.colorbar(im01, cax=cax01) diff --git a/enav_utilities/scripts/test.py b/enav_utilities/scripts/test.py new file mode 100644 index 0000000..6bd437a --- /dev/null +++ b/enav_utilities/scripts/test.py @@ -0,0 +1,41 @@ +#!/usr/bin/env python + +from tracemalloc import start +from rosbags.rosbag1 import Reader +from rosbags.typesys import Stores, get_typestore + +import time as pytime + +if __name__ == "__main__": + # Create a typestore and get the string class. + typestore = get_typestore(Stores.LATEST) + + start_time = pytime.time() + + # Create reader instance and open for reading. + with Reader('/media/olamarre/My Passport/enav_dataset/run3_base_new.bag') as reader: + # Topic and msgtype information is available on .connections list. + + # for connection in reader.connections: + # print(connection.topic, connection.msgtype) + + print(f"{reader.message_count}") + + # # Iterate over messages. + # for connection, timestamp, rawdata in reader.messages(): + # if connection.topic == '/imu_raw/Imu': + # msg = typestore.deserialize_ros1(rawdata, connection.msgtype) + # print(msg.header.frame_id) + + # The .messages() method accepts connection filters. + connections = [x for x in reader.connections if x.topic == '/imu'] + for connection, timestamp, rawdata in reader.messages(connections=connections): + msg = typestore.deserialize_ros1(rawdata, connection.msgtype) + print(msg.header.frame_id) + break + + + print(f"Elapsed: {pytime.time()-start_time}") + + print(f"This is a test") + print(f"uh ohhhh") \ No newline at end of file diff --git a/enav_utilities/setup.py b/enav_utilities/setup.py new file mode 100644 index 0000000..9abbf66 --- /dev/null +++ b/enav_utilities/setup.py @@ -0,0 +1,27 @@ +from setuptools import setup, find_packages + +setup( + name="enav_utilities", + version="0.1.0", + description="Data fetching for the The Canadian planetary emulation terrain energy-aware rover navigation dataset (ENAV)", + + author="Olivier Lamarre", + author_email="olivier.lamarre@robotics.utias.utoronto.ca", + + url="https://starslab.ca/enav-planetary-dataset", + license="MIT", + + packages=find_packages(where="src"), + package_dir={"": "src"}, + + install_requires=[ + "affine==1.3.*", + "click==7.1.*", + "matplotlib==1.5.*", + "numpy==1.11.*", + "pathlib==1.0.*", + "pyparsing==2.0.*", + "rasterio==0.36.*", + "utm==0.7.*" + ], +) \ No newline at end of file diff --git a/enav_utilities/src/enav_utilities/__init__.py b/enav_utilities/src/enav_utilities/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/enav_utilities/src/enav_utilities/maps_loader.py b/enav_utilities/src/enav_utilities/maps_loader.py new file mode 100644 index 0000000..326caf9 --- /dev/null +++ b/enav_utilities/src/enav_utilities/maps_loader.py @@ -0,0 +1,112 @@ +#!/usr/bin/env python + +""" raster_load.py + + Load rasterized maps of the test environment + + Author: Olivier Lamarre + Affl.: Space and Terrestrial Autonomous Robotic Systems Laboratory + University of Toronto + Date: February 10, 2019 + """ + +import numpy as np +from pathlib import Path +import rasterio +from rasterio.plot import plotting_extent +import utm + + +class ENAVMapsLoader: + + # Map origin (North West corner or terrain) latitude & longitude (degrees) + _ORIGIN_LATLON = (45.518206644445, -73.393904468182) + + _UTM_ZONE = "18T" + + def __init__(self, dirpath): + """Init ENAV site & load maps + + Args: + dirpath (Path): path to the maps directory + """ + + try: + self.load_data_from_directory(dirpath) + except Exception as e: + raise IOError("Could not load maps from {}: {}".format(dirpath, e)) + + def load_data_from_directory(self, dirpath): + + # GeoTIFF maps + self.gtif = { + 'mosaic': rasterio.open(str(Path(dirpath, "mosaic_utm_20cm.tif"))), + 'dem': rasterio.open(str(Path(dirpath, 'dem_utm_20cm.tif'))), + 'slope': rasterio.open(str(Path(dirpath, 'slopemod_utm_20cm.tif'))), + 'aspect': rasterio.open(str(Path(dirpath, 'aspect_utm_20cm.tif'))), + } + + for datatype in self.gtif: + setattr(self, datatype, self.get_raster(datatype)) + + # RGB mosaic + mosaic_rgb = np.dstack(tuple(self.get_raster('mosaic', i) for i in range(1,4))) + setattr(self, 'mosaic_rgb', mosaic_rgb) + + # Adjustment of slope & aspect + self.slope[self.slope < 0] = 0 + self.aspect[self.aspect < 0] = 0 + self.slope = np.deg2rad(self.slope) + self.aspect = np.deg2rad(self.aspect) + + def get_raster(self, key, band=1): + return self.gtif[key].read(band) + + @property + def extent_utm(self): + """Matplotlib plotting extent in UTM coords (left, right, bottom, top)""" + return plotting_extent(self.gtif['mosaic']) # same for all rasters + + @property + def width(self): + """Map width (along easting direction) in meters""" + return self.extent_utm[1] - self.extent_utm[0] + + @property + def height(self): + """Map height (along northing direction) in meters""" + return self.extent_utm[3] - self.extent_utm[2] + + @property + def res_easting(self): + """Raster spatial resolution (m/px) along easting direction""" + return self.width/self.shape[1] + + @property + def res_northing(self): + """Raster spatial resolution (m/px) along northing direction""" + return self.height/self.shape[0] + + @property + def shape(self): + """Raster shape (num. pixels) along northing and easting directions""" + return self.mosaic.shape # same shape for all rasters + + @property + def origin_latlon(self): + """Latitude and longitude coordinates of map origin (degrees)""" + return self._ORIGIN_LATLON + + @property + def origin_utm(self): + """UTM coordinates (easting,northing) of map origin, in meters""" + return utm.from_latlon(*self._ORIGIN_LATLON)[0:2] + + @property + def zone_utm(self): + """UTM zone code of map""" + return self._UTM_ZONE + + +if __name__=="__main__": + pass \ No newline at end of file diff --git a/enav_utilities/rosbag_data_load.py b/enav_utilities/src/enav_utilities/rosbag_loader.py similarity index 77% rename from enav_utilities/rosbag_data_load.py rename to enav_utilities/src/enav_utilities/rosbag_loader.py index df9b325..2c80ba3 100644 --- a/enav_utilities/rosbag_data_load.py +++ b/enav_utilities/src/enav_utilities/rosbag_loader.py @@ -1,20 +1,18 @@ #!/usr/bin/env python """ rosbag_data_load.py - + Utility functions to load data from the energy-aware planetary navigation dataset Authors: Olivier Lamarre Oliver Limoyo - Affl.: Space and Terrestrial Autonomous Robotic Systems Laboratory University of Toronto - Date: January 27, 2019 """ import sys -import time +# import time import itertools import numpy as np @@ -24,10 +22,10 @@ import sensor_msgs.point_cloud2 as pc2 -class FetchEnergyDataset: +class ENAVRosbagLoader: def __init__(self, filename): - + # Initialize object by providing bag file path self.file = filename self.bag = rosbag.Bag(filename, 'r') @@ -43,19 +41,20 @@ def __init__(self, filename): "husky_encoder": "/joint_states", "husky_odometry": "/husky_velocity_estimate", "husky_cmd_vel": "/husky_commanded_velocity", - "pointclouds": "/omni_stitched_cloud", + "pointclouds": "/omni_stitched_cloud", "pose_estimates": "/global_odometry_utm", - "relative_sun_orientation": "/relative_sun_orientation", - "global_sun_orientation": "/global_sun_orientation", + "relative_sun_orientation": "/relative_sun_orientation", + "global_sun_orientation": "/global_sun_orientation", } - + # Data retrieval status self.status = 0 def load_cmd_vel_data(self, rel_time=False): """ Loads commanded velocities to the Husky rover base - Input: rel_time - set time relative to first msg (rather than absolute) + Input: rel_time - if true, returned time is the number of seconds from + the start of the bag (as opposed to an absolute timestamp) Return: numpy array of 7 columns: time [s], @@ -76,16 +75,11 @@ def load_cmd_vel_data(self, rel_time=False): print("Retrieving husky_cmd_vel data from {} ...".format(self.file)) print("Number of husky_cmd_vel messages: {}".format(tot_msg_count)) - for topic, msg, time in self.bag.read_messages(self.tpc_names["husky_cmd_vel"]): + time_offset = self.bag.get_start_time() if rel_time else 0 - # Retrieve time & adjust to relative value if needed - if rel_time: - if valid_msg_count == 0: - init_time = time.to_sec() - curr_time = time.to_sec() - init_time - else: - curr_time = time.to_sec() + for _, msg, time in self.bag.read_messages(self.tpc_names["husky_cmd_vel"]): + curr_time = time.to_sec() - time_offset temp = np.array([curr_time, msg.linear.x, msg.linear.y, @@ -100,19 +94,20 @@ def load_cmd_vel_data(self, rel_time=False): # Show process status: valid_msg_count +=1 self.status = round(100*float(valid_msg_count)/tot_msg_count) - + sys.stdout.write('\r') sys.stdout.write("Progress: {} %".format(self.status)) sys.stdout.flush() sys.stdout.write("\n") - + return data def load_est_vel_data(self, rel_time=False): """ Loads odometry data from the Husky computed from wheel encoders. - Input: rel_time - set time relative to first msg (rather than absolute) + Input: rel_time - if true, returned time is the number of seconds from + the start of the bag (as opposed to an absolute timestamp) Return: numpy array of 7 columns: time [s], @@ -133,16 +128,11 @@ def load_est_vel_data(self, rel_time=False): print("Retrieving husky_odometry data from {} ...".format(self.file)) print("Number of husky_odometry messages: {}".format(tot_msg_count)) - for topic, msg, time in self.bag.read_messages(self.tpc_names["husky_odometry"]): - - # Retrieve time & adjust to relative value if needed - if rel_time: - if valid_msg_count == 0: - init_time = time.to_sec() - curr_time = time.to_sec() - init_time - else: - curr_time = time.to_sec() + time_offset = self.bag.get_start_time() if rel_time else 0 + for _, msg, time in self.bag.read_messages(self.tpc_names["husky_odometry"]): + + curr_time = time.to_sec() - time_offset temp = np.array([curr_time, msg.twist.twist.linear.x, msg.twist.twist.linear.y, @@ -157,20 +147,21 @@ def load_est_vel_data(self, rel_time=False): # Show process status: valid_msg_count +=1 self.status = round(100*float(valid_msg_count)/tot_msg_count) - + sys.stdout.write('\r') sys.stdout.write("Progress: {} %".format(self.status)) sys.stdout.flush() sys.stdout.write("\n") - + return data def load_encoder_data(self, rel_time=False): """ Loads encoder data from the Husky. - Input: rel_time - set time relative to first msg (rather than absolute) - + Input: rel_time - if true, returned time is the number of seconds from + the start of the bag (as opposed to an absolute timestamp) + Return: numpy array of 9 columns: time [s], front_left_wheel position [rad], @@ -192,18 +183,14 @@ def load_encoder_data(self, rel_time=False): print("Retrieving husky_encoder data from {} ...".format(self.file)) print("Number of husky_encoder messages: {}".format(tot_msg_count)) - for topic, msg, time in self.bag.read_messages(self.tpc_names["husky_encoder"]): + time_offset = self.bag.get_start_time() if rel_time else 0 - # Retrieve time & adjust to relative value if needed - if rel_time: - if valid_msg_count == 0: - init_time = time.to_sec() - curr_time = time.to_sec() - init_time - else: - curr_time = time.to_sec() + for _, msg, time in self.bag.read_messages(self.tpc_names["husky_encoder"]): + + curr_time = time.to_sec() - time_offset - pos = np.asarray(msg.position) - vel = np.asarray(msg.velocity) + pos = np.asarray(msg.position) + vel = np.asarray(msg.velocity) enc = np.hstack((curr_time, pos,vel)) # Populate main data array @@ -212,19 +199,20 @@ def load_encoder_data(self, rel_time=False): # Show process status: valid_msg_count +=1 self.status = round(100*float(valid_msg_count)/tot_msg_count) - + sys.stdout.write('\r') sys.stdout.write("Progress: {} %".format(self.status)) sys.stdout.flush() sys.stdout.write("\n") - + return data def load_irradiance_data(self, rel_time=False): """ Loads solar irradiance data from the pyranometer - Input: rel_time - set timestamps relative to first reading (rather than absolute) + Input: rel_time - if true, returned time is the number of seconds from + the start of the bag (as opposed to an absolute timestamp) Return: numpy array with 3 columns: timestamp [s], @@ -241,17 +229,11 @@ def load_irradiance_data(self, rel_time=False): print("Retrieving pyranometer data from {} ...".format(self.file)) print("Number of pyranometer messages: {}".format(tot_msg_count)) - for topic, msg, time in self.bag.read_messages(self.tpc_names["pyranometer"]): + time_offset = self.bag.get_start_time() if rel_time else 0 - # Retrieve time & adjust to relative value if needed - if rel_time: - if valid_msg_count == 0: - init_time = time.to_sec() - curr_time = time.to_sec() - init_time - else: - curr_time = time.to_sec() - - # Retrieve current data + for _, msg, time in self.bag.read_messages(self.tpc_names["pyranometer"]): + + curr_time = time.to_sec() - time_offset temp = np.array([curr_time, msg.data, msg.theoretical_clearsky_horizontal]) @@ -262,20 +244,21 @@ def load_irradiance_data(self, rel_time=False): # Show process status: valid_msg_count +=1 self.status = round(100*float(valid_msg_count)/tot_msg_count) - + sys.stdout.write('\r') sys.stdout.write("Progress: {} %".format(self.status)) sys.stdout.flush() - + sys.stdout.write("\n") - + return data - + def load_energy_data(self, rel_time=False): """ Loads data from driving motors power monitors onboard the Husky Energy values are obtained by integrating power consumption values - Input: rel_time - set timestamps relative to first reading (rather than absolute) + Input: rel_time - if true, returned time is the number of seconds from + the start of the bag (as opposed to an absolute timestamp) Return: numpy array of 9 columns: timestamp [s], @@ -298,31 +281,24 @@ def load_energy_data(self, rel_time=False): print("Retrieving power data from {} ...".format(self.file)) print("Number of power messages: {}".format(tot_msg_count)) - for topic, msg, time in self.bag.read_messages(self.tpc_names["power"]): - #if topic == self.tpc_names["power"]: + time_offset = self.bag.get_start_time() if rel_time else 0 - # Retrieve time & adjust to relative value if needed - if rel_time: - if valid_msg_count == 0: - init_time = time.to_sec() - curr_time = time.to_sec() - init_time - else: - curr_time = time.to_sec() + for _, msg, time in self.bag.read_messages(self.tpc_names["power"]): - # Retrieve current data + curr_time = time.to_sec() - time_offset temp = np.array([curr_time, msg.left_driver_voltage, msg.left_driver_current, msg.right_driver_voltage, msg.right_driver_current]) - + # Populate main data array data = np.vstack([data,temp]) # Show process status: valid_msg_count +=1 self.status = round(100*float(valid_msg_count)/tot_msg_count) - + sys.stdout.write('\r') sys.stdout.write("Progress: {} %".format(self.status)) sys.stdout.flush() @@ -351,7 +327,8 @@ def load_energy_data(self, rel_time=False): def load_imu_data(self, rel_time=False): """ Loads IMU data - Input: rel_time - set timestamp relative to first reading (rather than absolute) + Input: rel_time - if true, returned time is the number of seconds from + the start of the bag (as opposed to an absolute timestamp) Return: numpy array of 11 columns: timestamp [s], @@ -376,17 +353,11 @@ def load_imu_data(self, rel_time=False): print("Retrieving IMU data from {} ...".format(self.file)) print("Number of IMU messages: {}".format(tot_msg_count)) - for topic, msg, time in self.bag.read_messages(self.tpc_names["imu"]): + time_offset = self.bag.get_start_time() if rel_time else 0 - # Retrieve time & adjust to relative value if needed - if rel_time: - if valid_msg_count == 0: - init_time = time.to_sec() - curr_time = time.to_sec() - init_time - else: - curr_time = time.to_sec() + for _, msg, time in self.bag.read_messages(self.tpc_names["imu"]): - # Retrieve current data + curr_time = time.to_sec() - time_offset temp = np.array([curr_time, msg.linear_acceleration.x, msg.linear_acceleration.y, @@ -398,7 +369,7 @@ def load_imu_data(self, rel_time=False): msg.orientation.y, msg.orientation.z, msg.orientation.w]) - + # Populate main data array data = np.vstack([data,temp]) @@ -414,18 +385,28 @@ def load_imu_data(self, rel_time=False): return data - def load_pointcloud_data(self, pc_source="omni_stitched_cloud", time_range=None, rel_time=False): - """ Loads pointcloud data from stereo omnidirectional camera mounted on the Husky - - Input: pc_source - get pointclouds from {omni_cloud0, ..., omni_cloud4, omni_stitched_cloud} - time_range - ROS time range (Unix epoch time) to load images from with tuple (start,end), None defaults to all - rel_time - set time relative to first msg (rather than absolute) - - Return: tuple of (times [s], clouds). + def load_pointcloud_data( + self, + pc_source="omni_stitched_cloud", + time_range=None, + rel_time=False + ): + """ Loads point cloud data from stereo omnidirectional camera + + Input: pc_source - point cloud source, from {omni_cloud0, ..., + omni_cloud4, omni_stitched_cloud} + time_range - Load clouds inside of this time range (2-tuple of + floating point timestamps (seconds)). If rel_time is True, + these time values are expected to be times elapsed from the + start of the bag, in seconds. Absolute ROS times otherwise. + rel_time - if true, returned time is the number of seconds from + the start of the bag (as opposed to an absolute timestamp) + + Return: tuple of (times [s], clouds). times is a numpy array of dimension: (batch,) - clouds is a list of numpy arrays. Each numpy - array (a single pointcloud) is of dimension: (n_points, 3), - where each column corresponds to x,y,z, respectively. + clouds is a list of numpy arrays. Each numpy + array (a single pointcloud) is of dimension: (n_points, 3), + where each column corresponds to x,y,z, respectively. """ if pc_source == "omni_stitched_cloud": @@ -443,11 +424,16 @@ def load_pointcloud_data(self, pc_source="omni_stitched_cloud", time_range=None, if time_range == None: print("Loading all {} pointclouds".format(tot_msg_count)) - # Calculate the amount of images to pre-allocate later pc_count = tot_msg_count + elif type(time_range) is tuple: - t_range = self.time_to_timestep(time_range, tot_msg_count) + if rel_time: + abs_time_range = tuple((t+self.bag.get_start_time() for t in time_range)) + else: + abs_time_range = time_range + + t_range = self.time_to_timestep(abs_time_range, tot_msg_count) # Quick check for tuple range if not (0 <= t_range[0] < tot_msg_count and 0 <= t_range[1] < tot_msg_count): @@ -456,49 +442,46 @@ def load_pointcloud_data(self, pc_source="omni_stitched_cloud", time_range=None, print("Loading pointclouds from timesteps {} to {}".format(t_range[0], t_range[1])) # Don't load all the data, instead slice the generator at the desired range - pc_gen = itertools.islice(pc_gen, t_range[0], t_range[1]) - - # Calculate the amount of images to pre-allocate later + pc_gen = itertools.islice(pc_gen, t_range[0], t_range[1]) pc_count = (t_range[1] - t_range[0]) + 1 + else: raise ValueError("Invalid type {} for 't_range', t_range is a tuple (start,end) or None" % type(t_range)) + time_offset = self.bag.get_start_time() if rel_time else 0 + time_data = np.empty((pc_count)) data = [] for i, (_, msg, time) in enumerate(pc_gen): - # Retrieve time & adjust to relative value if needed - if rel_time: - if valid_msg_count == 0: - init_time = time.to_sec() - curr_time = time.to_sec() - init_time - else: - curr_time = time.to_sec() - time_data[i] = curr_time + time_data[i] = time.to_sec() - time_offset # for PointCloud2 - pc = [[point[0], point[1], point[2]] for point in + pc = [[point[0], point[1], point[2]] for point in pc2.read_points(msg, field_names = ("x", "y", "z"), skip_nans=True)] - # for PointCloud # pc = [[point.x, point.y, point.z] for point in msg.points] + data.append(np.asarray(pc, dtype=np.float32)) valid_msg_count +=1 return (time_data, data) def load_image_data(self, img_source="mono_image", time_range=None, rel_time=False): - """ Loads image data from cameras mounted on the Husky + """ Loads image data from a specific camera - Input: string img_source - get images from {"omni_image0", ..., "omni_image9", + Input: string img_source - get images from {"omni_image0", ..., "omni_image9", "omni_stitched_image", "omni_stitched_disparity", "mono_image"} - time_range - ROS time range (Unix epoch time) to load images from - with tuple (start,end), None defaults to all - rel_time - set time relative to first msg (rather than absolute) - - Return: tuple of (time [s], images). + time_range - Load images inside of this time range (2-tuple of + floating point timestamps (seconds)). If rel_time is True, + these time values are expected to be times elapsed from the + start of the bag, in seconds. Absolute ROS times otherwise. + rel_time - if true, returned time is the number of seconds from + the start of the bag (as opposed to an absolute timestamp) + + Return: tuple of (time [s], images). time is a numpy array of dimension: (batch,). - images is a numpy array of dimension: + images is a numpy array of dimension: (batch, height, width) or (batch , height, width, channel). """ @@ -534,35 +517,38 @@ def load_image_data(self, img_source="mono_image", time_range=None, rel_time=Fal # Calculate the amount of images to pre-allocate later img_count = tot_msg_count elif type(time_range) is tuple: - - t_range = self.time_to_timestep(time_range, tot_msg_count) + + if rel_time: + abs_time_range = tuple((t+self.bag.get_start_time() for t in time_range)) + else: + abs_time_range = time_range + + t_range = self.time_to_timestep(abs_time_range, tot_msg_count) # Quick check for tuple range if not (0 <= t_range[0] < tot_msg_count and 0 <= t_range[1] < tot_msg_count): - raise ValueError("Invalid timestep range {} for image dataset of size {}" \ - .format(t_range, tot_msg_count)) + raise ValueError( + "Invalid timestep range {} for image dataset of size {}. " \ + .format(t_range, tot_msg_count) +\ + "Current rosbag start-end timestamps: {}-{}".format(self.bag.get_start_time(),self.bag.get_end_time()) + ) print("Loading images from timesteps {} to {}".format(t_range[0], t_range[1])) # Don't load all the data, instead slice the generator at the desired range - img_gen = itertools.islice(img_gen, t_range[0], t_range[1]) + img_gen = itertools.islice(img_gen, t_range[0], t_range[1]) # Calculate the amount of images to pre-allocate later img_count = (t_range[1] - t_range[0]) + 1 else: raise ValueError("Invalid type {} for 't_range', t_range is a tuple (start,end) or None" % type(t_range)) + time_offset = self.bag.get_start_time() if rel_time else 0 + time_data = np.empty((img_count)) for i, (_, msg, time) in enumerate(img_gen): img = np.fromstring(msg.data, dtype=np.uint8) - # Retrieve time & adjust to relative value if needed - if rel_time: - if valid_msg_count == 0: - init_time = time.to_sec() - curr_time = time.to_sec() - init_time - else: - curr_time = time.to_sec() - time_data[i] = curr_time + time_data[i] = time.to_sec() - time_offset if i == 0: # Get the dimensions from the first image and pre-allocate @@ -597,7 +583,7 @@ def energy_from_power(self, time, power): Return: energy - (batch,) energy array [J] """ - + n = time.shape[0] energy = np.zeros(n) @@ -605,12 +591,13 @@ def energy_from_power(self, time, power): energy[i+1] = energy[i] + np.trapz(power[i:i+2], x=time[i:i+2]) return energy - + def load_gps_data(self, ret_utm=False, rel_time=False): """ Loads GPS data Input: ret_utm - return the position as UTM coords (instead of lat-lon) - rel_time - set timestamp relative to first reading (rather than absolute) + rel_time - if true, returned time is the number of seconds from + the start of the bag (as opposed to an absolute timestamp) Return: numpy array of 4 columns if ret_utm argument is False: ROS timestamp [s], @@ -636,16 +623,11 @@ def load_gps_data(self, ret_utm=False, rel_time=False): print("Retrieving GPS data from {} ...".format(self.file)) print("Number of GPS messages: {}".format(tot_msg_count)) - for topic, msg, time in self.bag.read_messages(self.tpc_names["gps"]): + time_offset = self.bag.get_start_time() if rel_time else 0 - # Retrieve time & adjust to relative value if needed - if rel_time: - if valid_msg_count == 0: - init_time = time.to_sec() - curr_time = time.to_sec() - init_time - else: - curr_time = time.to_sec() + for _, msg, time in self.bag.read_messages(self.tpc_names["gps"]): + curr_time = time.to_sec() - time_offset lat = msg.latitude lon = msg.longitude alt = msg.altitude @@ -656,26 +638,27 @@ def load_gps_data(self, ret_utm=False, rel_time=False): temp = np.array([curr_time, easting, northing, alt]) else: temp = np.array([curr_time, lat, lon, alt]) - + # Populate main data array data = np.vstack([data,temp]) # Show process status: valid_msg_count +=1 self.status = round(100*float(valid_msg_count)/tot_msg_count) - + sys.stdout.write('\r') sys.stdout.write("Progress: {} %".format(self.status)) sys.stdout.flush() - + sys.stdout.write("\n") - + return data def load_VINS_data(self, rel_time=False): """ Loads pose estimates from VINS-Fusion - Input: rel_time - set time relative to first msg (rather than absolute) + Input: rel_time - if true, returned time is the number of seconds from + the start of the bag (as opposed to an absolute timestamp) Return: numpy array of 8 columns: time [s], @@ -689,25 +672,20 @@ def load_VINS_data(self, rel_time=False): """ tot_msg_count = self.bag.get_message_count(self.tpc_names["pose_estimates"]) - + data = np.empty((0,8), np.float) valid_msg_count = 0 print("Retrieving VINS pose estimate data from {} ...".format(self.file)) print("Number of VINS pose estimate messages: {}".format(tot_msg_count)) - + + time_offset = self.bag.get_start_time() if rel_time else 0 + # Retrieve latest path - for topic, msg, time in self.bag.read_messages(self.tpc_names["pose_estimates"]): - # Retrieve time & adjust to relative value if needed - if rel_time: - if valid_msg_count == 0: - init_time = time.to_sec() - curr_time = time.to_sec() - init_time - else: - curr_time = time.to_sec() + for _, msg, time in self.bag.read_messages(self.tpc_names["pose_estimates"]): - # Retrieve current data + curr_time = time.to_sec() - time_offset temp = np.array([curr_time, msg.pose.pose.position.x, msg.pose.pose.position.y, @@ -727,7 +705,7 @@ def load_VINS_data(self, rel_time=False): sys.stdout.write('\r') sys.stdout.write("Progress: {} %".format(self.status)) sys.stdout.flush() - + sys.stdout.write("\n") return data @@ -735,7 +713,8 @@ def load_VINS_data(self, rel_time=False): def load_sun_position_data(self, rel_time=False, reference="global"): """ Loads sun position data - Input: rel_time - set time relative to first msg (rather than absolute) + Input: rel_time - if true, returned time is the number of seconds from + the start of the bag (as opposed to an absolute timestamp) reference - sun pose vector with respect to "global" or "rover" frame Return: numpy array of 8 columns: @@ -748,16 +727,16 @@ def load_sun_position_data(self, rel_time=False, reference="global"): z_orientation, w_orientation """ - + tot_msg_count = self.bag.get_message_count(self.tpc_names["relative_sun_orientation"]) - + data = np.empty((0,8), np.float) valid_msg_count = 0 print("Retrieving sun orientation data from {} ...".format(self.file)) print("Number of sun orientation data messages: {}".format(tot_msg_count)) - + if reference == "global": data_generator = self.bag.read_messages(self.tpc_names["global_sun_orientation"]) elif reference == "relative": @@ -765,17 +744,12 @@ def load_sun_position_data(self, rel_time=False, reference="global"): else: raise NotImplementedError + time_offset = self.bag.get_start_time() if rel_time else 0 + # Retrieve latest path - for topic, msg, time in data_generator: - # Retrieve time & adjust to relative value if needed - if rel_time: - if valid_msg_count == 0: - init_time = time.to_sec() - curr_time = time.to_sec() - init_time - else: - curr_time = time.to_sec() + for _, msg, time in data_generator: - # Retrieve current data + curr_time = time.to_sec() - time_offset temp = np.array([curr_time, msg.pose.position.x, msg.pose.position.y, @@ -795,7 +769,7 @@ def load_sun_position_data(self, rel_time=False, reference="global"): sys.stdout.write('\r') sys.stdout.write("Progress: {} %".format(self.status)) sys.stdout.flush() - + sys.stdout.write("\n") return data diff --git a/requirements.txt b/requirements.txt deleted file mode 100644 index 5bf44ef..0000000 --- a/requirements.txt +++ /dev/null @@ -1,5 +0,0 @@ -argparse==1.4.0 -matplotlib==3.1.1 -numpy==1.16.0 -rasterio==1.0.18 -utm==0.4.2