From 78ca18357bc9e145538f72872d5abbf4b6a3b567 Mon Sep 17 00:00:00 2001
From: ailoginov <lai@satellitm.ru>
Date: Mon, 28 Oct 2024 18:58:52 +0300
Subject: [PATCH 1/2] Add new files

---
 launch/mover.launch.py | 25 ++++++++++++++++++++++
 lesson_05/mover.py     | 47 ++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 72 insertions(+)
 create mode 100644 launch/mover.launch.py
 create mode 100644 lesson_05/mover.py

diff --git a/launch/mover.launch.py b/launch/mover.launch.py
new file mode 100644
index 0000000..e94d087
--- /dev/null
+++ b/launch/mover.launch.py
@@ -0,0 +1,25 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    return LaunchDescription([
+         Node(
+             package='rqt_plot',
+             namespace='',
+             executable='rqt_plot',
+             name='rqt'
+         ),
+        Node(
+            package='turtlesim',
+            namespace='',
+            executable='turtlesim_node',
+            name='sim'
+        ),
+        Node(
+            package='lesson_05',
+            namespace='',
+            executable='mover',
+            name='mover'
+        )
+    ])
diff --git a/lesson_05/mover.py b/lesson_05/mover.py
new file mode 100644
index 0000000..64dc8fd
--- /dev/null
+++ b/lesson_05/mover.py
@@ -0,0 +1,47 @@
+import rclpy
+from rclpy.node import Node
+from rclpy.duration import Duration
+
+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.angle = math.pi / 2
+        self.SIDE_TIME = 5
+        self.LINEAR_SPEED = 0.5 
+        self.timer = self.create_timer(self.SIDE_TIME, self.pub_timer_callback)
+
+    def pose_callback(self, msg):
+        msg_pub = Twist()
+        msg_pub.linear.x = float(self.LINEAR_SPEED)
+
+        error = angle_diff(msg.theta, self.angle)
+        msg_pub.angular.z = - math.pi / 2 * error                       # P CONTROL
+        self.publisher_twist.publish(msg_pub)
+
+    def pub_timer_callback(self):
+        
+        self.angle += math.pi / 2 
+        self.angle %= 2 * math.pi 
+        self.get_logger().info('New angle set: "%f"' % self.angle)
+
+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 f036b63a24de44c8d729211b9412dfc34a55d776 Mon Sep 17 00:00:00 2001
From: ailoginov <lai@satellitm.ru>
Date: Mon, 28 Oct 2024 18:59:29 +0300
Subject: [PATCH 2/2] Add mover to setup file

---
 setup.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/setup.py b/setup.py
index 0e83277..a16ea8b 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',
+            'mover = lesson_05.mover:main',
         ],
     },
 )
-- 
GitLab