| Note: This tutorial assumes that you have completed the previous tutorials: Wait on Multiple Tasks. |
 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Wait for Conditions
Description: This tutorial shows you how to wait for conditions to become true.Tutorial Level: BEGINNER
Next Tutorial: Pause and Resume Tasks
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 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 lonely_turtle.py from teer_example_turtle. It uses some helper functions available in 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:
1 #!/usr/bin/env python
2 import math
3 import roslib; roslib.load_manifest('teer_tutorials')
4 import rospy
5 import numpy as np
6 from turtlesim.msg import Velocity
7 from turtlesim.msg import Pose
8 from turtlesim.srv import TeleportAbsolute
9 from turtlesim.srv import SetPen
10 from std_srvs.srv import Empty as EmptyServiceCall
11 from turtle_math import *
12 from teer_ros import *
13
14 turtle1_velocity = None
Then, as we want to use a condition variable, we have to subclass Scheduler and create the condition variable in it:
We can then create our tasks. We employ two tasks turtle1_go and turtle1_task. The first publishes turtlesim/Velocity commands every half second, while the second task takes care of following the square path:
1 def turtle1_go(target):
2
3 while True:
4 # set new speed commands
5 turtle1_velocity.publish(control_command(sched.turtle1_pose, target))
6 # wait for 1 s
7 yield WaitDuration(0.5)
8
9 def turtle1_task():
10 yield WaitCondition(lambda: sched.turtle1_pose is not None)
11
12 targets = [(2,2), (8,2), (8,8), (2,8)]
13 target_id = 0
14 while True:
15 sched.printd('Going to ' + str(targets[target_id]))
16 target = targets[target_id]
17 go_tid = sched.new_task(turtle1_go(target))
18 yield WaitCondition(lambda: dist(sched.turtle1_pose, target) < 0.1)
19 sched.kill_task(go_tid)
20 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.
Finally, we just have to create the main function with the glue to connect to ROS:
1 if __name__ == '__main__':
2 # create scheduler
3 sched = TurtleScheduler()
4 sched.new_task(turtle1_task())
5
6 # connect to turtlesim
7 rospy.init_node('teer_example_turtle')
8 # services
9 rospy.wait_for_service('reset')
10 reset_simulator = rospy.ServiceProxy('reset', EmptyServiceCall)
11 reset_simulator()
12 rospy.wait_for_service('clear')
13 clear_background = rospy.ServiceProxy('clear', EmptyServiceCall)
14 rospy.wait_for_service('turtle1/set_pen')
15 turtle1_set_pen = rospy.ServiceProxy('turtle1/set_pen', SetPen)
16 rospy.wait_for_service('turtle1/teleport_absolute')
17 turtle1_teleport = rospy.ServiceProxy('turtle1/teleport_absolute', TeleportAbsolute)
18 # subscriber/publisher
19 rospy.Subscriber('turtle1/pose', Pose, turtle1_pose_updated)
20 turtle1_velocity = rospy.Publisher('turtle1/command_velocity', Velocity)
21
22 # setup environment
23 turtle1_set_pen(0,0,0,0,1)
24 turtle1_teleport(2,2,0)
25 clear_background()
26
27 # run scheduler
28 sched.run()
Of course, we can easily add more turtles, as we will see in the next tutorial.
