void
SSLGamePlanner::run ()
{
init ();
// runPlan (curr_plan_index_);
//ros::Rate r(1000/ssl::config::TIME_STEP);
while (nh_.ok ())
{
//if global_state is updated and path_planner is connected
// then make a plan
if (update ())
{
evaluate ();
std::cout << "**" << std::endl;
std::cout << "allowed_near_ball? " << ssl_game_state.allowedNearBall () << std::endl;
std::cout << "can move? " << ssl_game_state.canMove () << std::endl;
std::cout << "can kick ball? " << ssl_game_state.canKickBall () << std::endl;
std::cout << "--" << std::endl;
runPlan (curr_plan_index_);
if (isPlanChanged () || isPoseControlUpdated ())
{
send ();
prev_plan_index_ = curr_plan_index_;
}
}
ros::spinOnce ();
//r.sleep();
}
}
/** \brief The main function for the worker threads, which determines what they run and what inputs they receive.
\param p Pointer to the ThreadHandle struct that holds the information for the worker thread
*/
void *WorkerThread(void *p){
long cpucoreid, numcpucores;
int init_flag, run_flag, perf_flag;
int one = 1;
data sleep_pass[1];
sleep_pass->i = &one;
ThreadHandle *MyHandle = (ThreadHandle *)p;
Plan *WorkerPlan, *BossPlan;
#ifdef LINUX_PLACEMENT
int affin_flag;
cpucoreid = sched_getcpu();
//syscall(__NR_getcpu(&cpucoreid, NULL, NULL));
numcpucores = sysconf(_SC_NPROCESSORS_ONLN);
EmitLog(MyRank, MyHandle->Num, "Starting on processor core", cpucoreid, PRINT_SOME);
#else
cpucoreid = -1;
numcpucores = -1;
EmitLog(MyRank, MyHandle->Num, "Starting...", -1, PRINT_SOME);
#endif
WorkerPlan = NULL;
for(;; ){
pthread_rwlock_rdlock( &(MyHandle->Lock) );
BossPlan = MyHandle->Plan;
pthread_rwlock_unlock( &(MyHandle->Lock) );
if(BossPlan == NULL){
if(DO_PERF){
EmitLog(MyRank, MyHandle->Num, "Printing performance data.", -1, PRINT_SOME);
perf_flag = perfPlan(WorkerPlan);
if(perf_flag != ERR_CLEAN){
EmitLog(MyRank, MyHandle->Num, "Performance recording error flag triggered, error number:", perf_flag, PRINT_SOME);
}
} //DO_PERF
EmitLog(MyRank, MyHandle->Num, "Thread exiting", -1, PRINT_SOME);
WorkerPlan = killPlan(WorkerPlan); /* clean up old plan */
pthread_exit((void *)0);
} else {
if(BossPlan != WorkerPlan){ /* if the plan was updated */
if((WorkerPlan != NULL) && (WorkerPlan->name != SLEEP) ){
if(DO_PERF){
EmitLog(MyRank, MyHandle->Num, "Printing performance data.", -1, PRINT_SOME);
perf_flag = perfPlan(WorkerPlan);
if(perf_flag != ERR_CLEAN){
EmitLog(MyRank, MyHandle->Num, "Performance recording error flag triggered, error number:", perf_flag, PRINT_SOME);
}
} //DO_PERF
}
#ifdef LINUX_PLACEMENT
numcpucores = sysconf(_SC_NPROCESSORS_ONLN);
cpucoreid = sched_getcpu();
//syscall(__NR_getcpu(&cpucoreid, NULL, NULL));
cpu_set_t cpuset;
affin_flag = pthread_getaffinity_np(MyHandle->ID, sizeof(cpu_set_t), &cpuset);
if(affin_flag != 0){
add_error(MyHandle,SYSTEM,2);
}
EmitLog(MyRank, MyHandle->Num, "New plan detected. Switching plans on core", cpucoreid, PRINT_SOME);
if(PRINT_RARELY <= verbose_flag){
int i;
EmitLog(MyRank, MyHandle->Num, "CPU set for this thread:", -1, PRINT_SOME);
printf("\t\t\t\t");
for(i = 0; i < numcpucores; i++){
if(CPU_ISSET(i, &cpuset)){
printf(" %d", i);
}
}
printf("\n");
}
#else /* ifdef LINUX_PLACEMENT */
EmitLog(MyRank, MyHandle->Num, "New plan detected. Switching plans.", -1, PRINT_SOME);
#endif /* ifdef LINUX_PLACEMENT */
WorkerPlan = killPlan(WorkerPlan); /* clean up old plan */
WorkerPlan = BossPlan; /* switch plans */
init_flag = InitPlan(WorkerPlan); /* initialize new plan */
if(init_flag != ERR_CLEAN){
add_error(MyHandle, WorkerPlan->name,init_flag);
EmitLog(MyRank, MyHandle->Num, "Initialization error flag triggered, error number:", init_flag, PRINT_ALWAYS);
MyHandle->Plan = (plan_list[SLEEP]->make)(sleep_pass);
continue;
}
}
run_flag = runPlan(WorkerPlan);
if(run_flag != ERR_CLEAN){
add_error(MyHandle, WorkerPlan->name,run_flag);
//MyHandle->Plan = (plan_list[SLEEP]->make)(sleep_pass);
EmitLog(MyRank, MyHandle->Num, "Runtime error flag triggered, error number:", run_flag, PRINT_ALWAYS);
}
}
}
return((void *)0); /* not reached */
} /* WorkerThread */
