Teaching Assistant: Roi Yehoshua
roiyeho@gmail.com
Agenda
•
•
•
•
TAO events
Allocating sub-plans
Defining custom Next protocols
Wandering robot example
(C)2014 Roi Yehoshua
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
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
writing @ at the beggining of the name.
– Example: @/Con.*/Event[NT]...e
(C)2014 Roi Yehoshua
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
Events Outside TAO
• Defining an 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
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
Events Interface
Function
Event waitEvent()
Block function until a new event arrives
Event tryGetEvent(bool&
success)
Get a new event if exists (non-blocking call)
bool isTerminated()
Check if the event system is closed
void drop_all()
Clear queue
void close()
Close event system, releasing all waiting processes
spinOne()
Send one /SPIN event for validation of STOP
conditions
spin(double rate_in_hz =
10)
Run spinOne() command with rate_in_hz
async_spin(double
rate_in_hz = 10, double
start_delay=0)
Run spin command after start_delay without
blocking current code execution
(C)2014 Roi Yehoshua
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
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
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
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:
$ rostopic pub decision_making/[NODE_NAME]/events std_msgs/String
"EVENT_NAME"
• For example, to send a STOP event to the
tao_events node, type:
rostopic pub decision_making/tao_events/events std_msgs/String "STOP"
(C)2014 Roi Yehoshua
Sending Events Example
(C)2014 Roi Yehoshua
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
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
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
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
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
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
Allocating SubPlans Demo
(C)2014 Roi Yehoshua
Decision Graph
(C)2014 Roi Yehoshua
Wandering Robot Plan
• Now we will write a TAO plan for a wandering robot
• The wandering plan will be composed of two subplans:
– 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
Wandering.cpp (1)
#include <iostream>
#include
#include
#include
#include
<ros/ros.h>
<random_numbers/random_numbers.h>
<sensor_msgs/LaserScan.h>
<geometry_msgs/Twist.h>
#include
#include
#include
#include
<decision_making/TAO.h>
<decision_making/TAOStdProtocols.h>
<decision_making/ROSTask.h>
<decision_making/DecisionMaking.h>
using namespace std;
using namespace decision_making;
(C)2014 Roi Yehoshua
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
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
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
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
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
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
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
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
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
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
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
1.57 -urdf -param robot_description -model lizi1" output="screen"/>
<node name="wandering" pkg="tao_wandering" type="wandering_node" output="screen" />
</launch>
(C)2014 Roi Yehoshua
Wandering Demo
(C)2014 Roi Yehoshua
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
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
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
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
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)
{
TAO_START_CONDITION(true);
TAO_ALLOCATE_EMPTY;
WM.turnLeft = true;
TAO_CALL_TASK(turnTask);
TAO_STOP_CONDITION(true);
TAO_NEXT_EMPTY;
WM.turnFinished = true;
}
}
TAO_END
}
(C)2014 Roi Yehoshua
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
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
Wandering Demo
(C)2014 Roi Yehoshua
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