From 1af4ae230b28009122358286a8c7b08ababcb072 Mon Sep 17 00:00:00 2001
From: Anatoly <tolik.mokhov.2018@mail.ru>
Date: Mon, 28 Oct 2024 23:46:10 +0300
Subject: [PATCH 1/2] Add rqt

---
 launch/step_4.launch.py |  6 +++++
 lesson_05/step_4.py     | 54 -----------------------------------------
 2 files changed, 6 insertions(+), 54 deletions(-)
 delete mode 100644 lesson_05/step_4.py

diff --git a/launch/step_4.launch.py b/launch/step_4.launch.py
index bee9cc5..607837c 100644
--- a/launch/step_4.launch.py
+++ b/launch/step_4.launch.py
@@ -16,4 +16,10 @@ def generate_launch_description():
             executable='step_4',
             name='square'
         )
+        Node(
+            package='rqt_gui',
+            namespace='',
+            executable='rqt_gui',
+            name='rqt'
+        ),
     ])
diff --git a/lesson_05/step_4.py b/lesson_05/step_4.py
deleted file mode 100644
index 0aadcee..0000000
--- a/lesson_05/step_4.py
+++ /dev/null
@@ -1,54 +0,0 @@
-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)
-        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.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


From d7e1f371226be995d8c6f581d36045a823d7f0a0 Mon Sep 17 00:00:00 2001
From: Anatoly <tolik.mokhov.2018@mail.ru>
Date: Tue, 29 Oct 2024 02:02:16 +0300
Subject: [PATCH 2/2] Added P regulator

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

diff --git a/lesson_05/step_4.py b/lesson_05/step_4.py
new file mode 100644
index 0000000..e4f0655
--- /dev/null
+++ b/lesson_05/step_4.py
@@ -0,0 +1,54 @@
+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)
+        self.kp = 1
+    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)-error)*self.kp
+            self.get_logger().info('Turn left')
+        else:
+            msg_pub.angular.z = (math.radians(-90.0)-error)*self.kp
+            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)
+
+    mover.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()
-- 
GitLab