/* LICENSE: ========================================================================= CMPack'04 Source Code Release for OPEN-R SDK 1.1.5-r2 for ERS7 Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso] School of Computer Science, Carnegie Mellon University All rights reserved. ========================================================================= */ #include "TeamMsgMgr.h" #include "../headers/CircBufPacket.h" #include "../WorldModel/WorldModel.h" #include "../Main/MainObject.h" #include "../Main/RobotMain.h" #include "../Behaviors/constants.h" #include "../headers/system_config.h" #include "../headers/Geometry.h" #include "../headers/Dictionary.h" #include "../headers/field.h" #include "../headers/Config.h" #include "../headers/Util.h" #include "../headers/Utility.h" #include "../Behaviors/constants.h" #include "../Vision/VisionInterface.h" #include "../CommMgr/CommMgrInterface.h" static int swmodelThrottle = 0; const bool debug_print_activation = false; const bool debug_print_state = false; const bool debug_print_token = false; const bool debug_print_roles = false; const bool debug_print_status_recv = false; const bool debug_print_requests = false; const bool debug_print_all_ball_posns = false; const bool debug_print_time_between_msgs = false; const bool debug_print_combinations = false; const bool debug_print_leader_ball_dist = false; const bool debug_print_teammates = false; const bool debug_print_yields = false; const bool debug_print_recv_token = false; const double same_ball_dist_cutoff = 1750.0; const ulong swm_send_interval = SecToTime(1.0 / 6.0); // 8 Hz // How long can we go without seeing a dog saying it has // the token before we assume that it is lost and create a // new one? const ulong lost_token_timeout = SecToTime(5.0); // When we're passing the token to another robot, how long do // we wait before we assume it got dropped and renege on // the hand off? const ulong yield_token_timeout = SecToTime(2.0); // When we've asked for the token, how long do we wait for a reply // from the dog that has it before we go back to being a supporter? const ulong request_token_timeout = SecToTime(1.0); // If we've been told we can't have the token recently, // don't take the initiative when we ask for it. const ulong anticipation_rejection_timeout = SecToTime(2.0); // If we're at least this far from the primary, we always take // the inititive after we request the token const double inititive_dist_thresh = 800.0; // We force ourselves to be primary for this long after a // request no matter what reply we receive from the primary const double ignore_rejection_timeout = SecToTime(1.0); const double min_ignore_interval = SecToTime(10.0); const double min_ignore_distance = 1000.0; // Constant used for exponential averaging of activations. // mean = (1.0 - act_a)*mean + act_a*new_act const double act_alpha = 0.15; // How much higher does our activation have to be than the leader's // before we ask the leader to give us the token? Note that the // leader does NOT use this value when deciding whether or not // to pass the token. (Otherwise this leads to a situation where // we decay their activation, making it artifically low, send a // request when our_act > their_act, BUT our_act < their_act+role_hyst_thresh. // Then we get rejected and don't ask again for a bit. So they'll // give us the token if our activation is higher but we won't ask // unless our activation is higher by this threshold. const double role_hyst_thresh = 0.10; // Our activation decays with the square of time since we've seen // the ball. The decay is 100% at no_ball_time_cutoff. // mult_factor = 1.0 - (time_delta/no_ball_time_cutoff)^2 const ulong no_ball_time_cutoff = SecToTime(4.0); const ulong shared_info_time_cutoff = SecToTime(5.0); // The primary attacker must have an activation greater than this // before we differentiate our roles and have supporters - otherwise, // if everyone has low activations, we all look for the ball as // primary attackers. const double min_differentiation_act = 0.01; // We receive a bonus to our activation if we've recently // acquired the token. This bonus decays linearly over the // interval between 0 and token_time_bonus_length microseconds. //const ulong token_time_bonus_length = SecToTime(5.0); //const double token_time_bonus_magnitude = 1.0; // We must have held the token for at least this long before we // will pass it to another robot. const ulong min_token_hold_time = SecToTime(0.1); // debugging parameter which controls exponential averaging // of time between messages from each peer const double btwn_alpha = .05; const double time_better_scale = SecToTime(10.0); const double time_better_max_bonus = .10; TeamMsgMgr::TeamMsgMgr() { my_world_model = NULL; } void TeamMsgMgr::init() { last_role_message = MSG_NONE; i_am_goalie = false; goalie_clearing = false; force_hold_token = false; okay_to_ignore_role = true; started_ignore = 0; my_id = 1; my_team_id = DEFEND_CYAN_GOAL; leader_id = 0; my_goal_color = DEFEND_CYAN_GOAL; my_time = 0; time_my_act_lower = time_last_broadcast = time_last_saw_ball = 0; last_saw_token = last_passed_token = last_requested_token = 0; last_rejected = 0; role_state = SUPPORTER_PLAYING; time_got_token = 0; supporter_role = RoleCommand::COM_DEF_SUPP; last_role_update_time = 0; last_role_for_teammate1 = RoleCommand::COM_DEF_SUPP; last_role_for_teammate2 = RoleCommand::COM_OFF_SUPP; my_status.HaveToken = 0; max_seen_token = 0; tmmOutStream = NULL; my_status.HavePassedRecently = false; // load our teamplay configuration from file. Should we be // doing anything for teamplay? If so, should we send a special // team id or use our goal color to determine our ID? Dictionary team_config; team_config.read("/MS/config/teamplay.cfg"); int buf; if(!team_config.getValueInt("teamplay", buf)) use_teamplay = true; else use_teamplay = buf; if(!team_config.getValueInt("default_teams", buf)) use_default_team_id = true; else use_default_team_id = buf; if(!team_config.getValueInt("special_team", buf)) special_team_id = 255; else special_team_id = buf; if(!use_default_team_id) my_team_id = special_team_id; } void TeamMsgMgr::initializeStreams(PacketStreamCollection *out_psc) { int tmm_id; tmm_id = out_psc->allocateStream((9*(5*sizeof(float) + 2*sizeof(uchar) + 1*sizeof(int)))*10,10); tmmOutStream = out_psc->getStream(tmm_id); tmmOutStream->type = SPOutEncoder::TeamMsgMgrObjLead; tmmOutStream->binary = true; } void TeamMsgMgr::setHavePassedRecently(bool have_passed) { my_status.HavePassedRecently = have_passed; } void TeamMsgMgr::updateNetStatus(CommMgr::NetStatus status) { net_status = status; } void TeamMsgMgr::updateSelf(WorldModel *wm, VisionInterface::ObjectInfo *obj_info, ulong time) { my_time = time; if(my_world_model==NULL) my_world_model = wm; // Our goal color can in fact change now that we switch colors // via button presses my_goal_color = wm->goalColor; if(use_default_team_id) my_team_id = wm->goalColor; // Do we see the ball this frame? If so, update time_last_saw_ball if(obj_info->ball.confidence > 0.5) time_last_saw_ball = my_time; // Are we kicking? If so, update time_last_kicked. if (wm->lastMotionStateType() == Motion::STATE_KICKING) { time_last_kicked = my_time; } // Fill in what I want to send to my teammates about myself // (fills in shared_info struct and robot_obs array) wm->fillInStatusMsg(my_status); my_status.IsGoalie = i_am_goalie; my_status.GoalieClearing = goalie_clearing; my_status.time_since_saw_ball = my_time - time_last_saw_ball; my_status.NumTeammates = countActiveTeammates(); my_status.IsPenalized = RobotMain::GetObject().robot_state==ROBOCUP_STATE_PENALIZED; my_status.timestamp = time; // We're ready to receive a pass if we're not the goalie, we saw the // ball recently, we're relatively close to the ball (but not too // close), and we're facing the ball. my_status.ReadyForPass = false; vector2d ball; if (obj_info->ball.confidence > 0.5) { ball = wm->ball_posn_global(); } else { ball = getBestSharedBallPosn().mean; } if (!i_am_goalie && GVector::distance(wm->my_position(), ball) < 2000.0 && GVector::distance(wm->my_position(), ball) > 500.0 && fabs(wm->GlobalToLocal(ball).angle()) < RAD(45)) { my_status.ReadyForPass = true; } // We smooth our own activation with an exponential average. my_status.Activation = (1.0 - act_alpha)*my_status.Activation + act_alpha*getMyActivation(wm); // We weight our teammates activations less heavily as they // age. decayActivations(); // Update our role assignment timers roleUpdate(); // Broadcast information to our teammates at a rate of // swm_send_interval. if(my_time - swm_send_interval > time_last_broadcast) { doStatusQueue(); } static int debug_counter = 0; if(debug_counter==30) { debugPrintOuts(); debug_counter = 0; } else { debug_counter++; } // We will only send messages if we're set to use teamplay. if(use_teamplay) MainObject::GetObject().SendStatusAndRoleMsgs(my_team_id); } void TeamMsgMgr::receiveStatusMsg(ulong time, int sender_id, int sender_team, StatusMsgEntry *msg) { // Throw away junk messages if(sender_id < 1) { pprintf(TextOutputStream, "Message from invalid sender ID %d\n", sender_id); return; } // If we aren't using teamplay or their team id doesn't match ours, // we should discard this message if(!use_teamplay || (use_default_team_id && sender_team!=my_goal_color) || (!use_default_team_id && sender_team!=special_team_id)) return; teammate[sender_id] = *msg; if(debug_print_status_recv) { pprintf(TextOutputStream, "Received from: %d act: %lf token: %d\n", sender_id, teammate[sender_id].Activation, teammate[sender_id].HaveToken); } if(debug_print_time_between_msgs) { // Track how long it is on average between messages from robots if(time_between_msgs.find(sender_id)==time_between_msgs.end()) { time_between_msgs[sender_id] = 0; } else { double delta = time - timestamp[sender_id]; delta = delta / 1000000.0; time_between_msgs[sender_id] = (1.0 - btwn_alpha)*time_between_msgs[sender_id] + btwn_alpha*delta; } } timestamp[sender_id] = time; // We need to update timestamps used in the message structure as // only the timestamp field in the header was converted by CommMgr. // We just copy the timestamp fields. Robot observations are stored // as observation ages rather than timestamps because we don't know // the clock offsets here. teammate[sender_id].timestamp = time; for(int i=0; iupdateModelTeamMsg(sender_id, teammate[sender_id]); } } int TeamMsgMgr::countActiveTeammates() { timestamp_iterator it = timestamp.begin(); int retval = 0; while(it!=timestamp.end()) { if(my_time - (*it).second < shared_info_time_cutoff) { if(debug_print_teammates) { double delta = my_time - (*it).second; delta /= 1000000.0; pprintf(TextOutputStream, "I see teammate: %d (aged %.2lf)\n", (*it).first, delta); } retval++; } else { if(debug_print_teammates) { double delta = my_time - (*it).second; delta /= 1000000.0; pprintf(TextOutputStream, " I MISS TEAMMATE: %d (age %.2lf)\n", (*it).first, delta); } } it++; } return retval; } /* Called immediately after the shared world model has been updated. This is where we monitor our timers to see if we should regenerate the token, get rid of a duplicate token, or do a timeout based state transition. */ void TeamMsgMgr::roleUpdate() { teammate_iterator tm_it; // Do we see a token somewhere? We need to know the highest // token value that's being reported. In the case of a tie, // we use network ID as a tie breaker. We assume that only one // token can exist at a time - if this isn't the case, the situation // should resolve itself quickly [we hope]. int max_token_id = 0; int max_token_value = -1; bool see_token = false; double leader_activation = 0.0; tm_it = teammate.begin(); while(tm_it!=teammate.end()) { if((*tm_it).second.HaveToken >= max_token_value) { max_token_id = (*tm_it).first; max_token_value = (*tm_it).second.HaveToken; } tm_it++; } if(max_token_id <= 0) { max_token_id = 0; max_token_value = -1; } // Do we actually see a token or are we pointed at an empty spot? see_token = (max_token_value > 0) && knownTeammate(max_token_id); // Is this token a new max? if(max_token_value > max_seen_token) max_seen_token = max_token_value; // We force ourselves to reassign roles if we're primary and it's // been a while since the last time we assigned roles. This is to // prevent issues where 2 rebooted robots will both end up as // defensive supporter, because the primary never bothers to call // roleUpdate, so the robots never get a role assignment and // therefore default to defender. if (my_time - last_role_update_time > SecToTime(1.0)) { assignRoles(); } // If we see a token, reset our lost token timer and // record the primary attacker's activation if(see_token) { last_saw_token = my_time; leader_id = max_token_id; // see_token => knownTeammate(max_token_id) // Record our primary attacker's activation. If we have an estimate // of the primary's activation using our own ball estimate, we use the // min of the two; the worst that can happen is they'll refuse to give // us the token. leader_activation = teammate[leader_id].Activation; // HACK UNDO if(ego_view_activation.find(leader_id)!=ego_view_activation.end() && ego_view_activation[leader_id] < leader_activation) leader_activation = ego_view_activation[leader_id]; // We should grab the supporter role that the leader assigned to us. supporter_role = RoleCommand::COM_DEF_SUPP; for(int i=0; i<3; i++) { if(knownTeammate(leader_id) && teammate[leader_id].Commands[i].ID==my_id) supporter_role = teammate[leader_id].Commands[i].Command; } if(my_status.Activation < leader_activation) time_my_act_lower = my_time; } else { leader_id = 0; } if(debug_print_token) { if(!knownTeammate(leader_id)) { pprintf(TextOutputStream, "token: %d leader_id: %d supporter_role: %d\n", my_status.HaveToken, leader_id, supporter_role); } else { pprintf(TextOutputStream, "token: %d leader_id: %d leader_act: %lf supporter_role: %d\n", my_status.HaveToken, leader_id, teammate[leader_id].Activation, supporter_role); } } // Now handle things on a state by state basis int max_index = -1; // index of teammate with highest act in PRIMARY_PLAYING case switch(role_state) { case PRIMARY_PLAYING: if(debug_print_state) pprintf(TextOutputStream, "Current state: PRIMARY_PLAYING\n"); // Do we see another robot with a higher ID claiming to // be the leader? Then we should yield to it. We // yield to higher tokens and use network id to break ties. if((max_token_value > my_status.HaveToken) || (max_token_value == my_status.HaveToken && max_token_id > my_id)) { role_state = SUPPORTER_PLAYING; time_got_token = 0; my_status.HaveToken = 0; assignRoles(); if(debug_print_yields) pprintf(TextOutputStream, "I am yielding to %d who has token %d\n", max_token_id, max_token_value); } // Do we see another robot that should be the primary attacker // instead of us? If so, pass Mr. Token. // find the index of the teammate with the highest activation. max_index = -1; tm_it = teammate.begin(); while(tm_it != teammate.end()) { // Handle the case when we are uninitialized // It is possible we'll pick a teammate who is penalized here if(max_index==-1) max_index = (*tm_it).first; // now see if the latest teammate has a higher activation than // our current max We make sure all new candidates are not // penalized and if tm[max_index] is penalized (from our // initial assignment), we pick the first non-penalized friend // we see. if(!(*tm_it).second.IsPenalized && ((*tm_it).second.Activation > teammate[max_index].Activation || teammate[max_index].IsPenalized)) max_index = (*tm_it).first; tm_it++; } // Did we find someone who isn't penalized? if(knownTeammate(max_index) && teammate[max_index].IsPenalized) max_index = -1; // see if we've a) found another teammate and b) if we should // pass the token to our friend if(max_index!=-1 && knownTeammate(max_index) && !force_hold_token && timeSinceGotToken() >= min_token_hold_time && ((teammate[max_index].Activation > my_status.Activation + role_hyst_thresh && !teammate[max_index].IsGoalie) || my_status.IsPenalized)) { role_state = PRIMARY_YIELDED; last_passed_token = my_time; // Notify the other robot that it's got the token // "leader_act" in this case is their activation (to explain // why we're foisting it off on them) last_role_message = MSG_TOKEN_PASS; MainObject::GetObject().queueRoleMessage(max_index, MSG_TOKEN_PASS, my_status.Activation, my_status.HaveToken + 1, teammate[max_index].Activation); // If we're going to send a message, we might as well piggy back a // status update with it doStatusQueue(); if(debug_print_yields && knownTeammate(max_index)) pprintf(TextOutputStream, "Yielding to %d (them: act %0.3lf pen %d me: %0.3lf %d\n", max_index, teammate[max_index].Activation, teammate[max_index].IsPenalized, my_status.Activation, my_status.IsPenalized); } break; case PRIMARY_YIELDED: if(debug_print_state) pprintf(TextOutputStream, "Current state: PRIMARY_YIELDED\n"); // Has any other robot picked up the token? If not, // have we timed out while waiting for someone to // receive a hand off? if(max_token_value > my_status.HaveToken) { role_state = SUPPORTER_PLAYING; my_status.HaveToken = 0; time_got_token = 0; assignRoles(); } else if(my_time > last_passed_token + yield_token_timeout) { role_state = PRIMARY_PLAYING; assignRoles(); if(debug_print_yields) pprintf(TextOutputStream, "Yield timed out\n"); } break; case SUPPORTER_PLAYING: if(debug_print_state) pprintf(TextOutputStream, "Current state: SUPPORTER_PLAYING\n"); // Has the token been missing for too long? If not, is our // activation high enough to warrent requesting the token? // We only request the token if we physically see the ball. // Hopefully this will keep us from sending requests when our // world model is inaccurate. if(my_time > last_saw_token + lost_token_timeout || !knownTeammate(max_token_id)) { role_state = PRIMARY_PLAYING; time_got_token = my_time; my_status.HaveToken = max_seen_token + 1; time_last_broadcast = 0; // force transmit of info assignRoles(); } else if (!my_status.IsPenalized && my_status.time_since_saw_ball==0 && see_token && my_status.Activation > (leader_activation + role_hyst_thresh) && (!my_status.IsGoalie || my_status.GoalieClearing)) { // We need to request the token if our activation is // higher than the activation of the current primary // attacker. The goalie can only request the token when it's // clearing the ball. last_role_message = MSG_TOKEN_REQ; MainObject::GetObject().queueRoleMessage(max_token_id, MSG_TOKEN_REQ, my_status.Activation, my_status.HaveToken, teammate[max_token_id].Activation); // If we're going to send a message, we might as well piggy back a // status update with it doStatusQueue(); role_state = SUPPORTER_ASKED; last_requested_token = my_time; if(debug_print_requests && knownTeammate(max_token_id)) { pprintf(TextOutputStream, "requesting. my_act: %lf their_id: %d their_act: %lf\n", my_status.Activation, max_token_id, teammate[max_token_id].Activation); } } else if(see_token && teammate[max_token_id].IsGoalie && !teammate[max_token_id].GoalieClearing) { // We need to request the token if the primary attacker // is the goalie and is not currently clearning the ball. last_role_message = MSG_TOKEN_REQ; MainObject::GetObject().queueRoleMessage(max_token_id, MSG_TOKEN_REQ, my_status.Activation, my_status.HaveToken, -1.0); // If we're going to send a message, we might as well piggy back a // status update with it doStatusQueue(); role_state = SUPPORTER_ASKED; last_requested_token = my_time; } break; case SUPPORTER_ASKED: if(debug_print_state) pprintf(TextOutputStream, "Current state: SUPPORTER_ASKED\n"); // We need to worry about our request timing out. We don't // worry about lost token timeouts 'cuz we'll handle that once // we're back in SUPPORTER_PLAYING. if(my_time > last_requested_token + request_token_timeout) { role_state = SUPPORTER_PLAYING; last_rejected = my_time; // this counts as rejection // We don't set our activation to zero (which would slow a // repeat request with the exponential averaging for smoothing) // 'cuz this is a network problem and we want to resend our // request ASAP. } break; } } void TeamMsgMgr::receiveRoleMessage(ulong time, int sender_id, int sender_team, int type, double activation, int token) { if(sender_id < 1) { pprintf(TextOutputStream, "Invalid sender_id %d\n", sender_id); return; } // If we aren't using teamplay or their team id doesn't match ours, // we should discard this message if(!use_teamplay || (use_default_team_id && sender_team!=my_goal_color) || (!use_default_team_id && sender_team!=special_team_id)) return; // UNDO HACK // When considering token requests we merge our activation, which // uses our local ball position with the ball position that they // are reporting. double local_activation = my_status.Activation; //double remote_activation = local_activation; //if(knownTeammate(sender_id) && // my_world_model) { // // age of their ball observation // ulong remote_age = my_time - teammate[sender_id].timestamp + // teammate[sender_id].time_since_saw_ball; // remote_activation = // getMyActivation(my_world_model, // teammate[sender_id].shared_info.ball_position.mean, // remote_age); // } //double alpha = (time - time_last_saw_ball); //alpha /= no_ball_time_cutoff; //if (alpha > 1.0) { // alpha = 1.0; // } //double merged_act = (1.0 - alpha)*local_activation + // alpha*remote_activation; double merged_act = local_activation; // Let's break this down by state. switch(role_state) { case PRIMARY_PLAYING: // Is someone asking us to yield our token? if(type==MSG_TOKEN_REQ) { // Should we actually yield? We yield if they have a higher // activation (NOT plus hysterisis) or if we're the goalie and // not clearing. A clearing goalie yields if they've got // a higher activation. It is important that the hysterisis is // on their end when they ask. (they decay our activation over time // so they'll ask prematurely and we'll say no then they'll wait // too long before asking again and it's bad) // The goalie does not yield while it's trying to clear. // The goalie always yields if it's not clearing if(knownTeammate(sender_id) && !force_hold_token && timeSinceGotToken() >= min_token_hold_time && !teammate[sender_id].IsPenalized && (((activation > merged_act) && !my_status.IsGoalie) || (my_status.IsGoalie && !my_status.GoalieClearing))) { role_state = PRIMARY_YIELDED; last_passed_token = time; // Notify the other robot that it's got the token last_role_message = MSG_TOKEN_PASS; MainObject::GetObject().queueRoleMessage(sender_id, MSG_TOKEN_PASS, my_status.Activation, my_status.HaveToken + 1, activation); // If we're going to send a message, we might as well piggy back a // status update with it doStatusQueue(); } else { // Notify the other robot that it can't have it last_role_message = MSG_NO_TOKEN; MainObject::GetObject().queueRoleMessage(sender_id, MSG_NO_TOKEN, my_status.Activation, my_status.HaveToken, activation); // If we're going to send a message, we might as well piggy back a // status update with it doStatusQueue(); } // end else clause of should yield test } break; case PRIMARY_YIELDED: case SUPPORTER_PLAYING: switch(type) { case MSG_TOKEN_REQ: // Notify the other robot that it can't have it; we don't // have the token so we can't give it to them. last_role_message = MSG_NO_TOKEN; MainObject::GetObject().queueRoleMessage(sender_id, MSG_NO_TOKEN, my_status.Activation, my_status.HaveToken, -2.0); // If we're going to send a message, we might as well piggy back a // status update with it doStatusQueue(); break; case MSG_TOKEN_PASS: if(debug_print_recv_token) pprintf(TextOutputStream, "Received token %d from %d\n", token, sender_id); role_state = PRIMARY_PLAYING; my_status.HaveToken = token; last_saw_token = time; time_got_token = time; leader_id = 0; // Tell everyone that I'm the leader now time_last_broadcast = 0; // force resend since WM changed // Don't do this - if we're passing the token back and // forth, it will make things even worse // last_rejected = 0; // we got the token assignRoles(); break; } break; case SUPPORTER_ASKED: switch(type) { case MSG_TOKEN_PASS: role_state = PRIMARY_PLAYING; my_status.HaveToken = token; time_got_token = time; last_saw_token = time; leader_id = 0; time_last_broadcast = 0; // force resend since WM changed // Don't do this - if we're passing the token back and // forth, it will make things even worse // last_rejected = 0; // we got the token assignRoles(); break; case MSG_NO_TOKEN: if(token >= my_status.HaveToken) { role_state = SUPPORTER_PLAYING; // update our data on this robot with the true value // so we don't ask again too soon. if(knownTeammate(sender_id)) teammate[sender_id].Activation = activation; last_rejected = time; // Delay asking again until exponential averaging brings // activation back up. my_status.Activation = 0; } break; case MSG_TOKEN_REQ: // Notify the other robot that it can't have it; we don't // have a token to give (yet) last_role_message = MSG_NO_TOKEN; MainObject::GetObject().queueRoleMessage(sender_id, MSG_NO_TOKEN, my_status.Activation, my_status.HaveToken, -3.0); // If we're going to send a message, we might as well piggy back a // status update with it doStatusQueue(); break; } break; } } bool TeamMsgMgr::isPrimaryAttacker() { // This one is all static roles //if (net_status == CommMgr::DISCONNECTED) { if (false) { const int defender_id = 16; const int negy_id = 17; const int posy_id = 14; vector2d ball = my_status.shared_info.ball_position.mean; //double score_sign = my_goal_color == DEFEND_YELLOW_GOAL ? 1.0 : -1.0; switch(my_id) { case defender_id: // Defensive, anywhere on field supporter_role = RoleCommand::COM_DEF_SUPP; return false; break; case negy_id: // Offensive, -y supporter_role = RoleCommand::COM_OFF_SUPP; if (ball.y < 100) { return true; } else { return false; } break; case posy_id: // Offensive, +y supporter_role = RoleCommand::COM_OFF_SUPP; if (ball.y > 100) { return true; } else { return false; } break; } return false; } else { // This is the normal stuff that assumes we're all communicating // This robot is the primary attacker if(leader_id==0 || role_state==PRIMARY_PLAYING || role_state==PRIMARY_YIELDED) return true; // We will pretend to be primary in certain circumstances, // for example, if we have just asked for the token. This is // generally the same for both supporters, but the defensive // supporter will NOT wander around looking for the ball when // no one knows where it is (the thought is that it should // go hang out near our goal in case the ball pops up on // our half) // If we have requested the token recently, we force playing // primary for a short time in case we need to flood the // primary with multiple requests before it smartens up and // passes the token. double primary_dist = GVector::distance(my_status.shared_info.my_position.mean, getPrimaryLoc()); if(okay_to_ignore_role && primary_dist > min_ignore_distance && (my_time - last_requested_token) < ignore_rejection_timeout) { // Keep track of when we started ignoring our role if(started_ignore==0) started_ignore = my_time; // If we've been doing this for too long, we need to stop // until a suitable interval has passed. if((my_time - started_ignore) > ignore_rejection_timeout) okay_to_ignore_role = false; return true; } // If we're not allowed to ignore our role anymore but a suitable // interval has passed, we'll reset things. if(!okay_to_ignore_role && (my_time - started_ignore) > min_ignore_interval) { okay_to_ignore_role = true; started_ignore = 0; } // THIS IS SUBSUMED (spelling? gack!) by the above // If we've asked and the primary attacker hasn't smacked us down recently // we will play primary until we get a reply // if(role_state==SUPPORTER_ASKED && // (((my_time - last_rejected) > anticipation_rejection_timeout) || // getPrimaryDist() < inititive_dist_thresh)) // return true; // If we are/were the defensive supporter, we do not take on the // role of primary. if(supporter_role==RoleCommand::COM_DEF_SUPP) return false; // The rest of these are special cases where the offensive supporter // is willing to take the inititive. // If the primary attacker's activation is lower than a threshold, // we act as primary until someone gets the ball. if(!knownTeammate(leader_id) || teammate[leader_id].Activation < min_differentiation_act) return true; // We need to ensure that the robot claiming to be leader // has seen the ball fairly recently. if(!validTeammate(leader_id)) return true; // Are we following the same ball as our fearless leader? If not, // ignore the leader. // if(!sameBallAs(leader_id)) { // return true; // } return false; } } bool TeamMsgMgr::isOffensiveSupporter() { // If we're the primary attacker, we can't be a supporter, now can we? // (this shouldn't come up) if(isPrimaryAttacker()) return false; return (supporter_role==RoleCommand::COM_OFF_SUPP || supporter_role==RoleCommand::COM_OFF_SUPP_LONG); } bool TeamMsgMgr::teammateHasPassedRecently(int uniform_number) { int valid = 0; timestamp_iterator time_it = timestamp.begin(); while(time_it != timestamp.end()) { if(my_time - (*time_it).second < shared_info_time_cutoff) { valid++; } if(valid==uniform_number) { return teammate[(*time_it).first].HavePassedRecently; } time_it++; } return false; } bool TeamMsgMgr::teammateReadyForPass(int uniform_number) { int valid = 0; timestamp_iterator time_it = timestamp.begin(); while(time_it != timestamp.end()) { if(my_time - (*time_it).second < shared_info_time_cutoff) { valid++; } if(valid==uniform_number) { return teammate[(*time_it).first].ReadyForPass; } time_it++; } return false; } Gaussian2 TeamMsgMgr::getTeammatePosn(int uniform_number) { Gaussian2 retval = Gaussian2(); int valid = 0; timestamp_iterator time_it = timestamp.begin(); while(time_it != timestamp.end()) { if(my_time - (*time_it).second < shared_info_time_cutoff) { valid++; } if(valid==uniform_number) { retval.mean = teammate[(*time_it).first].shared_info.my_position.mean; return retval; } time_it++; } retval.setsxsy(halfLength, halfLength, 0); return retval; } // Does not return data from this robot - only shared info // ('cuz otherwise you should just use vision or the wm) Gaussian2 TeamMsgMgr::getBestSharedBallPosn() { Gaussian2 retval = Gaussian2(); ulong best_time = 0; ulong second_best_time = 0; int best_id = 0; int second_best_id = 0; timestamp_iterator time_it; teammate_iterator tm_it; // Find the most recent data from a robot that actually saw // the ball and isn't us. time_it = timestamp.begin(); tm_it = teammate.begin(); while(time_it != timestamp.end() && tm_it != teammate.end()) { if((*time_it).second > best_time && (*tm_it).second.time_since_saw_ball==0) { second_best_id = best_id; second_best_time = best_time; best_id = (*time_it).first; best_time = (*time_it).second; } time_it++; tm_it++; } if(my_time - best_time < shared_info_time_cutoff && best_id != 0) { retval = teammate[best_id].shared_info.ball_position; // If the best time is from the primary attacker, // look for an alternative as the primary attacker // is often lost and leg locked. const ulong supp_time_diff_limit = SecToTime(1.0); if(my_time - second_best_time < shared_info_time_cutoff && best_id==leader_id && (best_time - second_best_time) < supp_time_diff_limit) { retval = teammate[second_best_id].shared_info.ball_position; } } return retval; } double TeamMsgMgr::getMyActivation(WorldModel *wm) { vector2d ball = getBestBallEst(wm); double activation = rawActivation(wm->my_position(), ball); double time_decay = getActTimeDecay(my_time - time_last_saw_ball); activation *= time_decay; // if we've just finished a kick, decay our activation further //if (my_time - time_last_kicked > SecToTime(0.25) && // my_time - time_last_kicked < SecToTime(2.0)) { // activation *= 0.5; // } // We scale our activation based on what our role is and which half of // the field the ball is on. double scale = getRoleScale(ball.x, isPrimaryAttacker(), isOffensiveSupporter()); // If we are penalized, our activation is zero. Force this here. if(my_status.IsPenalized) return 0.0; return activation*scale; } double TeamMsgMgr::getMyActivation(WorldModel *wm, vector2d ball, ulong ball_age) { double activation = rawActivation(wm->my_position(), ball); //// We do NOT do a time decay as we have no idea what the age of //// the arbitrary ball is. // We're adding this decay back in so the primary is more willing to // pass the token when it has no freaking idea where the ball is. double time_decay = getActTimeDecay(ball_age); activation *= time_decay; // if we've just finished a kick, decay our activation further // if (my_time - time_last_kicked > SecToTime(0.25) && // my_time - time_last_kicked < SecToTime(2.0)) { // activation *= 0.5; // } // We scale our activation based on what our role is and which half of // the field the ball is on. double scale = getRoleScale(ball.x, isPrimaryAttacker(), isOffensiveSupporter()); return activation*scale; } double TeamMsgMgr::calcActivation(WorldModel *wm, int id) { if(!knownTeammate(id) && id!=wm->myID) return 0; vector2d ball = getBestBallEst(wm); vector2d bot_posn = wm->my_position(); if(id!=wm->myID) bot_posn = teammate[id].shared_info.my_position.mean; double activation = rawActivation(bot_posn, ball); double time_decay = getActTimeDecay(my_time - time_last_saw_ball); activation *= time_decay; // We scale our activation based on what our role is and which half of // the field the ball is on. double scale = getRoleScale(ball.x, isPrimaryAttacker(), isOffensiveSupporter()); return activation*scale; } /* Calculate the basic activation for a robot at a given position when the ball is at another posn. We'll scale this in a variety of ways elsewhere. */ double TeamMsgMgr::rawActivation(vector2d robot_posn, vector2d ball_posn) { double ball_dist = (ball_posn - robot_posn).length(); double distance_comp; // distance to the ball component double long_distance_comp; // smaller weight distance component distance_comp = 1.0 - min(1000.0, ball_dist)/1000.0; long_distance_comp = 1.0 - min(2500.0, ball_dist)/2500.0; // Our angular component is calculated by figuring out how // "behind" the ball we are. We want to be directly behind // it so our opponents can't push it past us. vector2d me_to_ball = (ball_posn - robot_posn).norm(); vector2d defend_goal_x = vector2d(1.0, 0); if(my_world_model && my_world_model->teamColor==TEAM_COLOR_RED) defend_goal_x *= -1.0; double cos_theta = defend_goal_x.dot(me_to_ball); double angle = acos(cos_theta); double angular_comp = angle/M_PI; // We scale our angular component by distance - otherwise a // further robot with a slightly better angle can go to // the ball instead of the closer robot when both are far // from the ball. angular_comp *= long_distance_comp; double activation = 0.60*distance_comp + 0.20*long_distance_comp + 0.20*angular_comp; return activation; } // We receive a bonus to our activation based on how long // our potential has been higher than the current leader's // potential. double TeamMsgMgr::getTimeBetterBonus() { double bonus = 0; double time_better = my_time - time_my_act_lower; bonus = time_better / time_better_scale; bonus = bound(bonus, 0.0, time_better_max_bonus); return bonus; } vector2d TeamMsgMgr::getBestBallEst(WorldModel *wm) { Gaussian2 ball_est; vector2d ball; if(wm->ball_tracker_observation_global(ball_est)) ball = ball_est.mean; else if(wm->teammate_ball_tracker_observation_global(ball_est)) ball = ball_est.mean; else ball = wm->ball_posn_global(); return ball; } double TeamMsgMgr::getRoleScale(double ball_x, bool is_primary, bool is_off_supp) { return 1.0; double scale = 1.0; double score_sign = 1.0; if(my_world_model) score_sign = (my_world_model->teamColor==TEAM_COLOR_BLUE) ? -1.0 : 1.0; if(is_primary) { // The primary attacker is always on the "correct" // half of the field. (We need to scale it's value rather // than use scale=1.0 so it can switch intelligently // with the supporter that belongs on the same half of // the field) scale = sigmoid_func(5.0*fabs(ball_x)/halfLength); } else if(is_off_supp) { // It's good for the ball to be on their half of // the field, bad for it to be on ours... scale = sigmoid_func(5.0*score_sign*ball_x/halfLength); } else { // opposite of the above. scale = sigmoid_func(-5.0*score_sign*ball_x/halfLength); } return scale; } double TeamMsgMgr::getActTimeDecay(ulong delta) { // Decay the activation based on how long it's been since we've // seen our ball double time_decay = delta; time_decay = 1.0 - (time_decay/no_ball_time_cutoff) * (time_decay/no_ball_time_cutoff); time_decay = max(0.0, time_decay); return time_decay; } void TeamMsgMgr::assignRoles() { last_role_update_time = my_time; for(int i=0; i<3; i++) { my_status.Commands[i].ID = 0; my_status.Commands[i].Command = RoleCommand::COM_DEF_SUPP; } // If we're not the primary attacker, don't tell people // what to do. if(max_seen_token > my_status.HaveToken) { return; } // Find the first 2 other active robots and call them our // teammates. (Only accept robots that aren't the goalie) int offset = 0; timestamp_iterator time_it = timestamp.begin(); while(time_it != timestamp.end() && offset < 3) { if(my_time - (*time_it).second < shared_info_time_cutoff) { // make sure we're not trying to assign roles to the goalie int id = (*time_it).first; if(!teammate[id].IsGoalie) { my_status.Commands[offset].ID = id; offset++; } } time_it++; } // Do we have two people to assign? if(offset==2) { int id_one = my_status.Commands[0].ID; int id_two = my_status.Commands[1].ID; double hysteresis_threshold = 100.0; double teammate1x = teammate[id_one].shared_info.my_position.mean.x; double teammate2x = teammate[id_two].shared_info.my_position.mean.x; if(my_goal_color==DEFEND_CYAN_GOAL) { if(teammate1x + hysteresis_threshold < teammate2x) { my_status.Commands[0].Command = RoleCommand::COM_OFF_SUPP; my_status.Commands[1].Command = RoleCommand::COM_DEF_SUPP; } else if (teammate1x > teammate2x + hysteresis_threshold) { my_status.Commands[0].Command = RoleCommand::COM_DEF_SUPP; my_status.Commands[1].Command = RoleCommand::COM_OFF_SUPP; } else { my_status.Commands[0].Command = last_role_for_teammate1; my_status.Commands[1].Command = last_role_for_teammate2; } } else { // We're defending the yellow goal if(teammate1x > teammate2x + hysteresis_threshold) { my_status.Commands[0].Command = RoleCommand::COM_OFF_SUPP; my_status.Commands[1].Command = RoleCommand::COM_DEF_SUPP; } else if (teammate1x + hysteresis_threshold < teammate2x) { my_status.Commands[0].Command = RoleCommand::COM_DEF_SUPP; my_status.Commands[1].Command = RoleCommand::COM_OFF_SUPP; } else { my_status.Commands[0].Command = last_role_for_teammate1; my_status.Commands[1].Command = last_role_for_teammate2; } } } else if(offset==1) { // Do we have anyone to assign? Default to defense my_status.Commands[0].Command = RoleCommand::COM_DEF_SUPP; } // extra-paranoid bugcheck - if the two supporters both have the // same role, switch the second one. if (my_status.Commands[0].Command == my_status.Commands[1].Command) { if (my_status.Commands[0].Command == RoleCommand::COM_DEF_SUPP) { my_status.Commands[1].Command = RoleCommand::COM_OFF_SUPP; } else { my_status.Commands[1].Command = RoleCommand::COM_DEF_SUPP; } } last_role_for_teammate1 = my_status.Commands[0].Command; last_role_for_teammate2 = my_status.Commands[1].Command; } void TeamMsgMgr::sendOutput() { #ifdef PLATFORM_APERIOS /* Send to SharedWorldModel output to be encoded */ if(config.spoutConfig.dumpTeamMsgMgrObj) { if (++swmodelThrottle >= config.spoutConfig.dumpTeamMsgMgrObj) { if(tmmOutStream!=NULL) { static uchar buf[9*(5*sizeof(float)+ 1*sizeof(int) + 2*sizeof(bool))]; int out_size=0; out_size=encoder.encodeTeamMsgMgr(buf,this); tmmOutStream->writeBinary(buf,out_size); } else { pprintf(TextOutputStream,"tmmOutStream is NULL when sending output"); } swmodelThrottle = 0; } } #endif } bool TeamMsgMgr::isLeaderGoalie() { if(!knownTeammate(leader_id) || leader_id==0) return false; return teammate[leader_id].IsGoalie==1; } // one of COM_DEF_SUPP, COM_OFF_SUPP, COM_OFF_SUPP_LONG (from RoleCommand enum) int TeamMsgMgr::getSupporterRole() { return supporter_role; } void TeamMsgMgr::setStuckInfo(StuckInfo _stuck_info) { stuck_fraction = _stuck_info.fraction_stuck; } void TeamMsgMgr::setPlayerInfo(int _my_id /* , int _my_goal_color*/) { my_id = _my_id; // my_goal_color = _my_goal_color; } ulong TeamMsgMgr::timeSinceGotToken() { return my_time - time_got_token; } void TeamMsgMgr::decayActivations() { timestamp_iterator time_it; teammate_iterator teammate_it; // We will weight each activation based on how old the data is. time_it = timestamp.begin(); teammate_it = teammate.begin(); while(time_it != timestamp.end() && teammate_it != teammate.end()) { double time_factor = 1.0 - TimeToSec(swm_send_interval)/25.0; (*teammate_it).second.Activation *= time_factor; // We need to timeout tokens for robots that have dropped off the // network if(my_time - (*time_it).second > shared_info_time_cutoff) (*teammate_it).second.HaveToken = 0; time_it++; teammate_it++; } } // Centralized place for some of the debug printouts. Others // are nested in the code. void TeamMsgMgr::debugPrintOuts() { if(debug_print_activation) pprintf(TextOutputStream, "Activation: %lf\n", my_status.Activation); // piggyback a dump on the half-second sending if(debug_print_all_ball_posns) { double saw_time = my_status.time_since_saw_ball; saw_time /= 1000000.0; pprintf(TextOutputStream, "I report ball at %.4lf, %.4lf with age %.4lf\n", my_status.shared_info.ball_position.mean.x, my_status.shared_info.ball_position.mean.y, saw_time); teammate_iterator team_it = teammate.begin(); while(team_it != teammate.end()) { saw_time = (*team_it).second.time_since_saw_ball; saw_time /= 1000000.0; pprintf(TextOutputStream, "robot %d reports ball at: %.4lf, %.4lf with age %.4lf\n", (*team_it).first, (*team_it).second.shared_info.ball_position.mean.x, (*team_it).second.shared_info.ball_position.mean.y, saw_time); team_it++; } } if(debug_print_time_between_msgs) { between_iterator between_it = time_between_msgs.begin(); while(between_it != time_between_msgs.end()) { if(validTeammate((*between_it).first)) { double delta = my_time - timestamp[(*between_it).first]; delta = delta / 1000000.0; pprintf(TextOutputStream, "Average time between updates from %d = %.2lf (last recv) %.2lf\n", (*between_it).first, (*between_it).second, delta); } between_it++; } } if(debug_print_roles) { pprintf(TextOutputStream, "isPrimary: %d isOffSupp: %d Activation %lf\n", isPrimaryAttacker(), isOffensiveSupporter(), my_status.Activation); } if(debug_print_leader_ball_dist) { if(validTeammate(leader_id) && my_status.time_since_saw_ball < shared_info_time_cutoff) pprintf(TextOutputStream, "leader ball err: %lf\n", GVector::distance(my_status.shared_info.ball_position.mean, teammate[leader_id].shared_info.ball_position.mean)); } } bool TeamMsgMgr::sameBallAs(int leader_id) { // We're not looking at the same ball as a // nonexistant teammate. if(!knownTeammate(leader_id)) return false; // If our teammate has recently seen the ball and we do NOT // see it ourselves we'll go with whatever they say if(my_status.time_since_saw_ball > shared_info_time_cutoff) return true; // If our leader hasn't seen the ball recently enough, we'll // go for it on our own if(!validTeammate(leader_id)) return false; if(GVector::distance(my_status.shared_info.ball_position.mean, teammate[leader_id].shared_info.ball_position.mean) > same_ball_dist_cutoff) { return false; } return true; } // Have we heard from this teammate recently enough to // listen to what they say about roles? bool TeamMsgMgr::validTeammate(int id) { // Have we heard from them at all? if(timestamp.find(id) == timestamp.end()) return false; // Now see how long it's been since they've seen the ball. // (currently this does not count the latency involved in // message passing, but perhaps that will change with clock // syncronization. ulong age = my_time - timestamp[id]; age += teammate[id].time_since_saw_ball; if(age > shared_info_time_cutoff) return false; return true; } vector2d TeamMsgMgr::getPrimaryLoc() { if(knownTeammate(leader_id)) return teammate[leader_id].shared_info.my_position.mean; else return my_status.shared_info.my_position.mean; } vector2d TeamMsgMgr::getOtherSuppLoc() { teammate_iterator team_it = teammate.begin(); while(team_it != teammate.end()) { // If we see a teammate who is not the leader and not // the goalie, return their position if((*team_it).first != (uint)leader_id && !(*team_it).second.IsGoalie) (*team_it).second.shared_info.my_position.mean; team_it++; } return vector2d(0.0,0.0); } double TeamMsgMgr::getPrimaryDist() { if(!knownTeammate(leader_id)) return 0.0; return (getPrimaryLoc() - my_status.shared_info.my_position.mean).length(); } bool TeamMsgMgr::networkOkay() { if(my_status.NumTeammates==0) return false; teammate_iterator team_it = teammate.begin(); while(team_it != teammate.end()) { if((*team_it).second.NumTeammates != my_status.NumTeammates) return false; team_it++; } return true; } int TeamMsgMgr::idWithHighestAct() { int max_id = 0; teammate_iterator team_it = teammate.begin(); while(team_it != teammate.end()) { // make sure we have a valid ID for the // comparison below. if(max_id==0) max_id = (*team_it).first; if((*team_it).second.Activation > teammate[max_id].Activation) max_id = (*team_it).first; team_it++; } return max_id; } bool TeamMsgMgr::knownTeammate(int id) { return teammate.find(id)!=teammate.end(); } // returns 0 if id is not found, an the uniform // number (from 1 to 3) if it is. int TeamMsgMgr::getUniformNumFromID(int id) { int uni_id = 0; teammate_iterator team_it = teammate.begin(); while(team_it != teammate.end()) { uni_id++; if((*team_it).first==(uint)id) return uni_id; team_it++; } return uni_id; } void TeamMsgMgr::doStatusQueue() { time_last_broadcast = my_time; MainObject::GetObject().queueStatusMessage(&my_status); my_status.HavePassedRecently = false; }