├── .clang-format ├── .gitignore ├── CMakeLists.txt ├── README.md ├── app ├── airsim_simulator_node.cpp └── tools │ ├── compute_infrared_compensation.py │ └── parse_config_to_airsim.py ├── cfg ├── airsim_simulation.yaml ├── demo.yaml └── visualization │ └── demo.rviz ├── docs ├── coordinate_systems.md ├── download_ue4_assets.md ├── settings.md └── tips_and_tricks.md ├── include ├── 3rd_party │ └── csv.h └── unreal_airsim │ ├── frame_converter.h │ ├── online_simulator │ ├── sensor_timer.h │ └── simulator.h │ └── simulator_processing │ ├── depth_to_pointcloud.h │ ├── infrared_id_compensation.h │ ├── odometry_drift_simulator │ ├── normal_distribution.h │ └── odometry_drift_simulator.h │ ├── processor_base.h │ └── processor_factory.h ├── launch ├── airsim_simulator.launch ├── demo.launch └── parse_config_to_airsim.launch ├── package.xml ├── src ├── frame_converter.cpp ├── online_simulator │ ├── sensor_timer.cpp │ └── simulator.cpp └── simulator_processing │ ├── depth_to_pointcloud.cpp │ ├── infrared_id_compensation.cpp │ ├── odometry_drift_simulator │ ├── normal_distribution.cpp │ └── odometry_drift_simulator.cpp │ └── processor_factory.cpp ├── unreal_airsim_https.rosinstall └── unreal_airsim_ssh.rosinstall /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | BasedOnStyle: Google 4 | ColumnLimit: 80 5 | DerivePointerAlignment: false 6 | PointerAlignment: Left 7 | IncludeBlocks: Preserve 8 | --- 9 | Language: Proto 10 | BasedOnStyle: Google 11 | ... 12 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | cmake-build-debug/ 2 | .idea/ 3 | .vscode/ 4 | 5 | AirsimPath.txt 6 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(unreal_airsim) 3 | add_definitions(-std=c++17) 4 | 5 | # Setup Airsim 6 | include(AirsimPath.txt) 7 | # To create the above file, navigate to the directory containing this CMakeLists.txt file and then run: 8 | # echo "set(AIRSIM_ROOT $HOME/[path to your AirSim install directory])" > ./AirsimPath.txt 9 | # Be sure to replace [path to AirSim install dir] with the actual path where you installed your copy of AirSim. 10 | 11 | # We add all these sources so they can be compiled with e.g. gcc, since the Airsim(Plugin) binaries need to be compiled 12 | # with clang for compatibility with unreal engine 13 | add_subdirectory("${AIRSIM_ROOT}/cmake/rpclib_wrapper" rpclib_wrapper) 14 | add_subdirectory("${AIRSIM_ROOT}/cmake/AirLib" AirLib) 15 | add_subdirectory("${AIRSIM_ROOT}/cmake/MavLinkCom" MavLinkCom) 16 | set(RPC_LIB_INCLUDES " ${AIRSIM_ROOT}/external/rpclib/rpclib-2.3.0/include") 17 | set(RPC_LIB rpc) # name of .a file with lib prefix 18 | 19 | find_package(catkin_simple REQUIRED) 20 | catkin_simple(ALL_DEPS_REQUIRED) 21 | 22 | include_directories( 23 | include 24 | ${catkin_INCLUDE_DIRS} 25 | ${AIRSIM_ROOT}/AirLib/deps/eigen3 26 | ${AIRSIM_ROOT}/AirLib/include 27 | ${RPC_LIB_INCLUDES} 28 | ${AIRSIM_ROOT}/MavLinkCom/include 29 | ${AIRSIM_ROOT}/MavLinkCom/common_utils 30 | ${OpenCV_INCLUDE_DIRS} 31 | ) 32 | 33 | catkin_package() 34 | 35 | ############# 36 | # LIBRARIES # 37 | ############# 38 | cs_add_library(${PROJECT_NAME} 39 | # Modules 40 | src/frame_converter.cpp 41 | src/online_simulator/simulator.cpp 42 | src/online_simulator/sensor_timer.cpp 43 | src/simulator_processing/processor_factory.cpp 44 | src/simulator_processing/depth_to_pointcloud.cpp 45 | src/simulator_processing/infrared_id_compensation.cpp 46 | src/simulator_processing/odometry_drift_simulator/odometry_drift_simulator.cpp 47 | src/simulator_processing/odometry_drift_simulator/normal_distribution.cpp 48 | ) 49 | 50 | ############### 51 | # Executables # 52 | ############### 53 | cs_add_executable(airsim_simulator_node 54 | app/airsim_simulator_node.cpp 55 | ) 56 | target_link_libraries(airsim_simulator_node ${PROJECT_NAME} ${catkin_LIBRARIES} AirLib ${RPC_LIB}) 57 | 58 | 59 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # unreal_airsim 2 | This repo contains simulation tools and utilities to perform realistic simulations base on [Unreal Engine](https://www.unrealengine.com/en-US/) (UE4), using microsoft [AirSim](https://github.com/microsoft/AirSim) as interface to UE4. 3 | 4 | ![preview](https://user-images.githubusercontent.com/36043993/82642589-7551ed00-9c0e-11ea-99b6-fab22fcff837.png) 5 | 6 | # Table of Contents 7 | **Getting started:** 8 | * [Installation](#Instalation) 9 | * [Contributing](#Contributing) 10 | * [Examples](#Examples) 11 | * [Troubleshooting](#Troubleshooting) 12 | 13 | **Documentation:** 14 | * [Settings](docs/settings.md) 15 | * [Coordinate Systems](docs/coordinate_systems.md) 16 | * [Getting UE4 Assets](docs/download_ue4_assets.md) 17 | * [Tips and Tricks](docs/tips_and_tricks.md) 18 | 19 | # Installation 20 | The following 3 components are necessary to utilize the full stack of unreal_airsim tools. 21 | 22 | **Unreal Engine** 23 | 24 | Install Unreal Engine. This repository was developped and tested on UE 4.25, which is the recommended version. 25 | To install UE4 on linux, you need to register with Epic Games and build it from source. 26 | Please follow the detailed instructions on [their website](https://docs.unrealengine.com/en-US/Platforms/Linux/BeginnerLinuxDeveloper/SettingUpAnUnrealWorkflow/index.html) to set everything up. 27 | If you plan to use *only* pre-compiled binaries as simulation worlds, this section can be omitted, 28 | 29 | **Airsim** 30 | 31 | Install **our fork** of AirSim, the UE4 Plugin: 32 | ```shell script 33 | export AIRSIM_PATH= # Set the target destination. 34 | cd $AIRSIM_PATH 35 | git clone git@github.com:ethz-asl/AirSim.git 36 | cd Airsim 37 | ./setup.sh 38 | ./build.sh 39 | ``` 40 | 41 | **unreal_airsim** 42 | 43 | Install unreal_airsim, containing the simulation ROS-package and tools. 44 | 45 | * If you haven't already installed ROS, please install it according to their instructions. 46 | This repo was developed on a desktop-full version of [ROS melodic](http://wiki.ros.org/melodic/Installation/Ubuntu/). 47 | 48 | * System dependencies: 49 | ```shell script 50 | sudo apt-get install python-wstool python-catkin-tools ros-melodic-cmake-modules 51 | ``` 52 | 53 | * If you haven't already set up a caktin worskpace: 54 | ```shell script 55 | mkdir -p ~/catkin_ws/src 56 | cd ~/catkin_ws 57 | catkin init 58 | catkin config --extend /opt/ros/melodic 59 | catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release 60 | catkin config --merge-devel 61 | ``` 62 | 63 | * Install via [SSH](https://help.github.com/en/github/authenticating-to-github/connecting-to-github-with-ssh): 64 | ```shell script 65 | cd ~/catkin_ws/src 66 | git clone git@github.com:ethz-asl/unreal_airsim.git 67 | wstool init . ./unreal_airsim/unreal_airsim_ssh.rosinstall 68 | wstool update 69 | ``` 70 | * Tell `unreal_airsim` where you installed AirSim by running: 71 | ```shell script 72 | cd ~/catkin_ws/src/unreal_airsim 73 | echo "set(AIRSIM_ROOT $AIRSIM_PATH)" > ./AirsimPath.txt 74 | ``` 75 | In case you didn't set `$AIRSIM_PATH` before, don't forget to replace the above `$AIRSIM_PATH` with the absolute path to the lcoation where AirSim is installed. 76 | 77 | * Build: 78 | ```shell script 79 | catkin build unreal_airsim 80 | source ../devel/setup.bash 81 | ``` 82 | 83 | # Contributing 84 | If you are adding features to this repo please consider opening back a PR, so others can use it as well. 85 | If you consider contributing, please adhere to the [google style guide](https://google.github.io/styleguide/cppguide.html) and setup our linter: 86 | ```shell script 87 | # Download the linter 88 | cd 89 | git clone git@github.com:ethz-asl/linter.git # SSH 90 | git clone https://github.com/ethz-asl/linter.git # HTTPS 91 | 92 | # install the linter 93 | cd linter 94 | echo ". $(realpath setup_linter.sh)" >> ~/.bashrc # Or the matching file for 95 | # your shell. 96 | bash 97 | 98 | # Register this repo for the linter 99 | roscd unreal_airsim 100 | init_linter_git_hooks 101 | ``` 102 | 103 | # Examples 104 | This demo briefly walks through the steps on how to use the online_simulator. 105 | The simulation is setup from a single configuration file, a minimal example is given in `cfg/demo.yaml`. 106 | The settings required for AirSim to produce the requested simulation must first be generated by running 107 | ```shell script 108 | roslaunch unreal_airsim parse_config_to_airsim.launch 109 | ``` 110 | It is strongly recommended to produce AirSim's Settings.json using this script and not changing it manually. 111 | Additional AirSim Settings can be set in the yaml-config using *CamelCase* params with identical names. 112 | 113 | In this demo, start the `Blocks` environment provided by AirSim, and run the game in the UE4 Editor (select 'Active Play Mode' = 'Selected Viewport', then 'Play'). 114 | Then run 115 | ```shell script 116 | roslaunch unreal_airsim demo.launch 117 | ``` 118 | to start the simulation and display the sensor readings in RVIZ. 119 | 120 | ![u_airsim_4](https://user-images.githubusercontent.com/36043993/79876617-90e98e00-83eb-11ea-8edb-f11156a716d1.png) 121 | 122 | External view of the UE4 game (left), camera image (center) and lidar readings (right) obtained from the simulator. 123 | 124 | # Troubleshooting 125 | 126 | ## Installation 127 | * **Include error 'xlocale.h" not found:** 128 | 129 | xlocale was removed/renamed from glibc somewhen, can fix via symlink: 130 | ```shell script 131 | ln -s /usr/include/locale.h /usr/include/xlocale.h 132 | ``` 133 | 134 | ## Running Unreal Engine 135 | * **The UE Editor freezes when 'Compiling Shaders':** 136 | 137 | According to [this thread](https://answers.unrealengine.com/questions/936174/view.html) this may happen when using Vulcan with less than 2GB of graphics card memory. 138 | Can be adjusted by switching back to OpenGL (uncomment `TargetedRHIs=GLSL_430` in `Engine/Config/BaseEngine.ini`). 139 | 140 | ## Starting the simulation 141 | * **Error at startup: bind(): address already in use:** 142 | 143 | Airsim connects to UE4 via a TCP port. If multiple instances of Airsim (i.e. the unreal game) are running, the selected port will already be taken. 144 | Note that the editor itself already loads the Airsim plugin, thus closing all instances of UE4 (editor or game) and starting a single game/editor usually fixes this. 145 | 146 | 147 | 148 | -------------------------------------------------------------------------------- /app/airsim_simulator_node.cpp: -------------------------------------------------------------------------------- 1 | #include "unreal_airsim/online_simulator/simulator.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | // Lets the simulator shutdown in a controlled fashion 8 | std::unique_ptr the_simulator; 9 | void sigintHandler(int sig) { 10 | if (the_simulator) { 11 | the_simulator->onShutdown(); 12 | } 13 | ros::shutdown(); 14 | } 15 | 16 | int main(int argc, char** argv) { 17 | ros::init(argc, argv, "airsim_simulator", ros::init_options::NoSigintHandler); 18 | 19 | // Setup logging 20 | google::InitGoogleLogging(argv[0]); 21 | google::InstallFailureSignalHandler(); 22 | google::ParseCommandLineFlags(&argc, &argv, false); 23 | 24 | // Run node 25 | ros::NodeHandle nh(""); 26 | ros::NodeHandle nh_private("~"); 27 | signal(SIGINT, sigintHandler); 28 | the_simulator = 29 | std::make_unique(nh, nh_private); 30 | 31 | int n_threads; 32 | nh_private.param("n_threads", n_threads, 33 | 0); // 0 defaults to #physical cores available 34 | ros::AsyncSpinner spinner(n_threads); 35 | spinner.start(); 36 | ros::waitForShutdown(); 37 | return 0; 38 | } 39 | -------------------------------------------------------------------------------- /app/tools/compute_infrared_compensation.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | """ 3 | In some cases, AirSim does not generate infrared values corresponding to the 4 | meshID, see https://github.com/microsoft/AirSim/issues/1992. 5 | 6 | Use this script to produce a infrared to meshID mapping for the currently 7 | running UE4 game, that can be read by the unreal_airsim simulator using the 8 | InfraredIdCompensation processor. 9 | 10 | Notice that this script will override all meshIDs, such that they need to be 11 | reset afterwards. 12 | 13 | AirSim with 14 | """ 15 | 16 | import csv 17 | import argparse 18 | import os 19 | import sys 20 | 21 | import numpy as np 22 | import airsim 23 | 24 | 25 | def compute_infrared_correction(destination, camera_name): 26 | """ 27 | Compute the transfer dynamics from segmentation id in a currently running 28 | UE4 game and the value in the infrared image and save it to file. 29 | """ 30 | # Parse destination 31 | path = destination 32 | if destination.endswith('.csv'): 33 | path = os.path.dirname(destination) 34 | else: 35 | destination = os.path.join(path, 'infrared_corrections.csv') 36 | if not os.path.isdir(path): 37 | os.makedirs(path) 38 | 39 | # Connect to AirSim 40 | client = airsim.MultirotorClient() 41 | client.confirmConnection() 42 | 43 | # Compute 44 | print "Computing infrared corrections:" 45 | with open(destination, 'w') as csvfile: 46 | writer = csv.writer(csvfile, 47 | delimiter=',', 48 | quotechar='|', 49 | quoting=csv.QUOTE_MINIMAL) 50 | writer.writerow(["MeshID", "InfraRedID"]) 51 | requests = [ 52 | airsim.ImageRequest(str(camera_name), airsim.ImageType.Infrared, 53 | False, False) 54 | ] 55 | for i in range(256): 56 | client.simSetSegmentationObjectID(r"[\w]*", i, True) 57 | responses = client.simGetImages(requests) 58 | response = responses[0] 59 | img1d = np.fromstring(response.image_data_uint8, dtype=np.uint8) 60 | writer.writerow([i, img1d[0]]) 61 | 62 | # Display Progress 63 | progress = i / 255.0 64 | progress_disc = int(progress * 50) 65 | sys.stdout.write('\r') 66 | sys.stdout.write("Progress: [%-50s] %.2f%%" % 67 | ('=' * progress_disc, progress * 100)) 68 | sys.stdout.flush() 69 | print "\nSaved infrared corrections in '%s'." % destination 70 | 71 | 72 | if __name__ == "__main__": 73 | parser = argparse.ArgumentParser() 74 | parser.add_argument("-d", 75 | "--destination", 76 | dest='d', 77 | help="Path or filename, ending in .csv, where to save " 78 | "the infrared corrections.", 79 | default=os.getcwd()) 80 | parser.add_argument("-c", 81 | "--camera_name", 82 | dest='c', 83 | help="Name of the camera to use in AirSim.", 84 | default="") 85 | args = parser.parse_args() 86 | compute_infrared_correction(args.d, args.c) 87 | -------------------------------------------------------------------------------- /app/tools/parse_config_to_airsim.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Python 2 support 4 | from __future__ import print_function 5 | 6 | import sys 7 | import os 8 | from datetime import datetime 9 | import json 10 | import yaml 11 | 12 | import rospy 13 | from scipy.spatial.transform import Rotation 14 | 15 | 16 | class ConfigParser(object): 17 | def __init__(self): 18 | """ Initialize ros node and read params """ 19 | # Params 20 | self.target_file_path = rospy.get_param( 21 | '~target_file_path', "") # Where to write the target config 22 | if self.target_file_path == "": 23 | self.target_file_path = os.path.join(os.path.expanduser("~"), 24 | "Documents", "AirSim", 25 | "settings.json") 26 | self.config_file_path = rospy.get_param( 27 | '~config_file_path', "no_source_path_given") # Where the source is 28 | 29 | # Initial / minimal config 30 | self.new_cfg = { 31 | "ConfigParserInfo": 32 | "This config was auto-created from source '%s' on %s." % 33 | (self.config_file_path, 34 | datetime.now().strftime("%d.%m.%Y, %H:%M:%S")), 35 | "SeeDocsAt": 36 | "https://github.com/Microsoft/AirSim/blob/master/docs/settings.md", 37 | "SettingsVersion": 38 | 1.2, 39 | "SimMode": 40 | "Multirotor" 41 | } 42 | self.yaml_cfg = None 43 | self.log_counter = [0, 0, 0] 44 | 45 | # Settings not to parse 46 | self.np_general = [ 47 | "SeeDocsAt", "SettingsVersion", "SimMode", "ConfigParserInfo" 48 | ] 49 | self.np_camera = [ 50 | "T_B_S", "X", "Y", "Z", "Pitch", "Roll", "Yaw", "CaptureSettings" 51 | ] 52 | self.image_type_map = { 53 | "Scene": 0, 54 | "DepthPlanar": 1, 55 | "DepthPerspective": 2, 56 | "DepthVis": 3, 57 | "DisparityNormalized": 4, 58 | "Segmentation": 5, 59 | "SurfaceNormals": 6, 60 | "Infrared": 7 61 | } 62 | self.np_lidar = ["T_B_S", "SensorType", "Enabled", "DataFrame"] 63 | self.np_imu = ["T_B_S", "SensorType", "Enabled"] 64 | 65 | def log(self, string, severity=0): 66 | if severity == 0: 67 | rospy.loginfo(string) 68 | elif severity == 1: 69 | rospy.logwarn(string) 70 | elif severity == 2: 71 | rospy.logerr(string) 72 | self.log_counter[severity] = self.log_counter[severity] + 1 73 | 74 | def parse(self): 75 | """ Execute the settings conversion """ 76 | info = "* Starting config parser for unreal_airsim to AirSim config " \ 77 | "conversion *" 78 | self.log("\n" + "*" * len(info) + "\n" + info + "\n" + "*" * len(info)) 79 | 80 | self.read_source_config() 81 | self.setup_target_file() 82 | self.forward_args(self.yaml_cfg, self.new_cfg, self.np_general) 83 | sensor_dict, camera_dict = self.parse_sensors() 84 | vehicle, vehicle_name = self.parse_vehicle() 85 | vehicle["Cameras"] = camera_dict 86 | vehicle["Sensors"] = sensor_dict 87 | self.new_cfg["Vehicles"] = {vehicle_name: vehicle} 88 | self.write_target_file() 89 | info = "* Settings parsing finished successfully (%i Warnings, " \ 90 | "%i Errors)! *" % (self.log_counter[1], self.log_counter[2]) 91 | self.log("\n" + "*" * len(info) + "\n" + info + "\n" + "*" * len(info)) 92 | self.log("This process will now terminate. This is intended behavior.") 93 | 94 | def write_target_file(self): 95 | j = json.dumps(self.new_cfg, indent=2) 96 | f = open(self.target_file_path, 'w') 97 | print(j, end="", file=f) 98 | f.close() 99 | self.log("Wrote config to target file '%s'." % self.target_file_path) 100 | 101 | def parse_vehicle(self): 102 | """ Read settings for a (the) vehicle """ 103 | # TODO(schmluk): maybe pass on other vehicle settings as well here 104 | vehicle = {"VehicleType": "SimpleFlight"} 105 | vehicle_name = rospy.get_param("vehicle_name", 106 | "airsim_drone") # default 107 | self.log("Using vehicle name '%s'." % vehicle_name) 108 | return vehicle, vehicle_name 109 | 110 | def parse_sensors(self): 111 | """ Identify all listed sensors and convert to AirSim settings """ 112 | sensors = self.yaml_cfg["sensors"] 113 | sensor_dict = {} 114 | camera_dict = {} 115 | for name in sensors: 116 | data = sensors[name] 117 | out = {} 118 | if not "sensor_type" in data: 119 | self.log( 120 | "Skipping sensor '%s', no 'sensor_type' given!" % name, 2) 121 | continue 122 | sensor_type = data["sensor_type"] 123 | if sensor_type == "Camera": 124 | self.log("Found camera '%s'." % name) 125 | image_type = "Scene" # default 126 | if "image_type" in data: 127 | image_type = data["image_type"] 128 | if image_type not in self.image_type_map: 129 | self.log( 130 | "Unknown image_type '%s', defaulting to 'Scene'!\n" 131 | "(Known types are %s)" % (image_type, ', '.join( 132 | " %s" % self.image_type_map[key] 133 | for key in self.image_type_map)), 1) 134 | image_type = "Scene" 135 | capture_settings = { 136 | "ImageType": self.image_type_map[image_type] 137 | } 138 | if "CaptureSettings" in data: 139 | capt_in = data["CaptureSettings"] 140 | self.forward_args(capt_in, capture_settings, ["ImageType"]) 141 | out["CaptureSettings"] = [capture_settings] 142 | self.forward_args(data, out, self.np_camera) 143 | T = None 144 | if "T_B_S" in data: 145 | T = data["T_B_S"] 146 | self.parse_transformation(T, out) 147 | camera_dict[name] = out 148 | elif sensor_type == "Lidar": 149 | self.log("Found lidar '%s'." % name) 150 | out = { 151 | "SensorType": 6, 152 | "Enabled": True, 153 | "DataFrame": "SensorLocalFrame" 154 | } 155 | self.forward_args(data, out, self.np_lidar) 156 | T = None 157 | if "T_B_S" in data: 158 | T = data["T_B_S"] 159 | self.parse_transformation(T, out) 160 | sensor_dict[name] = out 161 | elif sensor_type == "Imu": 162 | self.log("Found imu '%s'." % name) 163 | out = {"SensorType": 2, "Enabled": True} 164 | self.forward_args(data, out, self.np_imu) 165 | T = None 166 | if "T_B_S" in data: 167 | T = data["T_B_S"] 168 | self.parse_transformation(T, out) 169 | sensor_dict[name] = out 170 | return sensor_dict, camera_dict 171 | 172 | def read_source_config(self): 173 | """ Check that source file exists and read yml dict """ 174 | if not os.path.isfile(self.config_file_path): 175 | self.log( 176 | "Source file '%s' could not be found!" % self.config_file_path, 177 | 2) 178 | sys.exit() 179 | try: 180 | with open(self.config_file_path) as yaml_file: 181 | self.yaml_cfg = yaml.load(yaml_file, Loader=yaml.Loader) 182 | self.log("Read source: '%s'" % self.config_file_path) 183 | except OSError: 184 | self.log( 185 | "Source file '%s' is not a valid yaml file and could not be " 186 | "read!" % self.config_file_path, 2) 187 | sys.exit() 188 | 189 | def setup_target_file(self): 190 | """ Setup directory and backup existing configs if they have changed """ 191 | backup_old_file = True 192 | if os.path.isfile(self.target_file_path): 193 | if os.stat(self.target_file_path).st_size != 0: 194 | with open(self.target_file_path) as json_file: 195 | old_cfg = {} 196 | try: 197 | old_cfg = json.load(json_file) 198 | except OSError: 199 | backup_old_file = True 200 | self.log("Existing file '%s' could not be read." % 201 | self.target_file_path) 202 | if "ConfigParserInfo" in old_cfg: 203 | time_str = old_cfg["ConfigParserInfo"] 204 | time_str = time_str[-21:-1] 205 | time = datetime.strptime(time_str, "%d.%m.%Y, %H:%M:%S") 206 | change_time = datetime.fromtimestamp( 207 | os.path.getmtime(self.target_file_path)) 208 | if (change_time - time).total_seconds() < 30: 209 | # file has not changed, we give 30s leeway for parsing 210 | backup_old_file = False 211 | self.log( 212 | "Existing file '%s' was auto-generated and will be " 213 | "overwritten." % self.target_file_path) 214 | else: 215 | self.log( 216 | "Existing file '%s' has been changed since auto-" 217 | "generation." % self.target_file_path) 218 | elif old_cfg is {}: 219 | self.log("Existing file '%s' was not auto-generated." % 220 | self.target_file_path) 221 | else: 222 | self.log(f"Existing file {self.target_file_path} is empty.") 223 | backup_old_file = False 224 | 225 | else: 226 | self.log("No existing '%s' found." % self.target_file_path) 227 | backup_old_file = False 228 | target_dir = os.path.dirname(self.target_file_path) 229 | if not os.path.isdir(target_dir): 230 | os.mkdir(target_dir) 231 | rospy.loginfo("Created target directory '%s'." % target_dir) 232 | if backup_old_file: 233 | backup_name = self.target_file_path + datetime.now().strftime( 234 | ".backup_%d_%m_%Y_%H_%M_%S") 235 | os.rename(self.target_file_path, backup_name) 236 | self.log("Created backup of existing settings at '%s'." % 237 | backup_name) 238 | 239 | @staticmethod 240 | def forward_args(in_dict, out_dict, exclusion): 241 | """ Forward settings from one dict to the other, except exclusions""" 242 | for key in in_dict: 243 | if str(key[:1]).isupper(): 244 | # Airsim settings are CamelCase, unreal_airsim are lower_case 245 | if key not in exclusion: 246 | out_dict[key] = in_dict[key] 247 | 248 | def parse_transformation(self, list_in, dict_out): 249 | if list_in is None: 250 | dict_out["X"] = 0 251 | dict_out["Y"] = 0 252 | dict_out["Z"] = 0 253 | dict_out["Roll"] = 0 254 | dict_out["Pitch"] = 0 255 | dict_out["Yaw"] = 0 256 | else: 257 | if len(list_in) != 4: 258 | self.log( 259 | "Transformations are expected as 4x4 matrices, data " 260 | "will be ignored!", 1) 261 | self.parse_transformation(None, dict_out) 262 | return 263 | else: 264 | for l in list_in: 265 | if len(l) != 4: 266 | self.log( 267 | "Transformations are expected as 4x4 matrices," 268 | " data will be ignored!", 1) 269 | self.parse_transformation(None, dict_out) 270 | return 271 | dict_out["X"] = list_in[0][3] 272 | dict_out["Y"] = -list_in[1][3] 273 | dict_out["Z"] = -list_in[2][3] 274 | R = [x[:3] for x in list_in[:3]] 275 | # Support python 2 and 3 scipy versions. 276 | if sys.version_info < (3, 0): 277 | R = Rotation.from_dcm(R) 278 | else: 279 | R = Rotation.from_matrix(R) 280 | euler = R.as_euler( 281 | 'xyz', degrees=True 282 | ) # Airsim is quite inconsistent here with rotation parametrization 283 | dict_out["Roll"] = round(euler[0], 3) 284 | dict_out["Pitch"] = round(-euler[1], 3) 285 | dict_out["Yaw"] = round(-euler[2], 3) 286 | 287 | 288 | if __name__ == '__main__': 289 | rospy.init_node('config_parser', anonymous=True) 290 | parser = ConfigParser() 291 | parser.parse() 292 | -------------------------------------------------------------------------------- /cfg/airsim_simulation.yaml: -------------------------------------------------------------------------------- 1 | vehicle_name: airsim_drone 2 | state_refresh_rate: 100 3 | 4 | sensors: 5 | Scene_cam: 6 | sensor_type: Camera 7 | image_type: Scene 8 | rate: 3 9 | CaptureSettings: 10 | ImageType: 0 11 | Width: 256 12 | Height: 144 13 | FOV_Degrees: 90 14 | AutoExposureSpeed: 100 15 | AutoExposureBias: 0 16 | AutoExposureMaxBrightness: 0.64 17 | AutoExposureMinBrightness: 0.03 18 | MotionBlurAmount: 0 19 | TargetGamma: 1.0 20 | ProjectionMode: "" 21 | OrthoWidth: 5.12 22 | 23 | Depth_cam: 24 | sensor_type: Camera 25 | image_type: DepthPlanar 26 | rate: 3 27 | pixels_as_float: true 28 | 29 | Seg_cam: 30 | sensor_type: Camera 31 | image_type: Segmentation 32 | rate: 3 33 | 34 | Lidar: 35 | sensor_type: Lidar 36 | rate: 3 37 | # T_B_S: # 30 pitch 38 | # - [ 0.8660254, 0.0000000, 0.5000000, 0] 39 | # - [0.0000000, 1.0000000, 0.0000000, 0] 40 | # - [-0.5000000, 0.0000000, 0.8660254,0 ] 41 | # - [0, 0, 0, 1] 42 | # T_B_S: 43 | # - [1,0,0,0] 44 | # - [0,1,0,0] 45 | # - [0,0,1,0] 46 | # - [1,0,0,3] 47 | T_B_S: # Transformation body to sensor in body-frame (30pitch, 30 roll) 48 | - [ 0.8660254, 0.2500000, 0.433012, 0] 49 | - [-0.0000000, 0.8660254, -0.5000000, 0] 50 | - [-0.5000000, 0.4330127, 0.7500000, 0] 51 | - [0, 0, 0, 1] 52 | 53 | Imu: 54 | sensor_type: Imu 55 | rate: 100 56 | -------------------------------------------------------------------------------- /cfg/demo.yaml: -------------------------------------------------------------------------------- 1 | # General Settings 2 | vehicle_name: airsim_drone # Params in lower_case are processed by unreal_airsim 3 | state_refresh_rate: 100 4 | ClockSpeed: 1.0 # Params in CamelCase are forwarded to the AirSim config and use AirSim defaults 5 | 6 | # Which Sensors to mount in the simulation 7 | sensors: 8 | Scene_cam: 9 | sensor_type: Camera 10 | image_type: Scene 11 | rate: 3 12 | CaptureSettings: 13 | Width: 640 14 | Height: 480 15 | FOV_Degrees: 90 16 | T_B_S: 17 | - [1, 0, 0, 0.3] 18 | - [0, 1, 0, 0] 19 | - [0, 0, 1, 0] 20 | - [0, 0, 0, 1] 21 | 22 | Depth_cam: 23 | sensor_type: Camera 24 | image_type: DepthPlanar 25 | rate: 3 26 | pixels_as_float: true 27 | CaptureSettings: 28 | Width: 640 29 | Height: 480 30 | FOV_Degrees: 90 31 | T_B_S: 32 | - [1, 0, 0, 0.3] 33 | - [0, 1, 0, 0] 34 | - [0, 0, 1, 0] 35 | - [0, 0, 0, 1] 36 | 37 | Seg_cam: 38 | sensor_type: Camera 39 | image_type: Segmentation 40 | rate: 3 41 | CaptureSettings: 42 | Width: 640 43 | Height: 480 44 | FOV_Degrees: 90 45 | T_B_S: 46 | - [1, 0, 0, 0.3] 47 | - [0, 1, 0, 0] 48 | - [0, 0, 1, 0] 49 | - [0, 0, 0, 1] 50 | 51 | Lidar: 52 | sensor_type: Lidar 53 | rate: 3 54 | T_B_S: # Transformation to the sensor link in body-frame (0 yaw, 30 pitch, 0 roll) 55 | - [0.8660254, 0.0000000, 0.5000000, 0] 56 | - [0.0000000, 1.0000000, 0.0000000, 0] 57 | - [-0.5000000, 0.0000000, 0.8660254, 0] 58 | - [0, 0, 0, 1] 59 | NumberOfChannels: 16 60 | PointsPerSecond: 100000 61 | RotationsPerSecond: 10 62 | VerticalFOVUpper: 30 63 | VerticalFOVLower: -30 64 | 65 | Imu: 66 | sensor_type: Imu 67 | rate: 100 68 | 69 | # Post processing of simulation data can be added in a modular way 70 | processors: 71 | RGBD_cam: 72 | processor_type: DepthToPointcloud 73 | depth_camera_name: Depth_cam 74 | color_camera_name: Scene_cam 75 | -------------------------------------------------------------------------------- /cfg/visualization/demo.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: ~ 7 | Splitter Ratio: 0.5 8 | Tree Height: 309 9 | - Class: rviz/Selection 10 | Name: Selection 11 | - Class: rviz/Tool Properties 12 | Expanded: 13 | - /2D Pose Estimate1 14 | - /2D Nav Goal1 15 | - /Publish Point1 16 | Name: Tool Properties 17 | Splitter Ratio: 0.5886790156364441 18 | - Class: rviz/Views 19 | Expanded: 20 | - /Current View1 21 | Name: Views 22 | Splitter Ratio: 0.5 23 | - Class: rviz/Time 24 | Experimental: false 25 | Name: Time 26 | SyncMode: 0 27 | SyncSource: LidarPoints 28 | Preferences: 29 | PromptSaveOnExit: true 30 | Toolbars: 31 | toolButtonStyle: 2 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.029999999329447746 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/Axes 54 | Enabled: true 55 | Length: 1 56 | Name: world_frame 57 | Radius: 0.10000000149011612 58 | Reference Frame: 59 | Value: true 60 | - Class: rviz/Axes 61 | Enabled: true 62 | Length: 0.5 63 | Name: Drone 64 | Radius: 0.15000000596046448 65 | Reference Frame: airsim_drone 66 | Value: true 67 | - Alpha: 1 68 | Autocompute Intensity Bounds: true 69 | Autocompute Value Bounds: 70 | Max Value: 10 71 | Min Value: -10 72 | Value: true 73 | Axis: Z 74 | Channel Name: intensity 75 | Class: rviz/PointCloud2 76 | Color: 255; 255; 255 77 | Color Transformer: Intensity 78 | Decay Time: 0 79 | Enabled: true 80 | Invert Rainbow: false 81 | Max Color: 255; 255; 255 82 | Max Intensity: 4096 83 | Min Color: 0; 0; 0 84 | Min Intensity: 0 85 | Name: LidarPoints 86 | Position Transformer: XYZ 87 | Queue Size: 10 88 | Selectable: true 89 | Size (Pixels): 2 90 | Size (m): 0.20000000298023224 91 | Style: Points 92 | Topic: /airsim_drone/Lidar 93 | Unreliable: false 94 | Use Fixed Frame: true 95 | Use rainbow: true 96 | Value: true 97 | - Class: rviz/Image 98 | Enabled: true 99 | Image Topic: /airsim_drone/Scene_cam 100 | Max Value: 1 101 | Median window: 5 102 | Min Value: 0 103 | Name: SceneCamImage 104 | Normalize Range: true 105 | Queue Size: 2 106 | Transport Hint: raw 107 | Unreliable: false 108 | Value: true 109 | - Alpha: 1 110 | Autocompute Intensity Bounds: true 111 | Autocompute Value Bounds: 112 | Max Value: 10 113 | Min Value: -10 114 | Value: true 115 | Axis: Z 116 | Channel Name: intensity 117 | Class: rviz/PointCloud2 118 | Color: 255; 255; 255 119 | Color Transformer: RGB8 120 | Decay Time: 0 121 | Enabled: true 122 | Invert Rainbow: false 123 | Max Color: 255; 255; 255 124 | Max Intensity: 4096 125 | Min Color: 0; 0; 0 126 | Min Intensity: 0 127 | Name: RGBD_Cam 128 | Position Transformer: XYZ 129 | Queue Size: 10 130 | Selectable: true 131 | Size (Pixels): 3 132 | Size (m): 0.009999999776482582 133 | Style: Points 134 | Topic: /airsim_drone/RGBD_cam 135 | Unreliable: false 136 | Use Fixed Frame: true 137 | Use rainbow: true 138 | Value: true 139 | Enabled: true 140 | Global Options: 141 | Background Color: 48; 48; 48 142 | Default Light: true 143 | Fixed Frame: world 144 | Frame Rate: 30 145 | Name: root 146 | Tools: 147 | - Class: rviz/Interact 148 | Hide Inactive Objects: true 149 | - Class: rviz/MoveCamera 150 | - Class: rviz/Select 151 | - Class: rviz/FocusCamera 152 | - Class: rviz/Measure 153 | - Class: rviz/SetInitialPose 154 | Theta std deviation: 0.2617993950843811 155 | Topic: /initialpose 156 | X std deviation: 0.5 157 | Y std deviation: 0.5 158 | - Class: rviz/SetGoal 159 | Topic: /move_base_simple/goal 160 | - Class: rviz/PublishPoint 161 | Single click: true 162 | Topic: /clicked_point 163 | Value: true 164 | Views: 165 | Current: 166 | Class: rviz/Orbit 167 | Distance: 14.423685073852539 168 | Enable Stereo Rendering: 169 | Stereo Eye Separation: 0.05999999865889549 170 | Stereo Focal Distance: 1 171 | Swap Stereo Eyes: false 172 | Value: false 173 | Focal Point: 174 | X: 0 175 | Y: 0 176 | Z: 0 177 | Focal Shape Fixed Size: true 178 | Focal Shape Size: 0.05000000074505806 179 | Invert Z Axis: false 180 | Name: Current View 181 | Near Clip Distance: 0.009999999776482582 182 | Pitch: 0.47979745268821716 183 | Target Frame: 184 | Value: Orbit (rviz) 185 | Yaw: 3.143549919128418 186 | Saved: ~ 187 | Window Geometry: 188 | Displays: 189 | collapsed: false 190 | Height: 868 191 | Hide Left Dock: false 192 | Hide Right Dock: false 193 | QMainWindow State: 000000ff00000000fd000000040000000000000157000002c6fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001c0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001a005300630065006e006500430061006d0049006d0061006700650100000203000001000000001600ffffff000000010000010f000002c6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002c6000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004e30000003efc0100000002fb0000000800540069006d00650100000000000004e3000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000271000002c600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 194 | SceneCamImage: 195 | collapsed: false 196 | Selection: 197 | collapsed: false 198 | Time: 199 | collapsed: false 200 | Tool Properties: 201 | collapsed: false 202 | Views: 203 | collapsed: false 204 | Width: 1251 205 | X: 60 206 | Y: 27 207 | -------------------------------------------------------------------------------- /docs/coordinate_systems.md: -------------------------------------------------------------------------------- 1 | # Coordinate Systems 2 | Unreal Engine, AirSim, and unreal_airsim all use different coordinate systems and parametrization conventions. 3 | These systems and conventions as well as conversions are thus briefly introduced. 4 | 5 | ![frames](https://user-images.githubusercontent.com/36043993/81938412-2811c200-95f5-11ea-8a80-5c95efa48a88.png) 6 | 7 | ## Unreal Engine 8 | UE4 uses standard xyz-coordinates, although the coordinate system is **not** right-handed (i.e. x-forward, y-right, z-up). 9 | Default units is cm. 10 | Rotations are parametrized as Euler-XYZ (roll-pitch-yaw), however, rotations are mathematically positive around the x-axis and negative around the y- and z-axes. 11 | Default units are degrees. 12 | 13 | This convention is used for everything inside the UE4 Editor, e.g. when creating worlds or exporting ground truth. 14 | 15 | ## AirSim 16 | Airsim uses North-East-Down (NED) coordinates, where north is aligned with the Unreal Engine x-axis. 17 | Default units are m. 18 | Rotations are parametrized as Euler-ZYX (yaw-pitch-roll), default units are degrees. 19 | 20 | Airsim uses different frames, where the output of data is typically in `AirsimLocal` frame, which is (according to their docs) aligned with PlayerStart in UE4. 21 | Notice, that because the frame is NED, it is invariant w.r.t the orientation of PlayerStart. 22 | For data obtained in other frames (e.g. Lidar in SensorLocalFrame), the parametrization is still NED but with North aligned with the sensor-x-axis. 23 | 24 | * **Note:** Although `AirsimLocal` is supposed to lie at PlayerStart, there seems to be a lot of strange things in there. 25 | E.g. that the origin of a robot is not it's center but is shifted by its height/2 to the lowermost point. 26 | Furthermore, it's not clear whether they read out PlayerStart Coordinates or setup on connection, leading to strange behavior when connecting for multiple experiment runs inside a singel game.\ 27 | **TODO: Verify T_UnrealWorld_AirsimLocal**, as it is important for evaluation vs ground truth. 28 | 29 | This convention is used for everything interfacing with AirSim, such as client requests and responses, as well as in settings.json. 30 | 31 | ## unreal_airsim 32 | Unreal_airsim uses the [ASL coordinate conventions](https://github.com/ethz-asl/mav_tools/wiki/Coordinate-Systems), i.e. all coordinate systems are right handed (x-forward, y-left, z-up). 33 | The simulator frame is aligned with `AirsimLocal`, including the orientation of PlayerStart, thus coinciding fully with PlayerStart (odom frame) (ideally, as soon as above note is fixed). 34 | Note that cameras operate in camera frame (x-right, y-down, z-depth). 35 | 36 | Use this convention for everything that interfaces with the unreal_airsim simulator, e.g. published and subscribed topics, as well as in the unreal_airsim_settings.yaml. 37 | -------------------------------------------------------------------------------- /docs/download_ue4_assets.md: -------------------------------------------------------------------------------- 1 | # Downloading UE4 Assets 2 | ## Available Assets 3 | As of this writing, there's 6 different asset packs available at ASL. 4 | All asset packs were downloaded from the Unreal Market Place on UE 4.24. 5 | These packages can be used inside custom unreal worlds and usually come with a prebuilt demo. 6 | The following assets are currently available: 7 | 8 | **Indoor:** 9 | * Flat - [ArchViz Furniture - Flat Pack](https://www.unrealengine.com/marketplace/en-US/product/archviz-furniture-flat-pack) 10 | * StudioApartment - [Modern Studio Apartment](https://www.unrealengine.com/marketplace/en-US/product/modern-studio-apartment/reviews) 11 | 12 | **Outdoor:** 13 | * OldTown - [Old Town](https://www.unrealengine.com/marketplace/en-US/product/old-town) 14 | * ModularNeighborhoodPack - [Modular Neighborhood Pack](https://www.unrealengine.com/marketplace/en-US/product/modular-neighborhood-pack) 15 | 16 | **Underground:** 17 | * Underground_Bunker - [Underground Caves and Bunker](https://www.unrealengine.com/marketplace/en-US/product/underground-caves-and-bunker) 18 | * ModularSubwayTunnels - [Modular Subway Tunnels](https://www.unrealengine.com/marketplace/en-US/product/modular-subway-tunnel) 19 | 20 | ## Downloading an Asset Pack 21 | * The assets are stored on [GDrive](https://drive.google.com/drive/folders/1sad6Mr4RXUySrlpo2XLNs_GoyRkhRRth) at `Mapping/simulation/UE4_assets`. 22 | Please ask the mapping team for access to the required assets (.zip folder). 23 | * Unzip the downloaded folder. Unfortunately, Ubuntu unzip is not extremely robust, if you get an error use jar: 24 | ```shell script 25 | sudo apt-get install fastjar 26 | jar xvf OldTown.zip 27 | ``` 28 | 29 | ## Using the Asset Pack in the Unreal Editor 30 | Recall that the asset packs are only a collection of assets and not full projects. 31 | Therefore they can only be added to existing projects: 32 | * If you want to create a new project, create one using the UE4 editor. 33 | E.g. for a blank project use `File -> New Project... -> Game -> Blank`. 34 | * Move or copy the entire asset folder **as is** into `.../Unreal Projects//Content`. 35 | * The assets can now be accessed from the 'Content Browser' in the UE4 Editor, where you can drag and drop assets into your level. 36 | Many packs come with a pre-built demo *level*, which can be opened via `File -> Open Level` or by double clicking it in the Content Browser. 37 | -------------------------------------------------------------------------------- /docs/settings.md: -------------------------------------------------------------------------------- 1 | # Settings 2 | ## Introduction 3 | AirSim uses its own settings file, which is loaded from `~/Documents/AirSim/settings.json`, to setup the simulation in UE4. 4 | Because the unreal_airsim simulator needs to be aware of the same settings and for ease of use, a **single** settings file from the unreal_airsim repo is used for both. 5 | 6 | Specifiy your required settings (see [below](#Using-the-unreal_airsim-settings)) in `my_settings.yaml` and then use the Config Parser 7 | ``` 8 | roslaunch unreal_airsim parse_config_to_airsim.launch source:=path/to/my_settings.yaml 9 | ``` 10 | to generate the `settings.json` for you. 11 | The parser still allows you to specify arbitrary AirSim-Settings-fields, but also takes care of e.g. frame conversions and required settings for the simulator to run properly. 12 | Make sure to always have matching settings, i.e. load `my_settings.yaml` into the airsim simulator node and rerun the settings parser and restart the unreal game whenever the settings are changed. 13 | 14 | ## Using the unreal_airsim settings 15 | The unreal_airsim config file is structured into two parts, being the **General Settings** and **Components**. 16 | 17 | * The **General Settings** contain all parameters for the simulator and general settings for AirSim. 18 | 19 | * As of this writing, there exists 2 types of **Components**: sensors and processors, where sensors specify the sensor setup that is to be simulated, and processors can perform arbitrary pre- or post-processing steps that require access to the simulation. 20 | The components are all organized as `component_type/name/params`. 21 | The `name` is an unique specifier for the component to be built, but aside from default e.g. topic names it has no physical meaning. 22 | 23 | All parameters for general settings and sensors are listed in the `online_simulator/simulator.h` in the Config struct and set in `readParamsFromRos()`. 24 | All parameters for processors can be found in their individual `setupFromRos()` function. 25 | 26 | The parameter naming is such that all unreal_airsim params are in `lower_case`. 27 | To set AirSim params (as in settings.json), just add them with identical name and value in `CamelCase` to my_settings.yaml. 28 | They will be forwarded to the AirSim settings except when they interfere with crucial prerequisites. 29 | 30 | An example of how to construct an unreal_airsim settings file is given in `cfg/demo.yaml`. 31 | ![settings](https://user-images.githubusercontent.com/36043993/80593316-48505700-8a21-11ea-8f67-7108c47d6f1e.png) -------------------------------------------------------------------------------- /docs/tips_and_tricks.md: -------------------------------------------------------------------------------- 1 | # Tips and Tricks 2 | This page contains a number of tricks to make working with unreal easier. 3 | 4 | ## Performance 5 | * **Performance drops when I tab out of the UE4 game**: 6 | 7 | In `Editor Preferences/General/Miscellaneous/Performance` is a flag "Use Less CPU when in Background". Disable this flag to get full performance all the time. 8 | -------------------------------------------------------------------------------- /include/3rd_party/csv.h: -------------------------------------------------------------------------------- 1 | // Copyright: (2012-2015) Ben Strasser 2 | // License: BSD-3 3 | // 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // 3. Neither the name of the copyright holder nor the names of its contributors 17 | // may be used to endorse or promote products derived from this software 18 | // without specific prior written permission. 19 | // 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 24 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 25 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 26 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 27 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 28 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 29 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | // POSSIBILITY OF SUCH DAMAGE. 31 | 32 | #ifndef CSV_H 33 | #define CSV_H 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #ifndef CSV_IO_NO_THREAD 43 | #include 44 | #include 45 | #include 46 | #endif 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | namespace io { 53 | //////////////////////////////////////////////////////////////////////////// 54 | // LineReader // 55 | //////////////////////////////////////////////////////////////////////////// 56 | 57 | namespace error { 58 | struct base : std::exception { 59 | virtual void format_error_message() const = 0; 60 | 61 | const char* what() const noexcept override { 62 | format_error_message(); 63 | return error_message_buffer; 64 | } 65 | 66 | mutable char error_message_buffer[512]; 67 | }; 68 | 69 | const int max_file_name_length = 255; 70 | 71 | struct with_file_name { 72 | with_file_name() { std::memset(file_name, 0, sizeof(file_name)); } 73 | 74 | void set_file_name(const char* file_name) { 75 | if (file_name != nullptr) { 76 | strncpy(this->file_name, file_name, sizeof(this->file_name)); 77 | this->file_name[sizeof(this->file_name) - 1] = '\0'; 78 | } else { 79 | this->file_name[0] = '\0'; 80 | } 81 | } 82 | 83 | char file_name[max_file_name_length + 1]; 84 | }; 85 | 86 | struct with_file_line { 87 | with_file_line() { file_line = -1; } 88 | 89 | void set_file_line(int file_line) { this->file_line = file_line; } 90 | 91 | int file_line; 92 | }; 93 | 94 | struct with_errno { 95 | with_errno() { errno_value = 0; } 96 | 97 | void set_errno(int errno_value) { this->errno_value = errno_value; } 98 | 99 | int errno_value; 100 | }; 101 | 102 | struct can_not_open_file : base, with_file_name, with_errno { 103 | void format_error_message() const override { 104 | if (errno_value != 0) 105 | std::snprintf(error_message_buffer, sizeof(error_message_buffer), 106 | "Can not open file \"%s\" because \"%s\".", file_name, 107 | std::strerror(errno_value)); 108 | else 109 | std::snprintf(error_message_buffer, sizeof(error_message_buffer), 110 | "Can not open file \"%s\".", file_name); 111 | } 112 | }; 113 | 114 | struct line_length_limit_exceeded : base, with_file_name, with_file_line { 115 | void format_error_message() const override { 116 | std::snprintf( 117 | error_message_buffer, sizeof(error_message_buffer), 118 | "Line number %d in file \"%s\" exceeds the maximum length of 2^24-1.", 119 | file_line, file_name); 120 | } 121 | }; 122 | } // namespace error 123 | 124 | class ByteSourceBase { 125 | public: 126 | virtual int read(char* buffer, int size) = 0; 127 | virtual ~ByteSourceBase() {} 128 | }; 129 | 130 | namespace detail { 131 | 132 | class OwningStdIOByteSourceBase : public ByteSourceBase { 133 | public: 134 | explicit OwningStdIOByteSourceBase(FILE* file) : file(file) { 135 | // Tell the std library that we want to do the buffering ourself. 136 | std::setvbuf(file, 0, _IONBF, 0); 137 | } 138 | 139 | int read(char* buffer, int size) { return std::fread(buffer, 1, size, file); } 140 | 141 | ~OwningStdIOByteSourceBase() { std::fclose(file); } 142 | 143 | private: 144 | FILE* file; 145 | }; 146 | 147 | class NonOwningIStreamByteSource : public ByteSourceBase { 148 | public: 149 | explicit NonOwningIStreamByteSource(std::istream& in) : in(in) {} 150 | 151 | int read(char* buffer, int size) { 152 | in.read(buffer, size); 153 | return in.gcount(); 154 | } 155 | 156 | ~NonOwningIStreamByteSource() {} 157 | 158 | private: 159 | std::istream& in; 160 | }; 161 | 162 | class NonOwningStringByteSource : public ByteSourceBase { 163 | public: 164 | NonOwningStringByteSource(const char* str, long long size) 165 | : str(str), remaining_byte_count(size) {} 166 | 167 | int read(char* buffer, int desired_byte_count) { 168 | int to_copy_byte_count = desired_byte_count; 169 | if (remaining_byte_count < to_copy_byte_count) 170 | to_copy_byte_count = remaining_byte_count; 171 | std::memcpy(buffer, str, to_copy_byte_count); 172 | remaining_byte_count -= to_copy_byte_count; 173 | str += to_copy_byte_count; 174 | return to_copy_byte_count; 175 | } 176 | 177 | ~NonOwningStringByteSource() {} 178 | 179 | private: 180 | const char* str; 181 | long long remaining_byte_count; 182 | }; 183 | 184 | #ifndef CSV_IO_NO_THREAD 185 | class AsynchronousReader { 186 | public: 187 | void init(std::unique_ptr arg_byte_source) { 188 | std::unique_lock guard(lock); 189 | byte_source = std::move(arg_byte_source); 190 | desired_byte_count = -1; 191 | termination_requested = false; 192 | worker = std::thread([&] { 193 | std::unique_lock guard(lock); 194 | try { 195 | for (;;) { 196 | read_requested_condition.wait(guard, [&] { 197 | return desired_byte_count != -1 || termination_requested; 198 | }); 199 | if (termination_requested) return; 200 | 201 | read_byte_count = byte_source->read(buffer, desired_byte_count); 202 | desired_byte_count = -1; 203 | if (read_byte_count == 0) break; 204 | read_finished_condition.notify_one(); 205 | } 206 | } catch (...) { 207 | read_error = std::current_exception(); 208 | } 209 | read_finished_condition.notify_one(); 210 | }); 211 | } 212 | 213 | bool is_valid() const { return byte_source != nullptr; } 214 | 215 | void start_read(char* arg_buffer, int arg_desired_byte_count) { 216 | std::unique_lock guard(lock); 217 | buffer = arg_buffer; 218 | desired_byte_count = arg_desired_byte_count; 219 | read_byte_count = -1; 220 | read_requested_condition.notify_one(); 221 | } 222 | 223 | int finish_read() { 224 | std::unique_lock guard(lock); 225 | read_finished_condition.wait( 226 | guard, [&] { return read_byte_count != -1 || read_error; }); 227 | if (read_error) 228 | std::rethrow_exception(read_error); 229 | else 230 | return read_byte_count; 231 | } 232 | 233 | ~AsynchronousReader() { 234 | if (byte_source != nullptr) { 235 | { 236 | std::unique_lock guard(lock); 237 | termination_requested = true; 238 | } 239 | read_requested_condition.notify_one(); 240 | worker.join(); 241 | } 242 | } 243 | 244 | private: 245 | std::unique_ptr byte_source; 246 | 247 | std::thread worker; 248 | 249 | bool termination_requested; 250 | std::exception_ptr read_error; 251 | char* buffer; 252 | int desired_byte_count; 253 | int read_byte_count; 254 | 255 | std::mutex lock; 256 | std::condition_variable read_finished_condition; 257 | std::condition_variable read_requested_condition; 258 | }; 259 | #endif 260 | 261 | class SynchronousReader { 262 | public: 263 | void init(std::unique_ptr arg_byte_source) { 264 | byte_source = std::move(arg_byte_source); 265 | } 266 | 267 | bool is_valid() const { return byte_source != nullptr; } 268 | 269 | void start_read(char* arg_buffer, int arg_desired_byte_count) { 270 | buffer = arg_buffer; 271 | desired_byte_count = arg_desired_byte_count; 272 | } 273 | 274 | int finish_read() { return byte_source->read(buffer, desired_byte_count); } 275 | 276 | private: 277 | std::unique_ptr byte_source; 278 | char* buffer; 279 | int desired_byte_count; 280 | }; 281 | } // namespace detail 282 | 283 | class LineReader { 284 | private: 285 | static const int block_len = 1 << 20; 286 | std::unique_ptr buffer; // must be constructed before (and thus 287 | // destructed after) the reader! 288 | #ifdef CSV_IO_NO_THREAD 289 | detail::SynchronousReader reader; 290 | #else 291 | detail::AsynchronousReader reader; 292 | #endif 293 | int data_begin; 294 | int data_end; 295 | 296 | char file_name[error::max_file_name_length + 1]; 297 | unsigned file_line; 298 | 299 | static std::unique_ptr open_file(const char* file_name) { 300 | // We open the file in binary mode as it makes no difference under *nix 301 | // and under Windows we handle \r\n newlines ourself. 302 | FILE* file = std::fopen(file_name, "rb"); 303 | if (file == 0) { 304 | int x = errno; // store errno as soon as possible, doing it after 305 | // constructor call can fail. 306 | error::can_not_open_file err; 307 | err.set_errno(x); 308 | err.set_file_name(file_name); 309 | throw err; 310 | } 311 | return std::unique_ptr( 312 | new detail::OwningStdIOByteSourceBase(file)); 313 | } 314 | 315 | void init(std::unique_ptr byte_source) { 316 | file_line = 0; 317 | 318 | buffer = std::unique_ptr(new char[3 * block_len]); 319 | data_begin = 0; 320 | data_end = byte_source->read(buffer.get(), 2 * block_len); 321 | 322 | // Ignore UTF-8 BOM 323 | if (data_end >= 3 && buffer[0] == '\xEF' && buffer[1] == '\xBB' && 324 | buffer[2] == '\xBF') 325 | data_begin = 3; 326 | 327 | if (data_end == 2 * block_len) { 328 | reader.init(std::move(byte_source)); 329 | reader.start_read(buffer.get() + 2 * block_len, block_len); 330 | } 331 | } 332 | 333 | public: 334 | LineReader() = delete; 335 | LineReader(const LineReader&) = delete; 336 | LineReader& operator=(const LineReader&) = delete; 337 | 338 | explicit LineReader(const char* file_name) { 339 | set_file_name(file_name); 340 | init(open_file(file_name)); 341 | } 342 | 343 | explicit LineReader(const std::string& file_name) { 344 | set_file_name(file_name.c_str()); 345 | init(open_file(file_name.c_str())); 346 | } 347 | 348 | LineReader(const char* file_name, 349 | std::unique_ptr byte_source) { 350 | set_file_name(file_name); 351 | init(std::move(byte_source)); 352 | } 353 | 354 | LineReader(const std::string& file_name, 355 | std::unique_ptr byte_source) { 356 | set_file_name(file_name.c_str()); 357 | init(std::move(byte_source)); 358 | } 359 | 360 | LineReader(const char* file_name, const char* data_begin, 361 | const char* data_end) { 362 | set_file_name(file_name); 363 | init(std::unique_ptr(new detail::NonOwningStringByteSource( 364 | data_begin, data_end - data_begin))); 365 | } 366 | 367 | LineReader(const std::string& file_name, const char* data_begin, 368 | const char* data_end) { 369 | set_file_name(file_name.c_str()); 370 | init(std::unique_ptr(new detail::NonOwningStringByteSource( 371 | data_begin, data_end - data_begin))); 372 | } 373 | 374 | LineReader(const char* file_name, FILE* file) { 375 | set_file_name(file_name); 376 | init(std::unique_ptr( 377 | new detail::OwningStdIOByteSourceBase(file))); 378 | } 379 | 380 | LineReader(const std::string& file_name, FILE* file) { 381 | set_file_name(file_name.c_str()); 382 | init(std::unique_ptr( 383 | new detail::OwningStdIOByteSourceBase(file))); 384 | } 385 | 386 | LineReader(const char* file_name, std::istream& in) { 387 | set_file_name(file_name); 388 | init(std::unique_ptr( 389 | new detail::NonOwningIStreamByteSource(in))); 390 | } 391 | 392 | LineReader(const std::string& file_name, std::istream& in) { 393 | set_file_name(file_name.c_str()); 394 | init(std::unique_ptr( 395 | new detail::NonOwningIStreamByteSource(in))); 396 | } 397 | 398 | void set_file_name(const std::string& file_name) { 399 | set_file_name(file_name.c_str()); 400 | } 401 | 402 | void set_file_name(const char* file_name) { 403 | if (file_name != nullptr) { 404 | strncpy(this->file_name, file_name, sizeof(this->file_name)); 405 | this->file_name[sizeof(this->file_name) - 1] = '\0'; 406 | } else { 407 | this->file_name[0] = '\0'; 408 | } 409 | } 410 | 411 | const char* get_truncated_file_name() const { return file_name; } 412 | 413 | void set_file_line(unsigned file_line) { this->file_line = file_line; } 414 | 415 | unsigned get_file_line() const { return file_line; } 416 | 417 | char* next_line() { 418 | if (data_begin == data_end) return nullptr; 419 | 420 | ++file_line; 421 | 422 | assert(data_begin < data_end); 423 | assert(data_end <= block_len * 2); 424 | 425 | if (data_begin >= block_len) { 426 | std::memcpy(buffer.get(), buffer.get() + block_len, block_len); 427 | data_begin -= block_len; 428 | data_end -= block_len; 429 | if (reader.is_valid()) { 430 | data_end += reader.finish_read(); 431 | std::memcpy(buffer.get() + block_len, buffer.get() + 2 * block_len, 432 | block_len); 433 | reader.start_read(buffer.get() + 2 * block_len, block_len); 434 | } 435 | } 436 | 437 | int line_end = data_begin; 438 | while (buffer[line_end] != '\n' && line_end != data_end) { 439 | ++line_end; 440 | } 441 | 442 | if (line_end - data_begin + 1 > block_len) { 443 | error::line_length_limit_exceeded err; 444 | err.set_file_name(file_name); 445 | err.set_file_line(file_line); 446 | throw err; 447 | } 448 | 449 | if (buffer[line_end] == '\n' && line_end != data_end) { 450 | buffer[line_end] = '\0'; 451 | } else { 452 | // some files are missing the newline at the end of the 453 | // last line 454 | ++data_end; 455 | buffer[line_end] = '\0'; 456 | } 457 | 458 | // handle windows \r\n-line breaks 459 | if (line_end != data_begin && buffer[line_end - 1] == '\r') 460 | buffer[line_end - 1] = '\0'; 461 | 462 | char* ret = buffer.get() + data_begin; 463 | data_begin = line_end + 1; 464 | return ret; 465 | } 466 | }; 467 | 468 | //////////////////////////////////////////////////////////////////////////// 469 | // CSV // 470 | //////////////////////////////////////////////////////////////////////////// 471 | 472 | namespace error { 473 | const int max_column_name_length = 63; 474 | struct with_column_name { 475 | with_column_name() { 476 | std::memset(column_name, 0, max_column_name_length + 1); 477 | } 478 | 479 | void set_column_name(const char* column_name) { 480 | if (column_name != nullptr) { 481 | std::strncpy(this->column_name, column_name, max_column_name_length); 482 | this->column_name[max_column_name_length] = '\0'; 483 | } else { 484 | this->column_name[0] = '\0'; 485 | } 486 | } 487 | 488 | char column_name[max_column_name_length + 1]; 489 | }; 490 | 491 | const int max_column_content_length = 63; 492 | 493 | struct with_column_content { 494 | with_column_content() { 495 | std::memset(column_content, 0, max_column_content_length + 1); 496 | } 497 | 498 | void set_column_content(const char* column_content) { 499 | if (column_content != nullptr) { 500 | std::strncpy(this->column_content, column_content, 501 | max_column_content_length); 502 | this->column_content[max_column_content_length] = '\0'; 503 | } else { 504 | this->column_content[0] = '\0'; 505 | } 506 | } 507 | 508 | char column_content[max_column_content_length + 1]; 509 | }; 510 | 511 | struct extra_column_in_header : base, with_file_name, with_column_name { 512 | void format_error_message() const override { 513 | std::snprintf(error_message_buffer, sizeof(error_message_buffer), 514 | R"(Extra column "%s" in header of file "%s".)", column_name, 515 | file_name); 516 | } 517 | }; 518 | 519 | struct missing_column_in_header : base, with_file_name, with_column_name { 520 | void format_error_message() const override { 521 | std::snprintf(error_message_buffer, sizeof(error_message_buffer), 522 | R"(Missing column "%s" in header of file "%s".)", column_name, 523 | file_name); 524 | } 525 | }; 526 | 527 | struct duplicated_column_in_header : base, with_file_name, with_column_name { 528 | void format_error_message() const override { 529 | std::snprintf(error_message_buffer, sizeof(error_message_buffer), 530 | R"(Duplicated column "%s" in header of file "%s".)", 531 | column_name, file_name); 532 | } 533 | }; 534 | 535 | struct header_missing : base, with_file_name { 536 | void format_error_message() const override { 537 | std::snprintf(error_message_buffer, sizeof(error_message_buffer), 538 | "Header missing in file \"%s\".", file_name); 539 | } 540 | }; 541 | 542 | struct too_few_columns : base, with_file_name, with_file_line { 543 | void format_error_message() const override { 544 | std::snprintf(error_message_buffer, sizeof(error_message_buffer), 545 | "Too few columns in line %d in file \"%s\".", file_line, 546 | file_name); 547 | } 548 | }; 549 | 550 | struct too_many_columns : base, with_file_name, with_file_line { 551 | void format_error_message() const override { 552 | std::snprintf(error_message_buffer, sizeof(error_message_buffer), 553 | "Too many columns in line %d in file \"%s\".", file_line, 554 | file_name); 555 | } 556 | }; 557 | 558 | struct escaped_string_not_closed : base, with_file_name, with_file_line { 559 | void format_error_message() const override { 560 | std::snprintf(error_message_buffer, sizeof(error_message_buffer), 561 | "Escaped string was not closed in line %d in file \"%s\".", 562 | file_line, file_name); 563 | } 564 | }; 565 | 566 | struct integer_must_be_positive : base, 567 | with_file_name, 568 | with_file_line, 569 | with_column_name, 570 | with_column_content { 571 | void format_error_message() const override { 572 | std::snprintf( 573 | error_message_buffer, sizeof(error_message_buffer), 574 | R"(The integer "%s" must be positive or 0 in column "%s" in file "%s" in line "%d".)", 575 | column_content, column_name, file_name, file_line); 576 | } 577 | }; 578 | 579 | struct no_digit : base, 580 | with_file_name, 581 | with_file_line, 582 | with_column_name, 583 | with_column_content { 584 | void format_error_message() const override { 585 | std::snprintf( 586 | error_message_buffer, sizeof(error_message_buffer), 587 | R"(The integer "%s" contains an invalid digit in column "%s" in file "%s" in line "%d".)", 588 | column_content, column_name, file_name, file_line); 589 | } 590 | }; 591 | 592 | struct integer_overflow : base, 593 | with_file_name, 594 | with_file_line, 595 | with_column_name, 596 | with_column_content { 597 | void format_error_message() const override { 598 | std::snprintf( 599 | error_message_buffer, sizeof(error_message_buffer), 600 | R"(The integer "%s" overflows in column "%s" in file "%s" in line "%d".)", 601 | column_content, column_name, file_name, file_line); 602 | } 603 | }; 604 | 605 | struct integer_underflow : base, 606 | with_file_name, 607 | with_file_line, 608 | with_column_name, 609 | with_column_content { 610 | void format_error_message() const override { 611 | std::snprintf( 612 | error_message_buffer, sizeof(error_message_buffer), 613 | R"(The integer "%s" underflows in column "%s" in file "%s" in line "%d".)", 614 | column_content, column_name, file_name, file_line); 615 | } 616 | }; 617 | 618 | struct invalid_single_character : base, 619 | with_file_name, 620 | with_file_line, 621 | with_column_name, 622 | with_column_content { 623 | void format_error_message() const override { 624 | std::snprintf( 625 | error_message_buffer, sizeof(error_message_buffer), 626 | R"(The content "%s" of column "%s" in file "%s" in line "%d" is not a single character.)", 627 | column_content, column_name, file_name, file_line); 628 | } 629 | }; 630 | } // namespace error 631 | 632 | using ignore_column = unsigned int; 633 | static const ignore_column ignore_no_column = 0; 634 | static const ignore_column ignore_extra_column = 1; 635 | static const ignore_column ignore_missing_column = 2; 636 | 637 | template 638 | struct trim_chars { 639 | private: 640 | constexpr static bool is_trim_char(char) { return false; } 641 | 642 | template 643 | constexpr static bool is_trim_char(char c, char trim_char, 644 | OtherTrimChars... other_trim_chars) { 645 | return c == trim_char || is_trim_char(c, other_trim_chars...); 646 | } 647 | 648 | public: 649 | static void trim(char*& str_begin, char*& str_end) { 650 | while (str_begin != str_end && is_trim_char(*str_begin, trim_char_list...)) 651 | ++str_begin; 652 | while (str_begin != str_end && 653 | is_trim_char(*(str_end - 1), trim_char_list...)) 654 | --str_end; 655 | *str_end = '\0'; 656 | } 657 | }; 658 | 659 | struct no_comment { 660 | static bool is_comment(const char*) { return false; } 661 | }; 662 | 663 | template 664 | struct single_line_comment { 665 | private: 666 | constexpr static bool is_comment_start_char(char) { return false; } 667 | 668 | template 669 | constexpr static bool is_comment_start_char( 670 | char c, char comment_start_char, 671 | OtherCommentStartChars... other_comment_start_chars) { 672 | return c == comment_start_char || 673 | is_comment_start_char(c, other_comment_start_chars...); 674 | } 675 | 676 | public: 677 | static bool is_comment(const char* line) { 678 | return is_comment_start_char(*line, comment_start_char_list...); 679 | } 680 | }; 681 | 682 | struct empty_line_comment { 683 | static bool is_comment(const char* line) { 684 | if (*line == '\0') return true; 685 | while (*line == ' ' || *line == '\t') { 686 | ++line; 687 | if (*line == 0) return true; 688 | } 689 | return false; 690 | } 691 | }; 692 | 693 | template 694 | struct single_and_empty_line_comment { 695 | static bool is_comment(const char* line) { 696 | return single_line_comment::is_comment(line) || 697 | empty_line_comment::is_comment(line); 698 | } 699 | }; 700 | 701 | template 702 | struct no_quote_escape { 703 | static const char* find_next_column_end(const char* col_begin) { 704 | while (*col_begin != sep && *col_begin != '\0') ++col_begin; 705 | return col_begin; 706 | } 707 | 708 | static void unescape(char*&, char*&) {} 709 | }; 710 | 711 | template 712 | struct double_quote_escape { 713 | static const char* find_next_column_end(const char* col_begin) { 714 | while (*col_begin != sep && *col_begin != '\0') 715 | if (*col_begin != quote) 716 | ++col_begin; 717 | else { 718 | do { 719 | ++col_begin; 720 | while (*col_begin != quote) { 721 | if (*col_begin == '\0') throw error::escaped_string_not_closed(); 722 | ++col_begin; 723 | } 724 | ++col_begin; 725 | } while (*col_begin == quote); 726 | } 727 | return col_begin; 728 | } 729 | 730 | static void unescape(char*& col_begin, char*& col_end) { 731 | if (col_end - col_begin >= 2) { 732 | if (*col_begin == quote && *(col_end - 1) == quote) { 733 | ++col_begin; 734 | --col_end; 735 | char* out = col_begin; 736 | for (char* in = col_begin; in != col_end; ++in) { 737 | if (*in == quote && (in + 1) != col_end && *(in + 1) == quote) { 738 | ++in; 739 | } 740 | *out = *in; 741 | ++out; 742 | } 743 | col_end = out; 744 | *col_end = '\0'; 745 | } 746 | } 747 | } 748 | }; 749 | 750 | struct throw_on_overflow { 751 | template 752 | static void on_overflow(T&) { 753 | throw error::integer_overflow(); 754 | } 755 | 756 | template 757 | static void on_underflow(T&) { 758 | throw error::integer_underflow(); 759 | } 760 | }; 761 | 762 | struct ignore_overflow { 763 | template 764 | static void on_overflow(T&) {} 765 | 766 | template 767 | static void on_underflow(T&) {} 768 | }; 769 | 770 | struct set_to_max_on_overflow { 771 | template 772 | static void on_overflow(T& x) { 773 | // using (std::numeric_limits::max) instead of 774 | // std::numeric_limits::max to make code including windows.h with its max 775 | // macro happy 776 | x = (std::numeric_limits::max)(); 777 | } 778 | 779 | template 780 | static void on_underflow(T& x) { 781 | x = (std::numeric_limits::min)(); 782 | } 783 | }; 784 | 785 | namespace detail { 786 | template 787 | void chop_next_column(char*& line, char*& col_begin, char*& col_end) { 788 | assert(line != nullptr); 789 | 790 | col_begin = line; 791 | // the col_begin + (... - col_begin) removes the constness 792 | col_end = 793 | col_begin + (quote_policy::find_next_column_end(col_begin) - col_begin); 794 | 795 | if (*col_end == '\0') { 796 | line = nullptr; 797 | } else { 798 | *col_end = '\0'; 799 | line = col_end + 1; 800 | } 801 | } 802 | 803 | template 804 | void parse_line(char* line, char** sorted_col, 805 | const std::vector& col_order) { 806 | for (int i : col_order) { 807 | if (line == nullptr) throw ::io::error::too_few_columns(); 808 | char *col_begin, *col_end; 809 | chop_next_column(line, col_begin, col_end); 810 | 811 | if (i != -1) { 812 | trim_policy::trim(col_begin, col_end); 813 | quote_policy::unescape(col_begin, col_end); 814 | 815 | sorted_col[i] = col_begin; 816 | } 817 | } 818 | if (line != nullptr) throw ::io::error::too_many_columns(); 819 | } 820 | 821 | template 822 | void parse_header_line(char* line, std::vector& col_order, 823 | const std::string* col_name, 824 | ignore_column ignore_policy) { 825 | col_order.clear(); 826 | 827 | bool found[column_count]; 828 | std::fill(found, found + column_count, false); 829 | while (line) { 830 | char *col_begin, *col_end; 831 | chop_next_column(line, col_begin, col_end); 832 | 833 | trim_policy::trim(col_begin, col_end); 834 | quote_policy::unescape(col_begin, col_end); 835 | 836 | for (unsigned i = 0; i < column_count; ++i) 837 | if (col_begin == col_name[i]) { 838 | if (found[i]) { 839 | error::duplicated_column_in_header err; 840 | err.set_column_name(col_begin); 841 | throw err; 842 | } 843 | found[i] = true; 844 | col_order.push_back(i); 845 | col_begin = 0; 846 | break; 847 | } 848 | if (col_begin) { 849 | if (ignore_policy & ::io::ignore_extra_column) 850 | col_order.push_back(-1); 851 | else { 852 | error::extra_column_in_header err; 853 | err.set_column_name(col_begin); 854 | throw err; 855 | } 856 | } 857 | } 858 | if (!(ignore_policy & ::io::ignore_missing_column)) { 859 | for (unsigned i = 0; i < column_count; ++i) { 860 | if (!found[i]) { 861 | error::missing_column_in_header err; 862 | err.set_column_name(col_name[i].c_str()); 863 | throw err; 864 | } 865 | } 866 | } 867 | } 868 | 869 | template 870 | void parse(char* col, char& x) { 871 | if (!*col) throw error::invalid_single_character(); 872 | x = *col; 873 | ++col; 874 | if (*col) throw error::invalid_single_character(); 875 | } 876 | 877 | template 878 | void parse(char* col, std::string& x) { 879 | x = col; 880 | } 881 | 882 | template 883 | void parse(char* col, const char*& x) { 884 | x = col; 885 | } 886 | 887 | template 888 | void parse(char* col, char*& x) { 889 | x = col; 890 | } 891 | 892 | template 893 | void parse_unsigned_integer(const char* col, T& x) { 894 | x = 0; 895 | while (*col != '\0') { 896 | if ('0' <= *col && *col <= '9') { 897 | T y = *col - '0'; 898 | if (x > ((std::numeric_limits::max)() - y) / 10) { 899 | overflow_policy::on_overflow(x); 900 | return; 901 | } 902 | x = 10 * x + y; 903 | } else 904 | throw error::no_digit(); 905 | ++col; 906 | } 907 | } 908 | 909 | template 910 | void parse(char* col, unsigned char& x) { 911 | parse_unsigned_integer(col, x); 912 | } 913 | template 914 | void parse(char* col, unsigned short& x) { 915 | parse_unsigned_integer(col, x); 916 | } 917 | template 918 | void parse(char* col, unsigned int& x) { 919 | parse_unsigned_integer(col, x); 920 | } 921 | template 922 | void parse(char* col, unsigned long& x) { 923 | parse_unsigned_integer(col, x); 924 | } 925 | template 926 | void parse(char* col, unsigned long long& x) { 927 | parse_unsigned_integer(col, x); 928 | } 929 | 930 | template 931 | void parse_signed_integer(const char* col, T& x) { 932 | if (*col == '-') { 933 | ++col; 934 | 935 | x = 0; 936 | while (*col != '\0') { 937 | if ('0' <= *col && *col <= '9') { 938 | T y = *col - '0'; 939 | if (x < ((std::numeric_limits::min)() + y) / 10) { 940 | overflow_policy::on_underflow(x); 941 | return; 942 | } 943 | x = 10 * x - y; 944 | } else 945 | throw error::no_digit(); 946 | ++col; 947 | } 948 | return; 949 | } else if (*col == '+') 950 | ++col; 951 | parse_unsigned_integer(col, x); 952 | } 953 | 954 | template 955 | void parse(char* col, signed char& x) { 956 | parse_signed_integer(col, x); 957 | } 958 | template 959 | void parse(char* col, signed short& x) { 960 | parse_signed_integer(col, x); 961 | } 962 | template 963 | void parse(char* col, signed int& x) { 964 | parse_signed_integer(col, x); 965 | } 966 | template 967 | void parse(char* col, signed long& x) { 968 | parse_signed_integer(col, x); 969 | } 970 | template 971 | void parse(char* col, signed long long& x) { 972 | parse_signed_integer(col, x); 973 | } 974 | 975 | template 976 | void parse_float(const char* col, T& x) { 977 | bool is_neg = false; 978 | if (*col == '-') { 979 | is_neg = true; 980 | ++col; 981 | } else if (*col == '+') 982 | ++col; 983 | 984 | x = 0; 985 | while ('0' <= *col && *col <= '9') { 986 | int y = *col - '0'; 987 | x *= 10; 988 | x += y; 989 | ++col; 990 | } 991 | 992 | if (*col == '.' || *col == ',') { 993 | ++col; 994 | T pos = 1; 995 | while ('0' <= *col && *col <= '9') { 996 | pos /= 10; 997 | int y = *col - '0'; 998 | ++col; 999 | x += y * pos; 1000 | } 1001 | } 1002 | 1003 | if (*col == 'e' || *col == 'E') { 1004 | ++col; 1005 | int e; 1006 | 1007 | parse_signed_integer(col, e); 1008 | 1009 | if (e != 0) { 1010 | T base; 1011 | if (e < 0) { 1012 | base = T(0.1); 1013 | e = -e; 1014 | } else { 1015 | base = T(10); 1016 | } 1017 | 1018 | while (e != 1) { 1019 | if ((e & 1) == 0) { 1020 | base = base * base; 1021 | e >>= 1; 1022 | } else { 1023 | x *= base; 1024 | --e; 1025 | } 1026 | } 1027 | x *= base; 1028 | } 1029 | } else { 1030 | if (*col != '\0') throw error::no_digit(); 1031 | } 1032 | 1033 | if (is_neg) x = -x; 1034 | } 1035 | 1036 | template 1037 | void parse(char* col, float& x) { 1038 | parse_float(col, x); 1039 | } 1040 | template 1041 | void parse(char* col, double& x) { 1042 | parse_float(col, x); 1043 | } 1044 | template 1045 | void parse(char* col, long double& x) { 1046 | parse_float(col, x); 1047 | } 1048 | 1049 | template 1050 | void parse(char* col, T& x) { 1051 | // Mute unused variable compiler warning 1052 | (void)col; 1053 | (void)x; 1054 | // GCC evalutes "false" when reading the template and 1055 | // "sizeof(T)!=sizeof(T)" only when instantiating it. This is why 1056 | // this strange construct is used. 1057 | static_assert(sizeof(T) != sizeof(T), 1058 | "Can not parse this type. Only buildin integrals, floats, " 1059 | "char, char*, const char* and std::string are supported"); 1060 | } 1061 | 1062 | } // namespace detail 1063 | 1064 | template , 1065 | class quote_policy = no_quote_escape<','>, 1066 | class overflow_policy = throw_on_overflow, 1067 | class comment_policy = no_comment> 1068 | class CSVReader { 1069 | private: 1070 | LineReader in; 1071 | 1072 | char* row[column_count]; 1073 | std::string column_names[column_count]; 1074 | 1075 | std::vector col_order; 1076 | 1077 | template 1078 | void set_column_names(std::string s, ColNames... cols) { 1079 | column_names[column_count - sizeof...(ColNames) - 1] = std::move(s); 1080 | set_column_names(std::forward(cols)...); 1081 | } 1082 | 1083 | void set_column_names() {} 1084 | 1085 | public: 1086 | CSVReader() = delete; 1087 | CSVReader(const CSVReader&) = delete; 1088 | CSVReader& operator=(const CSVReader&); 1089 | 1090 | template 1091 | explicit CSVReader(Args&&... args) : in(std::forward(args)...) { 1092 | std::fill(row, row + column_count, nullptr); 1093 | col_order.resize(column_count); 1094 | for (unsigned i = 0; i < column_count; ++i) col_order[i] = i; 1095 | for (unsigned i = 1; i <= column_count; ++i) 1096 | column_names[i - 1] = "col" + std::to_string(i); 1097 | } 1098 | 1099 | char* next_line() { return in.next_line(); } 1100 | 1101 | template 1102 | void read_header(ignore_column ignore_policy, ColNames... cols) { 1103 | static_assert(sizeof...(ColNames) >= column_count, 1104 | "not enough column names specified"); 1105 | static_assert(sizeof...(ColNames) <= column_count, 1106 | "too many column names specified"); 1107 | try { 1108 | set_column_names(std::forward(cols)...); 1109 | 1110 | char* line; 1111 | do { 1112 | line = in.next_line(); 1113 | if (!line) throw error::header_missing(); 1114 | } while (comment_policy::is_comment(line)); 1115 | 1116 | detail::parse_header_line( 1117 | line, col_order, column_names, ignore_policy); 1118 | } catch (error::with_file_name& err) { 1119 | err.set_file_name(in.get_truncated_file_name()); 1120 | throw; 1121 | } 1122 | } 1123 | 1124 | template 1125 | void set_header(ColNames... cols) { 1126 | static_assert(sizeof...(ColNames) >= column_count, 1127 | "not enough column names specified"); 1128 | static_assert(sizeof...(ColNames) <= column_count, 1129 | "too many column names specified"); 1130 | set_column_names(std::forward(cols)...); 1131 | std::fill(row, row + column_count, nullptr); 1132 | col_order.resize(column_count); 1133 | for (unsigned i = 0; i < column_count; ++i) col_order[i] = i; 1134 | } 1135 | 1136 | bool has_column(const std::string& name) const { 1137 | return col_order.end() != 1138 | std::find(col_order.begin(), col_order.end(), 1139 | std::find(std::begin(column_names), std::end(column_names), 1140 | name) - 1141 | std::begin(column_names)); 1142 | } 1143 | 1144 | void set_file_name(const std::string& file_name) { 1145 | in.set_file_name(file_name); 1146 | } 1147 | 1148 | void set_file_name(const char* file_name) { in.set_file_name(file_name); } 1149 | 1150 | const char* get_truncated_file_name() const { 1151 | return in.get_truncated_file_name(); 1152 | } 1153 | 1154 | void set_file_line(unsigned file_line) { in.set_file_line(file_line); } 1155 | 1156 | unsigned get_file_line() const { return in.get_file_line(); } 1157 | 1158 | private: 1159 | void parse_helper(std::size_t) {} 1160 | 1161 | template 1162 | void parse_helper(std::size_t r, T& t, ColType&... cols) { 1163 | if (row[r]) { 1164 | try { 1165 | try { 1166 | ::io::detail::parse(row[r], t); 1167 | } catch (error::with_column_content& err) { 1168 | err.set_column_content(row[r]); 1169 | throw; 1170 | } 1171 | } catch (error::with_column_name& err) { 1172 | err.set_column_name(column_names[r].c_str()); 1173 | throw; 1174 | } 1175 | } 1176 | parse_helper(r + 1, cols...); 1177 | } 1178 | 1179 | public: 1180 | template 1181 | bool read_row(ColType&... cols) { 1182 | static_assert(sizeof...(ColType) >= column_count, 1183 | "not enough columns specified"); 1184 | static_assert(sizeof...(ColType) <= column_count, 1185 | "too many columns specified"); 1186 | try { 1187 | try { 1188 | char* line; 1189 | do { 1190 | line = in.next_line(); 1191 | if (!line) return false; 1192 | } while (comment_policy::is_comment(line)); 1193 | 1194 | detail::parse_line(line, row, col_order); 1195 | 1196 | parse_helper(0, cols...); 1197 | } catch (error::with_file_name& err) { 1198 | err.set_file_name(in.get_truncated_file_name()); 1199 | throw; 1200 | } 1201 | } catch (error::with_file_line& err) { 1202 | err.set_file_line(in.get_file_line()); 1203 | throw; 1204 | } 1205 | 1206 | return true; 1207 | } 1208 | }; 1209 | } // namespace io 1210 | #endif 1211 | -------------------------------------------------------------------------------- /include/unreal_airsim/frame_converter.h: -------------------------------------------------------------------------------- 1 | #ifndef UNREAL_AIRSIM_FRAME_CONVERTER_H_ 2 | #define UNREAL_AIRSIM_FRAME_CONVERTER_H_ 3 | 4 | // ROS 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace unreal_airsim { 13 | 14 | /*** 15 | * Conversions between ros and airsim frames. See the coordinate/frames section 16 | * in the Readme/doc for more info. Currently uses only rotation, no offset. The 17 | * rotation setup is in ROS coordinates. 18 | */ 19 | class FrameConverter { 20 | public: 21 | FrameConverter(); 22 | virtual ~FrameConverter() = default; 23 | 24 | // setup, these take the rotation of the airsim to the ros in ros frame as 25 | // input 26 | void reset(); 27 | void setupFromYaw(double yaw); // rad 28 | void setupFromQuat(const Eigen::Quaterniond& quat); 29 | 30 | // accessors 31 | const Eigen::Matrix3d& getRotation() const { return rotation_; } 32 | const Eigen::Matrix3d& getRotationInverse() const { return rot_inverse_; } 33 | 34 | // interfaces 35 | void airsimToRos(geometry_msgs::Vector3* point) const; 36 | void airsimToRos(Eigen::Vector3d* point) const; 37 | void airsimToRos(geometry_msgs::Point* point) const; 38 | void airsimToRos(geometry_msgs::Quaternion* orientation) const; 39 | void airsimToRos(Eigen::Quaterniond* orientation) const; 40 | void airsimToRos(geometry_msgs::Pose* pose) const; 41 | void airsimToRos(geometry_msgs::Transform* pose) const; 42 | 43 | void rosToAirsim(Eigen::Vector3d* point) const; 44 | void rosToAirsim(geometry_msgs::Point* point) const; 45 | void rosToAirsim(Eigen::Quaterniond* orientation) const; 46 | void rosToAirsim(geometry_msgs::Quaternion* orientation) const; 47 | void rosToAirsim(geometry_msgs::Pose* pose) const; 48 | 49 | // transformations 50 | void transformPointAirsimToRos(double* x, double* y, double* z) const; 51 | void transformOrientationAirsimToRos(double* w, double* x, double* y, 52 | double* z) const; 53 | void transformPointRosToAirsim(double* x, double* y, double* z) const; 54 | void transformOrientationRosToAirsim(double* w, double* x, double* y, 55 | double* z) const; 56 | 57 | protected: 58 | Eigen::Matrix3d rotation_; 59 | Eigen::Matrix3d rot_inverse_; 60 | }; 61 | 62 | } // namespace unreal_airsim 63 | 64 | #endif // UNREAL_AIRSIM_FRAME_CONVERTER_H_ 65 | -------------------------------------------------------------------------------- /include/unreal_airsim/online_simulator/sensor_timer.h: -------------------------------------------------------------------------------- 1 | #ifndef UNREAL_AIRSIM_ONLINE_SIMULATOR_SENSOR_TIMER_H_ 2 | #define UNREAL_AIRSIM_ONLINE_SIMULATOR_SENSOR_TIMER_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | #include "unreal_airsim/frame_converter.h" 13 | 14 | namespace unreal_airsim { 15 | class AirsimSimulator; 16 | 17 | /*** 18 | * This class manages the sensor timers and callbacks for the simulator. 19 | */ 20 | class SensorTimer { 21 | public: 22 | SensorTimer(const ros::NodeHandle& nh, double rate, bool is_private, 23 | const std::string& vehicle_name, AirsimSimulator* parent); 24 | virtual ~SensorTimer() = default; 25 | 26 | void timerCallback(const ros::TimerEvent&); 27 | 28 | double getRate() const; 29 | bool isPrivate() const; 30 | void signalShutdown(); 31 | void addSensor(const AirsimSimulator& simulator, int sensor_index); 32 | 33 | protected: 34 | AirsimSimulator* parent_; // Acces to owner 35 | 36 | // general 37 | bool is_shutdown_; 38 | double rate_; // rate of the sensor callback 39 | bool is_private_; // whether this timer runs on multiple sensors or a single 40 | // one 41 | msr::airlib::MultirotorRpcLibClient airsim_client_; 42 | ros::Timer timer_; 43 | std::string vehicle_name_; 44 | ros::NodeHandle nh_; 45 | tf2_ros::TransformBroadcaster tf_broadcaster_; 46 | ros::Publisher transform_pub_; 47 | 48 | // methods 49 | void processCameras(); 50 | void processLidars(); 51 | void processImus(); 52 | 53 | // cameras 54 | std::vector camera_pubs_; 55 | std::vector camera_frame_names_; 56 | std::vector image_requests_; 57 | 58 | // lidars 59 | std::vector lidar_pubs_; 60 | std::vector lidar_names_; 61 | std::vector lidar_frame_names_; 62 | 63 | // imus 64 | std::vector imu_pubs_; 65 | std::vector imu_names_; 66 | std::vector imu_frame_names_; 67 | }; 68 | 69 | } // namespace unreal_airsim 70 | 71 | #endif // UNREAL_AIRSIM_ONLINE_SIMULATOR_SENSOR_TIMER_H_ 72 | -------------------------------------------------------------------------------- /include/unreal_airsim/online_simulator/simulator.h: -------------------------------------------------------------------------------- 1 | #ifndef UNREAL_AIRSIM_ONLINE_SIMULATOR_SIMULATOR_H_ 2 | #define UNREAL_AIRSIM_ONLINE_SIMULATOR_SIMULATOR_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | // AirSim 15 | #include 16 | #include 17 | 18 | #include "unreal_airsim/frame_converter.h" 19 | #include "unreal_airsim/online_simulator/sensor_timer.h" 20 | #include "unreal_airsim/simulator_processing/processor_base.h" 21 | 22 | #include "unreal_airsim/simulator_processing/odometry_drift_simulator/odometry_drift_simulator.h" 23 | 24 | namespace unreal_airsim { 25 | /*** 26 | * This class implements a simulation interface with airsim. 27 | * Current application case is for a single Multirotor Vehicle. 28 | */ 29 | class AirsimSimulator { 30 | public: 31 | /*** 32 | * Available settings for the simulation and their defaults. 33 | */ 34 | struct Config { 35 | // general settings 36 | double state_refresh_rate = 100; // hz 37 | int time_publisher_interval = 38 | 2; // ms, this is the interval (in wall-time) in which the airsim time 39 | // is published as sim_time, i.e. 500 Hz. This only happens if 40 | // use_sim_time=true during launch. 41 | std::string simulator_frame_name = "odom"; 42 | 43 | // vehicle (the multirotor) 44 | std::string vehicle_name = 45 | "airsim_drone"; // Currently assumes a single drone, multi-vehicle sim 46 | // could be setup 47 | double velocity = 1.0; // m/s, for high level movement commands 48 | msr::airlib::DrivetrainType drive_train_type = 49 | msr::airlib::DrivetrainType::MaxDegreeOfFreedom; // this is currently 50 | // fixed 51 | 52 | // sensors 53 | bool publish_sensor_transforms = true; // publish transforms when receiving 54 | // sensor measurements to guarantee correct tfs. 55 | // TODO(schmluk): This is mostly a time syncing problem, maybe easiest to 56 | // publish the body pose based on these readings. 57 | struct Sensor { 58 | inline static const std::string TYPE_CAMERA = "Camera"; 59 | inline static const std::string TYPE_LIDAR = "Lidar"; 60 | inline static const std::string TYPE_IMU = "Imu"; 61 | std::string name = ""; 62 | std::string sensor_type = ""; 63 | std::string output_topic; // defaults to vehicle_name/sensor_name 64 | std::string frame_name; // defaults to vehicle_name/sensor_name 65 | double rate = 10.0; // Hz 66 | bool force_separate_timer = 67 | false; // By default all sensors of identical rate are synced into 1 68 | // timer, but that can slow down overall performance for a 69 | // specific sensor 70 | Eigen::Vector3d translation; // T_B_S, default is unit transform 71 | Eigen::Quaterniond rotation; 72 | }; 73 | struct Camera : Sensor { 74 | std::string image_type_str = "Scene"; 75 | bool pixels_as_float = false; 76 | msr::airlib::ImageCaptureBase::ImageType image_type = 77 | msr::airlib::ImageCaptureBase::ImageType::Scene; 78 | msr::airlib::CameraInfo camera_info; // The info is read from UE4 79 | }; 80 | std::vector> sensors; 81 | }; 82 | 83 | AirsimSimulator(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private); 84 | virtual ~AirsimSimulator() = default; 85 | 86 | // ROS callbacks 87 | void simStateCallback(const ros::TimerEvent&); 88 | void startupCallback(const ros::TimerEvent&); 89 | void onShutdown(); // called by the sigint handler 90 | 91 | // Control 92 | /** 93 | * NOTE(Schmluk): Airsim also exposes a number of other control interfaces, 94 | * mostly low level. If needed, some of these could also be included here, but 95 | * set pose should do for most purposes. 96 | */ 97 | void commandPoseCallback(const geometry_msgs::Pose& msg); 98 | 99 | // Acessors 100 | const Config& getConfig() const { return config_; } 101 | const FrameConverter& getFrameConverter() const { return frame_converter_; } 102 | ros::Time getTimeStamp(msr::airlib::TTimePoint airsim_stamp); 103 | OdometryDriftSimulator* getOdometryDriftSimulator() { 104 | return &odometry_drift_simulator_; 105 | } 106 | 107 | protected: 108 | // ROS 109 | ros::NodeHandle nh_; 110 | ros::NodeHandle nh_private_; 111 | ros::Timer sim_state_timer_; 112 | ros::Timer startup_timer_; 113 | ros::Publisher odom_pub_; 114 | ros::Publisher pose_pub_; 115 | ros::Publisher collision_pub_; 116 | ros::Publisher sim_is_ready_pub_; 117 | ros::Publisher time_pub_; 118 | ros::Subscriber command_pose_sub_; 119 | tf2_ros::TransformBroadcaster tf_broadcaster_; 120 | tf2_ros::StaticTransformBroadcaster static_tf_broadcaster_; 121 | 122 | // Odometry simulator 123 | OdometryDriftSimulator odometry_drift_simulator_; 124 | 125 | // Read sim time from AirSim 126 | std::thread timer_thread_; 127 | 128 | // components 129 | std::vector> 130 | sensor_timers_; // These manage the actual sensor reading/publishing 131 | std::vector> 132 | processors_; // Various post-processing 133 | 134 | // Airsim clients (These can be blocking and thus slowing down tasks if only 135 | // one is used) 136 | msr::airlib::MultirotorRpcLibClient airsim_state_client_; 137 | msr::airlib::MultirotorRpcLibClient airsim_move_client_; 138 | msr::airlib::MultirotorRpcLibClient airsim_time_client_; 139 | 140 | // tools 141 | Config config_; 142 | FrameConverter frame_converter_; // the world-to-airsim transformation 143 | 144 | // variables 145 | bool is_connected_; // whether the airsim client is connected 146 | bool is_running_; // whether the simulator setup successfully and is working 147 | bool is_shutdown_; // After setting is shutdown no more airsim requests are 148 | // allowed. 149 | bool use_sim_time_; // Publish ros time based on the airsim clock 150 | 151 | // setup methods 152 | bool setupAirsim(); // Connect to Airsim and verify 153 | bool setupROS(); 154 | bool readParamsFromRos(); 155 | bool initializeSimulationFrame(); 156 | bool startSimTimer(); 157 | 158 | // methods 159 | void readSimTimeCallback(); 160 | 161 | // helper methods 162 | bool readTransformFromRos(const std::string& topic, 163 | Eigen::Vector3d* translation, 164 | Eigen::Quaterniond* rotation); 165 | }; 166 | 167 | } // namespace unreal_airsim 168 | 169 | #endif // UNREAL_AIRSIM_ONLINE_SIMULATOR_SIMULATOR_H_ 170 | -------------------------------------------------------------------------------- /include/unreal_airsim/simulator_processing/depth_to_pointcloud.h: -------------------------------------------------------------------------------- 1 | #ifndef UNREAL_AIRSIM_SIMULATOR_PROCESSOR_DEPTH_TO_POINTCLOUD_H_ 2 | #define UNREAL_AIRSIM_SIMULATOR_PROCESSOR_DEPTH_TO_POINTCLOUD_H_ 3 | 4 | #include "unreal_airsim/simulator_processing/processor_base.h" 5 | 6 | // ROS 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | namespace unreal_airsim::simulator_processor { 15 | /*** 16 | * Absorbs a depth and optionally a color image and transforms it into a point 17 | * cloud in camera frame (x left, y down, z - into image plane) 18 | */ 19 | class DepthToPointcloud : public ProcessorBase { 20 | public: 21 | DepthToPointcloud() = default; 22 | virtual ~DepthToPointcloud() = default; 23 | 24 | bool setupFromRos(const ros::NodeHandle& nh, const std::string& ns) override; 25 | 26 | // ROS callbacks 27 | void depthImageCallback(const sensor_msgs::ImagePtr& msg); 28 | void colorImageCallback(const sensor_msgs::ImagePtr& msg); 29 | void segmentationImageCallback(const sensor_msgs::ImagePtr& msg); 30 | 31 | protected: 32 | // setup 33 | static ProcessorFactory::Registration registration_; 34 | 35 | // ROS 36 | ros::NodeHandle nh_; 37 | ros::Publisher pub_; 38 | ros::Subscriber depth_sub_; 39 | ros::Subscriber color_sub_; 40 | ros::Subscriber segmentation_sub_; 41 | 42 | // queues 43 | std::mutex queue_guard; 44 | std::deque depth_queue_; 45 | std::deque color_queue_; 46 | std::deque segmentation_queue_; 47 | 48 | // variables 49 | bool use_color_; 50 | bool use_segmentation_; 51 | bool is_setup_; 52 | float fov_; // depth cam intrinsics, fov in degrees 53 | float focal_length_; 54 | float vx_; 55 | float vy_; 56 | 57 | // params 58 | int max_queue_length_; 59 | float max_depth_; // points beyond this depth [m] will be discarded 60 | float max_ray_length_; // points beyond this ray length [m] will be discarded 61 | bool use_infrared_compensation_; 62 | 63 | // methods 64 | void findMatchingMessagesToPublish( 65 | const sensor_msgs::ImagePtr& reference_msg); 66 | void publishPointcloud(const sensor_msgs::ImagePtr& depth_ptr, 67 | const sensor_msgs::ImagePtr& color_ptr, 68 | const sensor_msgs::ImagePtr& segmentation_ptr); 69 | }; 70 | 71 | } // namespace unreal_airsim::simulator_processor 72 | 73 | #endif // UNREAL_AIRSIM_SIMULATOR_PROCESSOR_DEPTH_TO_POINTCLOUD_H_ 74 | -------------------------------------------------------------------------------- /include/unreal_airsim/simulator_processing/infrared_id_compensation.h: -------------------------------------------------------------------------------- 1 | #ifndef UNREAL_AIRSIM_SIMULATOR_PROCESSING_INFRARED_ID_COMPENSATION_H_ 2 | #define UNREAL_AIRSIM_SIMULATOR_PROCESSING_INFRARED_ID_COMPENSATION_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include "unreal_airsim/simulator_processing/processor_base.h" 11 | 12 | namespace unreal_airsim::simulator_processor { 13 | /*** 14 | * Reads an image of image type 'Infrared' and shifts the values s.t. they match 15 | * the object segmentation ID. This is necessary as apparently for some setups 16 | * that translation fails: https://github.com/microsoft/AirSim/issues/1992 17 | */ 18 | class InfraredIdCompensation : public ProcessorBase { 19 | public: 20 | InfraredIdCompensation() = default; 21 | ~InfraredIdCompensation() override = default; 22 | 23 | bool setupFromRos(const ros::NodeHandle& nh, const std::string& ns) override; 24 | 25 | // ROS callbacks 26 | void imageCallback(const sensor_msgs::ImagePtr& msg); 27 | 28 | protected: 29 | // setup 30 | static ProcessorFactory::Registration registration_; 31 | 32 | // ROS 33 | ros::NodeHandle nh_; 34 | ros::Publisher pub_; 35 | ros::Subscriber sub_; 36 | 37 | // the actual compensation values as measured for the current setup. Values 38 | // that can not be mapped to are 255, so if anything goes wrong it can be 39 | // addressed via this special id label. This table can be read from a file 40 | // created by the 'compute_infrared_corrections.py' tool. 41 | uint8_t infrared_compensation_[256]{ 42 | 0, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 1, 255, 43 | 255, 255, 255, 255, 255, 255, 255, 2, 255, 255, 255, 255, 255, 3, 255, 44 | 255, 255, 255, 255, 4, 255, 255, 255, 5, 255, 255, 255, 6, 255, 255, 45 | 255, 7, 255, 255, 8, 255, 255, 255, 9, 255, 255, 10, 255, 255, 11, 46 | 255, 12, 255, 255, 13, 255, 14, 255, 255, 15, 255, 16, 255, 17, 255, 47 | 18, 255, 19, 255, 20, 255, 21, 255, 22, 255, 23, 24, 255, 25, 255, 48 | 26, 27, 255, 28, 255, 29, 30, 255, 31, 32, 255, 33, 34, 35, 255, 49 | 36, 37, 255, 38, 39, 40, 255, 41, 42, 43, 44, 45, 255, 46, 47, 50 | 48, 49, 50, 51, 255, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 51 | 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 76, 77, 52 | 78, 79, 80, 81, 82, 83, 85, 86, 87, 88, 90, 91, 92, 93, 94, 53 | 96, 97, 98, 100, 101, 103, 104, 105, 107, 108, 109, 111, 112, 113, 115, 54 | 116, 118, 119, 121, 122, 124, 125, 127, 128, 130, 131, 133, 134, 136, 137, 55 | 139, 141, 142, 144, 145, 147, 149, 151, 152, 154, 156, 157, 159, 161, 162, 56 | 164, 166, 168, 169, 171, 173, 175, 177, 178, 180, 182, 184, 186, 188, 190, 57 | 192, 193, 195, 197, 199, 201, 203, 205, 207, 209, 211, 213, 215, 217, 219, 58 | 221, 223, 225, 228, 230, 232, 234, 236, 239, 241, 243, 245, 248, 250, 252, 59 | 255}; 60 | }; 61 | 62 | } // namespace unreal_airsim::simulator_processor 63 | 64 | #endif // UNREAL_AIRSIM_SIMULATOR_PROCESSING_INFRARED_ID_COMPENSATION_H_ 65 | -------------------------------------------------------------------------------- /include/unreal_airsim/simulator_processing/odometry_drift_simulator/normal_distribution.h: -------------------------------------------------------------------------------- 1 | #ifndef UNREAL_AIRSIM_SIMULATOR_PROCESSING_ODOMETRY_DRIFT_SIMULATOR_NORMAL_DISTRIBUTION_H_ 2 | #define UNREAL_AIRSIM_SIMULATOR_PROCESSING_ODOMETRY_DRIFT_SIMULATOR_NORMAL_DISTRIBUTION_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | namespace unreal_airsim { 11 | class NormalDistribution { 12 | public: 13 | struct Config { 14 | // Initialize from ROS params 15 | static Config fromRosParams(const ros::NodeHandle& nh); 16 | 17 | // Distribution parameters 18 | double mean = 0.0; 19 | double stddev = 0.0; 20 | 21 | // Validity queries and assertions 22 | bool isValid(const std::string& error_msg_prefix = "") const; 23 | Config& checkValid() { 24 | CHECK(isValid()); 25 | return *this; 26 | } 27 | 28 | // Write config values to stream, e.g. for logging 29 | friend std::ostream& operator<<(std::ostream& os, const Config& config); 30 | }; 31 | 32 | explicit NormalDistribution(double mean = 0.0, double stddev = 1.0) 33 | : mean_(mean), stddev_(stddev) { 34 | CHECK_GE(stddev_, 0.0) << "Standard deviation must be non-negative"; 35 | } 36 | explicit NormalDistribution(const Config& config) 37 | : NormalDistribution(config.mean, config.stddev) {} 38 | 39 | double getMean() const { return mean_; } 40 | double getStddev() const { return stddev_; } 41 | 42 | // Return a sample from the normal distribution N(mean_, stddev_) 43 | double operator()() { 44 | CHECK_GE(stddev_, 0) << "The standard deviation must be non-negative."; 45 | // Random engine 46 | // TODO(victorr): Add proper random seed handling (and option to provide it) 47 | // NOTE: The noise generator is static to ensure that all instances draw 48 | // subsequent (different) numbers from the same pseudo random 49 | // sequence. If the generator is instance specific, there's a risk 50 | // that multiple instances use generators with the same seed and 51 | // output the same sequence. 52 | static std::mt19937 noise_generator_; 53 | 54 | // Draw a sample from the standard normal N(0,1) and 55 | // scale it using the change of variables formula 56 | return normal_distribution_(noise_generator_) * stddev_ + mean_; 57 | } 58 | 59 | private: 60 | const double mean_, stddev_; 61 | 62 | // Standard normal distribution 63 | std::normal_distribution normal_distribution_; 64 | }; 65 | } // namespace unreal_airsim 66 | 67 | #endif // UNREAL_AIRSIM_SIMULATOR_PROCESSING_ODOMETRY_DRIFT_SIMULATOR_NORMAL_DISTRIBUTION_H_ 68 | -------------------------------------------------------------------------------- /include/unreal_airsim/simulator_processing/odometry_drift_simulator/odometry_drift_simulator.h: -------------------------------------------------------------------------------- 1 | #ifndef UNREAL_AIRSIM_SIMULATOR_PROCESSING_ODOMETRY_DRIFT_SIMULATOR_ODOMETRY_DRIFT_SIMULATOR_H_ 2 | #define UNREAL_AIRSIM_SIMULATOR_PROCESSING_ODOMETRY_DRIFT_SIMULATOR_ODOMETRY_DRIFT_SIMULATOR_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include "unreal_airsim/simulator_processing/odometry_drift_simulator/normal_distribution.h" 15 | 16 | namespace unreal_airsim { 17 | class OdometryDriftSimulator { 18 | public: 19 | using NoiseDistribution = NormalDistribution; 20 | using Transformation = kindr::minimal::QuatTransformationTemplate; 21 | 22 | struct Config { 23 | // Initialize from ROS params 24 | static Config fromRosParams(const ros::NodeHandle& nh); 25 | 26 | // Whether and how to publish the ground truth pose 27 | bool publish_ground_truth_pose = false; 28 | std::string ground_truth_frame_suffix = "_ground_truth"; 29 | 30 | // Params of the distributions used to generate the pose drift and noise 31 | float velocity_noise_frequency_hz = 1; 32 | using NoiseConfigMap = std::map; 33 | NoiseConfigMap velocity_noise = { 34 | {"x", {}}, {"y", {}}, {"z", {}}, {"yaw", {}}}; 35 | NoiseConfigMap pose_noise = {{"x", {}}, {"y", {}}, {"z", {}}, 36 | {"yaw", {}}, {"pitch", {}}, {"roll", {}}}; 37 | 38 | // Validity queries and assertions 39 | bool isValid() const; 40 | Config& checkValid() { 41 | CHECK(isValid()); 42 | return *this; 43 | } 44 | 45 | // Write config values to stream, e.g. for logging 46 | friend std::ostream& operator<<(std::ostream& os, const Config& config); 47 | }; 48 | 49 | explicit OdometryDriftSimulator(Config config); 50 | ~OdometryDriftSimulator() = default; 51 | 52 | void start() { 53 | reset(); 54 | started_publishing_ = true; 55 | } 56 | void reset(); 57 | void tick(const geometry_msgs::TransformStamped& ground_truth_pose_msg); 58 | 59 | Transformation getSimulatedPose() const { return current_simulated_pose_; } 60 | Transformation getGroundTruthPose() const; 61 | geometry_msgs::TransformStamped getSimulatedPoseMsg() const; 62 | geometry_msgs::TransformStamped getGroundTruthPoseMsg() const { 63 | return last_ground_truth_pose_msg_; 64 | } 65 | 66 | Transformation convertDriftedToGroundTruthPose( 67 | const Transformation& simulated_pose) const; 68 | Transformation convertGroundTruthToDriftedPose( 69 | const Transformation& ground_truth_pose) const; 70 | geometry_msgs::TransformStamped convertDriftedToGroundTruthPoseMsg( 71 | const geometry_msgs::TransformStamped& simulated_pose_msg) const; 72 | geometry_msgs::TransformStamped convertGroundTruthToDriftedPoseMsg( 73 | const geometry_msgs::TransformStamped& ground_truth_pose_msg) const; 74 | 75 | void publishTfs() const; 76 | 77 | private: 78 | // Settings 79 | const Config config_; 80 | bool started_publishing_; 81 | 82 | // Simulator state 83 | ros::Time last_velocity_noise_sampling_time_; 84 | const ros::Duration velocity_noise_sampling_period_; 85 | Transformation::Vector3 current_linear_velocity_noise_sample_W_; 86 | Transformation::Vector3 current_angular_velocity_noise_sample_W_; 87 | Transformation integrated_pose_drift_; 88 | Transformation current_pose_noise_; 89 | Transformation current_simulated_pose_; 90 | geometry_msgs::TransformStamped last_ground_truth_pose_msg_; 91 | 92 | // Noise distributions 93 | struct VelocityNoiseDistributions { 94 | explicit VelocityNoiseDistributions( 95 | const Config::NoiseConfigMap& velocity_noise_configs); 96 | NoiseDistribution x, y, z; 97 | NoiseDistribution yaw; 98 | } velocity_noise_; 99 | struct PoseNoiseDistributions { 100 | explicit PoseNoiseDistributions( 101 | const Config::NoiseConfigMap& pose_noise_configs); 102 | NoiseDistribution x, y, z; 103 | NoiseDistribution yaw, pitch, roll; 104 | } pose_noise_; 105 | 106 | // Transform publishing 107 | mutable tf2_ros::TransformBroadcaster transform_broadcaster_; 108 | void publishSimulatedPoseTf() const; 109 | void publishGroundTruthPoseTf() const; 110 | }; 111 | } // namespace unreal_airsim 112 | 113 | #endif // UNREAL_AIRSIM_SIMULATOR_PROCESSING_ODOMETRY_DRIFT_SIMULATOR_ODOMETRY_DRIFT_SIMULATOR_H_ 114 | -------------------------------------------------------------------------------- /include/unreal_airsim/simulator_processing/processor_base.h: -------------------------------------------------------------------------------- 1 | #ifndef UNREAL_AIRSIM_SIMULATOR_PROCESSING_PROCESSOR_BASE_H_ 2 | #define UNREAL_AIRSIM_SIMULATOR_PROCESSING_PROCESSOR_BASE_H_ 3 | 4 | #include "unreal_airsim/simulator_processing/processor_factory.h" 5 | 6 | #include 7 | 8 | #include 9 | 10 | namespace unreal_airsim { 11 | class AirsimSimulator; 12 | namespace simulator_processor { 13 | /*** 14 | * Base class for all simulator_processors. 15 | */ 16 | class ProcessorBase { 17 | public: 18 | ProcessorBase() = default; 19 | virtual ~ProcessorBase() = default; 20 | 21 | virtual bool setupFromRos( 22 | const ros::NodeHandle& nh, 23 | const std::string& ns) = 0; // params can be retrieved as ns+param_name 24 | 25 | protected: 26 | // these fields are set by the factory 27 | friend ProcessorFactory; 28 | AirsimSimulator* parent_; 29 | std::string name_; 30 | }; 31 | 32 | } // namespace simulator_processor 33 | } // namespace unreal_airsim 34 | 35 | #endif // UNREAL_AIRSIM_SIMULATOR_PROCESSING_PROCESSOR_BASE_H_ 36 | -------------------------------------------------------------------------------- /include/unreal_airsim/simulator_processing/processor_factory.h: -------------------------------------------------------------------------------- 1 | #ifndef UNREAL_AIRSIM_SIMULATOR_PROCESSING_PROCESSOR_FACTORY_H_ 2 | #define UNREAL_AIRSIM_SIMULATOR_PROCESSING_PROCESSOR_FACTORY_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | namespace unreal_airsim { 16 | class AirsimSimulator; 17 | 18 | namespace simulator_processor { 19 | class ProcessorBase; 20 | 21 | /*** 22 | * Static Factory with a singleton registration map. Processors can register 23 | * themselves with a name using the static Registration struct, such that they 24 | * can then be created via the name at runtime. 25 | */ 26 | class ProcessorFactory { 27 | public: 28 | typedef std::function FactoryMethod; 29 | virtual ~ProcessorFactory() = default; 30 | 31 | // Allow processors to register themselves to the factory using a static 32 | // Registration struct 33 | template 34 | struct Registration { 35 | explicit Registration(const std::string& name) { 36 | if (getRegistryMap().find(name) == getRegistryMap().end()) { 37 | getRegistryMap().insert(std::pair( 38 | name, []() { return new ProcessorType(); })); 39 | } else { 40 | LOG(FATAL) 41 | << "Cannot register already existing simulator_processor name '" 42 | << name << "'!"; 43 | } 44 | } 45 | }; 46 | 47 | static std::unique_ptr createFromRos(const std::string& name, 48 | const std::string& type, 49 | const ros::NodeHandle& nh, 50 | const std::string& ns, 51 | AirsimSimulator* parent); 52 | 53 | private: 54 | ProcessorFactory() = default; 55 | typedef std::unordered_map TypeMap; 56 | static TypeMap& getRegistryMap(); 57 | }; 58 | 59 | } // namespace simulator_processor 60 | 61 | } // namespace unreal_airsim 62 | 63 | #endif // UNREAL_AIRSIM_SIMULATOR_PROCESSING_PROCESSOR_FACTORY_H_ 64 | -------------------------------------------------------------------------------- /launch/airsim_simulator.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /launch/demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /launch/parse_config_to_airsim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | unreal_airsim 4 | 0.0.0 5 | Utilities and ROS-interfaces to UE4 via Airsim. 6 | 7 | Lukas Schmid 8 | Lukas Schmid 9 | 10 | BSD 11 | 12 | catkin 13 | catkin_simple 14 | 15 | message_generation 16 | message_runtime 17 | rospy 18 | roscpp 19 | eigen_catkin 20 | glog_catkin 21 | gflags_catkin 22 | minkindr_conversions 23 | std_msgs 24 | sensor_msgs 25 | geometry_msgs 26 | rosgraph_msgs 27 | tf2_ros 28 | cv_bridge 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /src/frame_converter.cpp: -------------------------------------------------------------------------------- 1 | #include "unreal_airsim/frame_converter.h" 2 | 3 | #include 4 | #include 5 | 6 | namespace unreal_airsim { 7 | 8 | FrameConverter::FrameConverter() { reset(); } 9 | 10 | void FrameConverter::reset() { 11 | rotation_ = Eigen::Matrix3d::Identity(); 12 | rot_inverse_ = Eigen::Matrix3d::Identity(); 13 | } 14 | 15 | void FrameConverter::setupFromYaw(double yaw) { 16 | double yaw_offset = std::fmod(yaw, 2.0 * M_PI); 17 | rotation_ = Eigen::AngleAxisd(yaw_offset, Eigen::Vector3d::UnitZ()); 18 | rot_inverse_ = rotation_.inverse(); 19 | } 20 | 21 | void FrameConverter::setupFromQuat(const Eigen::Quaterniond& quat) { 22 | rotation_ = quat; 23 | rot_inverse_ = rotation_.inverse(); 24 | } 25 | 26 | void FrameConverter::transformPointAirsimToRos(double* x, double* y, 27 | double* z) const { 28 | // axis orientations 29 | Eigen::Vector3d p(*x, -*y, -*z); // coordinate axis inversions 30 | p = rotation_ * p; // rotate 31 | *x = p[0]; 32 | *y = p[1]; 33 | *z = p[2]; 34 | } 35 | 36 | void FrameConverter::transformOrientationAirsimToRos(double* w, double* x, 37 | double* y, 38 | double* z) const { 39 | Eigen::Quaterniond q(*w, *x, -*y, 40 | -*z); // this defines a mirroring of q on the YZ-plane 41 | if (std::fabs(q.norm() - 1.0) > 1e-3) { 42 | LOG(WARNING) << "Received non-normalized quaternion (norm is " << q.norm() 43 | << ")."; 44 | q.normalize(); 45 | } 46 | q = rotation_ * q; // rotate 47 | *x = q.x(); 48 | *y = q.y(); 49 | *z = q.z(); 50 | *w = q.w(); 51 | } 52 | 53 | void FrameConverter::transformPointRosToAirsim(double* x, double* y, 54 | double* z) const { 55 | // axis orientations 56 | Eigen::Vector3d p(*x, *y, *z); 57 | p = rot_inverse_ * p; // rotate 58 | *x = p[0]; // coordinate axis inversions 59 | *y = -p[1]; 60 | *z = -p[2]; 61 | } 62 | 63 | void FrameConverter::transformOrientationRosToAirsim(double* w, double* x, 64 | double* y, 65 | double* z) const { 66 | Eigen::Quaterniond q(*w, *x, *y, *z); 67 | if (std::fabs(q.norm() - 1.0) > 1e-3) { 68 | LOG(WARNING) << "Received non-normalized quaternion (norm is " << q.norm() 69 | << ")."; 70 | q.normalize(); 71 | } 72 | q = rot_inverse_ * q; // rotate 73 | *x = q.x(); // this defines a mirroring of q on the YZ-plane 74 | *y = -q.y(); 75 | *z = -q.z(); 76 | *w = q.w(); 77 | } 78 | 79 | // Interfaces 80 | 81 | void FrameConverter::airsimToRos(geometry_msgs::Vector3* point) const { 82 | transformPointAirsimToRos(&(point->x), &(point->y), &(point->z)); 83 | } 84 | 85 | void FrameConverter::airsimToRos(geometry_msgs::Point* point) const { 86 | transformPointAirsimToRos(&(point->x), &(point->y), &(point->z)); 87 | } 88 | 89 | void FrameConverter::airsimToRos(Eigen::Vector3d* point) const { 90 | transformPointAirsimToRos(&(point->x()), &(point->y()), &(point->z())); 91 | } 92 | 93 | void FrameConverter::airsimToRos(geometry_msgs::Quaternion* orientation) const { 94 | transformOrientationAirsimToRos(&(orientation->w), &(orientation->x), 95 | &(orientation->y), &(orientation->z)); 96 | } 97 | 98 | void FrameConverter::airsimToRos(Eigen::Quaterniond* orientation) const { 99 | transformOrientationAirsimToRos(&(orientation->w()), &(orientation->x()), 100 | &(orientation->y()), &(orientation->z())); 101 | } 102 | 103 | void FrameConverter::airsimToRos(geometry_msgs::Pose* pose) const { 104 | airsimToRos(&(pose->position)); 105 | airsimToRos(&(pose->orientation)); 106 | } 107 | 108 | void FrameConverter::airsimToRos(geometry_msgs::Transform* pose) const { 109 | airsimToRos(&(pose->translation)); 110 | airsimToRos(&(pose->rotation)); 111 | } 112 | 113 | void FrameConverter::rosToAirsim(Eigen::Vector3d* point) const { 114 | transformPointRosToAirsim(&(point->x()), &(point->y()), &(point->z())); 115 | } 116 | 117 | void FrameConverter::rosToAirsim(geometry_msgs::Point* point) const { 118 | transformPointRosToAirsim(&(point->x), &(point->y), &(point->z)); 119 | } 120 | 121 | void FrameConverter::rosToAirsim(geometry_msgs::Quaternion* orientation) const { 122 | transformOrientationRosToAirsim(&(orientation->w), &(orientation->x), 123 | &(orientation->y), &(orientation->z)); 124 | } 125 | 126 | void FrameConverter::rosToAirsim(Eigen::Quaterniond* orientation) const { 127 | transformOrientationRosToAirsim(&(orientation->w()), &(orientation->x()), 128 | &(orientation->y()), &(orientation->z())); 129 | } 130 | 131 | void FrameConverter::rosToAirsim(geometry_msgs::Pose* pose) const { 132 | rosToAirsim(&(pose->position)); 133 | rosToAirsim(&(pose->orientation)); 134 | } 135 | 136 | } // namespace unreal_airsim 137 | -------------------------------------------------------------------------------- /src/online_simulator/sensor_timer.cpp: -------------------------------------------------------------------------------- 1 | #include "unreal_airsim/online_simulator/sensor_timer.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include "unreal_airsim/online_simulator/simulator.h" 15 | 16 | namespace unreal_airsim { 17 | 18 | SensorTimer::SensorTimer(const ros::NodeHandle& nh, double rate, 19 | bool is_private, const std::string& vehicle_name, 20 | AirsimSimulator* parent) 21 | : nh_(nh), 22 | is_private_(is_private), 23 | rate_(rate), 24 | vehicle_name_(vehicle_name), 25 | is_shutdown_(false), 26 | parent_(parent) { 27 | timer_ = nh_.createTimer(ros::Duration(1.0 / rate), 28 | &SensorTimer::timerCallback, this); 29 | if (parent_->getConfig().publish_sensor_transforms) { 30 | transform_pub_ = nh_.advertise( 31 | parent_->getConfig().vehicle_name + "sensor_ground_truth_transforms", 32 | 100); 33 | } 34 | } 35 | 36 | double SensorTimer::getRate() const { return rate_; } 37 | bool SensorTimer::isPrivate() const { return is_private_; } 38 | 39 | void SensorTimer::signalShutdown() { is_shutdown_ = true; } 40 | 41 | void SensorTimer::timerCallback(const ros::TimerEvent&) { 42 | processCameras(); 43 | processLidars(); 44 | processImus(); 45 | } 46 | 47 | void SensorTimer::addSensor(const AirsimSimulator& simulator, 48 | int sensor_index) { 49 | AirsimSimulator::Config::Sensor* sensor = 50 | simulator.getConfig().sensors[sensor_index].get(); 51 | if (sensor->sensor_type == AirsimSimulator::Config::Sensor::TYPE_CAMERA) { 52 | auto camera = (AirsimSimulator::Config::Camera*)sensor; 53 | camera_pubs_.push_back( 54 | nh_.advertise(camera->output_topic, 5)); 55 | camera_frame_names_.push_back(camera->frame_name); 56 | msr::airlib::ImageCaptureBase::ImageRequest request; 57 | request.camera_name = camera->name; 58 | request.compress = false; 59 | request.image_type = camera->image_type; 60 | request.pixels_as_float = camera->pixels_as_float; 61 | image_requests_.push_back(request); 62 | } else if (sensor->sensor_type == 63 | AirsimSimulator::Config::Sensor::TYPE_LIDAR) { 64 | lidar_pubs_.push_back( 65 | nh_.advertise(sensor->output_topic, 5)); 66 | lidar_names_.push_back(sensor->name); 67 | lidar_frame_names_.push_back(sensor->frame_name); 68 | } else if (sensor->sensor_type == AirsimSimulator::Config::Sensor::TYPE_IMU) { 69 | imu_pubs_.push_back( 70 | nh_.advertise(sensor->output_topic, 5)); 71 | imu_names_.push_back(sensor->name); 72 | imu_frame_names_.push_back(sensor->frame_name); 73 | } 74 | } 75 | 76 | void SensorTimer::processCameras() { 77 | if (is_shutdown_) { 78 | return; 79 | } 80 | if (!image_requests_.empty()) { 81 | // get images from unreal. 82 | std::vector responses = 83 | airsim_client_.simGetImages(image_requests_, vehicle_name_); 84 | ros::Time timestamp = parent_->getTimeStamp( 85 | responses[0].time_stamp); // these are synchronized 86 | 87 | // process responses 88 | for (size_t i = 0; i < responses.size(); ++i) { 89 | if (camera_pubs_[i].getNumSubscribers() > 0) { 90 | sensor_msgs::ImagePtr msg(new sensor_msgs::Image); 91 | if (responses[i].pixels_as_float) { 92 | // Encode float images 93 | cv::Mat cv_image = 94 | cv::Mat(responses[i].height, responses[i].width, CV_32FC1); 95 | memcpy(cv_image.data, responses[i].image_data_float.data(), 96 | responses[i].image_data_float.size() * sizeof(float)); 97 | cv_bridge::CvImage(std_msgs::Header(), "32FC1", cv_image) 98 | .toImageMsg(*msg); 99 | } else { 100 | msg->height = responses[i].height; 101 | msg->width = responses[i].width; 102 | msg->is_bigendian = 0; 103 | if (responses[i].image_type == 104 | msr::airlib::ImageCaptureBase::ImageType::Infrared) { 105 | // IR images are published as 1C mono images. 106 | msg->step = responses[i].width; 107 | msg->encoding = "mono8"; 108 | msg->data.resize(responses[i].image_data_uint8.size() / 3); 109 | for (size_t j = 0; j < msg->data.size(); ++j) { 110 | msg->data[j] = responses[i].image_data_uint8[j * 3]; 111 | } 112 | } else { 113 | // all others are 3C RGB images. 114 | msg->step = responses[i].width * 3; 115 | msg->encoding = "bgr8"; 116 | msg->data = responses[i].image_data_uint8; 117 | } 118 | } 119 | msg->header.stamp = timestamp; 120 | msg->header.frame_id = camera_frame_names_[i]; 121 | 122 | // Ground truth transforms. 123 | if (parent_->getConfig().publish_sensor_transforms) { 124 | auto rotation = 125 | Eigen::Quaterniond(responses[i].camera_orientation.w(), 126 | responses[i].camera_orientation.x(), 127 | responses[i].camera_orientation.y(), 128 | responses[i].camera_orientation.z()); 129 | parent_->getFrameConverter().airsimToRos(&(rotation)); 130 | // Camera frames are x right, y down, z depth 131 | rotation = rotation * Eigen::Quaterniond(0.5, -0.5, 0.5, -0.5); 132 | geometry_msgs::TransformStamped transformStamped; 133 | transformStamped.header.stamp = timestamp; 134 | transformStamped.header.frame_id = 135 | parent_->getConfig().simulator_frame_name; 136 | transformStamped.transform.translation.x = 137 | responses[i].camera_position[0]; 138 | transformStamped.transform.translation.y = 139 | responses[i].camera_position[1]; 140 | transformStamped.transform.translation.z = 141 | responses[i].camera_position[2]; 142 | transformStamped.transform.rotation.x = rotation.x(); 143 | transformStamped.transform.rotation.y = rotation.y(); 144 | transformStamped.transform.rotation.z = rotation.z(); 145 | transformStamped.transform.rotation.w = rotation.w(); 146 | parent_->getFrameConverter().airsimToRos( 147 | &(transformStamped.transform.translation)); 148 | // Publish the ground truth transform. 149 | transformStamped.child_frame_id = 150 | camera_frame_names_[i] + "_ground_truth"; 151 | tf_broadcaster_.sendTransform(transformStamped); 152 | transform_pub_.publish(transformStamped); 153 | 154 | // Publish the robot transform, use both for naming consistency. 155 | transformStamped = 156 | parent_->getOdometryDriftSimulator() 157 | ->convertGroundTruthToDriftedPoseMsg(transformStamped); 158 | transformStamped.child_frame_id = camera_frame_names_[i]; 159 | tf_broadcaster_.sendTransform(transformStamped); 160 | transform_pub_.publish(transformStamped); 161 | } 162 | 163 | camera_pubs_[i].publish(msg); 164 | } 165 | } 166 | } 167 | } 168 | 169 | void SensorTimer::processLidars() { 170 | if (is_shutdown_) { 171 | return; 172 | } 173 | for (size_t i = 0; i < lidar_names_.size(); ++i) { 174 | msr::airlib::LidarData lidar_data = 175 | airsim_client_.getLidarData(lidar_names_[i], vehicle_name_); 176 | sensor_msgs::PointCloud2Ptr msg(new sensor_msgs::PointCloud2); 177 | msg->header.frame_id = lidar_frame_names_[i]; 178 | msg->header.stamp = parent_->getTimeStamp(lidar_data.time_stamp); 179 | msg->height = 1; 180 | msg->width = lidar_data.point_cloud.size() / 3; 181 | msg->fields.resize(3); 182 | msg->fields[0].name = "x"; 183 | msg->fields[1].name = "y"; 184 | msg->fields[2].name = "z"; 185 | int offset = 0; 186 | for (size_t d = 0; d < msg->fields.size(); ++d, offset += 4) { 187 | msg->fields[d].offset = offset; 188 | msg->fields[d].datatype = sensor_msgs::PointField::FLOAT32; 189 | msg->fields[d].count = 1; 190 | } 191 | msg->point_step = offset; 192 | msg->is_bigendian = false; 193 | msg->row_step = msg->point_step * msg->width; 194 | msg->is_dense = false; 195 | std::vector data_std = lidar_data.point_cloud; 196 | for (size_t j = 0; j < data_std.size(); j += 3) { 197 | // points are in sensor-Frame but with airsim axis 198 | data_std[j + 1] *= -1.0; 199 | data_std[j + 2] *= -1.0; 200 | } 201 | auto bytes = reinterpret_cast(&data_std[0]); 202 | vector lidar_msg_data( 203 | bytes, bytes + sizeof(float) * data_std.size()); 204 | msg->data = std::move(lidar_msg_data); 205 | 206 | // Ground truth transform 207 | if (parent_->getConfig().publish_sensor_transforms) { 208 | geometry_msgs::TransformStamped transformStamped; 209 | transformStamped.header.stamp = msg->header.stamp; 210 | transformStamped.header.frame_id = 211 | parent_->getConfig().simulator_frame_name; 212 | transformStamped.child_frame_id = lidar_frame_names_[i] + "_ground_truth"; 213 | transformStamped.transform.translation.x = lidar_data.pose.position[0]; 214 | transformStamped.transform.translation.y = lidar_data.pose.position[1]; 215 | transformStamped.transform.translation.z = lidar_data.pose.position[2]; 216 | transformStamped.transform.rotation.x = lidar_data.pose.orientation.x(); 217 | transformStamped.transform.rotation.y = lidar_data.pose.orientation.y(); 218 | transformStamped.transform.rotation.z = lidar_data.pose.orientation.z(); 219 | transformStamped.transform.rotation.w = lidar_data.pose.orientation.w(); 220 | parent_->getFrameConverter().airsimToRos(&(transformStamped.transform)); 221 | tf_broadcaster_.sendTransform(transformStamped); 222 | transform_pub_.publish(transformStamped); 223 | 224 | // Robot frame transform. 225 | transformStamped = 226 | parent_->getOdometryDriftSimulator() 227 | ->convertGroundTruthToDriftedPoseMsg(transformStamped); 228 | transformStamped.child_frame_id = lidar_frame_names_[i]; 229 | tf_broadcaster_.sendTransform(transformStamped); 230 | transform_pub_.publish(transformStamped); 231 | } 232 | lidar_pubs_[i].publish(msg); 233 | } 234 | } 235 | 236 | void SensorTimer::processImus() { 237 | if (is_shutdown_) { 238 | return; 239 | } 240 | for (size_t i = 0; i < imu_names_.size(); ++i) { 241 | msr::airlib::ImuBase::Output imu_data = 242 | airsim_client_.getImuData(imu_names_[i], vehicle_name_); 243 | 244 | sensor_msgs::ImuPtr msg(new sensor_msgs::Imu); 245 | // orientation 246 | msg->header.frame_id = imu_frame_names_[i]; 247 | msg->header.stamp = parent_->getTimeStamp(imu_data.time_stamp); 248 | msg->orientation.x = imu_data.orientation.x(); 249 | msg->orientation.y = imu_data.orientation.y(); 250 | msg->orientation.z = imu_data.orientation.z(); 251 | msg->orientation.w = imu_data.orientation.w(); 252 | parent_->getFrameConverter().airsimToRos( 253 | &(msg->orientation)); // transform to simulation frame 254 | // Rates (these should also be in sensor-frame but with airsim axis, 255 | // TODO(schmluk): Test this 256 | msg->angular_velocity.x = imu_data.angular_velocity.x(); 257 | msg->angular_velocity.y = -imu_data.angular_velocity.y(); 258 | msg->angular_velocity.z = -imu_data.angular_velocity.z(); 259 | msg->linear_acceleration.x = imu_data.linear_acceleration.x(); 260 | msg->linear_acceleration.y = -imu_data.linear_acceleration.y(); 261 | msg->linear_acceleration.z = -imu_data.linear_acceleration.z(); 262 | 263 | // TODO(schmluk): covariances? 264 | // imu_msg.orientation_covariance = ; 265 | // imu_msg.angular_velocity_covariance = ; 266 | // imu_msg.linear_acceleration_covariance = ; 267 | 268 | imu_pubs_[i].publish(msg); 269 | } 270 | } 271 | 272 | } // namespace unreal_airsim 273 | -------------------------------------------------------------------------------- /src/online_simulator/simulator.cpp: -------------------------------------------------------------------------------- 1 | #include "unreal_airsim/online_simulator/simulator.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include "unreal_airsim/simulator_processing/processor_factory.h" 8 | 9 | STRICT_MODE_OFF 10 | #ifndef RPCLIB_MSGPACK 11 | #define RPCLIB_MSGPACK clmdep_msgpack 12 | #endif // !RPCLIB_MSGPACK 13 | #include "rpc/rpc_error.h" 14 | STRICT_MODE_ON 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | namespace unreal_airsim { 33 | 34 | AirsimSimulator::AirsimSimulator(const ros::NodeHandle& nh, 35 | const ros::NodeHandle& nh_private) 36 | : nh_(nh), 37 | nh_private_(nh_private), 38 | is_connected_(false), 39 | is_running_(false), 40 | is_shutdown_(false), 41 | odometry_drift_simulator_( 42 | OdometryDriftSimulator::Config::fromRosParams(nh_private)) { 43 | // configure 44 | readParamsFromRos(); 45 | 46 | // airsim 47 | bool success = setupAirsim(); 48 | if (success) { 49 | LOG(INFO) << "Connected to the Airsim Server."; 50 | is_connected_ = true; 51 | } else { 52 | std::raise(SIGINT); 53 | return; 54 | } 55 | 56 | // setup everything 57 | initializeSimulationFrame(); 58 | setupROS(); 59 | startSimTimer(); 60 | 61 | // Startup the vehicle simulation via callback 62 | startup_timer_ = nh_private_.createTimer( 63 | ros::Duration(0.1), &AirsimSimulator::startupCallback, this); 64 | } 65 | 66 | bool AirsimSimulator::readParamsFromRos() { 67 | AirsimSimulator::Config defaults; 68 | 69 | // params 70 | nh_.param("/use_sim_time", use_sim_time_, false); 71 | nh_private_.param("state_refresh_rate", config_.state_refresh_rate, 72 | defaults.state_refresh_rate); 73 | nh_private_.param("time_publisher_interval", config_.time_publisher_interval, 74 | defaults.time_publisher_interval); 75 | nh_private_.param("simulator_frame_name", config_.simulator_frame_name, 76 | defaults.simulator_frame_name); 77 | nh_private_.param("vehicle_name", config_.vehicle_name, 78 | defaults.vehicle_name); 79 | nh_private_.param("velocity", config_.velocity, defaults.velocity); 80 | nh_private_.param("publish_sensor_transforms", 81 | config_.publish_sensor_transforms, 82 | defaults.publish_sensor_transforms); 83 | 84 | // Verify params valid 85 | if (config_.state_refresh_rate <= 0.0) { 86 | config_.state_refresh_rate = defaults.state_refresh_rate; 87 | LOG(WARNING) << "Param 'state_refresh_rate' expected > 0.0, set to '" 88 | << defaults.state_refresh_rate << "' (default)."; 89 | } 90 | if (config_.time_publisher_interval < 0) { 91 | config_.time_publisher_interval = defaults.time_publisher_interval; 92 | LOG(WARNING) << "Param 'time_publisher_interval' expected >= 0, set to '" 93 | << defaults.time_publisher_interval << "' (default)."; 94 | } 95 | if (config_.velocity <= 0.0) { 96 | config_.velocity = defaults.velocity; 97 | LOG(WARNING) << "Param 'velocity' expected > 0.0, set to '" 98 | << defaults.velocity << "' (default)."; 99 | } 100 | 101 | // setup sensors 102 | std::vector keys; 103 | nh_private_.getParamNames(keys); 104 | std::string sensor_ns = "sensors/"; 105 | std::string full_ns = nh_private_.getNamespace() + "/" + sensor_ns; 106 | std::string sensor_name; 107 | std::vector sensors; 108 | size_t pos; 109 | for (auto const& key : keys) { 110 | if ((pos = key.find(full_ns)) != std::string::npos) { 111 | sensor_name = key; 112 | sensor_name.erase(0, pos + full_ns.length()); 113 | pos = sensor_name.find('/'); 114 | if (pos != std::string::npos) { 115 | sensor_name = sensor_name.substr(0, pos); 116 | } 117 | if (std::find(sensors.begin(), sensors.end(), sensor_name) == 118 | sensors.end()) { 119 | sensors.push_back(sensor_name); 120 | } 121 | } 122 | } 123 | for (auto const& name : sensors) { 124 | // currently pass all settings via params, maybe could add some smart 125 | // identification here 126 | if (!nh_private_.hasParam(sensor_ns + name + "/sensor_type")) { 127 | LOG(WARNING) << "Sensor '" << name 128 | << "' has no sensor_type and will be ignored!"; 129 | continue; 130 | } 131 | Config::Sensor* sensor_cfg; 132 | std::string sensor_type; 133 | nh_private_.getParam(sensor_ns + name + "/sensor_type", sensor_type); 134 | 135 | if (sensor_type == Config::Sensor::TYPE_CAMERA) { 136 | auto* cfg = new Config::Camera(); 137 | Config::Camera cam_defaults; 138 | nh_private_.param(sensor_ns + name + "/pixels_as_float", 139 | cfg->pixels_as_float, cfg->pixels_as_float); 140 | // cam types default to visual (Scene) camera, but make sure if something 141 | // else is intended a warning is thrown 142 | std::string read_img_type_default = "Param is not string"; 143 | if (!nh_private_.hasParam(sensor_ns + name + "/image_type")) { 144 | read_img_type_default = cam_defaults.image_type_str; 145 | } 146 | nh_private_.param(sensor_ns + name + "/image_type", cfg->image_type_str, 147 | read_img_type_default); 148 | if (cfg->image_type_str == "Scene") { 149 | cfg->image_type = msr::airlib::ImageCaptureBase::ImageType::Scene; 150 | } else if (cfg->image_type_str == "DepthPerspective") { 151 | cfg->image_type = 152 | msr::airlib::ImageCaptureBase::ImageType::DepthPerspective; 153 | } else if (cfg->image_type_str == "DepthPlanar") { 154 | cfg->image_type = msr::airlib::ImageCaptureBase::ImageType::DepthPlanar; 155 | } else if (cfg->image_type_str == "DepthVis") { 156 | cfg->image_type = msr::airlib::ImageCaptureBase::ImageType::DepthVis; 157 | } else if (cfg->image_type_str == "DisparityNormalized") { 158 | cfg->image_type = 159 | msr::airlib::ImageCaptureBase::ImageType::DisparityNormalized; 160 | } else if (cfg->image_type_str == "SurfaceNormals") { 161 | cfg->image_type = 162 | msr::airlib::ImageCaptureBase::ImageType::SurfaceNormals; 163 | } else if (cfg->image_type_str == "Segmentation") { 164 | cfg->image_type = 165 | msr::airlib::ImageCaptureBase::ImageType::Segmentation; 166 | } else if (cfg->image_type_str == "Infrared") { 167 | cfg->image_type = msr::airlib::ImageCaptureBase::ImageType::Infrared; 168 | } else { 169 | LOG(WARNING) << "Unrecognized ImageType '" << cfg->image_type_str 170 | << "' for camera '" << name << "', set to '" 171 | << cam_defaults.image_type_str << "' (default)."; 172 | cfg->image_type = cam_defaults.image_type; 173 | cfg->image_type_str = cam_defaults.image_type_str; 174 | } 175 | cfg->camera_info = airsim_state_client_.simGetCameraInfo(name); 176 | sensor_cfg = (Config::Sensor*)cfg; 177 | } else if (sensor_type == Config::Sensor::TYPE_LIDAR) { 178 | sensor_cfg = new Config::Sensor(); 179 | } else if (sensor_type == Config::Sensor::TYPE_IMU) { 180 | sensor_cfg = new Config::Sensor(); 181 | } else { 182 | LOG(WARNING) << "Unknown sensor_type '" << sensor_type << "' for sensor '" 183 | << name << "', sensor will be ignored!"; 184 | continue; 185 | } 186 | 187 | // general settings 188 | sensor_cfg->name = name; 189 | sensor_cfg->sensor_type = sensor_type; 190 | nh_private_.param(sensor_ns + name + "/output_topic", 191 | sensor_cfg->output_topic, 192 | config_.vehicle_name + "/" + name); 193 | nh_private_.param(sensor_ns + name + "/frame_name", sensor_cfg->frame_name, 194 | config_.vehicle_name + "/" + name); 195 | nh_private_.param(sensor_ns + name + "/force_separate_timer", 196 | sensor_cfg->force_separate_timer, 197 | sensor_cfg->force_separate_timer); 198 | double rate; 199 | nh_private_.param(sensor_ns + name + "/rate", rate, sensor_cfg->rate); 200 | if (rate <= 0) { 201 | LOG(WARNING) << "Param 'rate' for sensor '" << name 202 | << "' expected > 0.0, set to '" << sensor_cfg->rate 203 | << "' (default)."; 204 | } else { 205 | sensor_cfg->rate = rate; 206 | } 207 | readTransformFromRos(sensor_ns + name + "/T_B_S", 208 | &(sensor_cfg->translation), &(sensor_cfg->rotation)); 209 | config_.sensors.push_back(std::unique_ptr(sensor_cfg)); 210 | } 211 | return true; 212 | } 213 | 214 | bool AirsimSimulator::setupAirsim() { 215 | // This is implemented explicitly to avoid Airsim printing and make it clearer 216 | // for us what is going wrong 217 | int timeout = 0; 218 | while (airsim_state_client_.getConnectionState() != 219 | RpcLibClientBase::ConnectionState::Connected && 220 | ros::ok()) { 221 | std::this_thread::sleep_for(std::chrono::milliseconds(300)); 222 | timeout++; 223 | if (timeout > 10) { 224 | // connection state will remain RpcLibClientBase::ConnectionState::Initial 225 | // if the unreal game was not running when creating the client (in the 226 | // constructor) 227 | LOG(FATAL) 228 | << "Unable to connect to the Airsim Server (timeout after 5s). " 229 | "Is a UE4 game with enabled Airsim plugin running?"; 230 | return false; 231 | } 232 | } 233 | 234 | // check versions 235 | bool versions_matching = 236 | true; // Check both in one run to spare running into the issue twice 237 | int server_ver = 0; 238 | try { 239 | server_ver = airsim_state_client_.getServerVersion(); 240 | } catch (rpc::rpc_error& e) { 241 | LOG(FATAL) << "Could not get server version from AirSim Plugin: " 242 | << e.get_error().as(); 243 | return false; 244 | } 245 | int client_ver = airsim_state_client_.getClientVersion(); 246 | int server_min_ver = airsim_state_client_.getMinRequiredServerVersion(); 247 | int client_min_ver = airsim_state_client_.getMinRequiredClientVersion(); 248 | if (client_ver < client_min_ver) { 249 | LOG(FATAL) << "Airsim Client version is too old (is: " << client_ver 250 | << ", min: " << client_min_ver 251 | << "). Update and rebuild the Airsim library."; 252 | versions_matching = false; 253 | } 254 | if (server_ver < server_min_ver) { 255 | LOG(FATAL) << "Airsim Server version is too old (is: " << server_ver 256 | << ", min: " << server_min_ver 257 | << "). Update and rebuild the Airsim UE4 Plugin."; 258 | versions_matching = false; 259 | } 260 | return versions_matching; 261 | } 262 | 263 | bool AirsimSimulator::setupROS() { 264 | // General 265 | sim_state_timer_ = 266 | nh_.createTimer(ros::Duration(1.0 / config_.state_refresh_rate), 267 | &AirsimSimulator::simStateCallback, this); 268 | odom_pub_ = nh_.advertise( 269 | config_.vehicle_name + "/ground_truth/odometry", 5); 270 | pose_pub_ = nh_.advertise( 271 | config_.vehicle_name + "/ground_truth/pose", 5); 272 | collision_pub_ = 273 | nh_.advertise(config_.vehicle_name + "/collision", 1); 274 | sim_is_ready_pub_ = nh_.advertise("simulation_is_ready", 1); 275 | if (use_sim_time_) { 276 | time_pub_ = nh_.advertise("/clock", 50); 277 | } 278 | 279 | // control interfaces 280 | command_pose_sub_ = 281 | nh_.subscribe(config_.vehicle_name + "/command/pose", 10, 282 | &AirsimSimulator::commandPoseCallback, this); 283 | 284 | // sensors 285 | for (size_t i = 0; i < config_.sensors.size(); ++i) { 286 | // Find or allocate the sensor timer 287 | SensorTimer* timer = nullptr; 288 | if (!config_.sensors[i]->force_separate_timer) { 289 | for (const auto& t : sensor_timers_) { 290 | if (!t->isPrivate() && t->getRate() == config_.sensors[i]->rate) { 291 | timer = t.get(); 292 | break; 293 | } 294 | } 295 | } 296 | if (timer == nullptr) { 297 | sensor_timers_.push_back(std::make_unique( 298 | nh_, config_.sensors[i]->rate, 299 | config_.sensors[i]->force_separate_timer, config_.vehicle_name, 300 | this)); 301 | timer = sensor_timers_.back().get(); 302 | } 303 | timer->addSensor(*this, i); 304 | 305 | // Save camera params (e.g. FOV) as they are needed to generate pointcloud 306 | if (config_.sensors[i]->sensor_type == Config::Sensor::TYPE_CAMERA) { 307 | auto camera = (Config::Camera*)config_.sensors[i].get(); 308 | // This assumes the camera exists, which should always be the case with 309 | // the auto-generated-config. 310 | camera->camera_info = airsim_move_client_.simGetCameraInfo( 311 | camera->name, config_.vehicle_name); 312 | // TODO(Schmluk): Might want to also publish the camera info or convert 313 | // to intrinsics etc 314 | } 315 | 316 | if (!config_.publish_sensor_transforms) { 317 | // Broadcast all sensor mounting transforms via static tf. 318 | geometry_msgs::TransformStamped static_transformStamped; 319 | Eigen::Quaterniond rotation = config_.sensors[i]->rotation; 320 | if (config_.sensors[i]->sensor_type == Config::Sensor::TYPE_CAMERA) { 321 | // Camera frames are x right, y down, z depth 322 | rotation = Eigen::Quaterniond(0.5, -0.5, 0.5, -0.5) * rotation; 323 | } 324 | static_transformStamped.header.stamp = ros::Time::now(); 325 | static_transformStamped.header.frame_id = config_.vehicle_name; 326 | static_transformStamped.child_frame_id = config_.sensors[i]->frame_name; 327 | static_transformStamped.transform.translation.x = 328 | config_.sensors[i]->translation.x(); 329 | static_transformStamped.transform.translation.y = 330 | config_.sensors[i]->translation.y(); 331 | static_transformStamped.transform.translation.z = 332 | config_.sensors[i]->translation.z(); 333 | static_transformStamped.transform.rotation.x = rotation.x(); 334 | static_transformStamped.transform.rotation.y = rotation.y(); 335 | static_transformStamped.transform.rotation.z = rotation.z(); 336 | static_transformStamped.transform.rotation.w = rotation.w(); 337 | static_tf_broadcaster_.sendTransform(static_transformStamped); 338 | } 339 | } 340 | 341 | // Simulator processors (find names and let them create themselves) 342 | std::vector keys; 343 | nh_private_.getParamNames(keys); 344 | std::string proc_ns = "processors/"; 345 | std::string full_ns = nh_private_.getNamespace() + "/" + proc_ns; 346 | std::string proc_name; 347 | std::vector processors; 348 | size_t pos; 349 | for (auto const& key : keys) { 350 | if ((pos = key.find(full_ns)) != std::string::npos) { 351 | proc_name = key; 352 | proc_name.erase(0, pos + full_ns.length()); 353 | pos = proc_name.find('/'); 354 | if (pos != std::string::npos) { 355 | proc_name = proc_name.substr(0, pos); 356 | } 357 | if (std::find(processors.begin(), processors.end(), proc_name) == 358 | processors.end()) { 359 | processors.push_back(proc_name); 360 | } 361 | } 362 | } 363 | for (auto const& name : processors) { 364 | if (!nh_private_.hasParam(full_ns + name + "/processor_type")) { 365 | LOG(ERROR) << "Sensor processor '" << name 366 | << "' does not name a 'processor_type' and will be ignored."; 367 | continue; 368 | } 369 | std::string type; 370 | nh_private_.getParam(full_ns + name + "/processor_type", type); 371 | processors_.push_back(simulator_processor::ProcessorFactory::createFromRos( 372 | name, type, nh_, full_ns + name + "/", this)); 373 | } 374 | return true; 375 | } 376 | 377 | bool AirsimSimulator::initializeSimulationFrame() { 378 | if (is_shutdown_) { 379 | return false; 380 | } 381 | // For frame conventions see coords/frames section in the readme/doc 382 | msr::airlib::Pose pose = 383 | airsim_state_client_.simGetVehiclePose(config_.vehicle_name); 384 | Eigen::Quaterniond ori(pose.orientation.w(), pose.orientation.x(), 385 | pose.orientation.y(), pose.orientation.z()); 386 | Eigen::Vector3d euler = ori.toRotationMatrix().eulerAngles( 387 | 2, 1, 0); // yaw-pitch-roll in airsim coords 388 | double yaw = euler[0]; 389 | /** 390 | * NOTE(schmluk): make the coordinate rotation snap to full 45 degree 391 | * rotations, wrong initializations would induce large errors for the 392 | * evaluation of the constructed map. 393 | */ 394 | const double kSnappingAnglesDegrees = 45; 395 | const double kSnappingRangeDegrees = 10; 396 | double snapping_angle = kSnappingAnglesDegrees / 180.0 * M_PI; 397 | double diff = fmod(yaw, snapping_angle); 398 | if (diff > snapping_angle / 2.0) { 399 | diff -= snapping_angle; 400 | } 401 | if (abs(diff) < kSnappingRangeDegrees / 180.0 * M_PI) { 402 | yaw -= diff; 403 | } 404 | frame_converter_.setupFromYaw(yaw); 405 | return true; 406 | } 407 | 408 | void AirsimSimulator::commandPoseCallback(const geometry_msgs::Pose& msg) { 409 | if (!is_running_) { 410 | return; 411 | } 412 | 413 | // Input pose is in drifting odom frame, we therefore 414 | // first convert it back into Unreal GT frame 415 | OdometryDriftSimulator::Transformation T_drift_command; 416 | tf::poseMsgToKindr(msg, &T_drift_command); 417 | const OdometryDriftSimulator::Transformation T_gt_command = 418 | odometry_drift_simulator_.convertDriftedToGroundTruthPose( 419 | T_drift_command); 420 | OdometryDriftSimulator::Transformation::Position t_gt_current_position = 421 | odometry_drift_simulator_.getGroundTruthPose().getPosition(); 422 | 423 | // Use position + yaw as setpoint 424 | auto command_pos = T_gt_command.getPosition(); 425 | auto command_ori = T_gt_command.getEigenQuaternion(); 426 | frame_converter_.rosToAirsim(&command_ori); 427 | double yaw = tf2::getYaw( 428 | tf2::Quaternion(command_ori.x(), command_ori.y(), command_ori.z(), 429 | command_ori.w())); // Eigen's eulerAngles apparently 430 | // messes up some wrap arounds or direcions and gets the wrong yaws in some 431 | // cases 432 | yaw = yaw / M_PI * 180.0; 433 | constexpr double kMinMovingDistance = 0.1; // m 434 | airsim_move_client_.cancelLastTask(); 435 | if ((command_pos - t_gt_current_position).norm() >= kMinMovingDistance) { 436 | frame_converter_.rosToAirsim(&command_pos); 437 | auto yaw_mode = msr::airlib::YawMode(false, yaw); 438 | airsim_move_client_.moveToPositionAsync( 439 | command_pos.x(), command_pos.y(), command_pos.z(), config_.velocity, 440 | 3600, config_.drive_train_type, yaw_mode, -1, 1, config_.vehicle_name); 441 | } else { 442 | // This second command catches the case if the total distance is too small, 443 | // where the moveToPosition command returns without satisfying the yaw. If 444 | // this is always run then apparently sometimes the move command is 445 | // overwritten. 446 | airsim_move_client_.rotateToYawAsync(yaw, 3600, 5, config_.vehicle_name); 447 | } 448 | } 449 | 450 | void AirsimSimulator::startupCallback(const ros::TimerEvent&) { 451 | // Startup the drone, this should set the MAV hovering at 'PlayerStart' in 452 | // unreal 453 | startup_timer_.stop(); 454 | airsim_move_client_.enableApiControl( 455 | true); // Also disables user control, which is good 456 | airsim_move_client_.armDisarm(true); 457 | airsim_move_client_.takeoffAsync(2)->waitOnLastTask(); 458 | airsim_move_client_.moveToPositionAsync(0, 0, 0, 5)->waitOnLastTask(); 459 | is_running_ = true; 460 | std_msgs::Bool msg; 461 | msg.data = true; 462 | sim_is_ready_pub_.publish(msg); 463 | odometry_drift_simulator_.start(); 464 | LOG(INFO) << "Airsim simulation is ready!"; 465 | } 466 | 467 | bool AirsimSimulator::startSimTimer() { 468 | /*** 469 | * NOTE(schmluk): Because, although airsim and ros both use the system clock 470 | * as default for time-stamping according to their docs, these are drifting 471 | * clocks! If use_sim_time is set in the launch file, we just use airsim time 472 | * as ros time and publish it at a fixed wall-time frequency (see param 473 | * time_publisher_interval). 474 | */ 475 | if (use_sim_time_) { 476 | std::thread([this]() { 477 | while (is_connected_) { 478 | auto next = std::chrono::steady_clock::now() + 479 | std::chrono::milliseconds(config_.time_publisher_interval); 480 | readSimTimeCallback(); 481 | if (next > std::chrono::steady_clock::now()) { 482 | std::this_thread::sleep_until(next); 483 | } 484 | } 485 | }) 486 | .detach(); 487 | } 488 | return true; 489 | } 490 | 491 | void AirsimSimulator::readSimTimeCallback() { 492 | /** 493 | * TODO(schmluk): make this nice. 494 | * This is currently a work-around as getting only the time stamp is not yet 495 | * exposed. However, this call does not run on the game thread afaik and was 496 | * not measured to slow down other tasks. Although this queries sim time via 497 | * RPC call, it can run at ~4000 Hz so delay should be <1 ms. 498 | */ 499 | uint64_t ts = 500 | airsim_time_client_.getMultirotorState(config_.vehicle_name).timestamp; 501 | rosgraph_msgs::Clock msg; 502 | msg.clock.fromNSec(ts); 503 | time_pub_.publish(msg); 504 | } 505 | 506 | ros::Time AirsimSimulator::getTimeStamp(msr::airlib::TTimePoint airsim_stamp) { 507 | if (use_sim_time_ && airsim_stamp > 0) { 508 | ros::Time t; 509 | t.fromNSec(airsim_stamp); 510 | return t; 511 | } else { 512 | return ros::Time::now(); 513 | } 514 | } 515 | 516 | void AirsimSimulator::simStateCallback(const ros::TimerEvent&) { 517 | if (is_shutdown_) { 518 | return; 519 | } 520 | if (airsim_state_client_.getConnectionState() != 521 | RpcLibClientBase::ConnectionState::Connected) { 522 | LOG(FATAL) << "Airsim client was disconnected!"; 523 | is_connected_ = false; 524 | is_running_ = false; 525 | raise(SIGINT); 526 | return; 527 | } 528 | /*** 529 | * Note(schmluk): getMultiRotorState returns the *estimated* current state. 530 | * According to their docs and current implementation, this is not implemented 531 | * and resturns the *ground truth* state. However, this might change in the 532 | * future. Alternavtively, we can force ground truth via 533 | * msr::airlib::Kinematics::State state = 534 | * airsim_state_client_.simGetGroundTruthKinematics(config_.vehicle_name); But 535 | * that comes without a timestamp. 536 | */ 537 | msr::airlib::MultirotorState state = 538 | airsim_state_client_.getMultirotorState(config_.vehicle_name); 539 | ros::Time stamp = getTimeStamp(state.timestamp); 540 | 541 | // convert airsim pose to ROS 542 | geometry_msgs::TransformStamped transformStamped; 543 | transformStamped.header.stamp = stamp; 544 | transformStamped.header.frame_id = config_.simulator_frame_name; 545 | transformStamped.child_frame_id = config_.vehicle_name; 546 | tf::vectorEigenToMsg(state.kinematics_estimated.pose.position.cast(), 547 | transformStamped.transform.translation); 548 | tf::quaternionEigenToMsg( 549 | state.kinematics_estimated.pose.orientation.cast(), 550 | transformStamped.transform.rotation); 551 | frame_converter_.airsimToRos(&transformStamped.transform); 552 | 553 | // simulate odometry drift 554 | odometry_drift_simulator_.tick(transformStamped); 555 | 556 | // publish TFs, odom msgs and pose msgs 557 | odometry_drift_simulator_.publishTfs(); 558 | if (odom_pub_.getNumSubscribers() > 0) { 559 | nav_msgs::Odometry odom_msg; 560 | odom_msg.header.stamp = stamp; 561 | odom_msg.header.frame_id = config_.simulator_frame_name; 562 | odom_msg.child_frame_id = config_.vehicle_name; 563 | 564 | tf::poseKindrToMsg(odometry_drift_simulator_.getSimulatedPose(), 565 | &odom_msg.pose.pose); 566 | 567 | odom_msg.twist.twist.linear.x = state.kinematics_estimated.twist.linear.x(); 568 | odom_msg.twist.twist.linear.y = state.kinematics_estimated.twist.linear.y(); 569 | odom_msg.twist.twist.linear.z = state.kinematics_estimated.twist.linear.z(); 570 | odom_msg.twist.twist.angular.x = 571 | state.kinematics_estimated.twist.angular.x(); 572 | odom_msg.twist.twist.angular.y = 573 | state.kinematics_estimated.twist.angular.y(); 574 | odom_msg.twist.twist.angular.z = 575 | state.kinematics_estimated.twist.angular.z(); 576 | // TODO(schmluk): verify that these twist conversions work as intended 577 | frame_converter_.airsimToRos(&odom_msg.twist.twist.linear); 578 | frame_converter_.airsimToRos(&odom_msg.twist.twist.angular); 579 | 580 | odom_pub_.publish(odom_msg); 581 | } 582 | if (pose_pub_.getNumSubscribers() > 0) { 583 | geometry_msgs::PoseStamped pose_msg; 584 | pose_msg.header.stamp = stamp; 585 | pose_msg.header.frame_id = config_.simulator_frame_name; 586 | tf::poseKindrToMsg(odometry_drift_simulator_.getSimulatedPose(), 587 | &pose_msg.pose); 588 | pose_pub_.publish(pose_msg); 589 | } 590 | 591 | // collision (the CollisionInfo in the state does not get updated for whatever 592 | // reason) 593 | if (airsim_state_client_.simGetCollisionInfo(config_.vehicle_name) 594 | .has_collided) { 595 | LOG(WARNING) << "Collision detected for '" << config_.vehicle_name << "'!"; 596 | std_msgs::Bool msg; 597 | msg.data = true; 598 | collision_pub_.publish(msg); 599 | } 600 | } 601 | 602 | bool AirsimSimulator::readTransformFromRos(const std::string& topic, 603 | Eigen::Vector3d* translation, 604 | Eigen::Quaterniond* rotation) { 605 | // This is implemented separately to catch all exceptions when parsing xmlrpc 606 | // defaults: Unit transformation 607 | *translation = Eigen::Vector3d(); 608 | *rotation = Eigen::Quaterniond(1, 0, 0, 0); 609 | if (!nh_private_.hasParam(topic)) { 610 | return false; 611 | } 612 | XmlRpc::XmlRpcValue matrix; 613 | nh_private_.getParam(topic, matrix); 614 | if (matrix.getType() != XmlRpc::XmlRpcValue::TypeArray) { 615 | LOG(WARNING) << "Transformation '" << topic << "' expected as 4x4 array."; 616 | return false; 617 | } else if (matrix.size() != 4) { 618 | LOG(WARNING) << "Transformation '" << topic << "' expected as 4x4 array."; 619 | return false; 620 | } 621 | Eigen::Matrix3d rot_mat; 622 | Eigen::Vector3d trans; 623 | double val; 624 | for (size_t i = 0; i < 3; ++i) { 625 | XmlRpc::XmlRpcValue row = matrix[i]; 626 | if (row.getType() != XmlRpc::XmlRpcValue::TypeArray) { 627 | LOG(WARNING) << "Transformation '" << topic << "' expected as 4x4 array."; 628 | return false; 629 | } else if (row.size() != 4) { 630 | LOG(WARNING) << "Transformation '" << topic << "' expected as 4x4 array."; 631 | return false; 632 | } 633 | for (size_t j = 0; j < 4; ++j) { 634 | try { 635 | val = row[j]; 636 | } catch (...) { 637 | try { 638 | int ival = row[j]; 639 | val = static_cast(ival); 640 | } catch (...) { 641 | LOG(WARNING) << "Unable to convert all entries of transformation '" 642 | << topic << "' to double."; 643 | return false; 644 | } 645 | } 646 | if (j < 3) { 647 | rot_mat(i, j) = val; 648 | } else { 649 | trans[i] = val; 650 | } 651 | } 652 | } 653 | *translation = trans; 654 | *rotation = rot_mat; 655 | return true; 656 | } 657 | 658 | void AirsimSimulator::onShutdown() { 659 | is_shutdown_ = true; 660 | for (const auto& timer : sensor_timers_) { 661 | timer->signalShutdown(); 662 | } 663 | if (is_connected_) { 664 | LOG(INFO) << "Shutting down: resetting airsim server."; 665 | airsim_state_client_.reset(); 666 | airsim_state_client_.enableApiControl(false); 667 | } 668 | } 669 | 670 | } // namespace unreal_airsim 671 | -------------------------------------------------------------------------------- /src/simulator_processing/depth_to_pointcloud.cpp: -------------------------------------------------------------------------------- 1 | #include "unreal_airsim/simulator_processing/depth_to_pointcloud.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include "unreal_airsim/online_simulator/simulator.h" 15 | 16 | namespace unreal_airsim::simulator_processor { 17 | 18 | ProcessorFactory::Registration 19 | DepthToPointcloud::registration_("DepthToPointcloud"); 20 | 21 | bool DepthToPointcloud::setupFromRos(const ros::NodeHandle& nh, 22 | const std::string& ns) { 23 | nh_ = nh; 24 | use_color_ = false; 25 | is_setup_ = false; 26 | 27 | // Get params. 28 | std::string depth_camera_name, color_camera_name, segmentation_camera_name, 29 | output_topic, depth_topic, color_topic, segmentation_topic; 30 | nh.param(ns + "max_queue_length", max_queue_length_, 10); 31 | nh.param(ns + "max_depth", max_depth_, 1e6f); 32 | nh.param(ns + "max_ray_length", max_ray_length_, 1e6f); 33 | nh.param(ns + "output_topic", output_topic, 34 | parent_->getConfig().vehicle_name + "/" + name_); 35 | if (nh.hasParam(ns + "depth_camera_name")) { 36 | nh.getParam(ns + "depth_camera_name", depth_camera_name); 37 | } else if (nh.hasParam(ns + "depth_topic")) { 38 | nh.getParam(ns + "depth_camera_topic", depth_topic); 39 | } else { 40 | LOG(FATAL) << "DepthToPointcloud requires the 'depth_camera_name' or " 41 | "'depth_camera_topic' param to be set!"; 42 | return false; 43 | } 44 | if (nh.hasParam(ns + "color_camera_name")) { 45 | nh.getParam(ns + "color_camera_name", color_camera_name); 46 | use_color_ = true; 47 | } else if (nh.hasParam(ns + "color_camera_topic")) { 48 | nh.getParam(ns + "color_camera_topic", color_topic); 49 | use_color_ = true; 50 | } 51 | if (nh.hasParam(ns + "segmentation_camera_name")) { 52 | nh.getParam(ns + "segmentation_camera_name", segmentation_camera_name); 53 | use_segmentation_ = true; 54 | } else if (nh.hasParam(ns + "segmentation_camera_topic")) { 55 | nh.getParam(ns + "segmentation_camera_topic", segmentation_topic); 56 | use_segmentation_ = true; 57 | } 58 | 59 | // Find source cameras. 60 | for (const auto& sensor : parent_->getConfig().sensors) { 61 | if (sensor->name == depth_camera_name) { 62 | depth_topic = sensor->output_topic; 63 | auto camera = 64 | (unreal_airsim::AirsimSimulator::Config::Camera*)sensor.get(); 65 | fov_ = camera->camera_info.fov; 66 | } 67 | if (sensor->name == color_camera_name) { 68 | color_topic = sensor->output_topic; 69 | } 70 | if (sensor->name == segmentation_camera_name) { 71 | segmentation_topic = sensor->output_topic; 72 | } 73 | } 74 | if (depth_topic.empty()) { 75 | LOG(FATAL) << "Could not find a Camera with name '" << depth_camera_name 76 | << "'!"; 77 | return false; 78 | } 79 | if (color_topic.empty() && use_color_) { 80 | LOG(FATAL) << "Could not find a Camera with name '" << color_camera_name 81 | << "'!"; 82 | return false; 83 | } 84 | if (segmentation_topic.empty() && use_segmentation_) { 85 | LOG(FATAL) << "Could not find a Camera with name '" 86 | << segmentation_camera_name << "'!"; 87 | return false; 88 | } 89 | 90 | // Ros. 91 | pub_ = nh_.advertise(output_topic, 5); 92 | depth_sub_ = nh_.subscribe(depth_topic, max_queue_length_, 93 | &DepthToPointcloud::depthImageCallback, this); 94 | if (use_color_) { 95 | color_sub_ = nh_.subscribe(color_topic, max_queue_length_, 96 | &DepthToPointcloud::colorImageCallback, this); 97 | } 98 | if (use_segmentation_) { 99 | segmentation_sub_ = 100 | nh_.subscribe(segmentation_topic, max_queue_length_, 101 | &DepthToPointcloud::segmentationImageCallback, this); 102 | } 103 | return true; 104 | } 105 | 106 | void DepthToPointcloud::depthImageCallback(const sensor_msgs::ImagePtr& msg) { 107 | // Use the first depth image to initialize intrinsics. 108 | if (!is_setup_) { 109 | vx_ = msg->width / 2; 110 | vy_ = msg->height / 2; 111 | focal_length_ = 112 | static_cast(msg->width) / (2.0 * std::tan(fov_ * M_PI / 360.0)); 113 | is_setup_ = true; 114 | } 115 | 116 | // Store depth img in queue. 117 | depth_queue_.push_back(msg); 118 | if (depth_queue_.size() > max_queue_length_) { 119 | depth_queue_.pop_front(); 120 | } 121 | findMatchingMessagesToPublish(msg); 122 | } 123 | 124 | void DepthToPointcloud::colorImageCallback(const sensor_msgs::ImagePtr& msg) { 125 | // Store color img in queue. 126 | color_queue_.push_back(msg); 127 | if (color_queue_.size() > max_queue_length_) { 128 | color_queue_.pop_front(); 129 | } 130 | findMatchingMessagesToPublish(msg); 131 | } 132 | 133 | void DepthToPointcloud::segmentationImageCallback( 134 | const sensor_msgs::ImagePtr& msg) { 135 | // Store segmentation img in queue. 136 | segmentation_queue_.push_back(msg); 137 | if (segmentation_queue_.size() > max_queue_length_) { 138 | segmentation_queue_.pop_front(); 139 | } 140 | findMatchingMessagesToPublish(msg); 141 | } 142 | 143 | void DepthToPointcloud::findMatchingMessagesToPublish( 144 | const sensor_msgs::ImagePtr& reference_msg) { 145 | std::deque::iterator depth_it, color_it, 146 | segmentation_it; 147 | // If a modality is not used we count it as found. 148 | bool found_color = true; 149 | bool found_segmentation = true; 150 | 151 | std::lock_guard guard(queue_guard); 152 | 153 | depth_it = 154 | std::find_if(depth_queue_.begin(), depth_queue_.end(), 155 | [reference_msg](const sensor_msgs::ImagePtr& s) { 156 | return s->header.stamp == reference_msg->header.stamp; 157 | }); 158 | if (depth_it == depth_queue_.end()) { 159 | // Depth image is always necessary. 160 | return; 161 | } 162 | 163 | if (use_color_) { 164 | // Check whether there is a color image with matching timestamp. 165 | color_it = 166 | std::find_if(color_queue_.begin(), color_queue_.end(), 167 | [reference_msg](const sensor_msgs::ImagePtr& s) { 168 | return s->header.stamp == reference_msg->header.stamp; 169 | }); 170 | if (color_it == color_queue_.end()) { 171 | found_color = false; 172 | } 173 | } 174 | 175 | if (use_segmentation_) { 176 | // Check whether there is a segmentation image with matching timestamp. 177 | segmentation_it = 178 | std::find_if(segmentation_queue_.begin(), segmentation_queue_.end(), 179 | [reference_msg](const sensor_msgs::ImagePtr& s) { 180 | return s->header.stamp == reference_msg->header.stamp; 181 | }); 182 | if (segmentation_it == segmentation_queue_.end()) { 183 | found_segmentation = false; 184 | } 185 | } 186 | 187 | if (found_color && found_segmentation) { 188 | publishPointcloud(*depth_it, use_color_ ? *color_it : nullptr, 189 | use_segmentation_ ? *segmentation_it : nullptr); 190 | depth_queue_.erase(depth_it); 191 | if (use_color_) { 192 | color_queue_.erase(color_it); 193 | } 194 | if (use_segmentation_) { 195 | segmentation_queue_.erase(segmentation_it); 196 | } 197 | } 198 | } 199 | 200 | void DepthToPointcloud::publishPointcloud( 201 | const sensor_msgs::ImagePtr& depth_ptr, 202 | const sensor_msgs::ImagePtr& color_ptr, 203 | const sensor_msgs::ImagePtr& segmentation_ptr) { 204 | /** 205 | * NOTE(schmluk): This method assumes that all images are from the same 206 | * simulated camera, i.e. are perfectly aligned and have identical settings 207 | * (resolution, intrinsics, extrinsics). This assumes we use planar depth 208 | * camera (ImageType::DepthPlanar) with image data as floats. We further 209 | * assume that segmentation is created using infrared (ImageType::Infrared), 210 | * i.e. the labels are provided in the first channel of the segmentation 211 | * image. 212 | */ 213 | if (pub_.getNumSubscribers() == 0) { 214 | return; 215 | } 216 | cv_bridge::CvImageConstPtr depth_img, color_img, segmentation_img; 217 | depth_img = cv_bridge::toCvShare(depth_ptr, depth_ptr->encoding); 218 | if (use_color_) { 219 | color_img = cv_bridge::toCvShare(color_ptr, color_ptr->encoding); 220 | } 221 | if (use_segmentation_) { 222 | segmentation_img = 223 | cv_bridge::toCvShare(segmentation_ptr, segmentation_ptr->encoding); 224 | } 225 | 226 | // figure out number of points 227 | int numpoints = depth_img->image.rows * depth_img->image.cols; 228 | 229 | // declare message and sizes 230 | sensor_msgs::PointCloud2 cloud; 231 | cloud.header.frame_id = depth_ptr->header.frame_id; 232 | cloud.header.stamp = depth_ptr->header.stamp; 233 | cloud.width = numpoints; 234 | cloud.height = 1; 235 | cloud.is_bigendian = false; 236 | cloud.is_dense = false; // It is safer to assume there may be invalid points 237 | // since I did not see proof otherwise 238 | 239 | // fields setup 240 | sensor_msgs::PointCloud2Modifier modifier(cloud); 241 | if (use_color_) { 242 | if (use_segmentation_) { 243 | modifier.setPointCloud2Fields(5, "x", 1, sensor_msgs::PointField::FLOAT32, 244 | "y", 1, sensor_msgs::PointField::FLOAT32, 245 | "z", 1, sensor_msgs::PointField::FLOAT32, 246 | "rgb", 1, sensor_msgs::PointField::FLOAT32, 247 | "id", 1, sensor_msgs::PointField::UINT8); 248 | } else { 249 | modifier.setPointCloud2Fields(4, "x", 1, sensor_msgs::PointField::FLOAT32, 250 | "y", 1, sensor_msgs::PointField::FLOAT32, 251 | "z", 1, sensor_msgs::PointField::FLOAT32, 252 | "rgb", 1, sensor_msgs::PointField::FLOAT32); 253 | } 254 | } else { 255 | if (use_segmentation_) { 256 | modifier.setPointCloud2Fields(4, "x", 1, sensor_msgs::PointField::FLOAT32, 257 | "y", 1, sensor_msgs::PointField::FLOAT32, 258 | "z", 1, sensor_msgs::PointField::FLOAT32, 259 | "id", 1, sensor_msgs::PointField::UINT8); 260 | } else { 261 | modifier.setPointCloud2Fields(3, "x", 1, sensor_msgs::PointField::FLOAT32, 262 | "y", 1, sensor_msgs::PointField::FLOAT32, 263 | "z", 1, sensor_msgs::PointField::FLOAT32); 264 | } 265 | } 266 | modifier.resize(numpoints); 267 | 268 | sensor_msgs::PointCloud2Iterator out_x(cloud, "x"); 269 | sensor_msgs::PointCloud2Iterator out_y(cloud, "y"); 270 | sensor_msgs::PointCloud2Iterator out_z(cloud, "z"); 271 | std::unique_ptr> out_rgb; 272 | std::unique_ptr> out_seg; 273 | if (use_color_) { 274 | out_rgb = std::make_unique>( 275 | cloud, "rgb"); 276 | } 277 | if (use_segmentation_) { 278 | out_seg = std::make_unique>(cloud, 279 | "id"); 280 | } 281 | for (int v = 0; v < depth_img->image.rows; v++) { 282 | for (int u = 0; u < depth_img->image.cols; u++) { 283 | float z = depth_img->image.at(v, u); 284 | if (z > max_depth_) { 285 | continue; 286 | } 287 | float x = (static_cast(u) - vx_) * z / focal_length_; 288 | float y = (static_cast(v) - vy_) * z / focal_length_; 289 | if (max_ray_length_ > 0.0) { 290 | float dist_square = x * x + y * y + z * z; 291 | if (dist_square > max_ray_length_ * max_ray_length_) { 292 | continue; 293 | } 294 | } 295 | out_x[0] = x; 296 | out_y[0] = y; 297 | out_z[0] = z; 298 | ++out_x; 299 | ++out_y; 300 | ++out_z; 301 | 302 | if (use_color_) { 303 | cv::Vec3b color = color_img->image.at(v, u); 304 | (*out_rgb)[0] = color[0]; 305 | (*out_rgb)[1] = color[1]; 306 | (*out_rgb)[2] = color[2]; 307 | ++(*out_rgb); 308 | } 309 | 310 | if (use_segmentation_) { 311 | (*out_seg)[0] = segmentation_img->image.at(v, u); 312 | ++(*out_seg); 313 | } 314 | } 315 | } 316 | pub_.publish(cloud); 317 | } 318 | 319 | } // namespace unreal_airsim::simulator_processor 320 | -------------------------------------------------------------------------------- /src/simulator_processing/infrared_id_compensation.cpp: -------------------------------------------------------------------------------- 1 | #include "unreal_airsim/simulator_processing/infrared_id_compensation.h" 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | #include "3rd_party/csv.h" 9 | #include "unreal_airsim/online_simulator/simulator.h" 10 | 11 | namespace unreal_airsim::simulator_processor { 12 | 13 | ProcessorFactory::Registration 14 | InfraredIdCompensation::registration_("InfraredIdCompensation"); 15 | 16 | bool InfraredIdCompensation::setupFromRos(const ros::NodeHandle& nh, 17 | const std::string& ns) { 18 | nh_ = nh; 19 | if (!nh_.hasParam(ns + "input_topic")) { 20 | LOG(ERROR) 21 | << "InfraredIdCompensation requires the 'input_topic' to be set."; 22 | return false; 23 | } 24 | if (!nh_.hasParam(ns + "output_topic")) { 25 | LOG(ERROR) 26 | << "InfraredIdCompensation requires the 'output_topic' to be set."; 27 | return false; 28 | } 29 | if (nh_.hasParam(ns + "correction_file")) { 30 | // Allow reading the corrections from file 31 | std::string correction_file; 32 | nh_.getParam(ns + "correction_file", correction_file); 33 | io::CSVReader<2> in(correction_file); 34 | in.read_header(io::ignore_extra_column, "MeshID", "InfraRedID"); 35 | int mesh, ir; 36 | std::fill_n(infrared_compensation_, 256, 255); 37 | int previous = -1; 38 | while (in.read_row(mesh, ir)) { 39 | if (ir > previous) { 40 | infrared_compensation_[ir] = mesh; 41 | previous = ir; 42 | } 43 | } 44 | LOG(INFO) << "Read infrared corrections from file '" << correction_file 45 | << "'."; 46 | } 47 | std::string input_topic, output_topic; 48 | nh_.getParam(ns + "input_topic", input_topic); 49 | nh_.getParam(ns + "output_topic", output_topic); 50 | pub_ = nh_.advertise(output_topic, 10); 51 | sub_ = nh_.subscribe(input_topic, 10, &InfraredIdCompensation::imageCallback, 52 | this); 53 | return true; 54 | } 55 | 56 | void InfraredIdCompensation::imageCallback(const sensor_msgs::ImagePtr& msg) { 57 | cv_bridge::CvImagePtr img = cv_bridge::toCvCopy(msg, msg->encoding); 58 | for (int v = 0; v < img->image.rows; v++) { 59 | for (int u = 0; u < img->image.cols; u++) { 60 | img->image.at(v, u) = 61 | infrared_compensation_[img->image.at(v, u)]; 62 | } 63 | } 64 | pub_.publish(img->toImageMsg()); 65 | } 66 | 67 | } // namespace unreal_airsim::simulator_processor 68 | -------------------------------------------------------------------------------- /src/simulator_processing/odometry_drift_simulator/normal_distribution.cpp: -------------------------------------------------------------------------------- 1 | #include "unreal_airsim/simulator_processing/odometry_drift_simulator/normal_distribution.h" 2 | 3 | #include 4 | 5 | namespace unreal_airsim { 6 | NormalDistribution::Config NormalDistribution::Config::fromRosParams( 7 | const ros::NodeHandle& nh) { 8 | Config config; 9 | nh.param("mean", config.mean, config.mean); 10 | nh.param("stddev", config.stddev, config.stddev); 11 | return config; 12 | } 13 | 14 | bool NormalDistribution::Config::isValid( 15 | const std::string& error_msg_prefix) const { 16 | if (stddev < 0.0) { 17 | LOG_IF(WARNING, !error_msg_prefix.empty()) 18 | << "The " << error_msg_prefix 19 | << "/stddev should be a non-negative float"; 20 | return false; 21 | } 22 | return true; 23 | } 24 | 25 | std::ostream& operator<<(std::ostream& os, 26 | const NormalDistribution::Config& config) { 27 | return os << "mean: " << config.mean << ", stddev: " << config.stddev; 28 | } 29 | } // namespace unreal_airsim 30 | -------------------------------------------------------------------------------- /src/simulator_processing/odometry_drift_simulator/odometry_drift_simulator.cpp: -------------------------------------------------------------------------------- 1 | #include "unreal_airsim/simulator_processing/odometry_drift_simulator/odometry_drift_simulator.h" 2 | 3 | #include 4 | #include 5 | 6 | namespace unreal_airsim { 7 | OdometryDriftSimulator::OdometryDriftSimulator(Config config) 8 | : config_(config.checkValid()), 9 | started_publishing_(false), 10 | velocity_noise_sampling_period_(1.f / config.velocity_noise_frequency_hz), 11 | velocity_noise_(config.velocity_noise), 12 | pose_noise_(config.pose_noise) { 13 | reset(); 14 | VLOG(1) << "Initialized drifting odometry simulator, with config:\n" 15 | << config_; 16 | } 17 | 18 | void OdometryDriftSimulator::reset() { 19 | last_velocity_noise_sampling_time_ = ros::Time(); 20 | current_linear_velocity_noise_sample_W_.setZero(); 21 | current_angular_velocity_noise_sample_W_.setZero(); 22 | integrated_pose_drift_.setIdentity(); 23 | current_pose_noise_.setIdentity(); 24 | current_simulated_pose_.setIdentity(); 25 | last_ground_truth_pose_msg_ = geometry_msgs::TransformStamped(); 26 | } 27 | 28 | void OdometryDriftSimulator::tick( 29 | const geometry_msgs::TransformStamped& ground_truth_pose_msg) { 30 | // Compute the time delta 31 | const ros::Time& current_timestamp = ground_truth_pose_msg.header.stamp; 32 | double delta_t = 0.0; 33 | if (!last_ground_truth_pose_msg_.header.stamp.isZero()) { 34 | delta_t = 35 | (current_timestamp - last_ground_truth_pose_msg_.header.stamp).toSec(); 36 | } 37 | last_ground_truth_pose_msg_ = ground_truth_pose_msg; 38 | if (delta_t < 0.0) { 39 | LOG(WARNING) << "Time difference between current and last received pose " 40 | "msg is negative. Skipping."; 41 | return; 42 | } 43 | 44 | // Get the ground truth pose 45 | Transformation ground_truth_pose; 46 | tf::transformMsgToKindr(ground_truth_pose_msg.transform, &ground_truth_pose); 47 | 48 | // Draw a random velocity noise sample, used to simulate drift 49 | if (last_velocity_noise_sampling_time_ + velocity_noise_sampling_period_ < 50 | current_timestamp) { 51 | last_velocity_noise_sampling_time_ = current_timestamp; 52 | 53 | // Sample the linear velocity noise in body frame 54 | const Transformation::Vector3 current_linear_velocity_noise_sample_B_ = { 55 | velocity_noise_.x(), velocity_noise_.y(), velocity_noise_.z()}; 56 | current_linear_velocity_noise_sample_W_ = 57 | ground_truth_pose.getRotation().rotate( 58 | current_linear_velocity_noise_sample_B_); 59 | 60 | // Sample the angular velocity noise directly in world frame, 61 | // since we only want to simulate drift on world frame yaw 62 | current_angular_velocity_noise_sample_W_ = {0.0, 0.0, 63 | velocity_noise_.yaw()}; 64 | } 65 | 66 | // Integrate the drift 67 | Transformation::Vector6 drift_delta_W_vec; 68 | drift_delta_W_vec << current_linear_velocity_noise_sample_W_, 69 | current_angular_velocity_noise_sample_W_; 70 | drift_delta_W_vec *= delta_t; 71 | const Transformation drift_delta_W = Transformation::exp(drift_delta_W_vec); 72 | integrated_pose_drift_ = drift_delta_W * integrated_pose_drift_; 73 | 74 | // Draw a random pose noise sample 75 | Transformation::Vector6 pose_noise_B_vec; 76 | pose_noise_B_vec << pose_noise_.x(), pose_noise_.y(), pose_noise_.z(), 77 | pose_noise_.roll(), pose_noise_.pitch(), pose_noise_.yaw(); 78 | current_pose_noise_ = Transformation::exp(pose_noise_B_vec); 79 | 80 | // Update the current simulated pose 81 | current_simulated_pose_ = 82 | integrated_pose_drift_ * ground_truth_pose * current_pose_noise_; 83 | } 84 | 85 | OdometryDriftSimulator::Transformation 86 | OdometryDriftSimulator::getGroundTruthPose() const { 87 | Transformation ground_truth_pose; 88 | tf::transformMsgToKindr(last_ground_truth_pose_msg_.transform, 89 | &ground_truth_pose); 90 | return ground_truth_pose; 91 | } 92 | 93 | geometry_msgs::TransformStamped OdometryDriftSimulator::getSimulatedPoseMsg() 94 | const { 95 | geometry_msgs::TransformStamped simulated_pose_msg = 96 | last_ground_truth_pose_msg_; 97 | tf::transformKindrToMsg(current_simulated_pose_, 98 | &simulated_pose_msg.transform); 99 | return simulated_pose_msg; 100 | } 101 | 102 | OdometryDriftSimulator::Transformation 103 | OdometryDriftSimulator::convertDriftedToGroundTruthPose( 104 | const OdometryDriftSimulator::Transformation& simulated_pose) const { 105 | return integrated_pose_drift_.inverse() * simulated_pose * 106 | current_pose_noise_.inverse(); 107 | } 108 | 109 | OdometryDriftSimulator::Transformation 110 | OdometryDriftSimulator::convertGroundTruthToDriftedPose( 111 | const OdometryDriftSimulator::Transformation& ground_truth_pose) const { 112 | return integrated_pose_drift_ * ground_truth_pose * current_pose_noise_; 113 | } 114 | 115 | geometry_msgs::TransformStamped 116 | OdometryDriftSimulator::convertDriftedToGroundTruthPoseMsg( 117 | const geometry_msgs::TransformStamped& simulated_pose_msg) const { 118 | Transformation simulated_pose; 119 | tf::transformMsgToKindr(simulated_pose_msg.transform, &simulated_pose); 120 | 121 | const Transformation ground_truth_pose = 122 | convertDriftedToGroundTruthPose(simulated_pose); 123 | 124 | geometry_msgs::TransformStamped ground_truth_pose_msg = simulated_pose_msg; 125 | tf::transformKindrToMsg(ground_truth_pose, &ground_truth_pose_msg.transform); 126 | return ground_truth_pose_msg; 127 | } 128 | 129 | geometry_msgs::TransformStamped 130 | OdometryDriftSimulator::convertGroundTruthToDriftedPoseMsg( 131 | const geometry_msgs::TransformStamped& ground_truth_pose_msg) const { 132 | Transformation ground_truth_pose; 133 | tf::transformMsgToKindr(ground_truth_pose_msg.transform, &ground_truth_pose); 134 | 135 | const Transformation simulated_pose = 136 | convertGroundTruthToDriftedPose(ground_truth_pose); 137 | 138 | geometry_msgs::TransformStamped simulated_pose_msg = ground_truth_pose_msg; 139 | tf::transformKindrToMsg(simulated_pose, &simulated_pose_msg.transform); 140 | return simulated_pose_msg; 141 | } 142 | 143 | OdometryDriftSimulator::VelocityNoiseDistributions::VelocityNoiseDistributions( 144 | const OdometryDriftSimulator::Config::NoiseConfigMap& 145 | velocity_noise_configs) 146 | : x(velocity_noise_configs.at("x")), 147 | y(velocity_noise_configs.at("y")), 148 | z(velocity_noise_configs.at("z")), 149 | yaw(velocity_noise_configs.at("yaw")) {} 150 | 151 | OdometryDriftSimulator::PoseNoiseDistributions::PoseNoiseDistributions( 152 | const OdometryDriftSimulator::Config::NoiseConfigMap& pose_noise_configs) 153 | : x(pose_noise_configs.at("x")), 154 | y(pose_noise_configs.at("y")), 155 | z(pose_noise_configs.at("z")), 156 | yaw(pose_noise_configs.at("yaw")), 157 | pitch(pose_noise_configs.at("pitch")), 158 | roll(pose_noise_configs.at("roll")) {} 159 | 160 | void OdometryDriftSimulator::publishSimulatedPoseTf() const { 161 | transform_broadcaster_.sendTransform(getSimulatedPoseMsg()); 162 | } 163 | 164 | void OdometryDriftSimulator::publishGroundTruthPoseTf() const { 165 | // Publish the ground truth pose with a frame name that differs from the 166 | // simulated pose to avoid conflicting TFs 167 | geometry_msgs::TransformStamped ground_truth_pose_msg = 168 | getGroundTruthPoseMsg(); 169 | ground_truth_pose_msg.child_frame_id += config_.ground_truth_frame_suffix; 170 | transform_broadcaster_.sendTransform(ground_truth_pose_msg); 171 | } 172 | 173 | void OdometryDriftSimulator::publishTfs() const { 174 | if (!started_publishing_) { 175 | return; 176 | } 177 | 178 | // Publish simulated pose TF 179 | publishSimulatedPoseTf(); 180 | 181 | // Publish true pose TF if requested 182 | if (config_.publish_ground_truth_pose) { 183 | publishGroundTruthPoseTf(); 184 | } 185 | } 186 | 187 | OdometryDriftSimulator::Config OdometryDriftSimulator::Config::fromRosParams( 188 | const ros::NodeHandle& nh) { 189 | Config config; 190 | 191 | nh.param("publish_ground_truth_pose", config.publish_ground_truth_pose, 192 | config.publish_ground_truth_pose); 193 | 194 | nh.param("ground_truth_frame_suffix", config.ground_truth_frame_suffix, 195 | config.ground_truth_frame_suffix); 196 | 197 | nh.param("velocity_noise_frequency_hz", config.velocity_noise_frequency_hz, 198 | config.velocity_noise_frequency_hz); 199 | 200 | for (auto& kv : config.velocity_noise) { 201 | kv.second = NormalDistribution::Config::fromRosParams( 202 | ros::NodeHandle(nh, "velocity_noise/" + kv.first)); 203 | } 204 | for (auto& kv : config.pose_noise) { 205 | kv.second = NormalDistribution::Config::fromRosParams( 206 | ros::NodeHandle(nh, "position_noise/" + kv.first)); 207 | } 208 | 209 | return config; 210 | } 211 | 212 | bool OdometryDriftSimulator::Config::isValid() const { 213 | bool is_valid = true; 214 | 215 | if (ground_truth_frame_suffix.empty()) { 216 | LOG(WARNING) 217 | << "The ground_truth_frame_suffix should be a non-empty string"; 218 | is_valid = false; 219 | } 220 | 221 | if (velocity_noise_frequency_hz <= 0.f) { 222 | LOG(WARNING) 223 | << "The velocity_noise_frequency_hz should be a positive float"; 224 | is_valid = false; 225 | } 226 | 227 | for (auto& kv : velocity_noise) { 228 | if (!kv.second.isValid("velocity_noise/" + kv.first + "/")) { 229 | is_valid = false; 230 | } 231 | } 232 | for (auto& kv : pose_noise) { 233 | if (!kv.second.isValid("position_noise/" + kv.first + "/")) { 234 | is_valid = false; 235 | } 236 | } 237 | 238 | return is_valid; 239 | } 240 | 241 | std::ostream& operator<<(std::ostream& os, 242 | const OdometryDriftSimulator::Config& config) { 243 | os << "-- publish_ground_truth_pose: " 244 | << (config.publish_ground_truth_pose ? "true" : "false") << "\n" 245 | << "-- ground_truth_frame_suffix: " << config.ground_truth_frame_suffix 246 | << "\n" 247 | << "-- velocity_noise_frequency_hz: " << config.velocity_noise_frequency_hz 248 | << "\n"; 249 | 250 | os << "-- velocity_noise/\n"; 251 | for (auto& kv : config.velocity_noise) { 252 | os << "---- " << kv.first << ": {" << kv.second << "}\n"; 253 | } 254 | os << "-- position_noise/\n"; 255 | for (auto& kv : config.pose_noise) { 256 | os << "---- " << kv.first << ": {" << kv.second << "}\n"; 257 | } 258 | 259 | return os; 260 | } 261 | } // namespace unreal_airsim 262 | -------------------------------------------------------------------------------- /src/simulator_processing/processor_factory.cpp: -------------------------------------------------------------------------------- 1 | #include "unreal_airsim/simulator_processing/processor_factory.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include "unreal_airsim/simulator_processing/processor_base.h" 8 | 9 | namespace unreal_airsim::simulator_processor { 10 | 11 | ProcessorFactory::TypeMap& ProcessorFactory::getRegistryMap() { 12 | static ProcessorFactory::TypeMap type_map_; 13 | return type_map_; 14 | } 15 | 16 | std::unique_ptr ProcessorFactory::createFromRos( 17 | const std::string& name, const std::string& type, const ros::NodeHandle& nh, 18 | const std::string& ns, AirsimSimulator* parent) { 19 | if (getRegistryMap().find(type) == getRegistryMap().end()) { 20 | const std::string typeList = 21 | getRegistryMap().empty() 22 | ? std::string() 23 | : std::accumulate( 24 | std::next(getRegistryMap().begin()), getRegistryMap().end(), 25 | getRegistryMap().begin()->first, 26 | [](std::string& a, std::pair b) { 27 | return a + ", " + b.first; 28 | }); 29 | LOG(ERROR) << "Unknown type '" << type << "' for simulator_processor '" 30 | << name << "'. Available types are: " << typeList << "."; 31 | return nullptr; 32 | } 33 | ProcessorBase* processor = getRegistryMap().at(type)(); 34 | processor->parent_ = parent; 35 | processor->name_ = name; 36 | if (!processor->setupFromRos(nh, ns)) { 37 | LOG(ERROR) << "Setup for simulator_processor '" << name 38 | << "' failed, it will be ignored."; 39 | return nullptr; 40 | } 41 | return std::unique_ptr(processor); 42 | } 43 | 44 | } // namespace unreal_airsim::simulator_processor 45 | -------------------------------------------------------------------------------- /unreal_airsim_https.rosinstall: -------------------------------------------------------------------------------- 1 | - git: 2 | local-name: catkin_simple 3 | uri: https://github.com/catkin/catkin_simple.git 4 | - git: 5 | local-name: eigen_catkin 6 | uri: https://github.com/ethz-asl/eigen_catkin.git 7 | - git: 8 | local-name: glog_catkin 9 | uri: https://github.com/ethz-asl/glog_catkin.git 10 | - git: 11 | local-name: gflags_catkin 12 | uri: https://github.com/ethz-asl/gflags_catkin.git 13 | -------------------------------------------------------------------------------- /unreal_airsim_ssh.rosinstall: -------------------------------------------------------------------------------- 1 | - git: 2 | local-name: catkin_simple 3 | uri: git@github.com:catkin/catkin_simple.git 4 | - git: 5 | local-name: eigen_catkin 6 | uri: git@github.com:ethz-asl/eigen_catkin.git 7 | - git: 8 | local-name: glog_catkin 9 | uri: git@github.com:ethz-asl/glog_catkin.git 10 | - git: 11 | local-name: gflags_catkin 12 | uri: git@github.com:ethz-asl/gflags_catkin.git 13 | --------------------------------------------------------------------------------