diff --git a/launch/square_proportional.launch.py b/launch/square_proportional.launch.py new file mode 100644 index 0000000000000000000000000000000000000000..13c8bf8b54447e49e0423b4b60131f9f34cae271 --- /dev/null +++ b/launch/square_proportional.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='square_proportional', + name='square_proportional' + ), + Node( + package='rqt_plot', + namespace='', + executable='rqt_plot', + arguments=['/turtle1/pose/theta', '/turtle1/pose/x', '/turtle1/pose/y'], + name='rqt_plot' + ) + ]) diff --git a/lesson_05/square_proportional.py b/lesson_05/square_proportional.py new file mode 100644 index 0000000000000000000000000000000000000000..f90fea6f1d89e74c61a92d9e1b6091d88fe069b3 --- /dev/null +++ b/lesson_05/square_proportional.py @@ -0,0 +1,53 @@ +import rclpy +from rclpy.node import Node + +from geometry_msgs.msg import Twist +from turtlesim.msg import Pose +import math + + +def ang_spd(a1, a2): + a = a1-a2 + K_p = 500 + return K_p*((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 + ANGULAR_SPEED = ang_spd(self.desired_heading, msg.theta) + msg_pub.angular.z = math.radians(ANGULAR_SPEED) + if ANGULAR_SPEED > 0: + self.get_logger().info('Turn left') + else: + 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()