diff --git a/PythonClient/ros/airsim_ros_client/.gitignore b/PythonClient/ros/airsim_ros_client/.gitignore
new file mode 100644
index 0000000000..6740d8f8a0
--- /dev/null
+++ b/PythonClient/ros/airsim_ros_client/.gitignore
@@ -0,0 +1,192 @@
+research/
+
+devel/
+logs/
+build/
+bin/
+lib/
+msg_gen/
+srv_gen/
+msg/*Action.msg
+msg/*ActionFeedback.msg
+msg/*ActionGoal.msg
+msg/*ActionResult.msg
+msg/*Feedback.msg
+msg/*Goal.msg
+msg/*Result.msg
+msg/_*.py
+
+# Bag files
+*.bag
+
+# Generated by dynamic reconfigure
+*.cfgc
+/cfg/cpp/
+/cfg/*.py
+
+# Ignore generated docs
+*.dox
+*.wikidoc
+
+# eclipse stuff
+.project
+.cproject
+
+# qcreator stuff
+CMakeLists.txt.user
+
+srv/_*.py
+*.pcd
+*.pyc
+qtcreator-*
+*.user
+
+/planning/cfg
+/planning/docs
+/planning/src
+
+*~
+
+# Emacs
+.#*
+
+# Catkin custom files
+CATKIN_IGNORE
+
+# Prerequisites
+*.d
+
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
+
+# Byte-compiled / optimized / DLL files
+__pycache__/
+*.py[cod]
+*$py.class
+
+# C extensions
+*.so
+
+# Distribution / packaging
+.Python
+build/
+develop-eggs/
+dist/
+downloads/
+eggs/
+.eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+wheels/
+*.egg-info/
+.installed.cfg
+*.egg
+MANIFEST
+
+# PyInstaller
+# Usually these files are written by a python script from a template
+# before PyInstaller builds the exe, so as to inject date/other infos into it.
+*.manifest
+*.spec
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+htmlcov/
+.tox/
+.coverage
+.coverage.*
+.cache
+nosetests.xml
+coverage.xml
+*.cover
+.hypothesis/
+.pytest_cache/
+
+# Translations
+*.mo
+*.pot
+
+# Django stuff:
+*.log
+local_settings.py
+db.sqlite3
+
+# Flask stuff:
+instance/
+.webassets-cache
+
+# Scrapy stuff:
+.scrapy
+
+# Sphinx documentation
+docs/_build/
+
+# PyBuilder
+target/
+
+# Jupyter Notebook
+.ipynb_checkpoints
+
+# pyenv
+.python-version
+
+# celery beat schedule file
+celerybeat-schedule
+
+# SageMath parsed files
+*.sage.py
+
+# Environments
+.env
+.venv
+env/
+venv/
+ENV/
+env.bak/
+venv.bak/
+
+# Spyder project settings
+.spyderproject
+.spyproject
+
+# Rope project settings
+.ropeproject
+
+# mkdocs documentation
+/site
+
+# mypy
+.mypy_cache/
diff --git a/PythonClient/ros/airsim_ros_client/CMakeLists.txt b/PythonClient/ros/airsim_ros_client/CMakeLists.txt
new file mode 100644
index 0000000000..b545c65966
--- /dev/null
+++ b/PythonClient/ros/airsim_ros_client/CMakeLists.txt
@@ -0,0 +1,31 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(airsim_ros_client)
+
+## Find catkin macros and libraries
+find_package(catkin REQUIRED COMPONENTS
+ rospy
+ sensor_msgs
+ geometry_msgs
+ nav_msgs
+ tf2_ros
+)
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+catkin_package(
+# INCLUDE_DIRS include
+# LIBRARIES airsim_python_wrapper
+# CATKIN_DEPENDS rospy sensor_msgs geometry_msgs nav_msgs tf2_ros
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+include_directories(
+ ${catkin_INCLUDE_DIRS}
+)
diff --git a/PythonClient/ros/airsim_ros_client/README.md b/PythonClient/ros/airsim_ros_client/README.md
new file mode 100644
index 0000000000..2c91837e42
--- /dev/null
+++ b/PythonClient/ros/airsim_ros_client/README.md
@@ -0,0 +1,115 @@
+# airsim_ros_client
+
+## Overview
+
+This repository is meant to integrate ROS and AirSim plugin using the python APIs available for the simulator.
+
+The `airsim_ros_client` package has been tested under ROS Kinetic and Ubuntu 16.04LTS. The source code is released under [MIT Licence](LICENSE).
+
+## Installation
+
+#### Dependencies
+
+Install the python depenencies:
+```bash
+# AirSim APIs
+pip install airsim
+```
+
+#### Building
+* To build from source, clone the latest version from this repository into your catkin workspace
+```bash
+mkdir -p ~/catkin_ws/src
+cd ~/catkin_ws/src
+git clone https://github.com/Mayankm96/airsim_ros_client.git
+```
+* To compile the package:
+```bash
+cd ~/catkin_ws
+catkin_make
+source devel/setup.bash
+```
+
+#### Running AirSim in Unreal Engine
+
+Before running the nodes in the package, you need to run Airsim plugin in the Unreal Engine. In case you are unfamiliar on how to do so, refer to the tutorials available [here](https://github.com/Microsoft/AirSim#tutorials).
+
+A sample `settings.json` file used to run the simulator with the ROS package is available [here](docs/settings.json). Copy it to the `~/Documents/AirSim` directory to use the package without any further modifications.
+
+## Usage
+
+### Running the `tf` publisher of drone model (DJI M100)
+
+To use the [`urdf`](urdf) model of the drone used in AirSim simulator, then run:
+```
+roslaunch airsim_ros_client publish_tf.launch
+```
+
+__NOTE:__ In the modified blueprint of the drone for UE4, all cameras are downward-facing.
+
+### Running image publisher
+
+Change the IP and Port configurations in [`pubImages.launch`](launch/pubImages.launch) to match the settings in which Airsim is running. Then:
+```
+roslaunch airsim_ros_client pubImages.launch
+```
+
+## Nodes
+
+### airsim_img_publisher
+
+This is a client node at ([`img_publisher.py`](scripts/img_publisher.py)) interfaces with the AirSim plugin to retrieve the drone's pose and camera images **(rgb, depth)**.
+
+#### Published Topics
+
+* **`/airsim/rgb/image_raw`** ([sensor_msgs/Image])
+
+ The rgb camera images in `rgba8` encoding.
+
+* **`/airsim/depth`** ([sensor_msgs/Image])
+
+ The depth camera images in `32FC1` encoding.
+
+* **`/airsim/camera_info`** ([sensor_msgs/CameraInfo])
+
+ The rgb camera paramters.
+
+* **`/airsim/depth/camera_info`** ([sensor_msgs/CameraInfo])
+
+ The depth camera paramters.
+
+* **`/airsim/pose`** ([geometry_msgs/PoseStamped])
+
+ The position/orientation of the quadcoper (`base_link`)
+
+* **`/odom`** ([nav_msgs/Odometry])
+
+ The odometry of the quadcoper (`base_link`) in the `world` frame
+
+* **`/tf`**
+
+ tf tree with the origin (`world`), the position/orientation of the quadcoper (`base_link`)
+
+
+#### Parameters
+* **Camera parameters:** `Fx`, `Fy`, `cx`, `cz`, `width`, `height`
+* **Publishing frequency:** `loop_rate`
+
+### airsim_follow_trajectory
+
+This is a client node at ([`follow_trajectory.py`](scripts/follow_trajectory.py)) interfaces with the AirSim plugin to follow a trajectory.
+
+#### Subscribed Topics
+
+* **`/trajectory/spline_marker_array`** ([visualization_msgs/MarkerArray])
+
+ An array of waypoints to follow.
+
+#### Parameters
+* **Velocity:** `velocity`
+
+[sensor_msgs/Image]: http://docs.ros.org/api/sensor_msgs/html/msg/Image.html
+[sensor_msgs/CameraInfo]: http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html
+[geometry_msgs/PoseStamped]: http://docs.ros.org/melodic/api/geometry_msgs/html/msg/PoseStamped.html
+[nav_msgs/Odometry]: http://docs.ros.org/melodic/api/nav_msgs/html/msg/Odometry.html
+[visualization_msgs/MarkerArray]: http://docs.ros.org/melodic/api/visualization_msgs/html/msg/MarkerArray.html/Odometry.html
diff --git a/PythonClient/ros/airsim_ros_client/docs/dji_m100.png b/PythonClient/ros/airsim_ros_client/docs/dji_m100.png
new file mode 100644
index 0000000000..191b526b20
Binary files /dev/null and b/PythonClient/ros/airsim_ros_client/docs/dji_m100.png differ
diff --git a/PythonClient/ros/airsim_ros_client/docs/settings.json b/PythonClient/ros/airsim_ros_client/docs/settings.json
new file mode 100644
index 0000000000..24e00ef99a
--- /dev/null
+++ b/PythonClient/ros/airsim_ros_client/docs/settings.json
@@ -0,0 +1,90 @@
+{
+ "SettingsVersion": 1.2,
+ "DefaultVehicleConfig": "",
+ "SimMode": "Multirotor",
+ "RecordUIVisible": true,
+ "LogMessagesVisible": true,
+ "ViewMode": "",
+ "RpcEnabled": true,
+ "PhysicsEngineName": "",
+ "EnableCollisionPassthrogh": true,
+ "CameraDefaults": {
+ "CaptureSettings": [
+ {
+ "ImageType": 0,
+ "Width": 640,
+ "Height": 360,
+ "FOV_Degrees": 90,
+ "AutoExposureSpeed": 100,
+ "AutoExposureBias": 0,
+ "AutoExposureMaxBrightness": 0.03,
+ "AutoExposureMinBrightness": 0.03
+ },
+ {
+ "ImageType": 1,
+ "Width": 640,
+ "Height": 360,
+ "FOV_Degrees": 90,
+ "AutoExposureSpeed": 100,
+ "AutoExposureBias": 0,
+ "AutoExposureMaxBrightness": 0.03,
+ "AutoExposureMinBrightness": 0.03
+ },
+ {
+ "ImageType": 2,
+ "Width": 640,
+ "Height": 360,
+ "FOV_Degrees": 90,
+ "AutoExposureSpeed": 100,
+ "AutoExposureBias": 0,
+ "AutoExposureMaxBrightness": 0.03,
+ "AutoExposureMinBrightness": 0.03
+ },
+ {
+ "ImageType": 3,
+ "Width": 640,
+ "Height": 360,
+ "FOV_Degrees": 90,
+ "AutoExposureSpeed": 100,
+ "AutoExposureBias": 0,
+ "AutoExposureMaxBrightness": 0.03,
+ "AutoExposureMinBrightness": 0.03
+ },
+ {
+ "ImageType": 4,
+ "Width": 640,
+ "Height": 360,
+ "FOV_Degrees": 90,
+ "AutoExposureSpeed": 100,
+ "AutoExposureBias": 0,
+ "AutoExposureMaxBrightness": 0.03,
+ "AutoExposureMinBrightness": 0.03
+ },
+ {
+ "ImageType": 5,
+ "Width": 640,
+ "Height": 360,
+ "FOV_Degrees": 90,
+ "AutoExposureSpeed": 100,
+ "AutoExposureBias": 0,
+ "AutoExposureMaxBrightness": 0.03,
+ "AutoExposureMinBrightness": 0.03
+ },
+ {
+ "ImageType": 6,
+ "Width": 640,
+ "Height": 360,
+ "FOV_Degrees": 90,
+ "AutoExposureSpeed": 100,
+ "AutoExposureBias": 0,
+ "AutoExposureMaxBrightness": 0.03,
+ "AutoExposureMinBrightness": 0.03
+ }
+ ]
+ },
+ "SubWindows": [
+ {"WindowID": 0, "CameraID": 0, "ImageType": 3, "Visible": true},
+ {"WindowID": 1, "CameraID": 0, "ImageType": 5, "Visible": true},
+ {"WindowID": 2, "CameraID": 0, "ImageType": 0, "Visible": true}
+ ]
+}
diff --git a/PythonClient/ros/airsim_ros_client/launch/include/octomap.launch b/PythonClient/ros/airsim_ros_client/launch/include/octomap.launch
new file mode 100644
index 0000000000..1b94ce52b8
--- /dev/null
+++ b/PythonClient/ros/airsim_ros_client/launch/include/octomap.launch
@@ -0,0 +1,34 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/PythonClient/ros/airsim_ros_client/launch/include/rgbPointCloudConverter.launch b/PythonClient/ros/airsim_ros_client/launch/include/rgbPointCloudConverter.launch
new file mode 100644
index 0000000000..3683e566bb
--- /dev/null
+++ b/PythonClient/ros/airsim_ros_client/launch/include/rgbPointCloudConverter.launch
@@ -0,0 +1,21 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/PythonClient/ros/airsim_ros_client/launch/include/rviz_display.launch b/PythonClient/ros/airsim_ros_client/launch/include/rviz_display.launch
new file mode 100644
index 0000000000..aa993d3f0a
--- /dev/null
+++ b/PythonClient/ros/airsim_ros_client/launch/include/rviz_display.launch
@@ -0,0 +1,14 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/PythonClient/ros/airsim_ros_client/launch/pubColoredPointCloud.launch b/PythonClient/ros/airsim_ros_client/launch/pubColoredPointCloud.launch
new file mode 100644
index 0000000000..0e1731dcb6
--- /dev/null
+++ b/PythonClient/ros/airsim_ros_client/launch/pubColoredPointCloud.launch
@@ -0,0 +1,9 @@
+
+
+
+
+
+
+
+
+
diff --git a/PythonClient/ros/airsim_ros_client/launch/pubImages.launch b/PythonClient/ros/airsim_ros_client/launch/pubImages.launch
new file mode 100644
index 0000000000..d64bbcc26b
--- /dev/null
+++ b/PythonClient/ros/airsim_ros_client/launch/pubImages.launch
@@ -0,0 +1,22 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/PythonClient/ros/airsim_ros_client/launch/pubPointCloud.launch b/PythonClient/ros/airsim_ros_client/launch/pubPointCloud.launch
new file mode 100644
index 0000000000..3a55ee8e45
--- /dev/null
+++ b/PythonClient/ros/airsim_ros_client/launch/pubPointCloud.launch
@@ -0,0 +1,36 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/PythonClient/ros/airsim_ros_client/launch/utils/create_urdf.launch b/PythonClient/ros/airsim_ros_client/launch/utils/create_urdf.launch
new file mode 100644
index 0000000000..86a30eceb8
--- /dev/null
+++ b/PythonClient/ros/airsim_ros_client/launch/utils/create_urdf.launch
@@ -0,0 +1,10 @@
+
+
+
+
+
+
+
+
+
+
diff --git a/PythonClient/ros/airsim_ros_client/launch/utils/publish_tf.launch b/PythonClient/ros/airsim_ros_client/launch/utils/publish_tf.launch
new file mode 100644
index 0000000000..bab483caf6
--- /dev/null
+++ b/PythonClient/ros/airsim_ros_client/launch/utils/publish_tf.launch
@@ -0,0 +1,14 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/PythonClient/ros/airsim_ros_client/launch/utils/recordBag.launch b/PythonClient/ros/airsim_ros_client/launch/utils/recordBag.launch
new file mode 100644
index 0000000000..f05e3100a1
--- /dev/null
+++ b/PythonClient/ros/airsim_ros_client/launch/utils/recordBag.launch
@@ -0,0 +1,13 @@
+
+
+
+
+
+
+
+
+
diff --git a/PythonClient/ros/airsim_ros_client/meshes/DJI_M100_CAD.STL b/PythonClient/ros/airsim_ros_client/meshes/DJI_M100_CAD.STL
new file mode 100644
index 0000000000..3646f652b9
Binary files /dev/null and b/PythonClient/ros/airsim_ros_client/meshes/DJI_M100_CAD.STL differ
diff --git a/PythonClient/ros/airsim_ros_client/package.xml b/PythonClient/ros/airsim_ros_client/package.xml
new file mode 100644
index 0000000000..2e88ca8287
--- /dev/null
+++ b/PythonClient/ros/airsim_ros_client/package.xml
@@ -0,0 +1,31 @@
+
+
+ airsim_ros_client
+ 0.0.1
+ The airsim_ros_client package integrates ROS and AirSim plugin using the python APIs available for the simulator.
+
+ Mayank Mittal
+
+ MIT
+
+ catkin
+
+ rospy
+ sensor_msgs
+ geometry_msgs
+ nav_msgs
+ tf2_ros
+
+ rospy
+ sensor_msgs
+ geometry_msgs
+ nav_msgs
+ tf2_ros
+
+ rospy
+ sensor_msgs
+ geometry_msgs
+ nav_msgs
+ tf2_ros
+
+
diff --git a/PythonClient/ros/airsim_ros_client/rviz/octomapConfig.rviz b/PythonClient/ros/airsim_ros_client/rviz/octomapConfig.rviz
new file mode 100644
index 0000000000..a6d88c39f6
--- /dev/null
+++ b/PythonClient/ros/airsim_ros_client/rviz/octomapConfig.rviz
@@ -0,0 +1,249 @@
+Panels:
+ - Class: rviz/Displays
+ Help Height: 0
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /TF1/Frames1
+ Splitter Ratio: 0.5
+ Tree Height: 245
+ - Class: rviz/Selection
+ Name: Selection
+ - Class: rviz/Tool Properties
+ Expanded:
+ - /2D Pose Estimate1
+ - /2D Nav Goal1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.588679016
+ - Class: rviz/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz/Time
+ Experimental: false
+ Name: Time
+ SyncMode: 0
+ SyncSource: Image
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.0299999993
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 10
+ Reference Frame:
+ Value: true
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 17.6900005
+ Min Value: 1.9690001
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/PointCloud2
+ Color: 255; 255; 255
+ Color Transformer: Intensity
+ Decay Time: 0
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Max Intensity: 4096
+ Min Color: 0; 0; 0
+ Min Intensity: 0
+ Name: PointCloud2
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.0500000007
+ Style: Flat Squares
+ Topic: /airsim/points
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ - Class: rviz/Image
+ Enabled: true
+ Image Topic: /airsim/depth
+ Max Value: 1
+ Median window: 5
+ Min Value: 0
+ Name: Image
+ Normalize Range: true
+ Queue Size: 2
+ Transport Hint: raw
+ Unreliable: false
+ Value: true
+ - Angle Tolerance: 0.100000001
+ Class: rviz/Odometry
+ Covariance:
+ Orientation:
+ Alpha: 0.5
+ Color: 255; 255; 127
+ Color Style: Unique
+ Frame: Local
+ Offset: 1
+ Scale: 1
+ Value: true
+ Position:
+ Alpha: 0.300000012
+ Color: 204; 51; 204
+ Scale: 1
+ Value: true
+ Value: true
+ Enabled: true
+ Keep: 100
+ Name: Odometry
+ Position Tolerance: 0.100000001
+ Shape:
+ Alpha: 1
+ Axes Length: 1
+ Axes Radius: 0.100000001
+ Color: 255; 25; 0
+ Head Length: 0.300000012
+ Head Radius: 0.100000001
+ Shaft Length: 1
+ Shaft Radius: 0.0500000007
+ Value: Arrow
+ Topic: /odom
+ Unreliable: false
+ Value: true
+ - Class: rviz/Image
+ Enabled: true
+ Image Topic: /airsim/rgb/image_raw
+ Max Value: 1
+ Median window: 5
+ Min Value: 0
+ Name: Image
+ Normalize Range: true
+ Queue Size: 2
+ Transport Hint: raw
+ Unreliable: false
+ Value: true
+ - Class: rviz/TF
+ Enabled: true
+ Frame Timeout: 15
+ Frames:
+ All Enabled: false
+ base_link:
+ Value: true
+ bottom_back_optical:
+ Value: false
+ bottom_center_optical:
+ Value: false
+ front_center_optical:
+ Value: true
+ front_left_optical:
+ Value: false
+ front_right_optical:
+ Value: false
+ world:
+ Value: true
+ Marker Scale: 5
+ Name: TF
+ Show Arrows: true
+ Show Axes: true
+ Show Names: true
+ Tree:
+ world:
+ base_link:
+ bottom_back_optical:
+ {}
+ bottom_center_optical:
+ {}
+ front_center_optical:
+ {}
+ front_left_optical:
+ {}
+ front_right_optical:
+ {}
+ Update Interval: 0
+ Value: true
+ - Class: rviz/MarkerArray
+ Enabled: true
+ Marker Topic: /occupied_cells_vis_array
+ Name: MarkerArray
+ Namespaces:
+ {}
+ Queue Size: 100
+ Value: true
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Default Light: true
+ Fixed Frame: world
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz/Interact
+ Hide Inactive Objects: true
+ - Class: rviz/MoveCamera
+ - Class: rviz/Select
+ - Class: rviz/FocusCamera
+ - Class: rviz/Measure
+ - Class: rviz/SetInitialPose
+ Topic: /initialpose
+ - Class: rviz/SetGoal
+ Topic: /move_base_simple/goal
+ - Class: rviz/PublishPoint
+ Single click: true
+ Topic: /clicked_point
+ Value: true
+ Views:
+ Current:
+ Class: rviz/Orbit
+ Distance: 14.8764544
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.0599999987
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Focal Point:
+ X: 0
+ Y: 0
+ Z: 0
+ Focal Shape Fixed Size: false
+ Focal Shape Size: 0.0500000007
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.00999999978
+ Pitch: 0.519796729
+ Target Frame: base_link
+ Value: Orbit (rviz)
+ Yaw: 6.12357378
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 936
+ Hide Left Dock: false
+ Hide Right Dock: false
+ Image:
+ collapsed: false
+ QMainWindow State: 000000ff00000000fd0000000400000000000001f0000002fdfc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000420000013b000000dc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000183000000e60000001800fffffffb0000000a0049006d006100670065010000026f000000d00000001800ffffff000000010000010f000002fdfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000042000002fd000000b500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000042fc0100000002fb0000000800540069006d00650100000000000007800000027000fffffffb0000000800540069006d0065010000000000000450000000000000000000000475000002fd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: false
+ Width: 1920
+ X: 0
+ Y: 27
diff --git a/PythonClient/ros/airsim_ros_client/rviz/pclConfig.rviz b/PythonClient/ros/airsim_ros_client/rviz/pclConfig.rviz
new file mode 100644
index 0000000000..7bd64104e8
--- /dev/null
+++ b/PythonClient/ros/airsim_ros_client/rviz/pclConfig.rviz
@@ -0,0 +1,242 @@
+Panels:
+ - Class: rviz/Displays
+ Help Height: 75
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Odometry1/Status1
+ - /TF1/Frames1
+ Splitter Ratio: 0.5
+ Tree Height: 230
+ - Class: rviz/Selection
+ Name: Selection
+ - Class: rviz/Tool Properties
+ Expanded:
+ - /2D Pose Estimate1
+ - /2D Nav Goal1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.588679016
+ - Class: rviz/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz/Time
+ Experimental: false
+ Name: Time
+ SyncMode: 0
+ SyncSource: Image
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.0299999993
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 10
+ Reference Frame:
+ Value: true
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 17.6900005
+ Min Value: 1.9690001
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/PointCloud2
+ Color: 255; 255; 255
+ Color Transformer: RGB8
+ Decay Time: 0
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Max Intensity: 4096
+ Min Color: 0; 0; 0
+ Min Intensity: 0
+ Name: PointCloud2
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.0500000007
+ Style: Flat Squares
+ Topic: /airsim/points/colored
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ - Class: rviz/Image
+ Enabled: true
+ Image Topic: /airsim/rgb/image_raw
+ Max Value: 1
+ Median window: 5
+ Min Value: 0
+ Name: Image
+ Normalize Range: true
+ Queue Size: 2
+ Transport Hint: raw
+ Unreliable: false
+ Value: true
+ - Angle Tolerance: 0.100000001
+ Class: rviz/Odometry
+ Covariance:
+ Orientation:
+ Alpha: 0.5
+ Color: 255; 255; 127
+ Color Style: Unique
+ Frame: Local
+ Offset: 1
+ Scale: 1
+ Value: true
+ Position:
+ Alpha: 0.300000012
+ Color: 204; 51; 204
+ Scale: 1
+ Value: true
+ Value: true
+ Enabled: true
+ Keep: 100
+ Name: Odometry
+ Position Tolerance: 0.100000001
+ Shape:
+ Alpha: 1
+ Axes Length: 1
+ Axes Radius: 0.100000001
+ Color: 255; 25; 0
+ Head Length: 0.300000012
+ Head Radius: 0.100000001
+ Shaft Length: 1
+ Shaft Radius: 0.0500000007
+ Value: Arrow
+ Topic: /odom
+ Unreliable: false
+ Value: true
+ - Class: rviz/Image
+ Enabled: true
+ Image Topic: /airsim/depth
+ Max Value: 1
+ Median window: 5
+ Min Value: 0
+ Name: Image
+ Normalize Range: true
+ Queue Size: 2
+ Transport Hint: raw
+ Unreliable: false
+ Value: true
+ - Class: rviz/TF
+ Enabled: true
+ Frame Timeout: 15
+ Frames:
+ All Enabled: false
+ base_link:
+ Value: true
+ bottom_back_optical:
+ Value: false
+ bottom_center_optical:
+ Value: false
+ front_center_optical:
+ Value: true
+ front_left_optical:
+ Value: false
+ front_right_optical:
+ Value: false
+ world:
+ Value: true
+ Marker Scale: 1
+ Name: TF
+ Show Arrows: true
+ Show Axes: true
+ Show Names: true
+ Tree:
+ world:
+ base_link:
+ bottom_back_optical:
+ {}
+ bottom_center_optical:
+ {}
+ front_center_optical:
+ {}
+ front_left_optical:
+ {}
+ front_right_optical:
+ {}
+ Update Interval: 0
+ Value: true
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Default Light: true
+ Fixed Frame: world
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz/Interact
+ Hide Inactive Objects: true
+ - Class: rviz/MoveCamera
+ - Class: rviz/Select
+ - Class: rviz/FocusCamera
+ - Class: rviz/Measure
+ - Class: rviz/SetInitialPose
+ Topic: /initialpose
+ - Class: rviz/SetGoal
+ Topic: /move_base_simple/goal
+ - Class: rviz/PublishPoint
+ Single click: true
+ Topic: /clicked_point
+ Value: true
+ Views:
+ Current:
+ Class: rviz/Orbit
+ Distance: 200.377228
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.0599999987
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Focal Point:
+ X: 0
+ Y: 0
+ Z: 0
+ Focal Shape Fixed Size: false
+ Focal Shape Size: 0.0500000007
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.00999999978
+ Pitch: 0.499797285
+ Target Frame:
+ Value: Orbit (rviz)
+ Yaw: 2.06539917
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 1056
+ Hide Left Dock: false
+ Hide Right Dock: false
+ Image:
+ collapsed: false
+ QMainWindow State: 000000ff00000000fd00000004000000000000019100000396fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000172000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001a00000010e0000001600fffffffb0000000a0049006d00610067006501000002b40000010a0000001600ffffff000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004930000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: false
+ Width: 1855
+ X: 65
+ Y: 24
diff --git a/PythonClient/ros/airsim_ros_client/rviz/urdf.rviz b/PythonClient/ros/airsim_ros_client/rviz/urdf.rviz
new file mode 100644
index 0000000000..8bbbea0049
--- /dev/null
+++ b/PythonClient/ros/airsim_ros_client/rviz/urdf.rviz
@@ -0,0 +1,192 @@
+Panels:
+ - Class: rviz/Displays
+ Help Height: 78
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Global Options1
+ - /Status1
+ - /RobotModel1
+ - /TF1
+ Splitter Ratio: 0.5
+ Tree Height: 769
+ - Class: rviz/Selection
+ Name: Selection
+ - Class: rviz/Tool Properties
+ Expanded:
+ - /2D Pose Estimate1
+ - /2D Nav Goal1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.588679016
+ - Class: rviz/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz/Time
+ Experimental: false
+ Name: Time
+ SyncMode: 0
+ SyncSource: ""
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.0299999993
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 10
+ Reference Frame:
+ Value: true
+ - Alpha: 0.5
+ Class: rviz/RobotModel
+ Collision Enabled: false
+ Enabled: true
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ base_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ bottom_back_optical:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ bottom_center_optical:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ front_center_optical:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ front_left_optical:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ front_right_optical:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Name: RobotModel
+ Robot Description: robot_description
+ TF Prefix: ""
+ Update Interval: 0
+ Value: true
+ Visual Enabled: true
+ - Class: rviz/TF
+ Enabled: true
+ Frame Timeout: 15
+ Frames:
+ All Enabled: true
+ base_link:
+ Value: true
+ bottom_back_optical:
+ Value: true
+ bottom_center_optical:
+ Value: true
+ front_center_optical:
+ Value: true
+ front_left_optical:
+ Value: true
+ front_right_optical:
+ Value: true
+ Marker Scale: 0.5
+ Name: TF
+ Show Arrows: true
+ Show Axes: true
+ Show Names: true
+ Tree:
+ base_link:
+ bottom_back_optical:
+ {}
+ bottom_center_optical:
+ {}
+ front_center_optical:
+ {}
+ front_left_optical:
+ {}
+ front_right_optical:
+ {}
+ Update Interval: 0
+ Value: true
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Default Light: true
+ Fixed Frame: base_link
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz/Interact
+ Hide Inactive Objects: true
+ - Class: rviz/MoveCamera
+ - Class: rviz/Select
+ - Class: rviz/FocusCamera
+ - Class: rviz/Measure
+ - Class: rviz/SetInitialPose
+ Topic: /initialpose
+ - Class: rviz/SetGoal
+ Topic: /move_base_simple/goal
+ - Class: rviz/PublishPoint
+ Single click: true
+ Topic: /clicked_point
+ Value: true
+ Views:
+ Current:
+ Class: rviz/Orbit
+ Distance: 1.99444067
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.0599999987
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Focal Point:
+ X: 0
+ Y: 0
+ Z: 0
+ Focal Shape Fixed Size: true
+ Focal Shape Size: 0.0500000007
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.00999999978
+ Pitch: 1.03979683
+ Target Frame:
+ Value: Orbit (rviz)
+ Yaw: 5.53676701
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 1056
+ Hide Left Dock: false
+ Hide Right Dock: false
+ QMainWindow State: 000000ff00000000fd00000004000000000000016a00000390fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000390000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000390fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000390000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f00000044fc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004ba0000039000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: false
+ Width: 1855
+ X: 65
+ Y: 24
diff --git a/PythonClient/ros/airsim_ros_client/scripts/follow_trajectory.py b/PythonClient/ros/airsim_ros_client/scripts/follow_trajectory.py
new file mode 100644
index 0000000000..66ae55d524
--- /dev/null
+++ b/PythonClient/ros/airsim_ros_client/scripts/follow_trajectory.py
@@ -0,0 +1,59 @@
+#!/usr/bin/env python
+
+# airsim
+import setup_path
+import airsim
+# standard python
+import math
+import sys
+import numpy as np
+# ROS
+import rospy
+import tf2_ros
+# ROS messages
+from visualization_msgs.msg import MarkerArray
+from geometry_msgs.msg import Point
+from geometry_msgs.msg import Pose
+
+velocity = 0.5
+
+# connect to the AirSim simulator
+client = airsim.MultirotorClient()
+client.confirmConnection()
+client.enableApiControl(True)
+
+def trajectory_callback(spline_marker_array_msg):
+ # follow the last published trajectory
+ last_traj_num = len(spline_marker_array_msg.markers)
+ following_path = []
+
+ global velocity
+
+ for pose in spline_marker_array_msg.markers[last_traj_num-1].points:
+ # add point to trajectory queue
+ print("[NED] Adding point (%f, %f, %f)" % (pose.x, - pose.y, - pose.z))
+ following_path.append(airsim.Vector3r(pose.x, - pose.y, - pose.z))
+
+ client.moveOnPathAsync(following_path, velocity, 3e+38, airsim.DrivetrainType.ForwardOnly, airsim.YawMode(False,0), -1, 1).join()
+
+ client.enableApiControl(False)
+
+def airsim_follow():
+ ## Start ROS ---------------------------------------------------------------
+ rospy.init_node('airsim_follow_trajectory')
+
+ ## Parameters
+ global velocity
+ velocity = rospy.get_param("/velocity", 0.5)
+
+ ## Subscribers --------------------------------------------------------------
+ rospy.Subscriber("/trajectory/spline_marker_array", MarkerArray, trajectory_callback)
+
+ rospy.spin()
+
+if __name__ == '__main__':
+ try:
+ airsim_follow()
+ except rospy.ROSInterruptException:
+ pass
+
diff --git a/PythonClient/ros/airsim_ros_client/scripts/img_publisher.py b/PythonClient/ros/airsim_ros_client/scripts/img_publisher.py
new file mode 100644
index 0000000000..97394f4703
--- /dev/null
+++ b/PythonClient/ros/airsim_ros_client/scripts/img_publisher.py
@@ -0,0 +1,270 @@
+#!/usr/bin/env python
+
+# airsim
+import setup_path
+import airsim
+# standard python
+import math
+import sys
+import numpy as np
+from numpy.lib.stride_tricks import as_strided
+# ROS
+import rospy
+import tf2_ros
+# ROS Image message
+from sensor_msgs.msg import Image
+from sensor_msgs.msg import CameraInfo
+from nav_msgs.msg import Odometry
+from geometry_msgs.msg import PoseStamped
+from geometry_msgs.msg import TransformStamped
+
+# Source: https://github.com/eric-wieser/ros_numpy/blob/master/src/ros_numpy/image.py
+name_to_dtypes = {
+ "rgb8": (np.uint8, 3),
+ "rgba8": (np.uint8, 4),
+ "rgb16": (np.uint16, 3),
+ "rgba16": (np.uint16, 4),
+ "bgr8": (np.uint8, 3),
+ "bgra8": (np.uint8, 4),
+ "bgr16": (np.uint16, 3),
+ "bgra16": (np.uint16, 4),
+ "mono8": (np.uint8, 1),
+ "mono16": (np.uint16, 1),
+
+ # for bayer image (based on cv_bridge.cpp)
+ "bayer_rggb8": (np.uint8, 1),
+ "bayer_bggr8": (np.uint8, 1),
+ "bayer_gbrg8": (np.uint8, 1),
+ "bayer_grbg8": (np.uint8, 1),
+ "bayer_rggb16": (np.uint16, 1),
+ "bayer_bggr16": (np.uint16, 1),
+ "bayer_gbrg16": (np.uint16, 1),
+ "bayer_grbg16": (np.uint16, 1),
+
+ # OpenCV CvMat types
+ "8UC1": (np.uint8, 1),
+ "8UC2": (np.uint8, 2),
+ "8UC3": (np.uint8, 3),
+ "8UC4": (np.uint8, 4),
+ "8SC1": (np.int8, 1),
+ "8SC2": (np.int8, 2),
+ "8SC3": (np.int8, 3),
+ "8SC4": (np.int8, 4),
+ "16UC1": (np.int16, 1),
+ "16UC2": (np.int16, 2),
+ "16UC3": (np.int16, 3),
+ "16UC4": (np.int16, 4),
+ "16SC1": (np.uint16, 1),
+ "16SC2": (np.uint16, 2),
+ "16SC3": (np.uint16, 3),
+ "16SC4": (np.uint16, 4),
+ "32SC1": (np.int32, 1),
+ "32SC2": (np.int32, 2),
+ "32SC3": (np.int32, 3),
+ "32SC4": (np.int32, 4),
+ "32FC1": (np.float32, 1),
+ "32FC2": (np.float32, 2),
+ "32FC3": (np.float32, 3),
+ "32FC4": (np.float32, 4),
+ "64FC1": (np.float64, 1),
+ "64FC2": (np.float64, 2),
+ "64FC3": (np.float64, 3),
+ "64FC4": (np.float64, 4)
+}
+
+def numpy_to_image(arr, encoding):
+ if not encoding in name_to_dtypes:
+ raise TypeError('Unrecognized encoding {}'.format(encoding))
+
+ im = Image(encoding=encoding)
+
+ # extract width, height, and channels
+ dtype_class, exp_channels = name_to_dtypes[encoding]
+ dtype = np.dtype(dtype_class)
+ if len(arr.shape) == 2:
+ im.height, im.width, channels = arr.shape + (1,)
+ elif len(arr.shape) == 3:
+ im.height, im.width, channels = arr.shape
+ else:
+ raise TypeError("Array must be two or three dimensional")
+
+ # check type and channels
+ if exp_channels != channels:
+ raise TypeError("Array has {} channels, {} requires {}".format(
+ channels, encoding, exp_channels
+ ))
+ if dtype_class != arr.dtype.type:
+ raise TypeError("Array is {}, {} requires {}".format(
+ arr.dtype.type, encoding, dtype_class
+ ))
+
+ # make the array contiguous in memory, as mostly required by the format
+ contig = np.ascontiguousarray(arr)
+ im.data = contig.tostring()
+ im.step = contig.strides[0]
+ im.is_bigendian = (
+ arr.dtype.byteorder == '>' or
+ arr.dtype.byteorder == '=' and sys.byteorder == 'big'
+ )
+
+ return im
+
+def publish_tf_msg(sim_pose_msg):
+ br = tf2_ros.TransformBroadcaster()
+
+ t = TransformStamped()
+ # populate tf ros message
+ t.header.stamp = sim_pose_msg.header.stamp
+ t.header.frame_id = "world"
+ t.child_frame_id = "base_link"
+ t.transform.translation.x = sim_pose_msg.pose.position.x
+ t.transform.translation.y = sim_pose_msg.pose.position.y
+ t.transform.translation.z = sim_pose_msg.pose.position.z
+ t.transform.rotation.x = sim_pose_msg.pose.orientation.x
+ t.transform.rotation.y = sim_pose_msg.pose.orientation.y
+ t.transform.rotation.z = sim_pose_msg.pose.orientation.z
+ t.transform.rotation.w = sim_pose_msg.pose.orientation.w
+
+ br.sendTransform(t)
+
+def get_camera_params():
+ # read parameters
+ width = rospy.get_param('~width', 640)
+ height = rospy.get_param('~height', 360)
+ Fx = rospy.get_param('~Fx', 320)
+ Fy = rospy.get_param('~Fy', 320)
+ Cx = rospy.get_param('~Cx', 320)
+ Cy = rospy.get_param('~Cy', 180)
+
+ # create sensor message
+ camera_info_msg = CameraInfo()
+ camera_info_msg.distortion_model = 'plumb_bob'
+ camera_info_msg.width = width
+ camera_info_msg.height = height
+ camera_info_msg.K = [Fx, 0, Cx,
+ 0, Fy, Cy,
+ 0, 0, 1]
+ camera_info_msg.D = [0, 0, 0, 0]
+
+ camera_info_msg.P = [Fx, 0, Cx, 0,
+ 0, Fy, Cy, 0,
+ 0, 0, 1, 0]
+ camera_info_msg.header.frame_id = "front_center_optical"
+ return camera_info_msg
+
+def convert_ned_to_enu(pos_ned, orientation_ned):
+ pos_enu = airsim.Vector3r( pos_ned.x_val,
+ - pos_ned.y_val,
+ - pos_ned.z_val)
+ orientation_enu = airsim.Quaternionr( orientation_ned.w_val,
+ - orientation_ned.z_val,
+ - orientation_ned.x_val,
+ orientation_ned.y_val)
+ return pos_enu, orientation_ned
+
+def get_sim_pose(client):
+ # get state of the multirotor
+ drone_state = client.getMultirotorState()
+ pos_ned = drone_state.kinematics_estimated.position
+ orientation_ned = drone_state.kinematics_estimated.orientation
+
+ pos, orientation = convert_ned_to_enu(pos_ned, orientation_ned)
+
+ # populate PoseStamped ros message
+ sim_pose_msg = PoseStamped()
+ sim_pose_msg.pose.position.x = pos.x_val
+ sim_pose_msg.pose.position.y = pos.y_val
+ sim_pose_msg.pose.position.z = pos.z_val
+ sim_pose_msg.pose.orientation.w = orientation.w_val
+ sim_pose_msg.pose.orientation.x = orientation.x_val
+ sim_pose_msg.pose.orientation.y = orientation.y_val
+ sim_pose_msg.pose.orientation.z = orientation.z_val
+ sim_pose_msg.header.seq = 1
+ sim_pose_msg.header.frame_id = "world"
+
+ return sim_pose_msg
+
+def convert_posestamped_to_odom_msg(sim_pose_msg):
+ # populate odom ros message
+ odom_msg = Odometry()
+ odom_msg.pose.pose = sim_pose_msg.pose
+ odom_msg.header.frame_id = "world"
+ odom_msg.child_frame_id = "base_link"
+
+ return odom_msg
+
+def get_image_messages(client):
+ # get camera images from the multirotor
+ responses = client.simGetImages([
+ airsim.ImageRequest("0", airsim.ImageType.Scene, False, False),
+ airsim.ImageRequest("0", airsim.ImageType.DepthPlanner, True)
+ ])
+
+ # convert scene uint8 array to NumPy 2D array using
+ scene_img1d = np.fromstring(responses[0].image_data_uint8, dtype=np.uint8) # get numpy array
+ scene_img_rgba = scene_img1d.reshape(responses[0].height, responses[0].width, 4) # reshape array to image array H X W
+
+ # convert depth float array to NumPy 2D array using
+ depth_img = airsim.list_to_2d_float_array(responses[1].image_data_float, responses[1].width, responses[1].height)
+
+ # Populate image message
+ rgb_msg = numpy_to_image(scene_img_rgba, "rgba8")
+ depth_msg = numpy_to_image(depth_img, "32FC1")
+
+ return rgb_msg, depth_msg
+
+def airpub():
+ ## Start ROS ---------------------------------------------------------------
+ rospy.init_node('airsim_img_publisher', anonymous=False)
+ loop_rate = rospy.get_param('~loop_rate', 10)
+ rate = rospy.Rate(loop_rate)
+
+ ## Publishers --------------------------------------------------------------
+ # image publishers
+ rgb_pub = rospy.Publisher("airsim/rgb/image_raw", Image, queue_size=1)
+ depth_pub = rospy.Publisher("airsim/depth", Image, queue_size=1)
+ # camera paramters publisher
+ rgb_cam_pub = rospy.Publisher("airsim/camera_info", CameraInfo, queue_size=1)
+ depth_cam_pub = rospy.Publisher("airsim/depth/camera_info", CameraInfo, queue_size=1)
+ # odometry publisher
+ odom_pub = rospy.Publisher("odom", Odometry, queue_size=1)
+ # pose publisher
+ pose_pub = rospy.Publisher("airsim/pose", PoseStamped, queue_size=1)
+
+ ## Main --------------------------------------------------------------------
+ # connect to the AirSim simulator
+ client = airsim.MultirotorClient()
+ client.confirmConnection()
+ client.simSetCameraOrientation(0, airsim.to_quaternion(0, 0, 0))
+ # client.simSetCameraOrientation(0, airsim.to_quaternion(-math.pi/2, 0, 0))
+
+ while not rospy.is_shutdown():
+
+ camera_info_msg = get_camera_params()
+ sim_pose_msg = get_sim_pose(client)
+ odom_msg = convert_posestamped_to_odom_msg(sim_pose_msg)
+ rgb_msg, depth_msg = get_image_messages(client)
+
+ # header message
+ sim_pose_msg.header.stamp = rospy.Time.now()
+ odom_msg.header.stamp = sim_pose_msg.header.stamp
+ camera_info_msg.header.stamp = sim_pose_msg.header.stamp
+ rgb_msg.header = camera_info_msg.header
+ depth_msg.header = camera_info_msg.header
+
+ # publish message
+ pose_pub.publish(sim_pose_msg)
+ publish_tf_msg(sim_pose_msg)
+ odom_pub.publish(odom_msg)
+ rgb_cam_pub.publish(camera_info_msg)
+ depth_cam_pub.publish(camera_info_msg)
+ rgb_pub.publish(rgb_msg)
+ depth_pub.publish(depth_msg)
+
+ rate.sleep()
+
+if __name__ == '__main__':
+ try:
+ airpub()
+ except rospy.ROSInterruptException:
+ pass
diff --git a/PythonClient/ros/airsim_ros_client/scripts/setup_path.py b/PythonClient/ros/airsim_ros_client/scripts/setup_path.py
new file mode 100644
index 0000000000..362113fea8
--- /dev/null
+++ b/PythonClient/ros/airsim_ros_client/scripts/setup_path.py
@@ -0,0 +1,52 @@
+# Import this module to automatically setup path to local airsim module
+# This module first tries to see if airsim module is installed via pip
+# If it does then we don't do anything else
+# Else we look up grand-parent folder to see if it has airsim folder
+# and if it does then we add that in sys.path
+
+import os,sys,inspect,logging
+
+#this class simply tries to see if airsim
+class SetupPath:
+ @staticmethod
+ def getDirLevels(path):
+ path_norm = os.path.normpath(path)
+ return len(path_norm.split(os.sep))
+
+ @staticmethod
+ def getCurrentPath():
+ cur_filepath = os.path.abspath(inspect.getfile(inspect.currentframe()))
+ return os.path.dirname(cur_filepath)
+
+ @staticmethod
+ def getGrandParentDir():
+ cur_path = SetupPath.getCurrentPath()
+ if SetupPath.getDirLevels(cur_path) >= 2:
+ return os.path.dirname(os.path.dirname(cur_path))
+ return ''
+
+ @staticmethod
+ def getParentDir():
+ cur_path = SetupPath.getCurrentPath()
+ if SetupPath.getDirLevels(cur_path) >= 1:
+ return os.path.dirname(cur_path)
+ return ''
+
+ @staticmethod
+ def addAirSimModulePath():
+ # if airsim module is installed then don't do anything else
+ #import pkgutil
+ #airsim_loader = pkgutil.find_loader('airsim')
+ #if airsim_loader is not None:
+ # return
+
+ parent = SetupPath.getParentDir()
+ if parent != '':
+ airsim_path = os.path.join(parent, 'airsim')
+ client_path = os.path.join(airsim_path, 'client.py')
+ if os.path.exists(client_path):
+ sys.path.insert(0, parent)
+ else:
+ logging.warning("airsim module not found in parent folder. Using installed package (pip install airsim).")
+
+SetupPath.addAirSimModulePath()
diff --git a/PythonClient/ros/airsim_ros_client/urdf/frame/dji_m100_frame.xacro b/PythonClient/ros/airsim_ros_client/urdf/frame/dji_m100_frame.xacro
new file mode 100644
index 0000000000..e72e640cac
--- /dev/null
+++ b/PythonClient/ros/airsim_ros_client/urdf/frame/dji_m100_frame.xacro
@@ -0,0 +1,43 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/PythonClient/ros/airsim_ros_client/urdf/materials.urdf.xacro b/PythonClient/ros/airsim_ros_client/urdf/materials.urdf.xacro
new file mode 100644
index 0000000000..c76bab392e
--- /dev/null
+++ b/PythonClient/ros/airsim_ros_client/urdf/materials.urdf.xacro
@@ -0,0 +1,36 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/PythonClient/ros/airsim_ros_client/urdf/robot/dji_m100_airsim.urdf b/PythonClient/ros/airsim_ros_client/urdf/robot/dji_m100_airsim.urdf
new file mode 100644
index 0000000000..3529e81897
--- /dev/null
+++ b/PythonClient/ros/airsim_ros_client/urdf/robot/dji_m100_airsim.urdf
@@ -0,0 +1,86 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/PythonClient/ros/airsim_ros_client/urdf/robot/dji_m100_airsim.urdf.xacro b/PythonClient/ros/airsim_ros_client/urdf/robot/dji_m100_airsim.urdf.xacro
new file mode 100644
index 0000000000..59b3f1d38a
--- /dev/null
+++ b/PythonClient/ros/airsim_ros_client/urdf/robot/dji_m100_airsim.urdf.xacro
@@ -0,0 +1,11 @@
+
+
+
+
+
+
+
+
+
+
diff --git a/PythonClient/ros/airsim_ros_client/urdf/sensors/airsim_cam.xacro b/PythonClient/ros/airsim_ros_client/urdf/sensors/airsim_cam.xacro
new file mode 100644
index 0000000000..27e4dbfdf3
--- /dev/null
+++ b/PythonClient/ros/airsim_ros_client/urdf/sensors/airsim_cam.xacro
@@ -0,0 +1,50 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+