Esempi in C++ (Cpp) per carmen_grid_mapping_init_parameters

Esempio n. 1

File: localize_ackerman_using_map.cpp Progetto: LCAD-UFES/carmen_lcad

void
localalize_using_map_initialize(carmen_map_config_t *main_map_config)
{
	map_config = *main_map_config;
	
	carmen_grid_mapping_create_new_map(&sum_remission_map, map_config.x_size, map_config.y_size, map_config.resolution);
	carmen_grid_mapping_create_new_map(&sum_sqr_remission_map, map_config.x_size, map_config.y_size, map_config.resolution);
	carmen_grid_mapping_create_new_map(&count_remission_map, map_config.x_size, map_config.y_size, map_config.resolution);
	carmen_grid_mapping_create_new_map(&current_mean_remission_map, map_config.x_size, map_config.y_size, map_config.resolution);
	carmen_grid_mapping_create_new_map(&current_variance_remission_map, map_config.x_size, map_config.y_size, map_config.resolution);

	carmen_grid_mapping_init_parameters(map_config.resolution, map_config.x_size * map_config.resolution);

	memset(&robot_pose, 0, sizeof(carmen_pose_3D_t));
	carmen_point_t pose;

	pose.x = robot_pose.position.x = (main_map_config->x_size * main_map_config->resolution) / 2.0;
	pose.y = robot_pose.position.y = (main_map_config->y_size * main_map_config->resolution) / 2.0;


	carmen_grid_mapping_get_map_origin(&pose, &g_map_origin.x, &g_map_origin.y);

}

Esempio n. 2

File: grid_mapping_main.cpp Progetto: ulyssesrr/carmen_lcad

