(C)2014 Roi Yehoshua
Agenda• TAO team decisions• Synchronized Next protocols• Synchronized Alloc protocols• Patrolling and formation examples
(C)2014 Roi Yehoshua
TAO Team Decisions• 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
Patrolling Example• 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
PatrolAsync
Move To
Goal
Turn To Goal
SwitchGoal Start
(C)2014 Roi Yehoshua
PatrolAsync TAO Machine (1)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
PatrolAsync TAO Machine (2) 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
PatrolAsync TAO Machine (3) 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}
(C)2014 Roi Yehoshua
Synchronizing the Start of Behavior• 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
PatrolSync
Move To
Goal
Turn To Goal
SwitchGoal Start
TAO_START_PROTOCOL
• Robots synchronize changing of their goals
(C)2014 Roi Yehoshua
PatrolSync TAO MachineTAO_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); }}
(C)2014 Roi Yehoshua
Synchronizing Next Plan Selection• 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
Synchronizing Next Plan Selection• Decision process in a synchronized next protocol:
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);}
(C)2014 Roi Yehoshua
• Robots decide in which direction to turn to goal (left or right) in a synchronized manner
PatrolSyncNext
Move To
Goal
Turn To
Goal
SwitchGoal StartTurn
LeftTurn Right
NextRandomSync
(C)2014 Roi Yehoshua
NextRandomSync Classclass 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<int> 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
PatrolSyncNext TAO Machine (1)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
PatrolSyncNext TAO Machine (2)TAO_PLAN(TurnToGoalLeft){ TAO_START_CONDITION(true); cout<<"TAO_PLAN(TurnToGoalLeft)"<<endl; TAO_ALLOCATE_EMPTY; TAO_CONTEXT.parameters<WorldModel>().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
PatrolSyncNext TAO Machine (3)TAO_PLAN(TurnToGoalRight){ TAO_START_CONDITION(true); cout<<"TAO_PLAN(TurnToGoalRight)"<<endl; TAO_ALLOCATE_EMPTY; TAO_CONTEXT.parameters<WorldModel>().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
patrol_sync_next.launch<launch> <!-- Simulation ====================================================== --> <include file="$(find lizi_description)/launch/lizi_wall.launch" /> <node name="shared_memory" pkg="teamwork" type="shared_memory" /> <node name="patrol_1" pkg="dm_teamwork_examples" type="patrol_sync_next" args="1" /> <node name="patrol_2" pkg="dm_teamwork_examples" type="patrol_sync_next" args="2" /></launch>
(C)2014 Roi Yehoshua
Custom Allocation Protocol• 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
Formation Example• 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
Custom Allocation Protocolclass 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<string> 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
Custom Allocation Protocol for (size_t i = 1; i < agents.size(); ++i) { string followerName = "follower" + boost::lexical_cast<string>(i); team()->subteam(followerName)->add(team()->agent(agents[i])); makeDecision(team()->subteam(followerName), followerBehaviorId); } ROS_INFO("End decision"); return true; }};
(C)2014 Roi Yehoshua
Formation TAO Machine (1)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
Formation TAO Machine (2)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
Formation TAO Machine (3)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}
(C)2014 Roi Yehoshua
Homework (not for submission)• 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