1. Multi-Robot Systems with ROS Lesson 10
Teaching Assistant: Roi Yehoshua

2. Agenda TAO events Allocating sub-plans Defining custom Next protocols
Wandering robot example
(C)2014 Roi Yehoshua

3. TAO Events
TAO defines an event distribution system using an EventQueue
It’s used for sharing events inside TAO machines
It’s possible to insert external events to the system (from ROS or other custom source)
It is thread safe
(C)2014 Roi Yehoshua

4. TAO Event Each Event is a path containing:
all context names
when this event was created
an event short name at the end
Example: /ContextName/EventName
When you compare two events you can use regular expressions as the name of one event by at the beggining of the name.
(C)2014 Roi Yehoshua

5. Events Inside TAO TAO_RAISE(/STOP) - raise global event
TAO_RAISE(STOP) - raise an event related to TAO context
TAO_RAISE(PLAN_NAME/STOP) - raise an event related to TAO PLAN context event
TAO_STOP_CONDITION(event==TAO_EVENT(STOP)) - check condition based on an occurred event
TAO_EVENTS_DROP - Clear current events queue
(C)2014 Roi Yehoshua

6. Events Outside TAO Defining an event: Raising the event:
Event e("/STOP") - global / without context
Event e("STOP",call_context) - related to call_context
Raising the event:
events->raiseEvent(e);
(C)2014 Roi Yehoshua

7. Events Interface
Event waitEvent() - block function until a new event arrives
Event tryGetEvent(bool& success) - get a new event if exists (non-blocking call)
bool isTerminated() const - check if the event system is closed
void drop_all() -
(C)2014 Roi Yehoshua

8. Events Interface Function Block function until a new event arrives
Event waitEvent()
Get a new event if exists (non-blocking call)
Event tryGetEvent(bool& success)
Check if the event system is closed
bool isTerminated()
Clear queue
void drop_all()
Close event system, releasing all waiting processes
void close()
Send one /SPIN event for validation of STOP conditions
spinOne()
Run spinOne() command with rate_in_hz
spin(double rate_in_hz = 10)
Run spin command after start_delay without blocking current code execution
async_spin(double rate_in_hz = 10, double start_delay=0)
(C)2014 Roi Yehoshua

9. RosEventQueue A class derived from decision_making::EventQueue
RosEventQueue creates a connection between ROS (/decision_making/NODE_NAME/events topic) and the internal EventQueue
Must be created after ros::init and ros_decision_making_init
(C)2014 Roi Yehoshua

10. Events Example
In the following example, we will add a support for a STOP event to our Incrementer plan
Copy TaskWithStopCondition.cpp to Events.cpp
In the TAO machine definition, add a clause to the STOP condition that checks if a STOP event has occurred
(C)2014 Roi Yehoshua

