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(¤t_mean_remission_map, map_config.x_size, map_config.y_size, map_config.resolution); carmen_grid_mapping_create_new_map(¤t_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); }
/* 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); }