flap_ears.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 
00003 import roslib
00004 roslib.load_manifest( 'robotis' )
00005 roslib.load_manifest( 'hrl_rfid' )
00006 roslib.load_manifest( 'geometry_msgs' )
00007 import rospy
00008 import robotis.ros_robotis as rr
00009 import hrl_rfid.ros_M5e_client as rmc
00010 from geometry_msgs.msg import PointStamped
00011 
00012 import time
00013 import math
00014 from threading import Thread
00015 import optparse
00016 
00017 class TagGroundTruth( Thread ):
00018     def __init__( self, tag_x, tag_y, tag_z, baseframe = '/map', date = '' ):
00019         Thread.__init__( self )
00020         self.should_run = True
00021 
00022         self.tag_gt = PointStamped()
00023         self.tag_gt.header.stamp = rospy.Time(0)
00024         self.tag_gt.header.frame_id = baseframe
00025         self.tag_gt.point.x = tag_x
00026         self.tag_gt.point.y = tag_y
00027         self.tag_gt.point.z = tag_z
00028 
00029         rospy.logout('Publishing tag groundtruth at <%4.3f, %4.3f, %4.3f> in frame %s' % ( tag_x, tag_y, tag_z, baseframe ))
00030         self.pub = rospy.Publisher( 'tag_gt/' + date, PointStamped )
00031         self.start()
00032 
00033     def run( self ):
00034         rate = rospy.Rate( 5 )
00035         while self.should_run and not rospy.is_shutdown():
00036             self.tag_gt.header.stamp = rospy.Time.now()
00037             self.pub.publish( self.tag_gt )
00038             rate.sleep()
00039             
00040     def stop(self):
00041         self.should_run = False
00042         self.join(3)
00043         if (self.isAlive()):
00044             raise RuntimeError("Unable to stop thread")
00045         
00046 
00047 
00048 
00049 if __name__ == '__main__':
00050     p = optparse.OptionParser()
00051     p.add_option('-x', action='store', type='float', dest='x', default=None,
00052                  help='tag groundtruth pose in x')
00053     p.add_option('-y', action='store', type='float', dest='y', default=None,
00054                  help='tag groundtruth pose in y')
00055     p.add_option('-z', action='store', type='float', dest='z', default=None,
00056                  help='tag groundtruth pose in z')
00057     p.add_option('-d', action='store', type='string', dest='date', default=None,
00058                  help='date (yyyymmdd)')
00059     opt, args = p.parse_args()
00060 
00061     rospy.init_node( 'flapper' )
00062 
00063     if opt.x == None or opt.y == None or opt.z == None or opt.date == None:
00064         print 'Requires <x,y,z> and date arguments!'
00065         exit()
00066 
00067     p_left = rr.ROS_Robotis_Client( 'left_pan' )
00068     t_left = rr.ROS_Robotis_Client( 'left_tilt' )
00069     
00070     p_right = rr.ROS_Robotis_Client( 'right_pan' )
00071     t_right = rr.ROS_Robotis_Client( 'right_tilt' )
00072 
00073     EX_1 = 1.350
00074     EX_2 = 0.520  # 30 deg.
00075 #    EX_2 = -1.000 
00076 
00077     tgt = TagGroundTruth( opt.x, opt.y, opt.z, '/map', opt.date )
00078 
00079     p_left.move_angle( -1.0 * EX_1, math.radians(10), blocking = False )
00080     t_left.move_angle( 0.0, math.radians(10), blocking = False )
00081     
00082     p_right.move_angle( EX_1, math.radians(10), blocking = False )
00083     t_right.move_angle( 0.0, math.radians(10), blocking = False )
00084 
00085     forward = True
00086 
00087     while not rospy.is_shutdown():
00088         if not p_left.is_moving() and not p_right.is_moving():
00089             if forward:
00090                 p_left.move_angle( -1.0 * EX_1, math.radians(30), blocking = False )
00091                 p_right.move_angle( EX_1, math.radians(30), blocking = False )
00092                 forward = False
00093             else:
00094                 p_left.move_angle( EX_2, math.radians(30), blocking = False )
00095                 p_right.move_angle( -1.0 * EX_2, math.radians(30), blocking = False )
00096                 forward = True
00097         time.sleep(0.1)
00098     tgt.stop()


rfid_people_following
Author(s): Travis Deyle (Healthcare Robotics Lab, Georgia Tech)
autogenerated on Wed Nov 27 2013 11:38:30