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 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()
def shortLines():
global naptime
rospy.init_node('path_list_test')
naptime = rospy.Rate(20.0)
pathListPub = rospy.Publisher('path', PathListMsg)
rospy.Subscriber("seg_status", SegStatusMsg, segStatusCallback)
print "Entering main loop"
count = 1
path = list()
pathList = PathListMsg()
pathSegments = dict()
lengths = [.2, 1, .3, .5, .6]
max_speeds = [1, .1, .25, .5, 3]
min_speeds = [0, 0, 0, 0, 0]
accel = [.25, .5, .1, .25, 1]
decel = [-.25, -.2, -.2, -.2, -1]
startx = 8.42
starty = 15.09
startYaw = pi / 180.0 * -132.39
while not rospy.is_shutdown():
if (count <= len(lengths)):
newSeg = PathSegmentMsg()
newSeg.seg_type = PathSegmentMsg.LINE
newSeg.seg_number = count
newSeg.seg_length = lengths[count - 1]
newSeg.ref_point.x = startx
newSeg.ref_point.y = starty
if (count > 1):
newSeg.ref_point.x += sum(lengths[:count - 2]) * cos(startYaw)
newSeg.ref_point.y += sum(lengths[:count - 2]) * sin(startYaw)
newSeg.init_tan_angle = createQuat(0, 0, startYaw)
newSeg.max_speeds.linear.x = max_speeds[count - 1]
newSeg.min_speeds.linear.x = min_speeds[count - 1]
newSeg.accel_limit = accel[count - 1]
newSeg.decel_limit = decel[count - 1]
path.append(newSeg)
pathSegments[count] = len(path) - 1
pathList.segments = path
print "Appending segment number %i" % count
for i, seg in enumerate(path):
if seg.seg_number <= lastSegNumber:
"Deleting segment number %i" % seg.seg_number
del path[i]
pathList.segments = path
pathListPub.publish(pathList)
count += 1
naptime.sleep()
def shortLines():
global naptime
rospy.init_node('path_list_test')
naptime = rospy.Rate(20.0)
pathListPub = rospy.Publisher('path',PathListMsg)
rospy.Subscriber("seg_status",SegStatusMsg,segStatusCallback)
print "Entering main loop"
count = 1
path = list()
pathList = PathListMsg()
pathSegments = dict()
lengths = [.2,1,.3,.5,.6]
max_speeds = [1,.1,.25,.5,3]
min_speeds = [0,0,0,0,0]
accel = [.25,.5,.1,.25,1]
decel = [-.25,-.2,-.2,-.2,-1]
startx = 8.42
starty = 15.09
startYaw = pi/180.0*-132.39
while not rospy.is_shutdown():
if(count <= len(lengths)):
newSeg = PathSegmentMsg()
newSeg.seg_type = PathSegmentMsg.LINE
newSeg.seg_number = count
newSeg.seg_length = lengths[count-1]
newSeg.ref_point.x = startx
newSeg.ref_point.y = starty
if(count > 1):
newSeg.ref_point.x += sum(lengths[:count-2])*cos(startYaw)
newSeg.ref_point.y += sum(lengths[:count-2])*sin(startYaw)
newSeg.init_tan_angle = createQuat(0,0,startYaw)
newSeg.max_speeds.linear.x = max_speeds[count-1]
newSeg.min_speeds.linear.x = min_speeds[count-1]
newSeg.accel_limit = accel[count-1]
newSeg.decel_limit = decel[count-1]
path.append(newSeg)
pathSegments[count] = len(path)-1
pathList.segments = path
print "Appending segment number %i" % count
for i,seg in enumerate(path):
if seg.seg_number <= lastSegNumber:
"Deleting segment number %i" % seg.seg_number
del path[i]
pathList.segments = path
pathListPub.publish(pathList)
count += 1
naptime.sleep()
def randomLines():
global naptime
rospy.init_node('path_list_test')
naptime = rospy.Rate(RATE)
pathListPub = rospy.Publisher('path', PathListMsg)
print "Entering main loop"
count = 0
path = list()
pathList = PathListMsg()
while not rospy.is_shutdown():
count += 1
if (count % 5 == 0): # on multiples of 5 add a new path segment
print "Appending a new segment"
newSeg = PathSegmentMsg()
newSeg.seg_number = count
newSeg.seg_length = random.uniform(0, 10)
newSeg.seg_type = 1 #(count % 3) + 1
newSeg.max_speeds.linear.x = random.uniform(0, 10)
newSeg.max_speeds.angular.z = random.uniform(0, 10)
newSeg.accel_limit = random.uniform(0, 10)
newSeg.decel_limit = random.uniform(-10, 0)
path.append(newSeg)
# if(count % 8 == 0): # on multiples of 8 remove a segment from the list
# print "Deleting a segment"
# if(len(path) != 0):
# del path[count % len(path)]
# if(count % 23 == 0): # on multiples of 23 remove all segments from the list
# print "Deleting the list"
# path = list()
pathList.segments = path
pathListPub.publish(pathList)
naptime.sleep()
def randomLines():
global naptime
rospy.init_node('path_list_test')
naptime = rospy.Rate(RATE)
pathListPub = rospy.Publisher('path',PathListMsg)
print "Entering main loop"
count = 0
path = list()
pathList = PathListMsg()
while not rospy.is_shutdown():
count += 1
if(count % 5 == 0): # on multiples of 5 add a new path segment
print "Appending a new segment"
newSeg = PathSegmentMsg()
newSeg.seg_number = count
newSeg.seg_length = random.uniform(0,10)
newSeg.seg_type = 1 #(count % 3) + 1
newSeg.max_speeds.linear.x = random.uniform(0,10)
newSeg.max_speeds.angular.z = random.uniform(0,10)
newSeg.accel_limit = random.uniform(0,10)
newSeg.decel_limit = random.uniform(-10,0)
path.append(newSeg)
# if(count % 8 == 0): # on multiples of 8 remove a segment from the list
# print "Deleting a segment"
# if(len(path) != 0):
# del path[count % len(path)]
# if(count % 23 == 0): # on multiples of 23 remove all segments from the list
# print "Deleting the list"
# path = list()
pathList.segments = path
pathListPub.publish(pathList)
naptime.sleep()
def publishFromFile(fullPath):
"""
Columns for file in order should be
seg_type (1,2,3)
seg_length (float)
ref_point.x (float)
ref_point.y (float)
init_tan_angle (float)
curvature (float)
max_v (float)
max_w (float)
min_v (float)
min_w (float)
accel_limit (float)
decel_limit (float)
"""
with open(fullPath, 'rb') as csvFile:
rospy.init_node('CustomPathPublisher')
pathListPublisher = rospy.Publisher('path', PathListMsg)
rospy.Subscriber('seg_status', SegStatusMsg, segStatusCallback)
dialect = csv.Sniffer().sniff(
csvFile.read(1024)) # auto detect delimiters
csvFile.seek(0)
reader = csv.reader(
csvFile, dialect) # open up a csv reader object with the csv file
segs = []
segs.append(PathSegmentMsg())
headers = next(reader)
for i, row in enumerate(reader):
print row
pathSeg = PathSegmentMsg()
pathSeg.seg_number = i + 1
try:
seg_type = int(row[0])
except ValueError:
print "Problem reading %s" % row[0]
print "\tMake sure the 1st column is a number"
print "\tDefaulting seg_type to 1"
seg_type = 1
if (seg_type == 1):
pathSeg.seg_type = PathSegmentMsg.LINE
elif (seg_type == 2):
pathSeg.seg_type = PathSegmentMsg.ARC
elif (seg_type == 3):
pathSeg.seg_type = PathSegmentMsg.SPIN_IN_PLACE
else:
print "Unknown segment type %s" % seg_type
pathSeg.seg_type = 4 # this could be useful when testing incorrect input
try:
seg_length = float(row[1])
except ValueError:
print "Problem reading %s" % row[1]
print "\tMake sure the 2nd column is a number"
print "\tDefaulting to length 1.0"
seg_length = 1.0
pathSeg.seg_length = seg_length
try:
x = float(row[2])
except ValueError:
print "Problem reading %s" % row[2]
print "\tMake sure the 3rd column is a number"
print "\tDefaulting x to 0.0"
x = 0.0
pathSeg.ref_point.x = x
try:
y = float(row[3])
except ValueError:
print "Problem reading %s" % row[3]
print "\tMake sure the 4th column is a number"
print "\tDefaulting y to 0.0"
y = 0.0
pathSeg.ref_point.y = y
try:
tan_angle = float(row[4])
except ValueError:
print "Problem reading %s" % row[4]
print "\tMake sure the 5th column is a number"
print "\tDefaulting tan_angle to 0.0"
tan_angle = 0.0
init_quat = quaternion_from_euler(0, 0, tan_angle)
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]
try:
curvature = float(row[5])
except ValueError:
print "Problem reading %s" % row[5]
print "\tMake sure the 6th column is a number"
print "\tDefaulting curvature to 0.0"
curvature = 0.0
pathSeg.curvature = curvature
try:
max_v = float(row[6])
except ValueError:
print "Problem reading %s" % row[6]
print "\tMake sure the 7th column is a number"
print "\tDefaulting max_v to 1.0"
max_v = 1.0
pathSeg.max_speeds.linear.x = max_v
try:
max_w = float(row[7])
except ValueError:
print "Problem reading %s" % row[7]
print "\tMake sure the 8th column is a number"
print "\tDefaulting max_w to 1.0"
max_w = 1.0
pathSeg.max_speeds.angular.z = max_w
try:
min_v = float(row[8])
except ValueError:
print "Problem reading %s" % row[8]
print "\tMake sure the 9th column is a number"
print "\tDefaulting min_v to 0.0"
min_v = 0.0
pathSeg.min_speeds.linear.x = min_v
try:
min_w = float(row[9])
except ValueError:
print "Problem reading %s" % row[9]
print "\tMake sure the 10th column is a number"
print "\tDefaulting min_w to 0.0"
min_w = 0.0
pathSeg.min_speeds.angular.z = min_w
try:
accel_limit = float(row[10])
except ValueError:
print "Problem reading %s" % row[10]
print "\tMake sure the 11th column is a number"
print "\tDefaulting accel_limit to 0.5"
accel_limit = 0.5
pathSeg.accel_limit = accel_limit
try:
decel_limit = float(row[11])
except ValueError:
print "Problem reading %s" % row[11]
print "\tMake sure the 12th column is a number"
print "\tDefaulting decel_limit to 0.5"
decel_limit = 0.5
pathSeg.decel_limit = decel_limit
segs.append(pathSeg)
print "About to publish"
pathList = PathListMsg()
naptime = rospy.Rate(RATE)
while (not rospy.is_shutdown()):
if (len(segs) == 0):
break
for i, seg in enumerate(segs):
if (seg.seg_number <= lastSegNumber):
print "Removing path segment %i" % (seg.seg_number)
del segs[i]
pathList.segments = segs
pathListPublisher.publish(pathList)
naptime.sleep()
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 publishFromFile(fullPath):
"""
Columns for file in order should be
seg_type (1,2,3)
seg_length (float)
ref_point.x (float)
ref_point.y (float)
init_tan_angle (float)
curvature (float)
max_v (float)
max_w (float)
min_v (float)
min_w (float)
accel_limit (float)
decel_limit (float)
"""
with open(fullPath,'rb') as csvFile:
rospy.init_node('CustomPathPublisher')
pathListPublisher = rospy.Publisher('path',PathListMsg)
rospy.Subscriber('seg_status',SegStatusMsg,segStatusCallback)
dialect = csv.Sniffer().sniff(csvFile.read(1024)) # auto detect delimiters
csvFile.seek(0)
reader = csv.reader(csvFile, dialect) # open up a csv reader object with the csv file
segs = []
segs.append(PathSegmentMsg())
headers = next(reader)
for i,row in enumerate(reader):
print row
pathSeg = PathSegmentMsg()
pathSeg.seg_number = i+1
try:
seg_type = int(row[0])
except ValueError:
print "Problem reading %s" % row[0]
print "\tMake sure the 1st column is a number"
print "\tDefaulting seg_type to 1"
seg_type = 1
if(seg_type == 1):
pathSeg.seg_type = PathSegmentMsg.LINE
elif(seg_type == 2):
pathSeg.seg_type = PathSegmentMsg.ARC
elif(seg_type == 3):
pathSeg.seg_type = PathSegmentMsg.SPIN_IN_PLACE
else:
print "Unknown segment type %s" % seg_type
pathSeg.seg_type = 4 # this could be useful when testing incorrect input
try:
seg_length = float(row[1])
except ValueError:
print "Problem reading %s" % row[1]
print "\tMake sure the 2nd column is a number"
print "\tDefaulting to length 1.0"
seg_length = 1.0
pathSeg.seg_length = seg_length
try:
x = float(row[2])
except ValueError:
print "Problem reading %s" % row[2]
print "\tMake sure the 3rd column is a number"
print "\tDefaulting x to 0.0"
x = 0.0
pathSeg.ref_point.x = x
try:
y = float(row[3])
except ValueError:
print "Problem reading %s" % row[3]
print "\tMake sure the 4th column is a number"
print "\tDefaulting y to 0.0"
y = 0.0
pathSeg.ref_point.y = y
try:
tan_angle = float(row[4])
except ValueError:
print "Problem reading %s" % row[4]
print "\tMake sure the 5th column is a number"
print "\tDefaulting tan_angle to 0.0"
tan_angle = 0.0
init_quat = quaternion_from_euler(0,0,tan_angle)
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]
try:
curvature = float(row[5])
except ValueError:
print "Problem reading %s" % row[5]
print "\tMake sure the 6th column is a number"
print "\tDefaulting curvature to 0.0"
curvature = 0.0
pathSeg.curvature = curvature
try:
max_v = float(row[6])
except ValueError:
print "Problem reading %s" % row[6]
print "\tMake sure the 7th column is a number"
print "\tDefaulting max_v to 1.0"
max_v = 1.0
pathSeg.max_speeds.linear.x = max_v
try:
max_w = float(row[7])
except ValueError:
print "Problem reading %s" % row[7]
print "\tMake sure the 8th column is a number"
print "\tDefaulting max_w to 1.0"
max_w = 1.0
pathSeg.max_speeds.angular.z = max_w
try:
min_v = float(row[8])
except ValueError:
print "Problem reading %s" % row[8]
print "\tMake sure the 9th column is a number"
print "\tDefaulting min_v to 0.0"
min_v = 0.0
pathSeg.min_speeds.linear.x = min_v
try:
min_w = float(row[9])
except ValueError:
print "Problem reading %s" % row[9]
print "\tMake sure the 10th column is a number"
print "\tDefaulting min_w to 0.0"
min_w = 0.0
pathSeg.min_speeds.angular.z = min_w
try:
accel_limit = float(row[10])
except ValueError:
print "Problem reading %s" % row[10]
print "\tMake sure the 11th column is a number"
print "\tDefaulting accel_limit to 0.5"
accel_limit = 0.5
pathSeg.accel_limit = accel_limit
try:
decel_limit = float(row[11])
except ValueError:
print "Problem reading %s" % row[11]
print "\tMake sure the 12th column is a number"
print "\tDefaulting decel_limit to 0.5"
decel_limit = 0.5
pathSeg.decel_limit = decel_limit
segs.append(pathSeg)
print "About to publish"
pathList = PathListMsg()
naptime = rospy.Rate(RATE)
while(not rospy.is_shutdown()):
if(len(segs) == 0):
break
for i,seg in enumerate(segs):
if(seg.seg_number <= lastSegNumber):
print "Removing path segment %i" % (seg.seg_number)
del segs[i]
pathList.segments = segs
pathListPublisher.publish(pathList)
naptime.sleep()