From dc36d14d34036c1bf5356e35beb6d9a15b384563 Mon Sep 17 00:00:00 2001
From: Alexandra Vatulina <altig23@gmail.com>
Date: Sun, 10 Nov 2024 21:25:22 +0000
Subject: [PATCH 1/2] step_5.launch.py

---
 step_5.launch.py | 25 +++++++++++++++++++++++++
 1 file changed, 25 insertions(+)
 create mode 100644 step_5.launch.py

diff --git a/step_5.launch.py b/step_5.launch.py
new file mode 100644
index 0000000..1107f82
--- /dev/null
+++ b/step_5.launch.py
@@ -0,0 +1,25 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='turtlesim',
+            namespace='',
+            executable='turtlesim_node',
+            name='sim'
+        ),
+        Node(
+            package='lesson_05',
+            namespace='',
+            executable='step_5',
+            name='square'
+        ),
+        Node(
+            package='rqt_plot',
+            namespace='',
+            executable='rqt_plot',
+            name='rqt'
+        )
+    ])
\ No newline at end of file
-- 
GitLab


From 2e0cd4d6730770d48fde5efbd9ff1370c7277a7d Mon Sep 17 00:00:00 2001
From: Alexandra Vatulina <altig23@gmail.com>
Date: Sun, 10 Nov 2024 21:40:53 +0000
Subject: [PATCH 2/2] step_5.py

---
 step_5.py | 47 +++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 47 insertions(+)
 create mode 100644 step_5.py

diff --git a/step_5.py b/step_5.py
new file mode 100644
index 0000000..af9758f
--- /dev/null
+++ b/step_5.py
@@ -0,0 +1,47 @@
+import rclpy
+from rclpy.node import Node
+
+from geometry_msgs.msg import Twist
+from turtlesim.msg import Pose
+import math
+
+
+def ang_spd(a1, a2):
+    a = a1-a2
+    K_p = 450
+    return K_p*((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 = 7.0
+        self.LINEAR_SPEED = 0.4
+        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
+        ANGULAR_SPEED = ang_spd(self.desired_heading, msg.theta)
+        msg_pub.angular.z = math.radians(ANGULAR_SPEED)
+        if ANGULAR_SPEED > 0:
+            self.get_logger().info('Turn left')
+        else:
+            self.get_logger().info('Turn right')
+        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)
-- 
GitLab