def get_velocity(self, pose, vel):
self.state.update(pose.x,
pose.y,
pose.theta,
vel,
is_backward=self.direction)
desired = Pose()
if self.last_idx > self.target_idx:
self.is_at_goal = False
# Calc control input
ai = self.pid_control(self.max_linear_speed, self.state.v)
di, self.target_idx = self.pure_pursuit_steer_control(
self.state, self.target_course, self.target_idx)
desired.xVel = self.state.v + ai
desired.thetaVel = abs(self.state.v) / self.wheel_base * np.tan(di)
self.test_theta_vel = abs(
self.state.v) / self.wheel_base * np.tan(di)
else:
if (self.ignore_angular_tolerance):
ai = self.pid_control(self.max_linear_speed, self.state.v)
desired.xVel = self.state.v + ai
self.is_at_goal = True
return desired
def getVelocity(self, cur, goal, dT):
desired = Pose()
d = self.getGoalDistance(cur, goal)
a = atan2(goal.y - cur.y, goal.x - cur.x) - cur.theta
b = cur.theta + a - goal.theta
if abs(d) < self.linearTolerance:
desired.xVel = 0
desired.thetaVel = -self.kB * b
else:
desired.xVel = self.kP * d
desired.thetaVel = self.kA * a + self.kB * b
# Adjust velocities if linear or angular rates or accel too high.
# TBD
return desired
def getVelocity(self, cur, goal, dT):
desired = Pose()
a = -cur.theta + atan2(goal.y - cur.y, goal.x - cur.x)
# 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 = cur.theta - goal.theta
b = -theta - a
d = self.getGoalDistance(cur, goal)
if self.forwardMovementOnly:
direction = 1
a = self.normalizePi(a)
b = self.normalizePi(b)
#if abs(a) > pi/2:
# b = 0
else:
direction = self.sign(cos(a))
a = self.normalizeHalfPi(a)
b = self.normalizeHalfPi(b)
if abs(d) < self.linearTolerance:
desired.xVel = 0
desired.thetaVel = self.kB * theta
else:
desired.xVel = self.kP * d * direction
desired.thetaVel = self.kA * a + self.kB * b
#print('current x:', cur.x, 'y:', cur.y, 'theta:', cur.theta,
# ' goal x:', goal.x, 'y:', goal.y, 'theta:', goal.theta)
#print(' theta:', theta, 'a:', a, 'b:', b,
# 'v:', desired.xVel, 'w:', desired.thetaVel)
#print(str(cur.x) + ',' + str(cur.y) + ',' + str(cur.theta))
# TBD: Adjust velocities if linear or angular rates
# or acceleration too high.
return desired
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 self.within_linear_tolerance:
desired.xVel = 0
desired.thetaVel = self.kB * theta
else:
desired.xVel = self.kP * d * direction
desired.thetaVel = self.kA * a + self.kB * b
# 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
if 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
#print("Theta vel:", str(desired.thetaVel))
#print("Min theta:", str(self.min_angular_speed))
#print("Linear vel:", str(desired.xVel))
#print("Ratio:", str(ratio))
return desired
