slope_terrain.C File Reference

#include <algorithm>
#include "conf.H"
#include "queue_factory.H"
#include <getopt.h>
#include "cbmesg_logger.H"
#include "proj_util.H"
#include "util.H"
#include "access_dq.H"
#include "rndf_lane_util.H"
#include "segment.H"

Classes
struct	slope_cell_t
struct	local_dtm_point_t
struct	obs_point_t
Defines
#define	SLOPE_TRUE 1
#define	SLOPE_FALSE 0
#define	SLOPE_NO_IDEA 2
Typedefs
typedef grid_t< slope_cell_t >	slope_grid_t
Functions
bool	compare_min_max (int nei_min_pitch, int nei_max_pitch, int nei_min_roll, int nei_max_roll, int cell_min_pitch, int cell_max_pitch, int cell_min_roll, int cell_max_roll, unsigned &max_slope_diff)
bool	check_min_z_diff (double nei_max_z, double nei_min_z, double cell_max_z, double cell_min_z, double min_z)
bool	check_z_diff (double nei_max_z, double nei_min_z, double cell_max_z, double cell_min_z, double diff_z)
unsigned	test_global_obstacle (slope_grid_t::iterator_t &iter)
unsigned	test_rel_slopes (slope_grid_t::iterator_t &iter, slope_grid_t::iterator_t &iter_neighbour, unsigned &max_slope_diff)
unsigned	test_absolute_z (slope_grid_t::iterator_t &iter, slope_grid_t::iterator_t &iter_neighbour)
unsigned	make_decission (unsigned abs_obstacle, unsigned rel_obstacle, unsigned z_obstacle)
unsigned	find_max_roughness (slope_grid_t::iterator_t &iter, slope_grid_t::iterator_t &iter_neighbour, unsigned max_slope_diff)
void	reset_cell (slope_grid_t::iterator_t &iter)
void	obstacle_detection_timing ()
void	refresh_dtm_z (local_dtm_data_t &dtm, slope_grid_t &grid, double tstamp, double center_x, double center_y)
vector< local_dtm_point_t >	find_neighbors (local_dtm_point_t cell_, const local_dtm_data_t &dtm_)
bool	is_blocked_cell (const local_dtm_point_t &cell, const local_dtm_data_t &dtm_)
bool	is_blocked_boundary_cell (const local_dtm_point_t &cell_, const local_dtm_data_t &dtm_)
lane_point_t	dtm_cell_to_utm (local_dtm_point_t cell_, const local_dtm_data_t &dtm_)
local_dtm_point_t	utm_to_cell_dtm (lane_point_t pt_, const local_dtm_data_t &dtm_)
vector< local_dtm_point_t >	find_blocked_boundary_neighbors (const local_dtm_point_t &boundary_cell_, const local_dtm_data_t &dtm_)
void	partition_neighbors_on_sides (vector< local_dtm_point_t > bbn_, segment_t path_, vector< local_dtm_point_t > &left_, vector< local_dtm_point_t > &right_, const local_dtm_data_t &dtm_)
lane_point_t	expand_dtm_path (local_dtm_point_t blocked_cell_, double cell_width_, const local_dtm_data_t &dtm_, segment_t path_)
lane_point_t	walk_dtm (double width_, lane_point_t start_wp_, lane_point_t &end_wp_, nav_data_t bot_pos_, const local_dtm_data_t &dtm_)
vector< lane_point_t >	find_dtm_path (const local_dtm_data_t &dtm_, double width_, nav_data_t bot_pos_, lane_point_t end_wp_)
void	publish_dtm_path (vector< lane_point_t > path_list_, float width_, data_queue_writer_t< lane_data_t > *lane_writer_)
void	fill_dtm (local_dtm_data_t &dtm, slope_grid_t &grid, double tstamp, double center_x, double center_y, float age_counter)
bool	obs_point_less_tstamp (const obs_point_t &s1, const obs_point_t &s2)
bool	scan_gp_less_tstamp (const scan_gp_data_t &s1, const scan_gp_data_t &s2)
void	publish_current_obstacles (slope_grid_t &grid, const vector< obs_point_t > &points, double age_counter, const local_dtm_data_t &dtm_, double tstamp_)
void	process_scan_gp_data (slope_grid_t &grid, scan_gp_data_t const &points, float age_counter)
void	process_scan_analysis_data (slope_grid_t &grid, scan_analysis_data_t const &points, float age_counter)
void	build_terrain_model (unsigned num_sensors, unsigned scan_gp_ids[], unsigned num_scan_analysis_queues, const char *rndf_filename_, bool provide_lane_, bool provide_dtm_)
bool	read_config (char const *config_filename, bool sick_status[])
unsigned	find_num_sensors (bool sick_status[])
void	get_scan_gp_ids (unsigned num_sicks, bool sick_status[], unsigned scan_gp_ids[])
int	main (int argc, char **argv)
Variables
data_logger_t< local_dtm_data_t >	local_dtm_logger
cbmesg_logger_t	msg_logger
data_queue_writer_t < scan_gp_data_t > *	scan_gp_dq
char const *	progname
double	cell_size = .32
unsigned	num_scan_analysis_queues = 1
int	abs_slope_threshold = 75
int	rel_slope_threshold = 55
unsigned	weight = 12
unsigned	triangle_density = 3
double	delta_z = 0.15
double	critical_z = 0.5
double	global_z = 0.10
double	no_obs_z = 0.10
double	base_tstamp = 0
unsigned	num_scan_gp_hits = 0
unsigned	MAX_SENSORS = 10
float	REFRESH_TIME = 2.0
float	grid_refresh_time = 30
unsigned	scan_gp_write_id = 0
float	D2R = 22.0 / ( 180.0 * 7 )
double	gps_z_error_threshold = 3.0
double	gps_garbage_look_ahead_time = 1.0

