Vansh madan_assignment1_submission

vansh madan

@@ -0,0 +1,99 @@
+task 1:
+#!/usr/bin/env python3
+
+import rospy
+from geometry_msgs.msg import Twist
+import time
+
+def draw_square():
+    pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
+    rospy.init_node('square_publisher', anonymous=True)
+
+    move_cmd = Twist()
+    turn_cmd = Twist()
+
+    # Hardcoded values for linear and angular speeds
+    move_cmd.linear.x = 1.0  # Move forward with 1.0 m/s
+    move_cmd.angular.z = 0.0
+
+    turn_cmd.linear.x = 0.0
+    turn_cmd.angular.z = 1.0  # Turn with 1.0 rad/s
+
+    rate = rospy.Rate(10)  # 10Hz
+
+    while not rospy.is_shutdown():
+        for i in range(4):  # 4 sides of a square
+            # Move forward
+            start_time = time.time()
+            while time.time() - start_time < 2.0:  # move forward for 2 seconds
+                pub.publish(move_cmd)
+                rate.sleep()
+
+            # Turn 90 degrees
+            start_time = time.time()
+            while time.time() - start_time < 1.57:  # turn 90 degrees (1.57 rad) at 1 rad/s
+                pub.publish(turn_cmd)
+                rate.sleep()
+
+        # Optional sleep between squares
+        rospy.sleep(1.0)
+
+if __name__ == '__main__':
+    try:
+        draw_square()
+    except rospy.ROSInterruptException:
+        pass
+
+
+task 2:
+  #!/usr/bin/env python3
+
+import rospy
+from turtlesim.msg import Pose
+import math
+
+class LoopCounter:
+    def __init__(self):
+        rospy.init_node('pose_subscriber', anonymous=True)
+        self.sub = rospy.Subscriber('/turtle1/pose', Pose, self.callback)
+        self.initial_pos = None
+        self.loop_count = 0
+        self.visited_quadrants = set()
+
+    def get_quadrant(self, x, y):
+        if x >= 5.5 and y >= 5.5:
+            return 1
+        elif x < 5.5 and y >= 5.5:
+            return 2
+        elif x < 5.5 and y < 5.5:
+            return 3
+        elif x >= 5.5 and y < 5.5:
+            return 4
+        return 0
+
+    def is_close(self, pos1, pos2, tol=0.5):
+        return math.hypot(pos1[0] - pos2[0], pos1[1] - pos2[1]) < tol
+
+    def callback(self, msg):
+        current_pos = (msg.x, msg.y)
+
+        if self.initial_pos is None:
+            self.initial_pos = current_pos
+            rospy.loginfo(f"Initial position set at: {self.initial_pos}")
+
+        # Track quadrants visited
+        quadrant = self.get_quadrant(msg.x, msg.y)
+        self.visited_quadrants.add(quadrant)
+
+        # Check if all quadrants visited and turtle returned to start
+        if len(self.visited_quadrants) == 4 and self.is_close(current_pos, self.initial_pos):
+            self.loop_count += 1
+            rospy.loginfo(f"Loop completed! Total loops: {self.loop_count}")
+            self.visited_quadrants.clear()  # Reset for next loop
+
+if __name__ == '__main__':
+    try:
+        LoopCounter()
+        rospy.spin()
+    except rospy.ROSInterruptException:
+        pass