launch/step_4.launch.py

 from launch import LaunchDescription
 from launch_ros.actions import Node
 
-
 def generate_launch_description():
     return LaunchDescription([
         Node(
@@ -15,5 +14,11 @@ def generate_launch_description():
             namespace='',
             executable='step_4',
             name='square'
+        ),
+        Node(
+            package='rqt_gui',
+            namespace='',
+            executable='rqt_gui',
+            name='rqt'
         )
-    ])
+    ])
\ No newline at end of file

lesson_05/step_4.py

@@ -7,46 +7,54 @@ import math
 
 
 def angle_diff(a1, a2):
-    a = a1-a2
-    return (a+math.pi)%(2*math.pi)-math.pi
+    a = a1 - a2
+    return (a + math.pi) % (2 * math.pi) - math.pi
 
 
 class RurMover(Node):
     def __init__(self):
-        super().__init__('minimal_publisher')
+        super().__init__('rur_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.desired_heading = - math.pi / 2
+        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)
+        self.BASE_LINEAR_SPEED = 0.5
+        self.Kp_angular = 1.0  # Пропорциональный коэффициент для угловой скорости
+        self.Kp_linear = 0.1  # Пропорциональный коэффициент для линейной скорости
+        self.timer = self.create_timer(self.SIDE_TIME, self.update_heading)
 
     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)
-        if error < 0:
-            msg_pub.angular.z = math.radians(90.0)
-            self.get_logger().info('Turn left')
-        else:
-            msg_pub.angular.z = math.radians(-90.0)
-            self.get_logger().info('Turn right')
-        self.publisher_twist.publish(msg_pub)
-
-    def pub_timer_callback(self):
+        self.log_current_state(msg)
+        twist_msg = self.calculate_twist(msg.theta)
+        self.publisher_twist.publish(twist_msg)
+
+    def log_current_state(self, msg):
+        self.get_logger().info(f'Current heading: {msg.theta:.2f}')
+        self.get_logger().info(f'Setpoint: {self.desired_heading:.2f}')
+
+    def calculate_twist(self, current_heading):
+        error = angle_diff(self.desired_heading, current_heading)
+        angular_z = self.Kp_angular * error
+        linear_x = max(0.0, self.BASE_LINEAR_SPEED - self.Kp_linear * abs(error))
+        
+        self.get_logger().info(f'Error: {error:.2f}, Angular velocity: {angular_z:.2f}')
+        self.get_logger().info(f'Linear velocity: {linear_x:.2f}')
+        
+        twist_msg = Twist()
+        twist_msg.linear.x = linear_x
+        twist_msg.angular.z = angular_z
+        return twist_msg
+
+    def update_heading(self):
         self.desired_heading += math.pi / 2
         self.desired_heading %= 2 * math.pi
-        self.get_logger().info('New angle setpoint: "%f"' % self.desired_heading)
+        self.get_logger().info(f'New angle setpoint: {self.desired_heading:.2f}')
+
 
 def main(args=None):
     rclpy.init(args=args)
-
     mover = RurMover()
-
     rclpy.spin(mover)
-
     mover.destroy_node()
     rclpy.shutdown()