void starpu_data_vmap_filters(starpu_data_handle_t root_handle, unsigned nfilters, va_list pa)
{
	unsigned i;
	for (i = 0; i < nfilters; i++)
	{
		struct starpu_data_filter *next_filter;
		next_filter = va_arg(pa, struct starpu_data_filter *);

		STARPU_ASSERT(next_filter);

		map_filter(root_handle, next_filter);
	}
}
/*
 * This function applies a data filter on all the elements of a partition
 */
static void map_filter(starpu_data_handle_t root_handle, struct starpu_data_filter *f)
{
	/* we need to apply the data filter on all leaf of the tree */
	if (root_handle->nchildren == 0)
	{
		/* this is a leaf */
		starpu_data_partition(root_handle, f);
	}
	else
	{
		/* try to apply the data filter recursively */
		unsigned child;
		for (child = 0; child < root_handle->nchildren; child++)
		{
			starpu_data_handle_t handle_child = starpu_data_get_child(root_handle, child);
			map_filter(handle_child, f);
		}
	}
}
Beispiel #3
0
static void laser_handler(carmen_robot_laser_message *laser) {

  int i, c, n;
  static int cluster_map[500];
  static int cluster_cnt;
  static double x[500], y[500];

  if (static_map.map == NULL || odom.timestamp == 0.0)
    return;

  carmen_localize_correct_laser(laser, &odom);

  kill_people();
  filter_motion();

  for (i = 0; i < num_filters; i++) {
    filters[i].updated = 0;
    filters[i].last_type = filters[i].type;
    filters[i].invisible = 0;
  }

  if (dist(last_sensor_update_odom.x - odom.globalpos.x,
	   last_sensor_update_odom.y - odom.globalpos.y) >= sensor_update_dist) {
    do_sensor_update = 1;
    for (i = 0; i < num_filters; i++)
      filters[i].do_motion_update = 1;
  }
  else
    do_sensor_update = 0;
  
  if (do_sensor_update) {
    last_sensor_update_odom.x = odom.globalpos.x;
    last_sensor_update_odom.y = odom.globalpos.y;
  }

  cluster_cnt = 0;
  for (i = 0; i < laser->num_readings; i++) {
    cluster_map[i] = 0;
    if (laser->range[i] < laser_max_range) {
      x[i] = laser->x + cos(laser->theta + (i-90)*M_PI/180.0) * laser->range[i];
      y[i] = laser->y + sin(laser->theta + (i-90)*M_PI/180.0) * laser->range[i];
      trace_laser(laser->x, laser->y, x[i], y[i]);
      if (!dot_filter(x[i], y[i]) && !map_filter(x[i], y[i], laser->range[i]))
	cluster_cnt = cluster(cluster_map, cluster_cnt, i, x, y);
    }
  }

  for (i = 0; i < num_filters; i++) {
    if (filters[i].invisible && !filters[i].updated) {
      if (++filters[i].invisible_cnt >= invisible_cnt)
	delete_filter(i);
    }
    else
      filters[i].invisible_cnt = 0;
  }      

  //printf("cluster_cnt = %d\n", cluster_cnt);

  for (c = 1; c <= cluster_cnt; c++) {
    n = 0;
    for (i = 0; i < laser->num_readings; i++)
      if (cluster_map[i] == c)
	n++;
    //printf("cluster %d has %d readings\n", c, n);
    if (n >= new_filter_threshold)
      add_new_dot_filter(cluster_map, c, laser->num_readings, x, y);
  }

#if 0
  for (i = 0; i < num_filters; i++) {
    if (filters[i].type != filters[i].last_type) {
      n = filters[i].type;
      filters[i].type = filters[i].last_type;
      //publish_dot_msg(&filters[i], 1);
      filters[i].type = n;
      /*
      switch (filters[i].type) {
      case CARMEN_DOT_PERSON:
	printf("publishing add person %d msg\n", filters[i].id);
	break;
      case CARMEN_DOT_TRASH:
	printf("publishing add trash %d msg\n", filters[i].id);
	break;
      case CARMEN_DOT_DOOR:
	printf("publishing add door %d msg\n", filters[i].id);
	break;
      }
      */
    }
    //if (filters[i].updated)
    //publish_dot_msg(&filters[i], 0);
  }
#endif

  /*
  for (i = 0; i < num_filters; i++) {
    printf("vel = %.4f, ", person_filter_velocity(&filters[i].person_filter));
    if (filters[i].type == CARMEN_DOT_PERSON) {
      printf("PERSON (x=%.2f, y=%.2f) (vx=%.4f, vy=%.4f, vxy=%.4f)\n",
	     filters[i].person_filter.x, filters[i].person_filter.y,
	     filters[i].person_filter.px, filters[i].person_filter.py,
	     filters[i].person_filter.pxy);
    }
    else if (filters[i].type == CARMEN_DOT_TRASH)
      printf("TRASH (x=%.2f, y=%.2f) (vx=%.4f, vy=%.4f, vxy=%.4f)\n",
	     filters[i].trash_filter.x, filters[i].trash_filter.y,
	     filters[i].trash_filter.px, filters[i].trash_filter.py,
	     filters[i].trash_filter.pxy);
    else
      printf("DOOR (x=%.2f, y=%.2f, t=%.2f) (vx=%.4f, vy=%.4f, vxy=%.4f, vt=%.4f)\n",
	     filters[i].door_filter.x, filters[i].door_filter.y,
	     filters[i].door_filter.t, filters[i].door_filter.px,
	     filters[i].door_filter.py, filters[i].door_filter.pxy,
	     filters[i].door_filter.pt);
  }
  printf("\n");
  */

  publish_all_dot_msgs();
}