Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-02-05 18:51:00 +08:00
Code Issues Actions 2 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

4
.travis.yml
View File

@@ -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

6
conf/modules/fdm_gazebo.xml
View File

@@ -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>

3
docker/ci/Dockerfile
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 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/*

234
sw/simulator/nps/nps_fdm_gazebo.cpp
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.
*
@@ -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,
NPS_ACTUATOR_THRUSTS, NPS_ACTUATOR_TORQUES, { }, { }, { } };
struct gazebo_actuators_t gazebo_actuators = { NPS_ACTUATOR_NAMES, 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.y = -body_g.y;
body_p.z = -body_g.z;
body_p.x = body_g.X();
body_p.y = -body_g.Y();
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.q = -body_g.y;
body_p.r = -body_g.z;
body_p.p = body_g.X();
body_p.q = -body_g.Y();
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.theta = -quat.GetPitch();
eulers.phi = quat.GetRoll();
eulers.psi = -quat.Yaw();
eulers.theta = -quat.Pitch();
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.theta = -quat.GetPitch();
eulers.phi = quat.GetRoll();
eulers.psi = -quat.Yaw() - M_PI / 2;
eulers.theta = -quat.Pitch();
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.y = xyz.x;
ltp.z = -xyz.z;
ltp.x = xyz.Y();
ltp.y = xyz.X();
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(
gazebodir + "models/");
gazebo::common::SystemPaths::Instance()->AddModelPaths(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(
pprz_home + "/sw/ext/tudelft_gazebo_models/models/");
gazebo::common::SystemPaths::Instance()->AddModelPaths(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) {
if(link->Get<string>("name") == "front_camera") {
while (link) {
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
gazebo::math::Vector3 vel = model->GetWorldLinearVel();
gazebo::math::Vector3 ang_vel = model->GetWorldAngularVel();
gazebo::common::SphericalCoordinatesPtr sphere =
world->GetSphericalCoordinates();
gazebo::math::Quaternion local_to_global_quat(0, 0,
-sphere->HeadingOffset().Radian());
ignition::math::Pose3d pose = model->WorldPose(); // In LOCAL xyz frame
ignition::math::Vector3d vel = model->WorldLinearVel();
ignition::math::Vector3d ang_vel = model->WorldAngularVel();
gazebo::common::SphericalCoordinatesPtr sphere = world->SphericalCoords();
ignition::math::Quaterniond 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(
sphere->PositionTransform(pose.pos.Ign(),
gazebo::common::SphericalCoordinates::LOCAL,
gazebo::common::SphericalCoordinates::ECEF));
fdm.ltpprz_pos = to_pprz_ned(
sphere->PositionTransform(pose.pos.Ign(),
gazebo::common::SphericalCoordinates::LOCAL,
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;
fdm.ecef_pos = to_pprz_ecef(sphere->PositionTransform(pose.Pos(), gazebo::common::SphericalCoordinates::LOCAL,
gazebo::common::SphericalCoordinates::ECEF));
fdm.ltpprz_pos = to_pprz_ned(sphere->PositionTransform(pose.Pos(), gazebo::common::SphericalCoordinates::LOCAL,
gazebo::common::SphericalCoordinates::GLOBAL));
fdm.lla_pos = to_pprz_lla(sphere->PositionTransform(pose.Pos(), 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(
sphere->VelocityTransform(vel.Ign(),
gazebo::common::SphericalCoordinates::LOCAL,
gazebo::common::SphericalCoordinates::ECEF));
fdm.body_ecef_vel = to_pprz_body(pose.rot.RotateVectorReverse(vel)); // Note: unused
fdm.ltp_ecef_vel = to_pprz_ned(
sphere->VelocityTransform(vel.Ign(),
gazebo::common::SphericalCoordinates::LOCAL,
gazebo::common::SphericalCoordinates::GLOBAL));
fdm.ecef_ecef_vel = to_pprz_ecef(sphere->VelocityTransform(vel, gazebo::common::SphericalCoordinates::LOCAL,
gazebo::common::SphericalCoordinates::ECEF));
fdm.body_ecef_vel = to_pprz_body(pose.Rot().RotateVectorReverse(vel)); // Note: unused
fdm.ltp_ecef_vel = to_pprz_ned(sphere->VelocityTransform(vel, gazebo::common::SphericalCoordinates::LOCAL,
gazebo::common::SphericalCoordinates::GLOBAL));
fdm.ltpprz_ecef_vel = fdm.ltp_ecef_vel; // ???
/* acceleration */
fdm.ecef_ecef_accel = to_pprz_ecef(
sphere->VelocityTransform(accel.Ign(),
gazebo::common::SphericalCoordinates::LOCAL,
gazebo::common::SphericalCoordinates::ECEF)); // Note: unused
fdm.body_ecef_accel = to_pprz_body(pose.rot.RotateVectorReverse(accel));
fdm.ltp_ecef_accel = to_pprz_ned(
sphere->VelocityTransform(accel.Ign(),
gazebo::common::SphericalCoordinates::LOCAL,
gazebo::common::SphericalCoordinates::GLOBAL)); // Note: unused
fdm.ecef_ecef_accel = to_pprz_ecef(sphere->VelocityTransform(accel, gazebo::common::SphericalCoordinates::LOCAL,
gazebo::common::SphericalCoordinates::ECEF)); // Note: unused
fdm.body_ecef_accel = to_pprz_body(pose.Rot().RotateVectorReverse(accel));
fdm.ltp_ecef_accel = to_pprz_ned(sphere->VelocityTransform(accel, 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.Ign() - world->Gravity()));
fdm.body_accel = to_pprz_body(pose.Rot().RotateVectorReverse(accel - 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(
sphere->VelocityTransform(-1 * world->Gravity(),
gazebo::common::SphericalCoordinates::LOCAL,
gazebo::common::SphericalCoordinates::GLOBAL)); // unused
fdm.ltp_h = to_pprz_ltp(
sphere->VelocityTransform(world->MagneticField(),
gazebo::common::SphericalCoordinates::LOCAL,
gazebo::common::SphericalCoordinates::GLOBAL));
fdm.ltp_g = to_pprz_ltp(sphere->VelocityTransform(-1 * world->Gravity(), gazebo::common::SphericalCoordinates::LOCAL,
gazebo::common::SphericalCoordinates::GLOBAL)); // unused
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] /
(2.0 * sqrt(u > 0.05 ? u : 0.05)) * udot;
double spinup_torque = gazebo_actuators.max_ang_momentum[i] / (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->AddRelativeTorque(gazebo::math::Vector3(0, 0, torque));
link->AddRelativeForce(ignition::math::Vector3d(0, 0, thrust));
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.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.center_y = cameras[i]->output_size.h/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.focal_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_y = MT9F002_FOCAL_Y,
.center_x = MT9F002_CENTER_X,
.center_y = MT9F002_CENTER_Y,
.Dhane_k = MT9F002_DHANE_K
};
.focal_x = MT9F002_FOCAL_X,
.focal_y = MT9F002_FOCAL_Y,
.center_x = MT9F002_CENTER_X,
.center_y = MT9F002_CENTER_Y,
.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]
- 0.368 * data_rgb[idx_rgb + 1]
- 0.071 * data_rgb[idx_rgb + 2] + 128; // V
data_yuv[idx_yuv] = 0.439 * data_rgb[idx_rgb]
- 0.368 * data_rgb[idx_rgb + 1]
- 0.071 * data_rgb[idx_rgb + 2] + 128; // V
}
data_yuv[idx_yuv + 1] = 0.257 * data_rgb[idx_rgb]
+ 0.504 * data_rgb[idx_rgb + 1]
+ 0.098 * data_rgb[idx_rgb + 2] + 16; // Y
data_yuv[idx_yuv + 1] = 0.257 * data_rgb[idx_rgb]
+ 0.504 * data_rgb[idx_rgb + 1]
+ 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);