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; }
static int next_waypoint_astar(carmen_ackerman_traj_point_p waypoint) { if (path.path == NULL) return -1; // int i; // carmen_ackerman_traj_point_t pose = *waypoint; // double min_pose_dist = carmen_distance_ackerman_traj(waypoint, &path.path[0]); // double pose_dist = 0; // double temp; // for (i = 1; i <= 30; i++) // { // pose_dist = carmen_distance_ackerman_traj(&pose, &path.path[0]); // if (pose_dist < 0.1 || pose_dist > min_pose_dist) // { // path.path[0].v = (path.path[0].v / fabs(path.path[0].v)) * (i / 20.0); // break; // } // printf("pose_dist %f x %f y %f\n",pose_dist, pose.x, pose.y); // min_pose_dist = pose_dist; // pose = predict_new_robot_position(*waypoint, 1, path.path[0].phi, (i / 20.0), &carmen_robot_ackerman_config); // } // printf("v %f d_p %f i %d %f\n",path.path[0].v, carmen_distance_ackerman_traj(waypoint, &path.path[0]), i, temp); double delta_theta = fabs(carmen_normalize_theta(waypoint->theta - path.path[0].theta)); replan = 0; if (carmen_distance_ackerman_traj(waypoint, &path.path[0]) <= 0.3 && delta_theta < carmen_degrees_to_radians(1.5)) { delete_path_point(0); min_delta_d = INTMAX_MAX; if (path.path_size <= 1) { replan = 1; return 1; } } if (min_delta_d != -1 && carmen_distance_ackerman_traj(waypoint, &path.path[0]) - min_delta_d > 0.5) { replan = 1; return -1; } min_delta_d = carmen_fmin(carmen_distance_ackerman_traj(waypoint, &path.path[0]), min_delta_d); 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; }