void carmen_graphics_update_ipc_callbacks_qt(QObject* parent, const char* updateIPC)
{
fd_set *open_fds;
int max_connection;
int index;
int callback_index;
carmen_graphics_callback_qt *callback;
carmen_graphics_callback_qt new_callback;
static carmen_list_t *existing_callbacks = NULL;
static int num_callbacks = 0;
QSocketNotifier *qt_socket = NULL;
if (existing_callbacks == NULL)
existing_callbacks =
carmen_list_create(sizeof(carmen_graphics_callback_qt), 10);
for (index = 0; index < num_callbacks; index++) {
callback = (carmen_graphics_callback_qt*) 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_graphics_callback_qt*) carmen_list_get(existing_callbacks, callback_index);
if (index == callback->fd) {
callback->ok = 1;
break;
}
}
if (callback_index == existing_callbacks->length) {
qt_socket = new QSocketNotifier(index, QSocketNotifier::Read);
QObject::connect(qt_socket, SIGNAL(activated(int)), parent, updateIPC);
new_callback.fd = index;
new_callback.ok = 1;
new_callback.callback_id = qt_socket->socket();
//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--;
}
}*/
}
static void add_node(carmen_list_t *node_list, int x, int y)
{
carmen_roadmap_vertex_t vertex;
vertex.id = node_list->length;
vertex.x = x;
vertex.y = y;
vertex.label = -1;
vertex.utility = 0;
vertex.bad = 0;
vertex.edges = carmen_list_create(sizeof(carmen_roadmap_edge_t), 10);
carmen_list_add(node_list, &vertex);
}
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--;
}
}
}
static void add_edge(carmen_roadmap_vertex_t *node,
carmen_roadmap_vertex_t *parent_node, double cost)
{
int i, neighbour_id;
carmen_roadmap_edge_t edge;
carmen_roadmap_edge_t *edges;
edges = (carmen_roadmap_edge_t*)(parent_node->edges->list);
for (i = 0; i < parent_node->edges->length; i++) {
neighbour_id = edges[i].id;
if (neighbour_id == node->id)
return;
}
edge.id = node->id;
edge.cost = cost;
edge.blocked = 0;
assert (edge.cost > 0);
carmen_list_add(parent_node->edges, &edge);
}
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);
}
