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