From d3eb39da2b4999fc81c7ecde28b691dec79f2915 Mon Sep 17 00:00:00 2001 From: vi Date: Fri, 25 Oct 2024 18:15:33 +0300 Subject: [PATCH 1/3] added node rqt --- launch/my_launch.py | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) create mode 100644 launch/my_launch.py diff --git a/launch/my_launch.py b/launch/my_launch.py new file mode 100644 index 0000000..5e222ac --- /dev/null +++ b/launch/my_launch.py @@ -0,0 +1,25 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='turtlesim', + namespace='', + executable='turtlesim_node', + name='sim' + ), + Node( + package='lesson_05', + namespace='', + executable='pid_01', + name='square' + ), + Node( + package='rqt_plot', + namespace='', + executable='rqt_plot', + name='rqt' + ) + ]) -- GitLab From f2134a7b8ac5c93aadd89bc00a0391aaf6814f46 Mon Sep 17 00:00:00 2001 From: vi Date: Fri, 25 Oct 2024 18:19:55 +0300 Subject: [PATCH 2/3] added p-regulator --- lesson_05/pid_01.py | 60 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 60 insertions(+) create mode 100644 lesson_05/pid_01.py diff --git a/lesson_05/pid_01.py b/lesson_05/pid_01.py new file mode 100644 index 0000000..f9db8fa --- /dev/null +++ b/lesson_05/pid_01.py @@ -0,0 +1,60 @@ +import rclpy +from rclpy.node import Node + +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.desired_heading = - math.pi / 2 + self.SIDE_TIME = 5.0 + self.LINEAR_SPEED = 0.5 + self.timer = self.create_timer(self.SIDE_TIME, self.pub_timer_callback) + + def pose_callback(self, msg): + self.get_logger().info('Current heading: "%f"' % msg.theta) + self.get_logger().info('Setpoint: "%f"' % self.desired_heading) + msg_pub = Twist() + msg_pub.linear.x = self.LINEAR_SPEED + error = angle_diff(self.desired_heading, msg.theta) + + + # p-regulator + p = self.desired_heading - msg.theta + kP = 0.6* 12 + + if p < 0: + msg_pub.angular.z = math.radians(p*kP) + self.get_logger().info('Turn left') + else: + msg_pub.angular.z = math.radians(-p*kP) + self.get_logger().info('Turn right') + 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() \ No newline at end of file -- GitLab From 8ce10dac0a9e26d5f1313555ac812b23eae4f264 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=92=D0=B8=D0=BE=D0=BB=D0=B5=D1=82=D1=82=D0=B0=20=D0=90?= =?UTF-8?q?=D0=B9=D0=B2=D0=B0=D0=B7=D1=8C=D1=8F=D0=BD?= Date: Wed, 13 Nov 2024 20:31:08 +0000 Subject: [PATCH 3/3] p-regulator, replace --- lesson_05/pid_01.py | 19 +++++++------------ 1 file changed, 7 insertions(+), 12 deletions(-) diff --git a/lesson_05/pid_01.py b/lesson_05/pid_01.py index f9db8fa..96777fc 100644 --- a/lesson_05/pid_01.py +++ b/lesson_05/pid_01.py @@ -20,6 +20,7 @@ class RurMover(Node): self.SIDE_TIME = 5.0 self.LINEAR_SPEED = 0.5 self.timer = self.create_timer(self.SIDE_TIME, self.pub_timer_callback) + self.pk = 5 def pose_callback(self, msg): self.get_logger().info('Current heading: "%f"' % msg.theta) @@ -27,18 +28,8 @@ class RurMover(Node): msg_pub = Twist() msg_pub.linear.x = self.LINEAR_SPEED error = angle_diff(self.desired_heading, msg.theta) - - - # p-regulator - p = self.desired_heading - msg.theta - kP = 0.6* 12 - - if p < 0: - msg_pub.angular.z = math.radians(p*kP) - self.get_logger().info('Turn left') - else: - msg_pub.angular.z = math.radians(-p*kP) - self.get_logger().info('Turn right') + + msg_pub.angular.z = error * self.pk self.publisher_twist.publish(msg_pub) def pub_timer_callback(self): @@ -56,5 +47,9 @@ def main(args=None): mover.destroy_node() rclpy.shutdown() +if __name__ == '__main__': + main() + + if __name__ == '__main__': main() \ No newline at end of file -- GitLab