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 main():
global segNumber
global segAbort
global pathList
global naptime
global pathSegPub
rospy.init_node('path_planner_alpha_main')
pathSegPub = rospy.Publisher('path', PathListMsg)
rospy.Subscriber('seg_status', SegStatusMsg, segStatusCallback)
rospy.Subscriber('point_list', PointListMsg, pointListCallback)
naptime = rospy.Rate(RATE)
print "Entering main loop"
while not rospy.is_shutdown():
# clear everything when something gets in the way
# of the planned path
if(segAbort):
segNumber = 0
pathList.segments = []
print "len(desPoints) %i" % len(desPoints)
print "len(pathList.segments) %i" % len(pathList.segments)
if(len(desPoints)-1 > len(pathList.segments)):
pathSeg = PathSegmentMsg()
num = len(pathList.segments)
yaw = m.atan2((desPoints[num+1].y-desPoints[num].y),(desPoints[num+1].x-desPoints[num].x))
pathSeg.init_tan_angle = yawToQuat(yaw)
pathSeg.ref_point = desPoints[num]
#if(theta > m.pi/6):
#SPIN TO NEW ANGLES
# addSegToList()
#Calculate the length of the given segment
#Add angle
xDist = m.pow((desPoints[num].x - desPoints[num+1].x),2)
yDist = m.pow((desPoints[num].y - desPoints[num+1].y),2)
pathSeg.seg_length = m.sqrt(xDist + yDist)
addSegToList(pathSeg)
pathSegPub.publish(pathList)
naptime.sleep()
def main():
global segNumber
global segAbort
global pathList
global naptime
global pathSegPub
rospy.init_node('path_planner_alpha_main')
pathSegPub = rospy.Publisher('path', PathListMsg)
rospy.Subscriber('seg_status', SegStatusMsg, segStatusCallback)
rospy.Subscriber('point_list', PointListMsg, pointListCallback)
naptime = rospy.Rate(RATE)
print "Entering main loop"
while not rospy.is_shutdown():
# clear everything when something gets in the way
# of the planned path
if (segAbort):
segNumber = 0
pathList.segments = []
print "len(desPoints) %i" % len(desPoints)
print "len(pathList.segments) %i" % len(pathList.segments)
if (len(desPoints) - 1 > len(pathList.segments)):
pathSeg = PathSegmentMsg()
num = len(pathList.segments)
yaw = m.atan2((desPoints[num + 1].y - desPoints[num].y),
(desPoints[num + 1].x - desPoints[num].x))
pathSeg.init_tan_angle = yawToQuat(yaw)
pathSeg.ref_point = desPoints[num]
#if(theta > m.pi/6):
#SPIN TO NEW ANGLES
# addSegToList()
#Calculate the length of the given segment
#Add angle
xDist = m.pow((desPoints[num].x - desPoints[num + 1].x), 2)
yDist = m.pow((desPoints[num].y - desPoints[num + 1].y), 2)
pathSeg.seg_length = m.sqrt(xDist + yDist)
addSegToList(pathSeg)
pathSegPub.publish(pathList)
naptime.sleep()