Define Documentation

#define SLOPE_FALSE 0

Referenced by build_terrain_model(), fill_dtm(), make_decission(), and test_rel_slopes().

#define SLOPE_NO_IDEA 2

Referenced by fill_dtm(), make_decission(), test_absolute_z(), test_global_obstacle(), and test_rel_slopes().

#define SLOPE_TRUE 1

Referenced by fill_dtm(), is_blocked_cell(), publish_current_obstacles(), test_absolute_z(), test_global_obstacle(), test_rel_slopes(), and walk_dtm().

Typedef Documentation

typedef grid_t<slope_cell_t> slope_grid_t

Function Documentation

void build_terrain_model	(	unsigned	num_sensors,
		unsigned	scan_gp_ids[],
		unsigned	num_scan_analysis_queues,
		const char *	rndf_filename_,
		bool	provide_lane_,
		bool	provide_dtm_
	)

bool check_min_z_diff	(	double	nei_max_z,
		double	nei_min_z,
		double	cell_max_z,
		double	cell_min_z,
		double	min_z
	)			`[inline]`

bool check_z_diff	(	double	nei_max_z,
		double	nei_min_z,
		double	cell_max_z,
		double	cell_min_z,
		double	diff_z
	)			`[inline]`

Referenced by test_absolute_z(), and test_rel_slopes().

bool compare_min_max	(	int	nei_min_pitch,
		int	nei_max_pitch,
		int	nei_min_roll,
		int	nei_max_roll,
		int	cell_min_pitch,
		int	cell_max_pitch,
		int	cell_min_roll,
		int	cell_max_roll,
		unsigned &	max_slope_diff
	)

References rel_slope_threshold.

Referenced by test_rel_slopes().

lane_point_t dtm_cell_to_utm	(	local_dtm_point_t	cell_,
		const local_dtm_data_t &	dtm_
	)

References cell_size, local_dtm_point_t::x, _point_t::x, local_dtm_point_t::y, and _point_t::y.

Referenced by expand_dtm_path(), and partition_neighbors_on_sides().

lane_point_t expand_dtm_path	(	local_dtm_point_t	blocked_cell_,
		double	cell_width_,
		const local_dtm_data_t &	dtm_,
		segment_t	path_
	)

References dtm_cell_to_utm(), find_blocked_boundary_neighbors(), partition_neighbors_on_sides(), cajun::segment_t::point_to_the_right(), cajun::segment_t::reverse(), and cajun::segment_t::segment_angle().

Referenced by walk_dtm().

void fill_dtm	(	local_dtm_data_t &	dtm,
		slope_grid_t &	grid,
		double	tstamp,
		double	center_x,
		double	center_y,
		float	age_counter
	)

