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--;
}
}*/
}
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--;
}
}
}
void carmen_roadmap_refine_graph(carmen_world_point_t *robot,
carmen_roadmap_t *road)
{
carmen_dot_t blocking_object;
double varx, vary, varxy;
double radius;
double x, y;
int i;
double theta;
carmen_world_point_t world_pt;
carmen_map_point_t map_pt;
carmen_roadmap_vertex_t *node_list;
carmen_roadmap_vertex_t *goal_node;
carmen_world_point_t goal;
get_blocking_object(&blocking_object, robot, road);
if (blocking_object.dot_type == carmen_dot_person) {
x = blocking_object.data.person.x;
y = blocking_object.data.person.y;
radius = blocking_object.data.person.r;
radius *= 1.1;
} else if (blocking_object.dot_type == carmen_dot_trash) {
//dbug: use convex hull
x = blocking_object.data.trash.x;
y = blocking_object.data.trash.y;
varx = blocking_object.data.trash.vx;
vary = blocking_object.data.trash.vy;
varxy = blocking_object.data.trash.vxy;
carmen_roadmap_refine_get_radius(varx, vary, varxy, &radius);
radius *= 1.1;
} else { // if (blocking_object.dot_type == carmen_dot_door) {
x = blocking_object.data.door.x;
y = blocking_object.data.door.y;
varx = blocking_object.data.door.vx;
vary = blocking_object.data.door.vy;
varxy = blocking_object.data.door.vxy;
carmen_roadmap_refine_get_radius(varx, vary, varxy, &radius);
radius *= 1.1;
}
node_list = (carmen_roadmap_vertex_t *)road->nodes->list;
for (theta = -M_PI; theta < M_PI; theta += M_PI/8) {
world_pt.pose.x = x + radius*cos(theta);
world_pt.pose.y = y + radius*sin(theta);
world_pt.map = road->c_space;
carmen_world_to_map(&world_pt, &map_pt);
if (road->c_space->map[map_pt.x][map_pt.y] > 1)
continue;
for (i = 0; i < road->nodes->length; i++) {
if (map_pt.x == node_list[i].x && map_pt.y == node_list[i].y)
break;
}
if (i == road->nodes->length) {
carmen_roadmap_add_node(road, map_pt.x, map_pt.y);
node_list = (carmen_roadmap_vertex_t *)road->nodes->list;
}
}
goal_node = (carmen_roadmap_vertex_t *)
carmen_list_get(road->nodes, road->goal_id);
map_pt.x = goal_node->x;
map_pt.y = goal_node->y;
map_pt.map = road->c_space;
carmen_map_to_world(&map_pt, &goal);
carmen_warn("Refine: Goal: %f %f\n", goal.pose.x, goal.pose.y);
carmen_roadmap_plan(road, &goal);
}
