def test_updateState_PosOffsetPosArc(self):
'''
Test the robot following a path starting from the same angle
but with a positive offset and larger radius
'''
pathSeg = PathSegmentMsg()
pathSeg.seg_type = pathSeg.ARC
pathSeg.seg_number = 1
pathSeg.seg_length = math.pi / 2.0
pathSeg.ref_point.x = 0.0
pathSeg.ref_point.y = 0.0
init_quat = quaternion_from_euler(0, 0, math.pi + math.pi / 2)
pathSeg.init_tan_angle.w = init_quat[3]
pathSeg.init_tan_angle.x = init_quat[0]
pathSeg.init_tan_angle.y = init_quat[1]
pathSeg.init_tan_angle.z = init_quat[2]
pathSeg.curvature = 1.0
maxSpeed = TwistMsg()
maxSpeed.linear.x = 1.0
maxSpeed.angular.z = 1.0
pathSeg.max_speeds = maxSpeed
minSpeed = TwistMsg()
pathSeg.min_speeds = minSpeed
pathSeg.accel_limit = 1.0
pathSeg.decel_limit = -1.0
state = State(pathSeg)
vel_cmd = TwistMsg()
vel_cmd.linear.x = 0.5
vel_cmd.angular.z = 0.0
point = PointMsg()
maxIter = 1000
count = 1
rhoDes = pathSeg.curvature - .3
angle = State.getYaw(pathSeg.init_tan_angle)
r = 1 / abs(rhoDes)
startAngle = angle - math.pi / 2.0
# extrapolate next point
while (state.segDistDone < 1.0 and maxIter > count):
# create where the robot should have moved
dAng = pathSeg.seg_length * (count / (maxIter / 2.0)) * rhoDes
arcAng = startAngle + dAng
point.x = pathSeg.ref_point.x + r * math.cos(arcAng)
point.y = pathSeg.ref_point.y + r * math.sin(arcAng)
state.updateState(vel_cmd, point, 0.0)
count += 1
self.assertTrue(count < maxIter)
self.assertTrue(state.segDistDone >= 1.0)
def test_stop(self):
'''
Test the stop method
'''
pathSeg = PathSegmentMsg()
pathSeg.seg_type = pathSeg.LINE
pathSeg.seg_number = 1
pathSeg.seg_length = math.sqrt(2)*2
pathSeg.ref_point.x = 0.0
pathSeg.ref_point.y = 0.0
pathSeg.ref_point.z = 0.0
init_quat = quaternion_from_euler(0,0,math.pi/4.0)
pathSeg.init_tan_angle.w = init_quat[3]
pathSeg.init_tan_angle.x = init_quat[0]
pathSeg.init_tan_angle.y = init_quat[1]
pathSeg.init_tan_angle.z = init_quat[2]
pathSeg.curvature = 0.0
maxSpeed = TwistMsg()
maxSpeed.linear.x = 1.0
maxSpeed.angular.z = 1.0
pathSeg.max_speeds = maxSpeed
minSpeed = TwistMsg()
pathSeg.min_speeds = minSpeed
pathSeg.accel_limit = 1.0
pathSeg.decel_limit = -1.0
state = State(pathSeg)
vel_cmd = TwistMsg()
vel_cmd.linear.x = 0.5
vel_cmd.angular.z = 1.5
point = PointMsg()
maxIter = 300
count = 1
# extrapolate next point
while(state.segDistDone < 1.0 and maxIter > count):
# create where the robot should have moved
point.x = pathSeg.seg_length*(count/(maxIter/2.0))*math.cos(math.pi/4.0)
point.y = pathSeg.seg_length*(count/(maxIter/2.0))*math.sin(math.pi/4.0)
state.updateState(vel_cmd, point, 0.0)
count += 1
self.assertTrue(count < maxIter)
self.assertTrue(state.segDistDone >= 1.0)
self.assertEquals(state.v, 0.5)
self.assertEquals(state.o, 1.5)
state.stop()
self.assertEquals(state.v, 0.0)
self.assertEquals(state.o, 0.0)
def test_updateState_NegOffsetNegArc(self):
'''
Test the robot following a path starting from the same angle
but with a negative offset and a smaller radius
'''
pathSeg = PathSegmentMsg()
pathSeg.seg_type = pathSeg.ARC
pathSeg.seg_number = 1
pathSeg.seg_length = math.pi/2.0
pathSeg.ref_point.x = 0.0
pathSeg.ref_point.y = 0.0
init_quat = quaternion_from_euler(0,0,3*math.pi/4.0-math.pi/2)
pathSeg.init_tan_angle.w = init_quat[3]
pathSeg.init_tan_angle.x = init_quat[0]
pathSeg.init_tan_angle.y = init_quat[1]
pathSeg.init_tan_angle.z = init_quat[2]
pathSeg.curvature = -1.0
maxSpeed = TwistMsg()
maxSpeed.linear.x = 1.0
maxSpeed.angular.z = 1.0
pathSeg.max_speeds = maxSpeed
minSpeed = TwistMsg()
pathSeg.min_speeds = minSpeed
pathSeg.accel_limit = 1.0
pathSeg.decel_limit = -1.0
state = State(pathSeg)
vel_cmd = TwistMsg()
vel_cmd.linear.x = 0.5
vel_cmd.angular.z = 0.0
point = PointMsg()
maxIter = 1000
count = 1
rhoDes = pathSeg.curvature + .3
angle = State.getYaw(pathSeg.init_tan_angle)
r=1/abs(rhoDes)
startAngle = angle + math.pi/2.0
# extrapolate next point
while(state.segDistDone < 1.0 and maxIter > count):
# create where the robot should have moved
dAng = pathSeg.seg_length*(count/(maxIter/2.0))*rhoDes
arcAng = startAngle+dAng
point.x = pathSeg.ref_point.x + r*math.cos(arcAng)
point.y = pathSeg.ref_point.y + r*math.sin(arcAng)
state.updateState(vel_cmd, point, 0.0)
count += 1
self.assertTrue(count < maxIter)
self.assertTrue(state.segDistDone >= 1.0)
def test_updateState_NegOffsetNegSpin(self):
'''
Test the robot following a path starting from an angle with
a positive offset
'''
pathSeg = PathSegmentMsg()
pathSeg.seg_type = pathSeg.SPIN_IN_PLACE
pathSeg.seg_number = 1
pathSeg.seg_length = math.pi
pathSeg.ref_point.x = 0.0
pathSeg.ref_point.y = 0.0
pathSeg.ref_point.z = 0.0
init_quat = quaternion_from_euler(0, 0, 0.0)
pathSeg.init_tan_angle.w = init_quat[3]
pathSeg.init_tan_angle.x = init_quat[0]
pathSeg.init_tan_angle.y = init_quat[1]
pathSeg.init_tan_angle.z = init_quat[2]
pathSeg.curvature = 1.0
ref_point = PointMsg()
ref_point.x = 1.0
ref_point.y = 1.0
pathSeg.ref_point = ref_point
maxSpeed = TwistMsg()
maxSpeed.linear.x = 1.0
maxSpeed.angular.z = 1.0
pathSeg.max_speeds = maxSpeed
minSpeed = TwistMsg()
pathSeg.min_speeds = minSpeed
pathSeg.accel_limit = 1.0
pathSeg.decel_limit = -1.0
state = State(pathSeg)
vel_cmd = TwistMsg()
vel_cmd.linear.x = 0.0
vel_cmd.angular.z = 0.5
point = PointMsg()
psi = 0.0
maxIter = 1000
count = 1
# extrapolate next point
while (state.segDistDone < 1.0 and maxIter > count):
# create where the robot should have moved
psi = 2.0 * state.pathSeg.seg_length * count / maxIter + math.pi / 4.0
state.updateState(vel_cmd, point, psi)
count += 1
self.assertTrue(count < maxIter)
self.assertTrue(state.segDistDone >= 1.0)
def test_updateState_NegOffsetNegSpin(self):
'''
Test the robot following a path starting from an angle with
a positive offset
'''
pathSeg = PathSegmentMsg()
pathSeg.seg_type = pathSeg.SPIN_IN_PLACE
pathSeg.seg_number = 1
pathSeg.seg_length = math.pi
pathSeg.ref_point.x = 0.0
pathSeg.ref_point.y = 0.0
pathSeg.ref_point.z = 0.0
init_quat = quaternion_from_euler(0,0,0.0)
pathSeg.init_tan_angle.w = init_quat[3]
pathSeg.init_tan_angle.x = init_quat[0]
pathSeg.init_tan_angle.y = init_quat[1]
pathSeg.init_tan_angle.z = init_quat[2]
pathSeg.curvature = 1.0
ref_point = PointMsg()
ref_point.x = 1.0
ref_point.y = 1.0
pathSeg.ref_point = ref_point
maxSpeed = TwistMsg()
maxSpeed.linear.x = 1.0
maxSpeed.angular.z = 1.0
pathSeg.max_speeds = maxSpeed
minSpeed = TwistMsg()
pathSeg.min_speeds = minSpeed
pathSeg.accel_limit = 1.0
pathSeg.decel_limit = -1.0
state = State(pathSeg)
vel_cmd = TwistMsg()
vel_cmd.linear.x = 0.0
vel_cmd.angular.z = 0.5
point = PointMsg()
psi = 0.0
maxIter = 1000
count = 1
# extrapolate next point
while(state.segDistDone < 1.0 and maxIter > count):
# create where the robot should have moved
psi = 2.0*state.pathSeg.seg_length*count/maxIter + math.pi/4.0
state.updateState(vel_cmd, point, psi)
count += 1
self.assertTrue(count < maxIter)
self.assertTrue(state.segDistDone >= 1.0)
def test_updateState_NegOffsetLine(self):
'''
Test the robot following a path starting with a negative
offset and crossing over the path
'''
pathSeg = PathSegmentMsg()
pathSeg.seg_type = pathSeg.LINE
pathSeg.seg_number = 1
pathSeg.seg_length = math.sqrt(2) * 2
pathSeg.ref_point.x = 0.0
pathSeg.ref_point.y = 0.0
pathSeg.ref_point.z = 0.0
init_quat = quaternion_from_euler(0, 0, math.pi / 4.0)
pathSeg.init_tan_angle.w = init_quat[3]
pathSeg.init_tan_angle.x = init_quat[0]
pathSeg.init_tan_angle.y = init_quat[1]
pathSeg.init_tan_angle.z = init_quat[2]
pathSeg.curvature = 0.0
maxSpeed = TwistMsg()
maxSpeed.linear.x = 1.0
maxSpeed.angular.z = 1.0
pathSeg.max_speeds = maxSpeed
minSpeed = TwistMsg()
pathSeg.min_speeds = minSpeed
pathSeg.accel_limit = 1.0
pathSeg.decel_limit = -1.0
state = State(pathSeg)
vel_cmd = TwistMsg()
vel_cmd.linear.x = 0.5
vel_cmd.angular.z = 0.0
point = PointMsg()
actSegLength = 2.0 * math.sqrt(2) + 1.0
maxIter = 1000
count = 1
# extrapolate next point
while (state.segDistDone < 1.0 and maxIter > count):
# create where the robot should have moved
point.x = actSegLength * (count / (maxIter / 2.0)) * math.cos(
3 * math.pi / 4.0) + 1.0
point.y = actSegLength * (count / (maxIter / 2.0)) * math.sin(
3 * math.pi / 4.0)
state.updateState(vel_cmd, point, 0.0)
count += 1
self.assertTrue(count < maxIter)
self.assertTrue(state.segDistDone >= 1.0)
def test_updateState_NegOffsetLine(self):
'''
Test the robot following a path starting with a negative
offset and crossing over the path
'''
pathSeg = PathSegmentMsg()
pathSeg.seg_type = pathSeg.LINE
pathSeg.seg_number = 1
pathSeg.seg_length = math.sqrt(2)*2
pathSeg.ref_point.x = 0.0
pathSeg.ref_point.y = 0.0
pathSeg.ref_point.z = 0.0
init_quat = quaternion_from_euler(0,0,math.pi/4.0)
pathSeg.init_tan_angle.w = init_quat[3]
pathSeg.init_tan_angle.x = init_quat[0]
pathSeg.init_tan_angle.y = init_quat[1]
pathSeg.init_tan_angle.z = init_quat[2]
pathSeg.curvature = 0.0
maxSpeed = TwistMsg()
maxSpeed.linear.x = 1.0
maxSpeed.angular.z = 1.0
pathSeg.max_speeds = maxSpeed
minSpeed = TwistMsg()
pathSeg.min_speeds = minSpeed
pathSeg.accel_limit = 1.0
pathSeg.decel_limit = -1.0
state = State(pathSeg)
vel_cmd = TwistMsg()
vel_cmd.linear.x = 0.5
vel_cmd.angular.z = 0.0
point = PointMsg()
actSegLength = 2.0*math.sqrt(2)+1.0
maxIter = 1000
count = 1
# extrapolate next point
while(state.segDistDone < 1.0 and maxIter > count):
# create where the robot should have moved
point.x = actSegLength*(count/(maxIter/2.0))*math.cos(3*math.pi/4.0) + 1.0
point.y = actSegLength*(count/(maxIter/2.0))*math.sin(3*math.pi/4.0)
state.updateState(vel_cmd, point, 0.0)
count += 1
self.assertTrue(count < maxIter)
self.assertTrue(state.segDistDone >= 1.0)
def test_stop(self):
'''
Test the stop method
'''
pathSeg = PathSegmentMsg()
pathSeg.seg_type = pathSeg.LINE
pathSeg.seg_number = 1
pathSeg.seg_length = math.sqrt(2) * 2
pathSeg.ref_point.x = 0.0
pathSeg.ref_point.y = 0.0
pathSeg.ref_point.z = 0.0
init_quat = quaternion_from_euler(0, 0, math.pi / 4.0)
pathSeg.init_tan_angle.w = init_quat[3]
pathSeg.init_tan_angle.x = init_quat[0]
pathSeg.init_tan_angle.y = init_quat[1]
pathSeg.init_tan_angle.z = init_quat[2]
pathSeg.curvature = 0.0
maxSpeed = TwistMsg()
maxSpeed.linear.x = 1.0
maxSpeed.angular.z = 1.0
pathSeg.max_speeds = maxSpeed
minSpeed = TwistMsg()
pathSeg.min_speeds = minSpeed
pathSeg.accel_limit = 1.0
pathSeg.decel_limit = -1.0
state = State(pathSeg)
vel_cmd = TwistMsg()
vel_cmd.linear.x = 0.5
vel_cmd.angular.z = 1.5
point = PointMsg()
maxIter = 300
count = 1
# extrapolate next point
while (state.segDistDone < 1.0 and maxIter > count):
# create where the robot should have moved
point.x = pathSeg.seg_length * (count /
(maxIter / 2.0)) * math.cos(
math.pi / 4.0)
point.y = pathSeg.seg_length * (count /
(maxIter / 2.0)) * math.sin(
math.pi / 4.0)
state.updateState(vel_cmd, point, 0.0)
count += 1
self.assertTrue(count < maxIter)
self.assertTrue(state.segDistDone >= 1.0)
self.assertEquals(state.v, 0.5)
self.assertEquals(state.o, 1.5)
state.stop()
self.assertEquals(state.v, 0.0)
self.assertEquals(state.o, 0.0)