| Note: This tutorial assumes that you have completed the previous tutorials: Pause and Resume 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. |
React on Termination
Description: This tutorial shows how a task can react just before dying.Tutorial Level: INTERMEDIATE
In teer, tasks can be killed. However, it might be necessary for a task to perform some functions just before dying, for instance killing a child task or flushing some files. This tutorial shows how to do this. Like the previous tutorial, this one requires turtlesim.
In this tutorial, one turtle does zigzag coverage and a second does situation awareness. When the second meets the first, it takes control of it, moves it to a certain position, and restarts its task. This tutorial is based on turtle_coverage.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 tutorials, the zigzag behaviour is implemented using two tasks, turtle1_go and turtle1_coverage. One novelty is the catching of the GeneratorExit exception, which allows turtle1_coverage to kill turtle1_go when killed. This provides a flexible mechanism to implement parent/child relationship between tasks:
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.2)
7
8 def turtle1_coverage():
9 yield WaitCondition(lambda: sched.turtle1_pose is not None)
10 try:
11 l = round(sched.turtle1_pose.y/2.)*2
12 if l > 8:
13 l = 2
14 sched.printd('Restarting at ' + str(l))
15 while True:
16 targets = [(2,l), (8,l), (9,l+0.5), (8,l+1), (2,l+1), (1,l+1.5)]
17 for target in targets:
18 go_tid = sched.new_task(turtle1_go(target))
19 yield WaitCondition(lambda: dist(sched.turtle1_pose, target) < 0.1)
20 sched.kill_task(go_tid)
21 l += 2
22 if l > 8:
23 l = 2
24 except GeneratorExit:
25 sched.kill_task(go_tid)
26 raise
The situation-awareness behaviour is implemented using a pair of turtle2_orbit and turtle2_orbiting tasks, the latter being the task taking control of the coverage turtle (turtle 1):
1 def turtle1_align(target_angle):
2 diff = angle_diff(sched.turtle1_pose.theta, target_angle)
3 sign_diff = math.copysign(1, diff)
4 while True:
5 # set new speed commands
6 turtle1_velocity.publish(Velocity(0,-sign_diff))
7 # wait for 1 s
8 yield WaitDuration(1)
9
10 def turtle2_orbit(target):
11 while True:
12 # set new speed commands
13 turtle2_velocity.publish(orbit_command(sched.turtle2_pose, target, 1.0))
14 # wait for 1 s
15 yield WaitDuration(0.2)
16
17 def turtle2_orbiting():
18 yield WaitCondition(lambda: sched.turtle2_pose is not None)
19 coverage_tid = sched.new_task(turtle1_coverage())
20 orbit_tid = sched.new_task(turtle2_orbit((5, 5.5)))
21 while True:
22 yield WaitCondition(lambda: dist(sched.turtle1_pose, sched.turtle2_pose) < 1)
23 sched.printd('Met friend, exchange data')
24 sched.pause_task(orbit_tid)
25 turtle2_velocity.publish(Velocity(0,0))
26 sched.kill_task(coverage_tid)
27 if sched.turtle1_pose.y < 5.5:
28 align_tid = sched.new_task(turtle1_align(math.pi/2))
29 yield WaitCondition(lambda: abs(angle_diff(sched.turtle1_pose.theta, math.pi/2)) < 0.1)
30 sched.kill_task(align_tid)
31 target = (sched.turtle1_pose.x, sched.turtle1_pose.y + 3)
32 else:
33 align_tid = sched.new_task(turtle1_align(-math.pi/2))
34 yield WaitCondition(lambda: abs(angle_diff(sched.turtle1_pose.theta, -math.pi/2)) < 0.1)
35 sched.kill_task(align_tid)
36 target = (sched.turtle1_pose.x, sched.turtle1_pose.y - 3)
37 go_tid = sched.new_task(turtle1_go(target))
38 yield WaitCondition(lambda: dist(sched.turtle1_pose, target) < 0.1)
39 sched.kill_task(go_tid)
40 sched.printd('Meeting done')
41 sched.resume_task(orbit_tid)
42 coverage_tid = sched.new_task(turtle1_coverage())
43 yield WaitDuration(6)
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(turtle2_orbiting())
11
12 # connect to turtlesim
13 rospy.init_node('teer_example_turtle')
14 # services
15 rospy.wait_for_service('reset')
16 reset_simulator = rospy.ServiceProxy('reset', EmptyServiceCall)
17 reset_simulator()
18 rospy.wait_for_service('clear')
19 clear_background = rospy.ServiceProxy('clear', EmptyServiceCall)
20 spawn_turtle = rospy.ServiceProxy('spawn', Spawn)
21 spawn_turtle(0,0,0, "turtle2")
22 rospy.wait_for_service('turtle1/set_pen')
23 turtle1_set_pen = rospy.ServiceProxy('turtle1/set_pen', SetPen)
24 rospy.wait_for_service('turtle1/teleport_absolute')
25 turtle1_teleport = rospy.ServiceProxy('turtle1/teleport_absolute', TeleportAbsolute)
26 rospy.wait_for_service('turtle2/set_pen')
27 turtle2_set_pen = rospy.ServiceProxy('turtle2/set_pen', SetPen)
28 rospy.wait_for_service('turtle2/teleport_absolute')
29 turtle2_teleport = rospy.ServiceProxy('turtle2/teleport_absolute', TeleportAbsolute)
30 # subscriber/publisher
31 rospy.Subscriber('turtle1/pose', Pose, turtle1_pose_updated)
32 turtle1_velocity = rospy.Publisher('turtle1/command_velocity', Velocity)
33 rospy.Subscriber('turtle2/pose', Pose, turtle2_pose_updated)
34 turtle2_velocity = rospy.Publisher('turtle2/command_velocity', Velocity)
35
36 # setup environment
37 turtle1_set_pen(0,0,0,0,1)
38 turtle2_set_pen(0,0,0,0,1)
39 turtle1_teleport(2,2,0)
40 turtle2_teleport(5.5,9,0)
41 clear_background()
42
43 # run scheduler
44 sched.run()
This concludes the tutorial series on teer. If you have additional questions feel free to ask them on answers.ros.org or if you want to contribute to teer, do not hesitate to contact us or to discuss on the ROS mailing list.
