diff --git a/lesson_04/__init__.py b/lesson_04/__init__.py deleted file mode 100644 index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000 diff --git a/lesson_04/turtle_a.py b/lesson_04/turtle_a.py new file mode 100644 index 0000000000000000000000000000000000000000..f228a09b01bd8708774f84c38719722c938752be --- /dev/null +++ b/lesson_04/turtle_a.py @@ -0,0 +1,45 @@ +import rclpy +from rclpy.node import Node +from rclpy.duration import Duration + +from geometry_msgs.msg import Twist +import math + + +class RurMover(Node): + def __init__(self): + super().__init__('minimal_publisher') + self.publisher_twist = self.create_publisher(Twist, 'turtle1/cmd_vel', 10) + self.main() + + def publish_twist(self, linear, angular): + self.get_logger().info('Linear: "%d", angular: "%d"' % (linear, angular)) + msg = Twist() + msg.linear.x = float(linear) + msg.angular.z = math.radians(float(angular)) + self.publisher_twist.publish(msg) + self.get_clock().sleep_for(Duration(seconds=1.0)) + + def main(self): + + self.publish_twist(0, 75.0) + self.publish_twist(3.0, 0.0) + self.publish_twist(0, -150.0) + self.publish_twist(3.0, 0.0) + self.publish_twist(0, 180.2) + self.publish_twist(1.0, 0.0) + self.publish_twist(0, 72.3) + self.publish_twist(1.1, 0.0) + self.publish_twist(0, 72.3) + self.publish_twist(1.0, 0.0) + + +def main(args=None): + rclpy.init(args=args) + mover = RurMover() + rclpy.spin(mover) + mover.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main()