コード例 #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)
コード例 #2
0
ファイル: stateTests.py プロジェクト: Aand1/EECS-376-Alpha
    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)
コード例 #3
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)
コード例 #4
0
ファイル: stateTests.py プロジェクト: Aand1/EECS-376-Alpha
    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)
コード例 #5
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)
コード例 #6
0
ファイル: stateTests.py プロジェクト: Aand1/EECS-376-Alpha
    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)
コード例 #7
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)
コード例 #8
0
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()
コード例 #9
0
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()
コード例 #10
0
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()
コード例 #11
0
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()
コード例 #12
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()
コード例 #13
0
ファイル: stateTests.py プロジェクト: Aand1/EECS-376-Alpha
    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)
コード例 #14
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()