def __init__(self): # create object for utility functions self.uti = hp.Utils() # number of agents, ROS parameters self.Nd = rospy.get_param('N_defenders', 3) self.Ne = rospy.get_param('N_attackers', 2) self.nBase = rospy.get_param('nBase', 1) self.Base = rospy.get_param('Base', [1]) # grid size self.sector_size = rospy.get_param('sector_size', [5.0, 5.0]) self.grid_size = rospy.get_param('grid_size', [10, 10]) self.origin_shifts = rospy.get_param('origin_shifts', [0.0, 0.0]) # Simple 2D simulations, No Gazebo self.sim_2D = rospy.get_param('sim_2D', False) # capture distance, [m] self.cap_dist = rospy.get_param('capture_distance', 1.0) # compute sector size """ if (self.use_grid_corners): self.sector_size = [1,1] self.sector_size[0] = self.uti.grid_side_length / self.grid_size[0] self.sector_size[1] = self.sector_size[0] l = self.sector_size[0] self.cap_dist = sqrt(2*(l**2)) + 0.5 # 0.5 is extra margin for outdoor GPS uncertainty """ # game time, [sec] self.Tgame = 120 dstr = '/defender' estr = '/attacker' self.d_pose_topic_names = [] self.d_dlp_state_topics = [] for i in range(self.Nd): self.d_pose_topic_names.append(dstr + str(i + 1) + '/local_pose') self.d_dlp_state_topics.append(dstr + str(i + 1) + '/dlp_state') self.e_pose_topic_names = [] for i in range(self.Ne): self.e_pose_topic_names.append(estr + str(i + 1) + '/local_pose') # msgs self.d_msg = DefendersState() self.e_msg = EnemyState() self.master_msg = MasterCommand() # msg initialization self.d_msg.header.stamp = rospy.Time.now() self.d_msg.defenders_count = self.Nd #self.d_msg.defenders_position = [] #self.d_msg.defenders_sectors = [] p = Point32(0.0, 0.0, 0.0) for i in range(self.Nd): self.d_msg.defenders_position.append(p) self.d_msg.defenders_sectors.append(i) self.d_msg.defenders_next_sectors.append(i) self.d_msg.successful_prediction_rate.append(0.0) self.d_msg.connectivity_rate.append(0.0) self.e_msg.header.stamp = rospy.Time.now() self.e_msg.enemy_count = self.Ne self.e_msg.enemy_position = [] self.e_msg.enemy_sectors = [] self.e_msg.is_captured = [] for i in range(self.Ne): self.e_msg.enemy_position.append(p) self.e_msg.enemy_sectors.append(i + self.Nd) self.e_msg.is_captured.append(False) self.master_msg.allCaptured = False self.master_msg.gameEnd = False # Defenders predictions about their neighbors # List of lists self.d_predictions = [] # initialization x = [0] * self.Nd for i in range(self.Nd): self.d_predictions.append(x) # loop rate self.rate = rospy.Rate(50)
def __init__(self): # create object for utility functions self.uti = hp.Utils() # number of agents, ROS parameters self.Nd = rospy.get_param('N_defenders', 3) self.Ne = rospy.get_param('N_attackers', 2) self.nBase = rospy.get_param('nBase', 1) self.Base = rospy.get_param('Base', [1]) self.use_grid_corners = rospy.get_param('use_grid_corners', False) # grid size self.grid_size = rospy.get_param('grid_size', [10, 10]) # enforce square grid if (self.use_grid_corners): self.grid_size[1] = self.grid_size[0] # Whether to use Gazebo or not self.use_sim = rospy.get_param('use_sim', False) # capture distance, [m] self.cap_dist = rospy.get_param('capture_distance', 1.0) # GPS coords of grid zero position self.lat0 = rospy.get_param('lat0', 47.397742) self.lon0 = rospy.get_param('long0', 8.5455933) # GPS coordinates for East poitn w.r.t zero GPS point lat0/lon0 self.PE = rospy.get_param('grid_corner_PE', [1.0, 2.0]) self.uti.Po_lat = self.lat0 self.uti.Po_long = self.lon0 self.uti.PE_lat = self.PE[0] self.uti.PE_long = self.PE[1] # compute local rotation self.uti.compute_local_rot() # compute grid side lenght, assuming square grid self.uti.compute_grid_side_length() # compute sector size if (self.use_grid_corners): self.sector_size = [1, 1] self.sector_size[0] = self.uti.grid_side_length / self.grid_size[0] self.sector_size[1] = self.sector_size[0] l = self.sector_size[0] self.cap_dist = sqrt(2 * ( l**2)) + 0.5 # 0.5 is extra margin for outdoor GPS uncertainty # game time, [sec] self.Tgame = 120 self.outdoor = rospy.get_param('outdoor', False) self.d_gps_topic_names = [] for i in range(self.Nd): if self.outdoor: dstr = '/defender_' estr = '/attacker_' else: dstr = '/defender' estr = '/attacker' self.d_gps_topic_names.append(dstr + str(i + 1) + '/mavros/global_position/raw/fix') self.e_gps_topic_names = [] for i in range(self.Ne): self.e_gps_topic_names.append(estr + str(i + 1) + '/mavros/global_position/raw/fix') # msgs self.d_msg = DefendersState() self.e_msg = EnemyState() self.master_msg = MasterCommand() # msg initialization self.d_msg.header.stamp = rospy.Time.now() self.d_msg.defenders_count = self.Nd #self.d_msg.defenders_position = [] #self.d_msg.defenders_sectors = [] p = Point32(0.0, 0.0, 0.0) for i in range(self.Nd): self.d_msg.defenders_position.append(p) self.d_msg.defenders_sectors.append(i) self.e_msg.header.stamp = rospy.Time.now() self.e_msg.enemy_count = self.Ne self.e_msg.enemy_position = [] self.e_msg.enemy_sectors = [] self.e_msg.is_captured = [] for i in range(self.Ne): self.e_msg.enemy_position.append(p) self.e_msg.enemy_sectors.append(i + self.Nd) self.e_msg.is_captured.append(False) self.master_msg.allCaptured = False self.master_msg.gameEnd = False # loop rate self.rate = rospy.Rate(50)
def __init__(self): # create object for utility functions self.uti = hp.Utils() # whether to set offboard mode in code or through transmitter self.set_offb_b = rospy.get_param('set_offboard_in_code', True) self.use_grid_corners = rospy.get_param('use_grid_corners', False) # grid size self.grid_size = rospy.get_param('grid_size', [10, 10]) # enforce square grid if (self.use_grid_corners): self.grid_size[1] = self.grid_size[0] # msgs self.d_msg = DefendersState() self.e_msg = EnemyState() self.master_msg = MasterCommand() self.dlp_msg = DlpState() self.local_pose = Point(0.0, 0.0, 0.0) self.joy_msg = Joy() self.joy_msg.axes = [0.0, 0.0, 0.0] # step size multiplies joystick input self.joy_FACTOR = rospy.get_param('joystick_factor', 2.0) # my GPS coords self.my_lat = 0.0 self.my_lon = 0.0 # my current grid position (not mavros local_position) self.grid_pos_x = 0.0 self.grid_pos_y = 0.0 # gps coords of zero position in grid self.lat0 = rospy.get_param('lat0', 47.397742) self.lon0 = rospy.get_param('long0', 8.5455933) # GPS coordinates for East poitn w.r.t zero GPS point lat0/lon0 self.PE = rospy.get_param('grid_corner_PE', [1.0, 2.0]) self.uti.Po_lat = self.lat0 self.uti.Po_long = self.lon0 self.uti.PE_lat = self.PE[0] self.uti.PE_long = self.PE[1] # safety margin from fence boundaries; to account for GPS errors self.safety_margin = 0.1 # compute local rotation self.uti.compute_local_rot() # compute grid side lenght, assuming square grid self.uti.compute_grid_side_length() # compute sector size if (self.use_grid_corners): self.sector_size = [1, 1] self.sector_size[0] = self.uti.grid_side_length / self.grid_size[0] self.sector_size[1] = self.sector_size[0] # is gps used self.use_gps = rospy.get_param('use_gps', False) # Fence params self.fence_x_min = rospy.get_param('fence_min_x', 0.0) self.fence_x_max = rospy.get_param('fence_max_x', 5.0) self.fence_y_min = rospy.get_param('fence_min_y', 0.0) self.fence_y_max = rospy.get_param('fence_max_y', 5.0) if (self.use_grid_corners): self.fence_x_min = 0.0 + self.safety_margin * self.uti.grid_side_length self.fence_x_max = self.uti.grid_side_length - self.safety_margin * self.uti.grid_side_length self.fence_y_min = 0.0 self.fence_y_max = self.uti.grid_side_length - self.safety_margin * self.uti.grid_side_length # flags self.home_flag = False self.takeoff_flag = False self.land_flag = False self.arm_flag = False self.disarm_flag = False self.battle_flag = False self.iamCaptured = False self.master_msg.defender_cmd = 'battle' self.master_msg.attacker_cmd = 'battle' self.master_msg.allCaptured = False self.master_msg.gameEnd = False # agent ID self.my_id = 0 # Instantiate a setpoint topic structure self.setp = PositionTarget() # use position setpoints self.setp.type_mask = int('101111111000', 2) # get altitude setpoint from parameters self.altSp = rospy.get_param('altitude_setpoint') self.setp.position.z = self.altSp