| Note: This tutorial assumes that you have completed the previous tutorials: Wait for Conditions. |
 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. |
Pause and Resume Tasks
Description: This tutorial shows how to pause and resume a list of tasks.Tutorial Level: BEGINNER
Next Tutorial: React On Termination
In teer, a task can pause or resume other tasks. This tutorial shows an example of these functions. Like the previous tutorial, this one requires turtlesim.
In this tutorial, two turtles move in squares of opposite directions. When turtles meet, they fall in love, forget about their doing, and dance for a while; then they become bored and get back to business. This tutorial is based on turtle_dating.py from teer_example_turtle.
First, we import the packages and create the scheduler, this time with two condition variables, one for the pose of each turtle:
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 turtlesim.srv import Spawn
11 from std_srvs.srv import Empty as EmptyServiceCall
12 from turtle_math import *
13 from teer_ros import *
14
15 class TurtleScheduler(Scheduler):
16
17 turtle1_pose = ConditionVariable(None)
18 turtle2_pose = ConditionVariable(None)
19
20 def __init__(self):
21 super(TurtleScheduler,self).__init__()
Like in the previous example, turtle move using two tasks each (turtleN_go and turtleN_wandering):
1 def turtle1_go(target):
2 while True:
3 # set new speed commands
4 turtle1_velocity.publish(control_command(sched.turtle1_pose, target, 1.0))
5 # wait for 1 s
6 yield WaitDuration(0.5)
7
8 def turtle2_go(target):
9 while True:
10 # set new speed commands
11 turtle2_velocity.publish(control_command(sched.turtle2_pose, target, 0.7))
12 # wait for 1 s
13 yield WaitDuration(0.5)
14
15 def turtle1_wandering():
16 yield WaitCondition(lambda: sched.turtle1_pose is not None)
17
18 targets = [(2,2), (9,2), (9,9), (2,9)]
19 target_id = 0
20 while True:
21 sched.printd('Going to ' + str(targets[target_id]))
22 target = targets[target_id]
23 go_tid = sched.new_task(turtle1_go(target))
24 yield WaitCondition(lambda: dist(sched.turtle1_pose, target) < 0.1)
25 sched.kill_task(go_tid)
26 target_id = (target_id + 1) % len(targets)
27
28 def turtle2_wandering():
29 yield WaitCondition(lambda: sched.turtle2_pose is not None)
30
31 targets = [(2,9), (9,9), (9,2), (2,2)]
32 target_id = 0
33 while True:
34 sched.printd('Going to ' + str(targets[target_id]))
35 target = targets[target_id]
36 go_tid = sched.new_task(turtle2_go(target))
37 yield WaitCondition(lambda: dist(sched.turtle2_pose, target) < 0.1)
38 sched.kill_task(go_tid)
39 target_id = (target_id + 1) % len(targets)
In addition, a fifth task, cupidon, waits until turtles are close enough, pauses their tasks, makes them dance, and resumes their tasks afterwards:
1 def cupidon():
2 my_tid = sched.get_current_tid()
3 while True:
4 yield WaitCondition(lambda: dist(sched.turtle1_pose, sched.turtle2_pose) < 1)
5 sched.printd('Found friend, let\'s dance')
6 paused_tasks = sched.pause_all_tasks_except([my_tid])
7 turtle1_set_pen(255,0,0,0,0)
8 turtle2_set_pen(0,255,0,0,0)
9 for i in range(7):
10 turtle1_velocity.publish(Velocity(1, 1))
11 turtle2_velocity.publish(Velocity(1, -1))
12 yield WaitDuration(0.9)
13 turtle1_set_pen(0,0,0,0,1)
14 turtle2_set_pen(0,0,0,0,1)
15 sched.printd('Tired of dancing, going back to wandering')
16 sched.resume_tasks(paused_tasks)
17 yield WaitDuration(10)
Finally, we just have to create the callbacks and the main function with the glue to connect to ROS:
1 def turtle1_pose_updated(new_pose):
2 sched.turtle1_pose = new_pose
3
4 def turtle2_pose_updated(new_pose):
5 sched.turtle2_pose = new_pose
6
7 if __name__ == '__main__':
8 # create scheduler
9 sched = TurtleScheduler()
10 sched.new_task(turtle1_wandering())
11 sched.new_task(turtle2_wandering())
12 sched.new_task(cupidon())
13
14 # connect to turtlesim
15 rospy.init_node('teer_example_turtle')
16 # services
17 rospy.wait_for_service('reset')
18 reset_simulator = rospy.ServiceProxy('reset', EmptyServiceCall)
19 reset_simulator()
20 rospy.wait_for_service('clear')
21 clear_background = rospy.ServiceProxy('clear', EmptyServiceCall)
22 spawn_turtle = rospy.ServiceProxy('spawn', Spawn)
23 spawn_turtle(0,0,0, "turtle2")
24 rospy.wait_for_service('turtle1/set_pen')
25 turtle1_set_pen = rospy.ServiceProxy('turtle1/set_pen', SetPen)
26 rospy.wait_for_service('turtle1/teleport_absolute')
27 turtle1_teleport = rospy.ServiceProxy('turtle1/teleport_absolute', TeleportAbsolute)
28 rospy.wait_for_service('turtle2/set_pen')
29 turtle2_set_pen = rospy.ServiceProxy('turtle2/set_pen', SetPen)
30 rospy.wait_for_service('turtle2/teleport_absolute')
31 turtle2_teleport = rospy.ServiceProxy('turtle2/teleport_absolute', TeleportAbsolute)
32 # subscriber/publisher
33 rospy.Subscriber('turtle1/pose', Pose, turtle1_pose_updated)
34 turtle1_velocity = rospy.Publisher('turtle1/command_velocity', Velocity)
35 rospy.Subscriber('turtle2/pose', Pose, turtle2_pose_updated)
36 turtle2_velocity = rospy.Publisher('turtle2/command_velocity', Velocity)
37
38 # setup environment
39 turtle1_set_pen(0,0,0,0,1)
40 turtle2_set_pen(0,0,0,0,1)
41 turtle1_teleport(2,2,0)
42 turtle2_teleport(2,9,0)
43 clear_background()
44
45 # run scheduler
46 sched.run()
