#include <stdint.h>
#include "modules/computer_vision/lib/vision/image.h"

Include dependency graph for wedgebug.h:

This graph shows which files directly or indirectly include this file:

Data Structures
struct	kernel_C1

Functions
void	wedgebug_init (void)

void	wedgebug_periodic (void)

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)

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)

void	kernel_free (struct kernel_C1 *kernel)

uint8_t	getMedian (uint8_t *a, uint32_t n)

Variables
int	N_disparities
	Variable that saves previous mode to control the drone, for some memory.

int	block_size_disparities
	Number of disparity levels (0-this number)

int	min_disparity
	Block size used for the block matching (SBM) function.

int	max_disparity

uint16_t	K_median_h
	Width of kernel for the median kernel.

uint16_t	K_median_w
	SE size for the Sobel operation, to detect edges.

int	SE_opening_OCV

int	SE_closing_OCV
	SE size for the opening operation.

int	SE_dilation_OCV_1
	SE size for the closing operation.

int	SE_dilation_OCV_2
	SE size for the first dilation operation.

int	SE_erosion_OCV
	SE size for the second dilation operation (see state 3 "WEDGEBUG_START" and state 6 "POSITION_EDGE" )

uint16_t	threshold_median_depth
	Below this distance (in meters) it is considered that the robot has reached the goal, in DIRECT_CONTROL mode.

uint16_t	threshold_depth_of_edges
	Below this median depth (cm), an obstacle is considered to block the way (i.e. the blocking obstacle needs to be close)

int	threshold_edge_magnitude
	Below this depth (cm) edges are eligible for the WedgeBug algorith.

float	threshold_distance_to_goal
	Above this disparity edges are eligible for WedgeBug algorithm (i.e. edges cannot be very far away)

float	threshold_distance_to_angle
	Below this distance (in meters) it is considered that the robot has reached the goal.

int16_t	max_obstacle_confidence
	This is the confidence that no edge was found.

int16_t	max_free_path_confidence
	This is the max confidence that an obstacle was spotted.

int16_t	max_position_confidence
	This is the max confidence that an obstacle was not spotted.

int16_t	max_heading_confidence
	This is the max confidence that a specific position was reached.

int16_t	max_edge_found_micro_confidence
	This is the max confidence that a specific heading was reached.

int16_t	max_edge_found_macro_confidence
	This is the max confidence that edges (micro-see above) were found.

