From 85365a123158d4d723687a45e598b49c0d953856 Mon Sep 17 00:00:00 2001 From: sberestov Date: Mon, 28 Oct 2024 23:30:56 +0300 Subject: [PATCH 1/4] Added a script to launch the new file --- 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 9ee2a35945f64e7ce2cdc1275680bc42de534196 Mon Sep 17 00:00:00 2001 From: sberestov Date: Mon, 28 Oct 2024 23:34:52 +0300 Subject: [PATCH 2/4] Added a launch file for a new script --- launch/step_5.launch.py | 26 ++++++++++++++++++++++++++ 1 file changed, 26 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..9f3fdfd --- /dev/null +++ b/launch/step_5.launch.py @@ -0,0 +1,26 @@ +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='step_5', + name='square' + ), + Node( + package='rqt_plot', + namespace='', + executable='rqt_plot', + arguments=['/turtle1/pose/theta','/turtle1/pose/x'], + name='rqt' + ) + ]) -- GitLab From f559c2c5252ccc62e04061ac175cd330c0bbd443 Mon Sep 17 00:00:00 2001 From: sberestov Date: Tue, 29 Oct 2024 00:29:19 +0300 Subject: [PATCH 3/4] Added a P-regulator --- lesson_05/step_5.py | 49 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 49 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..64d4fee --- /dev/null +++ b/lesson_05/step_5.py @@ -0,0 +1,49 @@ +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) + msg_pub.angular.z = math.cos (math.pi / 2) + self.publisher_twist.publish(msg_pub) + + def pub_timer_callback(self): + self.desired_heading += math.pi / 2 + self.desired_heading %= math.pi * 2 + #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 2dd53a979dea8eff8d32298b60c0b7321f72a7a9 Mon Sep 17 00:00:00 2001 From: sberestov Date: Sat, 2 Nov 2024 17:30:49 +0300 Subject: [PATCH 4/4] Added a P-regulator --- lesson_05/step_5.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/lesson_05/step_5.py b/lesson_05/step_5.py index 64d4fee..4bcac73 100644 --- a/lesson_05/step_5.py +++ b/lesson_05/step_5.py @@ -19,21 +19,22 @@ class RurMover(Node): self.desired_heading = - math.pi / 2 self.SIDE_TIME = 5.0 self.LINEAR_SPEED = 0.5 + self.p_angular = 4.0 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) + 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) - msg_pub.angular.z = math.cos (math.pi / 2) + msg_pub.angular.z = self.p_angular * error self.publisher_twist.publish(msg_pub) def pub_timer_callback(self): self.desired_heading += math.pi / 2 - self.desired_heading %= math.pi * 2 - #self.get_logger().info('New angle setpoint: "%f"' % self.desired_heading) + 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) -- GitLab