lonely_turtle.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import math
00003 import roslib; roslib.load_manifest('teer_example_turtle')
00004 import rospy
00005 import numpy as np
00006 from turtlesim.msg import Velocity
00007 from turtlesim.msg import Pose
00008 from turtlesim.srv import TeleportAbsolute
00009 from turtlesim.srv import SetPen
00010 from std_srvs.srv import Empty as EmptyServiceCall
00011 from turtle_math import *
00012 from teer_ros import *
00013 
00014 class TurtleScheduler(Scheduler):
00015         """ A teer scheduler working with ROS """
00016         
00017         turtle1_pose = ConditionVariable(None)
00018         
00019         def __init__(self):
00020                 """ Init the ROS scheduler """
00021                 super(TurtleScheduler,self).__init__()
00022 
00023 def turtle1_go(target):
00024         """ Make turtle1 go to target, giving new speed command every second """
00025         
00026         while True:
00027                 # set new speed commands
00028                 turtle1_velocity.publish(control_command(sched.turtle1_pose, target))
00029                 # wait for 1 s
00030                 yield WaitDuration(0.5)
00031 
00032 def turtle1_task():
00033         """ Make turtle1 do a square in the environment """
00034         yield WaitCondition(lambda: sched.turtle1_pose is not None)
00035         
00036         targets = [(2,2), (8,2), (8,8), (2,8)]
00037         target_id = 0
00038         while True:
00039                 sched.printd('Going to ' + str(targets[target_id]))
00040                 target = targets[target_id]
00041                 go_tid =  sched.new_task(turtle1_go(target))
00042                 yield WaitCondition(lambda: dist(sched.turtle1_pose, target) < 0.1)
00043                 sched.kill_task(go_tid)
00044                 target_id = (target_id + 1) % len(targets)
00045 
00046 def turtle1_pose_updated(new_pose):
00047         """ We received a new pose of turtle1 from turtlesim, update condition variable in scheduler """
00048         sched.turtle1_pose = new_pose
00049 
00050 if __name__ == '__main__':
00051         # create scheduler
00052         sched = TurtleScheduler()
00053         sched.new_task(turtle1_task())
00054         
00055         # connect to turtlesim
00056         rospy.init_node('teer_example_turtle')
00057         # services
00058         rospy.wait_for_service('reset')
00059         reset_simulator = rospy.ServiceProxy('reset', EmptyServiceCall)
00060         reset_simulator()
00061         rospy.wait_for_service('clear')
00062         clear_background = rospy.ServiceProxy('clear', EmptyServiceCall)
00063         rospy.wait_for_service('turtle1/set_pen')
00064         turtle1_set_pen = rospy.ServiceProxy('turtle1/set_pen', SetPen)
00065         rospy.wait_for_service('turtle1/teleport_absolute')
00066         turtle1_teleport = rospy.ServiceProxy('turtle1/teleport_absolute', TeleportAbsolute)
00067         # subscriber/publisher
00068         rospy.Subscriber('turtle1/pose', Pose, turtle1_pose_updated)
00069         turtle1_velocity = rospy.Publisher('turtle1/command_velocity', Velocity)
00070         
00071         # setup environment
00072         turtle1_set_pen(0,0,0,0,1)
00073         turtle1_teleport(2,2,0)
00074         clear_background()
00075         
00076         # run scheduler
00077         sched.run()


teer_example_turtle
Author(s): Stéphane Magnenat
autogenerated on Sat Dec 28 2013 16:57:39