remove sleep and stop hardcoding time

Signed-off-by: Amneesh Singh <natto@weirdnatto.in>

src/atom/scripts/solution.py

@@ -7,26 +7,35 @@ from turtlesim.msg import Pose
 
 
 class Solution:
-    speed = 0.05
+    cur, init = (0, 0), (0, 0)
+    theta = 0
+    speed = 1
 
     def __init__(self, radius):
         rospy.init_node('turtle', anonymous=True)
         self.pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
-        self.sub = rospy.Subscriber('/turtle1/pose', Pose)
+        self.sub = rospy.Subscriber('/turtle1/pose', Pose, self.callback)
         self.rate = rospy.Rate(10)
         self.vel = Twist()
         self.radius = radius
 
+    def callback(self, pos):
+        self.cur = (pos.x, pos.y)
+        self.theta = pos.theta
+        rospy.loginfo("balls: %f - %f - %f",
+                      self.cur[0],
+                      self.cur[1],
+                      self.theta)
+
     def move_semicircle(self):
         self.vel.linear.x = self.radius * self.speed
         self.vel.angular.z = self.speed
-        req_time = math.pi / self.speed
-        t = rospy.Time.now()
 
-        while rospy.Time.now() <= t + rospy.Duration.from_sec(req_time):
+        while self.cur[0] >= self.init[0]:
+            if self.init == (0, 0):
+                self.init = self.cur
             self.pub.publish(self.vel)
             rospy.loginfo("wheeee")
-            self.rate.sleep()
 
         self.vel.linear.x = 0
         self.vel.angular.z = 0
@@ -34,27 +43,21 @@ class Solution:
 
     def rotate(self):
         self.vel.angular.z = self.speed
-        req_time = math.pi / (2 * self.speed)
-        t = rospy.Time.now()
+        angle = - math.pi / 2
 
-        while rospy.Time.now() <= t + rospy.Duration.from_sec(req_time):
+        while self.theta >= 0 or self.theta < angle:
             self.pub.publish(self.vel)
             rospy.loginfo("khhhhh")
-            self.rate.sleep()
 
         self.vel.angular.z = 0
         self.pub.publish(self.vel)
 
     def move_straight(self):
         self.vel.linear.x = self.speed * self.radius
-        req_time = 2 * radius / (self.vel.linear.x)
-        rospy.loginfo("acha %f\n", req_time)
-        t = rospy.Time.now()
 
-        while rospy.Time.now() <= t + rospy.Duration.from_sec(req_time):
+        while self.cur[1] > self.init[1]:
             self.pub.publish(self.vel)
             rospy.loginfo("drrrrr")
-            self.rate.sleep()
 
         self.vel.linear.x = 0
         self.pub.publish(self.vel)