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