int16_t	max_no_edge_found_confidence
	This is the max confidence that edges (macro-see above were found.

int	heat_map_type
	Variable to hold distance from robot to edge to goal (used in EDGE_SCAN (9) state)

uint8_t	save_images_flag
	Set to 1 if control mode of drone is changed, 0 otherwise.

Detailed Description

Author: Ralph Rudi schmidt ralph.nosp@m..r.s.nosp@m.chmid.nosp@m.t@ou.nosp@m.tlook.nosp@m..com An integration of the WegdeBug algorithm (Laubach 1999) for path finding, for drones with stereo vision.

Note.

Information on different flight modes (such as AP_MODE_GUIDED) can be found here paparazzi/sw/airborne/firmwares/rotorcraft/autopilot_static.h
When starting paparazzi and joystick is selected, the autopilot will start in the mode AP_MODE_ATTITUDE_DIRECT 4 (but engines are off)
When pressing circle on the dual shock 4 controller you activate the manual control mode AP_MODE_ATTITUDE_Z_HOLD 9
When pressing x on the dual shock 4 controller you activate the manual control mode AP_MODE_ATTITUDE_DIRECT 4

Author: Ralph Rudi schmidt ralph.nosp@m..r.s.nosp@m.chmid.nosp@m.t@ou.nosp@m.tlook.nosp@m..com An integration of the WegdeBug algorithm (Laubach 1999) for path finding, for drones with stereo vision.

Definition in file wedgebug.h.

Data Structure Documentation

◆ kernel_C1

struct kernel_C1

Definition at line 42 of file wedgebug.h.

Data Fields
uint32_t	buf_size	Size of values of weight buffer and values buffer.
void *	buf_values	Kernel value buffer. These are the values underneath the kernel.
void *	buf_weights	Kernel weight buffer.
uint16_t	h	Kernel height.
enum image_type	type
uint16_t	w

Function Documentation

◆ getMedian()

uint8_t getMedian	(	uint8_t *	a,
		uint32_t	n
	)

extern

Definition at line 469 of file wedgebug.c.

References foo.

Referenced by median_disparity_to_point().

Here is the caller graph for this function:

◆ kernel_create()

void kernel_create	(	struct kernel_C1 *	kernel,
		uint16_t	width,
		uint16_t	height,
		enum image_type	type
	)

Definition at line 427 of file wedgebug.c.

References foo, IMAGE_GRADIENT, IMAGE_INT16, IMAGE_JPEG, IMAGE_YUV422, and image_t::type.

Referenced by wedgebug_init().

Here is the caller graph for this function:

◆ kernel_free()

void kernel_free ( struct kernel_C1 * kernel )

extern

Definition at line 455 of file wedgebug.c.

References foo.

◆ post_disparity_crop_rect()

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

Definition at line 401 of file wedgebug.c.

References foo.

Referenced by SBM_OCV(), and wedgebug_init().

Here is the caller graph for this function:

◆ set_state()

void set_state	(	uint8_t	_state,
		uint8_t	change_allowed
	)

extern

Definition at line 420 of file wedgebug.c.

References current_state, and foo.

Referenced by wedgebug_periodic().

Here is the caller graph for this function:

◆ wedgebug_init()

void wedgebug_init ( void )

extern

Below this median depth (cm), an obstacle is considered to block the way (i.e. the blocking obstacle needs to be close)

Below this depth (cm) edges are eligible for the WedgeBug algorithm

Definition at line 1584 of file wedgebug.c.

References allow_state_change_EDGE_SCAN, allow_state_change_MOVE_TO_EDGE, allow_state_change_MOVE_TO_GOAL, allow_state_change_POSITION_EDGE, allow_state_change_POSITION_GOAL, allow_state_change_WEDGEBUG_START, block_size_disparities, kernel_C1::buf_size, c_img, c_img_cropped, clock_background_processes, clock_FSM, clock_total_time, clock_total_time_current, clock_total_time_previous, copy_left_img_func(), copy_right_img_func(), cv_add_to_device(), distance_robot_edge_goal, distance_traveled, edge_found_macro_confidence, edge_search_area, foo, free_path_confidence, image_t::h, img_size_t::h, crop_t::h, kernel_C1::h, heading_confidence, heat_map_type, image_create(), IMAGE_GRAYSCALE, IMAGE_INT16, IMAGE_YUV422, img_cropped_dims, img_cropped_info, img_depth_int16_cropped, img_dims, img_disparity_int16_cropped, img_disparity_int8_cropped, img_edges_int8_cropped, img_left, img_left_int8, img_left_int8_cropped, img_middle_int16_cropped, img_middle_int8_cropped, img_right, img_right_int8, initial_heading, ES_angles::initiated, is_edge_found_macro_flag, is_edge_found_micro_flag, is_heading_reached_flag, ES_angles::is_left_reached_flag, is_mode_changed_flag, is_no_edge_found_flag, is_obstacle_detected_flag, is_path_free_flag, ES_angles::is_right_reached_flag, is_setpoint_reached_flag, is_start_reached_flag, is_state_changed_flag, is_total_timer_on_flag, K_median_h, K_median_w, kernel_create(), kernel_median_dims, FloatRMat::m, max_edge_found_macro_confidence, max_edge_found_micro_confidence, max_free_path_confidence, max_heading_confidence, max_no_edge_found_confidence, max_obstacle_confidence, max_position_confidence, median_kernel, median_kernel16bit, N_disparities, no_edge_found_confidence, number_of_states, obstacle_confidence, position_confidence, post_disparity_crop_rect(), previous_mode, previous_state, principal_points(), Rcr, Rwnedwenu, save_images_flag, SE_closing_OCV, SE_dilation_OCV_1, SE_dilation_OCV_2, SE_erosion_OCV, SE_opening_OCV, SE_sobel_OCV, threshold_depth_of_edges, threshold_distance_to_angle, threshold_distance_to_goal, threshold_distance_to_goal_direct, threshold_distance_to_goal_manual, threshold_edge_magnitude, threshold_median_depth, kernel_C1::type, Va_to_Vb(), VCr, VDISTANCEPOSITIONwned, VGOALwenu, VGOALwned, VNEDwenu, VSTARTwenu, VSTARTwned, image_t::w, img_size_t::w, crop_t::w, kernel_C1::w, WaypointAlt, WaypointX, WaypointY, WEDGEBUG_CAMERA_LEFT_FPS, WEDGEBUG_CAMERA_RIGHT_FPS, FloatVect3::x, point_t::x, crop_t::x, FloatVect3::y, point_t::y, crop_t::y, and FloatVect3::z.

Here is the call graph for this function:

◆ wedgebug_periodic()

void wedgebug_periodic ( void )

extern

Definition at line 1837 of file wedgebug.c.

References allow_state_change_EDGE_SCAN, allow_state_change_MOVE_TO_EDGE, allow_state_change_MOVE_TO_GOAL, allow_state_change_POSITION_EDGE, allow_state_change_WEDGEBUG_START, AP_MODE_ATTITUDE_DIRECT, AP_MODE_ATTITUDE_Z_HOLD, AP_MODE_GUIDED, AP_MODE_NAV, AUTONOMOUS_GUIDED, AUTONOMOUS_NAV, autopilot_get_mode(), autopilot_guided_goto_ned(), background_processes_16bit(), c_img_cropped, clock_background_processes, clock_FSM, clock_total_time, clock_total_time_current, clock_total_time_previous, counter_cycles, counter_state, current_mode, current_state, DIRECT_CONTROL, distance_robot_edge_goal, distance_traveled, edge_found_macro_confidence, EDGE_SCAN, edge_search_area, find_best_edge_coordinates2(), FLOAT_ANGLE_NORMALIZE, float_vect3_norm_two_points(), float_vect_copy(), foo, free_path_confidence, guidance_h_hover_enter(), guidance_h_set_heading(), guidance_h_set_pos(), guidance_v_set_z(), heading, heading_confidence, ES_angles::heading_initial, ES_angles::heading_max_left, ES_angles::heading_max_right, heading_towards_setpoint_WNED(), heading_towards_waypoint(), heat_map_type, img_depth_int16_cropped, img_disparity_int8_cropped, img_edges_int8_cropped, img_left_int8, img_middle_int8_cropped, img_right_int8, initial_heading, ES_angles::initiated, is_edge_found_macro_flag, is_edge_found_micro_flag, is_heading_reached(), is_heading_reached_flag, ES_angles::is_left_reached_flag, is_mode_changed_flag, is_no_edge_found_flag, is_obstacle_detected_flag, is_path_free_flag, ES_angles::is_right_reached_flag, is_setpoint_reached(), is_setpoint_reached_flag, is_start_reached_flag, is_state_changed_flag, FloatRMat::m, max_edge_found_macro_confidence, max_edge_found_micro_confidence, max_edge_search_angle, max_free_path_confidence, max_heading_confidence, max_no_edge_found_confidence, max_obstacle_confidence, max_position_confidence, median_depth_in_front, median_depth_to_point(), median_kernel16bit, MOVE_TO_EDGE, MOVE_TO_GOAL, no_edge_found_confidence, number_of_states, obstacle_confidence, position_confidence, POSITION_EDGE, POSITION_GOAL, previous_mode, previous_state, FloatEulers::psi, Rcr, Rrwned, Rwnedwenu, save_image_gray(), save_image_HM(), save_images_flag, set_state(), stateGetNedToBodyEulers_f(), stateGetNedToBodyRMat_f(), stateGetPositionNed_f(), threshold_depth_of_edges, threshold_distance_to_angle, threshold_distance_to_goal, threshold_distance_to_goal_direct, threshold_median_depth, time, time_state, Va_to_Vb(), Vb_to_Va(), VCr, VDISTANCEPOSITIONwned, VEDGECOORDINATESc, VEDGECOORDINATESr, VEDGECOORDINATESwenu, VEDGECOORDINATESwned, VGOALc, VGOALr, VGOALwned, VHOLDINGPOINTwned, VNEDwenu, VPBESTEDGECOORDINATESwned, VRwned, WEDGEBUG_START, FloatVect3::x, NedCoor_f::x, FloatVect3::y, NedCoor_f::y, FloatVect3::z, and NedCoor_f::z.

Here is the call graph for this function:

Variable Documentation

◆ block_size_disparities

int block_size_disparities

extern

Number of disparity levels (0-this number)

Definition at line 256 of file wedgebug.c.

Referenced by background_processes(), background_processes_16bit(), and wedgebug_init().

◆ heat_map_type

int heat_map_type

extern

Variable to hold distance from robot to edge to goal (used in EDGE_SCAN (9) state)

Definition at line 266 of file wedgebug.c.

Referenced by background_processes(), background_processes_16bit(), wedgebug_init(), and wedgebug_periodic().

◆ K_median_h

uint16_t K_median_h

extern

Width of kernel for the median kernel.

Definition at line 113 of file wedgebug.c.

Referenced by wedgebug_init().

◆ K_median_w

uint16_t K_median_w

extern

SE size for the Sobel operation, to detect edges.

Definition at line 112 of file wedgebug.c.

Referenced by wedgebug_init().

◆ max_disparity

int max_disparity

extern

◆ max_edge_found_macro_confidence

int16_t max_edge_found_macro_confidence

extern

This is the max confidence that edges (micro-see above) were found.

Definition at line 166 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

◆ max_edge_found_micro_confidence

int16_t max_edge_found_micro_confidence

extern

This is the max confidence that a specific heading was reached.

Definition at line 165 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

◆ max_free_path_confidence

int16_t max_free_path_confidence

extern

This is the max confidence that an obstacle was spotted.

Definition at line 162 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

◆ max_heading_confidence

int16_t max_heading_confidence

extern

This is the max confidence that a specific position was reached.

Definition at line 164 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

◆ max_no_edge_found_confidence

int16_t max_no_edge_found_confidence

extern

This is the max confidence that edges (macro-see above were found.

Definition at line 167 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

◆ max_obstacle_confidence

int16_t max_obstacle_confidence

extern

This is the confidence that no edge was found.

Definition at line 161 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

◆ max_position_confidence

int16_t max_position_confidence

extern

This is the max confidence that an obstacle was not spotted.

Definition at line 163 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

◆ min_disparity

int min_disparity

extern

Block size used for the block matching (SBM) function.

Definition at line 257 of file wedgebug.c.

Referenced by CN_escape_velocity(), CN_potential_heading(), CN_potential_velocity(), CN_vector_escape_velocity(), and CN_vector_velocity().

◆ N_disparities

int N_disparities

extern

Variable that saves previous mode to control the drone, for some memory.

Definition at line 255 of file wedgebug.c.

Referenced by background_processes(), background_processes_16bit(), and wedgebug_init().

◆ save_images_flag

uint8_t save_images_flag

extern

Set to 1 if control mode of drone is changed, 0 otherwise.

Definition at line 180 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

◆ SE_closing_OCV

int SE_closing_OCV

extern

SE size for the opening operation.

Definition at line 107 of file wedgebug.c.

Referenced by background_processes(), background_processes_16bit(), and wedgebug_init().

◆ SE_dilation_OCV_1

int SE_dilation_OCV_1

extern

SE size for the closing operation.

Definition at line 108 of file wedgebug.c.

Referenced by background_processes(), background_processes_16bit(), and wedgebug_init().

◆ SE_dilation_OCV_2

int SE_dilation_OCV_2

extern

SE size for the first dilation operation.

Definition at line 109 of file wedgebug.c.

Referenced by wedgebug_init().

◆ SE_erosion_OCV

int SE_erosion_OCV

extern

SE size for the second dilation operation (see state 3 "WEDGEBUG_START" and state 6 "POSITION_EDGE" )

Definition at line 110 of file wedgebug.c.

Referenced by background_processes_16bit(), and wedgebug_init().

◆ SE_opening_OCV

int SE_opening_OCV

extern

Definition at line 106 of file wedgebug.c.

Referenced by background_processes(), background_processes_16bit(), and wedgebug_init().

◆ threshold_depth_of_edges

uint16_t threshold_depth_of_edges

extern

Below this median depth (cm), an obstacle is considered to block the way (i.e. the blocking obstacle needs to be close)

Definition at line 150 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

◆ threshold_distance_to_angle

float threshold_distance_to_angle

extern

Below this distance (in meters) it is considered that the robot has reached the goal.

Definition at line 146 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

◆ threshold_distance_to_goal

float threshold_distance_to_goal

extern

Above this disparity edges are eligible for WedgeBug algorithm (i.e. edges cannot be very far away)

Definition at line 145 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

◆ threshold_edge_magnitude

int threshold_edge_magnitude

extern

Below this depth (cm) edges are eligible for the WedgeBug algorith.

Definition at line 142 of file wedgebug.c.

Referenced by background_processes(), background_processes_16bit(), and wedgebug_init().

◆ threshold_median_depth

uint16_t threshold_median_depth

extern

Below this distance (in meters) it is considered that the robot has reached the goal, in DIRECT_CONTROL mode.

Definition at line 149 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

Data Structures

Functions

Variables

Detailed Description

Data Structure Documentation

◆ kernel_C1

Function Documentation

◆ getMedian()

◆ kernel_create()

◆ kernel_free()

◆ post_disparity_crop_rect()

◆ set_state()

◆ wedgebug_init()

◆ wedgebug_periodic()

Variable Documentation

◆ block_size_disparities

◆ heat_map_type

◆ K_median_h

◆ K_median_w

◆ max_disparity

◆ max_edge_found_macro_confidence

◆ max_edge_found_micro_confidence

◆ max_free_path_confidence

◆ max_heading_confidence

◆ max_no_edge_found_confidence

◆ max_obstacle_confidence

◆ max_position_confidence

◆ min_disparity

◆ N_disparities

◆ save_images_flag

◆ SE_closing_OCV

◆ SE_dilation_OCV_1

◆ SE_dilation_OCV_2

◆ SE_erosion_OCV

◆ SE_opening_OCV

◆ threshold_depth_of_edges

◆ threshold_distance_to_angle

◆ threshold_distance_to_goal

◆ threshold_edge_magnitude

◆ threshold_median_depth