References cell_size, dx, dy, find_max_roughness(), grid_refresh_time, make_decission(), num_scan_gp_hits, REFRESH_TIME, reset_cell(), SLOPE_FALSE, SLOPE_NO_IDEA, SLOPE_TRUE, test_absolute_z(), test_global_obstacle(), test_rel_slopes(), and triangle_density.

Referenced by build_terrain_model().

vector<local_dtm_point_t> find_blocked_boundary_neighbors	(	const local_dtm_point_t &	boundary_cell_,
		const local_dtm_data_t &	dtm_
	)

References find_neighbors(), and is_blocked_boundary_cell().

Referenced by expand_dtm_path().

vector<lane_point_t> find_dtm_path	(	const local_dtm_data_t &	dtm_,
		double	width_,
		nav_data_t	bot_pos_,
		lane_point_t	end_wp_
	)

References walk_dtm(), _point_t::x, and _point_t::y.

Referenced by build_terrain_model().

unsigned find_max_roughness	(	slope_grid_t::iterator_t &	iter,
		slope_grid_t::iterator_t &	iter_neighbour,
		unsigned	max_slope_diff
	)			`[inline]`

References cell_size, D2R, and triangle_density.

Referenced by fill_dtm().

vector<local_dtm_point_t> find_neighbors	(	local_dtm_point_t	cell_,
		const local_dtm_data_t &	dtm_
	)

References local_dtm_point_t::x, and local_dtm_point_t::y.

Referenced by find_blocked_boundary_neighbors(), and is_blocked_boundary_cell().

unsigned find_num_sensors ( bool sick_status[] )

References MAX_SENSORS.

Referenced by main().

void get_scan_gp_ids	(	unsigned	num_sicks,
		bool	sick_status[],
		unsigned	scan_gp_ids[]
	)

References MAX_SENSORS.

Referenced by main().

bool is_blocked_boundary_cell	(	const local_dtm_point_t &	cell_,
		const local_dtm_data_t &	dtm_
	)

References find_neighbors(), and is_blocked_cell().

Referenced by find_blocked_boundary_neighbors().

bool is_blocked_cell	(	const local_dtm_point_t &	cell,
		const local_dtm_data_t &	dtm_
	)

References SLOPE_TRUE, local_dtm_point_t::x, and local_dtm_point_t::y.

Referenced by is_blocked_boundary_cell().

int main	(	int	argc,
		char **	argv
	)

References build_terrain_model(), find_num_sensors(), get_scan_gp_ids(), local_dtm_logger, MAX_SENSORS, msg_logger, num_scan_gp_hits, progname, read_config(), REFRESH_TIME, and triangle_density.

unsigned make_decission	(	unsigned	abs_obstacle,
		unsigned	rel_obstacle,
		unsigned	z_obstacle
	)			`[inline]`

References SLOPE_FALSE, and SLOPE_NO_IDEA.

Referenced by fill_dtm().

bool obs_point_less_tstamp	(	const obs_point_t &	s1,
		const obs_point_t &	s2
	)			`[inline]`

References obs_point_t::tstamp.

void obstacle_detection_timing ( )

void partition_neighbors_on_sides	(	vector< local_dtm_point_t >	bbn_,
		segment_t	path_,
		vector< local_dtm_point_t > &	left_,
		vector< local_dtm_point_t > &	right_,
		const local_dtm_data_t &	dtm_
	)

References dtm_cell_to_utm(), and cajun::segment_t::point_side().

Referenced by expand_dtm_path().

void process_scan_analysis_data	(	slope_grid_t &	grid,
		scan_analysis_data_t const &	points,
		float	age_counter
	)

References abs_slope_threshold, and cell_size.

Referenced by build_terrain_model().

void process_scan_gp_data	(	slope_grid_t &	grid,
		scan_gp_data_t const &	points,
		float	age_counter
	)

References base_tstamp, cell_size, and REFRESH_TIME.

Referenced by build_terrain_model().

void publish_current_obstacles	(	slope_grid_t &	grid,
		const vector< obs_point_t > &	points,
		double	age_counter,
		const local_dtm_data_t &	dtm_,
		double	tstamp_
	)