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 testRotateRight(self):
cur = Pose()
goal = Pose()
goal.theta = -pi / 2
desired = self.controller.getVelocity(cur, goal, 0.1)
self.assertEquals(desired.xVel, 0)
self.assertLess(desired.thetaVel, 0)
def testStraightAhead(self):
cur = Pose()
goal = Pose()
goal.x = 1
desired = self.controller.getVelocity(cur, goal, 0.1)
self.assertGreater(desired.xVel, 0)
self.assertEquals(desired.thetaVel, 0)
def testStraightBack(self):
cur = Pose()
goal = Pose()
goal.x = -1
desired = self.controller.getVelocity(cur, goal, 0.1)
self.assertLess(desired.xVel, 0)
self.assertEquals(desired.thetaVel, 0)
def checkGoToGoal(self, x0, y0, th0, x1, y1, th1):
dTol = 0.05 # 5cm
thTol = 0.04 # Approx 2.5 degrees
self.controller.setLinearTolerance(dTol)
self.controller.setAngularTolerance(thTol)
cur = Pose()
cur.x = x0
cur.y = y0
cur.theta = th0
goal = Pose()
goal.x = x1
goal.y = y1
goal.theta = th1
lastDistance = self.controller.getGoalDistance(cur, goal)
dT = 0.05
for i in range(1000):
if self.controller.atGoal(cur, goal):
return
desired = self.controller.getVelocity(cur, goal, dT)
cur.x += dT * desired.xVel * cos(cur.theta)
cur.y += dT * desired.xVel * sin(cur.theta)
cur.theta += dT * desired.thetaVel
# If we get here, we didn't reach the goal.
self.assertFalse('Did not reach the goal.')
def testButtonHookRight(self):
cur = Pose()
goal = Pose()
goal.x = 1
goal.theta = -pi
desired = self.controller.getVelocity(cur, goal, 0.1)
self.assertGreater(desired.xVel, 0)
self.assertGreater(desired.thetaVel, 0)
def on_initial_pose(self, msg): #(self, msg):
q = [
msg.pose.pose.orientation.x, msg.pose.pose.orientation.x,
msg.pose.pose.orientation.x, msg.pose.pose.orientation.w
]
roll, pitch, yaw = euler_from_quaternion(q)
pose = Pose()
pose.x = msg.pose.pose.position.x
pose.y = msg.pose.pose.position.y
pose.theta = yaw
rospy.loginfo('Setting initial pose to ')
self.odometry.setPose(pose)
def testCurveBackLeft(self):
cur = Pose()
goal = Pose()
cur.x = 1
cur.y = 1
goal.theta = pi / 2
desired = self.controller.getVelocity(cur, goal, 0.1)
self.assertLess(desired.xVel, 0)
self.assertGreater(desired.thetaVel, 0)
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 __init__(self):
self.leftEncoder = Encoder()
self.rightEncoder = Encoder()
self.pose = Pose()
self.lastTime = 0
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
def testAtGoal(self):
cur = Pose()
desired = self.controller.getVelocity(cur, cur, 0.1)
self.assertEquals(desired.xVel, 0)
self.assertEquals(desired.thetaVel, 0)
def __init__(self, deltaTime=0.1):
self.leftEncoder = Encoder()
self.rightEncoder = Encoder()
self.pose = Pose()
self.lastTime = 0
self.deltaTime = deltaTime
