tb3_4

#!/usr/bin/env python
#################################################################################
# Copyright 2018 IWIN, SJTU
#
# https://iwin-fins.com
#################################################################################

# Authors: Hongbo Li, Han Wang#

import rospy
from geometry_msgs.msg import Twist, Point, Quaternion
import tf
from math import radians, copysign, sqrt, pow, pi, atan2,cos,sin
from tf.transformations import euler_from_quaternion
import numpy as np

msg = """
control your Turtlebot3!
-----------------------
this is tb3_4
-----------------------
"""

class GotoPoint():
    def __init__(self):
        rospy.init_node('tb3_4', anonymous=False)
        rospy.on_shutdown(self.shutdown)
        self.cmd_vel = rospy.Publisher('cmd_vel', Twist, queue_size=5)#5
        self.tb3_4_vel=rospy.Publisher('/tb3_4_vel',Twist,queue_size=5)#5
        position = Point()
        move_cmd = Twist()
        
        r = rospy.Rate(10)
        self.tf_listener = tf.TransformListener()
        self.odom_frame = '/tb3_4/odom' 
        try:
            self.tf_listener.waitForTransform(self.odom_frame, '/tb3_4/base_footprint', rospy.Time(), rospy.Duration(1.0))
            self.base_frame = '/tb3_4/base_footprint'
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            try:
                self.tf_listener.waitForTransform(self.odom_frame, '/tb3_4/base_link', rospy.Time(), rospy.Duration(1.0))
                self.base_frame = '/tb3_4/base_link'
            except (tf.Exception, tf.ConnectivityException, tf.LookupException):
                rospy.loginfo("Cannot find transform between odom and base_link or base_footprint")
                rospy.signal_shutdown("tf Exception")
        (position, rotation) = self.get_odom()
        
        self.tb3_4_pos=rospy.Publisher('/tb3_4_pos',Point,queue_size=5)#10
        self.tb3_4_pos.publish(position)

        tb3_4_rot=Point()
        tb3_4_rot.x=rotation
        self.tb3_4_ori=rospy.Publisher('/tb3_4_rot',Point,queue_size=5)#10
        self.tb3_4_ori.publish(tb3_4_rot)

      
        president=Twist()
        president.linear.x=0.5
        president.angular.z=0.5
        # president.angular.z=0.5
        # print president
        # print position.x
        self.cmd_vel.publish(president) 
        self.tb3_4_vel.publish(president)
        


    def get_odom(self):
        try:
            (trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
            rotation = euler_from_quaternion(rot)

        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            rospy.loginfo("TF Exception")
            return

        return (Point(*trans), rotation[2])
    def shutdown(self):
        self.cmd_vel.publish(Twist())
        rospy.sleep(1)
    
        



if __name__ == '__main__':
    try:
        while not rospy.is_shutdown():
            print(msg)
            GotoPoint()

    except:
        rospy.loginfo("shutdown program.")