int main()
{
struct priority_queue *q;
q = priority_queue_init(char_cmp);
assert(q != NULL);
assert(priority_queue_size(q) == 0);
priority_queue_add(q, (void *) 'a');
assert(priority_queue_size(q) == 1);
assert((char) priority_queue_get(q) == 'a');
priority_queue_add(q, (void *) 'b');
assert(priority_queue_size(q) == 2);
assert((char) priority_queue_get(q) == 'a');
priority_queue_add(q, (void *) 'c');
assert(priority_queue_size(q) == 3);
assert((char) priority_queue_pop(q) == 'a');
assert(priority_queue_size(q) == 2);
assert((char) priority_queue_pop(q) == 'b');
assert((char) priority_queue_pop(q) == 'c');
assert(priority_queue_pop(q) == NULL);
priority_queue_destory(q);
TEST_OK("priority queue test passed...");
}
/* priority_queue_t */
void _type_init_priority_queue(const void* cpv_input, void* pv_output)
{
bool_t b_result = false;
assert(cpv_input != NULL && pv_output != NULL);
b_result = _create_priority_queue_auxiliary((priority_queue_t*)cpv_input, (char*)pv_output);
assert(b_result);
priority_queue_init((priority_queue_t*)cpv_input);
}
struct gomp_team *
gomp_new_team (unsigned nthreads)
{
struct gomp_team *team;
int i;
team = get_last_team (nthreads);
if (team == NULL)
{
size_t extra = sizeof (team->ordered_release[0])
+ sizeof (team->implicit_task[0]);
team = gomp_malloc (sizeof (*team) + nthreads * extra);
#ifndef HAVE_SYNC_BUILTINS
gomp_mutex_init (&team->work_share_list_free_lock);
#endif
gomp_barrier_init (&team->barrier, nthreads);
gomp_mutex_init (&team->task_lock);
team->nthreads = nthreads;
}
team->work_share_chunk = 8;
#ifdef HAVE_SYNC_BUILTINS
team->single_count = 0;
#endif
team->work_shares_to_free = &team->work_shares[0];
gomp_init_work_share (&team->work_shares[0], false, nthreads);
team->work_shares[0].next_alloc = NULL;
team->work_share_list_free = NULL;
team->work_share_list_alloc = &team->work_shares[1];
for (i = 1; i < 7; i++)
team->work_shares[i].next_free = &team->work_shares[i + 1];
team->work_shares[i].next_free = NULL;
gomp_sem_init (&team->master_release, 0);
team->ordered_release = (void *) &team->implicit_task[nthreads];
team->ordered_release[0] = &team->master_release;
priority_queue_init (&team->task_queue);
team->task_count = 0;
team->task_queued_count = 0;
team->task_running_count = 0;
team->work_share_cancelled = 0;
team->team_cancelled = 0;
return team;
}
void
carmen_localize_ackerman_initialize_particles_uniform(carmen_localize_ackerman_particle_filter_p filter,
carmen_robot_ackerman_laser_message *laser,
carmen_localize_ackerman_map_p map)
{
priority_queue_p queue = priority_queue_init(filter->param->num_particles);
double *laser_x, *laser_y;
int i, j, x_l, y_l;
float angle, prob, ctheta, stheta;
carmen_point_t point;
queue_node_p mark;
int *beam_valid;
/* compute the correct laser_skip */
if (filter->param->laser_skip <= 0) {
filter->param->laser_skip =
floor(filter->param->integrate_angle / laser->config.angular_resolution);
}
fprintf(stderr, "\rDoing global localization... (%.1f%% complete)", 0.0);
filter->initialized = 0;
/* copy laser scan into temporary memory */
laser_x = (double *)calloc(laser->num_readings, sizeof(double));
carmen_test_alloc(laser_x);
laser_y = (double *)calloc(laser->num_readings, sizeof(double));
carmen_test_alloc(laser_y);
beam_valid = (int *)calloc(laser->num_readings, sizeof(int));
carmen_test_alloc(beam_valid);
for(i = 0; i < laser->num_readings; i++) {
if (laser->range[i] < laser->config.maximum_range &&
laser->range[i] < filter->param->max_range)
beam_valid[i] = 1;
else
beam_valid[i] = 0;
}
/* do all calculations in map coordinates */
for(i = 0; i < laser->num_readings; i++) {
angle = laser->config.start_angle +
i * laser->config.angular_resolution;
laser_x[i] = (filter->param->front_laser_offset +
laser->range[i] * cos(angle)) / map->config.resolution;
laser_y[i] = (laser->range[i] * sin(angle)) / map->config.resolution;
}
for(i = 0; i < filter->param->global_test_samples; i++) {
if(i % 10000 == 0)
{
fprintf(stderr, "\rDoing global localization... (%.1f%% complete)",
i / (double)filter->param->global_test_samples * 100.0);
carmen_ipc_sleep(0.001);
}
do {
point.x = carmen_uniform_random(0, map->config.x_size - 1);
point.y = carmen_uniform_random(0, map->config.y_size - 1);
} while(map->carmen_map.map[(int)point.x][(int)point.y] >
filter->param->occupied_prob ||
map->carmen_map.map[(int)point.x][(int)point.y] == -1);
point.theta = carmen_uniform_random(-M_PI, M_PI);
prob = 0.0;
ctheta = cos(point.theta);
stheta = sin(point.theta);
for(j = 0; j < laser->num_readings &&
(queue->last == NULL || prob > queue->last->prob);
j += filter->param->laser_skip)
{
if (beam_valid[j]) {
x_l = point.x + laser_x[j] * ctheta - laser_y[j] * stheta;
y_l = point.y + laser_x[j] * stheta + laser_y[j] * ctheta;
if(x_l >= 0 && y_l >= 0 && x_l < map->config.x_size &&
y_l < map->config.y_size)
prob += map->gprob[x_l][y_l];
else
prob -= 100;
}
}
priority_queue_add(queue, point, prob);
}
/* transfer samples from priority queue back into particles */
mark = queue->first;
for(i = 0; i < queue->num_elements; i++) {
filter->particles[i].x = (mark->point.x * map->config.resolution) + map->config.x_origin;
filter->particles[i].y = (mark->point.y * map->config.resolution) + map->config.y_origin;
filter->particles[i].theta = mark->point.theta;
mark = mark->next;
}
priority_queue_free(queue);
free(laser_x);
free(laser_y);
free(beam_valid);
if(filter->param->do_scanmatching) {
for(i = 0; i < filter->param->num_particles; i++) {
point.x = filter->particles[i].x;
point.y = filter->particles[i].y;
point.theta = filter->particles[i].theta;
carmen_localize_ackerman_laser_scan_gd(laser->num_readings, laser->range,
laser->config.angular_resolution,
laser->config.start_angle,
&point,
filter->param->front_laser_offset,
map,
filter->param->laser_skip);
filter->particles[i].x = point.x;
filter->particles[i].y = point.y;
filter->particles[i].theta = point.theta;
filter->particles[i].weight = 0.0;
}
}
filter->initialized = 1;
filter->first_odometry = 1;
filter->global_mode = 1;
filter->distance_travelled = 0;
fprintf(stderr, "\rDoing global localization... (%.1f%% complete)\n\n",
100.0);
}
