Update gazebo from v7 to v8/v9 compatibility (#2357)

* update gazebo from v7 to v8/v9 compatibility

* upgrade docker

.travis.yml

@@ -1,5 +1,5 @@
 sudo: required
-dist: trusty
+dist: xenial
 language: c
 compiler:
   - gcc
@@ -11,7 +11,7 @@ before_install:
   - wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
   - sudo apt-get update -q
 install:
-  - sudo apt-get install -y --force-yes paparazzi-dev paparazzi-jsbsim gcc-arm-embedded gcc-arm-linux-gnueabi libipc-run-perl rustc cargo libgazebo7-dev
+  - sudo apt-get install -y --force-yes paparazzi-dev paparazzi-jsbsim gcc-arm-embedded gcc-arm-linux-gnueabi g++-arm-linux-gnueabi libipc-run-perl rustc cargo libgazebo9-dev
 script:
   - arm-none-eabi-gcc --version
   - if [ "${COVERITY_SCAN_BRANCH}" != 1 ]; then if [ "${TRAVIS_EVENT_TYPE}" == "cron" ]; then make test_all_confs; else make test; fi; fi

conf/modules/fdm_gazebo.xml

@@ -7,7 +7,7 @@
       NPS doc: http://wiki.paparazziuav.org/wiki/NPS
 
       Usage:
-      1. Make sure gazebo 7 or 8 is installed. (sudo apt-get install gazebo8 libgazebo8-dev)
+      1. Make sure gazebo 9 is installed. (sudo apt-get install gazebo9 libgazebo9-dev)
       2. Prepare the Gazebo world and model:
         1. Prepare the UAV model (see conf/simulator/gazebo/models/ardrone/):
           - Place the aircraft model in the conf/simulator/gazebo/models/
@@ -134,7 +134,7 @@
   </doc>
   <header/>
   <makefile target="nps">
-  	<raw>
+    <raw>
       nps.CXXFLAGS += $(shell pkg-config gazebo --cflags)
       nps.LDFLAGS  += $(shell pkg-config gazebo --libs)
 
@@ -145,7 +145,7 @@
         nps.CXXFLAGS += $(shell pkg-config opencv)
         nps.LDFLAGS  += -lopencv_imgproc -lopencv_highgui -lopencv_core
       endif
-  	</raw>
+    </raw>
     <file name="nps_fdm_gazebo.cpp" dir="nps"/>
   </makefile>
 </module>

docker/ci/Dockerfile

@@ -5,6 +5,7 @@ RUN sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb
 RUN wget http://packages.osrfoundation.org/gazebo.key -O - | apt-key add -
 
 RUN DEBIAN_FRONTEND=noninteractive apt-get update && apt-get install -y \
-    libgazebo7-dev \
+    g++-arm-linux-gnueabi \
+    libgazebo8-dev \
     rustc cargo \
     && rm -rf /var/lib/apt/lists/*

sw/simulator/nps/nps_fdm_gazebo.cpp

@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2017 Tom van Dijk
+ * Copyright (C) 2017 Tom van Dijk, Kirk Scheeper
  *
  * This file is part of paparazzi.
  *
@@ -26,12 +26,6 @@
  * This is an FDM for NPS that uses Gazebo as the simulation engine.
  */
 
-// The transition from Gazebo 7 to 8 deprecates a large number of functions.
-// Ignore these errors for now...
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
-
-
 #include <cstdio>
 #include <cstdlib>
 #include <string>
@@ -39,13 +33,15 @@
 #include <sys/time.h>
 
 #include <gazebo/gazebo.hh>
-#include <gazebo/common/common.hh>
-#include <gazebo/math/gzmath.hh>
 #include <gazebo/physics/physics.hh>
 #include <gazebo/sensors/sensors.hh>
-#include <gazebo/gazebo_config.h>
 #include <sdf/sdf.hh>
 
+#include <gazebo/gazebo_config.h>
+#if GAZEBO_MAJOR_VERSION < 8
+#error "Paparazzi only supports gazebo versions > 7, please upgrade to a more recent version of Gazebo"
+#endif
+
 extern "C" {
 #include "nps_fdm.h"
 #include "nps_autopilot.h"
@@ -53,6 +49,7 @@ extern "C" {
 #include "generated/airframe.h"
 #include "generated/flight_plan.h"
 #include "autopilot.h"
+#include "subsystems/abi.h"
 
 #include "math/pprz_isa.h"
 #include "math/pprz_algebra_double.h"
@@ -112,8 +109,7 @@ struct gazebo_actuators_t {
   double max_ang_momentum[NPS_COMMANDS_NB];
 };
 
-struct gazebo_actuators_t gazebo_actuators = { NPS_ACTUATOR_NAMES,
+struct gazebo_actuators_t gazebo_actuators = { NPS_ACTUATOR_NAMES, NPS_ACTUATOR_THRUSTS, NPS_ACTUATOR_TORQUES, { }, { }, { }};
-    NPS_ACTUATOR_THRUSTS, NPS_ACTUATOR_TORQUES, { }, { }, { } };
 
 #if NPS_SIMULATE_LASER_RANGE_ARRAY
 extern "C" {
@@ -168,48 +164,48 @@ inline struct LlaCoor_d to_pprz_lla(ignition::math::Vector3d lla_i)
   return lla_p;
 }
 
-inline struct DoubleVect3 to_pprz_body(gazebo::math::Vector3 body_g)
+inline struct DoubleVect3 to_pprz_body(ignition::math::Vector3d body_g)
 {
   struct DoubleVect3 body_p;
-  body_p.x = body_g.x;
+  body_p.x = body_g.X();
-  body_p.y = -body_g.y;
+  body_p.y = -body_g.Y();
-  body_p.z = -body_g.z;
+  body_p.z = -body_g.Z();
   return body_p;
 }
 
-inline struct DoubleRates to_pprz_rates(gazebo::math::Vector3 body_g)
+inline struct DoubleRates to_pprz_rates(ignition::math::Vector3d body_g)
 {
   struct DoubleRates body_p;
-  body_p.p = body_g.x;
+  body_p.p = body_g.X();
-  body_p.q = -body_g.y;
+  body_p.q = -body_g.Y();
-  body_p.r = -body_g.z;
+  body_p.r = -body_g.Z();
   return body_p;
 }
 
-inline struct DoubleEulers to_pprz_eulers(gazebo::math::Quaternion quat)
+inline struct DoubleEulers to_pprz_eulers(ignition::math::Quaterniond quat)
 {
   struct DoubleEulers eulers;
-  eulers.psi = -quat.GetYaw();
+  eulers.psi = -quat.Yaw();
-  eulers.theta = -quat.GetPitch();
+  eulers.theta = -quat.Pitch();
-  eulers.phi = quat.GetRoll();
+  eulers.phi = quat.Roll();
   return eulers;
 }
 
-inline struct DoubleEulers to_global_pprz_eulers(gazebo::math::Quaternion quat)
+inline struct DoubleEulers to_global_pprz_eulers(ignition::math::Quaterniond quat)
 {
   struct DoubleEulers eulers;
-  eulers.psi = -quat.GetYaw() - M_PI / 2;
+  eulers.psi = -quat.Yaw() - M_PI / 2;
-  eulers.theta = -quat.GetPitch();
+  eulers.theta = -quat.Pitch();
-  eulers.phi = quat.GetRoll();
+  eulers.phi = quat.Roll();
   return eulers;
 }
 
-inline struct DoubleVect3 to_pprz_ltp(gazebo::math::Vector3 xyz)
+inline struct DoubleVect3 to_pprz_ltp(ignition::math::Vector3d xyz)
 {
   struct DoubleVect3 ltp;
-  ltp.x = xyz.y;
+  ltp.x = xyz.Y();
-  ltp.y = xyz.x;
+  ltp.y = xyz.X();
-  ltp.z = -xyz.z;
+  ltp.z = -xyz.Z();
   return ltp;
 }
 
@@ -228,13 +224,13 @@ void nps_fdm_init(double dt)
 #ifdef NPS_ACTUATOR_TIME_CONSTANTS
   // Set up low-pass filter to simulate delayed actuator response
   const float tau[NPS_COMMANDS_NB] = NPS_ACTUATOR_TIME_CONSTANTS;
-  for(int i=0; i<NPS_COMMANDS_NB; i++) {
+  for (int i = 0; i < NPS_COMMANDS_NB; i++) {
     init_first_order_low_pass(&gazebo_actuators.lowpass[i], tau[i], dt, 0.f);
   }
 #ifdef NPS_ACTUATOR_MAX_ANGULAR_MOMENTUM
   // Set up high-pass filter to simulate spinup torque
   const float Iwmax[NPS_COMMANDS_NB] = NPS_ACTUATOR_MAX_ANGULAR_MOMENTUM;
-  for(int i=0; i<NPS_COMMANDS_NB; i++) {
+  for (int i = 0; i < NPS_COMMANDS_NB; i++) {
     init_first_order_high_pass(&gazebo_actuators.highpass[i], tau[i], dt, 0.f);
     gazebo_actuators.max_ang_momentum[i] = Iwmax[i];
   }
@@ -338,28 +334,26 @@ static void init_gazebo(void)
   }
 
   cout << "Add Paparazzi paths: " << gazebodir << endl;
-  gazebo::common::SystemPaths::Instance()->AddModelPaths(
+  gazebo::common::SystemPaths::Instance()->AddModelPaths(gazebodir + "models/");
-    gazebodir + "models/");
   sdf::addURIPath("model://", gazebodir + "models/");
   sdf::addURIPath("world://", gazebodir + "world/");
 
   cout << "Add TU Delft paths: " << pprz_home + "/sw/ext/tudelft_gazebo_models/" << endl;
-  gazebo::common::SystemPaths::Instance()->AddModelPaths(
+  gazebo::common::SystemPaths::Instance()->AddModelPaths(pprz_home + "/sw/ext/tudelft_gazebo_models/models/");
-      pprz_home + "/sw/ext/tudelft_gazebo_models/models/");
   sdf::addURIPath("model://", pprz_home + "/sw/ext/tudelft_gazebo_models/models/");
   sdf::addURIPath("world://", pprz_home + "/sw/ext/tudelft_gazebo_models/world/");
 
   // get vehicles
   string vehicle_uri = "model://" + string(NPS_GAZEBO_AC_NAME) + "/" + string(NPS_GAZEBO_AC_NAME) + ".sdf";
   string vehicle_filename = sdf::findFile(vehicle_uri, false);
-  if(vehicle_filename.empty()) {
+  if (vehicle_filename.empty()) {
     cout << "ERROR, could not find vehicle " + vehicle_uri << endl;
     std::exit(-1);
   }
   cout << "Load vehicle: " << vehicle_filename << endl;
   sdf::SDFPtr vehicle_sdf(new sdf::SDF());
   sdf::init(vehicle_sdf);
-  if(!sdf::readFile(vehicle_filename, vehicle_sdf)) {
+  if (!sdf::readFile(vehicle_filename, vehicle_sdf)) {
     cout << "ERROR, could not read vehicle " + vehicle_filename << endl;
     std::exit(-1);
   }
@@ -377,14 +371,17 @@ static void init_gazebo(void)
   // bebop front camera
 #ifdef NPS_SIMULATE_MT9F002
   sdf::ElementPtr link = vehicle_sdf->Root()->GetFirstElement()->GetElement("link");
-  while(link) {
+  while (link) {
-    if(link->Get<string>("name") == "front_camera") {
+    if (link->Get<string>("name") == "front_camera") {
       int w = link->GetElement("sensor")->GetElement("camera")->GetElement("image")->GetElement("width")->Get<int>();
-      link->GetElement("sensor")->GetElement("camera")->GetElement("image")->GetElement("width")->Set(w * MT9F002_OUTPUT_SCALER);
+      link->GetElement("sensor")->GetElement("camera")->GetElement("image")->GetElement("width")->Set(
+        w * MT9F002_OUTPUT_SCALER);
       int h = link->GetElement("sensor")->GetElement("camera")->GetElement("image")->GetElement("height")->Get<int>();
-      link->GetElement("sensor")->GetElement("camera")->GetElement("image")->GetElement("height")->Set(h * MT9F002_OUTPUT_SCALER);
+      link->GetElement("sensor")->GetElement("camera")->GetElement("image")->GetElement("height")->Set(
+        h * MT9F002_OUTPUT_SCALER);
       int env = link->GetElement("sensor")->GetElement("camera")->GetElement("lens")->GetElement("env_texture_size")->Get<int>();
-      link->GetElement("sensor")->GetElement("camera")->GetElement("lens")->GetElement("env_texture_size")->Set(env * MT9F002_OUTPUT_SCALER);
+      link->GetElement("sensor")->GetElement("camera")->GetElement("lens")->GetElement("env_texture_size")->Set(
+        env * MT9F002_OUTPUT_SCALER);
       cout << "Applied MT9F002_OUTPUT_SCALER (=" << MT9F002_OUTPUT_SCALER << ") to " << link->Get<string>("name") << endl;
       link->GetElement("sensor")->GetElement("update_rate")->Set(MT9F002_TARGET_FPS);
       cout << "Applied MT9F002_TARGET_FPS (=" << MT9F002_TARGET_FPS << ") to " << link->Get<string>("name") << endl;
@@ -397,14 +394,14 @@ static void init_gazebo(void)
   // get world
   string world_uri = "world://" + string(NPS_GAZEBO_WORLD);
   string world_filename = sdf::findFile(world_uri, false);
-  if(world_filename.empty()) {
+  if (world_filename.empty()) {
     cout << "ERROR, could not find world " + world_uri << endl;
     std::exit(-1);
   }
   cout << "Load world: " << world_filename << endl;
   sdf::SDFPtr world_sdf(new sdf::SDF());
   sdf::init(world_sdf);
-  if(!sdf::readFile(world_filename, world_sdf)) {
+  if (!sdf::readFile(world_filename, world_sdf)) {
     cout << "ERROR, could not read world " + world_filename << endl;
     std::exit(-1);
   }
@@ -421,7 +418,7 @@ static void init_gazebo(void)
   }
 
   cout << "Get pointer to aircraft: " << NPS_GAZEBO_AC_NAME << endl;
-  model = world->GetModel(NPS_GAZEBO_AC_NAME);
+  model = world->ModelByName(NPS_GAZEBO_AC_NAME);
   if (!model) {
     cout << "Failed to find '" << NPS_GAZEBO_AC_NAME << "', exiting."
          << endl;
@@ -461,27 +458,25 @@ static void init_gazebo(void)
  */
 static void gazebo_read(void)
 {
-  static gazebo::math::Vector3 vel_prev;
+  static ignition::math::Vector3d vel_prev;
   static double time_prev;
 
   gazebo::physics::WorldPtr world = model->GetWorld();
-  gazebo::math::Pose pose = model->GetWorldPose(); // In LOCAL xyz frame
+  ignition::math::Pose3d pose = model->WorldPose(); // In LOCAL xyz frame
-  gazebo::math::Vector3 vel = model->GetWorldLinearVel();
+  ignition::math::Vector3d vel = model->WorldLinearVel();
-  gazebo::math::Vector3 ang_vel = model->GetWorldAngularVel();
+  ignition::math::Vector3d ang_vel = model->WorldAngularVel();
-  gazebo::common::SphericalCoordinatesPtr sphere =
+  gazebo::common::SphericalCoordinatesPtr sphere = world->SphericalCoords();
-    world->GetSphericalCoordinates();
+  ignition::math::Quaterniond local_to_global_quat(0, 0, -sphere->HeadingOffset().Radian());
-  gazebo::math::Quaternion local_to_global_quat(0, 0,
-      -sphere->HeadingOffset().Radian());
 
   /* Fill FDM struct */
-  fdm.time = world->GetSimTime().Double();
+  fdm.time = world->SimTime().Double();
 
   // Find world acceleration by differentiating velocity
   // model->GetWorldLinearAccel() does not seem to take the velocity_decay into account!
   // Derivation of the velocity also follows the IMU implementation of Gazebo itself:
   // https://bitbucket.org/osrf/gazebo/src/e26144434b932b4b6a760ddaa19cfcf9f1734748/gazebo/sensors/ImuSensor.cc?at=default&fileviewer=file-view-default#ImuSensor.cc-370
   double dt = fdm.time - time_prev;
-  gazebo::math::Vector3 accel = (vel - vel_prev) / dt;
+  ignition::math::Vector3d accel = (vel - vel_prev) / dt;
   vel_prev = vel;
   time_prev = fdm.time;
 
@@ -491,76 +486,57 @@ static void gazebo_read(void)
   // nan_count: unused
 
   /* position */
-  fdm.ecef_pos = to_pprz_ecef(
+  fdm.ecef_pos = to_pprz_ecef(sphere->PositionTransform(pose.Pos(), gazebo::common::SphericalCoordinates::LOCAL,
-                   sphere->PositionTransform(pose.pos.Ign(),
+                              gazebo::common::SphericalCoordinates::ECEF));
-                       gazebo::common::SphericalCoordinates::LOCAL,
+  fdm.ltpprz_pos = to_pprz_ned(sphere->PositionTransform(pose.Pos(), gazebo::common::SphericalCoordinates::LOCAL,
-                       gazebo::common::SphericalCoordinates::ECEF));
+                               gazebo::common::SphericalCoordinates::GLOBAL));
-  fdm.ltpprz_pos = to_pprz_ned(
+  fdm.lla_pos = to_pprz_lla(sphere->PositionTransform(pose.Pos(), gazebo::common::SphericalCoordinates::LOCAL,
-                     sphere->PositionTransform(pose.pos.Ign(),
+                            gazebo::common::SphericalCoordinates::SPHERICAL));
-                         gazebo::common::SphericalCoordinates::LOCAL,
+  fdm.hmsl = pose.Pos().Z();
-                         gazebo::common::SphericalCoordinates::GLOBAL));
+
-  fdm.lla_pos = to_pprz_lla(
-                  sphere->PositionTransform(pose.pos.Ign(),
-                      gazebo::common::SphericalCoordinates::LOCAL,
-                      gazebo::common::SphericalCoordinates::SPHERICAL));
-  fdm.hmsl = pose.pos.z;
   /* debug positions */
   fdm.lla_pos_pprz = fdm.lla_pos; // Don't really care...
   fdm.lla_pos_geod = fdm.lla_pos;
   fdm.lla_pos_geoc = fdm.lla_pos;
-  fdm.agl = pose.pos.z; // TODO Measure with sensor
+  fdm.agl = pose.Pos().Z(); // TODO Measure with sensor
 
   /* velocity */
-  fdm.ecef_ecef_vel = to_pprz_ecef(
+  fdm.ecef_ecef_vel = to_pprz_ecef(sphere->VelocityTransform(vel, gazebo::common::SphericalCoordinates::LOCAL,
-                        sphere->VelocityTransform(vel.Ign(),
+                                   gazebo::common::SphericalCoordinates::ECEF));
-                            gazebo::common::SphericalCoordinates::LOCAL,
+  fdm.body_ecef_vel = to_pprz_body(pose.Rot().RotateVectorReverse(vel)); // Note: unused
-                            gazebo::common::SphericalCoordinates::ECEF));
+  fdm.ltp_ecef_vel = to_pprz_ned(sphere->VelocityTransform(vel, gazebo::common::SphericalCoordinates::LOCAL,
-  fdm.body_ecef_vel = to_pprz_body(pose.rot.RotateVectorReverse(vel)); // Note: unused
+                                 gazebo::common::SphericalCoordinates::GLOBAL));
-  fdm.ltp_ecef_vel = to_pprz_ned(
-                       sphere->VelocityTransform(vel.Ign(),
-                           gazebo::common::SphericalCoordinates::LOCAL,
-                           gazebo::common::SphericalCoordinates::GLOBAL));
   fdm.ltpprz_ecef_vel = fdm.ltp_ecef_vel; // ???
 
   /* acceleration */
-  fdm.ecef_ecef_accel = to_pprz_ecef(
+  fdm.ecef_ecef_accel = to_pprz_ecef(sphere->VelocityTransform(accel, gazebo::common::SphericalCoordinates::LOCAL,
-                          sphere->VelocityTransform(accel.Ign(),
+                                     gazebo::common::SphericalCoordinates::ECEF)); // Note: unused
-                              gazebo::common::SphericalCoordinates::LOCAL,
+  fdm.body_ecef_accel = to_pprz_body(pose.Rot().RotateVectorReverse(accel));
-                              gazebo::common::SphericalCoordinates::ECEF)); // Note: unused
+  fdm.ltp_ecef_accel = to_pprz_ned(sphere->VelocityTransform(accel, gazebo::common::SphericalCoordinates::LOCAL,
-  fdm.body_ecef_accel = to_pprz_body(pose.rot.RotateVectorReverse(accel));
+                                   gazebo::common::SphericalCoordinates::GLOBAL)); // Note: unused
-  fdm.ltp_ecef_accel = to_pprz_ned(
-                         sphere->VelocityTransform(accel.Ign(),
-                             gazebo::common::SphericalCoordinates::LOCAL,
-                             gazebo::common::SphericalCoordinates::GLOBAL)); // Note: unused
   fdm.ltpprz_ecef_accel = fdm.ltp_ecef_accel; // ???
   fdm.body_inertial_accel = fdm.body_ecef_accel; // Approximate, unused.
-
+  fdm.body_accel = to_pprz_body(pose.Rot().RotateVectorReverse(accel - world->Gravity()));
-  fdm.body_accel = to_pprz_body(
-                     pose.rot.RotateVectorReverse(accel.Ign() - world->Gravity()));
 
   /* attitude */
   // ecef_to_body_quat: unused
-  fdm.ltp_to_body_eulers = to_global_pprz_eulers(local_to_global_quat * pose.rot);
+  fdm.ltp_to_body_eulers = to_global_pprz_eulers(local_to_global_quat * pose.Rot());
   double_quat_of_eulers(&(fdm.ltp_to_body_quat), &(fdm.ltp_to_body_eulers));
   fdm.ltpprz_to_body_quat = fdm.ltp_to_body_quat; // unused
   fdm.ltpprz_to_body_eulers = fdm.ltp_to_body_eulers; // unused
 
   /* angular velocity */
-  fdm.body_ecef_rotvel = to_pprz_rates(pose.rot.RotateVectorReverse(ang_vel));
+  fdm.body_ecef_rotvel = to_pprz_rates(pose.Rot().RotateVectorReverse(ang_vel));
   fdm.body_inertial_rotvel = fdm.body_ecef_rotvel; // Approximate
 
   /* angular acceleration */
   // body_ecef_rotaccel: unused
   // body_inertial_rotaccel: unused
   /* misc */
-  fdm.ltp_g = to_pprz_ltp(
+  fdm.ltp_g = to_pprz_ltp(sphere->VelocityTransform(-1 * world->Gravity(), gazebo::common::SphericalCoordinates::LOCAL,
-                sphere->VelocityTransform(-1 * world->Gravity(),
+                          gazebo::common::SphericalCoordinates::GLOBAL)); // unused
-                                          gazebo::common::SphericalCoordinates::LOCAL,
+  fdm.ltp_h = to_pprz_ltp(sphere->VelocityTransform(world->MagneticField(), gazebo::common::SphericalCoordinates::LOCAL,
-                                          gazebo::common::SphericalCoordinates::GLOBAL)); // unused
+                          gazebo::common::SphericalCoordinates::GLOBAL));
-  fdm.ltp_h = to_pprz_ltp(
-                sphere->VelocityTransform(world->MagneticField(),
-                                          gazebo::common::SphericalCoordinates::LOCAL,
-                                          gazebo::common::SphericalCoordinates::GLOBAL));
 
   /* atmosphere */
 #if GAZEBO_MAJOR_VERSION >= 8 && 0 // TODO implement
@@ -624,15 +600,14 @@ static void gazebo_write(double act_commands[], int commands_nb)
 #ifdef NPS_ACTUATOR_MAX_ANGULAR_MOMENTUM
     // Spinup torque
     double udot = update_first_order_high_pass(&gazebo_actuators.highpass[i], sp);
-    double spinup_torque = gazebo_actuators.max_ang_momentum[i] /
+    double spinup_torque = gazebo_actuators.max_ang_momentum[i] / (2.0 * sqrt(u > 0.05 ? u : 0.05)) * udot;
-        (2.0 * sqrt(u > 0.05 ? u : 0.05)) * udot;
     torque += spinup_torque;
 #endif
 
     // Apply force and torque to gazebo model
     gazebo::physics::LinkPtr link = model->GetLink(gazebo_actuators.names[i]);
-    link->AddRelativeForce(gazebo::math::Vector3(0, 0, thrust));
+    link->AddRelativeForce(ignition::math::Vector3d(0, 0, thrust));
-    link->AddRelativeTorque(gazebo::math::Vector3(0, 0, torque));
+    link->AddRelativeTorque(ignition::math::Vector3d(0, 0, torque));
   }
 }
 
@@ -693,13 +668,13 @@ static void init_gazebo_video(void)
     cameras[i]->sensor_size.h = cam->ImageHeight();
     cameras[i]->crop.w = cam->ImageWidth();
     cameras[i]->crop.h = cam->ImageHeight();
-    cameras[i]->camera_intrinsics.focal_x = cameras[i]->output_size.w/2.0f;
+    cameras[i]->camera_intrinsics.focal_x = cameras[i]->output_size.w / 2.0f;
-    cameras[i]->camera_intrinsics.center_x = cameras[i]->output_size.w/2.0f;
+    cameras[i]->camera_intrinsics.center_x = cameras[i]->output_size.w / 2.0f;
-    cameras[i]->camera_intrinsics.focal_y = cameras[i]->output_size.h/2.0f;
+    cameras[i]->camera_intrinsics.focal_y = cameras[i]->output_size.h / 2.0f;
-    cameras[i]->camera_intrinsics.center_y = cameras[i]->output_size.h/2.0f;
+    cameras[i]->camera_intrinsics.center_y = cameras[i]->output_size.h / 2.0f;
 #if NPS_SIMULATE_MT9F002
     // See boards/bebop/mt9f002.c
-    if(cam->Name() == "front_camera") {
+    if (cam->Name() == "front_camera") {
       cameras[i]->output_size.w = MT9F002_OUTPUT_WIDTH;
       cameras[i]->output_size.h = MT9F002_OUTPUT_HEIGHT;
       cameras[i]->sensor_size.w = MT9F002_OUTPUT_WIDTH;
@@ -707,12 +682,12 @@ static void init_gazebo_video(void)
       cameras[i]->crop.w = MT9F002_OUTPUT_WIDTH;
       cameras[i]->crop.h = MT9F002_OUTPUT_HEIGHT;
       cameras[i]->camera_intrinsics = {
-          .focal_x = MT9F002_FOCAL_X,
+        .focal_x = MT9F002_FOCAL_X,
-          .focal_y = MT9F002_FOCAL_Y,
+        .focal_y = MT9F002_FOCAL_Y,
-          .center_x = MT9F002_CENTER_X,
+        .center_x = MT9F002_CENTER_X,
-          .center_y = MT9F002_CENTER_Y,
+        .center_y = MT9F002_CENTER_Y,
-          .Dhane_k = MT9F002_DHANE_K
+        .Dhane_k = MT9F002_DHANE_K
-        };
+      };
     }
 #endif
     cameras[i]->fps = cam->UpdateRate();
@@ -775,7 +750,7 @@ static void read_image(
   int xstart = 0;
   int ystart = 0;
 #if NPS_SIMULATE_MT9F002
-  if(cam->Name() == "front_camera") {
+  if (cam->Name() == "front_camera") {
     image_create(img, MT9F002_OUTPUT_WIDTH, MT9F002_OUTPUT_HEIGHT, IMAGE_YUV422);
     xstart = cam->ImageWidth() * (0.5 + MT9F002_INITIAL_OFFSET_X) - MT9F002_OUTPUT_WIDTH / 2;
     ystart = cam->ImageHeight() * (0.5 + MT9F002_INITIAL_OFFSET_Y) - MT9F002_OUTPUT_HEIGHT / 2;
@@ -794,17 +769,17 @@ static void read_image(
       int idx_yuv = 2 * (img->w * y + x);
       int idx_px = img->w * y + x;
       if (idx_px % 2 == 0) { // Pick U or V
-        data_yuv[idx_yuv] = -0.148 * data_rgb[idx_rgb]
+        data_yuv[idx_yuv] = - 0.148 * data_rgb[idx_rgb]
                             - 0.291 * data_rgb[idx_rgb + 1]
                             + 0.439 * data_rgb[idx_rgb + 2] + 128; // U
       } else {
-        data_yuv[idx_yuv] = 0.439 * data_rgb[idx_rgb]
+        data_yuv[idx_yuv] =   0.439 * data_rgb[idx_rgb]
-                            - 0.368 * data_rgb[idx_rgb + 1]
+                              - 0.368 * data_rgb[idx_rgb + 1]
-                            - 0.071 * data_rgb[idx_rgb + 2] + 128; // V
+                              - 0.071 * data_rgb[idx_rgb + 2] + 128; // V
       }
-      data_yuv[idx_yuv + 1] = 0.257 * data_rgb[idx_rgb]
+      data_yuv[idx_yuv + 1] =   0.257 * data_rgb[idx_rgb]
-                              + 0.504 * data_rgb[idx_rgb + 1]
+                                + 0.504 * data_rgb[idx_rgb + 1]
-                              + 0.098 * data_rgb[idx_rgb + 2] + 16; // Y
+                                + 0.098 * data_rgb[idx_rgb + 2] + 16; // Y
     }
   }
   // Fill miscellaneous fields
@@ -884,7 +859,7 @@ static void gazebo_init_range_sensors(void)
        * given in the airframe file in LASER_RANGE_ARRAY_RANGE_ORIENTATION
        */
       for (int j = 0; j < LASER_RANGE_ARRAY_NUM_SENSORS; j++) {
-        struct DoubleEulers def = {0, range_orientation[j*2], range_orientation[j*2 + 1]};
+        struct DoubleEulers def = {0, range_orientation[j * 2], range_orientation[j * 2 + 1]};
         double_quat_of_eulers(&q_def, &def);
         // get angle between required angle and ray angle
         double angle = acos(QUAT_DOT_PRODUCT(q_ray, q_def));
@@ -896,7 +871,7 @@ static void gazebo_init_range_sensors(void)
 
 #if LASER_RANGE_ARRAY_SEND_AGL
           // find the sensor pointing down
-          if (fabs(range_orientation[j*2] + M_PI_2) < RadOfDeg(5)) {
+          if (fabs(range_orientation[j * 2] + M_PI_2) < RadOfDeg(5)) {
             ray_sensor_agl_index = j;
           }
 #endif
@@ -929,7 +904,8 @@ static void gazebo_read_range_sensors(void)
     } else {
       range = rays[i].sensor->Range(0);
     }
-    AbiSendMsgOBSTACLE_DETECTION(OBS_DETECTION_RANGE_ARRAY_NPS_ID, range, range_orientation[i*2], range_orientation[i*2 + 1]);
+    AbiSendMsgOBSTACLE_DETECTION(OBS_DETECTION_RANGE_ARRAY_NPS_ID, range, range_orientation[i * 2],
+                                 range_orientation[i * 2 + 1]);
 
     if (i == ray_sensor_agl_index) {
       float agl = rays[i].sensor->Range(0);