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