static int set_shortterm(vtimer_t *timer)
{
DEBUG("set_shortterm(): Absolute: %" PRIu32 " %" PRIu32 "\n", timer->absolute.seconds, timer->absolute.microseconds);
timer->priority_queue_entry.priority = timer->absolute.microseconds;
/* *** UGLY FIX BEGINS *** */
/* Workaround for a bug in a so far undiscovered location which causes the
* vtimer to add the same timer twice, locking the system in an infinite
* loop inside priority_queue_add. */
priority_queue_remove(&shortterm_priority_queue_root, timer_get_node(timer));
/* *** UGLY FIX ENDS *** */
priority_queue_add(&shortterm_priority_queue_root, timer_get_node(timer));
return 1;
}
void condition_variable::wait(unique_lock<mutex>& lock) noexcept {
if (!lock.owns_lock()) {
throw std::system_error(
std::make_error_code(std::errc::operation_not_permitted),
"Mutex not locked.");
}
priority_queue_node_t n;
n.priority = sched_active_thread->priority;
n.data = sched_active_pid;
n.next = NULL;
// the signaling thread may not hold the mutex, the queue is not thread safe
unsigned old_state = disableIRQ();
priority_queue_add(&m_queue, &n);
restoreIRQ(old_state);
mutex_unlock_and_sleep(lock.mutex()->native_handle());
if (n.data != -1u) {
// on signaling n.data is set to -1u
// if it isn't set, then the wakeup is either spurious or a timer wakeup
old_state = disableIRQ();
priority_queue_remove(&m_queue, &n);
restoreIRQ(old_state);
}
mutex_lock(lock.mutex()->native_handle());
}
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);
}
static int set_longterm(vtimer_t *timer)
{
timer->priority_queue_entry.priority = timer->absolute.seconds;
priority_queue_add(&longterm_priority_queue_root, timer_get_node(timer));
return 0;
}
static int _msg_send(msg_t *m, kernel_pid_t target_pid, bool block, unsigned state)
{
#ifdef DEVELHELP
if (!pid_is_valid(target_pid)) {
DEBUG("msg_send(): target_pid is invalid, continuing anyways\n");
}
#endif /* DEVELHELP */
tcb_t *target = (tcb_t*) sched_threads[target_pid];
m->sender_pid = sched_active_pid;
if (target == NULL) {
DEBUG("msg_send(): target thread does not exist\n");
restoreIRQ(state);
return -1;
}
DEBUG("msg_send() %s:%i: Sending from %" PRIkernel_pid " to %" PRIkernel_pid
". block=%i src->state=%i target->state=%i\n", RIOT_FILE_RELATIVE,
__LINE__, sched_active_pid, target_pid,
block, sched_active_thread->status, target->status);
if (target->status != STATUS_RECEIVE_BLOCKED) {
DEBUG("msg_send() %s:%i: Target %" PRIkernel_pid " is not RECEIVE_BLOCKED.\n",
RIOT_FILE_RELATIVE, __LINE__, target_pid);
if (queue_msg(target, m)) {
DEBUG("msg_send() %s:%i: Target %" PRIkernel_pid
" has a msg_queue. Queueing message.\n", RIOT_FILE_RELATIVE,
__LINE__, target_pid);
restoreIRQ(state);
if (sched_active_thread->status == STATUS_REPLY_BLOCKED) {
thread_yield_higher();
}
return 1;
}
if (!block) {
DEBUG("msg_send: %" PRIkernel_pid ": Receiver not waiting, block=%u\n",
sched_active_thread->pid, block);
restoreIRQ(state);
return 0;
}
DEBUG("msg_send: %" PRIkernel_pid ": send_blocked.\n",
sched_active_thread->pid);
priority_queue_node_t n;
n.priority = sched_active_thread->priority;
n.data = (unsigned int) sched_active_thread;
n.next = NULL;
DEBUG("msg_send: %" PRIkernel_pid ": Adding node to msg_waiters:\n",
sched_active_thread->pid);
priority_queue_add(&(target->msg_waiters), &n);
sched_active_thread->wait_data = (void*) m;
int newstatus;
if (sched_active_thread->status == STATUS_REPLY_BLOCKED) {
newstatus = STATUS_REPLY_BLOCKED;
}
else {
newstatus = STATUS_SEND_BLOCKED;
}
sched_set_status((tcb_t*) sched_active_thread, newstatus);
DEBUG("msg_send: %" PRIkernel_pid ": Back from send block.\n",
sched_active_thread->pid);
restoreIRQ(state);
thread_yield_higher();
}
else {
DEBUG("msg_send: %" PRIkernel_pid ": Direct msg copy from %"
PRIkernel_pid " to %" PRIkernel_pid ".\n",
sched_active_thread->pid, thread_getpid(), target_pid);
/* copy msg to target */
msg_t *target_message = (msg_t*) target->wait_data;
*target_message = *m;
sched_set_status(target, STATUS_PENDING);
restoreIRQ(state);
thread_yield_higher();
}
return 1;
}
static void recursive_insert(lm_trie_t *trie, ngram_raw_t **raw_ngrams, uint32 *counts, int order)
{
uint32 unigram_idx = 0;
uint32 *words;
float *probs;
const uint32 unigram_count = (uint32)counts[0];
priority_queue_t *ngrams = priority_queue_create(order, &ngram_ord_comparator);
ngram_raw_ord_t *ngram;
uint32 *raw_ngrams_ptr;
int i;
words = (uint32 *)ckd_calloc(order, sizeof(*words)); //for blanks catching
probs = (float *)ckd_calloc(order - 1, sizeof(*probs)); //for blanks prob generating
ngram = (ngram_raw_ord_t *)ckd_calloc(1, sizeof(*ngram));
ngram->order = 1;
ngram->instance.words = &unigram_idx;
priority_queue_add(ngrams, ngram);
raw_ngrams_ptr = (uint32 *)ckd_calloc(order - 1, sizeof(*raw_ngrams_ptr));
for (i = 2; i <= order; ++i) {
ngram_raw_ord_t *tmp_ngram = (ngram_raw_ord_t *)ckd_calloc(1, sizeof(*tmp_ngram));
tmp_ngram->order = i;
raw_ngrams_ptr[i-2] = 0;
tmp_ngram->instance = raw_ngrams[i - 2][raw_ngrams_ptr[i-2]];
priority_queue_add(ngrams, tmp_ngram);
}
for (;;) {
ngram_raw_ord_t *top = (ngram_raw_ord_t *)priority_queue_poll(ngrams);
if (top->order == 1) {
trie->unigrams[unigram_idx].next = unigram_next(trie, order);
words[0] = unigram_idx;
probs[0] = trie->unigrams[unigram_idx].prob;
if (++unigram_idx == unigram_count + 1) {
ckd_free(top);
break;
}
priority_queue_add(ngrams, top);
} else {
for (i = 0; i < top->order - 1; i++) {
if (words[i] != top->instance.words[i]) {
//need to insert dummy suffixes to make ngram of higher order reachable
int j;
assert(i > 0); //unigrams are not pruned without removing ngrams that contains them
for (j = i; j < top->order - 1; j++) {
middle_t *middle = &trie->middle_begin[j - 1];
bitarr_address_t address = middle_insert(middle, top->instance.words[j], j + 1, order);
//calculate prob for blank
float calc_prob = probs[j - 1] + trie->unigrams[top->instance.words[j]].bo;
probs[j] = calc_prob;
lm_trie_quant_mwrite(trie->quant, address, j - 1, calc_prob, 0.0f);
}
}
}
memcpy(words, top->instance.words, top->order * sizeof(*words));
if (top->order == order) {
float *weights = top->instance.weights;
bitarr_address_t address = longest_insert(trie->longest, top->instance.words[top->order - 1]);
lm_trie_quant_lwrite(trie->quant, address, weights[0]);
} else {
float *weights = top->instance.weights;
middle_t *middle = &trie->middle_begin[top->order - 2];
bitarr_address_t address = middle_insert(middle, top->instance.words[top->order - 1], top->order, order);
//write prob and backoff
probs[top->order - 1] = weights[0];
lm_trie_quant_mwrite(trie->quant, address, top->order - 2, weights[0], weights[1]);
}
raw_ngrams_ptr[top->order - 2]++;
if (raw_ngrams_ptr[top->order - 2] < counts[top->order - 1]) {
top->instance = raw_ngrams[top->order-2][raw_ngrams_ptr[top->order - 2]];
priority_queue_add(ngrams, top);
} else {
ckd_free(top);
}
}
}
assert(priority_queue_size(ngrams) == 0);
priority_queue_free(ngrams, NULL);
ckd_free(raw_ngrams_ptr);
ckd_free(words);
ckd_free(probs);
}
