35 #include <gazebo/gazebo.hh>
36 #include <gazebo/physics/physics.hh>
37 #include <gazebo/sensors/sensors.hh>
40 #include <gazebo/gazebo_config.h>
41 #if GAZEBO_MAJOR_VERSION < 8
42 #error "Paparazzi only supports gazebo versions > 7, please upgrade to a more recent version of Gazebo"
49 #include "generated/airframe.h"
50 #include "generated/flight_plan.h"
66 #if defined(NPS_DEBUG_VIDEO)
68 #include <opencv2/core/core.hpp>
69 #include <opencv2/highgui/highgui.hpp>
70 #include <opencv2/imgproc/imgproc.hpp>
75 #ifndef NPS_GAZEBO_WORLD
76 #define NPS_GAZEBO_WORLD "empty.world"
78 #ifndef NPS_GAZEBO_AC_NAME
79 #define NPS_GAZEBO_AC_NAME "ardrone"
83 #if NPS_SIMULATE_VIDEO
94 static void init_gazebo_video(
void);
95 static void gazebo_read_video(
void);
96 static void read_image(
98 gazebo::sensors::CameraSensorPtr cam);
100 gazebo::sensors::CameraSensorPtr cam;
101 gazebo::common::Time last_measurement_time;
109 #ifndef NPS_MT9F002_SENSOR_RES_DIVIDER
110 #define NPS_MT9F002_SENSOR_RES_DIVIDER 1
112 #endif // NPS_SIMULATE_VIDEO
125 #if NPS_SIMULATE_LASER_RANGE_ARRAY
130 static void gazebo_init_range_sensors(
void);
131 static void gazebo_read_range_sensors(
void);
135 std::shared_ptr<gazebo::sensors::SonarSensor>
sonar = NULL;
142 static gazebo::physics::ModelPtr
model = NULL;
145 static gazebo::sensors::ContactSensorPtr
ct;
150 static void gazebo_write(
double act_commands[],
int commands_nb);
156 ecef_p.
x = ecef_i.X();
157 ecef_p.
y = ecef_i.Y();
158 ecef_p.
z = ecef_i.Z();
174 lla_p.
lat = lla_i.X();
175 lla_p.
lon = lla_i.Y();
176 lla_p.
alt = lla_i.Z();
183 body_p.
x = body_g.X();
184 body_p.
y = -body_g.Y();
185 body_p.
z = -body_g.Z();
192 body_p.
p = body_g.X();
193 body_p.
q = -body_g.Y();
194 body_p.
r = -body_g.Z();
201 eulers.
psi = -quat.Yaw();
202 eulers.
theta = -quat.Pitch();
203 eulers.
phi = quat.Roll();
210 eulers.
psi = -quat.Yaw() - M_PI / 2;
211 eulers.
theta = -quat.Pitch();
212 eulers.
phi = quat.Roll();
237 #ifdef NPS_ACTUATOR_TIME_CONSTANTS
243 #ifdef NPS_ACTUATOR_MAX_ANGULAR_MOMENTUM
245 const float Iwmax[
NPS_COMMANDS_NB] = NPS_ACTUATOR_MAX_ANGULAR_MOMENTUM;
261 bool launch __attribute__((unused)),
262 double *act_commands,
274 #if NPS_SIMULATE_VIDEO
277 #if NPS_SIMULATE_LASER_RANGE_ARRAY
278 gazebo_init_range_sensors();
284 gazebo::runWorld(
model->GetWorld(), 1);
285 gazebo::sensors::run_once();
288 #if NPS_SIMULATE_VIDEO
291 #if NPS_SIMULATE_LASER_RANGE_ARRAY
292 gazebo_read_range_sensors();
300 double speed __attribute__((unused)),
301 double dir __attribute__((unused)))
306 double wind_north __attribute__((unused)),
307 double wind_east __attribute__((unused)),
308 double wind_down __attribute__((unused)))
313 double wind_speed __attribute__((unused)),
314 int turbulence_severity __attribute__((unused)))
320 double temp __attribute__((unused)),
321 double h __attribute__((unused)))
339 string gazebo_home =
"/conf/simulator/gazebo/";
340 string pprz_home(getenv(
"PAPARAZZI_HOME"));
341 string gazebodir = pprz_home + gazebo_home;
342 cout <<
"Gazebo directory: " << gazebodir << endl;
344 if (getenv(
"ROS_MASTER_URI")) {
346 cout <<
"Add ROS plugins... ";
347 gazebo::addPlugin(
"libgazebo_ros_paths_plugin.so");
348 gazebo::addPlugin(
"libgazebo_ros_api_plugin.so");
349 cout <<
"ok" << endl;
352 if (!gazebo::setupServer(0, NULL)) {
353 cout <<
"Failed to start Gazebo, exiting." << endl;
357 cout <<
"Add Paparazzi paths: " << gazebodir << endl;
358 gazebo::common::SystemPaths::Instance()->AddModelPaths(gazebodir +
"models/");
359 sdf::addURIPath(
"model://", gazebodir +
"models/");
360 sdf::addURIPath(
"world://", gazebodir +
"world/");
362 cout <<
"Add TU Delft paths: " << pprz_home +
"/sw/ext/tudelft_gazebo_models/" << endl;
363 gazebo::common::SystemPaths::Instance()->AddModelPaths(pprz_home +
"/sw/ext/tudelft_gazebo_models/models/");
364 sdf::addURIPath(
"model://", pprz_home +
"/sw/ext/tudelft_gazebo_models/models/");
365 sdf::addURIPath(
"world://", pprz_home +
"/sw/ext/tudelft_gazebo_models/world/");
369 string vehicle_filename = sdf::findFile(vehicle_uri,
false);
370 if (vehicle_filename.empty()) {
371 cout <<
"ERROR, could not find vehicle " + vehicle_uri << endl;
374 cout <<
"Load vehicle: " << vehicle_filename << endl;
375 sdf::SDFPtr vehicle_sdf(
new sdf::SDF());
377 if (!sdf::readFile(vehicle_filename, vehicle_sdf)) {
378 cout <<
"ERROR, could not read vehicle " + vehicle_filename << endl;
383 #if NPS_SIMULATE_VIDEO
385 sdf::ElementPtr link = vehicle_sdf->Root()->GetFirstElement()->GetElement(
"link");
387 if (link->Get<
string>(
"name") ==
"front_camera" && link->GetElement(
"sensor")->Get<
string>(
"name") ==
"mt9f002") {
388 if (NPS_MT9F002_SENSOR_RES_DIVIDER != 1) {
389 int w = link->GetElement(
"sensor")->GetElement(
"camera")->GetElement(
"image")->GetElement(
"width")->Get<
int>();
390 int h = link->GetElement(
"sensor")->GetElement(
"camera")->GetElement(
"image")->GetElement(
"height")->Get<
int>();
391 int env = link->GetElement(
"sensor")->GetElement(
"camera")->GetElement(
"lens")->GetElement(
"env_texture_size")->Get<
int>();
392 link->GetElement(
"sensor")->GetElement(
"camera")->GetElement(
"image")->GetElement(
"width")->Set(w / NPS_MT9F002_SENSOR_RES_DIVIDER);
393 link->GetElement(
"sensor")->GetElement(
"camera")->GetElement(
"image")->GetElement(
"height")->Set(
h / NPS_MT9F002_SENSOR_RES_DIVIDER);
394 link->GetElement(
"sensor")->GetElement(
"camera")->GetElement(
"lens")->GetElement(
"env_texture_size")->Set(env / NPS_MT9F002_SENSOR_RES_DIVIDER);
397 int fps =
Min(
MT9F002_TARGET_FPS, link->GetElement(
"sensor")->GetElement(
"update_rate")->Get<
int>());
398 link->GetElement(
"sensor")->GetElement(
"update_rate")->Set(fps);
400 }
else if (link->Get<
string>(
"name") ==
"bottom_camera" && link->GetElement(
"sensor")->Get<
string>(
"name") ==
"mt9v117") {
402 int fps =
Min(
MT9V117_TARGET_FPS, link->GetElement(
"sensor")->GetElement(
"update_rate")->Get<
int>());
403 link->GetElement(
"sensor")->GetElement(
"update_rate")->Set(fps);
406 link = link->GetNextElement(
"link");
411 #if NPS_SIMULATE_LASER_RANGE_ARRAY
412 vehicle_sdf->Root()->GetFirstElement()->AddElement(
"include")->GetElement(
"uri")->Set(
"model://range_sensors");
413 sdf::ElementPtr range_joint = vehicle_sdf->Root()->GetFirstElement()->AddElement(
"joint");
414 range_joint->GetAttribute(
"name")->Set(
"range_sensor_joint");
415 range_joint->GetAttribute(
"type")->Set(
"fixed");
416 range_joint->GetElement(
"parent")->Set(
"chassis");
417 range_joint->GetElement(
"child")->Set(
"range_sensors::base");
422 string world_filename = sdf::findFile(world_uri,
false);
423 if (world_filename.empty()) {
424 cout <<
"ERROR, could not find world " + world_uri << endl;
427 cout <<
"Load world: " << world_filename << endl;
428 sdf::SDFPtr world_sdf(
new sdf::SDF());
430 if (!sdf::readFile(world_filename, world_sdf)) {
431 cout <<
"ERROR, could not read world " + world_filename << endl;
436 world_sdf->Root()->GetFirstElement()->InsertElement(vehicle_sdf->Root()->GetFirstElement());
438 world_sdf->Write(pprz_home +
"/var/gazebo.world");
440 gazebo::physics::WorldPtr world = gazebo::loadWorld(pprz_home +
"/var/gazebo.world");
442 cout <<
"Failed to open world, exiting." << endl;
455 gazebo::sensors::run_once(
true);
456 gazebo::sensors::run_threads();
457 gazebo::runWorld(world, 1);
458 cout <<
"Sensors initialized..." << endl;
462 gazebo::sensors::SensorManager *mgr = gazebo::sensors::SensorManager::Instance();
463 ct = static_pointer_cast < gazebo::sensors::ContactSensor > (mgr->GetSensor(
"contactsensor"));
466 sonar = static_pointer_cast<gazebo::sensors::SonarSensor>(mgr->GetSensor(
"sonarsensor"));
468 cout <<
"Found sonar" << endl;
471 gazebo::physics::LinkPtr sonar_link =
model->GetLink(
"sonar");
474 if (sonar_link->GetSensorCount() != 1) {
475 cout <<
"ERROR: Link '" << sonar_link->GetName()
476 <<
"' should only contain 1 sensor!" << endl;
478 string name = sonar_link->GetSensorName(0);
479 sonar = static_pointer_cast< gazebo::sensors::SonarSensor > (mgr->GetSensor(name));
481 cout <<
"ERROR: Could not get pointer to '" << name <<
"'!" << endl;
484 sonar->SetActive(
true);
490 #ifdef MOTOR_MIXING_YAW_COEF
491 const double yaw_coef[] = MOTOR_MIXING_YAW_COEF;
498 cout <<
"Gazebo initialized successfully!" << endl;
511 static ignition::math::Vector3d vel_prev;
512 static double time_prev;
514 gazebo::physics::WorldPtr world =
model->GetWorld();
515 ignition::math::Pose3d pose =
model->WorldPose();
516 ignition::math::Vector3d vel =
model->WorldLinearVel();
517 ignition::math::Vector3d ang_vel =
model->WorldAngularVel();
518 gazebo::common::SphericalCoordinatesPtr sphere = world->SphericalCoords();
519 ignition::math::Quaterniond local_to_global_quat(0, 0, -sphere->HeadingOffset().Radian());
522 fdm.
time = world->SimTime().Double();
528 double dt =
fdm.
time - time_prev;
529 ignition::math::Vector3d accel = (vel - vel_prev) / dt;
546 for (
int i = 0; i < 3 * 3; i++) {
553 gazebo::common::SphericalCoordinates::GLOBAL));
565 double agl =
sonar->Range();
566 if (agl >
sonar->RangeMax()) agl = -1.0;
574 gazebo::common::SphericalCoordinates::GLOBAL));
581 gazebo::common::SphericalCoordinates::GLOBAL));
603 fdm.
ltp_g =
to_pprz_ltp(sphere->VelocityTransform(-1 * world->Gravity(), gazebo::common::SphericalCoordinates::LOCAL,
604 gazebo::common::SphericalCoordinates::GLOBAL));
605 fdm.
ltp_h =
to_pprz_ltp(sphere->VelocityTransform(world->MagneticField(), gazebo::common::SphericalCoordinates::LOCAL,
606 gazebo::common::SphericalCoordinates::GLOBAL));
609 #if GAZEBO_MAJOR_VERSION >= 8 && 0 // TODO implement
653 for (
int i = 0; i < commands_nb; ++i) {
658 #ifdef NPS_ACTUATOR_TIME_CONSTANTS
667 #ifdef NPS_ACTUATOR_MAX_ANGULAR_MOMENTUM
671 torque += spinup_torque;
676 link->AddRelativeForce(ignition::math::Vector3d(0, 0, thrust));
677 link->AddRelativeTorque(ignition::math::Vector3d(0, 0, torque));
681 #if NPS_SIMULATE_VIDEO
696 static void init_gazebo_video(
void)
698 gazebo::sensors::SensorManager *mgr = gazebo::sensors::SensorManager::Instance();
700 cout <<
"Initializing cameras..." << endl;
705 gazebo::physics::LinkPtr link =
model->GetLink(
cameras[i]->dev_name);
713 if (link->GetSensorCount() != 1) {
714 cout <<
"ERROR: Link '" << link->GetName()
715 <<
"' should only contain 1 sensor!" << endl;
718 string name = link->GetSensorName(0);
719 gazebo::sensors::CameraSensorPtr cam = static_pointer_cast
720 < gazebo::sensors::CameraSensor > (mgr->GetSensor(name));
722 cout <<
"ERROR: Could not get pointer to '" << name <<
"'!" << endl;
726 cam->SetActive(
true);
729 gazebo_cams[i].cam = cam;
730 gazebo_cams[i].last_measurement_time = cam->LastMeasurementTime();
746 if (cam->Name() ==
"mt9f002") {
762 }
else if (cam->Name() ==
"mt9v117") {
773 cout <<
"ok" << endl;
785 static void gazebo_read_video(
void)
788 gazebo::sensors::CameraSensorPtr &cam = gazebo_cams[i].cam;
790 if (cam == NULL) {
continue; }
793 if ((cam->LastMeasurementTime() - gazebo_cams[i].last_measurement_time).Float() < 0.005
794 || cam->LastMeasurementTime() == 0) {
continue; }
797 read_image(&img, cam);
800 cv::Mat RGB_cam(cam->ImageHeight(), cam->ImageWidth(), CV_8UC3, (
uint8_t *)cam->ImageData());
801 cv::cvtColor(RGB_cam, RGB_cam, cv::COLOR_RGB2BGR);
802 cv::namedWindow(
cameras[i]->dev_name, cv::WINDOW_AUTOSIZE);
803 cv::imshow(
cameras[i]->dev_name, RGB_cam);
811 gazebo_cams[i].last_measurement_time = cam->LastMeasurementTime();
825 static void read_image(
struct image_t *img, gazebo::sensors::CameraSensorPtr cam)
827 bool is_mt9f002 =
false;
828 if (cam->Name() ==
"mt9f002") {
836 const uint8_t *data_rgb = cam->ImageData();
838 for (
int x_yuv = 0; x_yuv < img->
w; ++x_yuv) {
839 for (
int y_yuv = 0; y_yuv < img->
h; ++y_yuv) {
850 int idx_rgb = 3 * (cam->ImageWidth() * y_rgb + x_rgb);
851 int idx_yuv = 2 * (img->
w * y_yuv + x_yuv);
852 int idx_px = img->
w * y_yuv + x_yuv;
853 if (idx_px % 2 == 0) {
854 data_yuv[idx_yuv] = - 0.148 * data_rgb[idx_rgb]
855 - 0.291 * data_rgb[idx_rgb + 1]
856 + 0.439 * data_rgb[idx_rgb + 2] + 128;
858 data_yuv[idx_yuv] = 0.439 * data_rgb[idx_rgb]
859 - 0.368 * data_rgb[idx_rgb + 1]
860 - 0.071 * data_rgb[idx_rgb + 2] + 128;
862 data_yuv[idx_yuv + 1] = 0.257 * data_rgb[idx_rgb]
863 + 0.504 * data_rgb[idx_rgb + 1]
864 + 0.098 * data_rgb[idx_rgb + 2] + 16;
868 gazebo::common::Time
ts = cam->LastMeasurementTime();
869 img->
ts.tv_sec =
ts.sec;
870 img->
ts.tv_usec =
ts.nsec / 1000.0;
876 #if NPS_SIMULATE_LASER_RANGE_ARRAY
899 struct gazebo_ray_t {
900 gazebo::sensors::RaySensorPtr sensor;
901 gazebo::common::Time last_measurement_time;
904 static struct gazebo_ray_t rays[LASER_RANGE_ARRAY_NUM_SENSORS] = {{NULL, 0}};
905 static float range_orientation[] = LASER_RANGE_ARRAY_ORIENTATIONS;
906 static uint8_t ray_sensor_agl_index = 255;
908 #define VL53L0_MAX_VAL 8.191f
910 static void gazebo_init_range_sensors(
void)
912 gazebo::sensors::SensorManager *mgr = gazebo::sensors::SensorManager::Instance();
913 gazebo::sensors::Sensor_V
sensors = mgr->GetSensors();
917 gazebo::sensors::RaySensorPtr ray_sensor;
918 uint8_t ray_sensor_count_selected = 0;
921 for (
uint16_t i = 0; i < sensor_count; i++) {
922 if (ray_sensor_count_selected > LASER_RANGE_ARRAY_NUM_SENSORS) {
926 if (
sensors.at(i)->Type() ==
"ray") {
927 ray_sensor = static_pointer_cast<gazebo::sensors::RaySensor>(
sensors.at(i));
930 cout <<
"ERROR: Could not get pointer to raysensor " << i <<
"!" << endl;
943 for (
int j = 0; j < LASER_RANGE_ARRAY_NUM_SENSORS; j++) {
944 struct DoubleEulers def = {0, range_orientation[j * 2], range_orientation[j * 2 + 1]};
949 if (fabs(angle) < RadOfDeg(5)) {
950 ray_sensor_count_selected++;
951 rays[j].sensor = ray_sensor;
952 rays[j].sensor->SetActive(
true);
954 #if LASER_RANGE_ARRAY_SEND_AGL
956 if (fabs(range_orientation[j * 2] + M_PI_2) < RadOfDeg(5)) {
957 ray_sensor_agl_index = j;
966 if (ray_sensor_count_selected != LASER_RANGE_ARRAY_NUM_SENSORS) {
967 cout <<
"ERROR: you have defined " << LASER_RANGE_ARRAY_NUM_SENSORS <<
" sensors in your airframe file, but only "
968 << ray_sensor_count_selected <<
" sensors have been found in the gazebo simulator, "
969 "with the same orientation as in the airframe file " << endl;
972 cout <<
"Initialized laser range array" << endl;
975 static void gazebo_read_range_sensors(
void)
980 for (
int i = 0; i < LASER_RANGE_ARRAY_NUM_SENSORS; i++) {
981 if ((rays[i].sensor->LastMeasurementTime() - rays[i].last_measurement_time).Float() < 0.005
982 || rays[i].sensor->LastMeasurementTime() == 0) {
continue; }
984 if (rays[i].sensor->Range(0) < 1e-5 || rays[i].sensor->Range(0) >
VL53L0_MAX_VAL) {
987 range = rays[i].sensor->Range(0);
990 range_orientation[i * 2 + 1]);
992 if (i == ray_sensor_agl_index) {
994 float agl = rays[i].sensor->Range(0);
996 if (agl > 1e-5 && !isinf(agl)) {
1000 rays[i].last_measurement_time = rays[i].sensor->LastMeasurementTime();
1003 #endif // NPS_SIMULATE_LASER_RANGE_ARRAY
1005 #pragma GCC diagnostic pop // Ignore -Wdeprecated-declarations