## For instruction on writing tutorials
## http://www.ros.org/wiki/WritingTutorials
####################################
##FILL ME IN
####################################
## for a custom note with links:
## note =
## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links 
## note.0= [[executive_teer/Tutorials/Wait Multiple|Wait on Multiple Tasks]]
## descriptive title for the tutorial
## title = Wait for Conditions
## multi-line description to be displayed in search 
## description = This tutorial shows you how to wait for conditions to become true.
## the next tutorial description (optional)
## next =
## links to next tutorial (optional)
## next.0.link=[[executive_teer/Tutorials/Pause Resume|Pause and Resume Tasks]]
## next.1.link=
## what level user is this tutorial for 
## level= BeginnerCategory
## keywords =
####################################

<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>

<<TableOfContents(4)>>

== About condition variables ==

Until now, we have seen tasks waiting a certain duration or for the termination of other tasks. However, in robotics it is often required to wait for conditions that depend on the outside world, for instance on the sensors. Teer provides this through ''condition variables''. A condition variable looks like a normal variable, excepted that when it is assigned, the scheduler checks tasks that wait on conditions referring to this variable, and if true, schedule these tasks. Condition variables are implemented using Python's [[http://users.rcn.com/python/download/Descriptor.htm|descriptor mechanism]] and a bit of introspection voodoo. Therefore, condition variables must be members of a subclass of `Scheduler`.

== Turtlesim ==

As we want to wait on a value for the external world, we will use [[turtlesim]] for this tutorial. Therefore, you have to add it as a dependency to the manifest:
{{{
<depend package="turtlesim"/>
}}}
The example we will walk through in this tutorial is based on [[https://github.com/ethz-asl/executive_teer/blob/master/teer_example_turtle/src/lonely_turtle.py|lonely_turtle.py]] from [[teer_example_turtle]]. It uses some helper functions available in [[https://raw.github.com/ethz-asl/executive_teer/master/teer_example_turtle/src/turtle_math.py|turtle_math.py]], which you must download to your tutorial's directory. You must run `turtlesim_node` before launching your node.

== Wait for a condition ==

First, we import the packages, including the message definitions from turtlesim:
{{{
#!Python
#!/usr/bin/env python
import math
import roslib; roslib.load_manifest('teer_tutorials')
import rospy
import numpy as np
from turtlesim.msg import Velocity
from turtlesim.msg import Pose
from turtlesim.srv import TeleportAbsolute
from turtlesim.srv import SetPen
from std_srvs.srv import Empty as EmptyServiceCall
from turtle_math import *
from teer_ros import *

turtle1_velocity = None
}}}

Then, as we want to use a condition variable, we have to subclass `Scheduler` and create the condition variable in it:
{{{
#!Python
class TurtleScheduler(Scheduler):
     
    turtle1_pose = ConditionVariable(None)
    
    def __init__(self):
        super(TurtleScheduler,self).__init__()
}}}

We can then create our tasks. We employ two tasks `turtle1_go` and `turtle1_task`. The first publishes <<MsgLink(turtlesim/Velocity)>> commands every half second, while the second task takes care of following the square path:
{{{
#!Python
def turtle1_go(target):
     
    while True:
        # set new speed commands
        turtle1_velocity.publish(control_command(sched.turtle1_pose, target))
        # wait for 1 s
        yield WaitDuration(0.5)

def turtle1_task():
    yield WaitCondition(lambda: sched.turtle1_pose is not None)
    
    targets = [(2,2), (8,2), (8,8), (2,8)]
    target_id = 0
    while True:
        sched.printd('Going to ' + str(targets[target_id]))
        target = targets[target_id]
        go_tid =  sched.new_task(turtle1_go(target))
        yield WaitCondition(lambda: dist(sched.turtle1_pose, target) < 0.1)
        sched.kill_task(go_tid)
        target_id = (target_id + 1) % len(targets)
}}}


We then create the callback for the turtle's pose messages, in which we assign the condition variable. As `turtle1_task` waits on condition involving this variable, these will be re-evaluated when `turtle1_pose` is assigned, and if they evaluate to true, `turtle1_task` will be scheduled for execution.
{{{
#!Python
def turtle1_pose_updated(new_pose):
    sched.turtle1_pose = new_pose
}}}

Finally, we just have to create the main function with the glue to connect to ROS:
{{{
#!Python
if __name__ == '__main__':
    # create scheduler
    sched = TurtleScheduler()
    sched.new_task(turtle1_task())
    
    # connect to turtlesim
    rospy.init_node('teer_example_turtle')
    # services
    rospy.wait_for_service('reset')
    reset_simulator = rospy.ServiceProxy('reset', EmptyServiceCall)
    reset_simulator()
    rospy.wait_for_service('clear')
    clear_background = rospy.ServiceProxy('clear', EmptyServiceCall)
    rospy.wait_for_service('turtle1/set_pen')
    turtle1_set_pen = rospy.ServiceProxy('turtle1/set_pen', SetPen)
    rospy.wait_for_service('turtle1/teleport_absolute')
    turtle1_teleport = rospy.ServiceProxy('turtle1/teleport_absolute', TeleportAbsolute)
    # subscriber/publisher
    rospy.Subscriber('turtle1/pose', Pose, turtle1_pose_updated)
    turtle1_velocity = rospy.Publisher('turtle1/command_velocity', Velocity)
    
    # setup environment
    turtle1_set_pen(0,0,0,0,1)
    turtle1_teleport(2,2,0)
    clear_background()
    
    # run scheduler
    sched.run()
}}}

Of course, we can easily add more turtles, as we will see [[executive_teer/Tutorials/Pause Resume|in the next tutorial]].

## AUTOGENERATED DO NOT DELETE 
## TutorialCategory
## FILL IN THE STACK TUTORIAL CATEGORY HERE