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()
