void carmen_minimize_gridmap(carmen_map_t *map, int *x_offset, int *y_offset) { int x, y; int min_x = map->config.x_size, min_y = map->config.y_size; int max_x = 0, max_y = 0; int new_width, new_height; double **new_map; double *new_complete_map; for (x = 0; x < map->config.x_size; x++) for (y = 0; y < map->config.y_size; y++) { if (map->map[x][y] != -1) { min_x = carmen_fmin(min_x, x); min_y = carmen_fmin(min_y, y); max_x = carmen_fmax(max_x, x); max_y = carmen_fmax(max_y, y); } } new_width = max_x - min_x + 1; new_height = max_y - min_y + 1; new_complete_map = (double *)calloc(new_width*new_height, sizeof(double)); carmen_test_alloc(new_complete_map); new_map = (double **)calloc(new_width, sizeof(double)); carmen_test_alloc(new_map); for (x = 0; x < new_width; x++) { new_map[x] = new_complete_map+x*new_height; for (y = 0; y < new_height; y++) memcpy(new_map[x], map->map[x+min_x]+min_y, new_height*sizeof(double)); } free(map->map); map->map = new_map; free(map->complete_map); map->complete_map = new_complete_map; map->config.x_size = new_width; map->config.y_size = new_height; *x_offset = min_x; *y_offset = min_y; }
double SpeedControlLogic(carmen_ackerman_path_point_t pose, carmen_simulator_ackerman_config_t *simulator_config) { double v_scl = simulator_config->max_v; //todo transformar esses valores em parametros no ini double a_scl = 0.1681; double b_scl = -0.0049; double safety_factor = 1; double kt = carmen_get_curvature_from_phi(pose.phi, pose.v, simulator_config->understeer_coeficient, simulator_config->distance_between_front_and_rear_axles); double v_cmd = fabs(simulator_config->target_v); double v_cmd_max = carmen_fmax(v_scl, (kt + simulator_config->delta_t - a_scl) / b_scl); double k_max_scl = carmen_fmin(simulator_config->maximum_steering_command_curvature, a_scl + b_scl * v_cmd); double kt_dt = carmen_get_curvature_from_phi(simulator_config->target_phi, pose.v, simulator_config->understeer_coeficient, simulator_config->distance_between_front_and_rear_axles); if (fabs(kt_dt) >= k_max_scl) v_cmd = fabs(safety_factor * v_cmd_max); if(pose.v != 0) return v_cmd * (pose.v / fabs(pose.v)); else return 0; }
unsigned char *carmen_graphics_convert_to_image(carmen_map_p map, int flags) { register float *data_ptr; unsigned char *image_data = NULL; register unsigned char *image_ptr = NULL; double value; int x_size, y_size; int x_index, y_index; int index; double max_val = -MAXDOUBLE, min_val = MAXDOUBLE; int rescale = flags & CARMEN_GRAPHICS_RESCALE; int invert = flags & CARMEN_GRAPHICS_INVERT; int rotate = flags & CARMEN_GRAPHICS_ROTATE; int black_and_white = flags & CARMEN_GRAPHICS_BLACK_AND_WHITE; if (map == NULL) { carmen_warn("carmen_graphics_convert_to_image was passed NULL map.\n"); return NULL; } x_size = map->config.x_size; y_size = map->config.y_size; image_data = (unsigned char *)calloc(x_size*y_size*3, sizeof(unsigned char)); carmen_test_alloc(image_data); if (rescale) { max_val = -MAXDOUBLE; min_val = MAXDOUBLE; data_ptr = map->complete_map; for (index = 0; index < map->config.x_size*map->config.y_size; index++) { max_val = carmen_fmax(max_val, *data_ptr); if (*data_ptr >= 0) min_val = carmen_fmin(min_val, *data_ptr); data_ptr++; } } if (max_val < 0) rescale = 0; image_ptr = image_data; data_ptr = map->complete_map; for (x_index = 0; x_index < x_size; x_index++) { for (y_index = 0; y_index < y_size; y_index++) { value = *(data_ptr++); if (rotate) image_ptr = image_data+y_index*x_size*3+x_index; if (value < 0 && value > -1.5) { if (black_and_white) { *(image_ptr++) = 255; *(image_ptr++) = 255; *(image_ptr++) = 255; } else { *(image_ptr++) = 0; *(image_ptr++) = 0; *(image_ptr++) = 255; } } else if (value < -1.5) { // for offlimits if (black_and_white) { *(image_ptr++) = 205; *(image_ptr++) = 205; *(image_ptr++) = 205; } else { *(image_ptr++) = 255; *(image_ptr++) = 0; *(image_ptr++) = 0; } } else if(!rescale && value > 1.0) { if (black_and_white) { *(image_ptr++) = 128; *(image_ptr++) = 128; *(image_ptr++) = 128; } else { *(image_ptr++) = 255; *(image_ptr++) = 0; *(image_ptr++) = 0; } } else { if (rescale) value = (value - min_val) / (max_val - min_val); if (!invert) value = 1 - value; for (index = 0; index < 3; index++) *(image_ptr++) = value * 255; } } } return image_data; }