From c2b4fe30f0cc9318a369205aa903dc75ef53eabb Mon Sep 17 00:00:00 2001
From: PsorokinMgorchakov <makgorchak@gmail.com>
Date: Mon, 11 Nov 2024 20:36:06 +0300
Subject: [PATCH] Add PID regulator + rebuilt

---
 config/control-config.yaml |  6 +--
 launch/step_1.launch.py    |  4 +-
 lesson_06/step_1.py        | 84 ++++++++++++++++++++++++++++++++++++++
 setup.py                   |  7 ++--
 4 files changed, 93 insertions(+), 8 deletions(-)
 create mode 100644 lesson_06/step_1.py

diff --git a/config/control-config.yaml b/config/control-config.yaml
index 80bbe66..c7ff5b8 100644
--- a/config/control-config.yaml
+++ b/config/control-config.yaml
@@ -4,8 +4,8 @@
         angular: true
         clamp: 15.0
         inverted: false
-        kd: 0.9
-        ki: 0.0
-        kp: 0.04
+        kd: 1.0
+        ki: 0.1
+        kp: 0.05
         start_type_description_service: true
         use_sim_time: false
diff --git a/launch/step_1.launch.py b/launch/step_1.launch.py
index 2908c5e..f908052 100644
--- a/launch/step_1.launch.py
+++ b/launch/step_1.launch.py
@@ -14,13 +14,15 @@ def generate_launch_description():
             package='lesson_06',
             namespace='waterstrider',
             executable='step_1',
-            name='controller'
+            name='controller',
+            output='screen'
         ),
         Node(
             package='lesson_06',
             namespace='waterstrider',
             executable='pid_node',
             name='pid',
+            output='screen'
             parameters=[config_file]
         )
     ])
diff --git a/lesson_06/step_1.py b/lesson_06/step_1.py
new file mode 100644
index 0000000..4dd7ee8
--- /dev/null
+++ b/lesson_06/step_1.py
@@ -0,0 +1,84 @@
+import rclpy
+from pid_node import PID
+from rclpy.node import Node
+
+from std_msgs.msg import Float64
+from geometry_msgs.msg import Twist
+from geometry_msgs.msg import PoseStamped, Pose
+
+from tf_transformations import euler_from_quaternion
+
+import math                                                                    
+
+def get_yaw_from_quaternion(q):
+    return euler_from_quaternion([q.x, q.y, q.z, q.w])[2]
+
+
+class RurMover(Node):
+    def __init__(self):
+        super().__init__('controller')
+        self.publisher_twist = self.create_publisher(Twist, '/waterstrider/twist_command', 10)
+        self.pose_subscriber = self.create_subscription(PoseStamped, '/waterstrider/ground_truth_to_tf_waterstrider/pose', self.pose_callback, 10)
+        self.PID_setpoint = self.create_publisher(Float64, '/pid/setpoint', 10)
+        self.PID_output = self.create_subscription(Float64, '/pid/output', self.PID_output_callback, 10)
+        self.PID_state = self.create_publisher(Float64, '/pid/state', 10)
+        self.SIDE_TIME = 10.0
+        self.LINEAR_SPEED = 1.0
+        self.desired_heading = - math.pi / 2
+        self.PID_angular = 0.0
+        self.timer = self.create_timer(self.SIDE_TIME, self.pub_timer_callback)
+        self.pose_history = []
+        msgSetPoint = Float64()
+        msgSetPoint.data = self.desired_heading
+        self.PID_setpoint.publish(msgSetPoint)
+
+    def PID_output_callback(self, msg):
+        self.PID_angular = msg.data
+    def pose_callback(self, msg):
+        heading = get_yaw_from_quaternion(msg.pose.orientation)
+        self.get_logger().info('Current heading: "%f"' % heading)
+        self.get_logger().info('Setpoint: "%f"' % self.desired_heading)
+        msgStatePose = Float64()
+        msgStatePose.data = heading
+        self.PID_state.publish(msgStatePose)
+        msg_pub = Twist()
+        msg_pub.linear.x = self.LINEAR_SPEED
+        msg_pub.angular.z = self.PID_angular
+        self.publisher_twist.publish(msg_pub)
+        self.pose_history.append(msg.pose)
+          if len(self.pose_history) >=2:
+            cog = self.calculate_cog()
+            self.get_logger().info('COG: "%f"' %cog)
+    def calculate_cog(self):
+        p1 = self.pose_history[-2]
+        p2 = self.pose_histor[-1]
+        dx = p2.position.x - p1.position.x
+        dy = p2.position.y - p1.position.y
+         return math.atan2(dy,dx)
+
+    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)
+        msgSetPoint = Float64()
+        msgSetPoint.data = self.desired_heading
+        self.PID_setpoint.publish(msgSetPoint)
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    mover = RurMover()
+    PID_node = PID()
+
+    executor = rclpy.executors.MultiThreadedExecutor()
+    executor.add_node(PID_node)
+    executor.add_node(mover)
+       try: executor.spin()
+       finally: executor.shutdown()
+         PID_node.destroy_node()
+
+    mover.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()
\ No newline at end of file
diff --git a/setup.py b/setup.py
index 1d42584..4e67cdb 100644
--- a/setup.py
+++ b/setup.py
@@ -10,7 +10,8 @@ package_name = 'lesson_06'
 setup(
     name=package_name,
     version='0.0.0',
-    packages=find_packages(exclude=['test']),
+    packages=find_packages(where=['src']),
+    package_dir={'':'src'}
     data_files=[
         ('share/ament_index/resource_index/packages',
             ['resource/' + package_name]),
@@ -28,10 +29,8 @@ setup(
     license='TODO: License declaration',
     tests_require=['pytest'],
     entry_points={
-        'console_scripts': [
-            'step_0 = lesson_06.step_0:main',
+        'console_scripts': 
             'step_1 = lesson_06.step_1:main',
-            'pid_node = lesson_06.pid_node:main',
         ],
     },
 )
-- 
GitLab