/* read all parameters from .ini file and command line. */
static void
read_ultrasonic_sensor_parameters(int argc, char **argv,
		BeanRangeFinderMeasurementModelParams *p_laser_params,
		ProbabilisticMapParams *p_map_params)
{
	char camera_string[256];

	sprintf(camera_string, "%s%d", "camera", atoi(argv[1]));

	carmen_param_t param_list[] = 
	{
			{(char *)"grid_mapping", (char *)"ultrasonic_sensor_sampling_step", CARMEN_PARAM_INT, &p_laser_params->sampling_step, 0, NULL},
			{(char *)"grid_mapping", (char *)"ultrasonic_sensor_num_beams", CARMEN_PARAM_INT, &p_laser_params->laser_beams, 0, NULL},
			{(char *)"grid_mapping", (char *)"ultrasonic_sensor_fov_range", CARMEN_PARAM_DOUBLE, &p_laser_params->fov_range, 0, NULL},
			{(char *)"grid_mapping", (char *)"ultrasonic_sensor_max_range", CARMEN_PARAM_DOUBLE, &p_laser_params->max_range, 0, NULL},
			{(char *)"grid_mapping", (char *)"ultrasonic_sensor_lambda_short", CARMEN_PARAM_DOUBLE, &p_laser_params->lambda_short, 0, NULL},
			{(char *)"grid_mapping", (char *)"ultrasonic_sensor_sigma_zhit", CARMEN_PARAM_DOUBLE, &p_laser_params->sigma_zhit, 0, NULL},
			{(char *)"grid_mapping", (char *)"ultrasonic_sensor_zhit", CARMEN_PARAM_DOUBLE, &p_laser_params->zhit, 0, NULL},
			{(char *)"grid_mapping", (char *)"ultrasonic_sensor_zmax", CARMEN_PARAM_DOUBLE, &p_laser_params->zmax, 0, NULL},
			{(char *)"grid_mapping", (char *)"ultrasonic_sensor_zrand", CARMEN_PARAM_DOUBLE, &p_laser_params->zrand, 0, NULL},
			{(char *)"grid_mapping", (char *)"ultrasonic_sensor_zshort", CARMEN_PARAM_DOUBLE, &p_laser_params->zshort, 0, NULL},

			{(char *)"robot", (char *)"frontlaser_offset", CARMEN_PARAM_DOUBLE, &p_laser_params->front_offset, 0, NULL},
			{(char *)"robot", (char *)"length", CARMEN_PARAM_DOUBLE, 				&robot_length, 0, NULL},
			{(char *)"robot", (char *)"width", CARMEN_PARAM_DOUBLE, 				&robot_width, 0, NULL},
			{(char *)"robot", (char *)"vertical_displacement_from_center", CARMEN_PARAM_DOUBLE, 	&robot_vertical_displacement_from_center, 0, NULL},


			{(char *)"grid_mapping", (char *)"map_locc", CARMEN_PARAM_INT, &p_map_params->locc, 0, NULL},
			{(char *)"grid_mapping", (char *)"map_lfree", CARMEN_PARAM_INT, &p_map_params->lfree, 0, NULL},
			{(char *)"grid_mapping", (char *)"map_l0", CARMEN_PARAM_INT, &p_map_params->l0, 0, NULL},
			{(char *)"grid_mapping", (char *)"map_log_odds_max", CARMEN_PARAM_INT, &p_map_params->log_odds_max, 0, NULL},
			{(char *)"grid_mapping", (char *)"map_log_odds_min", CARMEN_PARAM_INT, &p_map_params->log_odds_min, 0, NULL},
			{(char *)"grid_mapping", (char *)"map_log_odds_bias", CARMEN_PARAM_INT, &p_map_params->log_odds_bias, 0, NULL},
			{(char *)"grid_mapping", (char *)"map_grid_res", CARMEN_PARAM_DOUBLE, &p_map_params->grid_res, 0, NULL},
			{(char *)"grid_mapping", (char *)"map_range_factor", CARMEN_PARAM_DOUBLE, &p_map_params->range_factor, 0, NULL},
			{(char *)"grid_mapping", (char *)"map_width", CARMEN_PARAM_DOUBLE, &p_map_params->width, 0, NULL},
			{(char *)"grid_mapping", (char *)"map_height", CARMEN_PARAM_DOUBLE, &p_map_params->height, 0, NULL},

			{(char *) "car", 			  (char *) "x", 	CARMEN_PARAM_DOUBLE, &(car_pose_g.position.x), 0, NULL},
			{(char *) "car", 			  (char *) "y", 	CARMEN_PARAM_DOUBLE, &(car_pose_g.position.y), 0, NULL},
			{(char *) "car", 			  (char *) "z", 	CARMEN_PARAM_DOUBLE, &(car_pose_g.position.z), 0, NULL},
			{(char *) "car", 			  (char *) "yaw", 	CARMEN_PARAM_DOUBLE, &(car_pose_g.orientation.yaw), 0, NULL},
			{(char *) "car", 			  (char *) "pitch", CARMEN_PARAM_DOUBLE, &(car_pose_g.orientation.pitch), 0, NULL},
			{(char *) "car", 			  (char *) "roll", 	CARMEN_PARAM_DOUBLE, &(car_pose_g.orientation.roll), 0, NULL},

			{(char *) "sensor_board_1", (char *) "x", 	CARMEN_PARAM_DOUBLE, &(sensor_board_pose_g.position.x), 0, NULL},
			{(char *) "sensor_board_1", (char *) "y", 	CARMEN_PARAM_DOUBLE, &(sensor_board_pose_g.position.y), 0, NULL},
			{(char *) "sensor_board_1", (char *) "z", 	CARMEN_PARAM_DOUBLE, &(sensor_board_pose_g.position.z), 0, NULL},
			{(char *) "sensor_board_1", (char *) "yaw", 	CARMEN_PARAM_DOUBLE, &(sensor_board_pose_g.orientation.yaw), 0, NULL},
			{(char *) "sensor_board_1", (char *) "pitch", CARMEN_PARAM_DOUBLE, &(sensor_board_pose_g.orientation.pitch), 0, NULL},
			{(char *) "sensor_board_1", (char *) "roll", 	CARMEN_PARAM_DOUBLE, &(sensor_board_pose_g.orientation.roll), 0, NULL},

			{(char *) camera_string, 	  (char *) "x", 	CARMEN_PARAM_DOUBLE, &(camera_pose_g.position.x), 0, NULL},
			{(char *) camera_string,    (char *) "y", 	CARMEN_PARAM_DOUBLE, &(camera_pose_g.position.y), 0, NULL},
			{(char *) camera_string,    (char *) "z", 	CARMEN_PARAM_DOUBLE, &(camera_pose_g.position.z), 0, NULL},
			{(char *) camera_string,    (char *) "yaw", 	CARMEN_PARAM_DOUBLE, &(camera_pose_g.orientation.yaw), 0, NULL},
			{(char *) camera_string,    (char *) "pitch", CARMEN_PARAM_DOUBLE, &(camera_pose_g.orientation.pitch), 0, NULL},
			{(char *) camera_string,    (char *) "roll", 	CARMEN_PARAM_DOUBLE, &(camera_pose_g.orientation.roll), 0, NULL},

			{(char *) "ultrasonic_sensor_r1", (char *) "x", 	CARMEN_PARAM_DOUBLE, &(ultrasonic_sensor_r1_g.position.x), 0, NULL},
			{(char *) "ultrasonic_sensor_r1", (char *) "y", 	CARMEN_PARAM_DOUBLE, &(ultrasonic_sensor_r1_g.position.y), 0, NULL},
			{(char *) "ultrasonic_sensor_r1", (char *) "z", 	CARMEN_PARAM_DOUBLE, &(ultrasonic_sensor_r1_g.position.z), 0, NULL},
			{(char *) "ultrasonic_sensor_r1", (char *) "yaw", 	CARMEN_PARAM_DOUBLE, &(ultrasonic_sensor_r1_g.orientation.yaw), 0, NULL},
			{(char *) "ultrasonic_sensor_r1", (char *) "pitch", CARMEN_PARAM_DOUBLE, &(ultrasonic_sensor_r1_g.orientation.pitch), 0, NULL},
			{(char *) "ultrasonic_sensor_r1", (char *) "roll", 	CARMEN_PARAM_DOUBLE, &(ultrasonic_sensor_r1_g.orientation.roll), 0, NULL},

			{(char *) "ultrasonic_sensor_r2", (char *) "x", 	CARMEN_PARAM_DOUBLE, &(ultrasonic_sensor_r2_g.position.x), 0, NULL},
			{(char *) "ultrasonic_sensor_r2", (char *) "y", 	CARMEN_PARAM_DOUBLE, &(ultrasonic_sensor_r2_g.position.y), 0, NULL},
			{(char *) "ultrasonic_sensor_r2", (char *) "z", 	CARMEN_PARAM_DOUBLE, &(ultrasonic_sensor_r2_g.position.z), 0, NULL},
			{(char *) "ultrasonic_sensor_r2", (char *) "yaw", 	CARMEN_PARAM_DOUBLE, &(ultrasonic_sensor_r2_g.orientation.yaw), 0, NULL},
			{(char *) "ultrasonic_sensor_r2", (char *) "pitch", CARMEN_PARAM_DOUBLE, &(ultrasonic_sensor_r2_g.orientation.pitch), 0, NULL},
			{(char *) "ultrasonic_sensor_r2", (char *) "roll", 	CARMEN_PARAM_DOUBLE, &(ultrasonic_sensor_r2_g.orientation.roll), 0, NULL},

			{(char *) "ultrasonic_sensor_l2", (char *) "x", 	CARMEN_PARAM_DOUBLE, &(ultrasonic_sensor_l2_g.position.x), 0, NULL},
			{(char *) "ultrasonic_sensor_l2", (char *) "y", 	CARMEN_PARAM_DOUBLE, &(ultrasonic_sensor_l2_g.position.y), 0, NULL},
			{(char *) "ultrasonic_sensor_l2", (char *) "z", 	CARMEN_PARAM_DOUBLE, &(ultrasonic_sensor_l2_g.position.z), 0, NULL},
			{(char *) "ultrasonic_sensor_l2", (char *) "yaw", 	CARMEN_PARAM_DOUBLE, &(ultrasonic_sensor_l2_g.orientation.yaw), 0, NULL},
			{(char *) "ultrasonic_sensor_l2", (char *) "pitch", CARMEN_PARAM_DOUBLE, &(ultrasonic_sensor_l2_g.orientation.pitch), 0, NULL},
			{(char *) "ultrasonic_sensor_l2", (char *) "roll", 	CARMEN_PARAM_DOUBLE, &(ultrasonic_sensor_l2_g.orientation.roll), 0, NULL},

			{(char *) "ultrasonic_sensor_l1", (char *) "x", 	CARMEN_PARAM_DOUBLE, &(ultrasonic_sensor_l1_g.position.x), 0, NULL},
			{(char *) "ultrasonic_sensor_l1", (char *) "y", 	CARMEN_PARAM_DOUBLE, &(ultrasonic_sensor_l1_g.position.y), 0, NULL},
			{(char *) "ultrasonic_sensor_l1", (char *) "z", 	CARMEN_PARAM_DOUBLE, &(ultrasonic_sensor_l1_g.position.z), 0, NULL},
			{(char *) "ultrasonic_sensor_l1", (char *) "yaw", 	CARMEN_PARAM_DOUBLE, &(ultrasonic_sensor_l1_g.orientation.yaw), 0, NULL},
			{(char *) "ultrasonic_sensor_l1", (char *) "pitch", CARMEN_PARAM_DOUBLE, &(ultrasonic_sensor_l1_g.orientation.pitch), 0, NULL},
			{(char *) "ultrasonic_sensor_l1", (char *) "roll", 	CARMEN_PARAM_DOUBLE, &(ultrasonic_sensor_l1_g.orientation.roll), 0, NULL}


	};

	carmen_param_install_params(argc, argv, param_list, sizeof(param_list) / sizeof(param_list[0]));

	p_laser_params->start_angle = -0.5 * p_laser_params->fov_range;

	p_map_params->range_max = p_laser_params->max_range;
	p_map_params->range_step = p_laser_params->fov_range / (double)(p_laser_params->laser_beams-1);
	p_map_params->range_start = p_laser_params->start_angle;
	p_map_params->num_ranges = p_laser_params->laser_beams;

	p_map_params->grid_sx = round(p_map_params->width / p_map_params->grid_res);
	p_map_params->grid_sy = round(p_map_params->height / p_map_params->grid_res);
	p_map_params->grid_size = p_map_params->grid_sx * p_map_params->grid_sy;

	p_map_params->robot_width = robot_width;
	p_map_params->robot_length = robot_length;
	p_map_params->robot_vertical_displacement_from_center = robot_vertical_displacement_from_center;

	carmen_grid_mapping_init_parameters(p_map_params->grid_res, p_map_params->width);
}