Exemple #1
0
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;
}
Exemple #3
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;
}