void timercb(const ros::TimerEvent& e)
{
static int num_delays = 0;
ROS_DEBUG("coordinator timercb triggered");
if (gen_flag)
{
// Generate the robot ordering vector
gen_flag = generate_order();
return;
}
// get operating condition
if (ros::param::has("/operating_condition"))
ros::param::get("/operating_condition", operating_condition);
else
{
operating_condition = 4;
ros::param::set("/operating_condition", operating_condition);
}
// check to see if we are in run state
if(operating_condition == 1 || operating_condition == 2)
{
if(calibrated_flag)
{
if (num_delays < NUM_FRAME_DELAYS)
{
num_delays++;
return;
}
else if (num_delays < NUM_EKF_INITS+NUM_FRAME_DELAYS)
process_robots(1);
else
process_robots(operating_condition);
num_delays++;
}
return;
}
// are we in idle or stop condition?
else if(operating_condition == 0 || operating_condition == 3)
ROS_DEBUG_THROTTLE(1,"Coordinator node is idle due to operating condition");
// are we in emergency stop condition?
else if(operating_condition == 4)
ROS_WARN_THROTTLE(1,"Emergency Stop Requested");
// otherwise something terrible has happened
else
ROS_ERROR("Invalid value for operating_condition");
calibrated_flag = false;
calibrate_count = 0;
num_delays = 0;
return;
}
void server_cycle (SDL_Event *event)
{
int i;
if (current_cycles >= max_cycles) {
for (i = 0; i < max_robots; i++) {
if (fds[i].fd != -1) {
sockwrite(fds[i].fd, DRAW, "Max cycles reached!\n");
close(fds[i].fd);
}
}
ndprintf(stdout, "[GAME] Ended - Draw!\n");
exit(EXIT_SUCCESS);
}
current_cycles++;
struct itimerval itv;
itv.it_interval.tv_sec = 0;
itv.it_interval.tv_usec = 0;
itv.it_value.tv_sec = 0;
itv.it_value.tv_usec = 10000;
setitimer (ITIMER_REAL, &itv, NULL);
timer = 0;
cycle();
update_display(event);
process_robots();
}
int server_finished_cycle(event_t event)
{
if (max_passive_cycles && current_passive_cycles >= max_passive_cycles)
return 1;
current_passive_cycles++;
charge_timer();
cycle();
update_display();
update_display_results();
process_robots(2);
return 0;
}
int server_process_connections(event_t event)
{
int fd, i;
struct sockaddr *addr;
socklen_t addrlen = sizeof(addr);
int res = 0;
if (max_passive_cycles && current_passive_cycles >= max_passive_cycles)
ndprintf_die(stdout, "[INFO] Terminated on client connection timeout\n");
current_passive_cycles++;
charge_timer();
update_display();
update_display_msg("Waiting for connections");
process_robots(0);
/* TODO: This is horrible. We should just poll for an incoming
* connection, data on existing fd, a keypress, or X event, not
* looping like hell. */
fd = accept(sockd, (struct sockaddr *) &addr, &addrlen);
if (fd >= 0) {
if (max_robots == MAX_ROBOTS) {
ndprintf(stdout, "[INFO] Ignoring excessive connection\n");
close(fd);
} else {
if (!create_client(fd))
sockwrite(fd, ERROR, "Internal error");
else
sockwrite(fd, OK, "%d %d %d", game_type,
(int)(shot_speed / SPEED_RATIO), max_cycles);
if (autostart_robots > 0 && max_robots == autostart_robots)
res = 1;
}
}
if (event == EVENT_START)
res = 1;
if (res) {
ndprintf(stdout, "[GAME] Starting. All clients connected!\n");
/* notify all robots that are passively waiting for game start */
for (i = 0; i < max_robots; i++) {
if (!all_robots[i]->waiting)
continue;
all_robots[i]->waiting = 0;
sockwrite(fds[i].fd, OK, "1");
}
gettimeofday(&game_start, NULL);
current_passive_cycles = 0;
}
return res;
}
int server_cycle(event_t event)
{
int res;
if (current_cycles >= max_cycles || event == EVENT_FINISH) {
ndprintf(stdout, "[GAME] Ended - Draw!\n");
res = 1;
} else {
current_cycles++;
charge_timer();
cycle();
update_display();
res = process_robots(1);
}
if (res) {
gettimeofday(&game_end, NULL);
complete_ranking();
}
return res;
}