11. TaoEvents.cpp TAO(Incrementer) { TAO_PLANS { Increment }
TAO_START_PLAN(Increment);
TAO_BGN {
TAO_PLAN(Increment) {
TAO_START_CONDITION(true);
TAO_CALL_TASK(incrementTask);
TAO_ALLOCATE_EMPTY
TAO_STOP_CONDITION(WM.counter == 100 || event == TAO_EVENT("/STOP"));
TAO_NEXT_EMPTY
TAO_END
(C)2014 Roi Yehoshua

12. Sending Events
Once running, your model will be able to receive events over the decision_making/[NODE_NAME]/events topic 
Events to the model can be sent using:
For example, to send a STOP event to the tao_events node, type:
$ rostopic pub decision_making/[NODE_NAME]/events std_msgs/String "EVENT_NAME"
rostopic pub decision_making/tao_events/events std_msgs/String "STOP"
(C)2014 Roi Yehoshua

13. Sending Events Example
(C)2014 Roi Yehoshua

14. Allocating SubPlans
We will now change the incrementer plan so it allocates two sub-plans:
Even – this subplan will increment the counter when it has an even value
Odd – this subplan will increment the counter when it has an odd value
Copy BasicPlanWithWM.cpp to AllocatingSubPlans.cpp
(C)2014 Roi Yehoshua

15. AllocatingSubPlans.cpp (1)
struct WorldModel : public CallContextParameters {
int counter;
bool even_plan_finished;
bool odd_plan_finished;
string str() const {
stringstream s;
s << "counter = " << counter;
return s.str(); } };
#define WM TAO_CONTEXT.parameters<WorldModel>()
(C)2014 Roi Yehoshua

16. AllocatingSubPlans.cpp (2)
TAO_HEADER(Even)
TAO_HEADER(Odd) 
TAO(Incrementer) {
TAO_PLANS {
Increment }
TAO_START_PLAN(Increment);
TAO_BGN {
TAO_PLAN(Increment) {
TAO_START_CONDITION(WM.counter < 100); 
WM.even_plan_finished = false;
WM.odd_plan_finished = false; 
TAO_ALLOCATE(AllocFirstReady) {
TAO_SUBPLAN(Even);
TAO_SUBPLAN(Odd); } 
TAO_STOP_CONDITION(WM.even_plan_finished || WM.odd_plan_finished); 
TAO_NEXT(NextFirstReady) {
TAO_NEXT_PLAN(Increment);
TAO_END
(C)2014 Roi Yehoshua

17. AllocatingSubPlans.cpp (3)
TAO(Even) {
TAO_PLANS {
Even, }
TAO_START_PLAN(Even);
TAO_BGN {
TAO_PLAN(Even) {
TAO_START_CONDITION(WM.counter % 2 == 0); 
WM.counter++;
cout << "Even: " << WM.str() << endl;
boost::this_thread::sleep(boost::posix_time::milliseconds(100));
TAO_ALLOCATE_EMPTY
TAO_STOP_CONDITION(true);
TAO_NEXT_EMPTY 
WM.even_plan_finished = true;
TAO_END } 
(C)2014 Roi Yehoshua

18. AllocatingSubPlans.cpp (4)
TAO(Odd) {
TAO_PLANS{
Odd, }
TAO_START_PLAN(Odd);
TAO_BGN {
TAO_PLAN(Odd) {
TAO_START_CONDITION(WM.counter % 2 == 1); 
WM.counter++;
cout << "Odd: " << WM.str() << endl;
boost::this_thread::sleep(boost::posix_time::milliseconds(100));
TAO_ALLOCATE_EMPTY
TAO_STOP_CONDITION(true);
TAO_NEXT_EMPTY 
WM.odd_plan_finished = true;
TAO_END
(C)2014 Roi Yehoshua

19. SubPlans Behavior
The sub-plans start executing only after the parent plan reaches its stop condition (and not right after allocation)
If the parent’s stop condition is true before any of its sub-plans started running, then the TAO machine ends
If the parent’s stop condition is false and the parent’s plan has a next plan, then it is unpredictable whether the next plan or the sub-plan starts first
Once a plan starts executing, no other plan can execute at the same time (unless using tasks)
The entire TAO machine ends only when the parent plan and all its sub-plans are finished
(C)2014 Roi Yehoshua

20. Allocating SubPlans Demo
(C)2014 Roi Yehoshua

21. Decision Graph
(C)2014 Roi Yehoshua

22. Wandering Robot Plan
Now we will write a TAO plan for a wandering robot
The wandering plan will be composed of two sub-plans:
Drive – makes the robot drive forward until an obstacle is detected
Turn – makes the robot turn until the way becomes clear
Create a new package called tao_wandering:
$ cd ~/dmw/src
$ catkin_create_pkg tao_wandering roscpp decision_making decision_making_parser random_numbers sensor_msgs std_msgs geometry_msgs
(C)2014 Roi Yehoshua

23. Wandering.cpp (1) #include <iostream> #include <ros/ros.h>
#include <ros/ros.h>
#include <random_numbers/random_numbers.h>
#include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/Twist.h>
#include <decision_making/TAO.h>
#include <decision_making/TAOStdProtocols.h>
#include <decision_making/ROSTask.h>
#include <decision_making/DecisionMaking.h>
using namespace std;
using namespace decision_making;
(C)2014 Roi Yehoshua

24. Wandering.cpp (2) /*** Constants ***/
#define MIN_SCAN_ANGLE_RAD -45.0/180*M_PI
#define MAX_SCAN_ANGLE_RAD +45.0/180*M_PI
#define MIN_DIST_TO_OBSTACLE 0.8 // in meters
/*** Variables ***/
random_numbers::RandomNumberGenerator _randomizer;
ros::Publisher _velocityPublisher;
/*** World model ***/
struct WorldModel : public CallContextParameters {
bool obstacleDetected;
bool driveFinished;
bool turnFinished;
string str() const {
stringstream s;
s << "obstacleDetected = " << obstacleDetected;
return s.str(); } };
#define WM TAO_CONTEXT.parameters<WorldModel>()
(C)2014 Roi Yehoshua

25. Wandering.cpp (3) /*** TAO machine ***/ TAO_HEADER(Drive)
TAO_HEADER(Turn) 
TAO(Wandering) {
TAO_PLANS {
Wandering }
TAO_START_PLAN(Wandering);
TAO_BGN {
TAO_PLAN(Wandering) {
TAO_START_CONDITION(true); 
WM.driveFinished = false;
WM.turnFinished = false;
TAO_ALLOCATE(AllocFirstReady) {
TAO_SUBPLAN(Drive);
TAO_SUBPLAN(Turn); } 
TAO_STOP_CONDITION(WM.driveFinished || WM.turnFinished); 
TAO_NEXT(NextFirstReady) {
TAO_NEXT_PLAN(Wandering);
TAO_END
(C)2014 Roi Yehoshua

26. Wandering.cpp (4) TAO(Drive) { TAO_PLANS { Drive, }
TAO_START_PLAN(Drive);
TAO_BGN {
TAO_PLAN(Drive) {
TAO_START_CONDITION(!WM.obstacleDetected);
TAO_CALL_TASK(driveTask);
TAO_ALLOCATE_EMPTY
TAO_STOP_CONDITION(WM.obstacleDetected);
TAO_NEXT_EMPTY
WM.driveFinished = true;
TAO_END
(C)2014 Roi Yehoshua

27. Wandering.cpp (5) TAO(Turn) { TAO_PLANS{ Turn, } TAO_START_PLAN(Turn);
TAO_BGN {
TAO_PLAN(Turn) {
TAO_START_CONDITION(WM.obstacleDetected);
TAO_CALL_TASK(turnTask);
TAO_ALLOCATE_EMPTY
TAO_STOP_CONDITION(true);
TAO_NEXT_EMPTY
WM.turnFinished = true;
TAO_END
(C)2014 Roi Yehoshua

28. Wandering.cpp (6) /*** Task implementations ***/
TaskResult driveTask(string name, const CallContext& context, EventQueue& eventQueue) {
ROS_INFO("Driving...");
geometry_msgs::Twist forwardMessage;
forwardMessage.linear.x = 1.0;
// Preemptive wait
while (!eventQueue.isTerminated()) {
_velocityPublisher.publish(forwardMessage);
boost::this_thread::sleep(boost::posix_time::milliseconds(100.0)); }
ROS_INFO("Obstacle detected!");
return TaskResult::SUCCESS(); } 
(C)2014 Roi Yehoshua

29. Wandering.cpp (7)
TaskResult turnTask(string name, const CallContext& context, EventQueue& eventQueue) {
ROS_INFO("Turning...");
bool turnLeft = _randomizer.uniformInteger(0, 1);
geometry_msgs::Twist turnMessage;
turnMessage.angular.z = 2 * (turnLeft ? 1 : -1);
int timeToTurnMs = _randomizer.uniformInteger(2000, 4000);
int turnLoops = timeToTurnMs / 100;
for (int i = 0; i < turnLoops; i++) {
_velocityPublisher.publish(turnMessage);
boost::this_thread::sleep(boost::posix_time::milliseconds(100.0)); }
return TaskResult::SUCCESS();
(C)2014 Roi Yehoshua

30. Wandering.cpp (8) /*** ROS Subscriptions ***/
void onLaserScanMessage(const sensor_msgs::LaserScan::Ptr laserScanMessage, CallContext* context) {
bool obstacleFound = false;
int minIndex = ceil((MIN_SCAN_ANGLE_RAD - laserScanMessage->angle_min) / laserScanMessage->angle_increment);
int maxIndex = floor((MAX_SCAN_ANGLE_RAD - laserScanMessage->angle_min) / laserScanMessage->angle_increment);
for (int i = minIndex; i <= maxIndex; i++) {
if (laserScanMessage->ranges[i] < MIN_DIST_TO_OBSTACLE) {
obstacleFound = true; }
context->parameters<WorldModel>().obstacleDetected = obstacleFound;
(C)2014 Roi Yehoshua

31. Wandering.cpp (9) int main(int argc, char **argv) {
ros::init(argc, argv, "wandering_node");
ros::NodeHandle nh;
ros_decision_making_init(argc, argv);
// ROS spinner for topic subscriptions
ros::AsyncSpinner spinner(1);
spinner.start();
// Tasks registration
LocalTasks::registrate("driveTask", driveTask);
LocalTasks::registrate("turnTask", turnTask);
RosEventQueue eventQueue;
CallContext context;
context.createParameters(new WorldModel());
(C)2014 Roi Yehoshua

32. Wandering.cpp (10) // CallContext must define a team
teamwork::SharedMemory db;
teamwork::Teammates teammates;
teamwork::Team main_team = teamwork::createMainTeam(db, "main", teammates);
context.team(TAO_CURRENT_TEAM_NAME, main_team.ptr());
// Subscription for the laser topic and velocity publisher creation
ros::Subscriber laserSubscriber = nh.subscribe<void>("base_scan", 1,
boost::function<void(const sensor_msgs::LaserScan::Ptr)>(boost::bind(onLaserScanMessage, _1, &context)));
_velocityPublisher = nh.advertise<geometry_msgs::Twist>("cmd_vel", 100);
eventQueue.async_spin();
ROS_INFO("Starting wandering machine...");
TaoWandering(&context, &eventQueue);
eventQueue.close();
ROS_INFO("TAO finished.");
return 0; }
(C)2014 Roi Yehoshua

33. Wandering Launch File
Copy worlds directory from ~/catkin_ws/src/gazebo_navigation_multi package
Create a launch directory in the tao_wandering package
Add the following wandering.launch file
(C)2014 Roi Yehoshua

34. Wandering Launch File <launch> <master auto="start"/>
<param name="/use_sim_time" value="true"/>
<!-- start gazebo -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find tao_wandering)/worlds/willowgarage.world" />
</include>
<param name="robot_description" command="$(find xacro)/xacro.py $(find lizi_description)/urdf/lizi.urdf"/>
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-x 34 -y 18 -z 0 -Y urdf -param robot_description -model lizi1" output="screen"/>
<node name="wandering" pkg="tao_wandering" type="wandering_node" output="screen" />
</launch>
(C)2014 Roi Yehoshua

35. Wandering Demo
(C)2014 Roi Yehoshua

36. Custom Next Protocol To define your own next protocol:
Create a class that inherits from decision_making::ProtocolNext
Implement the pure virtual function decide()
Call setDecision at the end of the function with the chosen plan ID
(C)2014 Roi Yehoshua

37. Custom Next Protocol
In the following example, we will decompose the Turn plan into TurnLeft and TurnRight subplans
We will create a custom RandomNext protocol that will randomly choose the next plan from its set of candidate plans
We will use this protocol to make the Turn plan randomly choose between TurnLeft and TurnRight as its continuation plan
(C)2014 Roi Yehoshua

38. RandomNext Class
class NextRandom: public decision_making::ProtocolNext {
public:
NextRandom(int& res, decision_making::CallContext* call_context, decision_making::EventQueue* events):ProtocolNext(res, call_context, events){}
bool 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 setDecision(options[ready_index[i]].id); }
}; 
(C)2014 Roi Yehoshua

39. Turn Plan (1) TAO(Turn) { TAO_PLANS{ Turn, TurnLeft, TurnRight }
TAO_START_PLAN(Turn);
TAO_BGN {
TAO_PLAN(Turn) {
TAO_START_CONDITION(WM.obstacleDetected);
TAO_ALLOCATE_EMPTY
TAO_STOP_CONDITION(true);
TAO_NEXT(NextRandom) {
TAO_NEXT_PLAN(TurnLeft);
TAO_NEXT_PLAN(TurnRight);
(C)2014 Roi Yehoshua

40. Turn Plan (2) TAO_PLAN(TurnLeft) { TAO_START_CONDITION(true);
TAO_ALLOCATE_EMPTY;
WM.turnLeft = false;
TAO_CALL_TASK(turnTask);
TAO_STOP_CONDITION(true);
TAO_NEXT_EMPTY;
WM.turnFinished = true; }
TAO_PLAN(TurnRight)
WM.turnLeft = true;
TAO_END
(C)2014 Roi Yehoshua

41. World Model Changes
Add a boolean variable to the world model indicating which turn direction was chosen:
struct WorldModel : public CallContextParameters {
bool obstacleDetected;
bool driveFinished;
bool turnFinished;
bool turnLeft;
string str() const {
stringstream s;
s << "obstacleDetected = " << obstacleDetected;
return s.str(); } };
#define WM TAO_CONTEXT.parameters<WorldModel>()
(C)2014 Roi Yehoshua

42. Turn Task Make the following changes to the turnTask callback:
TaskResult turnTask(string name, const CallContext& context, EventQueue& eventQueue) {
if (context.parameters<WorldModel>().turnLeft)
ROS_INFO("Turning left...");
else
ROS_INFO("Turning right...");
geometry_msgs::Twist turnMessage;
turnMessage.angular.z = 2 * (context.parameters<WorldModel>().turnLeft ? 1 : -1);
int timeToTurnMs = _randomizer.uniformInteger(2000, 4000);
int turnLoops = timeToTurnMs / 100;
for (int i = 0; i < turnLoops; i++) {
_velocityPublisher.publish(turnMessage);
boost::this_thread::sleep(boost::posix_time::milliseconds(100.0)); } 
return TaskResult::SUCCESS(); }
(C)2014 Roi Yehoshua

43. Wandering Demo
(C)2014 Roi Yehoshua

44. Homework (not for submission)
Create a custom next protocol for the turn plan that will choose between the following 3 plans:
Turn random when there is an obstacle on the front
Turn left when there is an obstacle on the right side
Turn right when there is an obstacle on the left side
Add a support for pausing/resuming the robot by sending an appropriate event
(C)2014 Roi Yehoshua