Teaching Assistant: Roi Yehoshua

TAO team decisions Synchronized Next protocols Synchronized Alloc protocols Patrolling and formation examples (C)2014 Roi Yehoshua

Time synchronization – Start plan synchronization – Mutual termination of plan Team synchronization / allocation – Splitting to sub-teams and child plan allocation Team decision about next plan – Selection of next plan for all robots in team (C)2014 Roi Yehoshua

The following example defines a TAO machine for a team of robots that patrol along a wall – each robot covers a different segment of the wall Each robot runs a TAO machine with 4 plans: – Start – TurnToGoal – MoveToGoal – Switch There is a NEXT sequence relation between these plans (no allocations of sub-plans) In the basic example PatrolAsync.cpp there is no synchronization or decision making at the team level (C)2014 Roi Yehoshua

Move To Goal Turn To Goal Switch Goal Start

(C)2014 Roi Yehoshua TAO(Patrol) { TAO_PLANS { Start, TurnToGoal, MoveToGoal, Switch } TAO_START_PLAN(Start); TAO_BGN { TAO_PLAN(Start) { TAO_START_CONDITION(true); TAO_ALLOCATE_EMPTY; TAO_STOP_CONDITION(true); TAO_NEXT(NextFirstReady) { TAO_NEXT_PLAN(TurnToGoal); TAO_NEXT_PLAN(Switch); } TAO(Patrol) { TAO_PLANS { Start, TurnToGoal, MoveToGoal, Switch } TAO_START_PLAN(Start); TAO_BGN { TAO_PLAN(Start) { TAO_START_CONDITION(true); TAO_ALLOCATE_EMPTY; TAO_STOP_CONDITION(true); TAO_NEXT(NextFirstReady) { TAO_NEXT_PLAN(TurnToGoal); TAO_NEXT_PLAN(Switch); }

(C)2014 Roi Yehoshua TAO_PLAN(TurnToGoal) { TAO_START_CONDITION(not nearBy(WM.robotPosition, WM.goal)); cout<<"TAO_PLAN(TurnToGoal)"<<endl; TAO_ALLOCATE_EMPTY; TAO_CALL_TASK(TurnToGoal); TAO_STOP_CONDITION(headingOnTarget(WM.robotPosition, WM.goal)); TAO_NEXT(NextFirstReady) { TAO_NEXT_PLAN(MoveToGoal); } TAO_PLAN(MoveToGoal) { TAO_START_CONDITION(not nearBy(WM.robotPosition, WM.goal)); cout<<"TAO_PLAN(MoveToGoal)"<<endl; TAO_ALLOCATE_EMPTY; TAO_CALL_TASK(MoveToGoal); TAO_STOP_CONDITION(nearBy(WM.robotPosition, WM.goal)); TAO_NEXT(NextFirstReady) { TAO_NEXT_PLAN(Switch); TAO_NEXT_PLAN(TurnToGoal); } TAO_PLAN(TurnToGoal) { TAO_START_CONDITION(not nearBy(WM.robotPosition, WM.goal)); cout<<"TAO_PLAN(TurnToGoal)"<<endl; TAO_ALLOCATE_EMPTY; TAO_CALL_TASK(TurnToGoal); TAO_STOP_CONDITION(headingOnTarget(WM.robotPosition, WM.goal)); TAO_NEXT(NextFirstReady) { TAO_NEXT_PLAN(MoveToGoal); } TAO_PLAN(MoveToGoal) { TAO_START_CONDITION(not nearBy(WM.robotPosition, WM.goal)); cout<<"TAO_PLAN(MoveToGoal)"<<endl; TAO_ALLOCATE_EMPTY; TAO_CALL_TASK(MoveToGoal); TAO_STOP_CONDITION(nearBy(WM.robotPosition, WM.goal)); TAO_NEXT(NextFirstReady) { TAO_NEXT_PLAN(Switch); TAO_NEXT_PLAN(TurnToGoal); }

(C)2014 Roi Yehoshua TAO_PLAN(Switch) { TAO_START_CONDITION(nearBy(WM.robotPosition, WM.goal)); cout<<"TAO_PLAN(MoveToGoal)"<<endl; TAO_ALLOCATE_EMPTY; changeGoal(); TAO_STOP_CONDITION(true); TAO_NEXT(NextFirstReady) { TAO_NEXT_PLAN(TurnToGoal); } TAO_END } TAO_PLAN(Switch) { TAO_START_CONDITION(nearBy(WM.robotPosition, WM.goal)); cout<<"TAO_PLAN(MoveToGoal)"<<endl; TAO_ALLOCATE_EMPTY; changeGoal(); TAO_STOP_CONDITION(true); TAO_NEXT(NextFirstReady) { TAO_NEXT_PLAN(TurnToGoal); } TAO_END }

TAO_START_PROTOCOL – makes all the team members begin the same plan together Should be called immediately after TAO_START_CONDITION Uses a Barrier to synchronize team members so they all start the behavior together (C)2014 Roi Yehoshua

Move To Goal Turn To Goal Switch Goal Start TAO_START_PROTOCOL Robots synchronize changing of their goals

(C)2014 Roi Yehoshua TAO_PLAN(Switch) { TAO_START_CONDITION(nearBy(WM.robotPosition, WM.goal)); cout<<"TAO_PLAN(MoveToGoal)"<<endl; TAO_START_PROTOCOL TAO_ALLOCATE_EMPTY; changeGoal(); TAO_STOP_CONDITION(true); TAO_NEXT(NextFirstReady) { TAO_NEXT_PLAN(TurnToGoal); } TAO_PLAN(Switch) { TAO_START_CONDITION(nearBy(WM.robotPosition, WM.goal)); cout<<"TAO_PLAN(MoveToGoal)"<<endl; TAO_START_PROTOCOL TAO_ALLOCATE_EMPTY; changeGoal(); TAO_STOP_CONDITION(true); TAO_NEXT(NextFirstReady) { TAO_NEXT_PLAN(TurnToGoal); }

To define a protocol for choosing a next plan that all team members will commit to: – Create a class that inherits from decision_making::SynchProtocolNext – Implement the pure virtual function synch_decide() – Call makeDecision at the end of the function in order to store the decision in shared memory – Decision is stored as an Int32 variable, which represents the chosen plan number This is the place to implement any voting algorithm for choosing the next plan (C)2014 Roi Yehoshua

Decision process in a synchronized next protocol: (C)2014 Roi Yehoshua bool decide(){ team()->define("next_barrier"); team()->define("next_decision"); teamwork::Barrier wait_start = team()->barrier("next_barrier"); dec = Int32(); synch_decide(); if( getLowAgentName() == self()->name){ team()->mem("next_decision") = dec; } teamwork::Barrier wait_decision = team()->barrier("next_barrier"); dec = team()->mem("next_decision"); return setDecision(dec.data); } bool decide(){ team()->define("next_barrier"); team()->define("next_decision"); teamwork::Barrier wait_start = team()->barrier("next_barrier"); dec = Int32(); synch_decide(); if( getLowAgentName() == self()->name){ team()->mem("next_decision") = dec; } teamwork::Barrier wait_decision = team()->barrier("next_barrier"); dec = team()->mem("next_decision"); return setDecision(dec.data); }

Robots decide in which direction to turn to goal (left or right) in a synchronized manner (C)2014 Roi Yehoshua Move To Goal Turn To Goal Switch Goal Start Turn Left Turn Right NextRandomSync

(C)2014 Roi Yehoshua class NextRandomSync:public decision_making::SynchProtocolNext{ public: NextRandomSync(int& res, decision_making::CallContext* call_context, decision_making::EventQueue* events):SynchProtocolNext(res, call_context, events){} bool synch_decide(){ vector ready_index; for(size_t i=0; i<options.size(); i++) if( options[i].isReady) ready_index.push_back(i); if (ready_index.size() == 0) return false; int i = randomizer.uniformInteger(0, ready_index.size() - 1); return makeDecision(options[ready_index[i]].id); } }; class NextRandomSync:public decision_making::SynchProtocolNext{ public: NextRandomSync(int& res, decision_making::CallContext* call_context, decision_making::EventQueue* events):SynchProtocolNext(res, call_context, events){} bool synch_decide(){ vector ready_index; for(size_t i=0; i<options.size(); i++) if( options[i].isReady) ready_index.push_back(i); if (ready_index.size() == 0) return false; int i = randomizer.uniformInteger(0, ready_index.size() - 1); return makeDecision(options[ready_index[i]].id); } };

(C)2014 Roi Yehoshua TAO_PLAN(TurnToGoal) { TAO_START_CONDITION(not nearBy(WM.robotPosition, WM.goal)); cout<<"TAO_PLAN(TurnToGoal)"<<endl; TAO_ALLOCATE_EMPTY; TAO_STOP_CONDITION(true); TAO_NEXT(NextRandomSync) { TAO_NEXT_PLAN(TurnToGoalLeft); TAO_NEXT_PLAN(TurnToGoalRight); } TAO_PLAN(TurnToGoal) { TAO_START_CONDITION(not nearBy(WM.robotPosition, WM.goal)); cout<<"TAO_PLAN(TurnToGoal)"<<endl; TAO_ALLOCATE_EMPTY; TAO_STOP_CONDITION(true); TAO_NEXT(NextRandomSync) { TAO_NEXT_PLAN(TurnToGoalLeft); TAO_NEXT_PLAN(TurnToGoalRight); }

(C)2014 Roi Yehoshua TAO_PLAN(TurnToGoalLeft) { TAO_START_CONDITION(true); cout<<"TAO_PLAN(TurnToGoalLeft)"<<endl; TAO_ALLOCATE_EMPTY; TAO_CONTEXT.parameters ().turnLeft = true; TAO_CALL_TASK(TurnToGoal); TAO_STOP_CONDITION(headingOnTarget(WM.robotPosition, WM.goal)); TAO_NEXT(NextFirstReady) { TAO_NEXT_PLAN(MoveToGoal); } TAO_CLEANUP_BGN { publishVelocity(0,0); } TAO_CLEANUP_END } TAO_PLAN(TurnToGoalLeft) { TAO_START_CONDITION(true); cout<<"TAO_PLAN(TurnToGoalLeft)"<<endl; TAO_ALLOCATE_EMPTY; TAO_CONTEXT.parameters ().turnLeft = true; TAO_CALL_TASK(TurnToGoal); TAO_STOP_CONDITION(headingOnTarget(WM.robotPosition, WM.goal)); TAO_NEXT(NextFirstReady) { TAO_NEXT_PLAN(MoveToGoal); } TAO_CLEANUP_BGN { publishVelocity(0,0); } TAO_CLEANUP_END }

(C)2014 Roi Yehoshua TAO_PLAN(TurnToGoalRight) { TAO_START_CONDITION(true); cout<<"TAO_PLAN(TurnToGoalRight)"<<endl; TAO_ALLOCATE_EMPTY; TAO_CONTEXT.parameters ().turnLeft = false; TAO_CALL_TASK(TurnToGoal); TAO_STOP_CONDITION(headingOnTarget(WM.robotPosition, WM.goal)); TAO_NEXT(NextFirstReady) { TAO_NEXT_PLAN(MoveToGoal); } TAO_CLEANUP_BGN { publishVelocity(0,0); } TAO_CLEANUP_END } TAO_PLAN(TurnToGoalRight) { TAO_START_CONDITION(true); cout<<"TAO_PLAN(TurnToGoalRight)"<<endl; TAO_ALLOCATE_EMPTY; TAO_CONTEXT.parameters ().turnLeft = false; TAO_CALL_TASK(TurnToGoal); TAO_STOP_CONDITION(headingOnTarget(WM.robotPosition, WM.goal)); TAO_NEXT(NextFirstReady) { TAO_NEXT_PLAN(MoveToGoal); } TAO_CLEANUP_BGN { publishVelocity(0,0); } TAO_CLEANUP_END }

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua To define your own allocation protocol: – Create a class that inherits from decision_making::SynchProtocolAllocation – Implement the pure virtual function synch_decide() – Split the team into sub-teams and assign for each subteam its subplan – Call makeDecision for each subteam with its chosen plan ID

(C)2014 Roi Yehoshua In the Formation example, the team of robots is split into a leader (the agent with the lowest number) and followers For that purpose a custom allocation protocol AllocLowToLeaderSync was defined

(C)2014 Roi Yehoshua class AllocLowToLeaderSync : public decision_making::SynchProtocolAllocation { public: AllocLowToLeaderSync(int& res, decision_making::CallContext* call_context, decision_making::EventQueue* events):SynchProtocolAllocation(res, call_context, events) {} bool synch_decide() { ROS_INFO("Start decision"); int leaderBehaviorId; int followerBehaviorId; for (int i = 0; i < options.size(); ++i) { if (options[i].name == "Leader") leaderBehaviorId = options[i].id; if (options[i].name == "Follower") followerBehaviorId = options[i].id; } vector agents = team()->get_all_agents_names(); sort(agents.begin(), agents.end()); team()->subteam("leader")->add(team()->agent(agents[0])); makeDecision(team()->subteam("leader"), leaderBehaviorId); class AllocLowToLeaderSync : public decision_making::SynchProtocolAllocation { public: AllocLowToLeaderSync(int& res, decision_making::CallContext* call_context, decision_making::EventQueue* events):SynchProtocolAllocation(res, call_context, events) {} bool synch_decide() { ROS_INFO("Start decision"); int leaderBehaviorId; int followerBehaviorId; for (int i = 0; i < options.size(); ++i) { if (options[i].name == "Leader") leaderBehaviorId = options[i].id; if (options[i].name == "Follower") followerBehaviorId = options[i].id; } vector agents = team()->get_all_agents_names(); sort(agents.begin(), agents.end()); team()->subteam("leader")->add(team()->agent(agents[0])); makeDecision(team()->subteam("leader"), leaderBehaviorId);

(C)2014 Roi Yehoshua for (size_t i = 1; i < agents.size(); ++i) { string followerName = "follower" + boost::lexical_cast (i); team()->subteam(followerName)->add(team()->agent(agents[i])); makeDecision(team()->subteam(followerName), followerBehaviorId); } ROS_INFO("End decision"); return true; } }; for (size_t i = 1; i < agents.size(); ++i) { string followerName = "follower" + boost::lexical_cast (i); team()->subteam(followerName)->add(team()->agent(agents[i])); makeDecision(team()->subteam(followerName), followerBehaviorId); } ROS_INFO("End decision"); return true; } };

(C)2014 Roi Yehoshua TAO(Formation) { TAO_PLANS { Move } TAO_START_PLAN(Move); TAO_BGN { TAO_PLAN(Move) { TAO_START_CONDITION(true); ROS_INFO("Wait for barrier 'move'"); TAO_START_PROTOCOL ROS_INFO("Wait for barrier 'move'...DONE"); TAO_TEAM->define("leader_pos"); TAO_ALLOCATE(AllocLowToLeaderSync) { TAO_SUBPLAN(Leader); TAO_SUBPLAN(Follower); } TAO_STOP_CONDITION(false); TAO_NEXT_EMPTY; } TAO_END } TAO(Formation) { TAO_PLANS { Move } TAO_START_PLAN(Move); TAO_BGN { TAO_PLAN(Move) { TAO_START_CONDITION(true); ROS_INFO("Wait for barrier 'move'"); TAO_START_PROTOCOL ROS_INFO("Wait for barrier 'move'...DONE"); TAO_TEAM->define("leader_pos"); TAO_ALLOCATE(AllocLowToLeaderSync) { TAO_SUBPLAN(Leader); TAO_SUBPLAN(Follower); } TAO_STOP_CONDITION(false); TAO_NEXT_EMPTY; } TAO_END }

(C)2014 Roi Yehoshua TAO(Leader) { TAO_PLANS { Wander } TAO_START_PLAN(Wander); TAO_BGN { TAO_PLAN(Wander) { TAO_START_CONDITION(true); TAO_CALL_TASK(SendPosition); TAO_CALL_TASK(Wandering); TAO_ALLOCATE_EMPTY; TAO_STOP_CONDITION(false); TAO_NEXT_EMPTY; } TAO_END } TAO(Leader) { TAO_PLANS { Wander } TAO_START_PLAN(Wander); TAO_BGN { TAO_PLAN(Wander) { TAO_START_CONDITION(true); TAO_CALL_TASK(SendPosition); TAO_CALL_TASK(Wandering); TAO_ALLOCATE_EMPTY; TAO_STOP_CONDITION(false); TAO_NEXT_EMPTY; } TAO_END }

(C)2014 Roi Yehoshua TAO(Follower) { TAO_PLANS { Follow } TAO_START_PLAN(Follow); TAO_BGN { TAO_PLAN(Follow) { TAO_START_CONDITION(true); TAO_CALL_TASK(Follow); TAO_ALLOCATE_EMPTY; TAO_STOP_CONDITION(false); TAO_NEXT_EMPTY; } TAO_END } TAO(Follower) { TAO_PLANS { Follow } TAO_START_PLAN(Follow); TAO_BGN { TAO_PLAN(Follow) { TAO_START_CONDITION(true); TAO_CALL_TASK(Follow); TAO_ALLOCATE_EMPTY; TAO_STOP_CONDITION(false); TAO_NEXT_EMPTY; } TAO_END }

Extend the patrol example to a case where the robots start at random locations in the environment Implement a custom allocation protocol that allocates the wall segments to the nearest robots (C)2014 Roi Yehoshua