Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-31 03:57:45 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

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

Browse Source 
* update gazebo from v7 to v8/v9 compatibility

* upgrade docker
This commit is contained in:
Kirk Scheper
2018-11-28 11:03:41 +01:00
committed by Gautier Hattenberger
parent d7d627d886
commit a88bbac7eb
4 changed files with 112 additions and 135 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.travis.yml
+2 -2
View File
@@ -1,5 +1,5 @@
sudo: required sudo: required
dist: trusty dist: xenial
language: c language: c
compiler: compiler:
- gcc - gcc
@@ -11,7 +11,7 @@ before_install:
- wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add - - wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
- sudo apt-get update -q - sudo apt-get update -q
install: 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: script:
- arm-none-eabi-gcc --version - 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 - 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
+1 -1
View File
@@ -7,7 +7,7 @@
NPS doc: http://wiki.paparazziuav.org/wiki/NPS NPS doc: http://wiki.paparazziuav.org/wiki/NPS
Usage: 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: 2. Prepare the Gazebo world and model:
1. Prepare the UAV model (see conf/simulator/gazebo/models/ardrone/): 1. Prepare the UAV model (see conf/simulator/gazebo/models/ardrone/):
- Place the aircraft model in the conf/simulator/gazebo/models/ - Place the aircraft model in the conf/simulator/gazebo/models/
docker/ci/Dockerfile
+2 -1
View File
@@ -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 wget http://packages.osrfoundation.org/gazebo.key -O - | apt-key add -
RUN DEBIAN_FRONTEND=noninteractive apt-get update && apt-get install -y \ RUN DEBIAN_FRONTEND=noninteractive apt-get update && apt-get install -y \
libgazebo7-dev \ g++-arm-linux-gnueabi \
libgazebo8-dev \
rustc cargo \ rustc cargo \
&& rm -rf /var/lib/apt/lists/* && rm -rf /var/lib/apt/lists/*
sw/simulator/nps/nps_fdm_gazebo.cpp
+67 -91
View File
@@ -1,5 +1,5 @@
/* /*
* Copyright (C) 2017 Tom van Dijk * Copyright (C) 2017 Tom van Dijk, Kirk Scheeper
* *
* This file is part of paparazzi. * This file is part of paparazzi.
* *
@@ -26,12 +26,6 @@
* This is an FDM for NPS that uses Gazebo as the simulation engine. * 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 <cstdio>
#include <cstdlib> #include <cstdlib>
#include <string> #include <string>
@@ -39,13 +33,15 @@
#include <sys/time.h> #include <sys/time.h>
#include <gazebo/gazebo.hh> #include <gazebo/gazebo.hh>
#include <gazebo/common/common.hh>
#include <gazebo/math/gzmath.hh>
#include <gazebo/physics/physics.hh> #include <gazebo/physics/physics.hh>
#include <gazebo/sensors/sensors.hh> #include <gazebo/sensors/sensors.hh>
#include <gazebo/gazebo_config.h>
#include <sdf/sdf.hh> #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" { extern "C" {
#include "nps_fdm.h" #include "nps_fdm.h"
#include "nps_autopilot.h" #include "nps_autopilot.h"
@@ -53,6 +49,7 @@ extern "C" {
#include "generated/airframe.h" #include "generated/airframe.h"
#include "generated/flight_plan.h" #include "generated/flight_plan.h"
#include "autopilot.h" #include "autopilot.h"
#include "subsystems/abi.h"
#include "math/pprz_isa.h" #include "math/pprz_isa.h"
#include "math/pprz_algebra_double.h" #include "math/pprz_algebra_double.h"
@@ -112,8 +109,7 @@ struct gazebo_actuators_t {
double max_ang_momentum[NPS_COMMANDS_NB]; 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 #if NPS_SIMULATE_LASER_RANGE_ARRAY
extern "C" { extern "C" {
@@ -168,48 +164,48 @@ inline struct LlaCoor_d to_pprz_lla(ignition::math::Vector3d lla_i)
return lla_p; 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; 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; 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; 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; 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; 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; 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; 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; 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; 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; return ltp;
} }
@@ -338,14 +334,12 @@ static void init_gazebo(void)
} }
cout << "Add Paparazzi paths: " << gazebodir << endl; 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("model://", gazebodir + "models/");
sdf::addURIPath("world://", gazebodir + "world/"); sdf::addURIPath("world://", gazebodir + "world/");
cout << "Add TU Delft paths: " << pprz_home + "/sw/ext/tudelft_gazebo_models/" << endl; 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("model://", pprz_home + "/sw/ext/tudelft_gazebo_models/models/");
sdf::addURIPath("world://", pprz_home + "/sw/ext/tudelft_gazebo_models/world/"); sdf::addURIPath("world://", pprz_home + "/sw/ext/tudelft_gazebo_models/world/");
@@ -380,11 +374,14 @@ static void init_gazebo(void)
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>(); 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>(); 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>(); 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; cout << "Applied MT9F002_OUTPUT_SCALER (=" << MT9F002_OUTPUT_SCALER << ") to " << link->Get<string>("name") << endl;
link->GetElement("sensor")->GetElement("update_rate")->Set(MT9F002_TARGET_FPS); link->GetElement("sensor")->GetElement("update_rate")->Set(MT9F002_TARGET_FPS);
cout << "Applied MT9F002_TARGET_FPS (=" << MT9F002_TARGET_FPS << ") to " << link->Get<string>("name") << endl; cout << "Applied MT9F002_TARGET_FPS (=" << MT9F002_TARGET_FPS << ") to " << link->Get<string>("name") << endl;
@@ -421,7 +418,7 @@ static void init_gazebo(void)
} }
cout << "Get pointer to aircraft: " << NPS_GAZEBO_AC_NAME << endl; 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) { if (!model) {
cout << "Failed to find '" << NPS_GAZEBO_AC_NAME << "', exiting." cout << "Failed to find '" << NPS_GAZEBO_AC_NAME << "', exiting."
<< endl; << endl;
@@ -461,27 +458,25 @@ static void init_gazebo(void)
*/ */
static void gazebo_read(void) static void gazebo_read(void)
{ {
static gazebo::math::Vector3 vel_prev; static ignition::math::Vector3d vel_prev;
static double time_prev; static double time_prev;
gazebo::physics::WorldPtr world = model->GetWorld(); 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 */ /* Fill FDM struct */
fdm.time = world->GetSimTime().Double(); fdm.time = world->SimTime().Double();
// Find world acceleration by differentiating velocity // Find world acceleration by differentiating velocity
// model->GetWorldLinearAccel() does not seem to take the velocity_decay into account! // model->GetWorldLinearAccel() does not seem to take the velocity_decay into account!
// Derivation of the velocity also follows the IMU implementation of Gazebo itself: // 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 // 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; 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; vel_prev = vel;
time_prev = fdm.time; time_prev = fdm.time;
@@ -491,75 +486,56 @@ static void gazebo_read(void)
// nan_count: unused // nan_count: unused
/* position */ /* 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::LOCAL,
gazebo::common::SphericalCoordinates::ECEF)); gazebo::common::SphericalCoordinates::ECEF));
fdm.ltpprz_pos = to_pprz_ned( fdm.ltpprz_pos = to_pprz_ned(sphere->PositionTransform(pose.Pos(), gazebo::common::SphericalCoordinates::LOCAL,
sphere->PositionTransform(pose.pos.Ign(),
gazebo::common::SphericalCoordinates::LOCAL,
gazebo::common::SphericalCoordinates::GLOBAL)); gazebo::common::SphericalCoordinates::GLOBAL));
fdm.lla_pos = to_pprz_lla( fdm.lla_pos = to_pprz_lla(sphere->PositionTransform(pose.Pos(), gazebo::common::SphericalCoordinates::LOCAL,
sphere->PositionTransform(pose.pos.Ign(),
gazebo::common::SphericalCoordinates::LOCAL,
gazebo::common::SphericalCoordinates::SPHERICAL)); gazebo::common::SphericalCoordinates::SPHERICAL));
fdm.hmsl = pose.pos.z; fdm.hmsl = pose.Pos().Z();
/* debug positions */ /* debug positions */
fdm.lla_pos_pprz = fdm.lla_pos; // Don't really care... fdm.lla_pos_pprz = fdm.lla_pos; // Don't really care...
fdm.lla_pos_geod = fdm.lla_pos; fdm.lla_pos_geod = fdm.lla_pos;
fdm.lla_pos_geoc = 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 */ /* 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::LOCAL,
gazebo::common::SphericalCoordinates::ECEF)); gazebo::common::SphericalCoordinates::ECEF));
fdm.body_ecef_vel = to_pprz_body(pose.rot.RotateVectorReverse(vel)); // Note: unused fdm.body_ecef_vel = to_pprz_body(pose.Rot().RotateVectorReverse(vel)); // Note: unused
fdm.ltp_ecef_vel = to_pprz_ned( fdm.ltp_ecef_vel = to_pprz_ned(sphere->VelocityTransform(vel, gazebo::common::SphericalCoordinates::LOCAL,
sphere->VelocityTransform(vel.Ign(),
gazebo::common::SphericalCoordinates::LOCAL,
gazebo::common::SphericalCoordinates::GLOBAL)); gazebo::common::SphericalCoordinates::GLOBAL));
fdm.ltpprz_ecef_vel = fdm.ltp_ecef_vel; // ??? fdm.ltpprz_ecef_vel = fdm.ltp_ecef_vel; // ???
/* acceleration */ /* 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::LOCAL,
gazebo::common::SphericalCoordinates::ECEF)); // Note: unused gazebo::common::SphericalCoordinates::ECEF)); // Note: unused
fdm.body_ecef_accel = to_pprz_body(pose.rot.RotateVectorReverse(accel)); fdm.body_ecef_accel = to_pprz_body(pose.Rot().RotateVectorReverse(accel));
fdm.ltp_ecef_accel = to_pprz_ned( fdm.ltp_ecef_accel = to_pprz_ned(sphere->VelocityTransform(accel, gazebo::common::SphericalCoordinates::LOCAL,
sphere->VelocityTransform(accel.Ign(),
gazebo::common::SphericalCoordinates::LOCAL,
gazebo::common::SphericalCoordinates::GLOBAL)); // Note: unused gazebo::common::SphericalCoordinates::GLOBAL)); // Note: unused
fdm.ltpprz_ecef_accel = fdm.ltp_ecef_accel; // ??? fdm.ltpprz_ecef_accel = fdm.ltp_ecef_accel; // ???
fdm.body_inertial_accel = fdm.body_ecef_accel; // Approximate, unused. 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 */ /* attitude */
// ecef_to_body_quat: unused // 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)); 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_quat = fdm.ltp_to_body_quat; // unused
fdm.ltpprz_to_body_eulers = fdm.ltp_to_body_eulers; // unused fdm.ltpprz_to_body_eulers = fdm.ltp_to_body_eulers; // unused
/* angular velocity */ /* 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 fdm.body_inertial_rotvel = fdm.body_ecef_rotvel; // Approximate
/* angular acceleration */ /* angular acceleration */
// body_ecef_rotaccel: unused // body_ecef_rotaccel: unused
// body_inertial_rotaccel: unused // body_inertial_rotaccel: unused
/* misc */ /* 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::LOCAL,
gazebo::common::SphericalCoordinates::GLOBAL)); // unused gazebo::common::SphericalCoordinates::GLOBAL)); // unused
fdm.ltp_h = to_pprz_ltp( fdm.ltp_h = to_pprz_ltp(sphere->VelocityTransform(world->MagneticField(), gazebo::common::SphericalCoordinates::LOCAL,
sphere->VelocityTransform(world->MagneticField(),
gazebo::common::SphericalCoordinates::LOCAL,
gazebo::common::SphericalCoordinates::GLOBAL)); gazebo::common::SphericalCoordinates::GLOBAL));
/* atmosphere */ /* atmosphere */
@@ -624,15 +600,14 @@ static void gazebo_write(double act_commands[], int commands_nb)
#ifdef NPS_ACTUATOR_MAX_ANGULAR_MOMENTUM #ifdef NPS_ACTUATOR_MAX_ANGULAR_MOMENTUM
// Spinup torque // Spinup torque
double udot = update_first_order_high_pass(&gazebo_actuators.highpass[i], sp); 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; torque += spinup_torque;
#endif #endif
// Apply force and torque to gazebo model // Apply force and torque to gazebo model
gazebo::physics::LinkPtr link = model->GetLink(gazebo_actuators.names[i]); 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));
} }
} }
@@ -929,7 +904,8 @@ static void gazebo_read_range_sensors(void)
} else { } else {
range = rays[i].sensor->Range(0); 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) { if (i == ray_sensor_agl_index) {
float agl = rays[i].sensor->Range(0); float agl = rays[i].sensor->Range(0);