From 0493a6579ad1b406889a1c73505c4ad1e94c54b4 Mon Sep 17 00:00:00 2001 From: ashtykalova Date: Mon, 28 Oct 2024 23:33:07 +0300 Subject: [PATCH 1/5] create step_5.py (step_4.py copy) --- lesson_05/step_5.py | 54 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 54 insertions(+) create mode 100644 lesson_05/step_5.py diff --git a/lesson_05/step_5.py b/lesson_05/step_5.py new file mode 100644 index 0000000..0aadcee --- /dev/null +++ b/lesson_05/step_5.py @@ -0,0 +1,54 @@ +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) + if error < 0: + msg_pub.angular.z = math.radians(90.0) + self.get_logger().info('Turn left') + else: + msg_pub.angular.z = math.radians(-90.0) + 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 6b2bb985536adc3f37a6f783c5329e41addd8602 Mon Sep 17 00:00:00 2001 From: ashtykalova Date: Mon, 28 Oct 2024 23:34:29 +0300 Subject: [PATCH 2/5] add PID regulator class --- lesson_05/step_5.py | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/lesson_05/step_5.py b/lesson_05/step_5.py index 0aadcee..9e2fb8d 100644 --- a/lesson_05/step_5.py +++ b/lesson_05/step_5.py @@ -5,6 +5,30 @@ from geometry_msgs.msg import Twist from turtlesim.msg import Pose import math +class PID: + def __init__(self, i_max = 0.0, i_min = 0.0, i_gain = 0.0, p_gain = 0.0, d_gain = 0.0,): + self.iMax = i_max # Maximum and minimum allowable integrator state + self.iMin = i_min + self.iGain = i_gain # integral gain + self.pGain = p_gain # proportional gain + self.dGain = d_gain # derivative gain + + self.dState = 0.0 # Last position input + self.iState = 0.0 # Integrator state + + def update_PID(self, error, position): + pTerm = self.pGain * error # calculate the proportional term + self.iState += error # calculate the integral state with appropriate limiting + if self.iState > self.iMax: + self.iState = self.iMax + elif self.iState < self.iMin: + self.iState = self.iMin + + iTerm = self.iGain * self.iState # calculate the integral term + dTerm = self.dGain * (position - self.dState) + self.dState = position + + return pTerm + iTerm - dTerm def angle_diff(a1, a2): a = a1-a2 -- GitLab From eb7dca268f8bfb4f063406ef370e448138abc87a Mon Sep 17 00:00:00 2001 From: ashtykalova Date: Mon, 28 Oct 2024 23:53:15 +0300 Subject: [PATCH 3/5] PID regulator is included in RurMover --- lesson_05/step_5.py | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/lesson_05/step_5.py b/lesson_05/step_5.py index 9e2fb8d..b83522f 100644 --- a/lesson_05/step_5.py +++ b/lesson_05/step_5.py @@ -44,6 +44,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.PID = PID() def pose_callback(self, msg): self.get_logger().info('Current heading: "%f"' % msg.theta) @@ -51,12 +52,7 @@ class RurMover(Node): msg_pub = Twist() msg_pub.linear.x = self.LINEAR_SPEED error = angle_diff(self.desired_heading, msg.theta) - if error < 0: - msg_pub.angular.z = math.radians(90.0) - self.get_logger().info('Turn left') - else: - msg_pub.angular.z = math.radians(-90.0) - self.get_logger().info('Turn right') + msg_pub.angular.z = self.PID.update_PID(error, msg.theta) self.publisher_twist.publish(msg_pub) def pub_timer_callback(self): -- GitLab From fdd622ba8d0b64b84dbc97e07137fce04c29882b Mon Sep 17 00:00:00 2001 From: ashtykalova Date: Mon, 28 Oct 2024 23:54:12 +0300 Subject: [PATCH 4/5] Add step_5 node --- setup.py | 1 + 1 file changed, 1 insertion(+) diff --git a/setup.py b/setup.py index 0e83277..223a33c 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', + 'step_5 = lesson_05.step_5:main', ], }, ) -- GitLab From 152c8634e10d6b36b1b72fa143f4e802e0a927bb Mon Sep 17 00:00:00 2001 From: ashtykalova Date: Mon, 28 Oct 2024 23:54:35 +0300 Subject: [PATCH 5/5] Create step_5 launch with rqt --- launch/step_5.launch.py | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) create mode 100644 launch/step_5.launch.py diff --git a/launch/step_5.launch.py b/launch/step_5.launch.py new file mode 100644 index 0000000..686110f --- /dev/null +++ b/launch/step_5.launch.py @@ -0,0 +1,24 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import ExecuteProcess + + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='turtlesim', + namespace='', + executable='turtlesim_node', + name='sim' + ), + Node( + package='lesson_05', + namespace='', + executable='step_5', + name='square' + ), + ExecuteProcess( + cmd=[['rqt']], + shell=False + ), + ]) -- GitLab