diff --git a/src/atom/scripts/solution.py b/src/atom/scripts/solution.py index a2afad8..f79e57c 100644 --- a/src/atom/scripts/solution.py +++ b/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)