static carmen_inline void screen_to_world(carmen_point_t *p, carmen_world_point_t *wp,
GtkMapViewer *map_view)
{
carmen_map_point_t mp;
screen_to_map(p, &mp, map_view);
carmen_map_to_world(&mp, wp);
}
static void pick_random_place(void)
{
carmen_map_point_t map_pt;
carmen_world_point_t world_pt;
map_pt.map = c_space;
do {
map_pt.x = carmen_uniform_random(0, c_space->config.x_size);
map_pt.y = carmen_uniform_random(0, c_space->config.y_size);
} while (c_space->map[map_pt.x][map_pt.y] < 0);
carmen_map_to_world(&map_pt, &world_pt);
carmen_navigator_set_goal(world_pt.pose.x, world_pt.pose.y);
carmen_navigator_go();
up_to_speed = 0;
}
/* handles the mouse moving in the drawing area
only calls the appropriate tool function if the mouse button is down.
always sets the cursor to the appropriate symbol */
static gint
motion_notify_event( GtkWidget *widget, GdkEventMotion *event )
{
int x, y;
GdkModifierType state;
static char label_buffer[255];
static carmen_map_point_t map_point;
static carmen_world_point_t world_point;
gtk_propagate_event(GTK_WIDGET(vrule), (GdkEvent *)event);
gtk_propagate_event(GTK_WIDGET(hrule), (GdkEvent *)event);
map_to_tmp();
if (event->is_hint)
gdk_window_get_pointer (event->window, &x, &y, &state);
else
{
x = event->x;
y = event->y;
state = event->state;
}
map_point.x = pix_x_to_map(x);
map_point.y = pix_y_to_map(y);
map_point.map = map;
carmen_map_to_world(&map_point, &world_point);
sprintf(label_buffer, "X: %6.2f Y: %6.2f", world_point.pose.x,
world_point.pose.y);
gtk_label_set_text(GTK_LABEL(coords_label), label_buffer);
if (tmp_pixmap == NULL)
return TRUE;
if (deleting_placename)
{
handle_deleting_placename_move(map_point.x, map_point.y);
}
else if ((state & GDK_BUTTON1_MASK))
{
switch(utensil)
{
case CARMEN_MAP_BRUSH:
draw_brush (widget, x, y);
x = x/mult - brush_size + .5;
y = y/mult - brush_size + .5;
break;
case CARMEN_MAP_RECTANGLE:
creating_rectangle(widget, 1, x, y);
break;
case CARMEN_MAP_CROP:
cropping(widget, 1, x, y);
break;
case CARMEN_MAP_LINE:
creating_line(1, x, y);
break;
case CARMEN_MAP_EYEDROPPER:
sample(x, y);
break;
case CARMEN_MAP_MOVER:
move(1, x, y);
default:
break;
}
}
redraw();
return TRUE;
}
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);
}
