00001
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
00075
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()