ROS下turtle3环绕移动的turtle2时距离偏差问题求助
海龟机器人嵌套环绕的距离偏差问题求助
我尝试让turtle2以半径2、角速度1环绕静止的turtle1,同时让turtle3以半径0.5、角速度2环绕turtle2,但turtle3会因为turtle2的移动产生距离偏差,无法稳定环绕。附上我的ROS代码及运行效果截图,求技术帮助。
#!/usr/bin/env python import roslib roslib.load_manifest('learning_tf') import rospy import math import tf import geometry_msgs.msg import turtlesim.srv if __name__ == '__main__': rospy.init_node('turtle_tf_listener') listener = tf.TransformListener() rospy.wait_for_service('spawn') spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn) # wait for the world to spawn rospy.sleep(0.3) (trans, rot) = listener.lookupTransform('/world', '/turtle1', rospy.Time(0)) r2 = 2 w2 = 1 # spawn second turtle along the orbit of the first still turtle spawner(trans[0], trans[1] - r2, 0, 'turtle2') turtle_vel2 = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist,queue_size=1) # wait for second turtle to spawn rospy.sleep(0.3) (trans, rot) = listener.lookupTransform('/world', '/turtle2', rospy.Time(0)) r3 = 0.5 w3 = 2 # spawn third turtle along the orbit of the second turtle spawner(trans[0], trans[1] - r3, 0, 'turtle3') turtle_vel3 = rospy.Publisher('turtle3/cmd_vel', geometry_msgs.msg.Twist,queue_size=1) rate = rospy.Rate(10.0) while not rospy.is_shutdown(): flag2 = True flag3 = True try: (trans, rot) = listener.lookupTransform('/world', '/turtle1', rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): flag2 = False if(flag2): angular2 = w2 linear2 = w2 * r2 cmd2 = geometry_msgs.msg.Twist() cmd2.linear.x = linear2 cmd2.angular.z = angular2 turtle_vel2.publish(cmd2) try: (trans, rot) = listener.lookupTransform('/turtle3', '/turtle2', rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): flag3 = False if(flag3): angular3 = w3 linear3 = w3 * r3 cmd3 = geometry_msgs.msg.Twist() cmd3.linear.x = linear3 cmd3.angular.z = angular3 turtle_vel3.publish(cmd3) rate.sleep()

内容的提问来源于stack exchange,提问作者A_Pumpkin
相关产品推荐
相关产品推荐

