1 / 27

Multi-Robot Systems with ROS Lesson 12

Multi-Robot Systems with ROS Lesson 12. Teaching Assistant: Roi Yehoshua roiyeho@gmail.com. Agenda. TAO team decisions Synchronized Next protocols Synchronized Alloc protocols Patrolling and formation examples. TAO Team Decisions. Time synchronization Start plan synchronization

lynda
Download Presentation

Multi-Robot Systems with ROS Lesson 12

An Image/Link below is provided (as is) to download presentation Download Policy: Content on the Website is provided to you AS IS for your information and personal use and may not be sold / licensed / shared on other websites without getting consent from its author. Content is provided to you AS IS for your information and personal use only. Download presentation by click this link. While downloading, if for some reason you are not able to download a presentation, the publisher may have deleted the file from their server. During download, if you can't get a presentation, the file might be deleted by the publisher.

E N D

Presentation Transcript

  1. Multi-Robot Systems with ROS Lesson 12 Teaching Assistant: RoiYehoshua roiyeho@gmail.com

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

  3. 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

  4. 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

  5. PatrolAsync Move To Goal Switch Goal Start Turn To Goal (C)2014 Roi Yehoshua

  6. 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

  7. 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

  8. 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

  9. 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

  10. PatrolSync • Robots synchronize changing of their goals TAO_START_PROTOCOL Move To Goal Switch Goal Start Turn To Goal (C)2014 Roi Yehoshua

  11. PatrolSync TAO Machine 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); } } (C)2014 Roi Yehoshua

  12. 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

  13. 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"); returnsetDecision(dec.data); } (C)2014 Roi Yehoshua

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

  15. NextRandomSync Class classNextRandomSync:publicdecision_making::SynchProtocolNext{ public: NextRandomSync(int& res, decision_making::CallContext* call_context, decision_making::EventQueue* events):SynchProtocolNext(res, call_context, events){} boolsynch_decide(){ vector<int> ready_index; for(size_ti=0; i<options.size(); i++) if( options[i].isReady) ready_index.push_back(i); if (ready_index.size() == 0) returnfalse; inti = randomizer.uniformInteger(0, ready_index.size() - 1); returnmakeDecision(options[ready_index[i]].id); } }; (C)2014 Roi Yehoshua

  16. 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

  17. 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

  18. 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

  19. 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

  20. 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

  21. 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

  22. Custom Allocation Protocol classAllocLowToLeaderSync : publicdecision_making::SynchProtocolAllocation { public: AllocLowToLeaderSync(int& res, decision_making::CallContext* call_context, decision_making::EventQueue* events):SynchProtocolAllocation(res, call_context, events) {} boolsynch_decide() { ROS_INFO("Start decision"); intleaderBehaviorId; intfollowerBehaviorId; for (inti = 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

  23. Custom Allocation Protocol for (size_ti = 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"); returntrue; } }; (C)2014 Roi Yehoshua

  24. 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

  25. 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

  26. 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

  27. 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 (C)2014 Roi Yehoshua

More Related