对于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)
