void carmen_graphics_update_ipc_callbacks(GdkInputFunction callback_Func)
{
fd_set *open_fds;
int max_connection;
int index;
int callback_index;
carmen_graphics_callback *callback;
carmen_graphics_callback new_callback;
static carmen_list_t *existing_callbacks = NULL;
static int num_callbacks = 0;
if (existing_callbacks == NULL)
existing_callbacks =
carmen_list_create(sizeof(carmen_graphics_callback), 10);
for (index = 0; index < num_callbacks; index++) {
callback = carmen_list_get(existing_callbacks, index);
callback->ok = 0;
}
open_fds = x_ipcGetConnections();
max_connection = x_ipcGetMaxConnection();
for (index = 0; index <= max_connection; index++) {
if (FD_ISSET(index, open_fds)) {
for (callback_index = 0; callback_index < num_callbacks;
callback_index++) {
callback = carmen_list_get(existing_callbacks, callback_index);
if (index == callback->fd) {
callback->ok = 1;
break;
}
}
if (callback_index == existing_callbacks->length) {
new_callback.fd = index;
new_callback.ok = 1;
new_callback.callback_id =
gdk_input_add(index, GDK_INPUT_READ, callback_Func, NULL);
carmen_list_add(existing_callbacks, &new_callback);
}
} /* End of if (FD_ISSET(index, open_fds)) */
} /* End of for (index = 0; index <= max_connection; index++) */
for (index = 0; index < num_callbacks; index++) {
callback = carmen_list_get(existing_callbacks, index);
if (callback->ok == 0) {
gdk_input_remove(callback->callback_id);
carmen_list_delete(existing_callbacks, index);
index--;
}
}
}
carmen_roadmap_t *carmen_roadmap_initialize(carmen_map_p new_map)
{
int x, y, i;
int **grid, *grid_ptr, **sample_grid;
int progress;
int size;
int growth_counter;
int x_offset[8] = {1, 1, 0, -1, -1, -1, 0, 1};
int y_offset[8] = {0, 1, 1, 1, 0, -1, -1, -1};
int total;
int sample;
int random_value;
int max_label;
carmen_roadmap_t *roadmap;
carmen_map_p c_space;
carmen_list_t *node_list;
c_space = carmen_map_copy(new_map);
node_list = carmen_list_create(sizeof(carmen_roadmap_vertex_t),
MAX_NUM_VERTICES);
size = c_space->config.x_size*c_space->config.y_size;
grid = (int **)calloc(c_space->config.x_size, sizeof(int *));
carmen_test_alloc(grid);
grid[0] = (int *)calloc(size, sizeof(int));
carmen_test_alloc(grid[0]);
for (x = 1; x < c_space->config.x_size; x++)
grid[x] = grid[0] + x*c_space->config.y_size;
sample_grid = (int **)calloc(c_space->config.x_size, sizeof(int *));
carmen_test_alloc(sample_grid);
sample_grid[0] = (int *)calloc(size, sizeof(int));
carmen_test_alloc(sample_grid);
for (x = 1; x < c_space->config.x_size; x++)
sample_grid[x] = sample_grid[0] + x*c_space->config.y_size;
grid_ptr = grid[0];
for (i = 0; i < size; i++)
*(grid_ptr++) = 0;
for (x = 0; x < c_space->config.x_size; x++)
for (y = 0; y < c_space->config.y_size; y++) {
if (x == 0 || y == 0 || x == c_space->config.x_size-1 ||
y == c_space->config.y_size-1 ||
c_space->map[x][y] > 0.001 || c_space->map[x][y] < 0) {
grid[x][y] = 1;
c_space->map[x][y] = 1;
}
}
growth_counter = 1;
do {
progress = 0;
for (x = 1; x < c_space->config.x_size-1; x++)
for (y = 1; y < c_space->config.y_size-1; y++) {
if (grid[x][y] == growth_counter) {
for (i = 0; i < 8; i+=2) {
if (grid[x+x_offset[i]][y+y_offset[i]] == 0) {
grid[x+x_offset[i]][y+y_offset[i]] = growth_counter+1;
c_space->map[x][y] = growth_counter+1;
}
progress = 1;
}
}
}
growth_counter++;
} while (progress);
total = 0;
for (x = 0; x < c_space->config.x_size; x++)
for (y = 0; y < c_space->config.y_size; y++) {
if (x == 0 || x == c_space->config.x_size-1 ||
y == 0 || y == c_space->config.y_size-1) {
sample_grid[x][y] = total;
c_space->map[x][y] = 1;
}
if (grid[x][y] < 6) {
sample_grid[x][y] = total;
if (grid[x][y] < 4)
c_space->map[x][y] = 1e6;
continue;
}
for (i = 0; i < 8; i+=2) {
if (grid[x+x_offset[i]][y+y_offset[i]] >= grid[x][y])
break;
}
if (i >= 8) {
total += 100;
sample_grid[x][y] = total;
continue;
}
for (i = 0; i < 8; i+=2) {
if (grid[x+x_offset[i]][y+y_offset[i]] >= grid[x][y]+1)
break;
}
if (i == 8) {
total += 50;
sample_grid[x][y] = total;
continue;
}
total++;
sample_grid[x][y] = total;
}
max_label = 0;
x = 0;
y = 0;
for (sample = 0; sample < MAX_NUM_VERTICES; sample++) {
random_value = (int)carmen_uniform_random(0, total);
if (random_value < sample_grid[0][size/2]) {
grid_ptr = sample_grid[0];
for (i = 0; i < size-1; i++) {
if (random_value < *(grid_ptr++))
break;
}
} else {
grid_ptr = sample_grid[0]+size/2;
for (i = size/2; i < size-1; i++) {
if (random_value < *(grid_ptr++))
break;
}
}
if (grid[0][i] > 0) {
x = i / c_space->config.y_size;
y = i % c_space->config.y_size;
add_node(node_list, x, y);
grid[0][i] = 0;
}
}
add_node(node_list, 450, 140);
add_node(node_list, 550, 140);
add_node(node_list, 600, 141);
add_node(node_list, 550, 141);
free(grid[0]);
free(grid);
free(sample_grid[0]);
free(sample_grid);
roadmap = (carmen_roadmap_t *)calloc(1, sizeof(carmen_roadmap_t));
carmen_test_alloc(roadmap);
roadmap->nodes = node_list;
roadmap->goal_id = -1;
roadmap->c_space = c_space;
roadmap->avoid_people = 1;
return roadmap;
}
static void simulator_objects_handler(carmen_simulator_objects_message
*objects_msg)
{
carmen_dot_person_t person;
carmen_dot_trash_t trash;
carmen_traj_point_t p1;
carmen_dot_all_people_msg people_msg;
carmen_dot_all_trash_msg trash_msg;
int i;
IPC_RETURN_TYPE err;
people_msg.timestamp = carmen_get_time_ms();
strcpy(people_msg.host, carmen_get_tenchar_host_name());
trash_msg.timestamp = carmen_get_time_ms();
strcpy(trash_msg.host, carmen_get_tenchar_host_name());
if (person_list == NULL)
person_list = carmen_list_create(sizeof(carmen_dot_person_t), 10);
if (trash_list == NULL)
trash_list = carmen_list_create(sizeof(carmen_dot_trash_t), 10);
person_list->length = 0;
trash_list->length = 0;
p1.x = truepose.truepose.x;
p1.y = truepose.truepose.y;
for (i = 0; i < objects_msg->num_objects; i++) {
if (!points_are_visible(&p1, objects_msg->objects_list+i, map))
continue;
if (objects_msg->objects_list[i].t_vel < .2) {
trash.x = objects_msg->objects_list[i].x;
trash.y = objects_msg->objects_list[i].y;
//dbug: need to add convex hull code or won't work!
trash.vx = 0.375;
trash.vy = 0.375;
trash.vxy = 0.125;
trash.id = i;
carmen_list_add(trash_list, &trash);
} else {
person.x = objects_msg->objects_list[i].x;
person.y = objects_msg->objects_list[i].y;
person.vx = 0.375;
person.vy = 0.375;
person.vxy = 0.125;
person.id = i;
carmen_list_add(person_list, &person);
}
}
if (person_list->length > 0) {
people_msg.num_people = person_list->length;
people_msg.people = (carmen_dot_person_t *)person_list->list;
} else {
people_msg.num_people = 0;
people_msg.people = NULL;
}
err = IPC_publishData(CARMEN_DOT_ALL_PEOPLE_MSG_NAME, &people_msg);
carmen_test_ipc_exit(err, "Could not publish",
CARMEN_DOT_ALL_PEOPLE_MSG_NAME);
if (trash_list->length > 0) {
trash_msg.num_trash = trash_list->length;
trash_msg.trash = (carmen_dot_trash_t *)trash_list->list;
} else {
trash_msg.num_trash = 0;
trash_msg.trash = NULL;
}
err = IPC_publishData(CARMEN_DOT_ALL_TRASH_MSG_NAME, &trash_msg);
carmen_test_ipc_exit(err, "Could not publish",
CARMEN_DOT_ALL_TRASH_MSG_NAME);
}
