def calculate_defense(self, strategy_context=None):
# type: (StrategyContext) -> StrategyBase
active_robot_ids = self._get_active_robot_ids()
active_enemy_ids = self._get_active_enemy_ids()
status = Status()
nearest_enemy_id = self._objects.get_enemy_ids_sorted_by_distance_to_ball(
active_enemy_ids)[0]
for robot_id in active_robot_ids:
robot = self._objects.get_robot_by_id(robot_id)
if robot.get_role() == "GK":
status.status = "keeper"
elif robot.get_role() == "LDF":
status.status = "defence4"
elif robot.get_role() == "RDF":
status.status = "defence3"
elif robot.get_role() == "LFW":
#敵kickerとballの延長線上に移動
status.status = "move_linear"
if nearest_enemy_id != None:
status.pid_goal_pos_x, status.pid_goal_pos_y = functions.calculate_internal_dividing_point(
self._enemy[nearest_enemy_id].get_current_position()
[0],
self._enemy[nearest_enemy_id].get_current_position()
[1],
self._ball_params.get_current_position()[0],
self._ball_params.get_current_position()[1],
functions.distance_btw_two_points(
self._enemy[nearest_enemy_id].get_current_position(
), self._ball_params.get_current_position()) +
0.55, -0.55)
status.pid_goal_theta = math.atan2(
(self._ball_params.get_current_position()[1] -
self._robot[3].get_current_position()[1]),
(self._ball_params.get_current_position()[0] -
self._robot[2].get_current_position()[0]))
elif robot.get_role() == "RFW":
#フリーで最もゴールに近い敵idを返す
status.status = "move_linear"
free_enemy_id = self._get_free_enemy_id(4, nearest_enemy_id)
status.pid_goal_pos_x = (
self._ball_params.get_current_position()[0] +
self._enemy[free_enemy_id].get_current_position()[0]) / 2
status.pid_goal_pos_y = (
self._ball_params.get_current_position()[1] +
self._enemy[free_enemy_id].get_current_position()[1]) / 2
status.pid_goal_theta = math.atan2(
(self._ball_params.get_current_position()[1] -
self._robot[4].get_current_position()[1]),
(self._ball_params.get_current_position()[0] -
self._robot[4].get_current_position()[0]))
else:
status.status = "none"
self._dynamic_strategy.set_robot_status(robot_id, status)
result = self._dynamic_strategy
return result
def calcurate(self, strategy_context=None, referee_branch="KICKOFF_DEFENCE"):
# type: (StrategyContext) -> StrategyBase
if referee_branch == "NORMAL_START":
if self._detect_enemy_kick(strategy_context):
strategy_context.update("enemy_kick", True, namespace="world_model")
strategy_context.update("defence_or_attack", False, namespace="world_model")
else:
strategy_context.update("placed_ball_position", self._ball_params.get_current_position(), namespace="world_model")
active_robot_ids = self._get_active_robot_ids()
active_enemy_ids = self._get_active_enemy_ids()
status = Status()
nearest_enemy_id = self._objects.get_enemy_ids_sorted_by_distance_to_ball(active_enemy_ids)[0]
for idx, robot_id in enumerate(active_robot_ids):
robot = self._objects.get_robot_by_id(robot_id)
if robot.get_role() == "GK":
status.status = "keeper"
elif robot.get_role() == "LDF":
status.status = "defence4"
elif robot.get_role() == "RDF":
status.status = "defence3"
elif robot.get_role() == "LFW":
#敵kickerとballの延長線上に移動
status.status = "move_linear"
if nearest_enemy_id != None:
status.pid_goal_pos_x, status.pid_goal_pos_y = functions.calculate_internal_dividing_point(
self._objects.get_enemy_by_id(nearest_enemy_id).get_current_position()[0],
self._objects.get_enemy_by_id(nearest_enemy_id).get_current_position()[1],
self._ball_params.get_current_position()[0],
self._ball_params.get_current_position()[1],
functions.distance_btw_two_points(self._objects.get_enemy_by_id(nearest_enemy_id).get_current_position(),
self._ball_params.get_current_position()) + 0.8,
-0.8)
status.pid_goal_theta = math.atan2((self._ball_params.get_current_position()[1] - robot.get_current_position()[1]) , (self._ball_params.get_current_position()[0] - robot.get_current_position()[0]))
if status.pid_goal_pos_x >= -0.2 and status.pid_goal_pos_y >= 0:
status.pid_goal_pos_x = -0.2
status.pid_goal_pos_y = 0.8
elif status.pid_goal_pos_x >= -0.2 and status.pid_goal_pos_y < 0:
status.pid_goal_pos_x = -0.2
status.pid_goal_pos_y = -0.8
elif robot.get_role() == "RFW":
#固定位置
status.status = "move_linear"
free_enemy_id = self._get_free_enemy_id(4, nearest_enemy_id)
status.pid_goal_pos_x = -3.
status.pid_goal_pos_y = 0.
status.pid_goal_theta = math.atan2( (self._ball_params.get_current_position()[1] - robot.get_current_position()[1]) , (self._ball_params.get_current_position()[0] - robot.get_current_position()[0]) )
else:
status.status = "none"
self._dynamic_strategy.set_robot_status(robot_id, status)
result = self._dynamic_strategy
return result
def calcurate(self, strategy_context=None, referee_branch="PENALTY_DEFENCE"):
# type: (StrategyContext) -> StrategyBase
if referee_branch == "NORMAL_START":
if self._detect_enemy_kick(strategy_context):
strategy_context.update("enemy_kick", True, namespace="world_model")
strategy_context.update("defence_or_attack", False, namespace="world_model")
else:
strategy_context.update("placed_ball_position", self._ball_params.get_current_position(), namespace="world_model")
active_robot_ids = self._get_active_robot_ids()
active_enemy_ids = self._get_active_enemy_ids()
nearest_enemy_id = self._objects.get_enemy_ids_sorted_by_distance_to_ball(active_enemy_ids)[0]
for robot_id in active_robot_ids:
status = Status()
robot = self._objects.get_robot_by_id(robot_id)
if robot.get_role() == "GK":
status.status = "keeper"
elif robot.get_role() == "LDF":
#固定値へ移動
status.status = "move_linear"
status.pid_goal_pos_x = -4.0
status.pid_goal_pos_y = 4.0
status.pid_goal_theta = math.atan2((self._ball_params.get_current_position()[1] - robot.get_current_position()[1]) , (self._ball_params.get_current_position()[0] - robot.get_current_position()[0]))
elif robot.get_role() == "RDF":
#固定値へ移動
status.status = "move_linear"
status.pid_goal_pos_x = -3.5
status.pid_goal_pos_y = 4.0
status.pid_goal_theta = math.atan2((self._ball_params.get_current_position()[1] - robot.get_current_position()[1]) , (self._ball_params.get_current_position()[0] - robot.get_current_position()[0]))
elif robot.get_role() == "LFW":
#固定値へ移動
status.status = "move_linear"
status.pid_goal_pos_x = -3.0
status.pid_goal_pos_y = 4.0
status.pid_goal_theta = math.atan2((self._ball_params.get_current_position()[1] - robot.get_current_position()[1]) , (self._ball_params.get_current_position()[0] - robot.get_current_position()[0]))
elif robot.get_role() == "RFW":
#固定値へ移動
status.status = "move_linear"
status.pid_goal_pos_x = -2.5
status.pid_goal_pos_y = 4.0
status.pid_goal_theta = math.atan2((self._ball_params.get_current_position()[1] - robot.get_current_position()[1]) , (self._ball_params.get_current_position()[0] - robot.get_current_position()[0]))
else:
status.status = "none"
self._dynamic_strategy.set_robot_status(robot_id, status)
result = self._dynamic_strategy
return result
def calcurate(self, strategy_context=None):
# type: (StrategyContext) -> StrategyBase
if self._detect_enemy_kick(strategy_context):
strategy_context.update("enemy_kick", True, namespace="world_model")
strategy_context.update("defence_or_attack", False, namespace="world_model")
active_robot_ids = self._get_active_robot_ids()
active_enemy_ids = self._get_active_enemy_ids()
nearest_enemy_id = self._objects.get_enemy_ids_sorted_by_distance_to_ball(active_enemy_ids)[0]
second_nearest_enemy_id = self._objects.get_enemy_ids_sorted_by_distance_to_ball(active_enemy_ids)[1]
for robot_id in active_robot_ids:
status = Status()
robot = self._objects.get_robot_by_id(robot_id)
if robot.get_role() == "GK":
status.status = "keeper"
elif robot.get_role() == "LDF":
status.status = "defence4"
elif robot.get_role() == "RDF":
status.status = "defence3"
elif robot.get_role() == "LFW":
#敵kickerとballの延長線上に移動
status.status = "move_linear"
# if second_nearest_enemy_id != None:
if True:
# status.pid_goal_pos_x, status.pid_goal_pos_y = functions.calculate_internal_dividing_point(self._enemy[nearest_enemy_id].get_current_position()[0], self._enemy[nearest_enemy_id].get_current_position()[1], self._ball_params.get_current_position()[0], self._ball_params.get_current_position()[1], functions.distance_btw_two_points(self._enemy[nearest_enemy_id].get_current_position(), self._ball_params.get_current_position()) + 0.55, -0.55)
target_pos_xy = functions.calculate_internal_dividing_point_vector_args(config.GOAL_CENTER, self._ball_params.get_current_position(), 1, 1)
status.pid_goal_pos_x = target_pos_xy[0]
status.pid_goal_pos_y = target_pos_xy[1]
status.pid_goal_theta = math.atan2( (self._ball_params.get_current_position()[1] - robot.get_current_position()[1]) , (self._ball_params.get_current_position()[0] - robot.get_current_position()[0]) )
elif robot.get_role() == "RFW":
#フリーで最もゴールに近い敵idを返す
status.status = "move_linear"
free_enemy_id = self._get_free_enemy_id(4, nearest_enemy_id)
status.pid_goal_pos_x = (self._ball_params.get_current_position()[0] + self._enemy[free_enemy_id].get_current_position()[0]) / 2
status.pid_goal_pos_y = (self._ball_params.get_current_position()[1] + self._enemy[free_enemy_id].get_current_position()[1]) / 2
status.pid_goal_theta = math.atan2( (self._ball_params.get_current_position()[1] - robot.get_current_position()[1]) , (self._ball_params.get_current_position()[0] - robot.get_current_position()[0]) )
else:
status.status = "none"
self._dynamic_strategy.set_robot_status(robot_id, status)
result = self._dynamic_strategy
return result
def _get_default_status():
# type: () -> Status
status = Status()
status.status = "None"
status.pid_goal_pos_x = 0.
status.pid_goal_pos_y = 0.
status.pid_goal_theta = 0.
status.pid_circle_center_x = 0.
status.pid_circle_center_y = 0.
status.pass_target_pos_x = 0.
status.pass_target_pos_y = 0.
return status
def _defence_strategy(self, strategy_context=None):
#ball取得時、攻撃へ移行
if self._get_who_has_a_ball() == "robots":
strategy_context.update("defence_or_attack", True, namespace="world_model")
active_robot_ids = self._get_active_robot_ids()
active_enemy_ids = self._get_active_enemy_ids()
status = Status()
nearest_enemy_id = self._objects.get_enemy_ids_sorted_by_distance_to_ball(active_enemy_ids)[0]
for idx, robot_id in enumerate(active_robot_ids):
robot = self._objects.get_robot_by_id(robot_id)
if robot.get_role() == "GK":
status.status = "keeper"
elif robot.get_role() == "LDF":
status.status = "defence4"
elif robot.get_role() == "RDF":
status.status = "defence3"
elif robot.get_role() == "LFW":
#ballの位置に移動
status.status = "move_linear"
status.pid_goal_pos_x = self._ball_params.get_current_position()[0]
status.pid_goal_pos_y = self._ball_params.get_current_position()[1]
status.pid_goal_theta = math.atan2( (self._ball_params.get_current_position()[1] - robot.get_current_position()[1]) , (self._ball_params.get_current_position()[0] - robot.get_current_position()[0]) )
#ボールとフリーの機体の中点へ移動
elif robot.get_role() == "RFW":
#フリーで最もゴールに近い敵idを返す
status.status = "move_linear"
free_enemy_id = self._get_free_enemy_id(4, nearest_enemy_id)
status.pid_goal_pos_x = (self._ball_params.get_current_position()[0] + self._enemy[free_enemy_id].get_current_position()[0]) / 2
status.pid_goal_pos_y = (self._ball_params.get_current_position()[1] + self._enemy[free_enemy_id].get_current_position()[1]) / 2
status.pid_goal_theta = math.atan2( (self._ball_params.get_current_position()[1] - robot.get_current_position()[1]) , (self._ball_params.get_current_position()[0] - robot.get_current_position()[0]) )
else:
status.status = "none"
self._dynamic_strategy.set_robot_status(robot_id, status)
result = self._dynamic_strategy
return result
def main(self, Kpv=None, Kpr=None, Kdv=None, Kdr=None):
robot_id = str(4)
robot_color = 'blue'
status_topic_name = '/'+robot_color+'/robot_'+robot_id+'/status'
self.status_topic_publisher = rospy.Publisher(
status_topic_name, Status, queue_size=1)
pid_service_name = '/'+robot_color+'/robot_'+robot_id+'/set_pid'
self.pid_service_proxy = rospy.ServiceProxy(pid_service_name, pid)
rospy.wait_for_service(pid_service_name)
self.robot = {}
rospy.wait_for_message(
'/'+robot_color+'/robot_'+robot_id+'/odom', Odometry)
self.odom_listener = rospy.Subscriber(
'/'+robot_color+'/robot_'+robot_id+'/odom', Odometry, self.odom_listener_callback)
rospy.wait_for_message(
'/'+robot_color+'/robot_'+robot_id+'/odom', Odometry)
# {'Kdv': 5.436618301696918, 'Kpv': 3.111774552866889}
if Kpv is None:
Kpv = 3.111774552866889
if Kpr is None:
Kpr = 2.2
if Kdv is None:
Kdv = 5.436618301696918
if Kdr is None:
Kdr = 1.0
# kdr 4.78 | 5.037
next_param = {
'Kpv': Kpv,
'Kpr': Kpr,
'Kdv': Kdv,
'Kdr': Kdr,
}
status = Status()
status.status = "move_linear"
status.pid_goal_pos_x = 4.5
status.pid_goal_pos_y = 1.0
status.pid_goal_theta = 0.0
self.pid_service_proxy(2.0,
2.0,
2.0,
2.0)
self.publish(status)
rospy.sleep(3.0)
self.pid_service_proxy(next_param['Kpv'],
next_param['Kpr'],
next_param['Kdv'],
next_param['Kdr'])
loop_rate = rospy.Rate(config.ROBOT_LOOP_RATE)
total_elapsed_time = 0.0
# x, y, theta
global mode
if mode == "position":
positions = [
[4.5, 0.0, 0.3 * np.pi],
[4.5, 2.0, 0.9 * np.pi],
# [0.0, -1.0, 1.8 * np.pi],
# [0.0, 2.0, 0.6 * np.pi],
]
else:
positions = [
[4.5, 1.0, 0.3 * np.pi],
[4.5, 1.0, 0.9 * np.pi],
[4.5, 1.0, 1.8 * np.pi],
[4.5, 1.0, 0.6 * np.pi],
]
total_average_pos = 0.0
total_average_rot = 0.0
for pos in positions:
status.pid_goal_pos_x = pos[0]
status.pid_goal_pos_y = pos[1]
status.pid_goal_theta = pos[2]
# print(str(status))
list_length = 60
hantei_list_pos = [10.0 for _ in range(list_length)]
hantei_list_rot = [1.0 * np.pi for _ in range(list_length)]
area_pos = 0.02
area_rot = np.pi * 3.0 / 180.0
start_time = rospy.Time.now()
# Start measuring
time_limit = 4.0
additional_cost = 0.0
self.publish(status)
while not rospy.is_shutdown():
distance_to_target_pos = \
functions.distance_btw_two_points((status.pid_goal_pos_x, status.pid_goal_pos_y),
(self.robot['x'], self.robot['y']))
distance_to_target_rot = self.diff_theta(status.pid_goal_theta, self.robot['theta'])
hantei_list_pos.append(distance_to_target_pos)
hantei_list_pos.pop(0)
hantei_list_rot.append(distance_to_target_rot)
hantei_list_rot.pop(0)
average_pos = np.average(hantei_list_pos)
average_rot = np.average(hantei_list_rot)
# rospy.loginfo_throttle(1, "pos average: "+str(average_pos)+" rot average: "+str(average_rot))
current_elapsed_time = rospy.Time.now() - start_time
time_over = abs(current_elapsed_time.to_sec()) > time_limit
if mode == "position":
condition = (average_pos < area_pos and average_rot < area_rot) or time_over
else:
condition = average_rot < area_rot or time_over
if condition:
# if current_elapsed_time.to_sec() > 3.0:
# total_average_pos = total_average_pos + average_pos
# total_average_rot = total_average_rot + average_rot
if time_over:
# 評価関数が非線形だろうが不連続だろうがそんなものは気にしない
additional_cost = 10 * (average_pos + average_rot)
break
loop_rate.sleep()
end_time = rospy.Time.now()
elapsed_time = (end_time - start_time).to_sec() + additional_cost
total_elapsed_time = total_elapsed_time + elapsed_time
# print("time: " + str(elapsed_time) + " sec")
# print("")
if rospy.is_shutdown():
break
# print("total_elapsed_time:" + str(total_elapsed_time))
return 10.0 / total_elapsed_time
def calculate_1(self, strategy_context=None):
# print 'passed_1_flg:', self.passed_1_flg
# print 'received_1_flg:', self.received_1_flg
# print 'passed_2_flg:',self.passed_2_flg
# print 'received_2_flg:', self.received_2_flg
# print 'passed_3_flg:', self.passed_3_flg
# print 'received_3_flg:', self.received_3_flg
"""ハーフラインより左側の戦略（の予定）"""
ball_x, ball_y = self._objects.ball.get_current_position()
active_enemy_ids = self._get_active_enemy_ids()
nearest_enemy_id = self._objects.get_enemy_ids_sorted_by_distance_to_ball(
active_enemy_ids)[0]
for robot_id in self._robot_ids:
status = Status()
robot = self._objects.get_robot_by_id(robot_id)
if self.ball_position_nearest_id == robot_id:
if self.passed_1_flg:
# パス完了した場合
if self._objects.robot[robot_id].get_role() == "LDF":
status.status = "defence4"
else:
status.status = "defence3"
else:
# パス完了する前 パス先をposition1にする、パス完了したら完了flgを立てる
status.status = "pass"
status.pass_target_pos_x = self._objects.get_robot_by_id(
self.position_1_nearest_id).get_current_position()[0]
status.pass_target_pos_y = self._objects.get_robot_by_id(
self.position_1_nearest_id).get_current_position()[1]
if self._objects.get_has_a_ball(robot_id):
self.received_1_flg = True
if self.received_1_flg and self._objects.get_has_a_ball(
robot_id) == False:
self.passed_1_flg = True
elif robot_id == self.position_1_nearest_id:
if self.received_3_flg:
#シュートを打つ子がボールを受け取ったあとはこの機体もシュート体制に入る(おこぼれを狙うイメージ)
#status.status = "pass"
if self.position_2[1] > 0.:
status.status = "shoot_left"
# status.pass_target_pos_x = 6.0
# status.pass_target_pos_y = 0.55
else:
status.status = "shoot_right"
# status.pass_target_pos_x = 6.0
# status.pass_target_pos_y = -0.55
elif self.passed_2_flg:
# 自分がパスしたあとはゴール前のposition_3に移動
status.status = "move_linear"
status.pid_goal_pos_x = self.position_1[0]
status.pid_goal_pos_y = self.position_1[1]
status.pid_goal_theta = 0.
elif self.received_2_flg:
# レシーブしたあとはパスに切り替え
status.status = "pass"
status.pass_target_pos_x = self._objects.get_robot_by_id(
self.position_2_nearest_id).get_current_position()[0]
status.pass_target_pos_y = self._objects.get_robot_by_id(
self.position_2_nearest_id).get_current_position()[1]
if self._objects.get_has_a_ball(self.position_2_nearest_id,
threshold=0.20) == True:
self.passed_2_flg
else:
status.status = "receive"
status.pid_goal_pos_x = self.position_1[0]
status.pid_goal_pos_y = self.position_1[1]
if self._objects.get_has_a_ball(robot_id):
self.received_2_flg = True
elif robot_id == self.position_2_nearest_id:
if self.passed_3_flg:
status.status = "move_linear"
if nearest_enemy_id != None:
status.pid_goal_pos_x, status.pid_goal_pos_y = functions.calculate_internal_dividing_point(
self._enemy[nearest_enemy_id].get_current_position(
)[0],
self._enemy[nearest_enemy_id].get_current_position(
)[1],
self._ball_params.get_current_position()[0],
self._ball_params.get_current_position()[1],
functions.distance_btw_two_points(
self._enemy[nearest_enemy_id].
get_current_position(),
self._ball_params.get_current_position()) +
0.55, -0.55)
status.pid_goal_theta = math.atan2(
(self._ball_params.get_current_position()[1] -
robot.get_current_position()[1]),
(self._ball_params.get_current_position()[0] -
robot.get_current_position()[0]))
elif self.received_3_flg:
# status.status = "pass"
if self.position_3[1] > 0.:
status.status = "shoot_left"
# status.pass_target_pos_x = 6.0
# status.pass_target_pos_y = 0.55
else:
status.status = "shoot_right"
# status.pass_target_pos_x = 6.0
# status.pass_target_pos_y = -0.55
if self._objects.get_has_a_ball(robot_id) == False:
self.passed_3_flg = True
else:
status.status = "receive"
status.pid_goal_pos_x = self.position_2[0]
status.pid_goal_pos_y = self.position_2[1]
if self._objects.get_has_a_ball(robot_id):
self.received_3_flg = True
elif robot_id == self.GK_id:
status.status = "keeper"
elif robot.get_role() == "LDF":
status.status = "defence4"
elif robot.get_role() == "RDF":
status.status = "defence3"
else:
print 'error'
self._dynamic_strategy.set_robot_status(robot_id, status)
return self._dynamic_strategy
def calcurate(self, strategy_context=None, place_ball_position=[0, 0]):
# type: (StrategyContext) -> StrategyBase
if self._detect_enemy_kick(strategy_context):
strategy_context.update("enemy_kick",
True,
namespace="world_model")
strategy_context.update("defence_or_attack",
False,
namespace="world_model")
active_robot_ids = self._get_active_robot_ids()
active_enemy_ids = self._get_active_enemy_ids()
nearest_enemy_id = self._objects.get_enemy_ids_sorted_by_distance_to_ball(
active_enemy_ids)[0]
second_nearest_enemy_id = self._objects.get_enemy_ids_sorted_by_distance_to_ball(
active_enemy_ids)[1]
for robot_id in active_robot_ids:
status = Status()
robot = self._objects.get_robot_by_id(robot_id)
if robot.get_role() == "GK":
status.status = "keeper"
elif robot.get_role() == "LDF":
status.status = "defence4"
elif robot.get_role() == "RDF":
status.status = "defence3"
elif robot.get_role() == "LFW":
#敵kickerとballの延長線上に移動
status.status = "move_linear"
# if second_nearest_enemy_id != None:
if True:
# status.pid_goal_pos_x, status.pid_goal_pos_y = functions.calculate_internal_dividing_point(self._enemy[nearest_enemy_id].get_current_position()[0], self._enemy[nearest_enemy_id].get_current_position()[1], self._ball_params.get_current_position()[0], self._ball_params.get_current_position()[1], functions.distance_btw_two_points(self._enemy[nearest_enemy_id].get_current_position(), self._ball_params.get_current_position()) + 0.55, -0.55)
target_pos_xy = functions.calculate_internal_dividing_point_vector_args(
config.GOAL_CENTER,
self._ball_params.get_current_position(), 1, 1)
status.pid_goal_pos_x = target_pos_xy[0]
status.pid_goal_pos_y = target_pos_xy[1]
status.pid_goal_theta = math.atan2(
(self._ball_params.get_current_position()[1] -
robot.get_current_position()[1]),
(self._ball_params.get_current_position()[0] -
robot.get_current_position()[0]))
elif robot.get_role() == "RFW":
status.status = "move_linear"
# ボールプレースメントの場所とボールの場所でゴールに近い方のゴール側に陣取る
goal_pos = np.array(config.GOAL_CENTER)
place_pos = np.array(place_ball_position)
ball_pos = np.array(self._ball_params.get_current_position())
if np.linalg.norm(goal_pos -
place_pos) < np.linalg.norm(goal_pos -
ball_pos):
target_pos = place_pos + (
goal_pos - place_pos) * 1 / np.linalg.norm(goal_pos -
place_pos)
else:
target_pos = ball_pos + (
goal_pos - ball_pos) * 1 / np.linalg.norm(goal_pos -
ball_pos)
status.pid_goal_pos_x = target_pos[0]
status.pid_goal_pos_y = target_pos[1]
status.pid_goal_theta = np.arctan2(
(self._ball_params.get_current_position()[1] -
robot.get_current_position()[1]),
(self._ball_params.get_current_position()[0] -
robot.get_current_position()[0]))
else:
status.status = "none"
self._dynamic_strategy.set_robot_status(robot_id, status)
result = self._dynamic_strategy
return result
