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);
}
}
}
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();
}
