Esempio n. 1
0
 def updateImu(self, imu):
     self.imu_last_update = rospy.Time.now()
     self.imu_init = True
     
     if not self.imu_data :
         angle = euler_from_quaternion([imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w])
         imu_data =  PyKDL.Rotation.RPY(angle[0], angle[1], angle[2])
         imu_data = self.imu_tf.M * imu_data
         angle = imu_data.GetRPY()   
         self.last_imu_orientation = angle
         self.last_imu_update = imu.header.stamp
         self.imu_data = True
     else:
         angle = euler_from_quaternion([imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w])
         imu_data =  PyKDL.Rotation.RPY(angle[0], angle[1], angle[2])
         imu_data = self.imu_tf.M * imu_data
         angle = imu_data.GetRPY()   
         angle_quaternion = tf.transformations.quaternion_from_euler(angle[0], angle[1], angle[2])
         self.odom.pose.pose.orientation.x = angle_quaternion[0]
         self.odom.pose.pose.orientation.y = angle_quaternion[1]
         self.odom.pose.pose.orientation.z = angle_quaternion[2]
         self.odom.pose.pose.orientation.w = angle_quaternion[3]
         
         # Derive angular velocities from orientations ####################### 
         period = (imu.header.stamp - self.last_imu_update).to_sec()
         self.odom.twist.twist.angular.x = cola2_lib.normalizeAngle(angle[0] - self.last_imu_orientation[0]) / period 
         self.odom.twist.twist.angular.y = cola2_lib.normalizeAngle(angle[1] - self.last_imu_orientation[1]) / period 
         self.odom.twist.twist.angular.z = cola2_lib.normalizeAngle(angle[2] - self.last_imu_orientation[2]) / period 
         self.last_imu_orientation = angle
         self.last_imu_update = imu.header.stamp          
         #####################################################################
         
         if self.makePrediction():
             self.ekf.updatePrediction()
             self.publishData()
Esempio n. 2
0
    def updateImu(self, imu):
        config = self.config
        config.imu_last_update = imu.header.stamp #rospy.Time.now()
        config.imu_init = True
        
        if not config.imu_data :
            angle = euler_from_quaternion([imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w])
            imu_data =  PyKDL.Rotation.RPY(angle[0], angle[1], angle[2])
            imu_data = config.imu_tf.M * imu_data
            angle = imu_data.GetRPY()   
            config.last_imu_orientation = angle
            config.last_imu_update = imu.header.stamp
            
            # Initialize heading buffer in order to apply a savitzky_golay derivation
            if len(config.heading_buffer) == 0:
                config.heading_buffer.append(angle[2])
                
            inc = cola2_lib.normalizeAngle(angle[2] - config.heading_buffer[-1])
            config.heading_buffer.append(config.heading_buffer[-1] + inc)
            
            if len(config.heading_buffer) == len(config.savitzky_golay_coeffs):
                config.imu_data = True
        else:
            angle = euler_from_quaternion([imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w])
            imu_data =  PyKDL.Rotation.RPY(angle[0], angle[1], angle[2])
            imu_data = config.imu_tf.M * imu_data
            angle = imu_data.GetRPY()   
            angle_quaternion = tf.transformations.quaternion_from_euler(angle[0], angle[1], angle[2])
            config.odom.pose.pose.orientation.x = angle_quaternion[0]
            config.odom.pose.pose.orientation.y = angle_quaternion[1]
            config.odom.pose.pose.orientation.z = angle_quaternion[2]
            config.odom.pose.pose.orientation.w = angle_quaternion[3]
            
            # Derive angular velocities from orientations ####################### 
            period = (imu.header.stamp - config.last_imu_update).to_sec()
            
            # For yaw rate we apply a savitzky_golay derivation
            inc = cola2_lib.normalizeAngle(angle[2] - config.heading_buffer[-1])
            config.heading_buffer.append(config.heading_buffer[-1] + inc)
            config.heading_buffer.pop(0)
            config.odom.twist.twist.angular.z = convolve(config.heading_buffer, config.savitzky_golay_coeffs, mode='valid') / period

            # TODO: Roll rate and Pitch rate should be also savitzky_golay derivations
            config.odom.twist.twist.angular.x = cola2_lib.normalizeAngle(angle[0] - config.last_imu_orientation[0]) / period 
            config.odom.twist.twist.angular.y = cola2_lib.normalizeAngle(angle[1] - config.last_imu_orientation[1]) / period 

            config.last_imu_orientation = angle
            config.last_imu_update = imu.header.stamp          
            #####################################################################
            
            try:
                self.LOCK.acquire()
                if self.makePrediction(imu.header.stamp):
                    self.filter.updatePrediction()
                    self.publishData()
            finally:
                self.LOCK.release()
