Assignment_1_submission

Akshita/Assignment_1

@@ -0,0 +1,61 @@
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Twist
+from turtlesim.msg import Pose
+
+class DrawSquareNode(Node):
+
+    x_min = 5.544444561004639
+    y_min = 5.544444561004639
+    x_max = x_min + 3
+    y_max = y_min + 3
+
+    def __init__(self):
+        super().__init__("draw_square")
+        self.cmd_vel_pub = self.create_publisher(Twist, '/turtle1/cmd_vel', 10)
+        self.pose_sub = self.create_subscription(Pose, '/turtle1/pose', self.pose_callback, 10)
+        self.get_logger().info("Draw square node has been started")
+        self.counter = -1
+
+    def pose_callback(self, pose: Pose):
+        vel = Twist()
+        x_min = DrawSquareNode.x_min
+        y_min = DrawSquareNode.y_min
+        x_max = DrawSquareNode.x_max
+        y_max = DrawSquareNode.y_max
+        x = pose.x
+        y = pose.y
+
+        # self.get_logger().info(f'Hi {x}')
+
+        if(x == x_min and y == y_min):
+            self.counter += 1
+            self.get_logger().info(f'Counter = {self.counter}')
+
+        if((x>x_min-0.01 and x<x_min+0.01) and (y>y_min)):
+            vel.linear.y = -2.0
+            vel.linear.x = 0.0
+
+        elif((y>y_min-0.01 and y<y_min+0.01) and (x<x_max)):
+            vel.linear.x = 2.0
+            vel.linear.y = 0.0
+
+        elif((x>x_max-0.01 and x<x_max+0.01) and (y<y_max)):
+            vel.linear.y = 2.0
+            vel.linear.x = 0.0
+
+        elif((y>y_max-0.01 and y<y_max+0.01) and (x>x_min)):
+            vel.linear.x = -2.0
+            vel.linear.y = 0.0
+
+        self.cmd_vel_pub.publish(vel)
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = DrawSquareNode()
+    rclpy.spin(node)
+    rclpy.shutdown()
+
+if(__name__ == '__main__'):
+    main()