From e55c3d272215bbf3c17a3337209508c4cdd2c304 Mon Sep 17 00:00:00 2001
From: kvotchenko <kvotchenko@sdb.smtu.ru>
Date: Mon, 18 Nov 2024 18:33:47 +0300
Subject: [PATCH] =?UTF-8?q?=D0=98=D0=B7=D0=BC=D0=B5=D0=BD=D0=B5=D0=BD=20st?=
 =?UTF-8?q?ep=5F1.py?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 lesson_06/step_1.py | 78 +++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 78 insertions(+)
 create mode 100644 lesson_06/step_1.py

diff --git a/lesson_06/step_1.py b/lesson_06/step_1.py
new file mode 100644
index 0000000..1b8ddf6
--- /dev/null
+++ b/lesson_06/step_1.py
@@ -0,0 +1,78 @@
+import rclpy
+from rclpy.node import Node
+from rclpy.duration import Duration
+from geometry_msgs.msg import Twist
+from geometry_msgs.msg import PoseStamped, Pose
+from std_msgs.msg import Float64
+
+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.setpoint_publisher = self.create_publisher(Float64, '~/setpoint', 10)
+        self.state_publisher = self.create_publisher(Float64, '~/state', 10)
+        self.publisher_twist = self.create_publisher(Twist, '/waterstrider/twist_command', 10)
+        self.pid_subscriber = self.create_subscription(Float64, '~/output', self.pid_callback, 10)
+        self.pose_subscriber = self.create_subscription(PoseStamped, '/waterstrider/ground_truth_to_tf_waterstrider/pose', self.pose_callback, 10)
+        self.desired_heading = - math.pi / 2
+        self.pid = 0.0
+        self.SIDE_TIME = 10.0
+        self.LINEAR_SPEED = 1.0
+        self.SIDE_TIME2 = 2.0
+        self.timer = self.create_timer(self.SIDE_TIME, self.pub_timer_callback)
+        self.timer2 = self.create_timer(self.SIDE_TIME2, self.tw_tm)
+
+       
+
+    def pid_callback(self, msg):
+        if self.pid != msg.data:
+            self.pid = msg.data 
+    
+
+    def pose_callback(self, msg):
+        heading = get_yaw_from_quaternion(msg.pose.orientation)
+        stp = Float64()
+        stp.data = self.desired_heading
+        self.setpoint_publisher.publish(stp)
+        st = Float64()
+        st.data = float(heading)
+        self.state_publisher.publish(st)
+        self.get_clock().sleep_for(Duration(seconds=2.0))
+       
+
+    def tw_tm(self):
+        msg_pub = Twist()
+        msg_pub.linear.x = self.LINEAR_SPEED
+        msg_pub.angular.z = self.pid
+        self.publisher_twist.publish(msg_pub)
+        
+        
+    def pub_timer_callback(self):
+        self.desired_heading += math.pi / 2
+        self.desired_heading %= 2 * math.pi
+        
+
+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