def angle_between(angle, min_angle, max_angle): ''' Returns True if min_angle <= angle <= max_angle on a normalized range of [0, 2pi) ''' span = gps.normalize(max_angle - min_angle) to_test_angle = gps.normalize(angle - min_angle) return to_test_angle < span
def step_autonomy(self, t, dt): # Go to desired latitude, longitude, and maintain altitude # deconfliction: own_pos = self._own_pose.pose.pose.position own_lat = own_pos.lat own_lon = own_pos.lon own_alt = own_pos.rel_alt own_orientation = self._own_pose.pose.pose.orientation # Calculate bearing to target and from target to self bearing = gps.normalize( gps.gps_bearing(own_lat, own_lon, self._desired_lat, self._desired_lon)) lat = self._desired_lat lon = self._desired_lon dist = gps.gps_distance(own_lat, own_lon, self._desired_lat, self._desired_lon) bearing_adj = 0.0 if dist > 100: bearing_adj = math.pi / 4 * (-1 + self._id % 2 * 2) lat, lon = gps.gps_newpos(own_lat, own_lon, bearing + bearing_adj, 1000) if bearing_adj > 0: print 'ID: {} orbiting CW'.format(self._id) else: print 'ID: {} orbiting CCW'.format(self._id) self._wp = np.array([lat, lon, self._last_ap_wp[2]]) return True
def _s_turn(self): ''' Calculations associated with the S-turn to the virtual target ''' if self._s_turn_state == self.S1: # Offset turn self._gain = 2.0 self._bias = 0.5 * gps.signum(self._theta_dot) if math.fabs(gps.signum(self._theta_dot) - \ gps.signum(self._theta_dot_last)) == 2: self._s_turn_state = self.S2 self._theta2 = self._theta self._theta3 = self._theta2 + 0.75 * (self._theta_VT_init - self._theta2) self._theta3 = gps.normalize(self._theta3) self._s2_check = \ gps.signum(gps.normalize_pi(self._theta - self._theta3)) self.log_info("Shift to S-turn phase S2:\tTheta=%f\tTheta3=%f\tTheta_dot=%f"\ %(self._theta, self._theta3, self._theta_dot)) if self._s_turn_state == self.S2: # Middle portion of the S self._bias = -0.25 * gps.signum(self._theta_dot) self._gain = -2.0 if self._s2_check != \ gps.signum(gps.normalize_pi(self._theta - self._theta3)): self._s_turn_state = self.S3 self._theta3 = self._theta self._psi_p3 = self._xp[2] self.log_info("Shift to S-turn phase S3") self._bias_initialized = False # to facilitate a log entry if self._s_turn_state == self.S3: # Final turn to VT if (self._theta_dot_VT_init < 0.0 and \ angle_between(self._psi_p_f, self._theta_VT_init, \ 2.0 * self._theta3 - self._psi_p3)) or \ (self._theta_dot_VT_init >= 0.0 and \ angle_between(self._psi_p_f, \ 2.0 * self._theta3 - self._psi_p3, \ self._theta_VT_init)): self._gain = math.fabs((gps.normalize_pi(self._psi_p_f - self._psi_p3)) / \ (gps.normalize_pi(self._psi_p_f - self._theta3))) self._bias = 0.0 if not self._bias_initialized: self._bias_initialized = True self.log_info("Phase S3 using 1-bias turn") else: if math.fabs(gps.normalize_pi(self._psi_p_f - self._xp[2]) / \ gps.normalize_pi(self._psi_p_f - self._theta)) < 2.0: self._gain = gps.normalize_pi(self._psi_p3) / \ gps.normalize_pi(math.pi/2.0 + self._theta3) self._bias = 0.0 else: self._gain = 2.0 self._bias = 0.0 if not self._bias_initialized: self._bias_initialized = True self.log_info("Phase S3 using 2-bias turn")
def _calculate_E(self): ''' Calculates the approximated integral of control energy square (E) @return calculated value ''' theta_p = gps.normalize(self._xp[2] - self._theta) theta_t = gps.normalize(self._xt[2] - self._theta) theta_tr = self._theta R = self._range delta_theta = 0.0025 * gps.signum(self._theta_dot) v_p = math.hypot(self._vp[0], self._vp[1]) v_t = math.hypot(self._vt[0], self._vt[1]) v_r = v_t * math.cos(theta_t) - v_p * math.cos(theta_p) v_theta = v_t * math.sin(theta_t) - v_p * math.sin(theta_p) E = 0.0 while math.cos(delta_theta) > math.cos(self._psi_p_f - theta_tr): E = E + math.pow((self._gain * v_p), 2) * \ (v_theta / R) * delta_theta R = R * math.exp(v_r / v_theta * delta_theta) theta_t = theta_t - delta_theta theta_p = theta_p + (self._gain - 1) * delta_theta v_r = v_t * math.cos(theta_t) - v_p * math.cos(theta_p) v_theta = v_t * math.sin(theta_t) - v_p * math.sin(theta_p) theta_tr = theta_tr + delta_theta return E
def _calculate_E(self): ''' Calculates the approximated integral of control energy square (E) @return calculated value ''' theta_p = gps.normalize(self._xp[2] - self._theta) theta_t = gps.normalize(self._xt[2] - self._theta) theta_tr = self._theta R = self._range delta_theta = 0.0025 * gps.signum(self._theta_dot) v_p = math.hypot(self._vp[0], self._vp[1]) v_t = math.hypot(self._vt[0], self._vt[1]) v_r = v_t * math.cos(theta_t) - v_p * math.cos(theta_p) v_theta = v_t * math.sin(theta_t) - v_p * math.sin(theta_p) E = 0.0 while math.cos(delta_theta) > math.cos(self._psi_p_t_f - theta_tr): E = E + math.pow((self._gain * v_p), 2) * \ (v_theta / R) * delta_theta R = R * math.exp(v_r / v_theta * delta_theta) theta_t = theta_t - delta_theta theta_p = theta_p + (self._gain - 1) * delta_theta v_r = v_t * math.cos(theta_t) - v_p * math.cos(theta_p) v_theta = v_t * math.sin(theta_t) - v_p * math.sin(theta_p) theta_tr = theta_tr + delta_theta return E
def _parallel_2_state(self): ''' Performs calculations and operations required for the "parallel 2" state ''' if not self._state_initialized: self._psi_p_t_f = gps.normalize(self._xt[2] - PNInterceptor.FIRE_ANGLE) self._bias = 0.0 self._state_initialized = True self._set_ctrl_params(self._xp, self._vp, self._xt, self._vt) self._gain = math.fabs((self._psi_p_t_f - self._xp[2]) / \ (self._psi_p_t_f - self._theta)) self._E = self._calculate_E() if self._E > min(numpy.mean(self._E_trend), \ numpy.median(self._E_trend)): # reached minimum self._state_initialized = False self._state = PNInterceptor.INTERCEPT self.log_info("Transition to intercept phase") self._E_trend = self._E_trend[1:] + [ self._E ]
def _parallel_1_state(self): ''' Performs calculations and operations required for the "parallel 1" state ''' if not self._state_initialized: bearing = gps.normalize(self._xt[2] - math.pi) self.wp_msg.lat, self.wp_msg.lon = \ gps.gps_newpos(self._xp[0], self._xp[1], bearing, 10000.0) self.publishWaypoint(self.wp_msg) self._state_initialized = True self._set_ctrl_params(self._xp, self._vp, self._xt, self._vt) if self._range <= PNInterceptor.R_TH2: self._state = PNInterceptor.PARALLEL_2 self._gain = 0.0 self._bias = 0.0 self._state_initialized = False self.log_info("Transition to parallel_2 state")
def _parallel_1_state(self): ''' Performs calculations and operations required for the "parallel 1" state ''' if not self._state_initialized: bearing = gps.normalize(self._xt[2] - math.pi) self.wp_msg.lat, self.wp_msg.lon = \ gps.gps_newpos(self._xp[0], self._xp[1], bearing, 10000.0) self.publishWaypoint(self.wp_msg) self._state_initialized = True self._set_ctrl_params(self._xp, self._vp, self._xt, self._vt) if self._range <= USMAInterceptor.R_TH2: self._state = USMAInterceptor.PARALLEL_2 self._gain = 0.0 self._bias = 0.0 self._state_initialized = False self.log_info("Transition to parallel_2 state")
def _parallel_2_state(self): ''' Performs calculations and operations required for the "parallel 2" state ''' if not self._state_initialized: self._psi_p_f = gps.normalize(self._xt[2] - USMAInterceptor.FIRE_ANGLE) self._bias = 0.0 self._state_initialized = True self._set_ctrl_params(self._xp, self._vp, self._xt, self._vt) self._gain = math.fabs(gps.normalize_pi(self._psi_p_f - self._xp[2]) / \ gps.normalize_pi(self._psi_p_f - self._theta)) self._E = self._calculate_E() if self._E > min(numpy.mean(self._E_trend), \ numpy.median(self._E_trend)): # reached minimum self._state_initialized = False self._state = USMAInterceptor.INTERCEPT self.log_info("Transition to intercept phase") self._E_trend = self._E_trend[1:] + [self._E]
def step_autonomy(self, t, dt): # Reset the action and target ID every loop self._action = ["None", 0] self._target_id = None # Search for the closest target every frame min_dist = np.inf for uav in self._reds.values(): if uav.vehicle_id not in self._shot: d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\ self._own_pose.pose.pose.position.lon,\ uav.state.pose.pose.position.lat,\ uav.state.pose.pose.position.lon) if d < min_dist: min_dist = d self._target_id = uav.vehicle_id # Determine if the target is within firing parameters if gps.hitable(self._own_pose.pose.pose.position.lat, \ self._own_pose.pose.pose.position.lon, \ self._own_pose.pose.pose.position.rel_alt, \ (self._own_pose.pose.pose.orientation.x, \ self._own_pose.pose.pose.orientation.y, \ self._own_pose.pose.pose.orientation.z, \ self._own_pose.pose.pose.orientation.w ), \ self._max_range, self._fov_width, self._fov_height,\ uav.state.pose.pose.position.lat,\ uav.state.pose.pose.position.lon, \ uav.state.pose.pose.position.rel_alt): self._action = ["Fire", uav.vehicle_id] print "RT=========(loop)========>ID: %d shot at Target %d" % ( self._id, uav.vehicle_id) '''if self._last_target_id == self._target_id: if d < 100: # how close do we have to be before I call chasing self._target_count += 1 if self._target_count > 100: self._target_count = 300 self._target_id = None elif self._target_count > 0: self._target_count -= 1 self._target_id = None ''' # If a target has been identified, move towards it if self._target_id != None: self._last_target_id = self._target_id # Calculate target and own position and orientation target_pos = self._reds[self._target_id].state.pose.pose.position target_orientation = self._reds[ self._target_id].state.pose.pose.orientation tgt_lat = target_pos.lat tgt_lon = target_pos.lon tgt_alt = target_pos.rel_alt own_pos = self._own_pose.pose.pose.position own_lat = own_pos.lat own_lon = own_pos.lon own_alt = own_pos.rel_alt own_orientation = self._own_pose.pose.pose.orientation # Calculate bearing to target and from target to self bearing = gps.normalize( gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon)) tgt_bearing = gps.normalize( gps.gps_bearing(tgt_lat, tgt_lon, own_lat, own_lon)) # Calculate heading/yaw of self and target own_rpy = qmath.quat_to_euler((own_orientation.x,\ own_orientation.y, \ own_orientation.z, \ own_orientation.w)) own_heading = gps.normalize(own_rpy[2]) tgt_rpy = qmath.quat_to_euler((target_orientation.x,\ target_orientation.y, \ target_orientation.z, \ target_orientation.w)) tgt_heading = gps.normalize(tgt_rpy[2]) # Calculate distance to target dist = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon) bearing_adj = 0.0 #print 'ID: {} targeting UAV: {} at bearing {} and distance {}'.format(self._id, self._target_id, math.degrees(bearing), dist) #print 'UAV: {} own bearing: {}'.format(self._target_id, math.degrees(tgt_heading)) # Calculate offset between bearing and target heading (measures degree of "head on") # This section needs more work # ---------------------------------------------------------------------------- heading_diff = own_heading - tgt_heading tgt_bearing_diff = gps.normalize_pi(tgt_heading - tgt_bearing) if math.fabs(heading_diff) > math.pi / 2: # moving towards each other if tgt_bearing > ( tgt_heading - self._fov_width_radians / 2 ) or tgt_bearing < (tgt_heading + self._fov_width_radians / 2): # we are in the "fan" if dist > self._max_range and math.fabs( tgt_bearing_diff) < self._fov_width_radians / 2: bearing_adj = math.copysign(ADJ_ANGLE, tgt_bearing_diff) print 'ID: {} toward UAV: {}: adjusting bearing by {}'.format( self._id, self._target_id, math.degrees(bearing_adj)) else: # heading in same general direction if math.fabs(tgt_bearing_diff) < math.pi / 2: # I am in front of target if dist < 2 * self._max_range and math.fabs( tgt_bearing_diff) < self._fov_width_radians / 2: bearing_adj = -math.copysign(ADJ_ANGLE, tgt_bearing_diff) print 'ID: {} away from UAV: {} adjusting bearing by {}'.format( self._id, self._target_id, math.degrees(bearing_adj)) # ---------------------------------------------------------------------------- lat, lon = gps.gps_newpos(own_lat, own_lon, bearing + bearing_adj, 1000) self._wp = np.array([lat, lon, own_alt]) # keep own alt # Determine if the target is within firing parameters if gps.hitable(self._own_pose.pose.pose.position.lat, \ self._own_pose.pose.pose.position.lon, \ self._own_pose.pose.pose.position.rel_alt, \ (self._own_pose.pose.pose.orientation.x, \ self._own_pose.pose.pose.orientation.y, \ self._own_pose.pose.pose.orientation.z, \ self._own_pose.pose.pose.orientation.w ), \ self._max_range, self._fov_width, self._fov_height,\ target_pos.lat, target_pos.lon, target_pos.rel_alt): self._action = ["Fire", self._target_id] print "RT=======================>ID: %d shot at Target %d" % ( self._id, self._target_id) else: # If a target hasn't been identified, return to the safe waypoint self._wp = self._safe_waypoint return True
def step_autonomy(self, t, dt): # Reset the action every loop self._action = ["None", 0] self._target_id = None own_lat = self._own_pose.pose.pose.position.lat own_lon = self._own_pose.pose.pose.position.lon own_alt = self._own_pose.pose.pose.position.alt # Search for the closest target min_dist = np.inf for uav in self._reds.values(): if uav.vehicle_id not in self._shot: d = gps.gps_distance(own_lat, \ own_lon, \ uav.state.pose.pose.position.lat, \ uav.state.pose.pose.position.lon) if d < min_dist: min_dist = d self._target_id = uav.vehicle_id # If a target has been identified, move towards it if self._target_id != None: target_pos = self._reds[self._target_id].state.pose.pose.position tgt_lat = target_pos.lat tgt_lon = target_pos.lon distance_to_target = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon) # Set bearing to look at the enemy vehicle bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon) # Set waypoint to move forward and strafe sideways once the enemy is within range self._evasion_range = self._weapons_range * 2 self._engagement_range = self._weapons_range * 2 / 3 if distance_to_target <= self._engagement_range: lat, lon = gps.gps_newpos( own_lat, own_lon, gps.normalize(bearing - self._fov_width / 2), 0.1) elif distance_to_target <= self._evasion_range: # Check to see if sidestep right waypoint is outside the battlebox # (only need to check distance_to_target/2 since enemy is closing) lat_right, lon_right = gps.gps_newpos(own_lat, own_lon, \ gps.normalize(bearing + (math.pi / 3)), distance_to_target/2) if self._bboxes[0].contains(lat_right, lon_right, own_alt): lat, lon = gps.gps_newpos(own_lat, own_lon, \ gps.normalize(bearing + (math.pi / 3)), distance_to_target) else: # If right sidestep is outside the battlebox, sidestep to the left lat, lon = gps.gps_newpos( own_lat, own_lon, gps.normalize(bearing - (math.pi / 3)), distance_to_target) else: lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, 1000) self._wp = np.array([lat, lon, target_pos.rel_alt]) # Determine if the target is within firing parameters if gps.hitable(self._own_pose.pose.pose.position.lat, \ self._own_pose.pose.pose.position.lon, \ self._own_pose.pose.pose.position.rel_alt, \ (self._own_pose.pose.pose.orientation.x, \ self._own_pose.pose.pose.orientation.y, \ self._own_pose.pose.pose.orientation.z, \ self._own_pose.pose.pose.orientation.w), \ self._weapons_range, self._fov_width, self._fov_height, \ target_pos.lat, target_pos.lon, target_pos.rel_alt): self._action = ["Fire", self._target_id] else: # If a target hasn't been identified, return to the safe waypoint self._wp = self._safe_waypoint return True
def step_autonomy(self, t, dt): # Get our current position own_lat = self._own_pose.pose.pose.position.lat own_lon = self._own_pose.pose.pose.position.lon own_alt = self._own_pose.pose.pose.position.rel_alt # was just alt # Reset the action every loop self._action = ["None", 0] self._last_target_id = self._target_id self._last_target_bearing = self._target_bearing self._target_id = None self._target_bearing = 0.0 bearing_adj = -0.7 # default adjust for CCW circling enemy # Search for the closest target min_dist = np.inf for uav in self._reds.values(): if uav.vehicle_id not in self._shot and ( uav.game_status == enums.GAME_ACTIVE_DEFENSE or uav.game_status == enums.GAME_ACTIVE_OFFENSE): target_pos = self._reds[ uav.vehicle_id].state.pose.pose.position d = gps.gps_distance(own_lat, \ own_lon, \ uav.state.pose.pose.position.lat, \ uav.state.pose.pose.position.lon) if gps.hitable(self._own_pose.pose.pose.position.lat, \ self._own_pose.pose.pose.position.lon, \ self._own_pose.pose.pose.position.rel_alt, \ (self._own_pose.pose.pose.orientation.x, \ self._own_pose.pose.pose.orientation.y, \ self._own_pose.pose.pose.orientation.z, \ self._own_pose.pose.pose.orientation.w), \ self._weapons_range, self._fov_width, self._fov_height, \ target_pos.lat, target_pos.lon, target_pos.rel_alt): self._action = ["Fire", uav.vehicle_id] print "Quad=======(loop)=======> ID: %d shot at enemy %d" % ( self._id, uav.vehicle_id) if d < min_dist: min_dist = d self._target_id = uav.vehicle_id # If a target has been identified, move towards it if self._target_id != None: target_pos = self._reds[self._target_id].state.pose.pose.position tgt_lat = target_pos.lat tgt_lon = target_pos.lon distance_to_target = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon) # Set bearing to look at the enemy vehicle bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon) self._target_bearing = bearing if self._target_id == self._last_target_id: bearing_diff = self._target_bearing - self._last_target_bearing if bearing_diff < 0.0: bearing_adj = math.sin(-UAV_SPEED / distance_to_target) else: bearing_adj = math.sin(UAV_SPEED / distance_to_target) # Set waypoint to move forward and strafe sideways once the enemy is within range self._evasion_range = self._weapons_range * 3 self._engagement_range = self._weapons_range * 2 / 3 if distance_to_target <= self._engagement_range: lat, lon = gps.gps_newpos(own_lat, own_lon, gps.normalize(bearing + bearing_adj), 0.1) # was - self._fov_width/2 #print "UAV %d in engagement range with enemy %d bearing %f distance %f using %f bearing adj" % (self._id, self._target_id, math.degrees(bearing), distance_to_target, bearing_adj) elif distance_to_target <= self._evasion_range: # Check to see if sidestep right waypoint is outside the battlebox # (only need to check distance_to_target/2 since enemy is closing) evade_sign = -2 * (self._id % 2) + 1 #print "UAV %d in evasion range with enemy %d" % (self._id, self._target_id) lat, lon = gps.gps_newpos( own_lat, own_lon, gps.normalize(bearing - evade_sign * (math.pi / 3)), distance_to_target) if not self._bboxes[0].contains(lat, lon, own_alt): lat, lon = gps.gps_newpos( own_lat, own_lon, gps.normalize(bearing + evade_sign * (math.pi / 3)), distance_to_target) #print "UAV %d sidestepping left" % self._id else: lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, 1000) # was 1000 self._wp = np.array([lat, lon, own_alt ]) # don't change alt, was target_pos.rel_alt # Determine if the target is within firing parameters if gps.hitable(self._own_pose.pose.pose.position.lat, \ self._own_pose.pose.pose.position.lon, \ self._own_pose.pose.pose.position.rel_alt, \ (self._own_pose.pose.pose.orientation.x, \ self._own_pose.pose.pose.orientation.y, \ self._own_pose.pose.pose.orientation.z, \ self._own_pose.pose.pose.orientation.w), \ self._weapons_range, self._fov_width, self._fov_height, \ target_pos.lat, target_pos.lon, target_pos.rel_alt): self._action = ["Fire", self._target_id] print "Quad====================> ID: %d shot at enemy %d" % ( self._id, self._target_id) else: # If a target hasn't been identified, return to the safe waypoint self._wp = self._safe_waypoint return True
def _offset_state(self): ''' Performs calculations and operations required for the "offset" state ''' if not self._state_initialized: # Set the virtual waypoint D2 meters along the target's path and # offset by D1 meters to the side that the pursuer is currently on theta = gps.gps_bearing(self._xp[0], self._xp[1], \ self._xt[0], self._xt[1]) offset_dir = \ gps.signum(gps.normalize_pi(theta + math.pi - self._xt[2])) tgt_to_VT = self._xt[2] + math.atan2(PNInterceptor.D1 * offset_dir, \ PNInterceptor.D2) self._VT = \ gps.gps_newpos(self._xt[0], self._xt[1], tgt_to_VT, \ math.hypot(PNInterceptor.D1, PNInterceptor.D2)) self._set_ctrl_params(self._xp, self._vp, \ (self._VT[0], self._VT[1], 0.0), (0.0, 0.0)) self._theta_VT_init = self._theta self._theta_dot_VT_init = self._theta_dot self._psi_p_VT_init = self._xp[2] self._psi_p_t_f = gps.normalize(self._xt[2] + math.pi) self._state_initialized = True self._set_ctrl_params(self._xp, self._vp, \ (self._VT[0], self._VT[1], 0.0), (0.0, 0.0)) if self._range < PNInterceptor.R_CAP_PARALLEL: self._state_initialized = False self._state = PNInterceptor.PARALLEL_1 self.log_info("Transition to parallel_1 phase") # Determine how to maneuver to the virtual target point # Direct turn to the virtual target if (self._theta_VT_init <= self._psi_p_t_f) and \ (self._psi_p_t_f <= ((2.0 * self._theta_VT_init) - self._psi_p_VT_init)): self._gain = math.fabs((self._psi_p_t_f - self._psi_p_VT_init) / \ (self._psi_p_t_f - self._theta_VT_init)) self._bias = 0.0 # Single turn to virtual target with 2 gains elif ((-math.pi - self._psi_p_VT_init + \ (2.0 * self._theta_VT_init)) <= self._psi_p_t_f) and \ (self._psi_p_t_f <= ((2.0 * self._theta_VT_init) - \ self._psi_p_VT_init)): if (gps.normalize(self._psi_p_t_f - self._xp[2]) / \ gps.normalize(self._psi_p_t_f - self._theta)) < 2.0: self._gain = (2.0 / math.pi) * self._psi_p_VT_init self._bias = 0.0 elif (gps.normalize(self._psi_p_t_f - self._xp[2]) / \ gps.normalize(self._psi_p_t_f - self._theta)) == 2.0: self._gain = 2.0 self._bias = 0.0 else: # should never get here self.log_warn("Unable to determine phase 2 2-step case") self.set_ready_state(False) # S-turn to virtual target else: if gps.signum(self._theta_dot) == \ gps.signum(self._theta_dot_VT_init): self._gain = 2.5 self._bias = 0.1 * gps.signum(self._theta_dot_VT_init) elif (gps.normalize(self._psi_p_t_f - self._xp[2]) / \ gps.normalize(self._psi_p_t_f - self._theta)) < 2.0: self._gain = 2.5 self._bias = 0.05 * gps.signum(self._theta_dot_VT_init) elif (gps.normalize(self._psi_p_t_f - self._xp[2]) / \ gps.normalize(self._psi_p_t_f - self._theta)) >= 2.0: self._gain = math.fabs((self._psi_p_t_f - self._xp[2]) / \ (self._psi_p_t_f - self._theta)) self._bias = 0.0 else: # should never get here self.log_warn("Unable to determine phase 2 3-step case") self.set_ready_state(False)
def _offset_state(self): ''' Performs calculations and operations required for the "offset" state ''' if not self._state_initialized: # Set the virtual waypoint D2 meters along the target's path and # offset by D1 meters to the side that the pursuer is currently on theta = gps.gps_bearing(self._xp[0], self._xp[1], \ self._xt[0], self._xt[1]) offset_dir = \ gps.signum(gps.normalize_pi(theta + math.pi - self._xt[2])) tgt_to_VT = self._xt[2] + math.atan2(USMAInterceptor.D1 * offset_dir, \ USMAInterceptor.D2) self._VT = \ gps.gps_newpos(self._xt[0], self._xt[1], tgt_to_VT, \ math.hypot(USMAInterceptor.D1, USMAInterceptor.D2)) self._set_ctrl_params(self._xp, self._vp, \ (self._VT[0], self._VT[1], self._xt[2]), \ (0.0, 0.0)) self._theta_VT_init = self._theta self._theta_dot_VT_init = self._theta_dot self._psi_p_VT_init = self._xp[2] self._psi_p_f = gps.normalize(self._xt[2] + math.pi) psi_p_f_rel = gps.normalize(self._psi_p_f - self._theta_VT_init) if gps.normalize_pi(self._theta_VT_init - self._psi_p_VT_init) > 0.0: psi_p_f_rel = gps.normalize(-psi_p_f_rel) if angle_between(psi_p_f_rel, 0.0, math.pi): self._VT_turn_type = USMAInterceptor.S_TURN self._s_turn_state = USMAInterceptor.S1 elif angle_between(psi_p_f_rel, (self._theta_VT_init - \ self._psi_p_VT_init), 0.0): self._VT_turn_type = USMAInterceptor.DIRECT_TURN else: self._VT_turn_type = USMAInterceptor.DIRECT_2BIAS self._state_initialized = True self._set_ctrl_params(self._xp, self._vp, \ (self._VT[0], self._VT[1], 0.0), (0.0, 0.0)) if self._range < USMAInterceptor.R_CAP_PARALLEL: self._state_initialized = False self._state = USMAInterceptor.PARALLEL_1 self.log_info("Transition to parallel_1 phase") # Determine how to maneuver to the virtual target point # Direct turn to the virtual target if self._VT_turn_type == USMAInterceptor.DIRECT_TURN: self._gain = math.fabs((gps.normalize_pi(self._psi_p_f - \ self._psi_p_VT_init)) / \ (gps.normalize_pi(self._psi_p_f - \ self._theta_VT_init))) self._bias = 0.0 # Single turn to virtual target with 2 gains elif self._VT_turn_type == USMAInterceptor.DIRECT_2BIAS: if math.fabs(gps.normalize_pi(self._psi_p_f - self._xp[2]) / \ gps.normalize_pi(self._psi_p_f - self._theta)) < 2.0: self._gain = gps.normalize_pi(self._psi_p_VT_init) / \ gps.normalize_pi(math.pi/2 + self._theta_VT_init) self._bias = 0.0 else: self._gain = 2.0 self._bias = 0.0 # S-turn to virtual target else: self._s_turn()
def step_autonomy(self, t, dt): # Get our current position own_lat = self._own_pose.pose.pose.position.lat own_lon = self._own_pose.pose.pose.position.lon own_alt = self._own_pose.pose.pose.position.rel_alt # was just alt # Reset the action every loop self._action = ["None", 0] self._target_id = None # Search for the closest target min_dist = MIN_DIST del t_id[:] del t_dist[:] for uav in self._reds.values(): if (uav.vehicle_id not in self._shot) and (uav.vehicle_id not in self._taken): d = gps.gps_distance(own_lat, \ own_lon, \ uav.state.pose.pose.position.lat, \ uav.state.pose.pose.position.lon) if d < min_dist: min_dist = d self._target_id = uav.vehicle_id self._bearing_adj = -(self._fov_width / 2) self._target_range = d # If a target has been identified, move towards it if self._target_id != None: msg = apmsg.MsgStat() msg.id = self._id msg.count = self._target_id msg.latency = self._target_range self.pubs['taken'].publish(msg) print '{} targeting UAV {} at a distance of {}'.format( self._id, self._target_id, self._target_range) print '{} has taken list of {}'.format(self._id, self._taken) target_pos = self._reds[self._target_id].state.pose.pose.position tgt_lat = target_pos.lat tgt_lon = target_pos.lon distance_to_target = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon) # Set bearing to look at the enemy vehicle bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon) # Set waypoint to move forward and strafe sideways once the enemy is within range self._evasion_range = self._weapons_range * 2 self._engagement_range = self._weapons_range if distance_to_target <= self._engagement_range: self._bearing_adj += 2.5 if self._bearing_adj > self._fov_width: self._bearing_adj = -self._fov_width bearing += math.radians( self._bearing_adj ) # bearing_adj in degrees; convert to radians lat, lon = gps.gps_newpos(own_lat, own_lon, gps.normalize(bearing), 0.1) # was - self._fov_width/2 #print "UAV %d in engagement range with enemy %d bearing %f" % (self._id, self._target_id, math.degrees(bearing)) elif distance_to_target <= self._evasion_range: # Check to see if sidestep right waypoint is outside the battlebox # (only need to check distance_to_target/2 since enemy is closing) #print "UAV %d in evasion range with enemy %d" % (self._id, self._target_id) lat_right, lon_right = gps.gps_newpos(own_lat, own_lon, \ gps.normalize(bearing + (math.pi / 3)), distance_to_target/2) if self._bboxes[0].contains(lat_right, lon_right, own_alt): lat, lon = gps.gps_newpos(own_lat, own_lon, \ gps.normalize(bearing + (math.pi / 3)), distance_to_target) #print "UAV %d sidestepping right" % self._id else: # If right sidestep is outside the battlebox, sidestep to the left lat, lon = gps.gps_newpos( own_lat, own_lon, gps.normalize(bearing - (math.pi / 3)), distance_to_target) #print "UAV %d sidestepping left" % self._id else: lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, 100) # was 1000 self._wp = np.array([lat, lon, target_pos.rel_alt ]) # +random.randint(-10,10)] # Determine if the target is within firing parameters if gps.hitable(self._own_pose.pose.pose.position.lat, \ self._own_pose.pose.pose.position.lon, \ self._own_pose.pose.pose.position.rel_alt, \ (self._own_pose.pose.pose.orientation.x, \ self._own_pose.pose.pose.orientation.y, \ self._own_pose.pose.pose.orientation.z, \ self._own_pose.pose.pose.orientation.w), \ self._weapons_range, self._fov_width, self._fov_height, \ target_pos.lat, target_pos.lon, target_pos.rel_alt): self._action = ["Fire", self._target_id] print "=================>ID: %d shot at Target %d" % ( self._id, self._target_id) else: # If a target hasn't been identified, return to the safe waypoint self._wp = self._safe_waypoint self._taken.clear( ) # clear the taken list in case it's full of old data return True
def step_autonomy(self, t, dt): # Reset the action and target ID every loop self._action = ["None", 0] # Search for the closest target every frame for uav in self._reds.values(): if uav.vehicle_id not in self._shot: # Calculate target and own position and orientation target_pos = self._reds[uav.vehicle_id].state.pose.pose.position target_orientation = self._reds[uav.vehicle_id].state.pose.pose.orientation tgt_lat = target_pos.lat tgt_lon = target_pos.lon tgt_alt = target_pos.rel_alt if self._bboxes[0].contains(tgt_lat, tgt_lon, tgt_alt): # added to only consider those in battlebox own_pos = self._own_pose.pose.pose.position own_lat = own_pos.lat own_lon = own_pos.lon own_alt = own_pos.rel_alt own_orientation = self._own_pose.pose.pose.orientation # Calculate bearing to target and from target to self bearing = gps.normalize(gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon)) tgt_bearing = gps.normalize(gps.gps_bearing(tgt_lat, tgt_lon, own_lat, own_lon)) # Calculate heading/yaw of self and target own_rpy = qmath.quat_to_euler((own_orientation.x,\ own_orientation.y, \ own_orientation.z, \ own_orientation.w)) own_heading = gps.normalize(own_rpy[2]) tgt_rpy = qmath.quat_to_euler((target_orientation.x,\ target_orientation.y, \ target_orientation.z, \ target_orientation.w)) tgt_heading = gps.normalize(tgt_rpy[2]) # Calculate distance to target dist = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon) bearing_adj = 0.0 # Calculate offset between bearing and target heading (measures degree of "head on") # ---------------------------------------------------------------------------- heading_diff = own_heading - tgt_heading tgt_bearing_diff = gps.normalize_pi( tgt_heading - tgt_bearing ) if math.fabs(heading_diff) > math.pi/2: # moving towards each other if tgt_bearing > (tgt_heading - self._fov_width_radians/2) or tgt_bearing < (tgt_heading + self._fov_width_radians/2): # we are in the "fan" if dist > 2*self._max_range and math.fabs(tgt_bearing_diff) < self._fov_width_radians/2: bearing_adj = math.copysign(ADJ_ANGLE, tgt_bearing_diff) #print 'ID: {} toward UAV: {}: adjusting bearing by {}'.format(self._id, uav.vehicle_id, math.degrees(bearing_adj)) elif dist < self._max_range and math.fabs(tgt_bearing_diff) > self._fov_width_radians*3: # along side each other bearing_adj = math.copysign(3*ADJ_ANGLE, tgt_bearing_diff) #print 'ID: {} along side UAV: {}: adjusting bearing by {}'.format(self._id, uav.vehicle_id, math.degrees(bearing_adj)) else: # heading in same general direction if math.fabs(tgt_bearing_diff) < math.pi/2: # I am in front of target if dist < 2*self._max_range and math.fabs(tgt_bearing_diff) < self._fov_width_radians/2: bearing_adj = -math.copysign(ADJ_ANGLE, tgt_bearing_diff) #print 'ID: {} away from UAV: {} adjusting bearing by {}'.format(self._id, uav.vehicle_id, math.degrees(bearing_adj)) # ---------------------------------------------------------------------------- lat, lon = gps.gps_newpos(own_lat, own_lon, bearing + bearing_adj, 1000) self._wp = np.array([lat, lon, own_alt]) # keep own alt # Determine if the target is within firing parameters if gps.hitable(own_lat, own_lon, own_alt, \ (own_orientation.x, \ own_orientation.y, \ own_orientation.z, \ own_orientation.w ), \ self._max_range, self._fov_width, self._fov_height,\ tgt_lat, tgt_lon, tgt_alt): self._action = ["Fire", uav.vehicle_id] print "EA=======================>ID: %d shot at Target %d" % (self._id, uav.vehicle_id) return True
def step_autonomy(self, t, dt): # Reset the action every loop self._action = ["None", 0] self._target_id = None own_lat = self._own_pose.pose.pose.position.lat own_lon = self._own_pose.pose.pose.position.lon own_alt = 400 if self._state == States.new_start: self.ordered_corners = self.closestCorners() lat, lon = self.fixed_wing_waypoint(own_lat, own_lon, self.ordered_corners[0][0], self.ordered_corners[0][1]) if gps.gps_distance(own_lat, own_lon, self.ordered_corners[0][0], self.ordered_corners[0][1]) < 50: #print("Starting patrol!") self.ordered_corners = self.closestCorners() self._state = States.patrol self.check_for_supply_route() elif self._state == States.patrol: lat, lon = self.fixed_wing_waypoint( own_lat, own_lon, self.ordered_corners[2][0], self.ordered_corners[2][1]) #At the time of ordering, #3rd closes waypoint is next in path, #1st closest is waypoint that you're currently at, #2nd closest is waypoint you visited previously if gps.gps_distance(own_lat, own_lon, self.ordered_corners[2][0], self.ordered_corners[2][1]) < 25: #print("Reached Waypoint at %f,%f!" % (lat, lon)) self.ordered_corners = self.closestCorners() self.check_for_supply_route() elif self._state == States.kill_supply: lat, lon = self.fixed_wing_waypoint(own_lat, own_lon, self.ordered_corners[1][0], self.ordered_corners[1][1]) if gps.gps_distance(own_lat, own_lon, self.ordered_corners[1][0], self.ordered_corners[1][1]) < 50: #print("Reached supply point at %f,%f!" % (lat, lon)) self.ordered_corners = self.closestCorners() ###################################################################### #If a target is in range, pursue and fire # Reset the action and target ID every loop self._action = ["None", 0] self._target_id = None # Search for the closest target every frame min_dist = self._weapons_range + 30 for uav in self._reds.values(): if uav.vehicle_id not in self._shot: d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\ self._own_pose.pose.pose.position.lon,\ uav.state.pose.pose.position.lat,\ uav.state.pose.pose.position.lon) if d < min_dist: #We also want to constrain our pursuits to enemies in front of us if gps.hitable(self._own_pose.pose.pose.position.lat, \ self._own_pose.pose.pose.position.lon, \ self._own_pose.pose.pose.position.rel_alt, \ (self._own_pose.pose.pose.orientation.x, \ self._own_pose.pose.pose.orientation.y, \ self._own_pose.pose.pose.orientation.z, \ self._own_pose.pose.pose.orientation.w), \ min_dist, self._fov_width*2, self._fov_height, \ uav.state.pose.pose.position.lat, \ uav.state.pose.pose.position.lon, \ uav.state.pose.pose.position.alt): min_dist = d self._target_id = uav.vehicle_id # If a target has been identified, move towards it if self._target_id != None: #print("Target Aquired!") target_pos = self._reds[self._target_id].state.pose.pose.position own_lat = self._own_pose.pose.pose.position.lat own_lon = self._own_pose.pose.pose.position.lon tgt_lat = target_pos.lat tgt_lon = target_pos.lon lat = tgt_lat lon = tgt_lon if self._vehicle_type == enums.AC_FIXED_WING: # When using fixed wing, Set the waypoint past the current # target, so we don't go into a loiter mode bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon) lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, 1000) # Determine if the target is within firing parameters if gps.hitable(self._own_pose.pose.pose.position.lat, \ self._own_pose.pose.pose.position.lon, \ self._own_pose.pose.pose.position.rel_alt, \ (self._own_pose.pose.pose.orientation.x, \ self._own_pose.pose.pose.orientation.y, \ self._own_pose.pose.pose.orientation.z, \ self._own_pose.pose.pose.orientation.w ), \ self._weapons_range, self._fov_width, self._fov_height,\ target_pos.lat, target_pos.lon, target_pos.rel_alt): self._action = ["Fire", self._target_id] ################################################################################# #Safety to make sure you don't fly out of bounds rpy = qmath.quat_to_euler((self._own_pose.pose.pose.orientation.x,\ self._own_pose.pose.pose.orientation.y, \ self._own_pose.pose.pose.orientation.z, \ self._own_pose.pose.pose.orientation.w)) own_bearing = rpy[2] left_lat, left_lon = gps.gps_newpos( own_lat, own_lon, gps.normalize(own_bearing - (math.pi / 2)), 20) right_lat, right_lon = gps.gps_newpos( own_lat, own_lon, gps.normalize(own_bearing + (math.pi / 2)), 20) front_lat, front_lon = gps.gps_newpos( own_lat, own_lon, gps.normalize(own_bearing + (math.pi / 2)), 50) if not self._bboxes[0].contains(left_lat, left_lon, own_alt) or \ not self._bboxes[0].contains(right_lat, right_lon, own_alt) or \ not self._bboxes[0].contains(front_lat, front_lon, own_alt): lat, lon = self.box_center self._wp = np.array([lat, lon, own_alt]) return True