From a3b88f74511b45c2bd903b0451e1a696968581c9 Mon Sep 17 00:00:00 2001
From: agrabarchuk <alex45678@mail.ru>
Date: Fri, 15 Nov 2024 23:13:44 +0400
Subject: [PATCH] =?UTF-8?q?=D0=B4=D0=BE=D0=B1=D0=B0=D0=B2=D0=BB=D1=8F?=
 =?UTF-8?q?=D0=B5=D0=BC=20=D1=84=D0=B0=D0=B9=D0=BB=20step=5Falex.py=20?=
 =?UTF-8?q?=D0=B8=20=D0=BE=D1=81=D1=83=D1=89=D0=B5=D1=81=D1=82=D0=B2=D0=BB?=
 =?UTF-8?q?=D1=8F=D0=B5=D0=BC=20=D1=83=D0=BF=D1=80=D0=B0=D0=B2=D0=BB=D0=B5?=
 =?UTF-8?q?=D0=BD=D0=B8=D0=B5=20=D1=87=D0=B5=D1=80=D0=B5=D0=BF=D0=B0=D1=85?=
 =?UTF-8?q?=D0=BE=D0=B9=20=D0=BF=D0=BE=20=D1=80=D0=BE=D0=B2=D0=BD=D0=BE?=
 =?UTF-8?q?=D0=BC=D1=83=20=D0=BA=D0=B2=D0=B0=D0=B4=D1=80=D0=B0=D1=82=D1=83?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 launch/step_alex.launch.py | 19 ++++++++
 lesson_05/step_alex.py     | 88 ++++++++++++++++++++++++++++++++++++++
 setup.py                   |  2 +
 3 files changed, 109 insertions(+)
 create mode 100644 launch/step_alex.launch.py
 create mode 100644 lesson_05/step_alex.py

diff --git a/launch/step_alex.launch.py b/launch/step_alex.launch.py
new file mode 100644
index 0000000..db84b82
--- /dev/null
+++ b/launch/step_alex.launch.py
@@ -0,0 +1,19 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='turtlesim',
+            namespace='',
+            executable='turtlesim_node',
+            name='turtle'
+        ),
+        Node(
+            package='lesson_05',
+            namespace='',
+            executable='step_alex',
+            name='square_alex'
+        )
+    ])
\ No newline at end of file
diff --git a/lesson_05/step_alex.py b/lesson_05/step_alex.py
new file mode 100644
index 0000000..5d58005
--- /dev/null
+++ b/lesson_05/step_alex.py
@@ -0,0 +1,88 @@
+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 SquareMover(Node):
+    def __init__(self):
+        super().__init__('square_mover')
+        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.current_pose = None
+
+        self.side_length = 3.0
+        self.linear_speed = 0.5
+        self.angular_speed_max = 1.0
+        self.kp_angular = 4.0
+
+        self.target_heading = 0.0
+        self.start_x = 0.0
+        self.start_y = 0.0
+        self.corner_count = 0
+        self.is_turning = False
+
+        self.get_logger().info('Node initialized, starting...')
+
+    def pose_callback(self, msg):
+        self.current_pose = msg
+        if self.is_turning:
+            self.handle_turning()
+        else:
+            self.handle_moving()
+
+    def handle_moving(self):
+        if self.current_pose is None:
+            return
+
+        distance = math.sqrt(
+            (self.current_pose.x - self.start_x) ** 2 + (self.current_pose.y - self.start_y) ** 2
+        )
+        if distance >= self.side_length:
+            self.is_turning = True
+            self.target_heading = (self.target_heading + math.pi / 2) % (2 * math.pi)
+            self.get_logger().info(f'Reached side, starting turn to {self.target_heading:.2f} radians')
+            return
+
+        angle_error = angle_diff(self.target_heading, self.current_pose.theta)
+        angular_speed = max(-self.angular_speed_max, min(self.kp_angular * angle_error, self.angular_speed_max))
+
+        msg = Twist()
+        msg.linear.x = self.linear_speed
+        msg.angular.z = angular_speed
+        self.publisher_twist.publish(msg)
+
+    def handle_turning(self):
+        if self.current_pose is None:
+            return
+
+        angle_error = angle_diff(self.target_heading, self.current_pose.theta)
+        if abs(angle_error) <= 0.01:
+            self.is_turning = False
+            self.corner_count += 1
+            if self.corner_count >= 4:
+                self.corner_count = 0
+                self.get_logger().info('Square complete, restarting...')
+            self.start_x = self.current_pose.x
+            self.start_y = self.current_pose.y
+            self.get_logger().info(f'Starting new side from ({self.start_x:.2f}, {self.start_y:.2f})')
+            return
+
+        angular_speed = max(-self.angular_speed_max, min(self.kp_angular * angle_error, self.angular_speed_max))
+        msg = Twist()
+        msg.angular.z = angular_speed
+        self.publisher_twist.publish(msg)
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = SquareMover()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()
diff --git a/setup.py b/setup.py
index 0e83277..4291027 100644
--- a/setup.py
+++ b/setup.py
@@ -27,6 +27,8 @@ setup(
             'step_2 = lesson_05.step_2:main',
             'step_3 = lesson_05.step_3:main',
             'step_4 = lesson_05.step_4:main',
+            'step_5 = lesson_05.step_5:main',
+            'step_alex = lesson_05.step_alex:main',
         ],
     },
 )
-- 
GitLab