mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-22 04:13:39 +08:00
StereoWedgeBug (#2555)
* First comit of clean branch
* Added new drone airframe files (xml and model folder) for gazebo
* Renamed sdf file for bebop drone with stereo cams
* Added custom configuration and control panel files
On branch stereoCamerasNewDrone
Changes to be committed:
new file: conf/userconf/tudelft/ralphthesis2020_conf.xml
new file: conf/userconf/tudelft/ralphthesis2020_control_panel.xml
* Added new files necessary for a new bebop model with stereo vision
On branch stereoCamerasNewDrone
Changes to be committed:
new file: conf/airframes/tudelft/bebop_ralphthesis2020_stereo.xml
new file: conf/flight_plans/tudelft/ralphthesis2020_stereo_cyberzoo.xml
modified: conf/simulator/gazebo/airframes/bebop_w_stereo_cams.xml
modified: conf/simulator/gazebo/models/bebop_w_stereo_cams/bebop_w_stereo_cams.sdf
modified: conf/simulator/gazebo/models/bebop_w_stereo_cams/model.config
modified: conf/userconf/tudelft/ralphthesis2020_conf.xml
* Replaced cameras on bebop (front + down) with a stereo setup
On branch stereoCamerasNewDrone
Changes to be committed:
modified: conf/simulator/gazebo/models/bebop_w_stereo_cams/bebop_w_stereo_cams.sdf
* Solved low resolution in left camera
On branch stereoCamerasNewDrone
Changes to be committed:
modified: conf/simulator/gazebo/models/bebop_w_stereo_cams/bebop_w_stereo_cams.sdf
* Created new module "wedgebu" and added it to the airframe "bebop_ralphthesis2020_stereo.xml"
Also, used "make" to create tool for creating modules
On branch wedgeBug
Your branch is up to date with 'origin/wedgeBug'.
Changes to be committed:
modified: conf/airframes/tudelft/bebop_ralphthesis2020_stereo.xml
new file: conf/modules/wedgebug.xml
modified: conf/userconf/tudelft/ralphthesis2020_conf.xml
new file: sw/airborne/modules/wedgebug/wedgebug.c
new file: sw/airborne/modules/wedgebug/wedgebug.h
new file: sw/tools/create_module/create_module_ui.py
new file: sw/tools/create_module/datalink_ui.py
new file: sw/tools/create_module/event_ui.py
modified: sw/tools/create_module/files_create.py
new file: sw/tools/create_module/init_ui.py
new file: sw/tools/create_module/periodic_ui.py
* Added functions to copy images from left and right camera to global variables
On branch wedgeBug
Your branch is ahead of 'origin/wedgeBug' by 1 commit.
Changes to be committed:
modified: conf/airframes/tudelft/bebop_ralphthesis2020_stereo.xml
modified: conf/modules/wedgebug.xml
modified: conf/simulator/gazebo/models/bebop_w_stereo_cams/bebop_w_stereo_cams.sdf
modified: sw/airborne/modules/wedgebug/wedgebug.c
* Added function to compare individual pixels of images types (image_t)
* Tried to figure out why the C++ Mat object of the left gray pixels looks different to the left image
* A rough implementation and proof of the copy function (note: copy buf entries by incrementing pointer)
Changes to be committed:
deleted: Color_Image.jpg
modified: conf/airframes/tudelft/bebop_ralphthesis2020_stereo.xml
deleted: merged_stereo_image.jpg
modified: sw/airborne/modules/wedgebug/wedgebug.c
modified: sw/airborne/modules/wedgebug/wedgebug.h
modified: sw/airborne/modules/wedgebug/wedgebug_opencv.cpp
modified: sw/airborne/modules/wedgebug/wedgebug_opencv.h
* Finalized 1) image merging function for YY image and 2) function for saving gray images using openCV
* Created C++ function to create and return disparity image. Also changed gazebo world.
* 1) Added FPS like controlls for drone, using Xbox controller. 2) Added cropping image functionality of block matching function.
* SBM can take 16 bit images now, save function uses an image_t struct and a new type called "IMAGE_OPENCV_DISP" was added, specifically for OpenCV disparity images.
* Added openCV dependent openin and closing functions
* Added 1) openCV dependent dilation and sobel operation functions, 2) 3d point structure to image.h 3) Loop in wedebug.c for findin edge point closest to target.
* Added functions to convert world coordinates to camera coordinates (turned out to be a wrong implementation. Next commit will be the correct implementation)
* Added functions to convert world to agent to camera coordinates (and vice versa)
* Cleaned up coordinate conversion functions
* 1. Created new world for drone testing, 2. Created new waytpoints for drone testing, 4. Created state machine template in module 5. Debugged the "SBM_OCV" function 6. Filled in the state for "Initial position", "Move to start", "Start position" and "Goal reached" 7. Started to fill in the state "Move to goal" 8. Created function to get median distance of object in front ("median_disparity_to_point") 9. Created function to determine angle between drone and goal point ("heading_towards_waypoin") 10. Cleaned up code, removed non-essential variables (especially images) and added new variables.
* Converted edge detection algorithm into a function and cleaned code more. Also created more global variables.
* Added draft approach for "EDGE_DETECTION"
* Quick save
* First draft of code. The robot can evade a board in its way and reach the goal. Also increase periodic loop speed to 15 hz to counter the fast turning of the drone
* Included code for measuring metrics and displaying them. Also, cleaned the code some more.
* Cleaned code, added comments to describe code and variables and got rid of bug in function calculating heading direction.
* Removed bug in POSITION_EDGE state, where a new holding point was not set, after an obstacle was encountered. Also added various shapes (with birck texture) for testing.
* Added more shapes for experiment and added settings to change in GCS
* Changed camera settings to mimic those of Mathies et al. 2014. The drone can now avoid the standard metal board and the brick wall (type 2 2 x 2 meters)!
* Implemented state machine for control modes.
* Simplified code
* Cleaned code
* Added a flag to detect changes in the navigation mode used
* Merged background processes under the condition that the state machine is running in certain state
* Debugged time measurement of total time (metric 1)
* Cleaned code
* MAde debug options state number independent (i.e. they have been renamed to include the name of the state and not the number)
* Remove states: POSITION_INITIAL, MOVE_TO_START and POSITION_START from finite state machine. Also all associate variables have been removed.
* Changed "NUMER_OF_STATES" to "NUMBER_OF_STATES". Also increased "max_obstacle_confidence" from 3 to 5, to avoid fake obstacle detection.
* Added metric measurement when in direct flight.
* Added aluminum bar as object for goal marker and creted button for drone to move to start position and another button for drone to start the experiment. In the current example the drone goes around a 2x2 meter wall.
* Added saving flag and functions for report (in c and cpp file)
* Added median kernel dimensions as options in settings
* Made unused waypoints invisible in the GCS
* Added two more objects for investigation (with a larger hole for the drone to fly through). Also fine tuned the code.
* Commit before change of depth image.
* Added 16 bit background processes for greater precision. Also change some funciton in C++ file to better handle 16bit images
* Using depht works, but its not fine tuned and code is messy.
* Ibligatory commit so that I can checkout a different branch.
* Added depth image on which edge detection is now based on (used to be based on disparity). It works relatively ok with all shapes investigated.
* A save after the experiments for the first repor thave been finalized.
* After demonstration
* Added code to toggle between p2DWedgeBug and p3DWedgeBug.
* Removed original files that were created during the previous merge process.
* Added background processes that works with a 16 bit disparity image (i.e. more precise distance measurement)
* Made finite state machine more readable. Also refined find_best_edge coordinates2 function. Additionally, added function to got from image place coordinates to camera coordinate using depth as input. Lastly, depth is now measured in cm when considerring the thresholds.
* Successfully integrated edge search over depth image instead of disparity image (tested in direct control mode only). Next step is to implement this for the guided mode.
* Addressed bug: The baseline used by the drone model was not the same as in the c file. This has been changed such that the baseline in the drone model now resembles the one in the c file.
By addressing this bug of the Bug algorithm the drone can now judge distances accurately. This in turn results in the drone being able to accurately avoid the wall (2mxm) obstacle.
* Changed flight plan to start experiment atuomatically.
* Just a checkpoint
* Added tree models.
* Removed non used objects from world file.
* Reverted back to opencv_bebop
* Reverted back to original files for push to master
* Added new shapes for experimentation
* Deleted blacklisted
* Cleaned code before push
* Added missing varibales and added declarations.
* Run style bash script and renamed image enum name (IMAGE_INT16)
* Corrected typos and delted unused control panel config file.
This commit is contained in:
Ralph R. Schmidt
GitHub
@@ -0,0 +1,286 @@
|
||||
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
|
||||
|
||||
<airframe name="bebop_avoider">
|
||||
<description>Vision Course TUDelft V2019
|
||||
</description>
|
||||
|
||||
|
||||
<firmware name="rotorcraft">
|
||||
<target name="ap" board="bebop">
|
||||
<define name="VIDEO_CAPTURE_PATH" value="/data/ftp/internal_000/images"/>
|
||||
<define name="FILE_LOGGER_PATH" value="/data/ftp/internal_000/log"/>
|
||||
</target>
|
||||
<target name="nps" board="pc">
|
||||
<module name="fdm" type="gazebo"/>
|
||||
<define name="VIDEO_CAPTURE_PATH" value="/tmp/paparazzi/images"/>
|
||||
<define name="FILE_LOGGER_PATH" value="/tmp/paparazzi/log"/>
|
||||
|
||||
</target>
|
||||
|
||||
<define name="ARRIVED_AT_WAYPOINT" value="0.5"/><!-- Detect arrival at waypoint when within 0.5m -->
|
||||
|
||||
<!-- Subsystem section -->
|
||||
<module name="telemetry" type="transparent_udp"/>
|
||||
<module name="radio_control" type="datalink"/>
|
||||
<module name="motor_mixing"/>
|
||||
<module name="actuators" type="bebop"/>
|
||||
<module name="imu" type="bebop"/>
|
||||
<module name="gps" type="datalink"/>
|
||||
<module name="stabilization" type="indi_simple"/>
|
||||
<module name="stabilization" type="rate_indi"/>
|
||||
<module name="ahrs" type="int_cmpl_quat">
|
||||
<configure name="USE_MAGNETOMETER" value="FALSE"/>
|
||||
<define name="AHRS_USE_GPS_HEADING" value="TRUE"/>
|
||||
</module>
|
||||
<module name="ins" type="extended"/>
|
||||
|
||||
<module name="logger_file"/>
|
||||
|
||||
<!-- Wedgebug modules -->
|
||||
<module name="wedgebug">
|
||||
|
||||
<define name="WEDGEBUG_CAMERA_LEFT" value = "front_camera"/>
|
||||
<define name="WEDGEBUG_CAMERA_RIGHT" value = "bottom_camera"/>
|
||||
|
||||
<define name="WEDGEBUG_CAMERA_LEFT_FPS" value = "25"/>
|
||||
<define name="WEDGEBUG_CAMERA_RIGHT_FPS" value = "25"/>
|
||||
|
||||
<define name="WEDGEBUG_CAMERA_LEFT_WIDTH" value = "320"/>
|
||||
<define name="WEDGEBUG_CAMERA_RIGHT_WIDTH" value = "320"/>
|
||||
<define name="WEDGEBUG_CAMERA_INTERLACED_WIDTH" value = "320"/>
|
||||
<define name="WEDGEBUG_CAMERA_DISPARITY_WIDTH" value = "320"/>
|
||||
|
||||
|
||||
<define name="WEDGEBUG_CAMERA_LEFT_HEIGHT" value = "240"/>
|
||||
<define name="WEDGEBUG_CAMERA_RIGHT_HEIGHT" value = "240"/>
|
||||
<define name="WEDGEBUG_CAMERA_INTERLACED_HEIGHT" value = "480"/>
|
||||
|
||||
<define name="WEDGEBUG_HFOV" value = "1.91986"/>
|
||||
<define name="WEDGEBUG_CAMERA_FOCAL_LENGTH" value = "112"/>
|
||||
<define name="WEDGEBUG_CAMERA_BASELINE" value = "120"/>
|
||||
|
||||
<define name="NUMBER_OF_STATES" value = "7"/>
|
||||
</module>
|
||||
|
||||
|
||||
<!-- Video/Camera modules -->
|
||||
<module name="bebop_cam"/>
|
||||
<!--module name="bebop_ae_awb">
|
||||
<define name="CV_AE_AWB_VERBOSE" value="0" />
|
||||
</module-->
|
||||
<module name="video_capture">
|
||||
<define name="VIDEO_CAPTURE_CAMERA" value="front_camera"/>
|
||||
<define name="VIDEO_CAPTURE_FPS" value="10"/>
|
||||
</module>
|
||||
|
||||
<module name="video_rtp_stream">
|
||||
<define name="VIEWVIDEO_CAMERA" value="front_camera"/>
|
||||
<define name="VIEWVIDEO_CAMERA2" value="bottom_camera"/>
|
||||
<define name="VIEWVIDEO_DOWNSIZE_FACTOR" value="1"/>
|
||||
<define name="VIEWVIDEO_QUALITY_FACTOR" value="40"/>
|
||||
</module>
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
</firmware>
|
||||
|
||||
<commands>
|
||||
<axis name="PITCH" failsafe_value="0"/>
|
||||
<axis name="ROLL" failsafe_value="0"/>
|
||||
<axis name="YAW" failsafe_value="0"/>
|
||||
<axis name="THRUST" failsafe_value="6000"/>
|
||||
</commands>
|
||||
|
||||
<servos driver="Default">
|
||||
<servo name="TOP_LEFT" no="0" min="3000" neutral="3000" max="9800" />
|
||||
<servo name="TOP_RIGHT" no="1" min="3000" neutral="3000" max="9800" />
|
||||
<servo name="BOTTOM_RIGHT" no="2" min="3000" neutral="3000" max="9800" />
|
||||
<servo name="BOTTOM_LEFT" no="3" min="3000" neutral="3000" max="9800" />
|
||||
</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"/>
|
||||
<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="BEBOP_FRONT_CAMERA" prefix="MT9F002_">
|
||||
<define name="OUTPUT_HEIGHT" value="520" />
|
||||
<define name="OUTPUT_WIDTH" value="240" />
|
||||
<define name="OFFSET_X" value="0.09" />
|
||||
<define name="ZOOM" value="1.25"/>
|
||||
<define name="GAIN_GREEN1" value="10.0"/>
|
||||
<define name="GAIN_GREEN2" value="10.0"/>
|
||||
<define name="GAIN_BLUE" value="12.5"/>
|
||||
<define name="GAIN_RED" value="9.0"/>
|
||||
<define name="TARGET_EXPOSURE" value="30" />
|
||||
</section>
|
||||
|
||||
<section name="AIR_DATA" prefix="AIR_DATA_">
|
||||
<define name="CALC_AIRSPEED" value="FALSE"/>
|
||||
<define name="CALC_TAS_FACTOR" value="FALSE"/>
|
||||
<define name="CALC_AMSL_BARO" value="TRUE"/>
|
||||
</section>
|
||||
|
||||
<section name="IMU" prefix="IMU_">
|
||||
<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"/>
|
||||
<!-- Magneto calibration -->
|
||||
<define name="MAG_X_NEUTRAL" value="0"/>
|
||||
<define name="MAG_Y_NEUTRAL" value="0"/>
|
||||
<define name="MAG_Z_NEUTRAL" value="0"/>
|
||||
</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 -->
|
||||
<!-- Delft -->
|
||||
<define name="H_X" value="0.3892503"/>
|
||||
<define name="H_Y" value="0.0017972"/>
|
||||
<define name="H_Z" value="0.9211303"/>
|
||||
|
||||
<define name="HEADING_UPDATE_GPS_MIN_SPEED" value="0"/>
|
||||
<define name="GRAVITY_HEURISTIC_FACTOR" value="0"/>
|
||||
</section>
|
||||
|
||||
<section name="INS" prefix="INS_">
|
||||
<define name="SONAR_MAX_RANGE" value="2.2"/>
|
||||
<define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
|
||||
|
||||
<!-- Use GPS altitude measurments and set the R gain -->
|
||||
<define name="USE_GPS_ALT" value="1"/>
|
||||
<define name="VFF_R_GPS" value="0.01"/>
|
||||
</section>
|
||||
|
||||
<section name="RC_SETPOINT" prefix="STABILIZATION_ATTITUDE_">
|
||||
<!-- setpoint limits for attitude stabilization rc flight -->
|
||||
<define name="SP_MAX_PHI" value="45" unit="deg"/>
|
||||
<define name="SP_MAX_THETA" value="45" unit="deg"/>
|
||||
<define name="SP_MAX_R" value="120" unit="deg/s"/>
|
||||
<define name="DEADBAND_A" value="0"/>
|
||||
<define name="DEADBAND_E" value="0"/>
|
||||
<define name="DEADBAND_R" value="50"/>
|
||||
</section>
|
||||
|
||||
<section name="ATTITUDE_REFERENCE" prefix="STABILIZATION_ATTITUDE_">
|
||||
<!-- attitude reference generation model -->
|
||||
<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.)"/>
|
||||
</section>
|
||||
|
||||
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
|
||||
<!-- control effectiveness -->
|
||||
<define name="G1_P" value="0.0397"/>
|
||||
<define name="G1_Q" value="0.0299"/>
|
||||
<define name="G1_R" value="0.0014"/>
|
||||
<define name="G2_R" value="0.1219"/>
|
||||
|
||||
<!-- reference acceleration for attitude control -->
|
||||
<define name="REF_ERR_P" value="600.0"/>
|
||||
<define name="REF_ERR_Q" value="600.0"/>
|
||||
<define name="REF_ERR_R" value="600.0"/>
|
||||
<define name="REF_RATE_P" value="28.0"/>
|
||||
<define name="REF_RATE_Q" value="28.0"/>
|
||||
<define name="REF_RATE_R" value="28.0"/>
|
||||
|
||||
<!--Maxium yaw rate, to avoid instability-->
|
||||
<define name="MAX_R" value="30" unit="deg/s"/>
|
||||
|
||||
<!-- second order filter parameters -->
|
||||
<define name="FILT_CUTOFF" value="8.0"/>
|
||||
<define name="ESTIMATION_FILT_CUTOFF" value="8.0"/>
|
||||
|
||||
<!-- first order actuator dynamics -->
|
||||
<define name="ACT_DYN_P" value="0.1"/>
|
||||
<define name="ACT_DYN_Q" value="0.1"/>
|
||||
<define name="ACT_DYN_R" value="0.1"/>
|
||||
|
||||
<!-- Adaptive Learning Rate -->
|
||||
<define name="USE_ADAPTIVE" value="FALSE"/>
|
||||
<define name="ADAPTIVE_MU" value="0.0001"/>
|
||||
</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="20"/>
|
||||
<define name="NOMINAL_HOVER_THROTTLE" value="0.68"/>
|
||||
<define name="ADAPT_THROTTLE_ENABLED" value="TRUE"/>
|
||||
|
||||
<define name="ADAPT_INITIAL_HOVER_THROTTLE" value="0.68" />
|
||||
<define name="ADAPT_MIN_HOVER_THROTTLE" value="0.55" />
|
||||
<define name="ADAPT_MAX_HOVER_THROTTLE" value="0.72" />
|
||||
<!-- <define name="ADAPT_NOISE_FACTOR" value="0.8" /> -->
|
||||
</section>
|
||||
|
||||
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
|
||||
<!-- Good weather -->
|
||||
<define name="MAX_BANK" value="20" unit="deg"/>
|
||||
<define name="REF_MAX_SPEED" value="0.5" unit="m/s"/>
|
||||
<!-- Bad weather -->
|
||||
<!-- define name="MAX_BANK" value="32" unit="deg"/ -->
|
||||
<define name="PGAIN" value="120"/>
|
||||
<define name="DGAIN" value="230"/>
|
||||
<define name="IGAIN" value="40"/>
|
||||
</section>
|
||||
|
||||
<section name="NAVIGATION" prefix="NAV_">
|
||||
<define name="CLIMB_VSPEED" value="1.0"/>
|
||||
<define name="DESCEND_VSPEED" value="-0.75"/>
|
||||
</section>
|
||||
|
||||
<section name="SIMULATOR" prefix="NPS_">
|
||||
<define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/>
|
||||
<define name="JSBSIM_MODEL" value="bebop" type="string"/>
|
||||
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
|
||||
<define name="MT9F002_SENSOR_RES_DIVIDER" value="4"/>
|
||||
</section>
|
||||
<include href="conf/simulator/gazebo/airframes/bebop_w_stereo_cams.xml"/>
|
||||
|
||||
<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"/>
|
||||
|
||||
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
|
||||
</section>
|
||||
|
||||
<section name="BAT">
|
||||
<define name="MILLIAMP_AT_FULL_THROTTLE" value="8700"/>
|
||||
<define name="CATASTROPHIC_BAT_LEVEL" value="9.9" unit="V"/>
|
||||
<define name="CRITIC_BAT_LEVEL" value="10.8" unit="V"/>
|
||||
<define name="LOW_BAT_LEVEL" value="11.1" unit="V"/>
|
||||
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
|
||||
</section>
|
||||
</airframe>
|
||||
@@ -0,0 +1,197 @@
|
||||
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
|
||||
|
||||
<flight_plan alt="1.0" ground_alt ="0" lat0="51.990634" lon0="4.376789" max_dist_from_home="20" name="Bebop with stereo cam and Wedgebug TU Delft Cyberzoo" security_height="0.4">
|
||||
<header>
|
||||
#include "subsystems/datalink/datalink.h"
|
||||
#include "subsystems/electrical.h"
|
||||
#include "subsystems/radio_control.h"
|
||||
#include "subsystems/ahrs.h"
|
||||
#define NPS_GAZEBO_WORLD "cyberzoo2019_ralphthesis2020.world" <!-- Current world is based on "cyberzoo2019_orange_poles_panels_mats.world" and replaced the original "cyberzoo_orange_poles.world" that was used here -->
|
||||
|
||||
#include "autopilot.h"
|
||||
inline void setNav(void){
|
||||
autopilot_mode_auto2 = AP_MODE_NAV;
|
||||
autopilot_static_set_mode(AP_MODE_NAV);
|
||||
}
|
||||
inline void setGuided(void){
|
||||
autopilot_mode_auto2 = AP_MODE_GUIDED;
|
||||
autopilot_static_set_mode(AP_MODE_GUIDED);
|
||||
}
|
||||
|
||||
|
||||
|
||||
</header>
|
||||
<waypoints>
|
||||
<!-- <waypoint lat="51.990631" lon="4.376796" name="HOME"/> -->
|
||||
<waypoint lat="51.990631" lon="4.376796" name="HOME"/>
|
||||
<waypoint name="_CLIMB" x="1.9" y="1.0"/>
|
||||
<waypoint name="_STDBY" x="1.9" y="1.0"/>
|
||||
<waypoint name="_TD" x="0.8" y="-1.7"/>
|
||||
<waypoint name="_GOAL1" x="1.9" y="1.0"/>
|
||||
<waypoint name="_TRAJECTORY" x="1.9" y="1.0"/>
|
||||
|
||||
|
||||
|
||||
|
||||
<waypoint name="START" x="0.0" y="0.0" alt= "1"/>
|
||||
<!-- <waypoint name="START2" x="2.0" y="1" alt= "1"/>-->
|
||||
<waypoint name="GOAL" x="0.0" y="8" alt= "1"/>
|
||||
<!-- <waypoint name="GOAL2" x="2.0" y="8" alt= "1"/>-->
|
||||
|
||||
|
||||
<waypoint x="-6.0" y="-2.0" name="_CZ1"/>
|
||||
<waypoint x="-6.0" y="10.0" name="_CZ2"/>
|
||||
<waypoint x="5.0" y="10.0" name="_CZ3"/>
|
||||
<waypoint x="5.0" y="-2.0" name="_CZ4"/>
|
||||
|
||||
<waypoint x="-5.0" y="-1.0" name="_OZ1"/>
|
||||
<waypoint x="-5.0" y="9.0" name="_OZ2"/>
|
||||
<waypoint x="4.0" y="9.0" name="_OZ3"/>
|
||||
<waypoint x="4.0" y="-1.0" name="_OZ4"/>
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
</waypoints>
|
||||
<sectors>
|
||||
<sector color="red" name="CyberZoo">
|
||||
<corner name="_CZ1"/>
|
||||
<corner name="_CZ2"/>
|
||||
<corner name="_CZ3"/>
|
||||
<corner name="_CZ4"/>
|
||||
</sector>
|
||||
<sector color="green" name="ObstacleZone">
|
||||
<corner name="_OZ1"/>
|
||||
<corner name="_OZ2"/>
|
||||
<corner name="_OZ3"/>
|
||||
<corner name="_OZ4"/>
|
||||
</sector>
|
||||
|
||||
|
||||
</sectors>
|
||||
<exceptions>
|
||||
<!-- RC lost -->
|
||||
<!--exception cond="((radio_control.status == RC_REALLY_LOST) &&
|
||||
!(IndexOfBlock('Holding point') > nav_block) &&
|
||||
!(nav_block >= IndexOfBlock('land here')) &&
|
||||
(autopilot_in_flight() == true) )" deroute="Standby"/-->
|
||||
<!-- Datalink lost (constant RPM descent) -->
|
||||
<exception cond="((datalink_time > 5) &&
|
||||
!(IndexOfBlock('Holding point') > nav_block) &&
|
||||
!(nav_block >= IndexOfBlock('land here')) &&
|
||||
(autopilot_in_flight() == true) )" deroute="land here"/>
|
||||
<!-- Geofencing XY -->
|
||||
<exception cond="(!InsideCyberZoo(GetPosX(), GetPosY()) &&
|
||||
!(IndexOfBlock('Holding point') > nav_block) &&
|
||||
!(nav_block >= IndexOfBlock('land here')) &&
|
||||
(autopilot_in_flight() == true) )" deroute="land here"/>
|
||||
<!-- Geofencing Z 3.5 -->
|
||||
<exception cond="((GetPosAlt() > 3.5) &&
|
||||
!(IndexOfBlock('Holding point') > nav_block) &&
|
||||
!(nav_block >= IndexOfBlock('land here')) &&
|
||||
(autopilot_in_flight() == true) )" deroute="land here"/>
|
||||
<!-- Geofencing Z 4.5 (constant RPM descent)-->
|
||||
<exception cond="((GetPosAlt() > 4.5) &&
|
||||
!(IndexOfBlock('Holding point') > nav_block) &&
|
||||
(autopilot_in_flight() == true) )" deroute="Landed"/>
|
||||
<!-- Bat low -->
|
||||
<exception cond="(electrical.bat_low &&
|
||||
!(IndexOfBlock('Holding point') > nav_block) &&
|
||||
!(nav_block >= IndexOfBlock('land here')) &&
|
||||
(autopilot_in_flight() == true) )" deroute="land here"/>
|
||||
<!-- Bat critical (constant RPM no stabilization)-->
|
||||
<exception cond="(electrical.bat_critical &&
|
||||
!(IndexOfBlock('Holding point') > nav_block) &&
|
||||
!(nav_block >= IndexOfBlock('land here')) &&
|
||||
(autopilot_in_flight() == true) )" deroute="land here"/>
|
||||
</exceptions>
|
||||
<blocks>
|
||||
<block name="Wait GPS">
|
||||
<call_once fun="NavKillThrottle()"/>
|
||||
<while cond="!GpsFixValid()"/>
|
||||
</block>
|
||||
<block name="Geo init">
|
||||
<while cond="LessThan(NavBlockTime(), 2)"/>
|
||||
<call_once fun="NavSetAltitudeReferenceHere()"/>
|
||||
</block>
|
||||
|
||||
<!-- Mdfd: Same as "1: Initial position" -->
|
||||
<block name="Holding point">
|
||||
<call_once fun="NavKillThrottle()"/>
|
||||
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
|
||||
</block>
|
||||
|
||||
<block key="r" name="Start Engine">
|
||||
<call_once fun="NavResurrect()"/>
|
||||
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
|
||||
<call_once fun="NavResurrect()"/>
|
||||
</block>
|
||||
|
||||
|
||||
<block key="t" name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
|
||||
<exception cond="GetPosAlt() > 0.8" deroute="Standby"/>
|
||||
<call_once fun="NavSetWaypointHere(WP__CLIMB)"/>
|
||||
<stay climb="nav_climb_vspeed" vmode="climb" wp="_CLIMB"/>
|
||||
</block>
|
||||
|
||||
<block key="s" name="Standby" strip_button="Standby" strip_icon="home.png">
|
||||
<call_once fun="NavSetWaypointHere(WP__STDBY)"/>
|
||||
<stay wp="_STDBY"/>
|
||||
</block>
|
||||
|
||||
|
||||
|
||||
|
||||
<!-- Mdfd: Same as "2: Move to start" -->
|
||||
<block name="1 - Move to Start">
|
||||
<call_once fun="NavResurrect()"/>
|
||||
<go wp="START"/>
|
||||
<stay wp="START"/>
|
||||
</block>
|
||||
|
||||
|
||||
|
||||
<block key="g" name="2 - Start Experiment" strip_button="Go" strip_icon="lookfore.png">
|
||||
<deroute block="Move to goal"/>
|
||||
</block>
|
||||
|
||||
|
||||
<!-- Mdfd: Same as "4: Move to goal" -->
|
||||
<block name="Move to goal">
|
||||
<call_once fun = "set_state(1,1)"/>
|
||||
<call_once fun="setGuided()"/>
|
||||
<deroute block="Position goal"/>
|
||||
</block>
|
||||
|
||||
|
||||
<!-- Mdfd: Same as "5: Position goal" -->
|
||||
<block name="Position goal">
|
||||
<stay wp="GOAL"/>
|
||||
</block>
|
||||
|
||||
|
||||
|
||||
|
||||
<block key="l" name="land here" strip_button="land here" strip_icon="land-right.png">
|
||||
<call_once fun="NavSetWaypointHere(WP__TD)"/>
|
||||
<go wp="_TD"/>
|
||||
<deroute block="Flare"/>
|
||||
</block>
|
||||
<block name="Land">
|
||||
<go wp="_TD"/>
|
||||
<deroute block="Flare"/>
|
||||
</block>
|
||||
<block name="Flare">
|
||||
<exception cond="NavDetectGround()" deroute="Holding point"/>
|
||||
<exception cond="!nav_is_in_flight()" deroute="Landed"/>
|
||||
<exception cond="0.10 > GetPosAlt()" deroute="Landed"/>
|
||||
<call_once fun="NavStartDetectGround()"/>
|
||||
<stay climb="nav_descend_vspeed" vmode="climb" wp="_TD"/>
|
||||
</block>
|
||||
<block name="Landed">
|
||||
<call_once fun="NavKillThrottle()"/>
|
||||
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
|
||||
</block>
|
||||
</blocks>
|
||||
</flight_plan>
|
||||
@@ -0,0 +1,92 @@
|
||||
<!-- Generic X-Box gamepad
|
||||
e.g. Logitech wireless gamepad F710
|
||||
|
||||
This config is for Xinput mode. Make sure slider switch on back of controller is on X (not D)
|
||||
Also make sure controller not in sports mode (mode light should be off)
|
||||
|
||||
Has six axes:
|
||||
axis 0: LTS_H (left thumb stick horizontal) (or DPad horizontal in sports mode)
|
||||
axis 1: LTS_V (left thumb stick vertical) (or DPad vertical in sports mode)
|
||||
axis 2: LT (left trigger)
|
||||
axis 3: RTS_H (right thumb stick horizontal)
|
||||
axis 4: RTS_V (right thumb stick vertical)
|
||||
axis 5: RT (right trigger)
|
||||
|
||||
It has 11 buttons.
|
||||
b0 - A (green)
|
||||
b1 - B (red)
|
||||
b2 - X (blue)
|
||||
b3 - Y (yellow)
|
||||
b4 - LB (left shoulder button)
|
||||
b5 - RB (right shoulder button)
|
||||
b6 - back
|
||||
b7 - start
|
||||
b8 - ?
|
||||
b9 - LSB (left stick button)
|
||||
b10 - RSB (right stick button)
|
||||
|
||||
and the DPad as a hat (in normal mode)
|
||||
You can use the Hat<Position>(<hat_name>) function to trigger events,
|
||||
where <Position> is one of
|
||||
Centered/Up/Right/Down/Left/RightUp/RightDown/LeftUp/LeftDown
|
||||
so e.g. HatDown(dpad)
|
||||
|
||||
-->
|
||||
|
||||
<joystick>
|
||||
<input>
|
||||
<axis index="3" name="yaw" limit="1.00" exponent="0.7" trim="0"/>
|
||||
<axis index="4" name="throttle"/>
|
||||
<axis index="2" name="LT" limit="1.00" trim="127"/>
|
||||
<axis index="0" name="roll" limit="1.00" exponent="0.7" trim="0"/>
|
||||
<axis index="1" name="pitch" limit="1.00" exponent="0.7" trim="0"/>
|
||||
<axis index="5" name="RT" limit="1.00" trim="127"/>
|
||||
<button index="0" name="A"/>
|
||||
<button index="1" name="B"/>
|
||||
<button index="2" name="X"/>
|
||||
<button index="3" name="Y"/>
|
||||
<button index="4" name="LB"/>
|
||||
<button index="5" name="RB"/>
|
||||
<button index="6" name="back"/>
|
||||
<button index="7" name="start"/>
|
||||
<button index="9" name="LSB"/>
|
||||
<button index="10" name="RSB"/>
|
||||
<hat index="0" name="dpad"/>
|
||||
</input>
|
||||
|
||||
<variables>
|
||||
<!-- manual by default and when pressing A, AUTO1 on B, AUTO2 on Y -->
|
||||
<var name="mode" default="0"/>
|
||||
<set var="mode" value="0" on_event="A"/>
|
||||
<set var="mode" value="1" on_event="B"/>
|
||||
<set var="mode" value="2" on_event="Y"/>
|
||||
</variables>
|
||||
|
||||
<messages period="0.05">
|
||||
|
||||
<message class="datalink" name="RC_4CH" send_always="true">
|
||||
<field name="mode" value="mode"/>
|
||||
<field name="throttle" value="Fit(-throttle,-127,127,0,127)"/>
|
||||
<field name="roll" value="roll"/>
|
||||
<field name="pitch" value="pitch"/>
|
||||
<field name="yaw" value="yaw"/>
|
||||
</message>
|
||||
|
||||
<!-- go to "Start Engine" block if in AUTO2 and pressing start button -->
|
||||
<message class="ground" name="JUMP_TO_BLOCK" on_event="(mode > 1) && start">
|
||||
<field name="block_id" value="IndexOfBlock('Start Engine')"/>
|
||||
</message>
|
||||
|
||||
<!-- go to "Takeoff" block if in AUTO2 and pressing up on dpad -->
|
||||
<message class="ground" name="JUMP_TO_BLOCK" on_event="(mode > 1) && HatUp(dpad)">
|
||||
<field name="block_id" value="IndexOfBlock('Takeoff')"/>
|
||||
</message>
|
||||
|
||||
<!-- go to "land here" block if in AUTO2 and pressing down on dpad -->
|
||||
<message class="ground" name="JUMP_TO_BLOCK" on_event="(mode > 1) && HatDown(dpad)">
|
||||
<field name="block_id" value="IndexOfBlock('land here')"/>
|
||||
</message>
|
||||
|
||||
|
||||
</messages>
|
||||
</joystick>
|
||||
@@ -0,0 +1,80 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="wedgebug" dir="wedgebug">
|
||||
<doc>
|
||||
<description> An integration of the WegdeBug algorithm (Laubach 1999) for path finding, for drones with stereo vision. </description>
|
||||
<define name="WEDGEBUG_CAMERA_RIGHT" value="front_camera|bottom_camera" description="Video device to use"/>
|
||||
<define name="WEDGEBUG_CAMERA_LEFT" value="front_camera|bottom_camera" description="Video device to use"/>
|
||||
|
||||
</doc>
|
||||
<settings>
|
||||
<dl_settings>
|
||||
<dl_settings name="W-Disparity">
|
||||
|
||||
<dl_setting var="SE_opening_OCV" min="5" step="1" max = "401" shortname="SE_op"/>
|
||||
<dl_setting var="SE_closing_OCV" min="5" step="1" max = "401" shortname="SE_clo"/>
|
||||
<dl_setting var="SE_dilation_OCV_1" min="5" step="1" max = "401" shortname="SE_dil_1"/>
|
||||
<dl_setting var="SE_dilation_OCV_2" min="5" step="1" max = "101" shortname="SE_dil_2"/>
|
||||
<dl_setting var="SE_erosion_OCV" min="5" step="1" max = "101" shortname="SE_ero"/>
|
||||
|
||||
<dl_setting var="K_median_w" min="1" step="1" max = "321" shortname="K_med_w"/>
|
||||
<dl_setting var="K_median_h" min="1" step="1" max = "321" shortname="K_med_h"/>
|
||||
<dl_setting var="threshold_median_depth" min="0" step="1" max = "10000" shortname="thr_obs_depth"/>
|
||||
<dl_setting var="threshold_depth_of_edges" min="0" step="1" max = "10000" shortname="thr_edge_depth"/>
|
||||
<dl_setting var="threshold_edge_magnitude" min="0" step="50" max = "50000" shortname="thr_edge_mag"/>
|
||||
<dl_setting var="threshold_distance_to_goal" min="0.00" step="0.05" max = "1.00" shortname="thr_goal_dist"/>
|
||||
<dl_setting var="threshold_distance_to_angle" min="0.0" step="0.1" max = "3.1" shortname="thr_angle_dist"/>
|
||||
|
||||
<dl_setting var="max_obstacle_confidence" min="0" step="1" max = "50" shortname="conf_obs"/>
|
||||
<dl_setting var="max_free_path_confidence " min="0" step="1" max = "50" shortname="conf_free"/>
|
||||
<dl_setting var="max_position_confidence" min="0" step="1" max = "50" shortname="conf_pos"/>
|
||||
<dl_setting var="max_heading_confidence" min="0" step="1" max = "50" shortname="conf_head"/>
|
||||
<dl_setting var="max_edge_found_micro_confidence" min="0" step="1" max = "50" shortname="conf_edge_mi"/>
|
||||
<dl_setting var="max_edge_found_macro_confidence" min="0" step="1" max = "50" shortname="conf_edge_mac"/>
|
||||
<dl_setting var="max_no_edge_found_confidence" min="0" step="1" max = "50" shortname="conf_no_edge"/>
|
||||
|
||||
|
||||
<dl_setting var="heat_map_type" min="0" step="1" max = "11" shortname="heatmap"/>
|
||||
|
||||
<dl_setting var="save_images_flag" min="0" step="1" max="1" shortname="Save Images">
|
||||
<strip_button name="Start image capture" icon="dcstart.png" value="1" group="cv"/>
|
||||
<strip_button name="Stop image capture" icon="dcstop.png" value="0" group="cv"/>
|
||||
</dl_setting>
|
||||
|
||||
|
||||
</dl_settings>
|
||||
|
||||
</dl_settings>
|
||||
</settings>
|
||||
<header>
|
||||
<file name="wedgebug.h"/>
|
||||
</header>
|
||||
<init fun="wedgebug_init()"/>
|
||||
<periodic fun="wedgebug_periodic()" freq="7"/> <!-- originally 4 -->
|
||||
<makefile target="ap|nps">
|
||||
<file name="wedgebug.c"/>
|
||||
<file name="wedgebug_opencv.cpp"/>
|
||||
|
||||
<flag name="CXXFLAGS" value="I$(PAPARAZZI_SRC)/sw/ext/opencv_bebop/install_pc/include"/> <!-- needed to include headers -->
|
||||
|
||||
<flag name="LDFLAGS" value="L$(PAPARAZZI_HOME)/sw/ext/opencv_bebop/install_pc/lib"/>
|
||||
<flag name="LDFLAGS" value="lopencv_world"/>
|
||||
<flag name="LDFLAGS" value="L$(PAPARAZZI_HOME)/sw/ext/opencv_bebop/install_pc/share/OpenCV/3rdparty/lib"/>
|
||||
<flag name="LDFLAGS" value="llibprotobuf"/>
|
||||
<flag name="LDFLAGS" value="lquirc"/>
|
||||
<flag name="LDFLAGS" value="L/usr/lib/x86_64-linux-gnu"/>
|
||||
<flag name="LDFLAGS" value="ljpeg"/>
|
||||
<flag name="LDFLAGS" value="lpng"/>
|
||||
<flag name="LDFLAGS" value="ltiff"/>
|
||||
<flag name="LDFLAGS" value="L/usr/lib/x86_64-linux-gnu/hdf5/serial"/>
|
||||
<flag name="LDFLAGS" value="lhdf5"/>
|
||||
<flag name="LDFLAGS" value="lpthread"/>
|
||||
<flag name="LDFLAGS" value="lsz"/>
|
||||
<flag name="LDFLAGS" value="lz"/>
|
||||
<flag name="LDFLAGS" value="ldl"/>
|
||||
<flag name="LDFLAGS" value="lm"/>
|
||||
<flag name="LDFLAGS" value="lfreetype"/>
|
||||
<flag name="LDFLAGS" value="lharfbuzz"/>
|
||||
<flag name="LDFLAGS" value="lrt"/>
|
||||
</makefile>
|
||||
</module>
|
||||
@@ -0,0 +1,13 @@
|
||||
<!DOCTYPE airframe SYSTEM "../../../airframes/airframe.dtd">
|
||||
|
||||
<airframe>
|
||||
<section name="SIMULATOR" prefix="NPS_">
|
||||
<define name="ACTUATOR_THRUSTS" value="2.09, 2.09, 2.09, 2.09" type="float[]"/>
|
||||
<define name="ACTUATOR_TORQUES" value="0.0250, 0.0250, 0.0250, 0.0250" type="float[]"/>
|
||||
<define name="ACTUATOR_TIME_CONSTANTS" value="0.02, 0.02, 0.02, 0.02" type="float[]"/>
|
||||
<define name="ACTUATOR_MAX_ANGULAR_MOMENTUM" value="0.19, 0.19, 0.19, 0.19" type="float[]"/>
|
||||
<define name="GAZEBO_AC_NAME" value="bebop_w_stereo_cams" type="string"/>
|
||||
<define name="BYPASS_AHRS" value="1"/>
|
||||
<define name="BYPASS_INS" value="1"/>
|
||||
</section>
|
||||
</airframe>
|
||||
@@ -0,0 +1,270 @@
|
||||
<?xml version='1.0'?>
|
||||
<sdf version='1.4'>
|
||||
<model name="bebop_w_stereo_cams">
|
||||
<pose>0 0 .1 0 0 1.57</pose>
|
||||
|
||||
<link name="chassis">
|
||||
<velocity_decay>
|
||||
<linear>0.001</linear>
|
||||
</velocity_decay>
|
||||
|
||||
<inertial>
|
||||
<mass>0.536</mass><!-- With bumpers. Tuned for correct nominal hover thrust, mass is higher than in real life (~0.40kg) -->
|
||||
<inertia>
|
||||
<ixx> 0.000906 </ixx>
|
||||
<iyy> 0.001242 </iyy>
|
||||
<izz> 0.002054 </izz>
|
||||
<ixy> 0. </ixy>
|
||||
<ixz> 0.000014 </ixz>
|
||||
<iyz> 0. </iyz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
|
||||
<collision name="collision">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.4 0.4 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<visual name="visual">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.15 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<diffuse>0.82 0.07 0.07 1</diffuse>
|
||||
<ambient>0.82 0.07 0.07 1</ambient>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<sensor name="contactsensor" type="contact">
|
||||
<contact>
|
||||
<collision>collision</collision>
|
||||
</contact>
|
||||
</sensor>
|
||||
</link>
|
||||
|
||||
<!-- MOTORS -->
|
||||
<link name="nw_motor">
|
||||
<pose>0.077 0.095 0 0 0 0</pose>
|
||||
<inertial>
|
||||
<mass>0.01</mass>
|
||||
<inertia>
|
||||
<ixx>0.0001</ixx>
|
||||
<iyy>0.0001</iyy>
|
||||
<izz>0.0001</izz>
|
||||
<ixy>0</ixy>
|
||||
<iyz>0</iyz>
|
||||
<ixz>0</ixz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<visual name="visual">
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.05</radius>
|
||||
<length>0.02</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<material>
|
||||
<diffuse>0.82 0.07 0.07 1</diffuse>
|
||||
<ambient>0.82 0.07 0.07 1</ambient>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint type="fixed" name="nw_motor_joint">
|
||||
<parent>chassis</parent>
|
||||
<child>nw_motor</child>
|
||||
</joint>
|
||||
|
||||
<link name="se_motor">
|
||||
<pose>-0.077 -0.095 0 0 0 0</pose>
|
||||
<inertial>
|
||||
<mass>0.01</mass>
|
||||
<inertia>
|
||||
<ixx>0.0001</ixx>
|
||||
<iyy>0.0001</iyy>
|
||||
<izz>0.0001</izz>
|
||||
<ixy>0</ixy>
|
||||
<iyz>0</iyz>
|
||||
<ixz>0</ixz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<visual name="visual">
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.05</radius>
|
||||
<length>0.02</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<material>
|
||||
<diffuse>0.1 0.1 0.1 1</diffuse>
|
||||
<ambient>0.1 0.1 0.1 1</ambient>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint type="fixed" name="se_motor_joint">
|
||||
<parent>chassis</parent>
|
||||
<child>se_motor</child>
|
||||
</joint>
|
||||
|
||||
<link name="ne_motor">
|
||||
<pose>0.077 -0.095 0 0 0 0</pose>
|
||||
<inertial>
|
||||
<mass>0.01</mass>
|
||||
<inertia>
|
||||
<ixx>0.0001</ixx>
|
||||
<iyy>0.0001</iyy>
|
||||
<izz>0.0001</izz>
|
||||
<ixy>0</ixy>
|
||||
<iyz>0</iyz>
|
||||
<ixz>0</ixz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<visual name="visual">
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.05</radius>
|
||||
<length>0.02</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<material>
|
||||
<diffuse>0.82 0.07 0.07 1</diffuse>
|
||||
<ambient>0.82 0.07 0.07 1</ambient>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint type="fixed" name="ne_motor_joint">
|
||||
<parent>chassis</parent>
|
||||
<child>ne_motor</child>
|
||||
</joint>
|
||||
|
||||
<link name="sw_motor">
|
||||
<pose>-0.077 0.095 0 0 0 0</pose>
|
||||
<inertial>
|
||||
<mass>0.01</mass>
|
||||
<inertia>
|
||||
<ixx>0.0001</ixx>
|
||||
<iyy>0.0001</iyy>
|
||||
<izz>0.0001</izz>
|
||||
<ixy>0</ixy>
|
||||
<iyz>0</iyz>
|
||||
<ixz>0</ixz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<visual name="visual">
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.05</radius>
|
||||
<length>0.02</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<material>
|
||||
<diffuse>0.1 0.1 0.1 1</diffuse>
|
||||
<ambient>0.1 0.1 0.1 1</ambient>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint type="fixed" name="sw_motor_joint">
|
||||
<parent>chassis</parent>
|
||||
<child>sw_motor</child>
|
||||
</joint>
|
||||
|
||||
|
||||
<!-- CAMERAS -->
|
||||
|
||||
<link name="front_camera"> <!-- left stereo camera -->
|
||||
<pose> 0.15 0.06 0 0 0 0</pose><!-- Bebop camera output is rotated 90 degrees and pitched slightly downwards -->
|
||||
<inertial>
|
||||
<mass>0.001</mass>
|
||||
<inertia>
|
||||
<ixx>0.0001</ixx>
|
||||
<iyy>0.0001</iyy>
|
||||
<izz>0.0001</izz>
|
||||
<ixy>0</ixy>
|
||||
<iyz>0</iyz>
|
||||
<ixz>0</ixz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<sensor type="camera" name="mt9f0022"> <!-- adjust with MT9F002_TARGET_FPS -->
|
||||
<update_rate>25.0</update_rate><!-- was 15.0 adjust with MT9F002_TARGET_FPS -->
|
||||
<camera>
|
||||
<image>
|
||||
<width>320</width><!-- Full sensor resolution -->
|
||||
<height>240</height><!-- Full sensor resolution -->
|
||||
<format>R8G8B8</format>
|
||||
</image>
|
||||
<horizontal_fov>1.91986217719376</horizontal_fov> <!-- 0.7175 -->
|
||||
<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>bottom_camera</child>
|
||||
</joint>
|
||||
|
||||
|
||||
|
||||
<link name="bottom_camera"> <!-- right stereo camera -->
|
||||
<pose> 0.15 -0.06 0.0 0 0 0</pose>
|
||||
<inertial>
|
||||
<mass>0.001</mass>
|
||||
<inertia>
|
||||
<ixx>0.0001</ixx>
|
||||
<iyy>0.0001</iyy>
|
||||
<izz>0.0001</izz>
|
||||
<ixy>0</ixy>
|
||||
<iyz>0</iyz>
|
||||
<ixz>0</ixz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<sensor type="camera" name="mt9v117">
|
||||
<update_rate>25.0</update_rate>
|
||||
<camera>
|
||||
<horizontal_fov>1.91986217719376</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>front_camera</child>
|
||||
</joint>
|
||||
</model>
|
||||
</sdf>
|
||||
@@ -0,0 +1,15 @@
|
||||
<?xml version="1.0"?>
|
||||
<model>
|
||||
<name>Bebop w. stereo camera (Paparazzi)</name>
|
||||
<version>1.0</version>
|
||||
<sdf version='1.4'>bebop_w_stereo_cams.sdf</sdf>
|
||||
|
||||
<author>
|
||||
<name>Ralph Rudi Schmidt</name>
|
||||
<email>ralph.r.schmidt@outlook.com</email>
|
||||
</author>
|
||||
|
||||
<description>
|
||||
Simple Bebop model which includes stereo vision and no downwards facing camera (http://wiki.paparazziuav.org).
|
||||
</description>
|
||||
</model>
|
||||
@@ -0,0 +1,15 @@
|
||||
<conf>
|
||||
|
||||
<aircraft
|
||||
name="bebop_stereo"
|
||||
ac_id="44"
|
||||
airframe="airframes/tudelft/bebop_ralphthesis2020_stereo.xml"
|
||||
radio="radios/dummy.xml"
|
||||
telemetry="telemetry/default_rotorcraft.xml"
|
||||
flight_plan="flight_plans/tudelft/ralphthesis2020_stereo_cyberzoo.xml"
|
||||
settings="settings/rotorcraft_basic.xml"
|
||||
settings_modules="modules/video_rtp_stream.xml modules/video_capture.xml modules/bebop_cam.xml modules/wedgebug.xml modules/ins_extended.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_indi_simple.xml [modules/nav_basic_rotorcraft.xml] modules/guidance_rotorcraft.xml [modules/gps.xml] modules/imu_common.xml"
|
||||
gui_color="white"
|
||||
release="c3090b6692780d8b3ad4b32cb9cef06f4186360c"
|
||||
/>
|
||||
</conf>
|
||||
@@ -54,6 +54,8 @@ void image_create(struct image_t *img, uint16_t width, uint16_t height, enum ima
|
||||
img->buf_size = sizeof(uint8_t) * 2 * width * height; // At maximum quality this is enough
|
||||
} else if (type == IMAGE_GRADIENT) {
|
||||
img->buf_size = sizeof(int16_t) * width * height;
|
||||
} else if (type == IMAGE_INT16) {
|
||||
img->buf_size = sizeof(int16_t) * width * height;
|
||||
} else {
|
||||
img->buf_size = sizeof(uint8_t) * width * height;
|
||||
}
|
||||
|
||||
@@ -36,7 +36,8 @@ enum image_type {
|
||||
IMAGE_YUV422, ///< UYVY format (uint16 per pixel)
|
||||
IMAGE_GRAYSCALE, ///< Grayscale image with only the Y part (uint8 per pixel)
|
||||
IMAGE_JPEG, ///< An JPEG encoded image (not per pixel encoded)
|
||||
IMAGE_GRADIENT ///< An image gradient (int16 per pixel)
|
||||
IMAGE_GRADIENT, ///< An image gradient (int16 per pixel)
|
||||
IMAGE_INT16 ///< An image to hold disparity image data from openCV (int16 per pixel)
|
||||
};
|
||||
|
||||
/* Main image structure */
|
||||
@@ -62,6 +63,17 @@ struct point_t {
|
||||
uint16_t y_sub; ///< The y subpixel coordinate of the point
|
||||
};
|
||||
|
||||
|
||||
/* 3d Image point structure */
|
||||
struct point3d_t {
|
||||
float X; ///< The x coordinate of the point
|
||||
float Y; ///< The y coordinate of the point
|
||||
float Z; ///< The y coordinate of the point
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
/* Vector structure for point differences */
|
||||
struct flow_t {
|
||||
struct point_t pos; ///< The original position the flow comes from in subpixels
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,111 @@
|
||||
/*
|
||||
* Copyright (C) Ralph Rudi schmidt <ralph.r.schmidt@outlook.com>
|
||||
|
||||
*
|
||||
* 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/wedgebug/wedgebug.h"
|
||||
* @author Ralph Rudi schmidt <ralph.r.schmidt@outlook.com>
|
||||
* An integration of the WegdeBug algorithm (Laubach 1999) for path finding, for drones with stereo vision.
|
||||
*/
|
||||
#ifndef WEDGEBUG_H
|
||||
#define WEDGEBUG_H
|
||||
|
||||
|
||||
|
||||
// Including library for types
|
||||
#include <stdint.h>
|
||||
#include "modules/computer_vision/lib/vision/image.h"// For image-related structures
|
||||
|
||||
// Periodic-type functions
|
||||
extern void wedgebug_init(void);
|
||||
extern void wedgebug_periodic(void);
|
||||
|
||||
|
||||
// Structures
|
||||
/* Kernel - processes single channel images */
|
||||
struct kernel_C1 {
|
||||
enum image_type type; // Type of image on which kernel is laid onto
|
||||
uint16_t w; //s/< Kernel width
|
||||
uint16_t h; ///< Kernel height
|
||||
uint32_t buf_size; ///< Size of values of weight buffer and values buffer
|
||||
void *buf_weights; ///< Kernel weight buffer
|
||||
void *buf_values; ///< Kernel value buffer. These are the values underneath the kernel
|
||||
};
|
||||
|
||||
|
||||
// Global variables - Defines as settings
|
||||
extern int N_disparities;
|
||||
extern int block_size_disparities;
|
||||
extern int min_disparity;
|
||||
extern int max_disparity;
|
||||
|
||||
|
||||
extern uint16_t K_median_h; // Height of kernel for the median kernel
|
||||
extern uint16_t K_median_w; // Width of kernel for the median kernel
|
||||
extern int SE_opening_OCV; // SE size for the opening operation
|
||||
extern int SE_closing_OCV; // SE size for the closing operation
|
||||
extern int
|
||||
SE_dilation_OCV_1;// SE size for the first dilation operation (Decides where edges are detected, increase to increase drone safety zone NOTE. This functionality should be replaced with c space expansion)
|
||||
extern int SE_dilation_OCV_2; // SE size for the second dilation operation (see state 6 "WEDGEBUG_START" )
|
||||
extern int
|
||||
SE_erosion_OCV; // SE size for the erosion operation (see state 3 "WEDGEBUG_START" and state 6 "POSITION_EDGE", its needed to "drag" the depth of the foreground objects over the edges detected)
|
||||
|
||||
|
||||
// Setting thresholds
|
||||
|
||||
extern uint16_t
|
||||
threshold_median_depth; //! Below this median depth (cm), an obstacle is considered to block the way (i.e. the blocking obstacle needs to be close)
|
||||
extern uint16_t threshold_depth_of_edges; //! Below this depth (cm) edges are eligible for the WedgeBug algorith
|
||||
|
||||
extern int
|
||||
threshold_edge_magnitude; // Edges with a magnitude above this value are detected. Above this value, edges are given the value 127, otherwise they are given the value zero.
|
||||
extern float threshold_distance_to_goal; // Above this threshold, the goal is considered reached
|
||||
extern float threshold_distance_to_angle; // Above this threshold, the angle/heading is considered reached
|
||||
extern int16_t max_obstacle_confidence; // This is the max confidence that an obstacle was spotted
|
||||
extern int16_t max_free_path_confidence; // This is the max confidence that an obstacle was not spotted
|
||||
extern int16_t max_position_confidence; // This is the max confidence that a specific position was reached
|
||||
extern int16_t max_heading_confidence; // This is the max confidence that a specific heading was reached
|
||||
extern int16_t max_edge_found_micro_confidence; // This is the max confidence that edges (micro-see above) were found
|
||||
extern int16_t max_edge_found_macro_confidence; // This is the max confidence that edges (macro-see above were found
|
||||
extern int16_t max_no_edge_found_confidence; // This is the max confidence that no edges were found
|
||||
|
||||
|
||||
|
||||
extern int heat_map_type; // Heat map used when saving image
|
||||
extern uint8_t save_images_flag;
|
||||
|
||||
|
||||
|
||||
// Global functions
|
||||
extern void post_disparity_crop_rect(struct crop_t *img_cropped_info, struct img_size_t *original_img_dims,
|
||||
const int disp_n, const int block_size);
|
||||
extern void set_state(uint8_t state, uint8_t change_allowed);
|
||||
void kernel_create(struct kernel_C1 *kernel, uint16_t width, uint16_t height, enum image_type type);
|
||||
extern void kernel_free(struct kernel_C1 *kernel);
|
||||
extern uint8_t getMedian(uint8_t *a, uint32_t n);
|
||||
extern int heat_map_type;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#endif // WEDGEBUG_H
|
||||
@@ -0,0 +1,454 @@
|
||||
/*
|
||||
* 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/>.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#include "wedgebug_opencv.h"
|
||||
|
||||
#include <opencv2/core/core.hpp>
|
||||
#include <opencv2/imgproc/imgproc.hpp>
|
||||
#include <opencv2/imgproc/types_c.h> // needed for CV_MOP_CLOSE
|
||||
#include <opencv2/imgcodecs/imgcodecs.hpp>
|
||||
#include <opencv2/calib3d/calib3d.hpp>
|
||||
#include <iostream>
|
||||
#include <stdint.h>
|
||||
|
||||
using namespace cv;
|
||||
|
||||
|
||||
|
||||
// Local declarations
|
||||
int transfer(const Mat *from, const image_t *to);
|
||||
|
||||
|
||||
// Local functions
|
||||
int transfer(const Mat *from, const image_t *to)
|
||||
{
|
||||
// Determining type of supplied image
|
||||
// 1) If image is of type uint16_t
|
||||
if (to->type == IMAGE_INT16) {
|
||||
typedef uint16_t img_dip_type;
|
||||
for (int i = 0; i < (from->rows * from->cols); i++) {
|
||||
((img_dip_type *)to->buf)[i] = from->at<img_dip_type>
|
||||
(i); // Using ".at" here, as accessing buffer is problematic with a cropped image as it maintains a connection to original image
|
||||
}
|
||||
}
|
||||
// 2) If image is of type uint8_t
|
||||
else if (to->type == IMAGE_GRAYSCALE) { // If image is of type uint8_t
|
||||
typedef uint8_t img_dip_type;
|
||||
for (int i = 0; i < (from->rows * from->cols); i++) {
|
||||
((img_dip_type *)to->buf)[i] = from->at<img_dip_type>
|
||||
(i); // Using ".at" here, as accessing buffer is problematic with a cropped image as it maintains a connection to original image
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// Global functions
|
||||
int save_image_gray(struct image_t *img, char *myString)
|
||||
{
|
||||
// Create a new image, using the original bebop image.
|
||||
|
||||
if (img->type == IMAGE_INT16) {
|
||||
Mat M(img->h, img->w, CV_16SC1, img->buf);
|
||||
Mat Mcopy = M.clone();
|
||||
//normalize(M, M, 0, 255, NORM_MINMAX);
|
||||
imwrite(myString, Mcopy);
|
||||
|
||||
} else if (img->type == IMAGE_GRAYSCALE) {
|
||||
Mat M(img->h, img->w, CV_8UC1, img->buf);
|
||||
Mat Mcopy = M.clone();
|
||||
|
||||
imwrite(myString, Mcopy);
|
||||
} else {
|
||||
std::cout << "This function only worked with images of type IMAGE_GRAYSCALE and IAMGE_OPENCV_DISP. Leaving function." <<
|
||||
std::endl;
|
||||
return -1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
int save_image_color(struct image_t *img, char *myString)
|
||||
{
|
||||
// Create a new image, using the original bebop image.
|
||||
Mat M(img->h, img->w, CV_8UC2, img->buf); // CV_8UC2 is the openCV format for UYVY images
|
||||
// Definition of Mat: Mat::Mat(int _rows, int _cols, int _type, void* _data, size_t _step)
|
||||
// Remember that void takes any data type including char
|
||||
Mat image;
|
||||
|
||||
cvtColor(M, image, CV_YUV2GRAY_Y422);
|
||||
imwrite(myString, image);
|
||||
|
||||
// Code below is for testing
|
||||
//double minVal;
|
||||
//double maxVal;
|
||||
//Point minLoc;
|
||||
//Point maxLoc;
|
||||
//minMaxLoc(image ,&minVal, &maxVal, &minLoc, &maxLoc);
|
||||
//std::cout << "Left1: Min=" << minVal << "; Max=" << maxVal << std::endl;
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
int save_image_HM(struct image_t *img, char *myString, int const heatmap)
|
||||
{
|
||||
{
|
||||
// Create a new image, using the original bebop image.
|
||||
|
||||
/*
|
||||
* Heat maps:
|
||||
* 0 = COLORMAP_AUTUMN
|
||||
* 1 = COLORMAP_BONE
|
||||
* 2 = COLORMAP_JET
|
||||
* 3 = COLORMAP_WINTER
|
||||
* 4 = COLORMAP_RAINBOW
|
||||
* 5 = COLORMAP_OCEAN
|
||||
* 6 = COLORMAP_SUMMER
|
||||
* 7 = COLORMAP_SPRING
|
||||
* 8 = COLORMAP_COOL
|
||||
* 9 = COLORMAP_HSV
|
||||
* 10 = COLORMAP_PINK
|
||||
* 11 = COLORMAP_HOT
|
||||
*/
|
||||
|
||||
|
||||
if (img->type == IMAGE_INT16) {
|
||||
Mat M(img->h, img->w, CV_16SC1, img->buf);
|
||||
Mat Mcopy = M.clone();
|
||||
|
||||
//normalize(Mcopy, Mcopy, 0, 255, NORM_MINMAX);
|
||||
Mcopy.convertTo(Mcopy, CV_8UC1); //Converting image to 8 bit image
|
||||
applyColorMap(Mcopy, Mcopy, heatmap);
|
||||
|
||||
imwrite(myString, Mcopy);
|
||||
|
||||
} else if (img->type == IMAGE_GRAYSCALE) {
|
||||
Mat M(img->h, img->w, CV_8UC1, img->buf);
|
||||
Mat Mcopy = M.clone();
|
||||
|
||||
applyColorMap(Mcopy, Mcopy, heatmap);
|
||||
|
||||
imwrite(myString, Mcopy);
|
||||
} else {
|
||||
std::cout << "This function only worked with images of type IMAGE_GRAYSCALE and IAMGE_OPENCV_DISP. Leaving function." <<
|
||||
std::endl;
|
||||
return -1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
int SBM_OCV(struct image_t *img_disp, const struct image_t *img_left, const struct image_t *img_right,
|
||||
const int ndisparities, const int SADWindowSize, const bool cropped)
|
||||
{
|
||||
// Defining variables
|
||||
Mat img_left_OCV(img_left->h, img_left->w, CV_8UC1, img_left->buf);
|
||||
Mat img_right_OCV(img_right->h, img_right->w, CV_8UC1, img_right->buf);
|
||||
Mat img_disp_OCV;
|
||||
Mat img_disp_2_OCV;
|
||||
|
||||
//std::cout << "OpenCV version : " << CV_VERSION << std::endl;
|
||||
//std::cout << "Major version : " << CV_MAJOR_VERSION << std::endl;
|
||||
//std::cout << "Minor version : " << CV_MINOR_VERSION << std::endl;
|
||||
//std::cout << "Subminor version : " << CV_SUBMINOR_VERSION << std::endl;
|
||||
|
||||
|
||||
|
||||
// Block matching
|
||||
Ptr<StereoBM> sbm = StereoBM::create(ndisparities, SADWindowSize);
|
||||
sbm->compute(img_left_OCV, img_right_OCV, img_disp_OCV); //type of img_disp_OCV is CV_16S i.e. int16_t
|
||||
|
||||
|
||||
// Determining type of supplied image
|
||||
// 1) If image is of type int16_t
|
||||
if (img_disp->type == IMAGE_INT16) {
|
||||
//std::cout << "int16_t" << std::endl;
|
||||
typedef int16_t img_dip_type;
|
||||
|
||||
// Cropping or not cropping:
|
||||
if (cropped == 0) { // If image should not be cropped
|
||||
for (int i = 0; i < (img_disp_OCV.rows * img_disp_OCV.cols); i++) {
|
||||
((img_dip_type *)img_disp->buf)[i] = img_disp_OCV.at<img_dip_type>
|
||||
(i); // Using ".at" here are accessing buffer is problematic with a cropped image as it maintains a connection to oriinal image
|
||||
}
|
||||
} else if (cropped == 1) { // If image should be cropped
|
||||
struct crop_t img_cropped_info;
|
||||
struct img_size_t original_img_dims = {img_left->w, img_left->h};
|
||||
|
||||
post_disparity_crop_rect(&img_cropped_info, &original_img_dims, ndisparities,
|
||||
SADWindowSize); // Function from wedebug.h
|
||||
Rect crop_area = Rect(img_cropped_info.x, img_cropped_info.y, img_cropped_info.w, img_cropped_info.h);
|
||||
Mat img_cropped = img_disp_OCV(crop_area);// <---
|
||||
|
||||
for (int i = 0; i < (img_cropped.rows * img_cropped.cols); i++) {
|
||||
((img_dip_type *)img_disp->buf)[i] = img_cropped.at<img_dip_type>
|
||||
(i); // Using ".at" here are accessing buffer is problematic with a cropped image as it maintains a connection to oriinal image
|
||||
}
|
||||
|
||||
} else {return -1;}
|
||||
}
|
||||
|
||||
|
||||
// 2) If image is of type uint8_t
|
||||
else if (img_disp->type == IMAGE_GRAYSCALE) { // If image is of type uint8_t
|
||||
|
||||
//std::cout << "uint8_t" << std::endl;
|
||||
typedef uint8_t img_dip_type;
|
||||
|
||||
// THe followig code is to display the depth map with depth capped at 256/
|
||||
// This is for debugging only such that the depth image has moe contrast when observing it
|
||||
//imwrite("/home/dureade/Documents/paparazzi_images/img_disp_int8_cropped_capped.bmp", img_disp_OCV);
|
||||
|
||||
|
||||
|
||||
img_disp_OCV = img_disp_OCV / 16; // We divide by 16 to get actual disparity values
|
||||
img_disp_OCV.convertTo(img_disp_OCV, CV_8UC1); // Now we convert 16bit image to 8 bit image
|
||||
|
||||
|
||||
|
||||
// Cropping or not cropping:
|
||||
if (cropped == 0) { // If image should not be cropped
|
||||
for (int i = 0; i < (img_disp_OCV.rows * img_disp_OCV.cols); i++) {
|
||||
((img_dip_type *)img_disp->buf)[i] = img_disp_OCV.data[i];
|
||||
}
|
||||
} else if (cropped == 1) { // If image should be cropped
|
||||
struct crop_t img_cropped_info;
|
||||
struct img_size_t original_img_dims = {img_left->w, img_left->h};
|
||||
|
||||
|
||||
post_disparity_crop_rect(&img_cropped_info, &original_img_dims, ndisparities,
|
||||
SADWindowSize); // Function from wedebug.h
|
||||
|
||||
Rect crop_area = Rect(img_cropped_info.x, img_cropped_info.y, img_cropped_info.w, img_cropped_info.h);
|
||||
Mat img_cropped = img_disp_OCV(crop_area); // Problem here
|
||||
|
||||
|
||||
// Test code for saving cropped left image
|
||||
//Mat img_left_cropped =img_left_OCV(crop_area);
|
||||
//imwrite("/home/dureade/Documents/paparazzi_images/for_report/img_left_int8_cropped.bmp", img_left_cropped);
|
||||
|
||||
for (int i = 0; i < (img_cropped.rows * img_cropped.cols); i++) {
|
||||
((img_dip_type *)img_disp->buf)[i] = img_cropped.at<img_dip_type>
|
||||
(i); // Using ".at" here are accessing buffer is problematic with a cropped image as it maintains a connection to oriinal image
|
||||
}
|
||||
} else {return -1;}
|
||||
|
||||
}
|
||||
|
||||
|
||||
// 3) If image is of unsupported type
|
||||
else {
|
||||
std::cout << "This function only worked with images of type IMAGE_GRAYSCALE and IAMGE_OPENCV_DISP. Leaving function." <<
|
||||
std::endl;
|
||||
return -1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
int opening_OCV(struct image_t *img_input, const struct image_t *img_output, const int SE_size, const int iteration)
|
||||
{
|
||||
Mat img_input_OCV;
|
||||
Mat img_output_OCV;
|
||||
Mat kernel = getStructuringElement(MORPH_RECT, Size(SE_size, SE_size));
|
||||
|
||||
if (img_input->type == IMAGE_INT16) {
|
||||
img_input_OCV = Mat(img_input->h, img_input->w, CV_16S, img_input->buf);
|
||||
} else if (img_input->type == IMAGE_GRAYSCALE) {
|
||||
img_input_OCV = Mat(img_input->h, img_input->w, CV_8UC1, img_input->buf);
|
||||
} else {
|
||||
std::cout << "This function only worked with images of type IMAGE_GRAYSCALE and IAMGE_OPENCV_DISP. Leaving function." <<
|
||||
std::endl;
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
||||
morphologyEx(img_input_OCV, img_output_OCV, CV_MOP_OPEN, kernel, Point(-1, -1), iteration);
|
||||
//erode(img_input_OCV, img_output_OCV, kernel);
|
||||
transfer(&img_output_OCV, img_output);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
int closing_OCV(struct image_t *img_input, const struct image_t *img_output, const int SE_size, const int iteration)
|
||||
{
|
||||
Mat img_input_OCV;
|
||||
Mat img_output_OCV;
|
||||
Mat kernel = getStructuringElement(MORPH_RECT, Size(SE_size, SE_size));
|
||||
|
||||
if (img_input->type == IMAGE_INT16) {
|
||||
img_input_OCV = Mat(img_input->h, img_input->w, CV_16S, img_input->buf);
|
||||
|
||||
} else if (img_input->type == IMAGE_GRAYSCALE) {
|
||||
img_input_OCV = Mat(img_input->h, img_input->w, CV_8UC1, img_input->buf);
|
||||
} else {
|
||||
std::cout << "This function only worked with images of type IMAGE_GRAYSCALE and IAMGE_OPENCV_DISP. Leaving function." <<
|
||||
std::endl;
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
||||
morphologyEx(img_input_OCV, img_output_OCV, CV_MOP_CLOSE, kernel, Point(-1, -1), iteration);
|
||||
//erode(img_input_OCV, img_output_OCV, kernel);
|
||||
transfer(&img_output_OCV, img_output);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
int dilation_OCV(struct image_t *img_input, const struct image_t *img_output, const int SE_size, const int iteration)
|
||||
{
|
||||
Mat img_input_OCV;
|
||||
Mat img_output_OCV;
|
||||
Mat kernel = getStructuringElement(MORPH_RECT, Size(SE_size, SE_size));
|
||||
|
||||
if (img_input->type == IMAGE_INT16) {
|
||||
img_input_OCV = Mat(img_input->h, img_input->w, CV_16S, img_input->buf);
|
||||
|
||||
} else if (img_input->type == IMAGE_GRAYSCALE) {
|
||||
img_input_OCV = Mat(img_input->h, img_input->w, CV_8UC1, img_input->buf);
|
||||
} else {
|
||||
std::cout << "This function only worked with images of type IMAGE_GRAYSCALE and IAMGE_OPENCV_DISP. Leaving function." <<
|
||||
std::endl;
|
||||
return -1;
|
||||
}
|
||||
|
||||
dilate(img_input_OCV, img_output_OCV, kernel, Point(-1, -1), iteration);
|
||||
//erode(img_input_OCV, img_output_OCV, kernel);
|
||||
transfer(&img_output_OCV, img_output);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
int erosion_OCV(struct image_t *img_input, const struct image_t *img_output, const int SE_size, const int iteration)
|
||||
{
|
||||
Mat img_input_OCV;
|
||||
Mat img_output_OCV;
|
||||
Mat kernel = getStructuringElement(MORPH_RECT, Size(SE_size, SE_size));
|
||||
|
||||
if (img_input->type == IMAGE_INT16) {
|
||||
img_input_OCV = Mat(img_input->h, img_input->w, CV_16S, img_input->buf);
|
||||
|
||||
} else if (img_input->type == IMAGE_GRAYSCALE) {
|
||||
img_input_OCV = Mat(img_input->h, img_input->w, CV_8UC1, img_input->buf);
|
||||
} else {
|
||||
std::cout << "This function only worked with images of type IMAGE_GRAYSCALE and IAMGE_OPENCV_DISP. Leaving function." <<
|
||||
std::endl;
|
||||
return -1;
|
||||
}
|
||||
|
||||
erode(img_input_OCV, img_output_OCV, kernel, Point(-1, -1), iteration);
|
||||
//erode(img_input_OCV, img_output_OCV, kernel);
|
||||
transfer(&img_output_OCV, img_output);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
int sobel_OCV(struct image_t *img_input, const struct image_t *img_output, const int kernel_size, const int thr)
|
||||
{
|
||||
|
||||
Mat img_input_OCV;
|
||||
Mat img_grad_x, img_grad_y, img_grad_mag;
|
||||
int ddepth = CV_32F;//CV_16S; // Format of gradient image output (CV_32F needed for normalization function)
|
||||
int delta = 0; // Value added to each gradient pixel
|
||||
int scale = 1; // Factor by which gradient pixels is increased
|
||||
|
||||
|
||||
if (img_input->type == IMAGE_INT16) {
|
||||
//std::cout << "IMAGE_OPEN_DISP" << std::endl;
|
||||
img_input_OCV = Mat(img_input->h, img_input->w, CV_16S, img_input->buf);
|
||||
Sobel(img_input_OCV, img_grad_x, ddepth, 1, 0, kernel_size, scale, delta, BORDER_DEFAULT); // Horizontal gradient
|
||||
//imwrite("/home/dureade/Documents/paparazzi_images/img_grad_x.bmp", img_grad_x*-1);
|
||||
Sobel(img_input_OCV, img_grad_y, ddepth, 0, 1, kernel_size, scale, delta, BORDER_DEFAULT); // Vertical gradient
|
||||
//imwrite("/home/dureade/Documents/paparazzi_images/img_grad_y.bmp", img_grad_y);
|
||||
magnitude(img_grad_x, img_grad_y, img_grad_mag); // Calculating magnitude
|
||||
|
||||
//imwrite("/home/dureade/Documents/paparazzi_images/for_report/img_grad_mag.bmp", img_grad_mag);
|
||||
threshold(img_grad_mag, img_grad_mag, thr, 127, THRESH_BINARY);
|
||||
} else if (img_input->type == IMAGE_GRAYSCALE) {
|
||||
//std::cout << "IMAGE_GRAYSCALE" << std::endl;
|
||||
img_input_OCV = Mat(img_input->h, img_input->w, CV_8UC1, img_input->buf);
|
||||
Sobel(img_input_OCV, img_grad_x, ddepth, 1, 0, kernel_size, scale, delta, BORDER_DEFAULT); // Horizontal gradient
|
||||
//imwrite("/home/dureade/Documents/paparazzi_images/img_grad_x.bmp", img_grad_x*-1);
|
||||
Sobel(img_input_OCV, img_grad_y, ddepth, 0, 1, kernel_size, scale, delta, BORDER_DEFAULT); // Vertical gradient
|
||||
//imwrite("/home/dureade/Documents/paparazzi_images/img_grad_y.bmp", img_grad_y);
|
||||
magnitude(img_grad_x, img_grad_y, img_grad_mag); // Calculating magnitude
|
||||
//imwrite("/home/dureade/Documents/paparazzi_images/for_report/img_grad_mag.bmp", img_grad_mag);
|
||||
threshold(img_grad_mag, img_grad_mag, thr, 127, THRESH_BINARY);
|
||||
} else {
|
||||
std::cout << "This function only worked with images of type IMAGE_GRAYSCALE and IAMGE_OPENCV_DISP. Leaving function." <<
|
||||
std::endl;
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
||||
|
||||
//normalize(img_grad_mag, img_grad_mag, 0, 255, NORM_MINMAX); // Normalizing magnitude between 0 and 255
|
||||
|
||||
img_grad_mag.convertTo(img_grad_mag, CV_8UC1); //Converting image to 8 bit image
|
||||
|
||||
transfer(&img_grad_mag, img_output); // Saving image into output images
|
||||
|
||||
Mat(img_output->h, img_output->w, CV_8UC1, img_output->buf);
|
||||
|
||||
|
||||
/*
|
||||
// Size of variables
|
||||
std::cout << "img_input_OCV = " << img_input_OCV.total() * img_input_OCV.elemSize() << std::endl;
|
||||
std::cout << "img_grad_x = " << img_grad_x.total() * img_grad_x.elemSize() << std::endl;
|
||||
std::cout << "img_grad_y = " << img_grad_y.total() * img_grad_y.elemSize() << std::endl;
|
||||
std::cout << "img_grad_mag = " << img_grad_mag.total() * img_grad_mag.elemSize() << std::endl;
|
||||
*/
|
||||
/*
|
||||
double minVal;
|
||||
double maxVal;
|
||||
Point minLoc;
|
||||
Point maxLoc;
|
||||
minMaxLoc(img_grad_mag ,&minVal, &maxVal, &minLoc, &maxLoc);
|
||||
std::cout << "grad_x: Min=" << minVal << "; Max=" << maxVal << std::endl;
|
||||
|
||||
|
||||
//imwrite("/home/dureade/Documents/paparazzi_images/img_grad_mag.bmp", img_grad_mag);
|
||||
//imwrite("/home/dureade/Documents/paparazzi_images/abs_grad_y.bmp", abs_grad_y);
|
||||
//imwrite("/home/dureade/Documents/paparazzi_images/grad.bmp", grad);
|
||||
//imwrite("/home/dureade/Documents/paparazzi_images/img_input_OCV.bmp", img_input_OCV);
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
|
||||
return 1;
|
||||
}
|
||||
@@ -0,0 +1,70 @@
|
||||
/*
|
||||
* Copyright (C) Ralph Rudi schmidt <ralph.r.schmidt@outlook.com>
|
||||
|
||||
*
|
||||
* 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/wedgebug/wedgebug_opencv.h"
|
||||
* @author Ralph Rudi schmidt <ralph.r.schmidt@outlook.com>
|
||||
* OpenCV functions to be used for the wedebug.
|
||||
*/
|
||||
|
||||
#ifndef WEDGEBUG_OPENCV_H
|
||||
#define WEDGEBUG_OPENCV_H
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
// Custom C headers to include
|
||||
#include "modules/computer_vision/lib/vision/image.h" // Needed for imae_t struct type
|
||||
#include "modules/wedgebug/wedgebug.h" // needed for calculating crop dimensions
|
||||
|
||||
|
||||
// Global functions:
|
||||
|
||||
extern int save_image_gray(struct image_t *img, char *myString);
|
||||
int save_image_color(struct image_t *img, char *myString);
|
||||
int save_image_HM(struct image_t *img, char *myString, int const heatmap);
|
||||
extern int SBM_OCV(struct image_t *img_disp, const struct image_t *img_left, const struct image_t *img_right,
|
||||
const int ndisparities, const int SADWindowSize, const bool cropped);
|
||||
extern int opening_OCV(struct image_t *img_input, const struct image_t *img_output, const int SE_size,
|
||||
const int iteration);
|
||||
extern int closing_OCV(struct image_t *img_input, const struct image_t *img_output, const int SE_size,
|
||||
const int iteration);
|
||||
extern int dilation_OCV(struct image_t *img_input, const struct image_t *img_output, const int SE_size,
|
||||
const int iteration);
|
||||
extern int erosion_OCV(struct image_t *img_input, const struct image_t *img_output, const int SE_size,
|
||||
const int iteration);
|
||||
int sobel_OCV(struct image_t *img_input, const struct image_t *img_output, const int kernel_size, const int thr);
|
||||
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // WEDGEBUG_OPENCV_H
|
||||
Submodule sw/ext/tudelft_gazebo_models updated: 1606051925...5e1141556c
Reference in New Issue
Block a user