def __init__(self, state, angle):
self.state = state
self.target_angle = angle
provisional = self.target_angle - self.state.angle
if provisional >= pi:
self.turn = provisional - 2*np.pi
elif provisional <= -pi:
self.turn = provisional + 2*np.pi
else:
self.turn = provisional
if self.turn > 0:
self.direction = 'L'
else:
self.direction = 'R'
self.target_angle = rectify_angle_pi(self.target_angle)
rospy.loginfo("Target angle: " + str(self.target_angle))
self.done = False
self.turning_angle = angle
print "here"
print provisional, self.target_angle, self.state.angle, self.direction
def __init__(self, state, angle):
if angle < 0:
self.direction = 'R'
else:
self.direction = 'L'
self.state = state
self.target_angle = rectify_angle_pi(state.angle + angle)
rospy.loginfo("Target angle: " + str(self.target_angle))
self.done = False
self.turning_angle = angle
def act(self):
closest = float("inf")
index = None
for i in xrange(len(self.state.ranges)):
if self.state.ranges[i] < closest:
if self.state.ranges[i] >= self.state.rmin:
closest = self.state.ranges[i]
index = i
self.targetangle = index * self.state.anginc + self.state.angle
self.targetangle = rectify_angle_pi(self.targetangle)
print self.state.angle, self.state.anglestart, index, closest
turnfuc = Turn(self.state, self.targetangle)
while not turnfuc.done:
turnfuc.act()
self.rate.sleep()
self.done = True
def follow(self):
print
self.state.anglestart
print
self.state.angle_max
closest = float("inf")
index = None
for i in xrange(45):
if self.state.ranges[i] < closest:
if self.state.ranges[i] >= self.state.rmin:
closest = self.state.ranges[i]
index = i
for i in xrange(315, 360):
if self.state.ranges[i] < closest:
if self.state.ranges[i] >= self.state.rmin:
closest = self.state.ranges[i]
index = i
self.targetangle = index * self.state.anginc + self.state.angle
self.targetangle = rectify_angle_pi(self.targetangle)
print
self.state.angle, self.state.anglestart, index, closest
turnfuc = Turn(self.state, self.targetangle)
while not turnfuc.done:
turnfuc.act()
self.rate.sleep()
# forwardfuc = Forward(self.state)
estdist = self.state.ranges[index]
drive_dist = estdist - .5
if drive_dist > 0:
forwardfuc = Forward(self.state, drive_dist)
while not forwardfuc.done:
forwardfuc.act()
self.rate.sleep()
self.done = True
def __init__(self, state, angle):
self.state = state
self.target_angle = rectify_angle_pi(state.angle + angle)
rospy.loginfo("Target angle: " + str( self.target_angle))
self.done = False
def act(self):
closest = float("inf")
index = None
for i in xrange(45):
if self.state.ranges[i] < closest:
if self.state.ranges[i] >= self.state.rmin:
closest = self.state.ranges[i]
index = i
for i in xrange(315, 360):
if self.state.ranges[i] < closest:
if self.state.ranges[i] >= self.state.rmin:
closest = self.state.ranges[i]
index = i
self.targetangle = index * self.state.anginc + self.state.angle
self.targetangle = rectify_angle_pi(self.targetangle)
# Forward
estdist = self.state.ranges[index]
drive_dist = estdist - .5
move_cmd = Twist()
if drive_dist > 0:
self.target_x = self.state.x + drive_dist * cos(self.state.angle)
self.target_y = self.state.y + drive_dist * sin(self.state.angle)
self.target_point = array((self.target_x, self.target_y))
self.total_dist = drive_dist
error = norm(self.target_point - array((self.state.x, self.state.y)))
rospy.loginfo("Current x, y: " + str(self.state.x) + ", " + str(self.state.y))
# if still not at target, compute the next move to make.
if (error > .02):
move_cmd.linear.x = compute_vel_from_dist(error)
provisional = self.targetangle - self.state.angle
if provisional != 0:
if provisional >= pi:
self.turn = provisional - 2 * np.pi
elif provisional <= -pi:
self.turn = provisional + 2 * np.pi
else:
self.turn = provisional
if self.turn > 0:
self.direction = 'L'
else:
self.direction = 'R'
self.targetangle = rectify_angle_pi(self.targetangle)
rospy.loginfo("Target angle: " + str(self.targetangle))
self.done = False
# turn act
error = abs(self.targetangle - self.state.angle)
rospy.loginfo("Current angle: " + str(self.state.angle))
if (error > .02):
if self.direction == 'L':
move_cmd.angular.z = compute_vel_from_angle(error)
elif self.direction == 'R':
move_cmd.angular.z = -compute_vel_from_angle(error)
self.state.cmd_vel.publish(move_cmd)
def get_target_angle(self):
closest, index = self.get_closest_info()
targetangle = index * self.state.anginc + self.state.angle
targetangle = rectify_angle_pi(self.targetangle)
return targetangle