turtlebot3 marker following

#!/usr/bin/env python

import rospy
from turtlesim.msg import Pose
from math import degrees, pi, cos, sin, asin, sqrt
from ar_track_alvar_msgs.msg import AlvarMarkers
from geometry_msgs.msg import Twist
from tf.transformations import euler_from_quaternion

TARGET_ID = 1
MAX_LIN_SPEED = 0.22
MAX_ANG_SPEED = 2.84
LIN_SPD = MAX_LIN_SPEED * 0.5
ANG_SPD = MAX_ANG_SPEED * 0.4

OFFSET = 0.1        # carrot offset from target
CAM_ARM = 0.048     # camera offset from turtlebot rotation centroid
X_TOLERANCE = 0.01  # tolerance of position.x when aligning center
ROT_CONST = 0.9     # 90 percent proportion when rotating
DIST_CONST = 10 / 7

LIM_NEAR = 0.1
LIM_ROT = 0.2


class FollowMarker:
    def __init__(self):
        rospy.init_node('tb3_keep_dist', anonymous = True)
        rospy.Subscriber('/ar_pose_marker', AlvarMarkers, self.get_marker)
        rospy.Subscriber('/tb3pose', Pose, self.get_pose)
        self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size = 10)        
        
        self.distance = 0   # distance from turtlebot rotation center to marker
        self.angle = 0

        self.z = 0          # distance from camera to marker
        self.center = 0     # postion.x

        self.tw = Twist()
        self.theta = 0

        self.tb3pose = Pose()
        self.org = Pose()

        self.is_center_aligned = False
        self.is_straight_end = False
        self.is_rotate_end = False
        self.is_moving = False

    def get_marker(self, msg):
        if len(msg.markers) > 0:
            for msg in msg.markers:
                if msg.id == TARGET_ID:
                    if self.is_center_aligned == False and self.is_moving == False:
                        self.move2marker(msg)
        else:
            print "lost forward turtlebot"
            self.tw.linear.x = 0
            self.tw.angular.z = 0
            self.pub.publish(self.tw)

    def move2marker(self, msg):
        self.tw.angular.z = 0;  self.pub.publish(self.tw)        
        self.align_center(msg)
        
        if self.is_center_aligned == True:
            self.is_moving = True
            self.is_center_aligned = False            
            self.get_marker_info(msg)
 
            if self.z > LIM_ROT and abs(self.theta) > pi / 6:
                self.calculate_carrot_coordinate()
                self.rotate(self.angle)
                if self.is_rotate_end == True:
                    self.is_rotate_end == False                
                    self.straight(self.distance)
                    if self.is_straight_end == True:
                        self.is_straight_end = False
                        self.rotate(-(self.theta+self.angle))
                self.init_marker_info()
            else:
                self.straight2marker() 
            self.is_moving = False
                        
    def align_center(self, msg):
        print("align")
        center = msg.pose.pose.position.x
        if center > X_TOLERANCE:
            print("turn right")
            self.tw.angular.z = (-MAX_ANG_SPEED) * 0.2
        elif center < -X_TOLERANCE:
            print("turn left")
            self.tw.angular.z = MAX_ANG_SPEED * 0.2
        else:
            self.tw.angular.z = 0        
        self.pub.publish(self.tw)
        if self.tw.angular.z == 0:
            self.is_center_aligned = True
       
    def straight2marker(self):
        print("straight")
        if self.center > X_TOLERANCE + 0.01:
            self.tw.angular.z = (-MAX_ANG_SPEED) * 0.08
        elif self.center < -X_TOLERANCE - 0.01:
            self.tw.angular.z = MAX_ANG_SPEED * 0.08
        else:
            self.tw.angular.z = 0
        
        if self.z > LIM_NEAR:
            self.tw.linear.x = MAX_LIN_SPEED * 0.4
        else:
            self.tw.linear.x = 0
        
        self.pub.publish(self.tw)

    def calculate_carrot_coordinate(self):
        print("calculate")
        self.distance = sqrt(OFFSET**2 + (self.z+CAM_ARM)**2 - (2*OFFSET*(self.z+CAM_ARM)*cos(self.theta)))
        if self.theta > 0:
            self.angle = asin(OFFSET/self.distance*sin(abs(self.theta)))
        else:
            self.angle = -asin(OFFSET/self.distance*sin(abs(self.theta)))

    def get_marker_info(self, msg):
                
        q = (msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
             msg.pose.pose.orientation.z, msg.pose.pose.orientation.w)
                                            
        quart = euler_from_quaternion(q)
        self.theta = quart[1]                        # range from (-3.14, 3.14) 
        self.z = DIST_CONST * msg.pose.pose.position.z
        self.center = msg.pose.pose.position.x

    def init_marker_info(self):
        self.theta = 0
        self.z = 0
        self.center = 0        

    def get_pose(self, msg):
        self.tb3pose = msg
        
    def update_org(self):
        while self.tb3pose.x == 0.0 or self.tb3pose.y == 0.0: pass
        
        self.org = self.tb3pose

    def elapsed_dist(self):
        return sqrt(pow((self.tb3pose.x - self.org.x), 2) + pow((self.tb3pose.y - self.org.y), 2))

    def straight(self, distance):
        tw = Twist()
        self.update_org()
        print("\ndistance: {} cm".format(round(distance*100, 2)))
        print("start from (%s, %s)" %(round(self.org.x, 4), round(self.org.y, 4)))
        
        if distance >= 0:
            tw.linear.x = LIN_SPD
        else:
            tw.linear.x = -LIN_SPD

        self.pub.publish(tw)
        while self.elapsed_dist() < abs(distance):  pass
        
        tw.linear.x = 0;    self.pub.publish(tw)
        print("stop to (%s, %s)" %(round(self.tb3pose.x, 2), round(self.tb3pose.y, 2)))
        self.is_straight_end = True

    def elapsed_angle(self):
        return abs(self.tb3pose.theta - self.org.theta)

    def rotate(self, angle):
        print("rotate")
        tw = Twist()
        self.update_org()
        
        if angle >= 0:	# angle(+): rotate left(ccw)
            tw.angular.z =  ANG_SPD;
        else:			# angle(-): rotate right(cw)
            tw.angular.z = -ANG_SPD;
            
        self.pub.publish(tw)
        print("before")
        while self.elapsed_angle() < ROT_CONST * abs(angle):    pass
        print("after")
        tw.angular.z =  0;  self.pub.publish(tw)
        self.is_rotate_end = True


if __name__ == '__main__':
    try:
        FollowMarker()
        rospy.spin()
        
    except rospy.ROSInterruptException:  pass