GazeboDrone project added to connect a gazebo drone to the AirSim drone

GazeboDrone/CMakeLists.txt

@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+
+project(GazeboDrone)
+
+set(AIRSIM_ROOT ..)
+set(CMAKE_CXX_COMPILER /usr/bin/g++)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 11)
+endif()
+
+find_package(gazebo REQUIRED)
+
+include_directories(
+    ${GAZEBO_INCLUDE_DIRS}
+    ${AIRSIM_ROOT}/AirLib/deps/eigen3
+    ${AIRSIM_ROOT}/AirLib/deps/rpclib/include
+    ${AIRSIM_ROOT}/AirLib/include
+    ${AIRSIM_ROOT}/AirLib/deps/MavLinkCom/include
+)
+
+link_directories(
+    ${GAZEBO_LIBRARY_DIRS}
+    ../../AirLib/lib/x64
+)
+
+list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")
+
+add_executable(${PROJECT_NAME} src/main.cpp)
+
+target_link_libraries(
+    ${PROJECT_NAME} 
+    ${GAZEBO_LIBRARIES}
+    pthread
+    AirLib
+    rpc
+    MavLinkCom
+)
+

GazeboDrone/README.md

@@ -0,0 +1,47 @@
+# General
+
+Derived from http://gazebosim.org/tutorials?tut=topics_subscribed
+
+Make sure you have installed gazebo dependencies
+
+```
+sudo apt-get install libgazebo9-dev
+```
+
+# AirLib build
+
+This project is built with g++, so AirLib needs to be built with g++ too. Change lines 56 and 57 of AirSim/build.sh with:
+```
+export CC="gcc-8"
+export CXX="g++-8"
+```
+then run ./setup.sh and ./build.sh
+
+# AirSim plugin
+
+The AirSim UE plugin needs to be built with clang, so you can't use the one compiled in the previous step. You can use [our binaries](https://github.com/microsoft/AirSim/releases) or you can clone AirSim again in another folder without the above change, and with that one, [run Blocks](https://microsoft.github.io/AirSim/build_linux/#how-to-use-airsim) or your environment.
+
+
+# AirSim settings
+
+Inside your `settings.json` file add this line:  
+`"PhysicsEngineName":"ExternalPhysicsEngine"`
+# Build
+
+```
+mkdir build && cd build
+cmake ..
+make
+```
+
+# Run
+
+Run AirSim and Gazebo
+
+then run:
+
+```
+cd build
+./GazeboDrone
+```
+

GazeboDrone/src/main.cpp

