int main(int ac, char **av)
{
Queue *queue;
int i;
char *str;
queue = NULL;
i = 0;
while (i < ac)
{
push_queue(&queue, av[i]);
++i;
}
print_queue(queue);
str = pop_queue(&queue);
printf("%s\n", str);
print_queue(queue);
str = pop_queue(&queue);
printf("%s\n", str);
print_queue(queue);
str = pop_queue(&queue);
printf("%s\n", str);
print_queue(queue);
return (0);
}
int main(void){
Pilha my_stack;
fila my_queue;
iniciar_stack(tamanho,&my_stack);
iniciar_queue(tamanho,&my_queue);
for(register unsigned int cont = 0 ; cont < 10 ; cont ++){
push_queue(cont,&my_queue);
printf("Fila(%d):%d\n",cont+1,cont);
}
int *aux;
aux = (int*)malloc((sizeof(tamanho)));
aux = listar_queue(my_queue);
for(register unsigned int cont = 0 ; cont < 10 ; cont ++){
int valor = aux[cont];
push_stack(valor,&my_stack);
printf("Fila(%d):%d\n",cont+1,aux[cont]);
pop_queue(&my_queue);
}
int quantidade;
aux = listar_stack(my_stack,&quantidade);
for(register unsigned int cont = 0 ; cont < 10 ; cont ++){
int valor = aux[cont];
push_queue(valor,&my_queue);
printf("Fila(%d):%d\n",cont+1,aux[cont]);
}
free(aux);
}
void clear_queue(queue_t * p_queue)
{
while (is_empty(p_queue) != 1)
{
pop_queue(p_queue, NULL);
}
}
void
dijkstra_calc_dest (dijkstra_graph_t *graph, int src, int dest)
{
assert (graph);
if (!graph->initialized) {
memcpy (graph->nodes_cc, graph->nodes, graph->n_nodes * sizeof(node_t));
graph->initialized = 1;
}
if (graph->src == src && graph->dest == dest)
return; // we've already computed this
graph->src = src;
graph->dest = dest;
// reset the heap
memset (graph->heap, 0, graph->n_nodes*sizeof (node_t *));
graph->heap_len = 0;
graph->heap_idx = 0;
// --- Dijkstra stuff; unreachable nodes will never make into the queue ---
memcpy (graph->nodes, graph->nodes_cc, graph->n_nodes * sizeof(node_t));
node_t *start = graph->nodes + src, *goal = graph->nodes + dest, *lead;
set_dist (graph, start, start, 0);
while ((lead = pop_queue (graph)) && lead != goal) {
for (edge_t *e = lead->edge; e; e = e->sibling)
set_dist (graph, e->node, lead, lead->dist + e->weight);
}
}
static void empty_queue(queue the_state_queue)
{
state_ptr state;
while ((state = pop_queue(the_state_queue)) != NULL) {
free(state);
}
}
/* --- Dijkstra stuff; unreachable nodes will never make into the queue --- */
void calc_all(node_t *const start) {
node_t *lead;
edge_t *e;
set_dist(start, start, 0);
while ((lead = pop_queue()))
for (e = lead->edge; e; e = e->sibling)
set_dist(e->nd, lead, lead->dist + e->len);
}
/*---- Session Callbacks end ----*/
static void track_ended(void) {
int tracks = 0;
if(g_currenttrack) {
g_currenttrack = NULL;
sp_session_player_unload(g_sess);
pop_queue();
try_playback_start();
}
}
void calc_all(node_t *start) {
node_t *lead;
edge_t *e;
set_dist(start, start, 0);
while ((lead = pop_queue()))
for (e = lead->edge; e; e = e->sibling)
set_dist(e->destination_node, lead, lead->distance + e->len);
}
TEAM *input_dat(FILE *fp, int *num_team)
{
char line[SIZE_STR], **token;
int i, num_token;
int cmp_pts();
TEAM team_home, team_away, *team_ary;
struct list *team_list, *p;
*num_team = 0;
team_list = init_list();
while (fgets(line, SIZE_STR, fp) != NULL) {
if (line[0] != '#') {
token = tokenize_str(line, "-\t\n", &num_token);
if (num_token >= 4) {
set_home_team(&team_home, token);
set_away_team(&team_away, token);
if ((p = search_team(team_list, team_home.name)) == NULL) {
push_queue(team_list, &team_home, sizeof(team_home));
(*num_team)++;
}
else {
add_team_result(p, team_home);
}
if ((p = search_team(team_list, team_away.name)) == NULL) {
push_queue(team_list, &team_away, sizeof(team_away));
(*num_team)++;
}
else {
add_team_result(p, team_away);
}
}
free_token_ary(token, num_token);
}
}
team_ary = (TEAM *) malloc(sizeof(TEAM) * (*num_team));
if (team_ary == NULL) {
fprintf(stderr, "ERROR: Unable to allocate memory.\n");
exit(EXIT_FAILURE);
}
for (i = 0; i < *num_team; i++) {
pop_queue(team_list, &team_ary[i], sizeof(TEAM));
}
delete_list(team_list);
/* 安定ソートで被ゴール数→ゴール数→得失点差→勝ち点の順で整列すると順位で並ぶ */
/* リーグ戦の規程によってはこのやり方ではダメ */
bubblesort(team_ary, *num_team, sizeof(TEAM), cmp_gag);
bubblesort(team_ary, *num_team, sizeof(TEAM), cmp_gfo);
bubblesort(team_ary, *num_team, sizeof(TEAM), cmp_gdi);
bubblesort(team_ary, *num_team, sizeof(TEAM), cmp_pts);
return team_ary;
}
/* LIFO Based queue test */
void testQ() {
struct queue *my_queue;
my_queue=NULL;
my_queue=alloc_queue(my_queue);
push_queue(my_queue,5);
push_queue(my_queue,10);
push_queue(my_queue,15);
push_queue(my_queue,20);
printf("Popped from queue: %d\n",pop_queue(my_queue));
printf("Popped from queue: %d\n",pop_queue(my_queue));
push_queue(my_queue,35);
printf("Popped from queue: %d\n",pop_queue(my_queue));
printf("Popped from queue: %d\n",pop_queue(my_queue));
}
static void
release_queue(struct msg_queue *queue) {
if (queue == NULL)
return;
struct msg * m;
while ((m=pop_queue(queue)) != NULL) {
skynet_free(m->buffer);
}
skynet_free(queue->data);
skynet_free(queue);
}
static void* APR_THREAD_FUNC seed_thread(apr_thread_t *thread, void *data)
#endif
{
mapcache_tile *tile;
mapcache_context seed_ctx = ctx;
seed_ctx.log = seed_log;
apr_pool_create(&seed_ctx.pool,ctx.pool);
tile = mapcache_tileset_tile_create(ctx.pool, tileset, grid_link);
tile->dimensions = dimensions;
while(1) {
struct seed_cmd cmd;
apr_status_t ret;
apr_pool_clear(seed_ctx.pool);
ret = pop_queue(&cmd);
if(ret != APR_SUCCESS || cmd.command == MAPCACHE_CMD_STOP) break;
tile->x = cmd.x;
tile->y = cmd.y;
tile->z = cmd.z;
if(cmd.command == MAPCACHE_CMD_SEED) {
/* aquire a lock on the metatile ?*/
mapcache_metatile *mt = mapcache_tileset_metatile_get(&seed_ctx, tile);
int isLocked = mapcache_lock_or_wait_for_resource(&seed_ctx, mapcache_tileset_metatile_resource_key(&seed_ctx,mt));
if(isLocked == MAPCACHE_TRUE) {
/* this will query the source to create the tiles, and save them to the cache */
mapcache_tileset_render_metatile(&seed_ctx, mt);
mapcache_unlock_resource(&seed_ctx, mapcache_tileset_metatile_resource_key(&seed_ctx,mt));
}
} else if (cmd.command == MAPCACHE_CMD_TRANSFER) {
int i;
mapcache_metatile *mt = mapcache_tileset_metatile_get(&seed_ctx, tile);
for (i = 0; i < mt->ntiles; i++) {
mapcache_tile *subtile = &mt->tiles[i];
mapcache_tileset_tile_get(&seed_ctx, subtile);
subtile->tileset = tileset_transfer;
tileset_transfer->cache->tile_set(&seed_ctx, subtile);
}
}
else { //CMD_DELETE
mapcache_tileset_tile_delete(&seed_ctx,tile,MAPCACHE_TRUE);
}
if(seed_ctx.get_error(&seed_ctx)) {
error_detected++;
ctx.log(&ctx,MAPCACHE_INFO,seed_ctx.get_error_message(&seed_ctx));
}
}
#ifdef USE_FORK
return 0;
#else
apr_thread_exit(thread,MAPCACHE_SUCCESS);
return NULL;
#endif
}
int main ()
{
int sq, eq, q[50], Max = 50;
initialise_queue(&sq, &eq);
add_to_queue(q, &sq, &eq, Max, 1);
add_to_queue(q, &sq, &eq, Max, 2);
add_to_queue(q, &sq, &eq, Max, 3);
add_to_queue(q, &sq, &eq, Max, 4);
print(q, &sq, &eq);
pop_queue(&sq, &eq, Max);
print(q, &sq, &eq);
printf("\n%d", queue_front(q, &sq));
}
void
expect_queue_to_accept_many_elements(int n_elements) {
int i;
Queue *q = new_queue();
for (i = 0; i < n_elements; i++) {
assert(push_queue(q, i) == 0);
}
for (i = 0; i < n_elements; i++) {
assert(pop_queue(q) == i);
}
destroy_queue(q);
}
vector* breadth_first_traversal(avl_tree* _Tree)
{
queue* _Queue = create_queue();
vector* _Vector = create_vector();
push_queue(_Queue, (void*)_m_root);
while (size_queue(_Queue) != 0)
{
avl_node* _Node = pop_queue(_Queue);
_Vector = _Node->m_value;
if (_Node->m_left_child != 0) push_queue(_Node->m_left_child);
if (_Node->m_right_child != 0) push_queue(_Node->m_right_child);
}
return _Vector;
}
static inline void delete_queue(queue *queue_pointer)
{
queue the_queue;
state_ptr cur_state;
the_queue = *queue_pointer;
if (the_queue == NULL)
return;
while (the_queue->num_elements > 0)
{
cur_state = pop_queue(the_queue);
free(cur_state);
}
if (the_queue->queue_size > 0)
free(the_queue->data_array);
free(the_queue);
queue_pointer = NULL;
}
static void number_connected_verts(struct mds* m, mds_id v,
struct mds_tag* tag, mds_id* label)
{
struct queue q;
struct mds_set adj[2];
int i;
adj[0].n = adj[1].n = 0;
if (!visit(m, tag, label, v))
return;
make_queue(&q, m->n[MDS_VERTEX]);
push_queue(&q, v);
while ( ! queue_empty(&q)) {
v = pop_queue(&q);
mds_get_adjacent(m, v, 1, &adj[1]);
adj[0].n = adj[1].n;
for (i = 0; i < adj[1].n; ++i)
adj[0].e[i] = other_vert(m, adj[1].e[i], v);
for (i = 0; i < adj[0].n; ++i)
if (visit(m, tag, label, adj[0].e[i]))
push_queue(&q, adj[0].e[i]);
}
free_queue(&q);
}
void
expect_push_queue_and_pop_queue_to_work_consistently() {
Queue *q = new_queue();
assert(push_queue(q, 1) == 0);
assert(push_queue(q, 2) == 0);
assert(push_queue(q, 3) == 0);
assert(push_queue(q, 4) == 0);
assert(push_queue(q, 5) == 0);
assert(pop_queue(q) == 1);
assert(pop_queue(q) == 2);
assert(pop_queue(q) == 3);
assert(pop_queue(q) == 4);
assert(pop_queue(q) == 5);
assert(pop_queue(q) == -1);
destroy_queue(q);
}
//*******************************************************************************
int pid_loop()
//Modified on May 8 to take into account a moving average, and a moving variance
//and also to remove the retraction of the piezo except on the first pass.
{
//This is the function to output a PID loop
//PID algorithm taken from Control System Desgin, by Karl Johan Astrom
//Chapter 6
//This algorithm is supposed to include integral wind-up and bumpless transition
int m;
lsampl_t data_to_card, data_from_card;
static comedi_t * dev_output, * dev_input;
static double bi, ad, bd; //PID coefficients
static double Pcontrib, Icontrib, Dcontrib; //individual PID contributions
static double FeedbackReading; //Readings of the error chann
static double v; //u is the actuator output, and v is the calculated output
static int j = 0;
static double LastDiffContrib;
static double Error;
static double LastError =0;
static double SecondLastError =0;
static double LastOutput =0;
//static double SummedPIDOutput; //Summed PID Output
static double SummedFeedbackReading; //Summed FeedbackReading
//static double SummedVariance;
static double M2_n;
static double delta;
static double alpha;
static struct queue PIDOutput_queue;//these are two queues to calculate the moving mean and variance
static struct queue FeedbackReadingVar_queue;
static struct queue FeedbackReading_queue;
static int NumbFirstSteps;
static double InitialStepSizeVoltage = 0.1;
static double InitialVoltageStep;
double last_mean, last_var, new_var; //popped values of mean and variance
//Initialize the queues
init_queue(&PIDOutput_queue);
init_queue(&FeedbackReadingVar_queue);
init_queue(&FeedbackReading_queue);
//rt_printk("Control channel device name is %s \n",device_names[ControlChannel.board_number]);
//rt_printk("Control channel subdevice %d and channel %d \n", ControlChannel.subdevice, ControlChannel.channel);
//rt_printk("Feedback channel device name is %s \n",device_names[FeedbackChannel.board_number]);
//rt_printk("Feedback channel subdevice %d and channel %d \n", FeedbackChannel.subdevice, FeedbackChannel.channel);
//dev_output is the channel that is to be controlled
dev_output = comedi_open(device_names[ControlChannel.board_number]);
//dev_input is the channel from which the error signal is read
dev_input = comedi_open(device_names[FeedbackChannel.board_number]);
//initialize the task
if(!(PIDloop_Task = rt_task_init_schmod(nam2num( "PIDLoop" ), // Name
0, // Priority
0, // Stack Size
0, //, // max_msg_size
SCHED_FIFO, // Policy
CPUMAP ))) // cpus_allowed
{
rt_printk("ERROR: Cannot initialize PIDLoop task\n");
exit(1);
}
//specify that this is to run on one CPU
rt_set_runnable_on_cpuid(PIDloop_Task, 0);
//lock memory and make hard real time
mlockall(MCL_CURRENT|MCL_FUTURE);
rt_make_hard_real_time();
//Convert PIDLoop_time, which is in nanoseconds, to tick time (sampling_interval, in counts)
sampling_interval =nano2count(PIDLoop_Time);
//sampling_interval =nano2count_cpuid(PIDLoop_Time, 0);
// Let's make this task periodic..
expected = rt_get_time() + 100*sampling_interval;
//expected = rt_get_time_cpuid(0) + 100*sampling_interval;
rt_task_make_periodic(PIDloop_Task, expected, sampling_interval); //period in counts
pid_loop_running = 1; //set the pid loop running flag to FALSE
//retract the tip completely, if it is the first PID pass
if(FirstPIDPass)
{
//data_to_card = (lsampl_t) 0;
//MaxZVoltage corresponds to the fully retracted piezo
//rt_printk("MaxZVoltage is %f \n", MaxZVoltage);
//rt_printk("MinZVoltage is %f \n", MinZVoltage);
//rt_printk("MinOutputVoltage is %f \n", MinOutputVoltage);
//rt_printk("PIDOutput is %f \n", PIDOutput);
//rt_printk("AmplifierGainSign is %i \n", AmplifierGainSign);
//rt_printk("OutputPhase is %i \n", OutputPhase);
NumbFirstSteps = (nearbyint((MaxZVoltage-PIDOutput)/InitialStepSizeVoltage))-1;
//rt_printk("NumbFirstSteps is %i \n", NumbFirstSteps);
//NumbFirstSteps = ((MaxZVoltage - PIDOutput)/InitialStepSizeVoltage)); //-1 to be safe
//Set the direction of the voltage step
//PIDOutput = CurrentZVoltage;
if (MaxZVoltage>=PIDOutput)
{InitialVoltageStep=InitialStepSizeVoltage;}
else {InitialVoltageStep=-InitialStepSizeVoltage;};
if (NumbFirstSteps>1)
{
for(j=0;j<NumbFirstSteps;j++)
{ PIDOutput+=InitialVoltageStep;
data_to_card = (lsampl_t) nearbyint(((PIDOutput - MinOutputVoltage)/OutputRange)*MaxOutputBits);
//rt_printk("Data_to_card is %i \n", data_to_card);
comedi_lock(dev_output, ControlChannel.subdevice);
m=comedi_data_write(dev_output, ControlChannel.subdevice, ControlChannel.channel, AO_RANGE, AREF_DIFF, data_to_card);
comedi_unlock(dev_output, ControlChannel.subdevice);
// And wait until the end of the period.
rt_task_wait_period();
}
}
//Initialize the errors
LastError = 0;
SecondLastError = 0;
LastOutput = PIDOutput;
LastDiffContrib =0;
Dcontrib = 0;
Icontrib = 0;
AveragedPIDOutput=LastOutput; //This is what the main program will actually read
FirstPIDPass = 0;
}
//rt_printk("AntiWindup time is %f \n", AntiWindup_Time);
bi = PropCoff*PIDLoop_Time/IntTime; //integral gain
//rt_printk("PropCoff is %f \n", PropCoff);
//rt_printk("IntTime is %f \n", IntTime);
//in Astrom's article, ad is defined as below in the code, but the actual
//derivation gives the coefficient we actually use
//ad = (2*DiffTime- PID_cutoff_N*PIDLoop_Time)/(2*DiffTime+PID_cutoff_N*PIDLoop_Time);
ad = (DiffTime)/(DiffTime+PID_cutoff_N*PIDLoop_Time);
//rt_printk("DiffTime is %f \n", DiffTime);
//same comment about bd
//bd = 2*PropCoff*PID_cutoff_N*DiffTime/(2*DiffTime + PID_cutoff_N*PIDLoop_Time); //derivative gain
bd = PropCoff*PID_cutoff_N*DiffTime/(DiffTime + PID_cutoff_N*PIDLoop_Time);
//rt_printk("MaxZVoltage is %f \n", MaxZVoltage);
//Now calculate the initial means and variances
//SummedPIDOutput = 0; //initialize parameters if we take averages
//First means
SummedFeedbackReading =0;
//j=1;
alpha = ((float) 1)/(PID_averages+1);
for (j=0;j<PID_averages;j++)
{
//make a first reading
comedi_lock(dev_input, FeedbackChannel.subdevice);
m = comedi_data_read(dev_input, FeedbackChannel.subdevice, FeedbackChannel.channel, AI_RANGE, AREF_DIFF, &data_from_card);
comedi_unlock(dev_input, FeedbackChannel.subdevice);
//Convert to a voltage reading
SummedFeedbackReading += ((((float) data_from_card)/MaxInputBits)*InputRange + MinInputVoltage);
}
AveragedFeedbackReading =SummedFeedbackReading/PID_averages;
//Since we are not changing the output, the mean has not changed, and the variance is 0
M2_n = 0;
PIDOutputVariance = 0;
//Initialize the circular buffers
for (j=0; j<PID_averages; j++)
{
push_queue(&FeedbackReading_queue, AveragedFeedbackReading);
push_queue(&FeedbackReadingVar_queue, PIDOutputVariance);
push_queue(&PIDOutput_queue, LastOutput);
}
//Now do the regular loop
while(pid_loop_running)
{
//rt_printk("Got here 1 \n");
//check to see if the PID parameters have changed
if(PIDParametersChanged)
{
//update the PID coefficients
bi = PropCoff*PIDLoop_Time/IntTime; //integral gain
ad = (DiffTime)/(DiffTime+PID_cutoff_N*PIDLoop_Time);
bd = PropCoff*PID_cutoff_N*DiffTime/(DiffTime + PID_cutoff_N*PIDLoop_Time);
PIDParametersChanged = 0;
} //end of if(PIDParametersChanged)
//continue with the rest of the loop
//Read the input reading
comedi_lock(dev_input, FeedbackChannel.subdevice);
m = comedi_data_read(dev_input, FeedbackChannel.subdevice, FeedbackChannel.channel, AI_RANGE, AREF_DIFF, &data_from_card);
comedi_unlock(dev_input, FeedbackChannel.subdevice);
//Convert to a voltage reading
FeedbackReading = ((((float) data_from_card)/MaxInputBits)*InputRange + MinInputVoltage);
//rt_printk("Data from card is %d \n", data_from_card);
//rt_printk("Feedback reading is %f \n", FeedbackReading);
//rt_printk("Input m is %d \n", m);
delta = (FeedbackReading - AveragedFeedbackReading);
//AveragedFeedbackReading = alpha*FeedbackReading+(1-alpha)*AveragedFeedbackReading; //running averange
//PIDOutputVariance = alpha*(delta*delta) + (1-alpha)*PIDOutputVariance;
//Venkat changed the following line to add logarithmic averaging on January 10, 2012
if(Logarithmic){
Error = AmplifierGainSign*OutputPhase*log10(fabs(FeedbackReading/SetPoint));
}
else {
Error = AmplifierGainSign*OutputPhase*(SetPoint - FeedbackReading);//multiply by OutputPhase+AmplifierGainSign
}
//Error = AmplifierGainSign*OutputPhase*(SetPoint - FeedbackReading);//multiply by OutputPhase+AmplifierGainSign
Pcontrib = PropCoff*(Error - LastError);
//Not sure of sign of second contribution in line below...should it be - ?
Dcontrib = ad*LastDiffContrib - bd*(Error - 2*LastError + SecondLastError);
v = LastOutput + Pcontrib + Icontrib + Dcontrib;
//next, take care of saturation of the output....anti-windup
PIDOutput = v;
PIDOutput =(PIDOutput>MaxOutputVoltage)? MaxOutputVoltage:PIDOutput;
PIDOutput =(PIDOutput<MinOutputVoltage)? MinOutputVoltage:PIDOutput;
//Calculate the averaged quantities
pop_queue(&FeedbackReading_queue, &last_mean);
AveragedFeedbackReading += (FeedbackReading - last_mean)/PID_averages;
push_queue(&FeedbackReading_queue, FeedbackReading);
pop_queue(&FeedbackReadingVar_queue, &last_var);
new_var = delta*delta;
PIDOutputVariance += (new_var - last_var)/PID_averages;
push_queue(&FeedbackReadingVar_queue, new_var);
//send the control signal
//rt_printk("FeedbackReading is %f \n", FeedbackReading);
//rt_printk("v is %f \n", v);
//rt_printk("PID output should be %f \n", PIDOutput);
data_to_card = (lsampl_t) nearbyint(((PIDOutput - MinOutputVoltage)/OutputRange)*MaxOutputBits);
//data_to_card = (lsampl_t) 0;
comedi_lock(dev_output, ControlChannel.subdevice);
m=comedi_data_write(dev_output, ControlChannel.subdevice, ControlChannel.channel, AO_RANGE, AREF_DIFF, data_to_card);
comedi_unlock(dev_output, ControlChannel.subdevice);
//rt_printk("Output m is %d \n", m);
//Update the integral contribution after the loop
Icontrib = bi*Error;
//Update parameters
LastError = Error;
SecondLastError = LastError;
LastDiffContrib = Dcontrib;
LastOutput = PIDOutput;
//rt_printk("PContrib is %f \n", Pcontrib);
//rt_printk("IContrib is %f \n", Icontrib);
//rt_printk("DContrib is %f \n", Dcontrib);
//rt_printk("PIDOutput is %f \n", PIDOutput);
//Next part is to take the averaged PID output for recording if j>PID_averages and PID_averages>1
//SummedPIDOutput+=PIDOutput;
//SummedFeedbackReading += FeedbackReading;
//j++;
//AveragedPIDOutput=((PID_averages>1)&&(j>PID_averages))?(SummedPIDOutput/PID_averages):AveragedPIDOutput;
//AveragedFeedbackReading=((PID_averages>1)&&(j>PID_averages))?(SummedFeedbackReading/PID_averages):AveragedFeedbackReading;
//SummedPIDOutput=(j>PID_averages)? 0:SummedPIDOutput;
//SummedFeedbackReading=(j>PID_averages)? 0:SummedFeedbackReading;
//j=(j>PID_averages)? 1:j;
//Calculate moving exponential averages and variance
//delta = PIDOutput - AveragedPIDOutput;
//AveragedPIDOutput = alpha*PIDOutput + (1-alpha)*AveragedPIDOutput;
//PIDOutputVariance = alpha*(delta*delta) + (1-alpha)*PIDOutputVariance;
//PIDOutputVariance = alpha*abs(delta) + (1-alpha)*PIDOutputVariance;
pop_queue(&PIDOutput_queue, &last_mean);
AveragedPIDOutput += (PIDOutput - last_mean)/PID_averages;
push_queue(&PIDOutput_queue, PIDOutput);
// And wait until the end of the period.
rt_task_wait_period();
}
//rt_printk("Got here 3 \n");
//rt_printk("pid_loop_running is %d \n", pid_loop_running);
rt_make_soft_real_time();
comedi_close(dev_input);
comedi_close(dev_output);
rt_task_delete(PIDloop_Task); //Self termination at end.
pthread_exit(NULL);
return 0;
}
void QContentHubServer::dispatch(msgpack::rpc::request req) {
try {
std::string method;
req.method().convert(&method);
if(method == "push") {
msgpack::type::tuple<std::string, std::string> params;
req.params().convert(¶ms);
push_queue(req, params.get<0>(), params.get<1>());
} else if(method == "pop") {
msgpack::type::tuple<std::string> params;
req.params().convert(¶ms);
pop_queue(req, params.get<0>());
} else if(method == "push_nowait") {
msgpack::type::tuple<std::string, std::string> params;
req.params().convert(¶ms);
push_queue_nowait(req, params.get<0>(), params.get<1>());
} else if(method == "pop_nowait") {
msgpack::type::tuple<std::string> params;
req.params().convert(¶ms);
pop_queue_nowait(req, params.get<0>());
} else if(method == "add") {
msgpack::type::tuple<std::string, int> params;
req.params().convert(¶ms);
add_queue(req, params.get<0>(), params.get<1>());
/*
} else if(method == "del") {
msgpack::type::tuple<std::string> params;
req.params().convert(¶ms);
del_queue(req, params.get<0>());
} else if(method == "fdel") {
msgpack::type::tuple<std::string> params;
req.params().convert(¶ms);
force_del_queue(req, params.get<0>());
*/
} else if(method == "set_capacity") {
msgpack::type::tuple<std::string, int> params;
req.params().convert(¶ms);
set_queue_capacity(req, params.get<0>(), params.get<1>());
} else if(method == "start") {
msgpack::type::tuple<std::string> params;
req.params().convert(¶ms);
start_queue(req, params.get<0>());
} else if(method == "stop") {
msgpack::type::tuple<std::string> params;
req.params().convert(¶ms);
stop_queue(req, params.get<0>());
} else if(method == "clear") {
msgpack::type::tuple<std::string> params;
req.params().convert(¶ms);
clear_queue(req, params.get<0>());
} else if(method == "stats") {
stats(req);
} else if(method == "stat_queue") {
msgpack::type::tuple<std::string> params;
req.params().convert(¶ms);
stat_queue(req, params.get<0>());
} else {
req.error(msgpack::rpc::NO_METHOD_ERROR);
}
} catch (msgpack::type_error& e) {
req.error(msgpack::rpc::ARGUMENT_ERROR);
return;
} catch (std::exception& e) {
req.error(std::string(e.what()));
return;
}
}
double farmer(int numprocs) {
MPI_Status status;
int i, flag, source, w_id;
double result, incoming[5], *derp;
int w_out[numprocs-1];
// Set up stack
stack *stack = new_stack();
double data[5] = {A, B, F(A), F(B), (F(A)+F(B)) * (B-A)/2};
push(data, stack);
// Set up queue
queue *queue = new_queue();
for (i=1; i<numprocs; i++) {
push_queue(i, queue);
w_out[i-1] = 0;
}
while (1) {
if (!is_empty(stack)) {
derp = pop(stack);
w_id = pop_queue(queue);
w_out[w_id] = 1;
MPI_Send(derp, 5, MPI_DOUBLE, w_id, TAG_DO_TASK, MPI_COMM_WORLD);
tasks_per_process[w_id]++;
}
// peek for messages
MPI_Iprobe(MPI_ANY_SOURCE, MPI_ANY_TAG, MPI_COMM_WORLD, &flag, &status);
// if there is a message
if (flag) {
switch(status.MPI_TAG) {
case TAG_ADD_TASK:
// receive data and push onto stack
source = status.MPI_SOURCE;
MPI_Recv(&incoming, 5, MPI_DOUBLE, source, TAG_ADD_TASK, MPI_COMM_WORLD, &status);
push(incoming, stack);
if (w_out[source]) {
push_queue(source, queue);
w_out[source] = 0;
}
break;
case TAG_RESULT:
source = status.MPI_SOURCE;
MPI_Recv(&incoming, 5, MPI_DOUBLE, source, TAG_RESULT, MPI_COMM_WORLD, &status);
result += incoming[4];
if (w_out[source]) {
push_queue(source, queue);
w_out[source] = 0;
}
break;
}
}
// ready to finish?
if (workers_available(queue) == numprocs-1 && is_empty(stack)) { break; }
}
// kill and free
for (i=1; i<numprocs; i++) {
MPI_Send(&data, 5, MPI_DOUBLE, i, TAG_KILL, MPI_COMM_WORLD);
}
free_stack(stack);
free_queue(queue);
return result;
}
void
carmen_conventional_dynamic_program(int goal_x, int goal_y)
{
double *utility_ptr;
int index;
double max_val, min_val;
int num_expanded;
int done;
struct timeval start_time, end_time;
int delta_sec, delta_usec;
static double last_time, cur_time, last_print;
queue state_queue;
state_ptr current_state;
if (costs == NULL)
return;
gettimeofday(&start_time, NULL);
cur_time = carmen_get_time();
if ((int)(cur_time - last_print) > 10) {
carmen_verbose("Time since last DP: %d secs, %d usecs\n", (int)(cur_time - last_time),
((int)((cur_time-last_time)*1e6)) % 1000000);
last_print = cur_time;
}
last_time = cur_time;
if (utility == NULL) {
utility = (double *)calloc(x_size*y_size, sizeof(double));
carmen_test_alloc(utility);
}
utility_ptr = utility;
for (index = 0; index < x_size * y_size; index++)
*(utility_ptr++) = -1;
if (is_out_of_map(goal_x, goal_y))
return;
max_val = -MAXDOUBLE;
min_val = MAXDOUBLE;
done = 0;
state_queue = make_queue();
current_state = carmen_conventional_create_state(goal_x, goal_y, 0);
max_val = MAX_UTILITY;
*(utility_value(goal_x, goal_y)) = max_val;
add_neighbours_to_queue(goal_x, goal_y, state_queue);
num_expanded = 1;
while ((current_state = pop_queue(state_queue)) != NULL) {
num_expanded++;
if (current_state->cost <= *(utility_value(current_state->x, current_state->y)))
add_neighbours_to_queue(current_state->x, current_state->y, state_queue);
if (current_state->cost < min_val)
min_val = current_state->cost;
free(current_state);
}
delete_queue(&state_queue);
gettimeofday(&end_time, NULL);
delta_usec = end_time.tv_usec - start_time.tv_usec;
delta_sec = end_time.tv_sec - start_time.tv_sec;
if (delta_usec < 0) {
delta_sec--;
delta_usec += 1000000;
}
// carmen_warn("Elasped time for dp: %d secs, %d usecs\n", delta_sec, delta_usec);
}
/*
* This function executes in signal handler context
*/
static void timerhandler(int whatever){
// printf("timer tick\n");
if (clock_state == TIMER_STOPPED) return;
long invalbuf[NUM_PAGES];
int numinval = 0;
int posval;
clock_stop();
for (int i = 0; i < NumNode; ++i){
numinval = 0;
for (int curr = activehead, last = curr;curr != -1;
last = curr,curr = req_queues[curr].next_active){
if (curr == activehead){
last = curr;
}
if (curr_owners[curr] != i){
continue;
}
q_dta_t* the_queue = &(req_queues[curr]);
if (the_queue->update_pending){
continue;
}
if (!the_queue->num_writers) continue;
if (popped[curr]) continue;
//better be true
assert(the_queue->num_writers);
qaddr = pagenum_to_addr(dsm_heaptop, curr, dsm_pagesize);
int sessionfd = pop_queue(the_queue);
popped[curr] = 1;
if (!READ_REQ(sessionfd)){
the_queue->num_writers--;
//was writing; need to wait for update
if (the_queue->listlen && !the_queue->num_writers){
for (int i = 0;i < the_queue->listlen;i++){
the_queue->num_parallel_readers++;
}
}
the_queue->update_pending = 1;
}else{
if (the_queue->num_parallel_readers > 0){
//read requests granted before any writers came along
//always at front of queue
the_queue->num_parallel_readers --;
}
if (the_queue->listlen && !the_queue->num_parallel_readers){
int nextsessionfd = queue_top(the_queue);
dsm_send(0, dsm_pagesize, SERVER_PAGE,(char*)pagenum_to_addr(dsm_heaptop,curr,dsm_pagesize) , ABS(nextsessionfd));
}
}
if ((!the_queue->listlen) || ABS(queue_top(the_queue)) != nid_to_sessionfd[i]){
invalbuf[numinval++] = curr * dsm_pagesize + dsm_heaptop;
}
if (the_queue->listlen){
//transfer ownership
int tmp;
posval = queue_top(the_queue);
curr_owners[curr] = (short)(long)hash_get((void*)(long)ABS(posval),sessionfd_to_nid,&tmp);
assert(tmp);
}else{
curr_owners[curr] = -1;
}
if (!the_queue->num_writers){
if (the_queue->listlen){
the_queue->q_state = QUEUE_READERS;
}else{
the_queue->q_state = QUEUE_EMPTY;
}
//take off the active list
if (curr == last){
//delete from head
activehead = req_queues[curr].next_active;
}else{
req_queues[last].next_active = req_queues[curr].next_active;
curr = last;
}
}
}
//TODO: send invalidation message to client
if (numinval) {
//printf("about to send %d inv to %d\n",numinval, nid_to_sessionfd[i]);
dsm_send(0, sizeof(long)*numinval,SERVER_INVALIDATE, (char*)invalbuf, nid_to_sessionfd[i]);
}
}
if (activehead != -1){
clock_start();
}
our_memset (popped, 0, sizeof(char)*NUM_PAGES);
//printf("END timer tick\n");
}
/**
* @name best_first_search
*
* Find the best segmentation by doing a best first search of the
* solution space.
*/
void Wordrec::best_first_search(CHUNKS_RECORD *chunks_record,
WERD_CHOICE *best_choice,
WERD_CHOICE *raw_choice,
STATE *state,
DANGERR *fixpt,
STATE *best_state) {
SEARCH_RECORD *the_search;
inT16 keep_going;
STATE guided_state; // not used
num_joints = chunks_record->ratings->dimension() - 1;
the_search = new_search(chunks_record, num_joints,
best_choice, raw_choice, state);
// The default state is initialized as the best choice. In order to apply
// segmentation adjustment, or any other contextual processing in permute,
// we give the best choice a poor rating to force the processed raw choice
// to be promoted to best choice.
the_search->best_choice->set_rating(100000.0);
evaluate_state(chunks_record, the_search, fixpt);
if (permute_debug) {
tprintf("\n\n\n =========== BestFirstSearch ==============\n");
best_choice->print("**Initial BestChoice**");
}
#ifndef GRAPHICS_DISABLED
save_best_state(chunks_record);
#endif
start_recording();
FLOAT32 worst_priority = 2.0f * prioritize_state(chunks_record, the_search);
if (worst_priority < wordrec_worst_state)
worst_priority = wordrec_worst_state;
if (segment_debug) {
print_state("BestFirstSearch", best_state, num_joints);
}
guided_state = *state;
do {
/* Look for answer */
if (!hash_lookup (the_search->closed_states, the_search->this_state)) {
if (tord_blob_skip) {
free_state (the_search->this_state);
break;
}
guided_state = *(the_search->this_state);
keep_going = evaluate_state(chunks_record, the_search, fixpt);
hash_add (the_search->closed_states, the_search->this_state);
if (!keep_going ||
(the_search->num_states > wordrec_num_seg_states) ||
(tord_blob_skip)) {
if (segment_debug)
tprintf("Breaking best_first_search on keep_going %s numstates %d\n",
((keep_going) ? "T" :"F"), the_search->num_states);
free_state (the_search->this_state);
break;
}
FLOAT32 new_worst_priority = 2.0f * prioritize_state(chunks_record,
the_search);
if (new_worst_priority < worst_priority) {
if (segment_debug)
tprintf("Lowering WorstPriority %f --> %f\n",
worst_priority, new_worst_priority);
// Tighten the threshold for admitting new paths as better search
// candidates are found. After lowering this threshold, we can safely
// popout everything that is worse than this score also.
worst_priority = new_worst_priority;
}
expand_node(worst_priority, chunks_record, the_search);
}
free_state (the_search->this_state);
num_popped++;
the_search->this_state = pop_queue (the_search->open_states);
if (segment_debug && !the_search->this_state)
tprintf("No more states to evalaute after %d evals", num_popped);
}
while (the_search->this_state);
state->part1 = the_search->best_state->part1;
state->part2 = the_search->best_state->part2;
stop_recording();
if (permute_debug) {
tprintf("\n\n\n =========== BestFirstSearch ==============\n");
// best_choice->debug_string(getDict().getUnicharset()).string());
best_choice->print("**Final BestChoice**");
}
// save the best_state stats
delete_search(the_search);
}
void* consumerThread(void *args) {
int id = *((int *)args);
double random;
struct drand48_data randBuffer;
int sock;
QElement *elmnt;
ssize_t rcvBytes;
int type;
int ret;
HttpGet httpGet;
ssize_t totalSize;
char *savedBuffer;
int lenBuffer = sizeof(char) * BUFSIZE;
char *buffer;
srand48_r(time(NULL), &randBuffer);
while (1) {
totalSize = 0;
pthread_mutex_lock(&queue_mutex);
while (hasPending == 0) {
LOG_INFO("Consumer[%d] is waiting.\n", id);
pthread_cond_wait(&queue_cond, &queue_mutex);
}
if (queue_clients.size > 0 && queue_webservers.size > 0) {
drand48_r(&randBuffer, &random);
if (random < 0.5) {
elmnt = pop_queue(&queue_clients);
} else {
elmnt = pop_queue(&queue_webservers);
}
} else if (queue_clients.size > 0) {
elmnt = pop_queue(&queue_clients);
} else {
elmnt = pop_queue(&queue_webservers);
}
hasPending--;
if (hasPending > 0) {
pthread_cond_signal(&queue_cond);
}
pthread_mutex_unlock(&queue_mutex);
buffer = (char *) malloc(lenBuffer);
if (buffer == NULL) {
LOG_ERROR("Not enough free memory space\n");
return NULL;
}
sock = elmnt->clSock;
free(elmnt);
rcvBytes = recv(sock, buffer, lenBuffer, 0);
if (rcvBytes < 0) {
LOG_ERROR("Consumer[%d]: An error occurred while receiving data from the client.\n", id);
continue;
} else if (rcvBytes == 0) {
LOG_ERROR("Consumer[%d]: The client has performed a shutdown.\n", id);
continue;
}
totalSize += rcvBytes;
type = findHttpMessageType(buffer, rcvBytes);
if (type == HTTP_GET) {
int webSock;
LOG_INFO("Consumer[%d]: Get Message received\n", id);
ret = parseHttpGetMsg(buffer, rcvBytes, &httpGet);
if (ret == HTTP_PARSER_ERROR || ret == NO_MEMORY_ERROR) {
sendHttpBadReqMsg(sock);
} else if (ret == HTTP_HOST_NOT_FOUND){
sendHttpNotFoundMsg(sock);
} else if (ret == OK) {
char* response = get_response_cache(cache, httpGet.request, httpGet.reqLen);
if (response != NULL) {
LOG_INFO("\tConsumer[%d]: Cache Hit\n", id);
update_cache(cache, httpGet.request, httpGet.reqLen);
ret = sendSingleHttpResponseMsg(response, strlen(response), sock);
if (ret != OK) {
LOG_ERROR("Consumer[%d]: Unable to send the response Message\n", id);
}
} else {
LOG_INFO("\tConsumer[%d]: No Cache Hit\n", id);
if (contains_request(httpGet.request, httpGet.reqLen) != FOUND) {
LOG_INFO("\tConsumer[%d]: Does not contain that request\n", id);
webSock = sendHttpGetMsgToServer(&httpGet);
if (webSock < 0) {
LOG_ERROR("Consumer[%d]: Did not send the HttpGetMessage to the Webserver\n", id);
} else {
if (save_request(httpGet.request, httpGet.reqLen, webSock, sock) != OK) {
LOG_ERROR("Consumer[%d]: An error occurred while saving the request\n", id);
}
if (insertFD(webSock) != OK) {
LOG_ERROR("Consumer[%d]: An error occurred while saving the socket\n", id);
}
}
} else {
LOG_INFO("\tConsumer[%d]: Does contain that request\n", id);
webSock = get_request(httpGet.request, httpGet.reqLen);
}
if (map_client_with_webserver(webSock, sock) != OK) {
LOG_ERROR("Consumer[%d]: An error occurred while making the mapping between client's socket and webserver's one.\n", id);
}
}
clear_httpGetMsg(&httpGet);
}
} else if (type == HTTP_RESPONSE) {
LOG_INFO("Consumer[%d]: Response Message received\n", id);
Queue* clSocks = find_appropriate_clients(sock);
if (clSocks == NULL) {
LOG_ERROR("Consumer[%d]: Could not find the appropriate clients.\n", id);
continue;
}
savedBuffer = (char *) malloc(sizeof(char) * rcvBytes);
if (savedBuffer == NULL) {
LOG_ERROR("Consumer[%d]: Not enough free memory space.\n", id);
continue;
}
memcpy(savedBuffer, buffer, rcvBytes);
char *t;
ssize_t prevSize = rcvBytes;
memset(buffer, 0, lenBuffer);
while ((rcvBytes = recv(sock, buffer, lenBuffer, 0)) != 0) {
totalSize += rcvBytes;
t = realloc(savedBuffer, totalSize);
if (t == NULL) {
LOG_ERROR("Consumer[%d]: Not enough free memory space.\n", id);
break;
}
savedBuffer = t;
memcpy(savedBuffer + prevSize, buffer, rcvBytes);
prevSize = totalSize;
}
QElement *iter;
char *req;
int clSock = -1;
for (iter = clSocks->head; iter != NULL; iter = iter->next) {
req = get_hashmap(&cl_req_map, &iter->clSock, sizeof(int));
if (req != NULL) {
if (put_response_cache(cache, req, strlen(req) + 1, savedBuffer, totalSize) != OK) {
LOG_ERROR("Consumer[%d]: Could not insert it in the cache\n", id);
}
clSock = iter->clSock;
break;
}
}
ret = sendHttpResponseMsg(savedBuffer, totalSize, clSocks);
if (ret != OK) {
LOG_ERROR("Consumer[%d]: Unable to send the response Message\n", id);
}
if (remove_appropiate_clients(sock) != OK) {
LOG_ERROR("Consumer[%d]: Could not remove the appropriate clients from the hash map\n", id);
}
if (remove_request(sock, clSock) != OK) {
LOG_ERROR("Consumer[%d]: Could not remove the request from the hash map\n", id);
}
if (savedBuffer != NULL) {
free(savedBuffer);
}
} else {
LOG_INFO("Consumer[%d]: Unknown type of message\n", id);
sendHttpBadReqMsg(sock);
}
free(buffer);
}
return NULL;
}
/* 清空队列 */
void clear_queue(Queue *q)
{
while(is_queue_empty(q) != 1){
pop_queue(q);
}
}
static void search(carmen_roadmap_vertex_t *start_node,
carmen_roadmap_t *roadmap)
{
int num_expanded;
state_ptr current_state;
int i, j;
double *fwd_utilities, *open_list;
int *parent_ptrs;
carmen_roadmap_vertex_t *node_list;
carmen_roadmap_edge_t *edges;
double min_neighbour_utility;
int min_neighbour, neighbour_id;
int min_edge;
double best_utility, utility;
int best_neighbour;
queue the_state_queue = NULL;
the_state_queue = make_queue();
node_list = (carmen_roadmap_vertex_t *)(roadmap->nodes->list);
fwd_utilities = (double *)calloc(roadmap->nodes->length, sizeof(double));
carmen_test_alloc(fwd_utilities);
for (i = 0; i < roadmap->nodes->length; i++)
fwd_utilities[i] = FLT_MAX;
open_list = (double *)calloc(roadmap->nodes->length, sizeof(double));
carmen_test_alloc(open_list);
for (i = 0; i < roadmap->nodes->length; i++)
open_list[i] = -1;
parent_ptrs = (int *)calloc(roadmap->nodes->length, sizeof(int));
carmen_test_alloc(parent_ptrs);
for (i = 0; i < roadmap->nodes->length; i++)
parent_ptrs[i] = -1;
push_state(start_node->id, start_node->id, 0, 0, the_state_queue);
fwd_utilities[start_node->id] = 0;
node_list[roadmap->goal_id].utility = 0;
num_expanded = 0;
while ((current_state = pop_queue(the_state_queue)) != NULL) {
num_expanded++;
if (0)
carmen_warn("Popped %d\n", current_state->id);
fwd_utilities[current_state->id] =
fwd_utilities[current_state->parent_id] + current_state->cost;
parent_ptrs[current_state->id] = current_state->parent_id;
open_list[current_state->id] = -2;
assert (fwd_utilities[current_state->id] < 1e5);
if (current_state->id == roadmap->goal_id) {
if (node_list[roadmap->goal_id].edges->length == 0)
construct_edges(roadmap, roadmap->goal_id);
empty_queue(the_state_queue);
} else {
add_neighbours_to_queue(roadmap, current_state->id,
roadmap->goal_id, fwd_utilities, open_list,
the_state_queue);
}
free(current_state);
}
carmen_warn("Num Expanded %d\n", num_expanded);
if (fwd_utilities[roadmap->goal_id] > FLT_MAX/2)
carmen_warn("PROBLEM\n");
else if (0) {
node_list[roadmap->goal_id].utility = 0;
i = roadmap->goal_id;
while (i != start_node->id) {
edges = (carmen_roadmap_edge_t *)(node_list[i].edges->list);
assert (node_list[i].edges->length > 0);
min_neighbour_utility = FLT_MAX;
min_neighbour = -1;
min_edge = -1;
for (j = 0; j < node_list[i].edges->length; j++) {
if (edges[j].cost > 1e5 || edges[j].blocked)
continue;
neighbour_id = edges[j].id;
assert (fwd_utilities[neighbour_id] >= 0);
if (fwd_utilities[neighbour_id] < min_neighbour_utility) {
min_neighbour_utility = fwd_utilities[neighbour_id];
min_neighbour = neighbour_id;
min_edge = j;
}
}
assert (min_neighbour >= 0);
assert (min_neighbour_utility < fwd_utilities[i]);
assert(node_list[min_neighbour].utility > FLT_MAX/2 ||
node_list[min_neighbour].utility ==
node_list[i].utility + edges[min_edge].cost);
if (node_list[min_neighbour].utility > FLT_MAX/2) {
node_list[min_neighbour].utility = node_list[i].utility +
edges[min_edge].cost;
assert(node_list[i].utility < FLT_MAX/2);
assert(fwd_utilities[i] < FLT_MAX/2);
assert(fwd_utilities[min_neighbour] < FLT_MAX/2);
assert(node_list[min_neighbour].utility < 1e5);
}
assert (node_list[i].utility < node_list[min_neighbour].utility);
// carmen_warn("%d %d %f %f\n", node_list[i].x, node_list[i].y, node_list[i].utility, FLT_MAX/2);
i = min_neighbour;
assert(node_list[i].utility < FLT_MAX/2);
}
} else {
node_list[roadmap->goal_id].utility = 0;
i = roadmap->goal_id;
while (i != start_node->id) {
min_neighbour = parent_ptrs[i];
assert (min_neighbour >= 0);
min_neighbour_utility = fwd_utilities[min_neighbour];
assert (min_neighbour_utility < fwd_utilities[i]);
edges = (carmen_roadmap_edge_t *)(node_list[i].edges->list);
assert (node_list[i].edges->length > 0);
for (min_edge = 0; min_edge < node_list[i].edges->length; min_edge++) {
if (edges[min_edge].id == min_neighbour)
break;
}
assert (min_edge < node_list[i].edges->length);
assert(node_list[min_neighbour].utility > FLT_MAX/2 ||
fabs(node_list[min_neighbour].utility -
(node_list[i].utility + edges[min_edge].cost)) < 1e6);
if (node_list[min_neighbour].utility > FLT_MAX/2) {
carmen_roadmap_edge_t *edges2;
edges2 = (carmen_roadmap_edge_t *)node_list[min_neighbour].edges->list;
node_list[min_neighbour].utility = node_list[i].utility +
edges[min_edge].cost;
best_neighbour = -1;
best_utility = FLT_MAX;
for (j = 0; j < node_list[min_neighbour].edges->length; j++) {
if (edges2[j].cost > 1e5 || edges2[j].blocked)
continue;
if (node_list[edges2[j].id].utility > FLT_MAX/2)
continue;
utility = node_list[edges2[j].id].utility;
if (utility < best_utility) {
best_utility = utility;
best_neighbour = edges2[j].id;
}
}
if (best_neighbour < 0)
continue;
assert (best_utility < FLT_MAX/2);
assert (node_list[best_neighbour].utility <
node_list[min_neighbour].utility);
assert(node_list[i].utility < FLT_MAX/2);
assert(fwd_utilities[i] < FLT_MAX/2);
assert(fwd_utilities[min_neighbour] < FLT_MAX/2);
assert(node_list[min_neighbour].utility < 1e5);
}
assert (node_list[i].utility < node_list[min_neighbour].utility);
// carmen_warn("%d %d %f %f\n", node_list[i].x, node_list[i].y, node_list[i].utility, FLT_MAX/2);
i = min_neighbour;
assert(node_list[i].utility < FLT_MAX/2);
}
}
/* Check to make sure that no node is a local minimum. The only node that's
allowed to be a local minimum is the goal.
*/
for (i = 0; i < roadmap->nodes->length; i++) {
if (i == roadmap->goal_id)
continue;
edges = (carmen_roadmap_edge_t *)(node_list[i].edges->list);
if (node_list[i].utility > FLT_MAX/2)
continue;
best_neighbour = -1;
best_utility = FLT_MAX;
for (j = 0; j < node_list[i].edges->length; j++) {
if (edges[j].cost > 1e5 || edges[j].blocked)
continue;
if (node_list[edges[j].id].utility > FLT_MAX/2)
continue;
utility = node_list[edges[j].id].utility;
if (utility < best_utility) {
best_utility = utility;
best_neighbour = edges[j].id;
}
}
if (best_neighbour < 0)
continue;
assert (best_utility < FLT_MAX/2);
assert (node_list[best_neighbour].utility < node_list[i].utility);
}
free(fwd_utilities);
delete_queue(&the_state_queue);
}
