diff --git a/launch/step_alex.launch.py b/launch/step_alex.launch.py new file mode 100644 index 0000000000000000000000000000000000000000..db84b8208ddf224839f2d71e9534f25c9cd32a97 --- /dev/null +++ b/launch/step_alex.launch.py @@ -0,0 +1,19 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='turtlesim', + namespace='', + executable='turtlesim_node', + name='turtle' + ), + Node( + package='lesson_05', + namespace='', + executable='step_alex', + name='square_alex' + ) + ]) \ No newline at end of file diff --git a/lesson_05/step_alex.py b/lesson_05/step_alex.py new file mode 100644 index 0000000000000000000000000000000000000000..5d58005e43a303fcac6240b0df38c4a81a777ab7 --- /dev/null +++ b/lesson_05/step_alex.py @@ -0,0 +1,88 @@ +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 SquareMover(Node): + def __init__(self): + super().__init__('square_mover') + 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.current_pose = None + + self.side_length = 3.0 + self.linear_speed = 0.5 + self.angular_speed_max = 1.0 + self.kp_angular = 4.0 + + self.target_heading = 0.0 + self.start_x = 0.0 + self.start_y = 0.0 + self.corner_count = 0 + self.is_turning = False + + self.get_logger().info('Node initialized, starting...') + + def pose_callback(self, msg): + self.current_pose = msg + if self.is_turning: + self.handle_turning() + else: + self.handle_moving() + + def handle_moving(self): + if self.current_pose is None: + return + + distance = math.sqrt( + (self.current_pose.x - self.start_x) ** 2 + (self.current_pose.y - self.start_y) ** 2 + ) + if distance >= self.side_length: + self.is_turning = True + self.target_heading = (self.target_heading + math.pi / 2) % (2 * math.pi) + self.get_logger().info(f'Reached side, starting turn to {self.target_heading:.2f} radians') + return + + angle_error = angle_diff(self.target_heading, self.current_pose.theta) + angular_speed = max(-self.angular_speed_max, min(self.kp_angular * angle_error, self.angular_speed_max)) + + msg = Twist() + msg.linear.x = self.linear_speed + msg.angular.z = angular_speed + self.publisher_twist.publish(msg) + + def handle_turning(self): + if self.current_pose is None: + return + + angle_error = angle_diff(self.target_heading, self.current_pose.theta) + if abs(angle_error) <= 0.01: + self.is_turning = False + self.corner_count += 1 + if self.corner_count >= 4: + self.corner_count = 0 + self.get_logger().info('Square complete, restarting...') + self.start_x = self.current_pose.x + self.start_y = self.current_pose.y + self.get_logger().info(f'Starting new side from ({self.start_x:.2f}, {self.start_y:.2f})') + return + + angular_speed = max(-self.angular_speed_max, min(self.kp_angular * angle_error, self.angular_speed_max)) + msg = Twist() + msg.angular.z = angular_speed + self.publisher_twist.publish(msg) + +def main(args=None): + rclpy.init(args=args) + node = SquareMover() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/setup.py b/setup.py index 0e83277e26896894fdb28dc65870d33952f3406f..429102796cb90aa8a86df8c08e6108a6cef245e4 100644 --- a/setup.py +++ b/setup.py @@ -27,6 +27,8 @@ 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', + 'step_alex = lesson_05.step_alex:main', ], }, )