Esempio n. 3
0
 def moveMode_X_Y_Z_YAW(self, req, now):
     tolerance = [req.position_tolerance.x, 
                  req.position_tolerance.y, 
                  req.position_tolerance.z, 
                  req.orientation_tolerance.roll, 
                  req.orientation_tolerance.pitch, 
                  req.orientation_tolerance.yaw]
     z_error = 0.0
     yaw_error = 0.0
     x_error = 0.0
     y_error = 0.0
 
     # Compute Errors
     # Altitude mode
     if req.altitude_mode : 
         z_error = self.altitude - req.altitude
         
         desired = [req.position.north, 
                    req.position.east, 
                    req.altitude, 
                    req.orientation.roll, 
                    req.orientation.pitch, 
                    req.orientation.yaw]
         
         current = [self.p[0], 
                    self.p[1], 
                    self.altitude, 
                    self.p[3], 
                    self.p[4], 
                    self.p[5]]
     # Depth Mode
     else : 
         z_error = req.position.depth - self.p[2]
         
         desired = [req.position.north, 
                    req.position.east, 
                    req.position.depth, 
                    req.orientation.roll, 
                    req.orientation.pitch, 
                    req.orientation.yaw]
         
         current = self.p
         
     yaw_error = cola2_lib.normalizeAngle(req.orientation.yaw - self.p[5])
     
     [x_error, y_error] = self.computePosError(self.p[0], 
                                               self.p[1], 
                                               self.p[5], 
                                               req.position.north, 
                                               req.position.east)
     
     error = array([x_error, y_error, z_error, 0.0, 0.0, yaw_error])
     
     #Compute Body Velocity request
     bvr = zeros(6)
     real_period = (now - self.past_time) #nano seconds to seconds
     self.past_time = now
     [bvr, self.ek_1, self.eik_1]  = self.pid_x_y_z_yaw.computePid(error, 
                                                                   zeros(6), 
                                                                   self.ek_1, 
                                                                   self.eik_1, 
                                                                   real_period)
 
     # Send Body Velocity Request
     if req.disable_axis.z == True:
         bvr[2] = 0.0
     if req.disable_axis.yaw == True:
         bvr[5] = 0.0
         
     success = self.checkTolerance([False, False, req.disable_axis.z, True, True, req.disable_axis.yaw], 
                                   current, 
                                   desired, 
                                   tolerance)
     print 'current: ', current
     print 'desired: ', desired
     print 'tolerance: ', tolerance
     
     
     return [success, bvr*self.max_velocity]
Esempio n. 4
0
 def moveMode_LOS(self, previous_req, req, now):
     z_error = 0.0
     yaw_error = 0.0
     tolerance = [req.position_tolerance.x, 
                  req.position_tolerance.y, 
                  req.position_tolerance.z, 
                  req.orientation_tolerance.roll, 
                  req.orientation_tolerance.pitch, 
                  req.orientation_tolerance.yaw]
     
     # Compute Error
     
     # Altitude mode
     if req.altitude_mode : 
         #TODO: Hem falta llegir la current altitude (hauria de ser ella enlloc de _p[2])!!!
         z_error = self.altitude - req.altitude 
         
         desired = [req.position.north, 
                    req.position.east, 
                    req.altitude, 
                    req.orientation.roll, 
                    req.orientation.pitch, 
                    req.orientation.yaw]
         
         current = [self.p[0], 
                    self.p[1], 
                    self.altitude, 
                    self.p[3], 
                    self.p[4], 
                    self.p[5]]
     
     # Depth Mode
     else : 
         z_error = req.position.depth - self.p[2]
         desired = [req.position.north, 
                    req.position.east, 
                    req.position.depth, 
                    req.orientation.roll, 
                    req.orientation.pitch, 
                    req.orientation.yaw]
         
         current = self.p
         
     #Compute cross-track error
     alpha = math.atan2(req.position.east - previous_req.position.east, req.position.north - previous_req.position.north)
     
     #print "Alpha: " + str(alpha)
     e = -(self.p[0] - previous_req.position.north)*sin(alpha) + (self.p[1] - previous_req.position.east)*cos(alpha)
     #print "e:" + str(e)
     
     beta = math.atan2(self.v[1], self.v[0])
     #print "beta: " + str(beta)
     
     delta = 4.0
     #print "cross track error: " + str(math.atan(-e/delta))
     
     yaw_error = self.p[5] - (alpha + math.atan(-e/delta)) + beta
     yaw_error = cola2_lib.normalizeAngle(yaw_error)
     #print "yaw_error: " + str(yaw_error)
     
     error = array([0.0, 0.0, z_error, 0.0, 0.0, -yaw_error])
     
     #Compute Body Velocity request
     bvr = zeros(6)
     real_period = (now - self.past_time) #nano seconds to seconds
     self.past_time = now
     [bvr, self.ek_1, self.eik_1]  = self.pid_x_z_yaw.computePid(error, 
                                                                 zeros(6),
                                                                 self.ek_1, 
                                                                 self.eik_1, 
                                                                 real_period)
     
     bvr *= self.max_velocity
     
     #TODO: All these vars have to be defined in some other place
     #Set x velocity to a constant value that depends on the yaw_error
     min_yaw_error = 0.05 #6 degrees
     max_yaw_error = self.max_angle_error
     min_x_v = self.min_velocity_los[0]
     max_x_v = self.max_velocity[0]
     
     SURGE_DEPEND_ON_YAW = False
     
     if SURGE_DEPEND_ON_YAW:
         error_angle = atan2(max_x_v - min_x_v, max_yaw_error - min_yaw_error)
         if abs(yaw_error) < min_yaw_error: bvr[0] = max_x_v
         elif abs(yaw_error) > max_yaw_error: bvr[0] = min_x_v
         else: bvr[0] = min_x_v + tan(error_angle)*(max_yaw_error - abs(yaw_error))
     else:
         bvr[0] = max_x_v
        
     #When the vehicle reaches or leaves a way-point the forward velocity follow a trapezoid
     #from min_distance to 0.0 distance respect to the way-point, the forward velocity decreases linearly 
     #from 100% of the computed value to min_tant_per_one*100 % of it.
     
     #TODO: All these vars have to be defined in some other place
     min_tant_per_one = 0.10     #tant per one
     min_distance = 5.0          #meters, this value has to be bigger than the acceptance sphere
     dist_angle = atan2(1-min_tant_per_one, min_distance)
     
     #distance to current way-point
     distance_current = sqrt((req.position.north-self.p[0])**2 + (req.position.east-self.p[1])**2)
     
     #distance to previous way-point
     distace_previous = sqrt((previous_req.position.north-self.p[0])**2 + (previous_req.position.east-self.p[1])**2)
     
     #Take the smaller one
     distance = min(distance_current, distace_previous)
     
     if distance < min_distance:
         tant_per_one = min_tant_per_one + tan(dist_angle)*distance
         bvr[0] *= tant_per_one
         if bvr[0] < min_x_v: bvr[0] = min_x_v    
    
     # Compute feedback
     success = self.checkTolerance([False, False, req.disable_axis.z, True, True, True], 
                                   current, 
                                   desired, 
                                   tolerance)
     
     #Check if the limit line has been crossed. Where the limit line is the line perpendicular to 
     #the desired path that pass through the current way-point, 
     inc_y = req.position.east - previous_req.position.east
     if inc_y != 0.0 :
         m_l = -(req.position.north - previous_req.position.north) / inc_y
     else: 
         m_l = 999999.9  
     c_l = -m_l * req.position.north + req.position.east
     current_d = (m_l * self.p[0] - self.p[1] + c_l)/sqrt(m_l**2 + 1)
     signe = (m_l * previous_req.position.north - previous_req.position.east + c_l)/sqrt(m_l**2 + 1)
     if signe * current_d < 0.0: 
         success = True
     # print "distance: ", current_d
     # print "signe: ", signe
     
     return [success, bvr]
