testies.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #       
00003 # License: BSD
00004 #   https://raw.github.com/robotics-in-concert/rocon_multimaster/master/rocon_gateway/LICENSE 
00005 #
00006 
00007 ##############################################################################
00008 # Imports
00009 ##############################################################################
00010 
00011 import math
00012 import rospy
00013 import rocon_utilities.console as console
00014 import visualization_msgs.msg as visualization_msgs
00015 from std_msgs.msg import Float32
00016 
00017 # Can't turn it much - different error params?
00018 # Sometimes jumps x-values near the edges -> due to bad z calculations on the fringe (out by 50cm!)
00019   # should be able to easily filter these out
00020 # a and b disappear near the edges, but I still get readings
00021 
00022 ##############################################################################
00023 # Functions
00024 ##############################################################################
00025 
00026 class Marker:
00027     def __init__(self, id, x, y, z):
00028         self.id = id
00029         self.update(x, y, z)
00030     
00031     def update(self, x, y, z):
00032         self.x = x
00033         self.y = y
00034         self.z = z
00035         self.distance = math.sqrt(x*x + z*z)
00036 
00037 class Jagi(object):
00038     def __init__(self):
00039         self.baseline = 0.26
00040         self.left_marker = Marker(id=3, x=0, y=0, z=0)
00041         self.right_marker = Marker(id=0, x=0, y=0, z=0)
00042         self.left_last_timestamp = rospy.get_rostime()
00043         self.right_last_timestamp = rospy.get_rostime()
00044         self.x_publisher = rospy.Publisher('x', Float32)
00045         self.z_publisher = rospy.Publisher('z', Float32)
00046         self.heading_publisher = rospy.Publisher('heading', Float32)
00047         self.subscriber = rospy.Subscriber('/visualization_marker', visualization_msgs.Marker, self.callback)
00048 
00049 
00050     def print_pythags_results(self):
00051         b = self.baseline/2 + (self.left_marker.distance*self.left_marker.distance - self.right_marker.distance*self.right_marker.distance)/(2*self.baseline)
00052         try:
00053             a = math.sqrt(self.left_marker.distance*self.left_marker.distance - b*b)
00054         except ValueError:
00055             print("Value error: measured_distance=%s, b=%s" % (self.left_marker.distance, b))
00056             return False # self.left_marker not initialised fully yet, so negative sqrt
00057         print(console.cyan + "           a" + console.reset + " : " + console.yellow + "%s" % a + console.reset)
00058         print(console.cyan + "           b" + console.reset + " : " + console.yellow + "%s" % b + console.reset)
00059         return True
00060 
00061     def print_pose_results(self):
00062         # angle between the robot and the first marker
00063         alpha = math.atan2(self.left_marker.x, self.left_marker.z)
00064         alpha_degrees = alpha*180.0/math.pi
00065         # alpha + beta is angle between the robot and the second marker 
00066         beta = math.atan2(self.right_marker.x, self.right_marker.z)
00067         beta_degrees = beta*180.0/math.pi
00068         # theta is the angle between the wall and the perpendicular in front of the robot
00069         theta = math.atan2((self.left_marker.z - self.right_marker.z), (self.right_marker.x - self.left_marker.x))
00070         theta_degrees = theta*180.0/math.pi
00071     
00072         print(console.cyan + "       alpha" + console.reset + " : " + console.yellow + "%s" % alpha_degrees + " degrees" + console.reset)
00073         print(console.cyan + "        beta" + console.reset + " : " + console.yellow + "%s" % beta_degrees + " degrees" + console.reset)
00074         print(console.cyan + "       theta" + console.reset + " : " + console.yellow + "%s" % theta_degrees + " degrees" + console.reset)
00075         
00076         # M1 = (self.left_marker.x, self.left_marker.z)
00077         # M2 = (self.right_marker.x, self.right_marker.z)
00078         # M3 = M1 + (M2 - M1)/2   # midpoint of M1, M2
00079         # Target stop position relative to marker mid point (40cm perpendicularly out)
00080         # M4 = (-0.4*sin(theta), -0.4*cos(theta))
00081         # Target stop position
00082         # M5 = M3 + M4
00083         target_x = self.left_marker.x + (self.right_marker.x - self.left_marker.x)/2 - 0.4*math.sin(theta)
00084         target_z = self.left_marker.z + (self.right_marker.z - self.left_marker.z)/2 - 0.4*math.cos(theta)
00085         target_heading = math.atan2(target_x, target_z)
00086         target_heading_degrees = target_heading*180.0/math.pi
00087         
00088         print(console.cyan + "      target" + console.reset + " : " + console.yellow + "(x=%s, z=%s, heading=%s)" % (target_x, target_z, target_heading_degrees) + console.reset)
00089         self.x_publisher.publish(Float32(target_x))
00090         self.z_publisher.publish(Float32(target_z))
00091         self.heading_publisher.publish(Float32(target_heading_degrees))
00092     
00093     def callback(self, data):
00094         if data.id == 0:
00095             self.right_marker.update(data.pose.position.x, data.pose.position.y, data.pose.position.z)
00096             self.right_last_timestamp = data.header.stamp
00097             print(console.cyan + "r: [x, y, z]" + console.reset + " : " + console.yellow + "[%s, %s, %s]" % (self.right_marker.x, self.right_marker.y, self.right_marker.z) + console.reset)
00098             print(console.cyan + "r: d" + console.reset + "         : " + console.yellow + "%s" % self.right_marker.distance + console.reset)
00099         else:
00100             self.left_last_timestamp = data.header.stamp
00101             self.left_marker.update(data.pose.position.x, data.pose.position.y, data.pose.position.z)
00102             print(console.cyan + "l: [x, y, z]" + console.reset + " : " + console.yellow + "[%s, %s, %s]" % (self.left_marker.x, self.left_marker.y, self.left_marker.z) + console.reset)
00103             print(console.cyan + "l: d" + console.reset + "         : " + console.yellow + "%s" % self.left_marker.distance + console.reset)
00104         #if math.fabs(self.left_last_timestamp - self.right_last_timestamp) < 0.1:
00105         if self.left_last_timestamp == self.right_last_timestamp:
00106             if self.print_pythags_results():
00107                 self.print_pose_results()
00108 
00109 ##############################################################################
00110 # Main
00111 ##############################################################################
00112 
00113 if __name__ == '__main__':
00114     #visualization_msgs/Marker
00115     rospy.init_node('roles_and_apps')
00116     jagi = Jagi()
00117     rospy.spin()


yocs_ar_marker_tracking
Author(s): Daniel Stonier, Jorge Santos
autogenerated on Thu Jun 6 2019 21:47:27