Python State.getYaw方法代码示例

# 需要导入模块: from state import State [as 别名]
# 或者: from state.State import getYaw [as 别名]
def update():
    '''
    This function is responsible for updating all the state variables each iteration of the node's main loop
    '''
    global currSeg
    global pathSegments
    global vTrajectory
    global wTrajectory
    global lastSegNumber
    
    oldVTraj = None
    oldWTraj = None

    if(len(vTrajectory) == 0 or len(wTrajectory) == 0):
        # Clear the deques because either they should both have elements or neither should
        vTrajectory.clear()
        wTrajectory.clear()
        pathSegments.clear() # clear out the path segments because they are now useless
        currSeg.newPathSegment()
        return

    # if it made it to here then there is at least one segment in vTrajectory and wTrajectory
    if(currSeg.pathSeg is None):
        currSeg.newPathSegment(pathSegments.get(vTrajectory[0].segNumber),position,State.getYaw(orientation))
        last_vel = TwistMsg()
        last_vel.linear.x = lastVCmd
        last_vel.angular.z = lastWCmd
        currSeg.updateState(last_vel,position,State.getYaw(orientation))

    
    if(currSeg.segDistDone >= vTrajectory[0].endS): # this segment is done
        oldVTraj = vTrajectory.popleft() # temporary storage, this will eventually be thrown away
        if(len(vTrajectory) == 0):
            lastSegNumber = oldVTraj.segNumber
            wTrajectory.clear()
            pathSegments.clear()
            currSeg.newPathSegment()
            return
        if(oldVTraj.segNumber != vTrajectory[0].segNumber):
            lastSegNumber = oldVTraj.segNumber
            wTrajectory.popleft() # could potentially be out of sync. This method does not account for that
            currSeg.newPathSegment(pathSegments.get(vTrajectory[0].segNumber),position,State.getYaw(orientation))
            pathSegments.pop(oldVTraj.segNumber) # remove no longer needed pathSegments

    if(currSeg.segDistDone >= wTrajectory[0].endS): # this segment is done
        oldWTraj = wTrajectory.popleft()
        if(len(wTrajectory) == 0):
            lastSegNumber = oldWTraj.segNumber
            vTrajectory.clear()
            pathSegments.clear()
            currSeg.newPathSegment()
            return
        if(oldWTraj.segNumber != wTrajectory[0].segNumber):
            lastSegNumber = oldWTraj.segNumber
            vTrajectory.popleft()
            currSeg.newPathSegment(pathSegments.get(wTrajectory[0].segNumber),position,State.getYaw(orientation))
            pathSegments.pop(oldWTraj.segNumber)

# 需要导入模块: from state import State [as 别名]
# 或者: from state.State import getYaw [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 getYaw [as 别名]
def stopForObs(desVelPub,segStatusPub):
    global obsExists
    global obsDist
    global currState
    global currSeg
    global nextSeg
    global RATE
    global segments
    # calculate the stopping acceleration
    # this is allowed to override the segment constraints, because its more important
    # to not crash than to follow the speed limit

    print "Obstacle detected!"
    dt = 1.0/RATE
    decel_rate = pow(currState.v,2)/(2*(obsDist-1.2))

    
    naptime = rospy.Rate(RATE)
    
    des_vel = TwistMsg()
    
    if(decel_rate > 0):
        while(currState.v-.0001 > 0 or currState.v+.0001 < 0):
            if(not obsExists):
                return # if the obstacle went away then resume without fully stopping
            
            # this should take care of negatives
            if(currState.v > 0):
                v_test = currState.v - decel_rate*dt
                des_vel.linear.x = max(v_test,0.0)
                desVelPub.publish(des_vel)
                currState.updateState(des_vel,pose.pose.position,State.getYaw(pose.pose.orientation))
            
            publishSegStatus(segStatusPub) # let everyone else know the status of the segment
            naptime.sleep()
        des_vel.linear.x = 0.0
        desVelPub.publish(des_vel)
        currState.stop()
    else: # should already be stopped
        desVelPub.publish(des_vel)
        currState.stop()
        publishSegStatus(segStatusPub)
        naptime.sleep()
    
    print "Waiting for obstacle to move..."
    startTime = rospy.Time.now()
    waitPeriod = rospy.Duration(3.0)
    while(obsExists):
        if(rospy.Time.now() - startTime > waitPeriod):
            print "Aborting"
            segments = Queue() # flush the queue, the callback thread probably won't appreciate this
            publishSegStatus(segStatusPub,True) # send the abort flag
            currSeg = None
            nextSeg = None
            break
        else:
            publishSegStatus(segStatusPub)
        naptime.sleep()
    return

