rover_obstacles.c File Reference

functions to create a grid map More...

#include "rover_obstacles.h"
#include "math/pprz_geodetic_float.h"
#include "modules/datalink/telemetry.h"
#include "state.h"
#include "firmwares/rover/navigation.h"
#include "modules/core/abi.h"

Include dependency graph for rover_obstacles.c:

Go to the source code of this file.

Macros
#define	P_FREE   0.4
#define	P_OCC   0.7
#define	L_MIN   -127
#define	L_MAX   127
#define	L0   0
#define	P_T   0.95
#define	SCALE   20.0f
#define	OBSTACLES_RECEIVE_ID   ABI_BROADCAST
#define	DECAY_INTERVAL   5000
#define	DECAY   5

Functions
static void	lidar_cb (uint8_t sender_id, uint32_t stamp, float distance, float angle)
static void	send_obstacle_grid (struct transport_tx *trans, struct link_device *dev)
static void	send_grid_init (struct transport_tx *trans, struct link_device *dev)
void	init_grid (uint8_t pa, uint8_t pb)
void	init_grid_4 (uint8_t wp1, uint8_t wp2, uint8_t wp3, uint8_t wp4)
void	obtain_cell_xy (float px, float py, int *cell_x, int *cell_y)
void	fill_cell (float px, float py)
void	fill_bayesian_cell (float px, float py)
void	fill_free_cells (float lidar, float angle)
void	decay_map ()
void	update_line_bayes (int x0, int y0, int x1, int y1)
void	update_cell (int x, int y, int new_value)
void	compute_cell_bayes (int x, int y, bool is_occupied)
void	check_probs (void)
void	ins_update_lidar (float distance, float angle)

Variables
static uint32_t	last_s = 0
static abi_event	lidar_ev
float	max_distance = 5
static float	POCC = 0
static float	PFREE = 0
static float	PT = 0
static int8_t	LT
static int8_t	LOCC
static int8_t	LFREE
world_grid	obstacle_grid
uint8_t	grid_block_size = GRID_BLOCK_SIZE

Detailed Description

functions to create a grid map

Definition in file rover_obstacles.c.

Macro Definition Documentation

DECAY

#define DECAY   5

Definition at line 58 of file rover_obstacles.c.

DECAY_INTERVAL

#define DECAY_INTERVAL   5000

Definition at line 57 of file rover_obstacles.c.

L0

#define L0   0

Definition at line 45 of file rover_obstacles.c.

L_MAX

#define L_MAX   127

Definition at line 44 of file rover_obstacles.c.

L_MIN

#define L_MIN   -127

Definition at line 43 of file rover_obstacles.c.

OBSTACLES_RECEIVE_ID

#define OBSTACLES_RECEIVE_ID   ABI_BROADCAST

Definition at line 53 of file rover_obstacles.c.

P_FREE

#define P_FREE   0.4

Definition at line 41 of file rover_obstacles.c.

P_OCC

#define P_OCC   0.7

Definition at line 42 of file rover_obstacles.c.

P_T

#define P_T   0.95

Definition at line 46 of file rover_obstacles.c.

SCALE

#define SCALE   20.0f

Definition at line 48 of file rover_obstacles.c.

Function Documentation

check_probs()

void check_probs

(

void

)

Definition at line 344 of file rover_obstacles.c.

References foo, bayesian_map::free, LFREE, LOCC, LT, world_grid::map, obstacle_grid, bayesian_map::occ, PFREE, POCC, PT, SCALE, and bayesian_map::threshold.

Referenced by compute_cell_bayes().

Here is the caller graph for this function:

compute_cell_bayes()

void compute_cell_bayes	(	int	x,
		int	y,
		bool	is_occupied
	)

Definition at line 325 of file rover_obstacles.c.

References check_probs(), foo, L_MAX, L_MIN, LFREE, LOCC, N_COL_GRID, N_ROW_GRID, obstacle_grid, update_cell(), world_grid::world, EnuCoor_f::x, and EnuCoor_f::y.

