From df9dc42269397bc50c5aa7b96fc42be9ab8b9566 Mon Sep 17 00:00:00 2001
From: RTarasov <romashkin226@gmail.com>
Date: Thu, 24 Oct 2024 18:12:36 +0300
Subject: [PATCH 1/2] Add node rqt

---
 launch/step_4.launch.py | 9 +++++++--
 1 file changed, 7 insertions(+), 2 deletions(-)

diff --git a/launch/step_4.launch.py b/launch/step_4.launch.py
index bee9cc5..d4f2825 100644
--- a/launch/step_4.launch.py
+++ b/launch/step_4.launch.py
@@ -1,7 +1,6 @@
 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
-- 
GitLab


From c3f3b899c6006db9f503fd0ca0f6d42243390477 Mon Sep 17 00:00:00 2001
From: RTarasov <romashkin226@gmail.com>
Date: Thu, 24 Oct 2024 18:14:12 +0300
Subject: [PATCH 2/2] =?UTF-8?q?=D0=94=D0=BE=D0=B1=D0=B0=D0=B2=D0=BB=D0=B5?=
 =?UTF-8?q?=D0=BD=20P-=D1=80=D0=B5=D0=B3=D1=83=D0=BB=D1=8F=D1=82=D0=BE?=
 =?UTF-8?q?=D1=80?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 lesson_05/step_4.py | 56 ++++++++++++++++++++++++++-------------------
 1 file changed, 32 insertions(+), 24 deletions(-)

diff --git a/lesson_05/step_4.py b/lesson_05/step_4.py
index 0aadcee..050c4c6 100644
--- a/lesson_05/step_4.py
+++ b/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()
 
-- 
GitLab