Wstęp do automatyki, elektroniki i telekomunikacji

#!/usr/bin/env python

import rospy

from geometry_msgs.msg import Twist

from turtlesim.msg  import Pose

global new_vel

new_vel = Twist()

def callback(pose):

        global new_vel

        new_vel = Twist()

        rospy.loginfo("Pozycja x: %8.2f",pose.x)

        rospy.loginfo("Pozycja y: %8.2f",pose.y)

        rospy.loginfo("Pozycja theta: %8.2f",pose.theta)

        new_vel.linear.x = pose.x + 0.01

if __name__== "__main__":

        global new_vel

        new_vel = Twist()

        rospy.init_node('zad2', anonymous=True)

        print("ready")

        pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)

        rospy.Subscriber( '/turtle1/pose' , Pose, callback)

        rate=rospy.Rate(10) # 10Hz

        while not rospy.is_shutdown():

                pub.publish(new_vel)#wyslaniepredkoscizadanej

                rate.sleep()

        print("END")