Python State.updateState方法代码示例

# 需要导入模块: from state import State [as 别名]
# 或者: from state.State import updateState [as 别名]
    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)

# 需要导入模块: from state import State [as 别名]
# 或者: from state.State import updateState [as 别名]
    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)

# 需要导入模块: from state import State [as 别名]
# 或者: from state.State import updateState [as 别名]
    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)

# 需要导入模块: from state import State [as 别名]
# 或者: from state.State import updateState [as 别名]
    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)

# 需要导入模块: from state import State [as 别名]
# 或者: from state.State import updateState [as 别名]
def main():
    global naptime
    global currSeg

    rospy.init_node('velocity_profiler_alpha')
    naptime = rospy.Rate(RATE) # this will be used globally by all functions that need to loop
    desVelPub = rospy.Publisher('des_vel',TwistMsg) # Steering reads this and adds steering corrections on top of the desired velocities
    segStatusPub = rospy.Publisher('seg_status', SegStatusMsg) # Lets the other nodes know what path segment the robot is currently executing
    rospy.Subscriber("motors_enabled", BoolMsg, eStopCallback) # Lets velocity profiler know the E-stop is enabled
    rospy.Subscriber("obstacles", ObstaclesMsg, obstaclesCallback) # Lets velocity profiler know where along the path there is an obstacle 
    rospy.Subscriber("cmd_vel", TwistMsg, velCmdCallback) # 
    rospy.Subscriber("path", PathListMsg, pathListCallback)
    rospy.Subscriber("map_pos", PoseStampedMsg, poseCallback)

    abortTime = None # will be set to the time an obstacle is detected

    if rospy.has_param('waitTime'):
        waitTime = rospy.Duration(rospy.get_param('waitTime'))
    else:
        waitTime = rospy.Duration(3.0)
    
    print "Velocity Profiler entering main loop"
    
    currSeg = State(dt=1/RATE)
    while not rospy.is_shutdown():
        # check where the robot is
        last_vel = TwistMsg()
        last_vel.linear.x = lastVCmd
        last_vel.angular.z = lastWCmd
        if(currSeg.pathSeg is not None):
            currSeg.updateState(last_vel,position,State.getYaw(orientation))

        # check if there are segments to execute
        if(len(vTrajectory) != 0 or len(wTrajectory) != 0): # Note: Either both or neither should have 0 elements
            # check for obstacles
            if(obsWithinPathSeg()):
                # set the timer if necessary
                if(abortTime is None):
                    abortTime = rospy.Time.now()
                else:
                    if(rospy.Time.now() - abortTime > waitTime):
                        # time to abort
                        abortTime = None # reset the time
                        
                        # this method will reset anything
                        # that needs to be reset
                        # this may need to block here until the
                        # path list changes
                        abortPath()

                        # make sure the robot is stopped
                        desVelPub.publish(TwistMsg())
                        
                        # publish the abort flag
                        publishSegStatus(segStatusPub,abort=False)
                        naptime.sleep()
                        continue
       
                des_vel = stopForObs()
            else:
                abortTime = None # make sure that the abortTime gets reset
                des_vel = getDesiredVelocity(vTrajectory[0],wTrajectory[0])
        else:
            des_vel = TwistMsg() # initialized to 0's by default
        desVelPub.publish(des_vel) # publish either the scheduled commands or 0's
        update() # remove completed segments and change currSeg's path segment when necessary
        publishSegStatus(segStatusPub)
        naptime.sleep()