Referenced by fill_bayesian_cell(), fill_free_cells(), and update_line_bayes().

Here is the call graph for this function:

Here is the caller graph for this function:

decay_map()

void decay_map

(

void

)

Definition at line 256 of file rover_obstacles.c.

References bayesian_map::decay, DECAY_INTERVAL, foo, get_sys_time_msec(), last_s, world_grid::map, N_COL_GRID, N_ROW_GRID, obstacle_grid, update_cell(), world_grid::world, EnuCoor_f::x, and EnuCoor_f::y.

Here is the call graph for this function:

fill_bayesian_cell()

void fill_bayesian_cell	(	float	px,
		float	py
	)

Definition at line 203 of file rover_obstacles.c.

References compute_cell_bayes(), foo, world_grid::is_ready, obstacle_grid, obtain_cell_xy(), rover_pos, stateGetPositionEnu_f(), update_line_bayes(), NedCoor_d::x, and NedCoor_d::y.

Referenced by ins_update_lidar().

Here is the call graph for this function:

Here is the caller graph for this function:

fill_cell()

void fill_cell	(	float	px,
		float	py
	)

Definition at line 194 of file rover_obstacles.c.

References obstacle_grid, obtain_cell_xy(), and world_grid::world.

Here is the call graph for this function:

fill_free_cells()

void fill_free_cells	(	float	lidar,
		float	angle
	)

Definition at line 222 of file rover_obstacles.c.

References compute_cell_bayes(), foo, world_grid::is_ready, max_distance, N_COL_GRID, N_ROW_GRID, obstacle_grid, obtain_cell_xy(), FloatEulers::psi, rover_pos, stateGetNedToBodyEulers_f(), stateGetPositionEnu_f(), update_line_bayes(), NedCoor_d::x, and NedCoor_d::y.

Here is the call graph for this function:

init_grid()

void init_grid	(	uint8_t	pa,
		uint8_t	pb
	)

Definition at line 109 of file rover_obstacles.c.

References DefaultPeriodic, world_grid::dx, world_grid::dy, foo, world_grid::is_ready, L0, N_COL_GRID, N_ROW_GRID, world_grid::now_row, obstacle_grid, register_periodic_telemetry(), send_grid_init(), WaypointX, WaypointY, world_grid::world, world_grid::xmax, world_grid::xmin, world_grid::ymax, and world_grid::ymin.

Here is the call graph for this function:

init_grid_4()

void init_grid_4	(	uint8_t	wp1,
		uint8_t	wp2,
		uint8_t	wp3,
		uint8_t	wp4
	)

Definition at line 136 of file rover_obstacles.c.

References DECAY, bayesian_map::decay, DefaultChannel, DefaultDevice, DefaultPeriodic, world_grid::dx, world_grid::dy, foo, bayesian_map::free, world_grid::is_ready, lidar_cb(), lidar_ev, bayesian_map::LT, world_grid::map, N_COL_GRID, N_ROW_GRID, world_grid::now_row, obstacle_grid, OBSTACLES_RECEIVE_ID, bayesian_map::occ, P_FREE, P_OCC, P_T, register_periodic_telemetry(), send_grid_init(), bayesian_map::threshold, WaypointX, WaypointY, world_grid::world, world_grid::xmax, world_grid::xmin, world_grid::ymax, and world_grid::ymin.

Here is the call graph for this function:

ins_update_lidar()

void ins_update_lidar	(	float	distance,
		float	angle
	)

Definition at line 370 of file rover_obstacles.c.

References convert_walls_to_ltp(), WallSystem::converted_to_ltp, fill_bayesian_cell(), foo, FloatEulers::psi, stateGetNedToBodyEulers_f(), stateGetPositionEnu_f(), stateIsLocalCoordinateValid(), wall_system, FloatVect2::x, EnuCoor_f::x, and EnuCoor_f::y.

Referenced by lidar_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

lidar_cb()

