def __init__(self):
rospy.init_node("Calculation_node")
self.robot_color = str(rospy.get_param("friend_color"))
self.robot_side = str(rospy.get_param("team_side"))
# Composition
self.objects = Objects(self.robot_color,
self.robot_side,
config.NUM_FRIEND_ROBOT,
config.NUM_ENEMY_ROBOT,
node="calculation")
self.robot_friend = self.objects.robot
self.robot_enemy = self.objects.enemy
self.ball_params = self.objects.ball
self.ball_sub_params = Ball_sub_params()
self.def_pos = Def_pos()
self.ball_frame = 10 # ボールの軌道直線フィッティングと速度の計算フレーム数
self.ball_move_threshold = 0.01 # ボールが移動したと判定する閾値[m]
self.same_pos_count = 0 # 停止判定用カウント
self.ball_pos_count = 0 # 計算用カウント、フレーム単位でカウント
self.calc_flag = False # 計算フラグ、停止判定時は計算しない
self.ball_pos_x_array = np.array([0.0] *
self.ball_frame) # ボールのx座標保存用配列
self.ball_pos_y_array = np.array([0.0] *
self.ball_frame) # ボールのy座標保存用配列
self.ball_vel_array = np.array([0.0] * self.ball_frame) # ボールの速度保存用配列
self.ball_vel_x_array = np.array([0.0] *
self.ball_frame) # ボールのx方向の速度保存用配列
self.ball_vel_y_array = np.array([0.0] *
self.ball_frame) # ボールのy方向の速度保存用配列
self.ball_vel_time_array = np.array([0.0] *
self.ball_frame) # 加速度計算用、時間配列
self.ball_vel = 0. # ボール速度
self.ball_vel_a = 0. # ボール速度の傾き
self.ball_vel_b = 0. # ボール速度の切片
self.ball_vel_x_a = 0. # x方向の速度の傾き
self.ball_vel_x_b = 0. # x方向の速度の切片
self.ball_vel_y_a = 0. # y方向の速度の傾き
self.ball_vel_y_b = 0. # y方向の速度の切片
self.ball_stop_time_x = 0. # x方向の停止までの時間
self.ball_stop_time_y = 0. # y方向の停止までの時間
# 守備の時のロボットのポジション座標計算用変数
# 現状、青チームのみ対応
self.g_up_x = -6.0 # ゴールポストの上側のx座標:y_GL
self.g_up_y = 0.6 # ゴールポストの上側のy座標:x_GL
self.g_down_x = -6.0 # ゴールポストの下側のx座標:y_GR
self.g_down_y = -0.6 # ゴールポストの下側のy座標:x_GR
self.g_center_x = -6.0 # ゴールの中央のx座標:y_GC
self.g_center_y = 0.0 # ゴールの中央のy座標:x_GC
self.p_area_up_x = -4.8 # ペナルティエリアの上側の角のx座標:y_PL
self.p_area_up_y = 1.2 # ペナルティエリアの上側の角のy座標:x_PL
self.p_area_down_x = -4.8 # ペナルティエリアの下側の角のx座標:y_PR
self.p_area_down_y = -1.2 # ペナルティエリアの下側の角のy座標:x_PR
self.line_up_x = 0.0 # ボールとゴールポストを結んだ線と防御ラインとの交点の上側のx座標:y_LL
self.line_up_y = 0.0 # ボールとゴールポストを結んだ線と防御ラインとの交点の上側のy座標:x_LL
self.line_down_x = 0.0 # ボールとゴールポストを結んだ線と防御ラインとの交点の下側のx座標:y_LR
self.line_down_y = 0.0 # ボールとゴールポストを結んだ線と防御ラインとの交点の下側のy座標:x_LR
self.line_up_r_x = 0.0 # ロボットの半径を考慮した補正後の座標:y_LL'
self.line_up_r_y = 0.0 # ロボットの半径を考慮した補正後の座標:x_LL'
self.line_down_r_x = 0.0 # ロボットの半径を考慮した補正後の座標:y_LR'
self.line_down_r_y = 0.0 # ロボットの半径を考慮した補正後の座標:x_LR'
self.offset_r = 0. # オフセット値
self.robot_r = 90.0 / 1000.0 # ロボット半径
self.robot_a = 1.0 # ロボットの加速度
self.ball_MAX_SPEED = 6.5 # ボールの最大速度
self.delay_time_ms = 100.0 # 遅延時間[ms]
self.L_a = 0.0 # 壁のラインとボールまでの距離
self.L_G = 0.0 # ボール到達までに移動可能な距離
def __init__(self, robot_id=None):
self.robot_color = str(rospy.get_param("friend_color"))
self.robot_side = str(rospy.get_param("team_side"))
if robot_id is None:
self.robot_id = str(rospy.get_param("~robot_num"))
else:
self.robot_id = str(robot_id)
self.robot_total = config.NUM_FRIEND_ROBOT
self.enemy_total = config.NUM_ENEMY_ROBOT
self.cmd = robot_commands() # type: robot_commands
self.cmd.kick_speed_x = 0
self.cmd.kick_speed_z = 0
self.cmd.dribble_power = 0
self._command_pub = RobotCommandPublisherWrapper(
self.robot_color, self.robot_id)
# Composition
self.objects = Objects(self.robot_color,
self.robot_side,
self.robot_total,
self.enemy_total,
node="robot" + str(self.robot_id))
self.ctrld_robot = self.objects.robot[int(
self.robot_id)] # type: entity.Robot
self.robot_friend = self.objects.robot
self.robot_enemy = self.objects.enemy
self.ball_params = self.objects.ball
self.pid = RobotPid(self.robot_id, self.objects, self.cmd)
self.status = RobotStatus(self.pid)
self.kick = RobotKick(self.pid, self.cmd, self.status)
self.defence = RobotDefence(self.status, self.kick)
self.keeper = RobotKeeper(self.kick)
# listner 起動
self.status_listener()
self.def_pos_listener()
self.set_pid_server()
rospy.Timer(rospy.Duration(1.0 / 30.0), self.pid.replan_timer_callback)
self._last_pub_time = rospy.Time.now()
self._last_vel_surge_sway_vec = [0.0, 0.0]
self._last_omega = 0.0
self._fps = []
self._current_loop_time = 0
self._last_loop_time = 0
rospy.Timer(rospy.Duration(1.0), self.print_fps_callback)
self.robot_fps_publisher = rospy.Publisher("fps/robot_" +
str(robot_id),
Float32,
queue_size=1)
self.loop_rate = rospy.Rate(ROBOT_LOOP_RATE)
rospy.loginfo("start robot node: " + self.robot_id)
def __init__(self):
rospy.init_node("world_model")
self._team_color = str(rospy.get_param('friend_color'))
self._team_side = str(rospy.get_param('team_side'))
"""----上の5つの変数、インスタンスをまとめたもの、callbackをもつ---"""
self._objects = Objects(self._team_color,
self._team_side,
config.NUM_FRIEND_ROBOT,
config.NUM_ENEMY_ROBOT,
node="world_model")
self.robot_pys = []
self.robot_py_ths = []
self.robot_ids = self._objects.get_robot_ids()
# スレッドを使わないときはここをコメントアウト --ここから--
for robot_id in self.robot_ids:
robot_py = robot.Robot(robot_id)
self.robot_pys.append(robot_py)
th = threading.Thread(target=robot_py.run)
th.setDaemon(True)
th.start()
self.robot_py_ths.append(th)
# スレッドを使わないときはここをコメントアウト --ここまで--
"""---Referee---"""
self._referee = Referee(self._objects)
self._stcalcurator = {
'normal_start_normal':
NormalStartStrategyCalcurator(self._objects),
'normal_start_kickoff':
NormalStartKickOffStrategyCalcurator(self._objects),
'normal_start_kickoff_defence':
NormalStartKickOffDefenceStrategyCalcurator(self._objects),
'stop':
StopStrategyCalculator(self._objects),
'penalty_attack':
PenaltyAttack(self._objects),
'penalty_defence':
NormalStartPenaltyDefenceStrategyCalcurator(self._objects),
'direct_free_attack':
DirectFreeAttack(self._objects),
'direct_free_defence':
DirectFreeDefence(self._objects),
'indirect_free_attack':
IndirectFreeAttack(self._objects),
'indirect_free_defence':
IndirectFreeDefence(self._objects),
'friend_ball_placement':
FriendBallPlacement(self._objects),
'enemy_ball_placement':
EnemyBallPlacement(self._objects)
}
self._status_publisher = WorldModelStatusPublisher(
self._team_color, robot_ids=self._objects.get_robot_ids())
# 積分などに必要な情報を保存するオブジェクト
self._strategy_context = StrategyContext()
# とりあえず例として"last_number"という名前で10フレーム分のコンテキストを作成。
# 初期値は全て0を指定。
self._strategy_context.register_new_context("last_number", 10, 0)
self._strategy_context.register_new_context("normal_strat_state",
2,
0,
namespace="normal_strat")
self._strategy_context.register_new_context("kickoff_complete",
1,
False,
namespace="world_model")
self._strategy_context.register_new_context("enemy_kick",
1,
False,
namespace="world_model")
self._strategy_context.register_new_context("referee_branch",
1,
"NONE",
namespace="world_model")
self._strategy_context.register_new_context("defence_or_attack",
1,
False,
namespace="world_model")
self._strategy_context.register_new_context("placed_ball_position",
1, [0, 0],
namespace="world_model")
self._strategy_context.register_new_context("indirect_finish",
1,
False,
namespace="world_model")
self._strategy_context.register_new_context("direct_finish",
1,
False,
namespace="world_model")
self._strategy_context.register_new_context("penalty_finish",
1,
False,
namespace="world_model")
self._loop_events = []
custom_referee_branch_subs = rospy.Subscriber(
"/force_referee_branch", String, self.force_referee_branch)
self.custom_referee_branch = ""
self.world_model_fps_publisher = rospy.Publisher("fps/world_model",
Float32,
queue_size=1)
class Robot(object):
def __init__(self, robot_id=None):
self.robot_color = str(rospy.get_param("friend_color"))
self.robot_side = str(rospy.get_param("team_side"))
if robot_id is None:
self.robot_id = str(rospy.get_param("~robot_num"))
else:
self.robot_id = str(robot_id)
self.robot_total = config.NUM_FRIEND_ROBOT
self.enemy_total = config.NUM_ENEMY_ROBOT
self.cmd = robot_commands() # type: robot_commands
self.cmd.kick_speed_x = 0
self.cmd.kick_speed_z = 0
self.cmd.dribble_power = 0
self._command_pub = RobotCommandPublisherWrapper(
self.robot_color, self.robot_id)
# Composition
self.objects = Objects(self.robot_color,
self.robot_side,
self.robot_total,
self.enemy_total,
node="robot" + str(self.robot_id))
self.ctrld_robot = self.objects.robot[int(
self.robot_id)] # type: entity.Robot
self.robot_friend = self.objects.robot
self.robot_enemy = self.objects.enemy
self.ball_params = self.objects.ball
self.pid = RobotPid(self.robot_id, self.objects, self.cmd)
self.status = RobotStatus(self.pid)
self.kick = RobotKick(self.pid, self.cmd, self.status)
self.defence = RobotDefence(self.status, self.kick)
self.keeper = RobotKeeper(self.kick)
# listner 起動
self.status_listener()
self.def_pos_listener()
self.set_pid_server()
rospy.Timer(rospy.Duration(1.0 / 30.0), self.pid.replan_timer_callback)
self._last_pub_time = rospy.Time.now()
self._last_vel_surge_sway_vec = [0.0, 0.0]
self._last_omega = 0.0
self._fps = []
self._current_loop_time = 0
self._last_loop_time = 0
rospy.Timer(rospy.Duration(1.0), self.print_fps_callback)
self.robot_fps_publisher = rospy.Publisher("fps/robot_" +
str(robot_id),
Float32,
queue_size=1)
self.loop_rate = rospy.Rate(ROBOT_LOOP_RATE)
rospy.loginfo("start robot node: " + self.robot_id)
def store_and_publish_commands(self):
acc_clip_threshold_max = 0.075
acc_clip_threshold_min = 0.01
current_pub_time = rospy.Time.now()
dt = (current_pub_time - self._last_pub_time).to_sec()
self.ctrld_robot.update_expected_velocity_context(
self.cmd.vel_x, self.cmd.vel_y, self.cmd.omega)
current_acc = np.array([
self.cmd.vel_surge - self._last_vel_surge_sway_vec[0],
self.cmd.vel_sway - self._last_vel_surge_sway_vec[1]
]) / dt
current_omega_acc = (self.cmd.omega - self._last_omega) / dt
clipped_acc \
= functions.clip_vector2(current_acc, acc_clip_threshold_max)
clipped_omega_acc, _ \
= functions.clip_vector2((current_omega_acc, 0.0), acc_clip_threshold_max / self.objects.robot[0].size_r)
vel_acc_clip_threshold_min = acc_clip_threshold_min
omega_acc_clip_threshold_min = acc_clip_threshold_min / self.objects.robot[
0].size_r
if -vel_acc_clip_threshold_min < clipped_acc[
0] < vel_acc_clip_threshold_min:
self.cmd.vel_surge = self._last_vel_surge_sway_vec[
0] + clipped_acc[0]
if -vel_acc_clip_threshold_min < clipped_acc[
1] < vel_acc_clip_threshold_min:
self.cmd.vel_sway = self._last_vel_surge_sway_vec[1] + clipped_acc[
1]
if -omega_acc_clip_threshold_min < clipped_omega_acc < omega_acc_clip_threshold_min:
self.cmd.omega = self._last_omega + clipped_omega_acc
self.ctrld_robot.handle_loop_callback()
self._command_pub.publish(self.cmd)
self._last_pub_time = rospy.Time.now()
self._last_vel_surge_sway_vec = (self.cmd.vel_surge, self.cmd.vel_sway)
self._last_omega = self.cmd.omega
# self.reset_cmd()
def reset_cmd(self):
default_cmd = robot_commands()
default_cmd.kick_speed_x = 0
default_cmd.kick_speed_z = 0
default_cmd.dribble_power = 0
self.cmd.vel_x = default_cmd.vel_x
self.cmd.vel_y = default_cmd.vel_y
self.cmd.vel_surge = default_cmd.vel_surge
self.cmd.vel_sway = default_cmd.vel_sway
self.cmd.omega = default_cmd.omega
self.cmd.theta = default_cmd.theta
self.cmd.kick_speed_x = default_cmd.kick_speed_x
self.cmd.kick_speed_z = default_cmd.kick_speed_z
self.cmd.dribble_power = default_cmd.dribble_power
def shutdown_cmd(self):
self.reset_cmd()
self.cmd.shutdown_flag = True
self.store_and_publish_commands()
def print_fps_callback(self, event):
if len(self._fps) > 0:
msg = Float32()
msg.data = sum(self._fps) / len(self._fps)
self.robot_fps_publisher.publish(msg)
self._fps = []
def run(self):
# Loop 処理
while not rospy.is_shutdown():
self.run_once()
def run_once(self):
if True: # インデント変えたくなかっただけのif True
self._current_loop_time = time.time()
elapsed_time = self._current_loop_time - self._last_loop_time
self._fps.append(1. / elapsed_time)
# 場外にいる場合、カルマンフィルタの信念(分散)を初期化する
if self.ctrld_robot.get_id(
) not in self.objects.get_active_robot_ids():
self.ctrld_robot.reset_own_belief()
# カルマンフィルタ,恒等関数フィルタの適用
# vision_positionからcurrent_positionを決定してつめる
for robot in self.robot_friend:
if robot.get_id() == self.ctrld_robot.get_id():
# kalman_filter(self.ctrld_robot)
identity_filter(self.ctrld_robot)
else:
identity_filter(robot)
for enemy in self.robot_enemy:
identity_filter(enemy)
self.ctrld_robot.set_max_velocity(
rospy.get_param(
"/robot_max_velocity",
default=config.ROBOT_MAX_VELOCITY)) # m/s 機体の最高速度
if self.status.robot_status == "move_linear":
self.pid.pid_linear(self.ctrld_robot.get_future_position()[0],
self.ctrld_robot.get_future_position()[1],
self.ctrld_robot.get_future_orientation())
elif self.status.robot_status == "move_circle":
self.pid.pid_circle(self.pid.pid_circle_center_x,
self.pid.pid_circle_center_y,
self.ctrld_robot.get_future_position()[0],
self.ctrld_robot.get_future_position()[1],
self.ctrld_robot.get_future_orientation())
if self.status.robot_status == "move_linear_straight":
self.pid.pid_straight(
self.ctrld_robot.get_future_position()[0],
self.ctrld_robot.get_future_position()[1],
self.ctrld_robot.get_future_orientation())
elif self.status.robot_status == "pass":
self.kick.pass_ball(
self.ctrld_robot.get_pass_target_position()[0],
self.ctrld_robot.get_pass_target_position()[1])
elif self.status.robot_status == "pass_chip":
self.kick.pass_ball(
self.ctrld_robot.get_pass_target_position()[0],
self.ctrld_robot.get_pass_target_position()[1],
is_tip_kick=True)
elif self.status.robot_status == "prepare_pass":
self.kick.pass_ball(
self.ctrld_robot.get_pass_target_position()[0],
self.ctrld_robot.get_pass_target_position()[1],
should_wait=True)
elif self.status.robot_status == "ball_place_kick":
self.kick.pass_ball(
self.ctrld_robot.get_pass_target_position()[0],
self.ctrld_robot.get_pass_target_position()[1],
place=True)
elif self.status.robot_status == "ball_place_dribble":
self.kick.dribble_ball(
self.ctrld_robot.get_pass_target_position()[0],
self.ctrld_robot.get_pass_target_position()[1],
ignore_penalty_area=True)
elif self.status.robot_status == "shoot":
self.kick.shoot_ball()
elif self.status.robot_status == "shoot_right":
self.kick.shoot_ball(target="right")
elif self.status.robot_status == "shoot_left":
self.kick.shoot_ball(target="left")
elif self.status.robot_status == "shoot_center":
self.kick.shoot_ball(target="center")
elif self.status.robot_status == "prepare_shoot":
self.kick.shoot_ball(should_wait=True)
elif self.status.robot_status == "penalty_shoot":
self.kick.shoot_ball(ignore_penalty_area=True)
elif self.status.robot_status == "penalty_shoot_right":
self.kick.shoot_ball(target="right", ignore_penalty_area=True)
elif self.status.robot_status == "penalty_shoot_left":
self.kick.shoot_ball(target="left", ignore_penalty_area=True)
elif self.status.robot_status == "penalty_shoot_center":
self.kick.shoot_ball(target="center", ignore_penalty_area=True)
elif self.status.robot_status == "receive":
self.kick.receive_ball(
self.ctrld_robot.get_future_position()[0],
self.ctrld_robot.get_future_position()[1])
elif self.status.robot_status == "receive_direct_pass":
self.kick.receive_and_direct_pass(
self.ctrld_robot.get_future_position(),
self.ctrld_robot.get_pass_target_position())
elif self.status.robot_status == "receive_direct_shoot":
self.kick.receive_and_direct_shoot(
self.ctrld_robot.get_future_position())
elif self.status.robot_status == "receive_direct_shoot_left":
self.kick.receive_and_direct_shoot(
self.ctrld_robot.get_future_position(), target="left")
elif self.status.robot_status == "receive_direct_shoot_right":
self.kick.receive_and_direct_shoot(
self.ctrld_robot.get_future_position(), target="right")
elif self.status.robot_status == "receive_direct_shoot_center":
self.kick.receive_and_direct_shoot(
self.ctrld_robot.get_future_position(), target="center")
elif self.status.robot_status == "defence1":
self.defence.move_defence(self.defence.def1_pos_x,
self.defence.def1_pos_y)
elif self.status.robot_status == "defence2":
self.defence.move_defence(self.defence.def2_pos_x,
self.defence.def2_pos_y)
elif self.status.robot_status == "defence3":
self.kick.receive_ball(self.defence.def1_pos_x,
self.defence.def1_pos_y)
elif self.status.robot_status == "defence4":
self.kick.receive_ball(self.defence.def2_pos_x,
self.defence.def2_pos_y)
elif self.status.robot_status == "keeper":
self.keeper.keeper()
elif self.status.robot_status == "keeper_pass_chip":
self.kick.pass_ball(
self.ctrld_robot.get_pass_target_position()[0],
self.ctrld_robot.get_pass_target_position()[1],
is_tip_kick=True,
ignore_penalty_area=True)
elif self.status.robot_status == "stop":
self.reset_cmd()
self.status.robot_status = "none"
elif self.status.robot_status == "halt":
self.reset_cmd()
elif self.status.robot_status == "shutdown":
self.shutdown_cmd()
self.store_and_publish_commands()
self._last_loop_time = self._current_loop_time
self.loop_rate.sleep()
"""
def goal_pose_listener(self):
rospy.Subscriber("/robot_" + self.robot_id + "/goal_pose", Pose, self.ctrld_robot.goal_pose_callback)
def target_pose_listener(self):
rospy.Subscriber("/robot_" + self.robot_id + "/target_pose", Pose, self.ctrld_robot.target_pose_callback)
"""
def status_listener(self):
rospy.Subscriber(
"/" + self.robot_color + "/robot_" + self.robot_id + "/status",
Status, self.status.status_callback)
def set_pid_server(self):
service_name = "/" + self.robot_color + "/robot_" + self.robot_id + "/set_pid"
if service_name not in rosservice.get_service_list():
rospy.Service(service_name, pid, self.pid.set_pid_callback)
"""
def kick(self, req):
self.cmd.kick_speed_x = 3
return True
def kick_end(self, req):
self.cmd.kick_speed_x = 0
return True
def kick_server(self):
rospy.Service("/robot_0/kick", Kick, self.kick)
rospy.Service("/robot_0/kick_end", Kick, self.kick_end)
"""
def def_pos_listener(self):
rospy.Subscriber("/" + self.robot_color + "/def_pos", Def_pos,
self.defence.def_pos_callback)