From 9d1e71b1ea1430dfa0439fcb815a1e87f0ecb5c0 Mon Sep 17 00:00:00 2001 From: tomvand Date: Tue, 13 Jun 2017 22:12:11 +0200 Subject: [PATCH] Add Gazebo simulation to NPS (#2069) * Setup code for Gazebo Currently only partially initializes the fdm struct, which produces incorrect results in the GCS. Compilation, however, already seems to work as expected! Next step is to fetch data from Gazebo. * Closed-loop flight in Gazebo Finished the first implementation of the Gazebo FDM. It is now possible to perform closed-loop flights using Paparazzi together with Gazebo. Look at the example_ardrone2.xml airframe file for the required gazebo tags. The following modifications are made: - change fdm type to "gazebo" - add definitions for ACTUATOR_THRUSTS and ACTUATOR_TORQUES. These were obtained from the JSBsim ardrone model. The oredr is the same as the ACTUATOR_NAMES. The Gazebo world needs an aircraft model that is (by default) named "paparazzi_uav". This model should include links with the names listed in ACTUATOR_NAMES in the airframe file, as these are used to apply forces on the quadrotor. Known bugs: - No fixed wing aircraft yet. - Not all fields in the fdm struct are set. Most of the atmosphere fields are not implemented yet. - AHRS and INS need to be bypassed, these do not yet work correctly. - No quadrotor models are supplied with paparazzi yet (although it is included in the example world file). * Added cameras, but sensor update causes crash As in the quick standalone examples, gazebo regularly crashes when cameras are used. This is an intermediary commit where most of the camera infrastructure is present, but not functional yet because of an unknown bug regarding Gazebo. * Simplified problem, located crash Crash occurs when gazebo::sensors::run_once() is called during the fdm update step. The exception (Ogre internal exception, cannot create GL vertex buffer) only occurs after the main loop has been performed a few times (10-20x typically). Ignoring the error by catching the exception does not make it go away. * Add ogre to fdm_gazebo makefile * Further changes for debugging, not fixed yet * FIX bug Turns out that fdm_init and fdm_run_step are not running in the same thread, which apparently caused some problems with Gazebo. The gazebo initialization code that starts the server is now moved to the run_step function, and cameras work fine now. Next step is to get the images from Gazebo to the video_thread_nps module. * Transport images to Paparazzi Added code that converts Gazebo's RGB888 images to Paparazzi's YUV422 format, and calls the registered video callbacks. * Finish video streaming Finished video streaming. Video streaming in the paparazzi master branch does not seem to work in simulation or even on a real ardrone. Therefore, reverted to v5.10 viewvideo.c and rtp.c and now the streaming works correctly. * Separate UAV model from world. Added a models/ directory to conf/simulator/gazebo, now the UAV model can be included in the world file. The models/ dir is added to Gazebo's search paths when the server is launched, so it does not require any additional steps from the user. * Move Gazebo modifications from examples/ardrone2.xml to examples/ardrone2_gazebo.xml Reverted changes to the original airframe and moved these to a separate example. * Remove .sdp file Remove .sdp file that should not have been included with commits. * Move changes in conf_example.xml to separate entry Moved the changes to conf_example.xml (selected flightplan, modules etc.) to a separate etry named 'ardrone2 (Gazebo)'. * Clean up changes to computer_vision module - Removed unused viewvideo_nps.c. - Add doc comments in video_thread_nps.c * Fix bug in conf_example Spaces in aircraft name caused compilation error, changed to 'ardrone2_gazebo'. * Fix duplicate ac_id in conf_example * Remove debug code from nps_fdm_gazebo.cpp and add comments * Reduce weight of cameras Cameras weighed 10 grams, which was quite much. Now reduced to 1 gram. * Fix camera not active During the cleanup of the ardrone gazebo model, the always_on tag was removed. Now, the cameras are activated during gazebo_video_init Also removed further debug code from nps_fdm_gazebo.cpp. * Restore master branch video streamer and rtp encoder The streamer of the master branh does not work correctly atm, but this will be moved to a separate issue. * Fix compilation warnings Fixed all compilation warnings (tested with Gazebo 8). Unused arguments are now marked as such. The NPS makefile is modified to use CXXFLAGS in addition to CFLAGS, which solves the std=c++11 related warnings. fdm_gazebo.cpp now ignores deprecated warnings, as all of these come from Gazebo. Not the cleanest solution, but it works for now. * Add documentation to fdm module xml and add copyright notices where appropriate * Fix code style Fixed code style using the tool included with paparazzi. * Fix missing model.config .gitignore caused the model.config files for the Gazebo models to be excluded from the commit. Added an exception to .gitignore and added the missing file. * Fix INS by adding placeholder atmospheric data values Added placeholder values to the atmospheric section of the NPM struct. These should be valid for low altitude, low speed flights. Previously, these values were uninitialized which caused INS to fail (incorrect altitude estimates). With these placeholder values, the INS works ok and no longer needs to be bypassed. * Fix makefile targets in video_rtp_stream and cv_colorfilter (Re)added target="ap|nps" attribute to the module .xml files. --- .gitignore | 1 + conf/Makefile.nps | 1 + conf/airframes/examples/ardrone2_gazebo.xml | 209 +++++++ conf/conf_example.xml | 11 + conf/modules/cv_colorfilter.xml | 2 +- conf/modules/fdm_gazebo.xml | 92 +++ conf/modules/video_rtp_stream.xml | 2 +- .../gazebo/models/ardrone/ardrone.sdf | 234 +++++++ .../gazebo/models/ardrone/model.config | 15 + conf/simulator/gazebo/world/ardrone.world | 41 ++ sw/airborne/boards/pc_sim.h | 4 + .../computer_vision/video_thread_nps.c | 119 ++-- .../computer_vision/video_thread_nps.h | 39 ++ .../modules/computer_vision/viewvideo_nps.c | 42 -- sw/simulator/nps/nps_fdm_gazebo.cpp | 588 ++++++++++++++++++ 15 files changed, 1313 insertions(+), 87 deletions(-) create mode 100644 conf/airframes/examples/ardrone2_gazebo.xml create mode 100644 conf/modules/fdm_gazebo.xml create mode 100644 conf/simulator/gazebo/models/ardrone/ardrone.sdf create mode 100644 conf/simulator/gazebo/models/ardrone/model.config create mode 100644 conf/simulator/gazebo/world/ardrone.world create mode 100644 sw/airborne/modules/computer_vision/video_thread_nps.h delete mode 100644 sw/airborne/modules/computer_vision/viewvideo_nps.c create mode 100644 sw/simulator/nps/nps_fdm_gazebo.cpp diff --git a/.gitignore b/.gitignore index fd228d9706f..4883fb4eadf 100644 --- a/.gitignore +++ b/.gitignore @@ -65,6 +65,7 @@ paparazzi.sublime-workspace /conf/maps_data/* /conf/maps.xml /conf/gps/ublox_conf +!/conf/simulator/gazebo/**/*.config # /doc/pprz_algebra/ /doc/pprz_algebra/headfile.log diff --git a/conf/Makefile.nps b/conf/Makefile.nps index d2d57ede4cc..ccbda581275 100644 --- a/conf/Makefile.nps +++ b/conf/Makefile.nps @@ -45,6 +45,7 @@ CFLAGS += $(shell pkg-config --cflags-only-I ivy-glib) CXXFLAGS = -W -Wall CXXFLAGS += $(INCLUDES) CXXFLAGS += $($(TARGET).CFLAGS) +CXXFLAGS += $($(TARGET).CXXFLAGS) CXXFLAGS += $(USER_CFLAGS) $(BOARD_CFLAGS) CXXFLAGS += -O$(OPT) CXXFLAGS += $(DEBUG_FLAGS) diff --git a/conf/airframes/examples/ardrone2_gazebo.xml b/conf/airframes/examples/ardrone2_gazebo.xml new file mode 100644 index 00000000000..6db02e1b3ba --- /dev/null +++ b/conf/airframes/examples/ardrone2_gazebo.xml @@ -0,0 +1,209 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + +
+ + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + +
+ + + +
+ + + + + + + +
+ +
+ + +
+ + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + +
+ +
+ + + + + + + +
+ +
+ + + + + + + +
+ +
+ + + + +
+ +
+ + + + + +
+ diff --git a/conf/conf_example.xml b/conf/conf_example.xml index 457bb9a3cc1..1cc7083575b 100644 --- a/conf/conf_example.xml +++ b/conf/conf_example.xml @@ -241,6 +241,17 @@ settings_modules="modules/video_rtp_stream.xml modules/geo_mag.xml modules/air_data.xml modules/gps_ubx_ucenter.xml modules/ins_extended.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml" gui_color="red" /> + - + diff --git a/conf/modules/fdm_gazebo.xml b/conf/modules/fdm_gazebo.xml new file mode 100644 index 00000000000..ce32db93a71 --- /dev/null +++ b/conf/modules/fdm_gazebo.xml @@ -0,0 +1,92 @@ + + + + + + Gazebo backend for NPS simulator + 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) + 2. Prepare the Gazebo world and model: + a) Prepare the UAV model (see conf/simulator/gazebo/models/ardrone/): + Place the aircraft model in the conf/simulator/gazebo/models/ + folder, this folder is added to Gazebo's search path when NPS is + launched. + Gazebo uses a Front, Left, Up coordinate system for aircraft, so + make sure the +x axis points forwards. + The model should include a link for each motor with the same names + as those listed in NPS_ACTUATOR_NAMES (see below), e.g. 'nw_motor'. + Camera links should have the name specified in .dev_name in the + corresponding video_config_t struct, see sw/airborne/boards/pc_sim.h + and sw/airborne/modules/computer_vision/video_thread_nps.c. + b) Prepare the world (see conf/simulator/gazebo/worlds/ardrone.world). + Pay attention to the following: + The real-time update rate should be set to zero, as this is + already handled by Paparazzi: + <physics type="ode"> + <max_step_size>0.001</max_step_size> + <real_time_update_rate>0</real_time_update_rate><!-- Handled by Paparazzi! --> + </physics> + Spherical coordinates should be provided for navigation. + At this moment, there is an issue where Gazebo incorrectly + uses a WSU coordinate system instead of ENU. This can be fixed + by setting the heading to 180 degrees as shown below: + <spherical_coordinates> + <surface_model>EARTH_WGS84</surface_model> + <latitude_deg>51.9906</latitude_deg> + <longitude_deg>4.37679</longitude_deg> + <elevation>0</elevation> + <heading_deg>180</heading_deg><!-- Temporary fix for issue https://bitbucket.org/osrf/gazebo/issues/2022/default-sphericalcoordinates-frame-should --> + </spherical_coordinates> + 3. Prepare the airframe file (see examples/ardrone2_gazebo.xml): + a) Select Gazebo as the FDM (Flight Dynamics Model) + <target name="nps" board="pc"> + <module name="fdm" type="gazebo"/> + </target> + b) Add actuator thrusts and torques to the SIMULATOR section: + <section name="SIMULATOR" prefix="NPS_"> + <define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/> + <define name="ACTUATOR_THRUSTS" value="1.55, 1.55, 1.55, 1.55" type="double[]"/> + <define name="ACTUATOR_TORQUES" value="0.155, -0.155, 0.155, -0.155" type="double[]"/> + ... + <section> + The thrusts and torques are expressed in SI units (N, Nm) and should + be in the same order as the ACTUATOR_NAMES. + c) In the same section, bypass the AHRS and INS as these are not + supported yet: + <section name="SIMULATOR" prefix="NPS_"> + ... + <define name="BYPASS_AHRS" value="1"/> + <define name="BYPASS_INS" value="1"/> + ... + <section> + d) If required, enable video thread simulation: + <section name="SIMULATOR" prefix="NPS_"> + ... + <define name="SIMULATE_VIDEO" value="1"/> + ... + <section> + e) If required, specify the Gazebo world and aircraft name: + <section name="SIMULATOR" prefix="NPS_"> + ... + <define name="GAZEBO_WORLD" value="my_world.world"/> + <define name="GAZEBO_AC_NAME" value="my_uav"/> + <section> + 4. Make sure all included modules work with nps. At the moment, most of + the modules that depend on video_thread are only built when ap is + selected as the target. As a quick fix, try to remove the target + attribute from the makefile element in the module xml, e.g.: + <makefile target="ap"> ---> <makefile> + + +
+ + + nps.CXXFLAGS += $(shell pkg-config gazebo --cflags) + nps.LDFLAGS += $(shell pkg-config gazebo --libs) + + + + + diff --git a/conf/modules/video_rtp_stream.xml b/conf/modules/video_rtp_stream.xml index f9523dd6fae..815659bde59 100644 --- a/conf/modules/video_rtp_stream.xml +++ b/conf/modules/video_rtp_stream.xml @@ -34,7 +34,7 @@
- + diff --git a/conf/simulator/gazebo/models/ardrone/ardrone.sdf b/conf/simulator/gazebo/models/ardrone/ardrone.sdf new file mode 100644 index 00000000000..6445c39ad8f --- /dev/null +++ b/conf/simulator/gazebo/models/ardrone/ardrone.sdf @@ -0,0 +1,234 @@ + + + + 0 0 .1 0 0 0 + + + + 0.4 + + 0.005 + 0.005 + 0.010 + 0. + 0. + 0. + + + + + + + .4 .4 .05 + + + + + + + + .2 .2 .05 + + + + + + + 0.15 0 0 0 0 0 + + 0.001 + + .0001 + .0001 + .0001 + 0 + 0 + 0 + + + + 30.0 + + 1.3962634 + + 1280 + 720 + R8G8B8 + + + 0.02 + 300 + + + gaussian + + 0.0 + 0.007 + + + + + + + chassis + front_camera + + + + 0 0 -.03 0 1.57 0 + + 0.001 + + .0001 + .0001 + .0001 + 0 + 0 + 0 + + + + 30.0 + + 1.3962634 + + 320 + 240 + R8G8B8 + + + 0.02 + 300 + + + gaussian + + 0.0 + 0.007 + + + + + + + chassis + bottom_camera + + + + 0.12 0.12 0 0 0 0 + + 0.01 + + .0001 + .0001 + .0001 + 0 + 0 + 0 + + + + + + 0.10 + .07 + + + + + + + chassis + nw_motor + + + + -0.12 -0.12 0 0 0 0 + + 0.01 + + .0001 + .0001 + .0001 + 0 + 0 + 0 + + + + + + 0.10 + .07 + + + + + + + chassis + se_motor + + + + 0.12 -0.12 0 0 0 0 + + 0.01 + + .0001 + .0001 + .0001 + 0 + 0 + 0 + + + + + + 0.10 + .07 + + + + + + + chassis + ne_motor + + + + -0.12 0.12 0 0 0 0 + + 0.01 + + .0001 + .0001 + .0001 + 0 + 0 + 0 + + + + + + 0.10 + .07 + + + + + + + chassis + sw_motor + + + \ No newline at end of file diff --git a/conf/simulator/gazebo/models/ardrone/model.config b/conf/simulator/gazebo/models/ardrone/model.config new file mode 100644 index 00000000000..6a427c8df28 --- /dev/null +++ b/conf/simulator/gazebo/models/ardrone/model.config @@ -0,0 +1,15 @@ + + + ARDrone2 (Paparazzi) + 1.0 + ardrone.sdf + + + Tom van Dijk + tomvand@users.noreply.github.com + + + + Simple ARDrone2 model for use with Paparazzi's NPS (http://wiki.paparazziuav.org). + + \ No newline at end of file diff --git a/conf/simulator/gazebo/world/ardrone.world b/conf/simulator/gazebo/world/ardrone.world new file mode 100644 index 00000000000..8c1f9972271 --- /dev/null +++ b/conf/simulator/gazebo/world/ardrone.world @@ -0,0 +1,41 @@ + + + + + 0.001 + 0 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.1 0.1 0.1 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.5 -1 + + + EARTH_WGS84 + 51.9906 + 4.37679 + 0 + 180 + + + + model://ground_plane + + + model://ardrone + + + \ No newline at end of file diff --git a/sw/airborne/boards/pc_sim.h b/sw/airborne/boards/pc_sim.h index bdbc8062795..11786598a53 100644 --- a/sw/airborne/boards/pc_sim.h +++ b/sw/airborne/boards/pc_sim.h @@ -20,4 +20,8 @@ #endif extern struct video_config_t webcam; +// Simulated cameras, see modules/computer_vision/video_thread_nps.c +extern struct video_config_t front_camera; +extern struct video_config_t bottom_camera; + #endif /* CONFIG_PC_SIM_H */ diff --git a/sw/airborne/modules/computer_vision/video_thread_nps.c b/sw/airborne/modules/computer_vision/video_thread_nps.c index f1111e0d7d2..438dee5e1b2 100644 --- a/sw/airborne/modules/computer_vision/video_thread_nps.c +++ b/sw/airborne/modules/computer_vision/video_thread_nps.c @@ -1,5 +1,5 @@ /* - * Copyright (C) 2015 + * Copyright (C) 2017 Tom van Dijk * * This file is part of Paparazzi. * @@ -20,62 +20,95 @@ */ /** - * Dummy C implementation for simulation - * The V4L2 could also work in simulation, but must be adapted a bit. + * Video thread dummy for simulation. * + * + * Keeps track of added devices, which can be referenced by simulation code + * such as in simulator/nps/fdm_gazebo.c. */ // Own header +#include "video_thread_nps.h" #include "video_thread.h" #include "cv.h" #include "lib/vision/image.h" +#include "modules/computer_vision/lib/v4l/v4l2.h" +#include "peripherals/video_device.h" + +#include + +// Camera structs for use in modules. +// See boards/pc_sim.h +// Default values from ARDrone can be overwritten by simulator. +struct video_config_t front_camera = { + .output_size = { .w = 1280, .h = 720 }, + .sensor_size = { .w = 1280, .h = 720 }, + .crop = { .x = 0, .y = 0, .w = 1280, .h = 720 }, + .dev_name = "front_camera", + .subdev_name = NULL, + .format = V4L2_PIX_FMT_UYVY, + .buf_cnt = 10, + .filters = 0, + .cv_listener = NULL, + .fps = 0 +}; -// Initialize the video_thread structure with the defaults -struct video_thread_t video_thread = { - .is_running = FALSE +struct video_config_t bottom_camera = { + .output_size = { .w = 320, .h = 240 }, + .sensor_size = { .w = 320, .h = 240 }, + .crop = { .x = 0, .y = 0, .w = 320, .h = 240 }, + .dev_name = "bottom_camera", + .subdev_name = NULL, + .format = V4L2_PIX_FMT_UYVY, + .buf_cnt = 10, + .filters = 0, + .cv_listener = NULL, + .fps = 0 }; +// Keep track of added devices. +struct video_config_t *cameras[VIDEO_THREAD_MAX_CAMERAS] = { NULL }; + // All dummy functions -void video_thread_init(void) {} +void video_thread_init(void) +{ +} void video_thread_periodic(void) { - struct image_t img; - image_create(&img, 320, 240, IMAGE_YUV422); - int i, j; - uint8_t u, v; - -#ifdef SMARTUAV_SIMULATOR - SMARTUAV_IMPORT(&img); -#else - if (video_thread.is_running) { - u = 0; - v = 255; - } else { - u = 255; - v = 0; - } - uint8_t *p = (uint8_t *) img.buf; - for (j = 0; j < img.h; j++) { - for (i = 0; i < img.w; i += 2) { - *p++ = u; - *p++ = j; - *p++ = v; - *p++ = j; - } - } - video_thread.is_running = ! video_thread.is_running; -#endif - - // Calling this function will not work because video_config_t is NULL (?) and img - // has no timestamp - // Commenting out for now - // cv_run_device(NULL,&img); - image_free(&img); } -void video_thread_start(void) {} -void video_thread_stop(void) {} -void video_thread_take_shot(bool take __attribute__((unused))) {} +void video_thread_start(void) +{ +} +void video_thread_stop(void) +{ +} -bool add_video_device(struct video_config_t *device __attribute__((unused))){ return true; } +/** + * Keep track of video devices added by modules. + */ +bool add_video_device(struct video_config_t *device) +{ + // Loop over camera array + for (int i = 0; i < VIDEO_THREAD_MAX_CAMERAS; ++i) { + // If device is already registered, break + if (cameras[i] == device) { + break; + } + // If camera slot is already used, continue + if (cameras[i] != NULL) { + continue; + } + // No initialization, should be handled by simulation! + // Store device pointer + cameras[i] = device; + // Debug statement + printf("[video_thread_nps] Added %s to camera array.\n", + device->dev_name); + // Successfully added + return true; + } + // Camera array is full + return false; +} diff --git a/sw/airborne/modules/computer_vision/video_thread_nps.h b/sw/airborne/modules/computer_vision/video_thread_nps.h new file mode 100644 index 00000000000..feff357c9f0 --- /dev/null +++ b/sw/airborne/modules/computer_vision/video_thread_nps.h @@ -0,0 +1,39 @@ +/* + * Copyright (C) 2017 Tom van Dijk + * + * This file is part of Paparazzi. + * + * Paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * Paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + * + */ + +/** + * @file modules/computer_vision/video_thread_nps.h + * + * This header gives NPS access to the list of added cameras. + */ + +#ifndef VIDEO_THREAD_NPS_H +#define VIDEO_THREAD_NPS_H + +#include "peripherals/video_device.h" + +#ifndef VIDEO_THREAD_MAX_CAMERAS +#define VIDEO_THREAD_MAX_CAMERAS 4 +#endif + +extern struct video_config_t *cameras[VIDEO_THREAD_MAX_CAMERAS]; + +#endif diff --git a/sw/airborne/modules/computer_vision/viewvideo_nps.c b/sw/airborne/modules/computer_vision/viewvideo_nps.c deleted file mode 100644 index cd1120521dd..00000000000 --- a/sw/airborne/modules/computer_vision/viewvideo_nps.c +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Copyright (C) 2012-2013 - * - * This file is part of Paparazzi. - * - * Paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * Paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -/** - * Dummy C implementation for simulation - * The V4L2 could also work in simulation, but must be adapted a bit. - */ - -// Own header -#include "viewvideo.h" - -// Initialize the viewvideo structure with the defaults -struct viewvideo_t viewvideo = { - .is_streaming = FALSE, - .downsize_factor = 1, - .quality_factor = 99, -}; - -// All dummy functions -void viewvideo_init(void) {} -void viewvideo_periodic(void) {} -void viewvideo_start(void) {} -void viewvideo_stop(void) {} -void viewvideo_take_shot(bool take __attribute__((unused))) {} diff --git a/sw/simulator/nps/nps_fdm_gazebo.cpp b/sw/simulator/nps/nps_fdm_gazebo.cpp new file mode 100644 index 00000000000..0e47bce2aa4 --- /dev/null +++ b/sw/simulator/nps/nps_fdm_gazebo.cpp @@ -0,0 +1,588 @@ +/* + * Copyright (C) 2017 Tom van Dijk + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file nps_fdm_gazebo.cpp + * Flight Dynamics Model (FDM) for NPS using Gazebo. + * + * 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 +#include +#include +#include + +#include +#include +#include +#include +#include + +extern "C" { +#include + +#include "nps_fdm.h" +#include "math/pprz_algebra_double.h" + +#include "generated/airframe.h" +#include "autopilot.h" + +#ifdef NPS_SIMULATE_VIDEO +#include "modules/computer_vision/cv.h" +#include "modules/computer_vision/video_thread_nps.h" +#include "modules/computer_vision/lib/vision/image.h" +#include "mcu_periph/sys_time.h" +#endif +} + +using namespace std; + +#ifndef NPS_GAZEBO_WORLD +#define NPS_GAZEBO_WORLD "ardrone.world" +#endif +#ifndef NPS_GAZEBO_AC_NAME +#define NPS_GAZEBO_AC_NAME "paparazzi_uav" +#endif + +// Add video handling functions if req'd. +#ifdef NPS_SIMULATE_VIDEO +static void init_gazebo_video(void); +static void gazebo_read_video(void); +static void read_image( + struct image_t *img, + gazebo::sensors::CameraSensorPtr cam); +struct gazebocam_t { + gazebo::sensors::CameraSensorPtr cam; + gazebo::common::Time last_measurement_time; +}; +static struct gazebocam_t gazebo_cams[VIDEO_THREAD_MAX_CAMERAS] = +{ { NULL, 0 } }; +#endif + +/// Holds all necessary NPS FDM state information +struct NpsFdm fdm; + +// Pointer to Gazebo data +static bool gazebo_initialized = false; +static gazebo::physics::ModelPtr model = NULL; + +// Helper functions +static void init_gazebo(void); +static void gazebo_read(void); +static void gazebo_write(double commands[], int commands_nb); + +// Conversion routines +inline struct EcefCoor_d to_pprz_ecef(ignition::math::Vector3d ecef_i) +{ + struct EcefCoor_d ecef_p; + ecef_p.x = ecef_i.X(); + ecef_p.y = ecef_i.Y(); + ecef_p.z = ecef_i.Z(); + return ecef_p; +} + +inline struct NedCoor_d to_pprz_ned(ignition::math::Vector3d global) +{ + struct NedCoor_d ned; + ned.x = global.Y(); + ned.y = global.X(); + ned.z = -global.Z(); + return ned; +} + +inline struct LlaCoor_d to_pprz_lla(ignition::math::Vector3d lla_i) +{ + struct LlaCoor_d lla_p; + lla_p.lat = lla_i.X(); + lla_p.lon = lla_i.Y(); + lla_p.alt = lla_i.Z(); + return lla_p; +} + +inline struct DoubleVect3 to_pprz_body(gazebo::math::Vector3 body_g) +{ + struct DoubleVect3 body_p; + 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) +{ + struct DoubleRates body_p; + 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) +{ + struct DoubleEulers eulers; + eulers.psi = -quat.GetYaw() - M_PI / 2; + eulers.theta = -quat.GetPitch(); + eulers.phi = quat.GetRoll(); + return eulers; +} + +inline struct DoubleVect3 to_pprz_ltp(gazebo::math::Vector3 xyz) +{ + struct DoubleVect3 ltp; + ltp.x = xyz.y; + ltp.y = xyz.x; + ltp.z = -xyz.z; + return ltp; +} + +// External functions, interface with Paparazzi's NPS as declared in nps_fdm.h + +/** + * Set JSBsim specific fields that are not used for Gazebo. + * @param dt + */ +void nps_fdm_init(double dt) +{ + fdm.init_dt = dt; // JSBsim specific + fdm.curr_dt = dt; // JSBsim specific + fdm.nan_count = 0; // JSBsim specific +} + +/** + * Update the simulation state. + * @param launch + * @param commands + * @param commands_nb + */ +void nps_fdm_run_step( + bool launch __attribute__((unused)), + double *commands, + int commands_nb) +{ + // Initialize Gazebo if req'd. + // Initialization is peformed here instead of in nps_fdm_init because: + // - Video devices need to added at this point. Video devices have not been + // added yet when nps_fdm_init is called. + // - nps_fdm_init runs on a different thread then nps_fdm_run_step, which + // causes problems with Gazebo. + if (!gazebo_initialized) { + init_gazebo(); + gazebo_read(); +#ifdef NPS_SIMULATE_VIDEO + init_gazebo_video(); +#endif + gazebo_initialized = true; + } + + // Update the simulation for a single timestep. + gazebo::runWorld(model->GetWorld(), 1); + gazebo::sensors::run_once(); + gazebo_write(commands, commands_nb); + gazebo_read(); +#ifdef NPS_SIMULATE_VIDEO + gazebo_read_video(); +#endif +} + +// TODO Atmosphere functions have not been implemented yet. +// Starting at version 8, Gazebo has its own atmosphere and wind model. +void nps_fdm_set_wind( + double speed __attribute__((unused)), + double dir __attribute__((unused))) +{ +} + +void nps_fdm_set_wind_ned( + double wind_north __attribute__((unused)), + double wind_east __attribute__((unused)), + double wind_down __attribute__((unused))) +{ +} + +void nps_fdm_set_turbulence( + double wind_speed __attribute__((unused)), + int turbulence_severity __attribute__((unused))) +{ +} + +/** Set temperature in degrees Celcius at given height h above MSL */ +void nps_fdm_set_temperature( + double temp __attribute__((unused)), + double h __attribute__((unused))) +{ +} + +// Internal functions +/** + * Set up a Gazebo server. + * + * Starts a Gazebo server, adds conf/simulator/gazebo/models to its model path + * and loads the world specified by NPS_GAZEBO_WORLD. + * + * This function also obtaines a pointer to the aircraft model, named + * NPS_GAZEBO_AC_NAME ('paparazzi_uav' by default). This pointer, 'model', + * is used to read the state and write actuator commands in gazebo_read and + * _write. + */ +static void init_gazebo(void) +{ + string gazebo_home = "/conf/simulator/gazebo/"; + string pprz_home(getenv("PAPARAZZI_HOME")); + string gazebodir = pprz_home + gazebo_home; + cout << "Gazebo directory: " << gazebodir << endl; + + if (!gazebo::setupServer(0, NULL)) { + cout << "Failed to start Gazebo, exiting." << endl; + std::exit(-1); + } + + cout << "Add Paparazzi model path: " << gazebodir + "models/" << endl; + gazebo::common::SystemPaths::Instance()->AddModelPaths( + gazebodir + "models/"); + + cout << "Load world: " << gazebodir + "world/" + NPS_GAZEBO_WORLD << endl; + gazebo::physics::WorldPtr world = gazebo::loadWorld( + gazebodir + "world/" + NPS_GAZEBO_WORLD); + if (!world) { + cout << "Failed to open world, exiting." << endl; + std::exit(-1); + } + + cout << "Get pointer to aircraft: " << NPS_GAZEBO_AC_NAME << endl; + model = world->GetModel(NPS_GAZEBO_AC_NAME); + if (!model) { + cout << "Failed to find '" << NPS_GAZEBO_AC_NAME << "', exiting." + << endl; + std::exit(-1); + } + + // Initialize sensors + gazebo::sensors::run_once(true); + gazebo::sensors::run_threads(); + gazebo::runWorld(world, 1); + cout << "Sensors initialized..." << endl; + + cout << "Gazebo initialized successfully!" << endl; +} + +/** + * Read Gazebo's simulation state and store the results in the fdm struct used + * by NPS. + * + * Not all fields are filled at the moment, as some of them are unused by + * paparazzi (see comments) and others are not available in Gazebo 7 + * (atmosphere). + */ +static void gazebo_read(void) +{ + 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 accel = model->GetWorldLinearAccel(); + 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()); + + /* Fill FDM struct */ + fdm.time = world->GetSimTime().Double(); + + // init_dt: unused + // curr_dt: unused + // on_ground: unused + // 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; + /* 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 + + /* 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.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.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())); + + /* attitude */ + // ecef_to_body_quat: unused + fdm.ltp_to_body_eulers = to_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_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)); + + /* atmosphere */ +#if GAZEBO_MAJOR_VERSION >= 8 && 0 // TODO implement + +#else + // Gazebo versions < 8 do not have atmosphere or wind support + // Use placeholder values. Valid for low altitude, low speed flights. + fdm.wind = {.x = 0, .y = 0, .z = 0}; + fdm.pressure_sl = 101325; // Pa + + fdm.airspeed = 0; + fdm.pressure = fdm.pressure_sl; // Pa + fdm.dynamic_pressure = fdm.pressure_sl; // Pa + fdm.temperature = 20.0; // C + fdm.aoa = 0; // rad + fdm.sideslip = 0; // rad +#endif + /* flight controls: unused */ + fdm.elevator = 0; + fdm.flap = 0; + fdm.left_aileron = 0; + fdm.right_aileron = 0; + fdm.rudder = 0; + /* engine: unused */ + fdm.num_engines = 0; +} + +/** + * Write actuator commands to Gazebo. + * + * This function takes the normalized commands and applies them as forces and + * torques in Gazebo. Since the commands are normalized in [0,1], their + * thrusts (NPS_ACTUATOR_THRUSTS) and torques (NPS_ACTUATOR_TORQUES) need to + * be specified in the airframe file. Their values need to be specified in the + * same order as the NPS_ACTUATOR_NAMES and should be provided in SI units. + * See conf/airframes/examples/ardrone2_gazebo.xml for an example. + * + * The forces and torques are applied to (the origin of) the links named in + * NPS_ACTUATOR_NAMES. See conf/simulator/gazebo/models/ardrone/ardrone.sdf + * for an example. + * + * @param commands + * @param commands_nb + */ +static void gazebo_write(double commands[], int commands_nb) +{ + const string names[] = NPS_ACTUATOR_NAMES; + const double thrusts[] = { NPS_ACTUATOR_THRUSTS }; + const double torques[] = { NPS_ACTUATOR_TORQUES }; + + for (int i = 0; i < commands_nb; ++i) { + double thrust = autopilot.motors_on ? thrusts[i] * commands[i] : 0.0; + double torque = autopilot.motors_on ? torques[i] * commands[i] : 0.0; + gazebo::physics::LinkPtr link = model->GetLink(names[i]); + link->AddRelativeForce(gazebo::math::Vector3(0, 0, thrust)); + link->AddRelativeTorque(gazebo::math::Vector3(0, 0, torque)); + // cout << "Motor '" << link->GetName() << "': thrust = " << thrust + // << " N, torque = " << torque << " Nm" << endl; + } +} + +#ifdef NPS_SIMULATE_VIDEO +/** + * Set up cameras. + * + * This function finds the video devices added through add_video_device + * (sw/airborne/modules/computer_vision/cv.h). The camera links in the Gazebo AC + * model should have the same name as the .dev_name field in the corresponding + * video_config_t struct stored in 'cameras[]' (computer_vision/ + * video_thread_nps.h). Pointers to Gazebo's cameras are stored in gazebo_cams + * at the same index as their 'cameras[]' counterpart. + * + * The video_config_t parameters are updated using the values provided by + * Gazebo. This should simplify the use of different UAVs with different camera + * setups. + */ +static void init_gazebo_video(void) +{ + gazebo::sensors::SensorManager *mgr = + gazebo::sensors::SensorManager::Instance(); + + cout << "Initializing cameras..." << endl; + // Loop over cameras registered in video_thread_nps + for (int i = 0; i < VIDEO_THREAD_MAX_CAMERAS && cameras[i] != NULL; ++i) { + // Find link in gazebo model + cout << "Setting up '" << cameras[i]->dev_name << "'... "; + gazebo::physics::LinkPtr link = model->GetLink(cameras[i]->dev_name); + if (!link) { + cout << "ERROR: Link '" << cameras[i]->dev_name << "' not found!" + << endl; + ; + continue; + } + // Get a pointer to the sensor using its full name + if (link->GetSensorCount() != 1) { + cout << "ERROR: Link '" << link->GetName() + << "' should only contain 1 sensor!" << endl; + continue; + } + string name = link->GetSensorName(0); + gazebo::sensors::CameraSensorPtr cam = static_pointer_cast + < gazebo::sensors::CameraSensor > (mgr->GetSensor(name)); + if (!cam) { + cout << "ERROR: Could not get pointer to '" << name << "'!" << endl; + continue; + } + // Activate sensor + cam->SetActive(true); + // Add to list of cameras + gazebo_cams[i].cam = cam; + gazebo_cams[i].last_measurement_time = cam->LastMeasurementTime(); + // Copy video_config settings from Gazebo's camera + cameras[i]->output_size.w = cam->ImageWidth(); + cameras[i]->output_size.h = cam->ImageHeight(); + cameras[i]->sensor_size.w = cam->ImageWidth(); + cameras[i]->sensor_size.h = cam->ImageHeight(); + cameras[i]->crop.w = cam->ImageWidth(); + cameras[i]->crop.h = cam->ImageHeight(); + cameras[i]->fps = cam->UpdateRate(); + cout << "ok" << endl; + } +} + +/** + * Read camera images. + * + * Polls gazebo cameras. If the last measurement time has been updated, a new + * frame is available. This frame is converted to Paparazzi's UYVY format + * and passed to cv_run_device which runs the callbacks registered by various + * modules. + */ +static void gazebo_read_video(void) +{ + for (int i = 0; i < VIDEO_THREAD_MAX_CAMERAS; ++i) { + gazebo::sensors::CameraSensorPtr &cam = gazebo_cams[i].cam; + // Skip unregistered or unfound cameras + if (cam == NULL) { continue; } + // Skip if not updated + // Also skip when LastMeasurementTime() is zero (workaround) + if (cam->LastMeasurementTime() == gazebo_cams[i].last_measurement_time + || cam->LastMeasurementTime() == 0) { continue; } + // Grab image, convert and send to video thread + struct image_t img; + read_image(&img, cam); + cv_run_device(cameras[i], &img); + // Free image buffer after use. + image_free(&img); + // Keep track of last update time. + gazebo_cams[i].last_measurement_time = cam->LastMeasurementTime(); + } +} + +/** + * Read Gazebo image and convert. + * + * Converts the current camera frame to the format used by Paparazzi. This + * includes conversion to UYVY. Gazebo's simulation time is used for the image + * timestamp. + * + * @param img + * @param cam + */ +static void read_image( + struct image_t *img, + gazebo::sensors::CameraSensorPtr cam) +{ + image_create(img, cam->ImageWidth(), cam->ImageHeight(), IMAGE_YUV422); + // Convert Gazebo's *RGB888* image to Paparazzi's YUV422 + const uint8_t *data_rgb = cam->ImageData(); + uint8_t *data_yuv = (uint8_t *)(img->buf); + for (int x = 0; x < img->w; ++x) { + for (int y = 0; y < img->h; ++y) { + int idx_rgb = 3 * (img->w * y + x); + 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] + - 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 + 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 + gazebo::common::Time ts = cam->LastMeasurementTime(); + img->ts.tv_sec = ts.sec; + img->ts.tv_usec = ts.nsec / 1000.0; + img->pprz_ts = ts.Double() * 1e6; + img->buf_idx = 0; // unused +} +#endif + +#pragma GCC diagnostic pop // Ignore -Wdeprecated-declarations