mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-03-23 15:34:18 +08:00
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.
This commit is contained in:
tomvand
Kirk Scheper
1
.gitignore
vendored
1
.gitignore
vendored
@@ -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
|
||||
|
||||
@@ -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)
|
||||
|
||||
209
conf/airframes/examples/ardrone2_gazebo.xml
Normal file
209
conf/airframes/examples/ardrone2_gazebo.xml
Normal file
@@ -0,0 +1,209 @@
|
||||
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
|
||||
|
||||
<airframe name="ardrone2">
|
||||
|
||||
<firmware name="rotorcraft">
|
||||
<target name="ap" board="ardrone2"/>
|
||||
|
||||
<target name="nps" board="pc">
|
||||
<module name="fdm" type="gazebo"/>
|
||||
</target>
|
||||
|
||||
<define name="USE_SONAR" value="TRUE"/>
|
||||
|
||||
<!-- Subsystem section -->
|
||||
<module name="telemetry" type="transparent_udp"/>
|
||||
<module name="radio_control" type="datalink"/>
|
||||
<module name="motor_mixing"/>
|
||||
<module name="actuators" type="ardrone2"/>
|
||||
<module name="imu" type="ardrone2"/>
|
||||
<!-- gps: "ublox" or change to "sirf" for usage with parrot flight recorder -->
|
||||
<module name="gps" type="ublox"/>
|
||||
<module name="stabilization" type="int_quat"/>
|
||||
<module name="ahrs" type="int_cmpl_quat"/>
|
||||
<module name="ins" type="extended"/>
|
||||
</firmware>
|
||||
|
||||
<modules main_freq="512">
|
||||
<module name="bat_voltage_ardrone2"/>
|
||||
<!-- remove the gps_ubx_ucenter module if you use the sirf gps (flight recorder) -->
|
||||
<module name="gps" type="ubx_ucenter"/>
|
||||
<module name="send_imu_mag_current"/>
|
||||
<module name="air_data"/>
|
||||
<module name="geo_mag"/>
|
||||
<!--module name="logger_file"/-->
|
||||
<module name="video_thread">
|
||||
</module>
|
||||
<module name="cv_colorfilter">
|
||||
<define name="COLORFILTER_CAMERA" value="front_camera"/>
|
||||
</module>
|
||||
|
||||
<module name="video_rtp_stream">
|
||||
<define name="VIEWVIDEO_CAMERA" value="front_camera"/>
|
||||
<define name="VIEWVIDEO_DOWNSIZE_FACTOR" value="4"/>
|
||||
<define name="VIEWVIDEO_QUALITY_FACTOR" value="60"/>
|
||||
</module>
|
||||
</modules>
|
||||
|
||||
<commands>
|
||||
<axis name="PITCH" failsafe_value="0"/>
|
||||
<axis name="ROLL" failsafe_value="0"/>
|
||||
<axis name="YAW" failsafe_value="0"/>
|
||||
<axis name="THRUST" failsafe_value="3000"/>
|
||||
</commands>
|
||||
|
||||
<servos driver="Default">
|
||||
<servo name="TOP_LEFT" no="0" min="0" neutral="1" max="500"/>
|
||||
<servo name="TOP_RIGHT" no="1" min="0" neutral="1" max="500"/>
|
||||
<servo name="BOTTOM_RIGHT" no="2" min="0" neutral="1" max="500"/>
|
||||
<servo name="BOTTOM_LEFT" no="3" min="0" neutral="1" max="500"/>
|
||||
</servos>
|
||||
|
||||
<section name="MIXING" prefix="MOTOR_MIXING_">
|
||||
<define name="TRIM_ROLL" value="0"/>
|
||||
<define name="TRIM_PITCH" value="0"/>
|
||||
<define name="TRIM_YAW" value="0"/>
|
||||
|
||||
<!-- Time cross layout (X), with order NW (CW), NE (CCW), SE (CW), SW (CCW) -->
|
||||
<define name="TYPE" value="QUAD_X"/>
|
||||
</section>
|
||||
|
||||
<command_laws>
|
||||
<call fun="motor_mixing_run(autopilot_get_motors_on(),FALSE,values)"/>
|
||||
<set servo="TOP_LEFT" value="motor_mixing.commands[MOTOR_FRONT_LEFT]"/>
|
||||
<set servo="TOP_RIGHT" value="motor_mixing.commands[MOTOR_FRONT_RIGHT]"/>
|
||||
<set servo="BOTTOM_RIGHT" value="motor_mixing.commands[MOTOR_BACK_RIGHT]"/>
|
||||
<set servo="BOTTOM_LEFT" value="motor_mixing.commands[MOTOR_BACK_LEFT]"/>
|
||||
</command_laws>
|
||||
|
||||
<section name="IMU" prefix="IMU_">
|
||||
<!-- Accelero -->
|
||||
<define name="ACCEL_X_NEUTRAL" value="2048"/>
|
||||
<define name="ACCEL_Y_NEUTRAL" value="2048"/>
|
||||
<define name="ACCEL_Z_NEUTRAL" value="2048"/>
|
||||
|
||||
<!-- Magneto calibration -->
|
||||
<define name="MAG_X_NEUTRAL" value="0"/>
|
||||
<define name="MAG_Y_NEUTRAL" value="0"/>
|
||||
<define name="MAG_Z_NEUTRAL" value="-180"/>
|
||||
<define name="MAG_X_SENS" value="16." integer="16"/>
|
||||
<define name="MAG_Y_SENS" value="16." integer="16"/>
|
||||
<define name="MAG_Z_SENS" value="16." integer="16"/>
|
||||
|
||||
<!-- Magneto current calibration -->
|
||||
<define name="MAG_X_CURRENT_COEF" value="0.0"/>
|
||||
<define name="MAG_Y_CURRENT_COEF" value="0.0"/>
|
||||
<define name="MAG_Z_CURRENT_COEF" value="0.0"/>
|
||||
|
||||
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
|
||||
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
|
||||
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
|
||||
</section>
|
||||
|
||||
<!-- local magnetic field -->
|
||||
<!-- http://wiki.paparazziuav.org/wiki/Subsystem/ahrs#Local_Magnetic_Field -->
|
||||
<section name="AHRS" prefix="AHRS_">
|
||||
<!-- values used if no GPS fix, on 3D fix is update by geo_mag module -->
|
||||
<!-- Toulouse -->
|
||||
<define name="H_X" value="0.513081"/>
|
||||
<define name="H_Y" value="-0.00242783"/>
|
||||
<define name="H_Z" value="0.858336"/>
|
||||
<!-- Delft -->
|
||||
<!--define name="H_X" value="0.3892503"/>
|
||||
<define name="H_Y" value="0.0017972"/>
|
||||
<define name="H_Z" value="0.9211303"/ -->
|
||||
</section>
|
||||
|
||||
<section name="INS" prefix="INS_">
|
||||
<define name="SONAR_MAX_RANGE" value="2.2"/>
|
||||
<define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
|
||||
</section>
|
||||
|
||||
|
||||
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
|
||||
<!-- setpoints -->
|
||||
<define name="SP_MAX_PHI" value="45" unit="deg"/>
|
||||
<define name="SP_MAX_THETA" value="45" unit="deg"/>
|
||||
<define name="SP_MAX_R" value="600" unit="deg/s"/>
|
||||
<define name="DEADBAND_A" value="0"/>
|
||||
<define name="DEADBAND_E" value="0"/>
|
||||
<define name="DEADBAND_R" value="250"/>
|
||||
|
||||
<!-- reference -->
|
||||
<define name="REF_OMEGA_P" value="450" unit="deg/s"/>
|
||||
<define name="REF_ZETA_P" value="0.9"/>
|
||||
<define name="REF_MAX_P" value="600." unit="deg/s"/>
|
||||
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
|
||||
|
||||
<define name="REF_OMEGA_Q" value="450" unit="deg/s"/>
|
||||
<define name="REF_ZETA_Q" value="0.9"/>
|
||||
<define name="REF_MAX_Q" value="600." unit="deg/s"/>
|
||||
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
|
||||
|
||||
<define name="REF_OMEGA_R" value="200" unit="deg/s"/>
|
||||
<define name="REF_ZETA_R" value="0.9"/>
|
||||
<define name="REF_MAX_R" value="300." unit="deg/s"/>
|
||||
<define name="REF_MAX_RDOT" value="RadOfDeg(4000.)"/>
|
||||
|
||||
<!-- feedback -->
|
||||
<define name="PHI_PGAIN" value="850"/>
|
||||
<define name="PHI_DGAIN" value="425"/>
|
||||
<define name="PHI_IGAIN" value="0"/>
|
||||
|
||||
<define name="THETA_PGAIN" value="850"/>
|
||||
<define name="THETA_DGAIN" value="425"/>
|
||||
<define name="THETA_IGAIN" value="0"/>
|
||||
|
||||
<define name="PSI_PGAIN" value="1000"/>
|
||||
<define name="PSI_DGAIN" value="700"/>
|
||||
<define name="PSI_IGAIN" value="0"/>
|
||||
|
||||
<!-- feedforward -->
|
||||
<define name="PHI_DDGAIN" value="0"/>
|
||||
<define name="THETA_DDGAIN" value="0"/>
|
||||
<define name="PSI_DDGAIN" value="100"/>
|
||||
</section>
|
||||
|
||||
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
|
||||
<define name="HOVER_KP" value="283"/>
|
||||
<define name="HOVER_KD" value="82"/>
|
||||
<define name="HOVER_KI" value="13"/>
|
||||
<define name="NOMINAL_HOVER_THROTTLE" value="0.655"/>
|
||||
<define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
|
||||
</section>
|
||||
|
||||
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
|
||||
<!-- Good weather -->
|
||||
<define name="MAX_BANK" value="20" unit="deg"/>
|
||||
<!-- Bad weather -->
|
||||
<!-- define name="MAX_BANK" value="32" unit="deg"/ -->
|
||||
<define name="PGAIN" value="79"/>
|
||||
<define name="DGAIN" value="100"/>
|
||||
<define name="IGAIN" value="30"/>
|
||||
</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[]"/>
|
||||
<define name="JSBSIM_MODEL" value="simple_ardrone2" type="string"/>
|
||||
<define name="SENSORS_PARAMS" value="nps_sensors_params_ardrone2.h" type="string"/>
|
||||
<define name="BYPASS_AHRS" value="1"/>
|
||||
<define name="SIMULATE_VIDEO" value="1"/>
|
||||
</section>
|
||||
|
||||
<section name="AUTOPILOT">
|
||||
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
|
||||
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
|
||||
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_Z_HOLD"/>
|
||||
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
|
||||
</section>
|
||||
|
||||
<section name="BAT">
|
||||
<define name="MILLIAMP_AT_FULL_THROTTLE" value="8700"/>
|
||||
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
|
||||
<define name="CRITIC_BAT_LEVEL" value="9.6" unit="V"/>
|
||||
<define name="LOW_BAT_LEVEL" value="9.7" unit="V"/>
|
||||
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
|
||||
</section>
|
||||
</airframe>
|
||||
@@ -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"
|
||||
/>
|
||||
<aircraft
|
||||
name="ardrone2_gazebo"
|
||||
ac_id="32"
|
||||
airframe="airframes/examples/ardrone2_gazebo.xml"
|
||||
radio="radios/dummy.xml"
|
||||
telemetry="telemetry/default_rotorcraft.xml"
|
||||
flight_plan="flight_plans/rotorcraft_optitrack.xml"
|
||||
settings="settings/rotorcraft_basic.xml settings/nps.xml"
|
||||
settings_modules="modules/video_rtp_stream.xml modules/cv_colorfilter.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"
|
||||
/>
|
||||
<aircraft
|
||||
name="bebop"
|
||||
ac_id="33"
|
||||
|
||||
@@ -27,7 +27,7 @@
|
||||
</header>
|
||||
|
||||
<init fun="colorfilter_init()"/>
|
||||
<makefile target="ap">
|
||||
<makefile target="ap|nps">
|
||||
<file name="colorfilter.c"/>
|
||||
</makefile>
|
||||
</module>
|
||||
|
||||
92
conf/modules/fdm_gazebo.xml
Normal file
92
conf/modules/fdm_gazebo.xml
Normal file
@@ -0,0 +1,92 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="fdm_gazebo" dir="fdm">
|
||||
<doc>
|
||||
<description>
|
||||
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>
|
||||
</description>
|
||||
</doc>
|
||||
<header/>
|
||||
<makefile target="nps">
|
||||
<raw>
|
||||
nps.CXXFLAGS += $(shell pkg-config gazebo --cflags)
|
||||
nps.LDFLAGS += $(shell pkg-config gazebo --libs)
|
||||
</raw>
|
||||
<file name="nps_fdm_gazebo.cpp" dir="nps"/>
|
||||
</makefile>
|
||||
</module>
|
||||
|
||||
@@ -34,7 +34,7 @@
|
||||
</header>
|
||||
|
||||
<init fun="viewvideo_init()"/>
|
||||
<makefile target="ap">
|
||||
<makefile target="ap|nps">
|
||||
|
||||
<file name="viewvideo.c"/>
|
||||
<!-- Include the needed Computer Vision files -->
|
||||
|
||||
234
conf/simulator/gazebo/models/ardrone/ardrone.sdf
Normal file
234
conf/simulator/gazebo/models/ardrone/ardrone.sdf
Normal file
@@ -0,0 +1,234 @@
|
||||
<?xml version='1.0'?>
|
||||
<sdf version='1.4'>
|
||||
<model name="paparazzi_uav">
|
||||
<pose>0 0 .1 0 0 0</pose>
|
||||
|
||||
<link name="chassis">
|
||||
<inertial><!-- Taken from Paparazzi's ARDrone model for JSBsim -->
|
||||
<mass>0.4</mass>
|
||||
<inertia>
|
||||
<ixx> 0.005 </ixx>
|
||||
<iyy> 0.005 </iyy>
|
||||
<izz> 0.010 </izz>
|
||||
<ixy> 0. </ixy>
|
||||
<ixz> 0. </ixz>
|
||||
<iyz> 0. </iyz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
|
||||
<collision name="collision">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>.4 .4 .05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<visual name="visual">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>.2 .2 .05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<link name="front_camera">
|
||||
<pose>0.15 0 0 0 0 0</pose>
|
||||
<inertial>
|
||||
<mass>0.001</mass>
|
||||
<inertia>
|
||||
<ixx>.0001</ixx>
|
||||
<iyy>.0001</iyy>
|
||||
<izz>.0001</izz>
|
||||
<ixy>0</ixy>
|
||||
<iyz>0</iyz>
|
||||
<ixz>0</ixz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<sensor type="camera" name="front_camera">
|
||||
<update_rate>30.0</update_rate>
|
||||
<camera name="front_camera">
|
||||
<horizontal_fov>1.3962634</horizontal_fov>
|
||||
<image>
|
||||
<width>1280</width>
|
||||
<height>720</height>
|
||||
<format>R8G8B8</format>
|
||||
</image>
|
||||
<clip>
|
||||
<near>0.02</near>
|
||||
<far>300</far>
|
||||
</clip>
|
||||
<noise>
|
||||
<type>gaussian</type>
|
||||
<!-- Noise is sampled independently per pixel on each frame. That pixel's
|
||||
noise value is added to each of its color channels, which at that point lie
|
||||
in the range [0,1]. -->
|
||||
<mean>0.0</mean>
|
||||
<stddev>0.007</stddev>
|
||||
</noise>
|
||||
</camera>
|
||||
</sensor>
|
||||
</link>
|
||||
|
||||
<joint name="front_camera_joint" type="fixed">
|
||||
<parent>chassis</parent>
|
||||
<child>front_camera</child>
|
||||
</joint>
|
||||
|
||||
<link name="bottom_camera">
|
||||
<pose>0 0 -.03 0 1.57 0</pose>
|
||||
<inertial>
|
||||
<mass>0.001</mass>
|
||||
<inertia>
|
||||
<ixx>.0001</ixx>
|
||||
<iyy>.0001</iyy>
|
||||
<izz>.0001</izz>
|
||||
<ixy>0</ixy>
|
||||
<iyz>0</iyz>
|
||||
<ixz>0</ixz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<sensor type="camera" name="bottom_camera">
|
||||
<update_rate>30.0</update_rate>
|
||||
<camera name="bottom_camera">
|
||||
<horizontal_fov>1.3962634</horizontal_fov>
|
||||
<image>
|
||||
<width>320</width>
|
||||
<height>240</height>
|
||||
<format>R8G8B8</format>
|
||||
</image>
|
||||
<clip>
|
||||
<near>0.02</near>
|
||||
<far>300</far>
|
||||
</clip>
|
||||
<noise>
|
||||
<type>gaussian</type>
|
||||
<!-- Noise is sampled independently per pixel on each frame. That pixel's
|
||||
noise value is added to each of its color channels, which at that point lie
|
||||
in the range [0,1]. -->
|
||||
<mean>0.0</mean>
|
||||
<stddev>0.007</stddev>
|
||||
</noise>
|
||||
</camera>
|
||||
</sensor>
|
||||
</link>
|
||||
|
||||
<joint name="bottom_camera_joint" type="fixed">
|
||||
<parent>chassis</parent>
|
||||
<child>bottom_camera</child>
|
||||
</joint>
|
||||
|
||||
<link name="nw_motor">
|
||||
<pose>0.12 0.12 0 0 0 0</pose>
|
||||
<inertial>
|
||||
<mass>0.01</mass>
|
||||
<inertia>
|
||||
<ixx>.0001</ixx>
|
||||
<iyy>.0001</iyy>
|
||||
<izz>.0001</izz>
|
||||
<ixy>0</ixy>
|
||||
<iyz>0</iyz>
|
||||
<ixz>0</ixz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<visual name="visual">
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.10</radius>
|
||||
<length>.07</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint type="fixed" name="nw_motor_joint">
|
||||
<parent>chassis</parent>
|
||||
<child>nw_motor</child>
|
||||
</joint>
|
||||
|
||||
<link name="se_motor">
|
||||
<pose>-0.12 -0.12 0 0 0 0</pose>
|
||||
<inertial>
|
||||
<mass>0.01</mass>
|
||||
<inertia>
|
||||
<ixx>.0001</ixx>
|
||||
<iyy>.0001</iyy>
|
||||
<izz>.0001</izz>
|
||||
<ixy>0</ixy>
|
||||
<iyz>0</iyz>
|
||||
<ixz>0</ixz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<visual name="visual">
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.10</radius>
|
||||
<length>.07</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint type="fixed" name="se_motor_joint">
|
||||
<parent>chassis</parent>
|
||||
<child>se_motor</child>
|
||||
</joint>
|
||||
|
||||
<link name="ne_motor">
|
||||
<pose>0.12 -0.12 0 0 0 0</pose>
|
||||
<inertial>
|
||||
<mass>0.01</mass>
|
||||
<inertia>
|
||||
<ixx>.0001</ixx>
|
||||
<iyy>.0001</iyy>
|
||||
<izz>.0001</izz>
|
||||
<ixy>0</ixy>
|
||||
<iyz>0</iyz>
|
||||
<ixz>0</ixz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<visual name="visual">
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.10</radius>
|
||||
<length>.07</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint type="fixed" name="ne_motor_joint">
|
||||
<parent>chassis</parent>
|
||||
<child>ne_motor</child>
|
||||
</joint>
|
||||
|
||||
<link name="sw_motor">
|
||||
<pose>-0.12 0.12 0 0 0 0</pose>
|
||||
<inertial>
|
||||
<mass>0.01</mass>
|
||||
<inertia>
|
||||
<ixx>.0001</ixx>
|
||||
<iyy>.0001</iyy>
|
||||
<izz>.0001</izz>
|
||||
<ixy>0</ixy>
|
||||
<iyz>0</iyz>
|
||||
<ixz>0</ixz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<visual name="visual">
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.10</radius>
|
||||
<length>.07</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint type="fixed" name="sw_motor_joint">
|
||||
<parent>chassis</parent>
|
||||
<child>sw_motor</child>
|
||||
</joint>
|
||||
</model>
|
||||
</sdf>
|
||||
15
conf/simulator/gazebo/models/ardrone/model.config
Normal file
15
conf/simulator/gazebo/models/ardrone/model.config
Normal file
@@ -0,0 +1,15 @@
|
||||
<?xml version="1.0"?>
|
||||
<model>
|
||||
<name>ARDrone2 (Paparazzi)</name>
|
||||
<version>1.0</version>
|
||||
<sdf version='1.4'>ardrone.sdf</sdf>
|
||||
|
||||
<author>
|
||||
<name>Tom van Dijk</name>
|
||||
<email>tomvand@users.noreply.github.com</email>
|
||||
</author>
|
||||
|
||||
<description>
|
||||
Simple ARDrone2 model for use with Paparazzi's NPS (http://wiki.paparazziuav.org).
|
||||
</description>
|
||||
</model>
|
||||
41
conf/simulator/gazebo/world/ardrone.world
Normal file
41
conf/simulator/gazebo/world/ardrone.world
Normal file
@@ -0,0 +1,41 @@
|
||||
<?xml version='1.0'?>
|
||||
<sdf version="1.6">
|
||||
<world name="default">
|
||||
<physics type="ode">
|
||||
<max_step_size>0.001</max_step_size>
|
||||
<real_time_update_rate>0</real_time_update_rate><!-- Handled by Paparazzi! -->
|
||||
</physics>
|
||||
<scene>
|
||||
<ambient>0.4 0.4 0.4 1</ambient>
|
||||
<background>0.7 0.7 0.7 1</background>
|
||||
<shadows>1</shadows>
|
||||
</scene>
|
||||
<light name='sun' type='directional'>
|
||||
<cast_shadows>1</cast_shadows>
|
||||
<pose frame=''>0 0 10 0 -0 0</pose>
|
||||
<diffuse>0.8 0.8 0.8 1</diffuse>
|
||||
<specular>0.1 0.1 0.1 1</specular>
|
||||
<attenuation>
|
||||
<range>1000</range>
|
||||
<constant>0.9</constant>
|
||||
<linear>0.01</linear>
|
||||
<quadratic>0.001</quadratic>
|
||||
</attenuation>
|
||||
<direction>-0.5 0.5 -1</direction>
|
||||
</light>
|
||||
<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>
|
||||
|
||||
<include>
|
||||
<uri>model://ground_plane</uri>
|
||||
</include>
|
||||
<include>
|
||||
<uri>model://ardrone</uri>
|
||||
</include>
|
||||
</world>
|
||||
</sdf>
|
||||
@@ -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 */
|
||||
|
||||
@@ -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"
|
||||
|
||||
// Initialize the video_thread structure with the defaults
|
||||
struct video_thread_t video_thread = {
|
||||
.is_running = FALSE
|
||||
#include <stdio.h>
|
||||
|
||||
// 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
|
||||
};
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
39
sw/airborne/modules/computer_vision/video_thread_nps.h
Normal file
39
sw/airborne/modules/computer_vision/video_thread_nps.h
Normal file
@@ -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
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
* @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
|
||||
@@ -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))) {}
|
||||
588
sw/simulator/nps/nps_fdm_gazebo.cpp
Normal file
588
sw/simulator/nps/nps_fdm_gazebo.cpp
Normal file
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user