Youbot根据马克点移动

    对于Youbot的基本移动我写了一段代码，可以根据马克点的坐标移动。

代码如下

#！/usr/bin/env python coding=utf-8

import rospy

from geometry_msgs.msg import Twist

import math

import tf

import geometry_msgs.msg

k11=0.0005

k22=0.0005

if __name__ == '__main__':

    rospy.init_node('youbot_tf_listener')

    pub=rospy.Publisher('cmd_vel',Twist,queue_size=3)

    listener = tf.TransformListener()

    rate = rospy.Rate(50.0)

    listener.waitForTransform("world", "youbot", rospy.Time(), rospy.Duration(4.0))

    listener.waitForTransform("world", "object", rospy.Time(), rospy.Duration(4.0))

    while not rospy.is_shutdown():

        try:

            (trans1,rot1) = listener.lookupTransform('world','youbot',rospy.Time(0))

            (trans2,rot2) = listener.lookupTransform('world','object',rospy.Time(0))

        except(tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):

            continue

        rospy.loginfo('距离远点的位置：x=%f, y=%f,z=%f \n 旋转四元数：a=%f,b=%f,c=%f,d=%f',trans1[0],trans1[1],trans1[2],rot1[0],rot1[1],rot1[2],rot1[3])

        rospy.loginfo('距离远点的位置：x=%f, y=%f,z=%f \n 旋转四元数：a=%f,b=%f,c=%f,d=%f',trans2[0],trans2[1],trans2[2],rot2[0],rot2[1],rot2[2],rot2[3])

        print(trans2[0]-trans1[0], trans2[2]-trans1[2])

        twist=Twist()

        #if abs((trans2[0]-trans1[0]))<200 and ((trans2[3]-trans1[3]))<200:

        if abs((trans2[0]-trans1[0]))<100 and abs((trans2[2]-trans1[2]))<100:

    v1=0

    v2 = 0

            print(v1,v2)

            twist.linear.x = v2

            twist.linear.y= v1

            pub.publish(twist)

            rate.sleep()

        else:

            v1 = (trans2[0]-trans1[0])*k11

    v2 = (trans2[2]-trans1[2])*k22

            print(v1,v2)

            twist.linear.x = v2

            twist.linear.y = v1

            pub.publish(twist)

            rate.sleep()

        #v1 = trans[0] * k11

        #v2 = trans[1] * k22

        #twist=Twist()

        #if abs((trans)[0])<200 and abs((trans[1]))<200:

    #v1=0

    #v2=0

            #twist.linear.x = v1

            #twist.linear.y = v2

            #pub.publish(twist)

    #break

#else:

            #v1 = trans[0] * k11

    #v2 = trans[1] * k22

            #twist.linear.x = v1

            #twist.linear.y = v2

            #pub.publish(twist) 

        #pub.publish(twist)

        rate.sleep()

    #twist.linear.x = 0.0

    #twist.linear.y = 0.0

    #pub.publish(twist)