Multi-Robot Systems with ROS Lesson 12

Teaching Assistant: Roi Yehoshuaroiyeho@gmail.com

(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