Esempio n. 5
0
    def moveMode_X_Z_YAW(self, req, now):
        x_error = 0.0
        z_error = 0.0
        yaw_error = 0.0
        tolerance = [req.position_tolerance.x, 
                     req.position_tolerance.y, 
                     req.position_tolerance.z, 
                     req.orientation_tolerance.roll, 
                     req.orientation_tolerance.pitch, 
                     req.orientation_tolerance.yaw]
        
        # Compute Error
        
        # Altitude mode
        if req.altitude_mode : 
            #TODO: Hem falta llegir la current altitude (hauria de ser ella enlloc de _p[2])!!!
            z_error = self.altitude - req.altitude 
            
            desired = [req.position.north, 
                       req.position.east, 
                       req.altitude, 
                       req.orientation.roll, 
                       req.orientation.pitch, 
                       req.orientation.yaw]
            
            current = [self.p[0], 
                       self.p[1], 
                       self.altitude, 
                       self.p[3], 
                       self.p[4], 
                       self.p[5]]
        
        # Depth Mode
        else : 
            z_error = req.position.depth - self.p[2]
            
            desired = [req.position.north, 
                       req.position.east, 
                       req.position.depth, 
                       req.orientation.roll, 
                       req.orientation.pitch, 
                       req.orientation.yaw]
            
            current = self.p
            
        inc_x = req.position.north - self.p[0]
        inc_y = req.position.east - self.p[1]
        desired_yaw = math.atan2(inc_y, inc_x)
        yaw_error = cola2_lib.normalizeAngle(desired_yaw - self.p[5])
        
        if abs(yaw_error) < self.max_angle_error :
            x_error = math.sqrt(pow(inc_x,2) + pow(inc_y,2))
       
        error = array([x_error, 0.0, z_error, 0.0, 0.0, yaw_error])
        
        #Compute Body Velocity request
        bvr = zeros(6)
        real_period = (now - self.past_time) #nano seconds to seconds
        self.past_time = now
        [bvr, self.ek_1, self.eik_1]  = self.pid_x_z_yaw.computePid(error, 
                                                                    zeros(6),
                                                                    self.ek_1, 
                                                                    self.eik_1, 
                                                                    real_period)
        
        # Compute feedback
        success = self.checkTolerance([False, False, req.disable_axis.z, True, True, True], 
                                      current, 
                                      desired, 
                                      tolerance)

        # When moving with waypoint mode (normaly short distances) reduce the surge velocity
        max = list(self.max_velocity)
        max[0] = max[0] / 2.0

        return [success, bvr * max]