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.

Wiki: executive_teer/Tutorials/On Termination (last edited 2012-02-24 16:32:41 by StephaneMagnenat)