@@ -0,0 +1,133 @@
+/*
+ * Copyright (C) 2012 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+// Copyright (c) Microsoft Corporation. All rights reserved.
+// Licensed under the MIT License.
+
+#include <gazebo/transport/transport.hh>
+#include <gazebo/msgs/msgs.hh>
+#include <gazebo/gazebo_client.hh>
+#include "common/common_utils/StrictMode.hpp"
+STRICT_MODE_OFF
+#ifndef RPCLIB_MSGPACK
+#define RPCLIB_MSGPACK clmdep_msgpack
+#endif // !RPCLIB_MSGPACK
+#include "rpc/rpc_error.h"
+STRICT_MODE_ON
+
+#include "vehicles/multirotor/api/MultirotorRpcLibClient.hpp"
+#include <chrono>
+
+#include <iostream>
+
+constexpr int NWIDTH = 7;
+static constexpr int MESSAGE_THROTTLE = 100;
+
+using namespace msr::airlib;
+
+msr::airlib::MultirotorRpcLibClient client;
+
+void cbLocalPose(ConstPosesStampedPtr& msg)
+{
+    std::cout << std::fixed;
+    std::cout << std::setprecision(3);
+    static int count = 0;
+    for (int i = 0; i < msg->pose_size(); i++) {
+        auto x = msg->pose(i).position().x();
+        auto y = msg->pose(i).position().y();
+        auto z = msg->pose(i).position().z();
+        auto ow = msg->pose(i).orientation().w();
+        auto ox = msg->pose(i).orientation().x();
+        auto oy = msg->pose(i).orientation().y();
+        auto oz = msg->pose(i).orientation().z();
+        if (count % MESSAGE_THROTTLE == 0) {
+            std::cout << "local (" << std::setw(2) << i << ") ";
+            std::cout << std::left << std::setw(32) << msg->pose(i).name();
+            std::cout << " x: " << std::right << std::setw(NWIDTH) << x;
+            std::cout << " y: " << std::right << std::setw(NWIDTH) << y;
+            std::cout << " z: " << std::right << std::setw(NWIDTH) << z;
+
+            std::cout << " ow: " << std::right << std::setw(NWIDTH) << ow;
+            std::cout << " ox: " << std::right << std::setw(NWIDTH) << ox;
+            std::cout << " oy: " << std::right << std::setw(NWIDTH) << oy;
+            std::cout << " oz: " << std::right << std::setw(NWIDTH) << oz;
+            std::cout << std::endl;
+        }
+        if (i == msg->pose_size() - 1) {
+            msr::airlib::Vector3r p(x, -y, -z);
+            msr::airlib::Quaternionr o(ow, ox, -oy, -oz);
+
+            client.simSetVehiclePose(Pose(p, o), true);
+        }
+    }
+    if (count % MESSAGE_THROTTLE == 0) {
+        std::cout << std::endl;
+    }
+
+    ++count;
+}
+
+void cbGobalPose(ConstPosesStampedPtr& msg)
+{
+    std::cout << std::fixed;
+    std::cout << std::setprecision(4);
+    static int count = 0;
+    if (count % MESSAGE_THROTTLE) {
+        ++count;
+        return;
+    }
+    ++count;
+
+    for (int i = 0; i < msg->pose_size(); i++) {
+        std::cout << "global (" << i << ") ";
+        std::cout << std::left << std::setw(32) << msg->pose(i).name();
+        std::cout << " x: " << std::right << std::setfill(' ') << std::setw(NWIDTH) << msg->pose(i).position().x();
+        std::cout << " y: " << std::right << std::setfill(' ') << std::setw(NWIDTH) << msg->pose(i).position().y();
+        std::cout << " z: " << std::right << std::setfill(' ') << std::setw(NWIDTH) << msg->pose(i).position().z();
+
+        std::cout << " ow: " << std::right << std::setfill(' ') << std::setw(NWIDTH) << msg->pose(i).orientation().w();
+        std::cout << " ox: " << std::right << std::setfill(' ') << std::setw(NWIDTH) << msg->pose(i).orientation().x();
+        std::cout << " oy: " << std::right << std::setfill(' ') << std::setw(NWIDTH) << msg->pose(i).orientation().y();
+        std::cout << " oz: " << std::right << std::setfill(' ') << std::setw(NWIDTH) << msg->pose(i).orientation().z();
+        std::cout << std::endl;
+    }
+    std::cout << std::endl;
+}
+
+int main(int _argc, char** _argv)
+{
+
+    client.confirmConnection();
+
+    // Load gazebo
+    gazebo::client::setup(_argc, _argv);
+
+    // Create our node for communication
+    gazebo::transport::NodePtr node(new gazebo::transport::Node());
+    node->Init();
+
+    // Listen to Gazebo topics
+    gazebo::transport::SubscriberPtr sub_pose1 = node->Subscribe("~/pose/local/info", cbLocalPose);
+    gazebo::transport::SubscriberPtr sub_pose2 = node->Subscribe("~/pose/info", cbGobalPose);
+
+    // Busy wait loop...replace with your own code as needed.
+    while (true)
+        gazebo::common::Time::MSleep(10);
+
+    // Make sure to shut everything down.
+    gazebo::client::shutdown();
+}