# 需要导入模块: from state import State [as 别名]
# 或者: from state.State import getYaw [as 别名]
def main():
    global RATE
    global lastVCmd
    global lastOCmd
    global obs
    global obsDist
    global obsExists
    global stopped
    global seg_number
    global currSeg
    global nextSeg
    global pose
    global ping_angle
    
    rospy.init_node('velocity_profiler_alpha')
    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_seg", PathSegmentMsg, pathSegmentCallback)
    rospy.Subscriber("map_pos", PoseStampedMsg, poseCallback)
    
    naptime = rospy.Rate(RATE)
    
    vel_cmd = TwistMsg()
    point = PointMsg()

    
    # while(not ros.Time.isValid()):
        #pass
    
    print "Entering main loop"
    
    while not rospy.is_shutdown():
        if(currSeg is not None):
         #   print "seg type %i " % currSeg.seg_type
          #  print "ref_point %s " % currSeg.ref_point
           # print "curv %f" % currSeg.curvature
           print "dist done %f" % currState.segDistDone
        if stopped:
            stopForEstop(desVelPub,segStatusPub)
            continue
        if(currSeg is not None):
            # Eventually this should work with arcs, spins and lines, but right
            # now its only working with lines
            if(currSeg.seg_type == PathSegmentMsg.LINE):
                # if there is an obstacle and the obstacle is within the segment length
                print ping_angle
                if(obsExists and obsDist/currSeg.seg_length < 1.0-currState.segDistDone and ping_angle > 60 and ping_angle < 140):
                    stopForObs(desVelPub,segStatusPub)
                    continue
            
            vel_cmd.linear.x = lastVCmd
            vel_cmd.angular.z = lastOCmd
            
            point.x = pose.pose.position.x
            point.y = pose.pose.position.y
            point.z = pose.pose.position.z
            
            currState.updateState(vel_cmd, point, State.getYaw(pose.pose.orientation)) # update where the robot is at
            (sVAccel, sVDecel, sWAccel, sWDecel) = computeTrajectory(currSeg,nextSeg) # figure out the switching points in the trajectory
            
            #print "sVAccel: %f sVDecel: %f" % (sVAccel,sVDecel)
            #print "sWAccel: %f, sWDecel: %f" % (sWAccel,sWDecel)
            
            des_vel = getVelCmd(sVAccel, sVDecel, sWAccel, sWDecel) # figure out the robot commands given the current state and the desired trajectory
            desVelPub.publish(des_vel) # publish the commands
            
            publishSegStatus(segStatusPub) # let everyone else know the status of the segment
            
            # see if its time to switch segments yet
            if(currState.segDistDone > 1.0):
                print "Finished segment type %i" % (currState.pathSeg.seg_type)
                print "currState.segDistDone %f" % (currState.segDistDone)
                currSeg = None
        else:
            # try and get new segments
            if(nextSeg is not None):
                currSeg = nextSeg # move the nextSegment up in line
                nextSeg = None # assume no segment until code below is run
            else: # didn't have a next segment before
                try: # so try and get a new one from the queue
                    currSeg = segments.get(False)
                except QueueEmpty: # if the queue is still empty then 
                    currSeg = None # just set it to None
                
            try: # see if a next segment is specified
                nextSeg = segments.get(False) # try and get it
            except QueueEmpty: # if nothing specified
                nextSeg = None # set to None
            
            point = PointMsg()
            point.x = pose.pose.position.x
            point.y = pose.pose.position.y
            point.z = pose.pose.position.z
            
            currState.newPathSegment(currSeg, point, pose.pose.orientation)
            des_vel = TwistMsg()
            desVelPub.publish(des_vel) # publish all zeros for the des_vel
#.........这里部分代码省略.........

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