From e04d87202f2f0ee8c84699d71989ba815d854ed0 Mon Sep 17 00:00:00 2001 From: sokolov_v Date: Tue, 2 Apr 2024 08:46:33 +0300 Subject: [PATCH] =?UTF-8?q?=D0=9D=D0=B0=D0=BF=D0=B8=D1=81=D0=B0=D0=BD?= =?UTF-8?q?=D0=B0=20=D0=BD=D0=BE=D0=B4=D0=B0,=20=D0=BF=D0=BE=D0=B7=D0=B2?= =?UTF-8?q?=D0=BE=D0=BB=D1=8F=D1=8E=D1=89=D0=B0=D1=8F=20=D0=BA=D0=B0=D1=82?= =?UTF-8?q?=D0=B0=D0=BC=D0=B0=D1=80=D0=B0=D0=BD=D1=83=20=D1=81=D0=BE=D0=B2?= =?UTF-8?q?=D0=B5=D1=80=D1=88=D0=B0=D1=82=D1=8C=20=D0=BE=D0=B1=D0=BE=D1=80?= =?UTF-8?q?=D0=BE=D1=82=20=D1=80=D0=B0=D0=B4=D0=B8=D1=83=D1=81=D0=BE=D0=BC?= =?UTF-8?q?=205=20=D0=B8=20=D1=83=D0=B2=D0=B5=D0=BB=D0=B8=D1=87=D0=B8?= =?UTF-8?q?=D0=B2=D0=B0=D1=8E=D1=89=D0=B8=D0=B9=20=D1=8D=D1=82=D0=BE=D1=82?= =?UTF-8?q?=20=D1=80=D0=B0=D0=B4=D0=B8=D1=83=D1=81=20=D0=BD=D0=B0=20=D0=BE?= =?UTF-8?q?=D0=BF=D1=80=D0=B5=D0=B4=D0=B5=D0=BB=D1=91=D0=BD=D0=BD=D1=83?= =?UTF-8?q?=D1=8E=20=D0=B2=D0=B5=D0=BB=D0=B8=D1=87=D0=B8=D0=BD=D1=83,=20?= =?UTF-8?q?=D1=80=D0=B5=D0=B0=D0=BB=D0=B8=D0=B7=D0=BE=D0=B2=D0=B0=D0=BD?= =?UTF-8?q?=D0=BE=20=D0=B7=D0=B0=D0=B4=D0=B0=D0=BD=D0=B8=D0=B5=20=D1=81?= =?UTF-8?q?=D0=BE=20=D0=B7=D0=B2=D1=91=D0=B7=D0=B4=D0=BE=D1=87=D0=BA=D0=BE?= =?UTF-8?q?=D0=B9,=20=D1=82=D0=B5=D0=BF=D0=B5=D1=80=D1=8C=20Course=20Over?= =?UTF-8?q?=20Ground=20=D0=BC=D0=BE=D0=B6=D0=BD=D0=BE=20=D0=BC=D0=BE=D0=BD?= =?UTF-8?q?=D0=B8=D1=82=D0=BE=D1=80=D0=B8=D1=82=D1=8C=20=D0=B2=20=D1=82?= =?UTF-8?q?=D0=BE=D0=BF=D0=B8=D0=BA=D0=B5=20=D1=87=D0=B5=D1=80=D0=B5=D0=B7?= =?UTF-8?q?=20rqt=20(=D1=80=D0=B0=D0=B7=D0=B4=D0=B5=D0=BB=20/waterstrider/?= =?UTF-8?q?pose)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- lesson_06/pid_node.py | 6 +-- lesson_06/step_0.py | 20 +++------ lesson_06/step_1.py | 97 +++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 106 insertions(+), 17 deletions(-) create mode 100644 lesson_06/step_1.py diff --git a/lesson_06/pid_node.py b/lesson_06/pid_node.py index 6c390a5..32e079a 100644 --- a/lesson_06/pid_node.py +++ b/lesson_06/pid_node.py @@ -18,9 +18,9 @@ class PID(Node): self.config['kp'] = self.declare_parameter('kp', 0.0).value self.config['ki'] = self.declare_parameter('ki', 0.0).value self.config['kd'] = self.declare_parameter('kd', 0.0).value - self.config['clamp'] = self.declare_parameter('clamp', 1.0).value - self.config['angular'] = self.declare_parameter('angular', False).value - self.config['inverted'] = self.declare_parameter('inverted', False).value + self.config['clamp'] = self.declare_parameter('clamp', 1.0).value #чтобы не подавать большое воздействие + self.config['angular'] = self.declare_parameter('angular', False).value #Угловое воздействие + self.config['inverted'] = self.declare_parameter('inverted', False).value #Инвертируем self.add_on_set_parameters_callback(self.parameter_callback) self.get_logger().warn(f"Parameters updated {self.config}") diff --git a/lesson_06/step_0.py b/lesson_06/step_0.py index c318b10..bb13561 100644 --- a/lesson_06/step_0.py +++ b/lesson_06/step_0.py @@ -1,21 +1,16 @@ import rclpy from rclpy.node import Node - -from geometry_msgs.msg import Twist -from geometry_msgs.msg import PoseStamped, Pose - -from tf_transformations import euler_from_quaternion - -import math +from geometry_msgs.msg import Twist, PoseStamped, Pose +from transformations import euler_from_quaternion +import math def angle_diff(a1, a2): - a = a1-a2 - return (a+math.pi)%(2*math.pi)-math.pi + a = a1 - a2 + return (a + math.pi) % (2 * math.pi) - math.pi 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') @@ -48,13 +43,10 @@ class RurMover(Node): def main(args=None): rclpy.init(args=args) - mover = RurMover() - rclpy.spin(mover) - mover.destroy_node() rclpy.shutdown() -if __name__ == '__main__': +if __name__ == 'main': main() \ No newline at end of file diff --git a/lesson_06/step_1.py b/lesson_06/step_1.py new file mode 100644 index 0000000..a2bbb84 --- /dev/null +++ b/lesson_06/step_1.py @@ -0,0 +1,97 @@ +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Twist, PoseStamped, Pose +from transformations import euler_from_quaternion +import math +import subprocess + +def angle_diff(a1, a2): + a = a1 - a2 + return (a + math.pi) % (2 * math.pi) - math.pi + +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.publisher_pose = self.create_publisher(PoseStamped, '/waterstrider/pose', 10) + self.previous_pose = None + self.radius = 5.0 # Задаём начальный радиус круга + self.radius_growth_rate = 5 # На сколько радиус увеличивается после каждого прокрута + self.SIDE_TIME = 10.0 + self.LINEAR_SPEED = 1.1 #Линейная скорость + self.timer = self.create_timer(self.SIDE_TIME, self.pub_timer_callback) + + def pose_callback(self, msg): + heading = get_yaw_from_quaternion(msg.pose.orientation) + self.get_logger().info('Current heading: "%f"' % heading) + + # вычисление линейной и угловой скоростей для кругового движения + linear_velocity = self.LINEAR_SPEED + angular_velocity = linear_velocity / self.radius # v = ωr -> ω = v/r + + msg_pub = Twist() + msg_pub.linear.x = linear_velocity + msg_pub.angular.z = angular_velocity + self.publisher_twist.publish(msg_pub) + + if self.previous_pose is None: + self.previous_pose = msg.pose + return + + # Получение текущих координат + current_x = msg.pose.position.x + current_y = msg.pose.position.y + current_z = msg.pose.position.z # Добавлено получение координаты Z + + # Получение предыдущих координат + previous_x = self.previous_pose.position.x + previous_y = self.previous_pose.position.y + + # Расчет дельты координат + delta_x = current_x - previous_x + delta_y = current_y - previous_y + + # Вычисление угла COG + cog_radians = math.atan2(delta_y, delta_x) + psi_x = cog_radians # Угол psi к оси X + + # Вывод значения геопозиции и COG в консоль + self.get_logger().info(f'X={current_x}, Y={current_y}, Z={current_z}, psi_X={psi_x}') + + # Обновление предыдущих координат + self.previous_pose = msg.pose + # Создание сообщения с текущими координатами и ориентацией + pose_msg = PoseStamped() + pose_msg.header.stamp = self.get_clock().now().to_msg() + pose_msg.header.frame_id = "world" # или другой frame_id, если необходимо + pose_msg.pose.position.x = current_x + pose_msg.pose.position.y = current_y + pose_msg.pose.position.z = current_z + + # Копирование ориентации из входящего сообщения + pose_msg.pose.orientation = msg.pose.orientation + + # Публикация сообщения + self.publisher_pose.publish(pose_msg) + + + def pub_timer_callback(self): + # Увеличиваем радиус следующего круга + self.radius += self.radius_growth_rate + self.get_logger().info('New radius: "%f"' % self.radius) + + + +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