static void lidar_cb ( uint8_t  sender_id, uint32_t  stamp, float  distance, float  angle  )

static

Definition at line 407 of file rover_obstacles.c.

References ins_update_lidar().

Referenced by init_grid_4().

Here is the call graph for this function:

Here is the caller graph for this function:

obtain_cell_xy()

void obtain_cell_xy	(	float	px,
		float	py,
		int *	cell_x,
		int *	cell_y
	)

Definition at line 182 of file rover_obstacles.c.

References world_grid::dx, world_grid::dy, foo, obstacle_grid, world_grid::xmin, and world_grid::ymin.

Referenced by fill_bayesian_cell(), fill_cell(), and fill_free_cells().

Here is the caller graph for this function:

send_grid_init()

static void send_grid_init ( struct transport_tx *  trans, struct link_device *  dev  )

static

Definition at line 95 of file rover_obstacles.c.

References dev, world_grid::dx, world_grid::dy, foo, bayesian_map::LT, world_grid::map, obstacle_grid, world_grid::xmax, world_grid::xmin, world_grid::ymax, and world_grid::ymin.

Referenced by init_grid(), and init_grid_4().

Here is the caller graph for this function:

send_obstacle_grid()

static void send_obstacle_grid ( struct transport_tx *  trans, struct link_device *  dev  )

static

Definition at line 80 of file rover_obstacles.c.

References dev, world_grid::dx, world_grid::dy, foo, N_COL_GRID, N_ROW_GRID, world_grid::now_row, obstacle_grid, world_grid::world, world_grid::xmax, world_grid::xmin, world_grid::ymax, and world_grid::ymin.

update_cell()

void update_cell	(	int	x,
		int	y,
		int	new_value
	)

Definition at line 306 of file rover_obstacles.c.

References DefaultChannel, DefaultDevice, foo, LT, bayesian_map::LT, world_grid::map, obstacle_grid, world_grid::world, EnuCoor_f::x, and EnuCoor_f::y.

Referenced by compute_cell_bayes(), and decay_map().

Here is the caller graph for this function:

update_line_bayes()

void update_line_bayes	(	int	x0,
		int	y0,
		int	x1,
		int	y1
	)

Definition at line 289 of file rover_obstacles.c.

References compute_cell_bayes(), and foo.

Referenced by fill_bayesian_cell(), and fill_free_cells().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

grid_block_size

uint8_t grid_block_size = GRID_BLOCK_SIZE

Definition at line 76 of file rover_obstacles.c.

last_s

uint32_t last_s = 0

static

Definition at line 59 of file rover_obstacles.c.

Referenced by decay_map().

LFREE

int8_t LFREE

static

Definition at line 73 of file rover_obstacles.c.

Referenced by check_probs(), and compute_cell_bayes().

lidar_ev

abi_event lidar_ev

static

Definition at line 63 of file rover_obstacles.c.

Referenced by init_grid_4().

LOCC

int8_t LOCC

static

Definition at line 73 of file rover_obstacles.c.

Referenced by check_probs(), and compute_cell_bayes().

LT

int8_t LT

static

Definition at line 73 of file rover_obstacles.c.

Referenced by check_probs(), and update_cell().

max_distance

float max_distance = 5

Definition at line 65 of file rover_obstacles.c.

Referenced by fill_free_cells().

obstacle_grid

world_grid obstacle_grid

Definition at line 75 of file rover_obstacles.c.

Referenced by check_probs(), compute_cell_bayes(), decay_map(), fill_bayesian_cell(), fill_cell(), fill_free_cells(), init_grid(), init_grid_4(), obtain_cell_xy(), send_grid_init(), send_obstacle_grid(), and update_cell().

PFREE

float PFREE = 0

static

Definition at line 70 of file rover_obstacles.c.

Referenced by check_probs().

POCC

float POCC = 0

static

Definition at line 69 of file rover_obstacles.c.

Referenced by check_probs().

PT

float PT = 0

static

Definition at line 71 of file rover_obstacles.c.

Referenced by check_probs().