def getVelocity(self, cur, goal, prev):
desired = Pose()
d = self.getGoalDistance(cur, goal)
self.theta_err = atan2(goal.y - cur.y,
goal.x - cur.x) - self.theta_change
# if self.theta_err > 3.14:
# self.theta_err = self.theta_err - 6.28
# if self.theta_err < -3.14:
# self.theta_err = self.theta_err + 6.28
desired.thetaVel = self.adjust_omega(self.theta_err)
self.theta_change = atan2(cur.y - prev.y, cur.x - prev.x)
if desired.thetaVel > self.OneDegree * 5:
desired.xVel = self.k2 / abs(self.theta_err)
if desired.xVel > 0.12:
desired.xVel = 0.12
if desired.xVel < 0.07:
desired.xVel = 0.07
elif abs(d) > self.linearTolerance:
desired.xVel = self.k3 * d
if desired.xVel > 0.12:
desired.xVel = 0.12
if desired.xVel < 0.07:
desired.xVel = 0.07
else:
desired.xVel = 0
return desired
# def getVelocity(self, cur, goal, prev, dT):
# desired = Pose()
# d = self.getGoalDistance(cur, goal)
# a = atan2(goal.y - cur.y, goal.x - cur.x)# - cur.theta
# b = atan2(cur.y - prev.y, cur.x - prev.x)
# desired.thetaVel = self.adjust_omega(a, b)
# # if a > 0:
# # if a > (0.0872665 * 2 ):
# # desired.thetaVel = 0.0872665 * 2 #+ self.kB*b
# # else:
# # desired.thetaVel = 0.0174533 * 2
# # elif a < 0:
# # if a < (-0.0872665 * 2):
# # desired.thetaVel = -0.0872665 * 2 #+ self.kB*
# # else:
# # desired.thetaVel = -0.0174533 * 2
# # else:
# # desired.thetaVel = 0
# if abs(d) > self.linearTolerance:
# desired.xVel = 0.2
# else:
# desired.xVel = 0
# def adjust_omega(self, theta_final, theta_counter):
# if abs(theta_final) > 0.349066:
# limit = 0.0872665
# else:
# limit = 0.0349066
# if (theta_final > theta_counter):
# if (theta_final - theta_counter) > limit:
# w_rand = limit
# else:
# w_rand = .01745
# elif (theta_final < theta_counter):
# if (theta_final - theta_counter) < -limit:
# w_rand = -limit
# else:
# w_rand = -.01745
# else:
# w_rand = 0
# if abs(desired.thetaVel) > 1:
# desired.thetaVel = 1
# return w_rand
def get_velocity(self, cur, goal, dT):
desired = Pose()
goal_heading = atan2(goal.y - cur.y, goal.x - cur.x)
a = -cur.theta + goal_heading
# In Automomous Mobile Robots, they assume theta_G=0. So for
# the error in heading, we have to adjust theta based on the
# (possibly non-zero) goal theta.
theta = self.normalize_pi(cur.theta - goal.theta)
b = -theta - a
# rospy.loginfo('cur=%f goal=%f a=%f b=%f', cur.theta, goal_heading,
# a, b)
d = self.get_goal_distance(cur, goal)
if self.forward_movement_only:
direction = 1
a = self.normalize_pi(a)
b = self.normalize_pi(b)
else:
direction = self.sign(cos(a))
a = self.normalize_half_pi(a)
b = self.normalize_half_pi(b)
# rospy.loginfo('After normalization, a=%f b=%f', a, b)
if abs(d) < self.linear_tolerance:
desired.xVel = 0
desired.thetaVel = self.kB * theta
else:
desired.xVel = self.kP * d * direction + (-self.error_prev +
d) * 7 * direction
print('iii: {}'.format(-self.error_prev + d))
desired.thetaVel = self.kA * a + self.kB * b
self.error_prev = d
# Adjust velocities if X velocity is too high.
if abs(desired.xVel) > self.max_linear_speed:
ratio = self.max_linear_speed / abs(desired.xVel)
desired.xVel *= ratio
desired.thetaVel *= ratio
# Adjust velocities if turning velocity too high.
if abs(desired.thetaVel) > self.max_angular_speed:
ratio = self.max_angular_speed / abs(desired.thetaVel)
desired.xVel *= ratio
desired.thetaVel *= ratio
# TBD: Adjust velocities if linear or angular acceleration
# too high.
# Adjust velocities if too low, so robot does not stall.
if abs(desired.xVel) > 0 and abs(desired.xVel) < self.min_linear_speed:
ratio = self.min_linear_speed / abs(desired.xVel)
desired.xVel *= ratio
desired.thetaVel *= ratio
elif desired.xVel == 0 and abs(
desired.thetaVel) < self.min_angular_speed:
ratio = self.min_angular_speed / abs(desired.thetaVel)
desired.xVel *= ratio
desired.thetaVel *= ratio
return desired
