From 9c28d36acf3319dc3c7cd45737f589dacedf310c Mon Sep 17 00:00:00 2001
From: Alexander Zhandarov <4l4st4rgrum@gmail.com>
Date: Sat, 17 Feb 2024 22:16:55 +0300
Subject: [PATCH] =?UTF-8?q?=D0=BA=D0=BE=D1=80=D1=80=D0=B5=D0=BA=D1=82?=
 =?UTF-8?q?=D0=BD=D1=8B=D0=B9=20=D0=B7=D0=B0=D0=BF=D1=83=D1=81=D0=BA=20rqt?=
 =?UTF-8?q?,=20=D0=B4=D0=BE=D0=B1=D0=B0=D0=B2=D0=BB=D0=B5=D0=BD=20=D0=BF?=
 =?UTF-8?q?=D1=80=D0=BE=D0=BF=D0=BE=D1=80=D1=86=D0=B8=D0=BE=D0=BD=D0=B0?=
 =?UTF-8?q?=D0=BB=D1=8C=D0=BD=D1=8B=D0=B9=20=D1=80=D0=B5=D0=B3=D1=83=D0=BB?=
 =?UTF-8?q?=D1=8F=D1=82=D0=BE=D1=80?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 lesson_05/homework.py | 57 +++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 57 insertions(+)
 create mode 100644 lesson_05/homework.py

diff --git a/lesson_05/homework.py b/lesson_05/homework.py
new file mode 100644
index 0000000..0d96402
--- /dev/null
+++ b/lesson_05/homework.py
@@ -0,0 +1,57 @@
+import rclpy
+from rclpy.node import Node
+
+from geometry_msgs.msg import Twist
+from turtlesim.msg import Pose
+import math
+
+
+def angle_diff(a1, a2):
+    a = a1-a2
+    return (a+math.pi)%(2*math.pi)-math.pi
+
+
+class RurMover(Node):
+    def __init__(self):
+        super().__init__('minimal_publisher')
+        self.publisher_twist = self.create_publisher(Twist, 'turtle1/cmd_vel', 10)
+        self.pose_subscriber = self.create_subscription(Pose, 'turtle1/pose', self.pose_callback, 10)
+        self.desired_heading = - math.pi / 2
+        self.SIDE_TIME = 5.0
+        self.LINEAR_SPEED = 0.5
+        self.timer = self.create_timer(self.SIDE_TIME, self.pub_timer_callback)
+
+    def pose_callback(self, msg):
+        self.get_logger().info('Current heading: "%f"' % msg.theta)
+        self.get_logger().info('Setpoint: "%f"' % self.desired_heading)
+        msg_pub = Twist()
+        msg_pub.linear.x = self.LINEAR_SPEED
+        error = angle_diff(self.desired_heading, msg.theta)
+        correction = math.radians(90.0) * error * 5
+
+        if error != 0:
+            if error < 0:
+                self.get_logger().info('Turn left')
+            else:
+                self.get_logger().info('Turn right')
+
+            msg_pub.angular.z = correction
+            self.publisher_twist.publish(msg_pub)
+
+    def pub_timer_callback(self):
+        self.desired_heading += math.pi / 2
+        self.desired_heading %= 2 * math.pi
+        self.get_logger().info('New angle setpoint: "%f"' % self.desired_heading)
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    mover = RurMover()
+
+    rclpy.spin(mover)
+
+    mover.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()
\ No newline at end of file
-- 
GitLab