def __init__(self,
team_color,
team_side,
robot_total,
enemy_total,
node="default"):
# type: (str, int, int, int, str) -> None
# シングルトンなので2度呼ばれたときに再作成しないようにする
if not self._node_name:
self._node_name = node
rospy.loginfo("initialized Objects: as \"" + self._node_name +
"\"")
self.team_color = team_color
self.team_side = team_side
self.robot_total = robot_total
self.enemy_total = enemy_total
self._robot_ids = range(self.robot_total)
self._enemy_ids = range(self.enemy_total)
self._active_robot_ids = range(self.robot_total)
self._active_enemy_ids = range(self.robot_total)
self.robot = [entity.Robot(id=i) for i in self._robot_ids
] # type: typing.List[entity.Robot]
self.enemy = [entity.Robot(id=i) for i in self._enemy_ids
] # type: typing.List[entity.Robot]
# roles = ["RFW", "LFW", "RDF", "LDF", "GK"]
# for robot, role in zip(self.robot, roles):
# robot.set_role(role)
roles = ["RFW", "LFW", "RDF", "LDF", "GK"]
for robot_id, role in zip(self._robot_ids, roles):
self.get_robot_by_id(robot_id).set_role(role)
self.roles = roles # 保存
rospy.Timer(rospy.Duration(1.0), self.redefine_roles)
self.ball = entity.Ball()
"""---ボール軌道の考慮時間幅(linear Regressionで軌道予測するため)---"""
self.ball_dynamics_window = 5
self.ball_dynamics = [[0., 0.]
for i in range(self.ball_dynamics_window)]
self.odom_listener()
self.existing_listener()
else:
rospy.loginfo("argument of Objects: \"node=" + node +
"\" not used because already initialized as \"" +
self._node_name + "\".")
def __init__(self, team_color, robot_total, enemy_total):
self.team_color = team_color
self.robot_total = robot_total
self.enemy_total = enemy_total
self.robot = [entity.Robot() for i in range(self.robot_total)]
self.enemy = [entity.Robot() for i in range(self.enemy_total)]
self.ball = entity.Ball()
"""---ボール軌道の考慮時間幅(linear Regressionで軌道予測するため)---"""
self.ball_dynamics_window = 5
self.ball_dynamics = [[0., 0.]
for i in range(self.ball_dynamics_window)]
self.odom_listener()
def __init__(self, robot_num,friend_num, enemy_num):
self.robot_r = 0.09 #定数はconfigからとってくるように書きたい
self.robot_num = int(robot_num)
self.friend_num = friend_num
self.enemy_num = enemy_num
self.friend = [entity.Robot() for i in range(int(self.friend_num))]
self.enemy = [entity.Robot() for i in range(int(self.enemy_num))]
self.current_x = 0
self.current_y = 0
self.current_theta = 0
self.pid_goal_pos_x = 0
self.pid_goal_pos_y = 0
self.pid_goal_theta = 0
self.pass_target_pos_x = 0
self.pass_target_pos_y = 0
def __init__(self):
rospy.init_node("world_model")
self.team_color = str(rospy.get_param("team_color"))
"""---devisionの選択---"""
self.devision = "A"
self.devision = "B"
"""---味方ロボット台数と敵ロボット台数---"""
self.robot_num = 4
self.enemy_num = 8
"""---フィールドとロボットのパラメータ---"""
self.robot_r = 0.09
if self.devision == "A":
self.field_x_size = 12.
self.field_y_size = 9.
elif self.devision == "B":
self.field_x_size = 9.
self.field_y_size = 6.
self.field_x_min = -self.field_x_size / 2.
self.field_x_max = self.field_x_size / 2.
self.field_y_min = -self.field_y_size / 2.
self.field_y_max = self.field_y_size / 2.
self.goal_y_size = 1.2
self.goal_y_min = -self.goal_y_size / 2.
self.goal_y_max = self.goal_y_size / 2.
"""---team cloorと攻撃方向---"""
#self.color = "Yellow"
self.color = "Blue"
self.goal_direction = "Right"
#self.goal_direction = "Left"
"""---PID制御用パラメータ---"""
self.Kpv = 0.3
self.Kpr = 0.1
"""---Potential法用パラメータ---"""
self.Kpv_2 = 0.5
self.Kpr_2 = 0.1
"""---refereeからの指示をもとにどのループを回すかの指標---"""
self.referee_branch = None
"""---ボール軌道の考慮時間幅(linear Regressionで軌道予測するため)---"""
self.ball_dynamics_window = 5
self.ball_dynamics_x = [0. for i in range(self.ball_dynamics_window)]
self.ball_dynamics_y = [0. for i in range(self.ball_dynamics_window)]
self.goal_keeper_x = self.field_x_min + 0.05
"""---robot、ballオブジェクトのインスタンス---"""
self.robot = [entity.Robot() for i in range(self.robot_num)]
self.enemy = [entity.Robot() for i in range(self.enemy_num)]
self.ball = entity.Ball()
"""---Publisherオブジェクトのインスタンス---"""
self.robot_0_pub = rospy.Publisher("/" + self.team_color +
"/robot_0/robot_commands",
robot_commands,
queue_size=10)
self.robot_1_pub = rospy.Publisher("/" + self.team_color +
"/robot_1/robot_commands",
robot_commands,
queue_size=10)
self.robot_2_pub = rospy.Publisher("/" + self.team_color +
"/robot_2/robot_commands",
robot_commands,
queue_size=10)
self.robot_3_pub = rospy.Publisher("/" + self.team_color +
"/robot_3/robot_commands",
robot_commands,
queue_size=10)
"""
self.robot_4_pub = rospy.Publisher("/robot_4/robot_commands", robot_commands, queue_size=10)
self.robot_5_pub = rospy.Publisher("/robot_5/robot_commands", robot_commands, queue_size=10)
self.robot_6_pub = rospy.Publisher("/robot_6/robot_commands", robot_commands, queue_size=10)
self.robot_7_pub = rospy.Publisher("/robot_7/robot_commands", robot_commands, queue_size=10)
"""
"""---status Publisherオブジェクトのインスタンス---"""
self.robot_0_status_pub = rospy.Publisher("/" + self.team_color +
"/robot_0/status",
Status,
queue_size=10)
self.robot_1_status_pub = rospy.Publisher("/" + self.team_color +
"/robot_1/status",
Status,
queue_size=10)
self.robot_2_status_pub = rospy.Publisher("/" + self.team_color +
"/robot_2/status",
Status,
queue_size=10)
self.robot_3_status_pub = rospy.Publisher("/" + self.team_color +
"/robot_3/status",
Status,
queue_size=10)
self.robot_4_status_pub = rospy.Publisher("/" + self.team_color +
"/robot_4/status",
Status,
queue_size=10)
self.robot_5_status_pub = rospy.Publisher("/" + self.team_color +
"/robot_5/status",
Status,
queue_size=10)
self.robot_6_status_pub = rospy.Publisher("/" + self.team_color +
"/robot_6/status",
Status,
queue_size=10)
self.robot_7_status_pub = rospy.Publisher("/" + self.team_color +
"/robot_7/status",
Status,
queue_size=10)
"""pid用のflag初期化"""
for id in range(self.robot_num):
self.robot[id].pid_init_flag = True
"""cmdの立ち上げ"""
self.cmd = [robot_commands() for i in range(self.robot_num)]
"""statusの立ち上げ"""
self.status = [Status() for i in range(self.robot_num)]
"""cmdのパラメータ初期化"""
for id in range(self.robot_num):
self.cmd[id].vel_surge = 0. #vel_x
self.cmd[id].vel_sway = 0. #vel_y
self.cmd[id].omega = 0. #vel_theta
self.cmd[id].kick_speed_x = 0
self.cmd[id].kick_speed_z = 0
self.cmd[id].dribble_power = 0
"""statusのパラメータ初期化"""
for id in range(self.robot_num):
self.status[id].status = "None"
"""positionの割り振り(動的にPositionが切り替わるのは多分大事、現状使っていない)"""
"""
if self.robot_num == 8:
self.robot[0].position = "GK"
self.robot[1].position = "LCB"
self.robot[2].position = "RCB"
self.robot[3].position = "LSB"
self.robot[4].position = "RSB"
self.robot[5].position = "LMF"
self.robot[6].position = "RMF"
elif self.robot_num == 4:
self.robot[0].position = "GK"
self.robot[1].position = "LCB"
self.robot[2].position = "CCB"
self.robot[3].position = "RCB"
"""
"""publishのrate(多分rospyのを使うからいらない)"""
self.publish_rate = 0.05 #s #
"""strategyの選択(いまはつかっていない)"""
self.strategy = None
"""refereeから受信する情報(現状cmdしか使っていない)"""
self.referee = None
"""---敵と味方どっちがボールをもっているか(現状使ってない)---"""
self.which_has_a_ball = None
"""---攻撃かdefenceか(これをうまく使っていきたい)---"""
self.attack_or_defence = None
"""---ポテンシャル法用のパラメータ（1/x）---"""
self.weight_enemy = 1.0
self.weight_goal = 100.0
self.delta = 0.0000001
"""---ポテンシャル法用のパラメータ（ガウス関数)---"""
self.weight_enemy_2 = 7.0
self.weight_goal_2 = 20000000.0
self.width_enemy = 0.1
self.width_goal = 0.1
self.delta_2 = 0.00001
def __init__(self, robot_num, enemy_num):
self.robot_num = robot_num
self.enemy_num = enemy_num
self.robot = [entity.Robot() for i in range(robot_num)]
self.enemy = [entity.Robot() for i in range(enemy_num)]
self.ball = entity.Ball()
self.field_x_size = 12000.
self.field_y_size = 9000.
self.field_x_min = - self.field_x_size / 2.
self.field_x_max = self.field_x_size / 2.
self.field_y_min = - self.field_y_size / 2.
self.field_y_max = self.field_y_size / 2.
self.goal_y_size = 1200.
self.goal_y_min = -self.goal_y_size / 2.
self.goal_y_max = self.goal_y_size / 2.
self.ball_dynamics_window = 10
self.ball_dynamics_x = [0. for i in range(self.ball_dynamics_window)]
self.ball_dynamics_y = [0. for i in range(self.ball_dynamics_window)]
self.goal_keeper_x = self.field_x_min + 20
# position
if self.robot_num == 8:
self.robot[0].position = "GK"
self.robot[1].position = "LCB"
self.robot[2].position = "RCB"
self.robot[3].position = "LSB"
self.robot[4].position = "RSB"
self.robot[5].position = "LMF"
self.robot[6].position = "RMF"
elif self.robot_num == 4:
self.robot[0].position = "GK"
self.robot[1].position = "LCB"
self.robot[2].position = "CCB"
self.robot[3].position = "RCB"
elif self.robot_num == 5:
self.robot[0].position = "GK"
self.robot[1].position = "LCB"
self.robot[2].position = "CCB"
self.robot[3].position = "RCB"
self.robot[4].position = "CF"
self.rate = 0.05 #s
self.strategy = None
self.referee = None
self.which_has_a_ball = None
self.attack_or_defence = None
# weight for potential
self.weight_enemy = 1.0
self.weight_goal = 5.0
self.delta = 0.1
self.speed = self.robot[0].max_velocity
self.counter = 0
def __init__(self):
rospy.init_node("world_model")
self.team_color = str(rospy.get_param("~team_color"))
self.robot_total = 8
self.enemy_total = 8
"""---robot、ballオブジェクトのインスタンス---"""
self.robot = [entity.Robot() for i in range(self.robot_total)]
self.enemy = [entity.Robot() for i in range(self.enemy_total)]
self.ball = entity.Ball()
"""statusの立ち上げ msg"""
self.status = [Status() for i in range(self.robot_total)]
"""statusのパラメータ初期化"""
for id in range(self.robot_total):
self.status[id].status = "None"
self.status[id].pid_goal_pos_x = 0.
self.status[id].pid_goal_pos_y = 0.
self.status[id].pid_goal_theta = 0.
self.status[id].pid_circle_center_x = 0.
self.status[id].pid_circle_center_y = 0.
self.status[id].pass_target_pos_x = 0.
self.status[id].pass_target_pos_y = 0.
#print(self.status)
"""----上の5つの変数、インスタンスをまとめたもの、callbackをもつ---"""
self.objects = Objects(self.robot_total, self.enemy_total, self.robot,
self.enemy, self.ball)
"""---World State(固定パラが多い)---"""
self.world_state = WorldState(self.objects)
"""---Referee---"""
self.referee = Referee(self.world_state)
"""---DecisionMaker---"""
self.decision_maker = DecisionMaker(self.world_state, self.objects,
self.referee, self.status)
"""---status Publisherオブジェクトのインスタンス---"""
self.robot_0_status_pub = rospy.Publisher("/" + self.team_color +
"/robot_0/status",
Status,
queue_size=10)
self.robot_1_status_pub = rospy.Publisher("/" + self.team_color +
"/robot_1/status",
Status,
queue_size=10)
self.robot_2_status_pub = rospy.Publisher("/" + self.team_color +
"/robot_2/status",
Status,
queue_size=10)
self.robot_3_status_pub = rospy.Publisher("/" + self.team_color +
"/robot_3/status",
Status,
queue_size=10)
self.robot_4_status_pub = rospy.Publisher("/" + self.team_color +
"/robot_4/status",
Status,
queue_size=10)
self.robot_5_status_pub = rospy.Publisher("/" + self.team_color +
"/robot_5/status",
Status,
queue_size=10)
self.robot_6_status_pub = rospy.Publisher("/" + self.team_color +
"/robot_6/status",
Status,
queue_size=10)
self.robot_7_status_pub = rospy.Publisher("/" + self.team_color +
"/robot_7/status",
Status,
queue_size=10)