static double compute_cost(carmen_roadmap_vertex_t *node, 
			   carmen_roadmap_vertex_t *parent_node,
			   carmen_map_p c_space)
{
  carmen_bresenham_param_t params;
  int x, y;
  double cost;

  carmen_get_bresenham_parameters(node->x, node->y, parent_node->x, 
				  parent_node->y, &params);
  if (node->x < 0 || node->x >= c_space->config.x_size || 
      parent_node->y < 0 || parent_node->y >= c_space->config.y_size)
    return 1e6;

  cost = 0;
  do {
    carmen_get_current_point(&params, &x, &y);
    if (c_space->map[x][y] > 1e5 || c_space->map[x][y] < 0) {
      return 1e6;
      break;
    }
    cost += 1;
  } while (carmen_get_next_point(&params));

  return hypot(node->x-parent_node->x, node->y-parent_node->y);
}
Example #2
0
static int points_are_visible(carmen_traj_point_t *p1, 
			      carmen_traj_point_t *p2,
			      carmen_map_t *c_space)
{
  carmen_bresenham_param_t params;
  int x, y;
  carmen_map_point_t mp1, mp2;

  carmen_trajectory_to_map(p1, &mp1, c_space);
  carmen_trajectory_to_map(p2, &mp2, c_space);

  carmen_get_bresenham_parameters(mp1.x, mp1.y, mp2.x, mp2.y, &params);
  if (mp1.x < 0 || mp1.x >= c_space->config.x_size || 
      mp1.y < 0 || mp1.y >= c_space->config.y_size)
    return 0;

  if (mp2.x < 0 || mp2.x >= c_space->config.x_size || 
      mp2.y < 0 || mp2.y >= c_space->config.y_size)
    return 0;

  do {
    carmen_get_current_point(&params, &x, &y);
    if (c_space->map[x][y] > .01 || c_space->map[x][y] < 0) 
      return 0;
  } while (carmen_get_next_point(&params));

  return 1;
}
int carmen_roadmap_is_visible(carmen_roadmap_vertex_t *node, 
			      carmen_world_point_t *position,
			      carmen_roadmap_t *roadmap)
{
  carmen_bresenham_param_t params;
  int x, y;
  carmen_map_point_t map_pt;
  carmen_map_p c_space;

  c_space = roadmap->c_space;
  carmen_world_to_map(position, &map_pt);

  carmen_get_bresenham_parameters(node->x, node->y, map_pt.x, 
				  map_pt.y, &params);
  if (node->x < 0 || node->x >= c_space->config.x_size || 
      map_pt.y < 0 || map_pt.y >= c_space->config.y_size)
    return 0;

  do {
    carmen_get_current_point(&params, &x, &y);
    if (c_space->map[x][y] > 1e5 || c_space->map[x][y] < 0) 
      return 0;
  } while (carmen_get_next_point(&params));

  return 1;
}
Example #4
0
void 
trace_laser(int x_1, int y_1, int x_2, int y_2, carmen_map_p true_map, 
	    carmen_map_p modify_map) 
{
  carmen_bresenham_param_t params;
  int X, Y;
  double true_map_value;
  double modified_map_value;

  carmen_get_bresenham_parameters(x_1, y_1, x_2, y_2, &params);
  
  do {
    carmen_get_current_point(&params, &X, &Y);
    if (!is_in_map(X, Y, modify_map))
      break;
    true_map_value = true_map->map[X][Y];
    modified_map_value = modify_map->map[X][Y];
    
    if (!is_empty(modified_map_value)  &&   is_empty(true_map_value)) 
      add_clear_point(X, Y, true_map, modify_map);
  } while (carmen_get_next_point(&params));
}
Example #5
0
void carmen_mapper_update_evidence_grid_general(evidence_grid *grid,
						double laser_x, double laser_y,
						double laser_theta, int num_readings,
						float *laser_range,
						float *laser_angle,
						double angular_resolution,
						double first_beam_angle)
{
  int i;
  int x1int, y1int, x2int, y2int, current_x, current_y, x_diff, y_diff;
  double d, theta, delta_theta, p_filled, temp_laser_x, temp_laser_y;
  carmen_bresenham_param_t b_params;
  double new_prob;
  double correction_angle = 0;

  if(grid->first) {
    grid->start_x = laser_x / grid->resolution;
    grid->start_y = laser_y / grid->resolution;
    grid->start_theta = laser_theta;
    grid->first = 0;
  }

  correction_angle = grid->theta_offset - grid->start_theta;

  laser_theta = laser_theta + correction_angle;
  temp_laser_x = laser_x * cos(correction_angle) -
    laser_y * sin(correction_angle);
  temp_laser_y = laser_x * sin(correction_angle) +
    laser_y * cos(correction_angle);

  laser_x = grid->size_x / 2.0 + (temp_laser_x / grid->resolution -
				  grid->start_x);
  laser_y = grid->size_y / 2.0 + (temp_laser_y / grid->resolution -
				  grid->start_y);

  x1int = (int)floor(laser_x);
  y1int = (int)floor(laser_y);

  theta = laser_theta + first_beam_angle;
  delta_theta = angular_resolution;

  for(i = 0; i < num_readings; i++) {
    if(laser_range[i] < LASER_RANGE_LIMIT) {
      theta = laser_theta + laser_angle[i];
      x2int = (int)(laser_x + (laser_range[i] + grid->wall_thickness) *
		    cos(theta) / grid->resolution);
      y2int = (int)(laser_y + (laser_range[i] + grid->wall_thickness) *
		    sin(theta) / grid->resolution);
      carmen_get_bresenham_parameters(x1int, y1int, x2int, y2int, &b_params);
      do {
	carmen_get_current_point(&b_params, &current_x, &current_y);
	if(current_x >= 0 && current_x < grid->size_x &&
	   current_y >= 0 && current_y < grid->size_y) {
	  /* Calculate distance from laser */
	  x_diff = abs(current_x - x1int);
	  y_diff = abs(current_y - y1int);
	  if(x_diff >= grid->distance_table_size ||
	     y_diff >= grid->distance_table_size)
	    d = 1e6;
	  else
	    d = grid->distance_table[x_diff][y_diff];

	  if(d < laser_range[i]) {       /* Free observation */
	    if(d < grid->max_sure_range)
	      p_filled = grid->emp_evidence;
	    else if(d < grid->max_range)
	      p_filled = grid->emp_evidence +
		(d - grid->max_sure_range) / grid->max_range *
		(grid->prior_occ - grid->emp_evidence);
	    else
	      break;
	  }
	  else {                               /* Filled observation */
	    if(d < grid->max_sure_range)
	      p_filled = grid->occ_evidence;
	    else if(d < grid->max_range)
	      p_filled = grid->occ_evidence +
		(d - grid->max_sure_range) / grid->max_range *
		(grid->prior_occ - grid->occ_evidence);
	    else
	      break;
	  }

	  if(grid->prob[current_x][current_y] == -1)
	    grid->prob[current_x][current_y] = grid->prior_occ;

	  /* Adjust the map */
	  new_prob = 1 - 1 / (1 + (1 - grid->prior_occ) / grid->prior_occ *
			      p_filled / (1 - p_filled) *
			      grid->prob[current_x][current_y] /
			      (1 - grid->prob[current_x][current_y]));

	  grid->prob[current_x][current_y] = new_prob;
	}
      } while(carmen_get_next_point(&b_params));
    }
  }
}