natto1784/atom
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

remove sleep and stop hardcoding time

Browse Source 
Signed-off-by: Amneesh Singh <natto@weirdnatto.in>
This commit is contained in:
Amneesh Singh
2022-12-11 00:36:27 +05:30
parent c37de6bf92
commit 55c35eba9d
1 changed files with 18 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
src/atom/scripts/solution.py
View File

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