diff --git a/GazeboDrone/CMakeLists.txt b/GazeboDrone/CMakeLists.txt new file mode 100644 index 0000000000..4142f789b4 --- /dev/null +++ b/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 +) + diff --git a/GazeboDrone/README.md b/GazeboDrone/README.md new file mode 100644 index 0000000000..1f4725d43e --- /dev/null +++ b/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 +``` + diff --git a/GazeboDrone/src/main.cpp b/GazeboDrone/src/main.cpp new file mode 100644 index 0000000000..7ca565b046 --- /dev/null +++ b/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 +#include +#include +#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 + +#include + +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(); +}