From ce5b025de14b8ac0cb3e496a314f6ddf5c1b48b6 Mon Sep 17 00:00:00 2001
From: Mango <mango.tcf@gmail.com>
Date: Wed, 23 Oct 2024 01:02:48 +0300
Subject: [PATCH] Added a PID-R and understood the redundancy of it in such a
 simple simulation

---
 launch/step_5.launch.py | 19 +++++++++++
 lesson_05/step_5.py     | 74 +++++++++++++++++++++++++++++++++++++++++
 setup.py                |  1 +
 3 files changed, 94 insertions(+)
 create mode 100644 launch/step_5.launch.py
 create mode 100644 lesson_05/step_5.py

diff --git a/launch/step_5.launch.py b/launch/step_5.launch.py
new file mode 100644
index 0000000..8e387e1
--- /dev/null
+++ b/launch/step_5.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='sim'
+        ),
+        Node(
+            package='lesson_05',
+            namespace='',
+            executable='step_5',
+            name='square'
+        )
+    ])
diff --git a/lesson_05/step_5.py b/lesson_05/step_5.py
new file mode 100644
index 0000000..3f48a28
--- /dev/null
+++ b/lesson_05/step_5.py
@@ -0,0 +1,74 @@
+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 = 0 #- math.pi / 2
+
+        # constants
+        self.SIDE_TIME = 5.0
+        self.LINEAR_SPEED_MAX = .3
+
+        # coefficients
+        self.k_p = 100
+        self.k_i = .1
+        self.k_d = 100
+
+        # misc
+        self.error_i = 0
+        self.error_old = 0
+        self.old_t = self.get_clock().now()
+
+        self.timer = self.create_timer(self.SIDE_TIME, self.pub_timer_callback)
+
+    def pose_callback(self, msg):
+        t = self.get_clock().now()
+        self.get_logger().info('Current heading: "%f"' % msg.theta)
+        self.get_logger().info('Setpoint: "%f"' % self.desired_heading)
+
+        msg_pub = Twist()
+
+        error_cur = angle_diff(self.desired_heading, msg.theta)
+        self.error_i += error_cur
+        u_p = error_cur * self.k_p
+        u_i = self.error_i * self.k_i * ((t - self.old_t).nanoseconds * 1e-9)
+        u_d = (error_cur - self.error_old) * self.k_d
+        self.get_logger().info(f"e: {error_cur:7.3f}, u_p: {u_p:7.3f}, u_i: {u_i:7.3f}, u_d: {u_d:7.3f}")
+        u = u_d + u_i + u_p
+        msg_pub.angular.z = math.radians(u)
+        msg_pub.linear.x = self.LINEAR_SPEED_MAX
+        self.publisher_twist.publish(msg_pub)
+        self.error_old = error_cur
+        self.old_t = self.get_clock().now()
+
+    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()
diff --git a/setup.py b/setup.py
index 0e83277..223a33c 100644
--- a/setup.py
+++ b/setup.py
@@ -27,6 +27,7 @@ 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',
         ],
     },
 )
-- 
GitLab