From b50a56a61ff0e21b1e870ad803e00d9cba3d96c8 Mon Sep 17 00:00:00 2001 From: Alexander Zhandarov <4l4st4rgrum@gmail.com> Date: Mon, 11 Nov 2024 23:45:27 +0300 Subject: [PATCH 1/4] =?UTF-8?q?=D0=A0=D0=B5=D0=B0=D0=BB=D0=B8=D0=B7=D0=BE?= =?UTF-8?q?=D0=B2=D0=B0=D0=BD=D0=BE=20=D0=B4=D0=B2=D0=B8=D0=B6=D0=B5=D0=BD?= =?UTF-8?q?=D0=B8=D0=B5=20=D0=BF=D0=BE=20=D0=BA=D0=B2=D0=B0=D0=B4=D1=80?= =?UTF-8?q?=D0=B0=D1=82=D1=83=20=D1=81=20=D0=BF=D0=BE=D0=B4=D0=B1=D0=BE?= =?UTF-8?q?=D1=80=D0=BE=D0=BC=20=D0=BA=D0=BE=D1=8D=D1=84=D0=B8=D1=86=D0=B8?= =?UTF-8?q?=D0=B5=D0=BD=D1=82=D0=BE=D0=B2=20=D1=80=D0=B5=D0=B3=D1=83=D0=BB?= =?UTF-8?q?=D1=8F=D1=82=D0=BE=D1=80=D0=B0=20=D0=B2=20rqt?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- launch/homework.launch.py | 26 ++++++++++++++ lesson_06/homework.py | 76 +++++++++++++++++++++++++++++++++++++++ 2 files changed, 102 insertions(+) create mode 100644 launch/homework.launch.py create mode 100644 lesson_06/homework.py diff --git a/launch/homework.launch.py b/launch/homework.launch.py new file mode 100644 index 0000000..6732b65 --- /dev/null +++ b/launch/homework.launch.py @@ -0,0 +1,26 @@ +import os +from ament_index_python import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + config_file = os.path.join(get_package_share_directory('lesson_06'), + 'config', + 'control-config.yaml') + return LaunchDescription([ + Node( + package='lesson_06', + namespace='waterstrider', + executable='homework', + name='controller' + ), + Node( + package='lesson_06', + namespace='waterstrider', + executable='pid_node', + name='pid', + parameters=[config_file] + ) + ]) diff --git a/lesson_06/homework.py b/lesson_06/homework.py new file mode 100644 index 0000000..43d279d --- /dev/null +++ b/lesson_06/homework.py @@ -0,0 +1,76 @@ +import rclpy +from rclpy.node import Node + +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 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.curr_heading = self.create_publisher(Float64, '/waterstrider/pid/state', 10) + self.heading_setpoint = self.create_publisher(Float64, '/waterstrider/pid/setpoint', 10) + self.pid_output = self.create_subscription(Float64, '/waterstrider/pid/output', self.pid_callback, 10) + + 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.heading = None + self.desired_heading = - math.pi / 2 + self.SIDE_TIME = 10.0 + self.LINEAR_SPEED = 1.0 + self.timer = self.create_timer(self.SIDE_TIME, self.pub_timer_callback) + + def pose_callback(self, msg): + self.heading = get_yaw_from_quaternion(msg.pose.orientation) + self.get_logger().info('Current heading: "%f"' % self.heading) + self.get_logger().info('Setpoint: "%f"' % self.desired_heading) + msg_pub = Twist() + msg_pub.linear.x = self.LINEAR_SPEED + + #для предотвращения разворотов по кругу при углах от pi до 2pi + if self.desired_heading <= math.pi / 2: + pass + elif self.heading < 0: + self.heading = 2 * math.pi + self.heading + + heading_msg = Float64(); heading_msg.data = self.heading + desired_heading_msg = Float64(); desired_heading_msg.data = self.desired_heading + self.curr_heading.publish(heading_msg) + self.heading_setpoint.publish(desired_heading_msg) + + def pid_callback(self, msg): + pid_out = msg.data + msg_pub = Twist() + msg_pub.linear.x = self.LINEAR_SPEED + msg_pub.angular.z = pid_out + self.publisher_twist.publish(msg_pub) + + 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() -- GitLab From 604897160e2aa40d6746a9f93f273b974a4d2f4b Mon Sep 17 00:00:00 2001 From: Alexander Zhandarov <4l4st4rgrum@gmail.com> Date: Tue, 12 Nov 2024 04:26:48 +0300 Subject: [PATCH 2/4] =?UTF-8?q?=D0=A0=D0=B5=D0=B0=D0=BB=D0=B8=D0=B7=D0=BE?= =?UTF-8?q?=D0=B2=D0=B0=D0=BD=D0=BE=20=D0=B2=D1=8B=D1=87=D0=B8=D1=81=D0=BB?= =?UTF-8?q?=D0=B5=D0=BD=D0=B8=D0=B5=20COG?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- lesson_06/homework.py | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/lesson_06/homework.py b/lesson_06/homework.py index 43d279d..91a0781 100644 --- a/lesson_06/homework.py +++ b/lesson_06/homework.py @@ -1,3 +1,5 @@ +from math import degrees + import rclpy from rclpy.node import Node @@ -31,6 +33,8 @@ class RurMover(Node): self.SIDE_TIME = 10.0 self.LINEAR_SPEED = 1.0 self.timer = self.create_timer(self.SIDE_TIME, self.pub_timer_callback) + self.prev_pose = None + self.curr_pose = None def pose_callback(self, msg): self.heading = get_yaw_from_quaternion(msg.pose.orientation) @@ -50,6 +54,19 @@ class RurMover(Node): self.curr_heading.publish(heading_msg) self.heading_setpoint.publish(desired_heading_msg) + self.prev_pose = self.curr_pose + self.curr_pose = msg.pose + if self.prev_pose != None: + cog = self.get_cog() + self.get_logger().info(f'cog data: {cog}') + + def get_cog(self): + delta_x = self.curr_pose.position.x - self.prev_pose.position.x + delta_y = self.curr_pose.position.y - self.prev_pose.position.y + cog_theta_radians = math.atan2(delta_y, delta_x) + cog_theta_degrees = degrees(cog_theta_radians) + return {' delta x' : delta_x, 'delta y' : delta_y, 'angle' : cog_theta_degrees} + def pid_callback(self, msg): pid_out = msg.data msg_pub = Twist() -- GitLab From e13a155253b4db0a2367d2322469c2451243a22e Mon Sep 17 00:00:00 2001 From: Alexander Zhandarov <4l4st4rgrum@gmail.com> Date: Sat, 30 Nov 2024 03:27:48 +0300 Subject: [PATCH 3/4] =?UTF-8?q?=D0=9F=D0=BE=D0=B4=D0=BE=D0=B1=D1=80=D0=B0?= =?UTF-8?q?=D0=BD=D1=8B=20=D0=BA=D0=BE=D1=8D=D1=84=D0=B8=D1=86=D0=B8=D0=B5?= =?UTF-8?q?=D0=BD=D1=82=D1=8B=20=D0=B4=D0=BB=D1=8F=20=D1=80=D0=B5=D0=B3?= =?UTF-8?q?=D1=83=D0=BB=D1=8F=D1=82=D0=BE=D1=80=D0=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- config/control-config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/config/control-config.yaml b/config/control-config.yaml index 80bbe66..d22a303 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 + kd: 8.0 ki: 0.0 - kp: 0.04 + kp: 2.0 start_type_description_service: true use_sim_time: false -- GitLab From a6018dd5e9248f519da15289b2eff8a3e0715bb4 Mon Sep 17 00:00:00 2001 From: Alexander Zhandarov <4l4st4rgrum@gmail.com> Date: Sat, 30 Nov 2024 04:00:44 +0300 Subject: [PATCH 4/4] =?UTF-8?q?=D0=A4=D1=83=D0=BD=D0=BA=D1=86=D0=B8=D1=8F?= =?UTF-8?q?=20get=5Fcog=20=D0=BF=D0=B5=D1=80=D0=B5=D0=B4=D0=B5=D0=BB=D0=B0?= =?UTF-8?q?=D0=BD=D0=B0=20=D0=BF=D0=BE=D0=B4=20=D1=80=D0=B0=D0=B1=D0=BE?= =?UTF-8?q?=D1=82=D1=83=20=D1=81=20=D0=BE=D1=87=D0=B5=D1=80=D0=B5=D0=B4?= =?UTF-8?q?=D1=8C=D1=8E?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- lesson_06/homework.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/lesson_06/homework.py b/lesson_06/homework.py index 91a0781..9d9234c 100644 --- a/lesson_06/homework.py +++ b/lesson_06/homework.py @@ -10,6 +10,7 @@ from std_msgs.msg import Float64 from tf_transformations import euler_from_quaternion import math +import queue def angle_diff(a1, a2): a = a1-a2 @@ -35,6 +36,7 @@ class RurMover(Node): self.timer = self.create_timer(self.SIDE_TIME, self.pub_timer_callback) self.prev_pose = None self.curr_pose = None + self.pose_queue = queue.Queue() def pose_callback(self, msg): self.heading = get_yaw_from_quaternion(msg.pose.orientation) @@ -54,13 +56,13 @@ class RurMover(Node): self.curr_heading.publish(heading_msg) self.heading_setpoint.publish(desired_heading_msg) - self.prev_pose = self.curr_pose - self.curr_pose = msg.pose - if self.prev_pose != None: + self.pose_queue.put(msg.pose) + if len(self.pose_queue.queue) >= 2: cog = self.get_cog() self.get_logger().info(f'cog data: {cog}') def get_cog(self): + self.prev_pose, self.curr_pose = self.pose_queue.get(), self.pose_queue.get() delta_x = self.curr_pose.position.x - self.prev_pose.position.x delta_y = self.curr_pose.position.y - self.prev_pose.position.y cog_theta_radians = math.atan2(delta_y, delta_x) -- GitLab