????對于Youbot的基本移動我寫了一段代碼杨何，可以根據(jù)馬克點的坐標(biāo)移動秽荞。
代碼如下
#咙好！/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 旋轉(zhuǎn)四元數(shù)：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 旋轉(zhuǎn)四元數(shù)：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)
