diff --git a/ball.py b/ball.py index 975f67c..3646bbe 100644 --- a/ball.py +++ b/ball.py @@ -1,67 +1,78 @@ import os import pandas as pd import math -def track_ball_position(udp, receive_packet, receive_game_controller_signal, stop_event, debug=False, sock=None): - balls_position = [] - packet = receive_packet(udp) - frame = packet.detection +def track_ball_position(udp, receive_packet, receive_game_controller_signal, stop_event, debug=False, sock=None): # ボール位置の追跡 + balls_position = [] # ボール位置を保存するリスト + packet = receive_packet(udp) # パケットを受信 + frame = packet.detection # フレームを取得 if debug: - print("frame: ", frame) + print("frame: ", frame) # デバッグ用にフレームを表示 if frame: - balls = frame.balls + balls = frame.balls # ボールのリストを取得 if balls: - ball = balls[0] + ball = balls[0] # 最初のボールを取得 # sockを引数で受け取るか、グローバルから参照 - state = receive_game_controller_signal(sock, stop_event) - balls_position.append([int(ball.x), int(ball.y), state,receive_packet(udp).detection.frame_number]) + state = receive_game_controller_signal(sock, stop_event) # ゲームコントローラーの信号を受信 + balls_position.append([int(ball.x), int(ball.y), state, frame.frame_number]) # ボール位置を保存 return balls_position -def store_ball_position(udp, receive_packet, receive_game_controller_signal, stop_event, path, debug=False, sock=None): - while not stop_event.is_set(): +def store_ball_position(udp, receive_packet, receive_game_controller_signal, stop_event, path, debug=False, sock=None): # ボール位置の保存 + while not stop_event.is_set(): # スレッドが停止されるまでループ try: - ball_position = track_ball_position(udp, receive_packet, receive_game_controller_signal, stop_event, debug,sock) - ballpojiPath = os.path.join(path, "ball_position.csv") + ball_position = track_ball_position(udp, receive_packet, receive_game_controller_signal, stop_event, debug,sock) # ボール位置を追跡 + ball_position_path = os.path.join(path, "ball_position.csv") # ボール位置のCSVファイルパス if not os.path.isdir(path): - os.mkdir(path) + os.mkdir(path) # 出力先ディレクトリが存在しない場合は作成 if ball_position: - df = pd.DataFrame(ball_position, columns=["x", "y", "state","frame_number"]) - if not os.path.exists(ballpojiPath): - df.to_csv(ballpojiPath, mode='w', header=True, index=False) + df = pd.DataFrame(ball_position, columns=["x", "y", "state","frame_number"]) # ボール位置のデータフレームを作成 + if not os.path.exists(ball_position_path): + df.to_csv(ball_position_path, mode='w', header=True, index=False) # ボール位置のCSVファイルが存在しない場合は新規作成 else: - df.to_csv(ballpojiPath, mode='a', header=False, index=False) + df.to_csv(ball_position_path, mode='a', header=False, index=False) # 存在する場合は追記 if debug: - print("ball_position: ", ball_position) + print("ball_position: ", ball_position) # デバッグ用にボール位置を表示 print("\n") - except KeyboardInterrupt: + except KeyboardInterrupt: # キーボード割り込みで終了 break -def ball_velocity(udp, receive_packet, receive_game_controller_signal, stop_event, path, debug, sock): - ball_posi = [] - while not stop_event.is_set(): +def ball_velocity(udp, receive_packet, receive_game_controller_signal, stop_event, path, debug=False, sock=None): # ボール速度の計算 + ball_posi = [] # 過去のボール位置のリスト + while not stop_event.is_set(): # スレッドが停止されるまでループ try: - - ball_position = track_ball_position(udp, receive_packet, receive_game_controller_signal, stop_event, debug, sock) + ball_position = track_ball_position(udp, receive_packet, receive_game_controller_signal, stop_event, debug, sock) # ボール位置を追跡 if ball_position: - ball_posi.append(ball_position[0]) # 最新データを追加 - # print("ball_position: ", ball_position) - print("---------------------------------\n") - print("ball_position[0]: ", ball_position[0]) - print("---------------------------------\n") - # print("ball_posi: ", ball_posi) - print("---------------------------------\n") - print("ball_posi[0]: ", ball_posi[0]) - if len(ball_posi) == 2: - x1, y1, state1, frame1 = ball_posi[0] - x2, y2, state2, frame2 = ball_posi[1] - dt = (frame2 - frame1) * (1/60) - vx = (x2 - x1) / dt - vy = (y2 - y1) / dt - speed = math.sqrt(vx ** 2 + vy ** 2) - print("vx:", vx) - print("vy:", vy) - print("ball_velocity:", speed) + ball_posi.append(ball_position[0]) # ボール位置をball_posiに追加 + if debug: + print("---------------------------------\n") + print("ball_position[0]: ", ball_position[0]) # デバッグ用に最新のボール位置を表示 + print("---------------------------------\n") + print("ball_posi[0]: ", ball_posi[0]) # デバッグ用にひとつ前のボール位置を表示 + print("---------------------------------\n") + if len(ball_posi) == 2: # 2つのボール位置がある場合 + x1, y1, state1, frame1 = ball_posi[0] # ひとつ前のボール位置,状態,フレーム番号 + x2, y2, state2, frame2 = ball_posi[1] # 最新のボール位置,状態,フレーム番号 + dt = (frame2 - frame1) * (1/60) # フレーム間隔を秒に変換 + vx = (x2 - x1) / dt # x方向の速度 + vy = (y2 - y1) / dt # y方向の速度 + speed = math.sqrt(vx ** 2 + vy ** 2) # ボールの速度 + direction_rad = math.atan2(vy, vx) # 速度の方向をラジアンで計算 + direction_deg = math.degrees(direction_rad) # 速度の方向を度に変換 + if debug: + print("vx:", vx) # x方向の速度を表示 + print("vy:", vy) # y方向の速度を表示 + print("ball_velocity:", speed) # ボールの速度を表示 + print("direction_deg:", direction_deg) # 速度の方向を表示 + new_velocity_data = [[speed, direction_deg, state1, state2, frame2]] # 新しいボール速度のデータ + df = pd.DataFrame(new_velocity_data, columns=["speed", "direction", "state1", "state2", "frame_number"]) # ボール速度のデータフレームを作成 + ball_velocity_path = os.path.join(path, "ball_velocity.csv") # ボール速度のCSVファイルパス + if not os.path.isdir(path): + os.mkdir(path) # 出力先ディレクトリが存在しない場合は作成 + if not os.path.exists(ball_velocity_path): + df.to_csv(ball_velocity_path, mode='w', header=True, index=False) # ボール速度のCSVファイルが存在しない場合は新規作成 + else: + df.to_csv(ball_velocity_path, mode='a', header=False, index=False) # 存在する場合は追記 ball_posi[0] = ball_posi[1] # 最新データを更新 ball_posi.pop(-1) # 2つを超えたら古いものを削除 - except KeyboardInterrupt: + except KeyboardInterrupt: # キーボード割り込みで終了 break \ No newline at end of file diff --git a/goal.py b/goal.py index 4644145..77ca7cb 100644 --- a/goal.py +++ b/goal.py @@ -1,28 +1,28 @@ import os import pandas as pd -def goal_scene(track_ball_position, track_robot_position, path, stop_event, debug=False): - robot_poji_goal_path = os.path.join(path, "robot_position_goal.csv") - robot_position_path = os.path.join(path, "robot_position.csv") - poji_goal_x=6000 - nega_goal_x=-6000 - poji_goal_y=600 - nega_goal_y=600 +def goal_scene(track_ball_position, track_robot_position, path, stop_event, debug=False): # ゴールシーンの抽出 + robot_poji_goal_path = os.path.join(path, "robot_position_goal.csv") # ロボットのポジションがゴールの時のCSVファイルパス + robot_position_path = os.path.join(path, "robot_position.csv") # ロボットのポジションのCSVファイルパス + poji_goal_x=6000 # 右側ゴールのX座標 + nega_goal_x=-6000 # 左側ゴールのX座標 + goal_top=900 # ゴールの上端 + goal_bottom=-900 # ゴールの下端 if not os.path.isdir(path): - os.mkdir(path) - while not stop_event.is_set(): + os.mkdir(path) # ディレクトリが存在しない場合は作成 + while not stop_event.is_set(): # スレッドが停止されるまでループ try: - balls_position = track_ball_position() - robot_poji = track_robot_position() - if balls_position and robot_poji: - ball_x, ball_y, state = balls_position[-1] + balls_position = track_ball_position() # ボールの位置を追跡 + robot_poji = track_robot_position() # ロボットの位置を追跡 + if balls_position and robot_poji: + ball_x, ball_y, state = balls_position[-1] # ボールの位置の最後の要素を取得 if ball_x > 6000: - if robot_poji and ((ball_x > poji_goal_x and (poji_goal_y > ball_y > nega_goal_y)) or (ball_x < nega_goal_x and (poji_goal_y > ball_y > nega_goal_y))): - df_robot_position = pd.read_csv(robot_position_path) - last_20_frames = df_robot_position.tail(120) - df = pd.DataFrame(last_20_frames) - df.to_csv(robot_poji_goal_path, header=True, index=False) + if robot_poji and ((ball_x > poji_goal_x and (goal_top > ball_y > goal_bottom)) or (ball_x < nega_goal_x and (goal_top > ball_y > goal_bottom))): # ゴールシーンの条件 + df_robot_position = pd.read_csv(robot_position_path) # ロボットのポジションのCSVファイルを読み込み + last_20_frames = df_robot_position.tail(120) # 最後の120フレームを取得 + df = pd.DataFrame(last_20_frames) # DataFrameを作成 + df.to_csv(robot_poji_goal_path, header=True, index=False) # ゴールシーンのCSVファイルを保存 if debug: - print("ゴールシーンを保存しました") - except KeyboardInterrupt: + print("ゴールシーンを保存しました") # デバッグ用にゴールシーンを保存したことを表示 + except KeyboardInterrupt: # キーボード割り込みが発生した場合 break \ No newline at end of file diff --git a/main.py b/main.py index 2cc279a..a969518 100644 --- a/main.py +++ b/main.py @@ -3,39 +3,52 @@ from ball import store_ball_position,track_ball_position,ball_velocity from robot import store_robot_position,track_robot_position from goal import goal_scene +#プレフィックス省略のため from ファイル名 import メソッド名の形をとる。今後メソッドを追加した場合は追記すること。 import os -path = "out/" -stop_event = threading.Event() -debug = False +path = "out/" # 出力先ディレクトリ +stop_event = threading.Event() # スレッド停止用イベント +debug = False # デバッグ用フラグ if __name__ == "__main__": - sock = setup_socket() - # main.py など - thread_ball = threading.Thread( - target=store_ball_position, - args=(udp, receive_packet, receive_game_controller_signal, stop_event, path, debug, sock) + sock = setup_socket() # ソケットのセットアップ + thread_ball = threading.Thread( # ボール位置のスレッド + target=store_ball_position, # ボール位置の保存 + args=(udp, receive_packet, receive_game_controller_signal, stop_event, path, debug, sock) # ボール位置のスレッド引数 ) - thread_ballvelocity = threading.Thread(target=ball_velocity, args=(udp, receive_packet,receive_game_controller_signal, stop_event, path, debug, sock)) - # thread_robot = threading.Thread(target=store_robot_position, args=(udp, receive_packet, path, stop_event, debug)) - # thread_goal = threading.Thread(target=goal_scene, args=(track_ball_position, track_robot_position, path, stop_event, debug)) - + + thread_ballvelocity = threading.Thread( # ボール速度のスレッド + target=ball_velocity, # ボール速度の保存 + args=(udp, receive_packet, receive_game_controller_signal, stop_event, path, debug, sock) # ボール速度のスレッド引数 + ) + + # thread_robot = threading.Thread( # ロボット位置のスレッド + # target=store_robot_position, # ロボット位置の保存 + # args=(udp, receive_packet, path, stop_event, debug) # ロボット位置のスレッド引数 + # ) + + # thread_goal = threading.Thread( # ゴール位置のスレッド + # target=goal_scene, # ゴール位置の保存 + # args=(track_ball_position, track_robot_position, path, stop_event, debug) # ゴール位置のスレッド引数 + # ) + + # スレッドの開始 # thread_goal.start() thread_ball.start() # thread_robot.start() thread_ballvelocity.start() - try: - thread_ball.join() + try: # スレッド終了を待機 + thread_ball.join() # thread_robot.join() # thread_goal.join() thread_ballvelocity.join() - except KeyboardInterrupt: - stop_event.set() + except KeyboardInterrupt: # キーボード割り込みで終了 + stop_event.set() # スレッド停止イベントをセット thread_ball.join() # thread_robot.join() # thread_goal.join() thread_ballvelocity.join() udp.close() - sock.close() + sock.close() print("ソケットを閉じました") \ No newline at end of file diff --git a/network.py b/network.py index ded7e87..558035d 100644 --- a/network.py +++ b/network.py @@ -3,51 +3,51 @@ import messages_robocup_ssl_wrapper_pb2 import ssl_gc_referee_message_pb2 -local = "127.0.0.1" -multicast = "224.5.23.2" -port = 10006 -buffer = 65536 -addr = ('', port) +local = "127.0.0.1" # ローカルアドレスを指定 +multicast = "224.5.23.2" # マルチキャストアドレスを指定 +port = 10006 # ポート番号を指定 +buffer = 65536 # バッファサイズを指定 +addr = ('', port) # アドレスを指定 -udp = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) -udp.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) -udp.bind(addr) -udp.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, socket.inet_aton(multicast) + socket.inet_aton(local)) +udp = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # UDPソケットを作成 +udp.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) # アドレスの再利用を許可 +udp.bind(addr) # ソケットをバインド +udp.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, socket.inet_aton(multicast) + socket.inet_aton(local)) # マルチキャストグループに参加 -def setup_socket(): - global sock - buffer_size = 4096 - multicast_group = '224.5.23.1' - server_address = ('', 10003) - sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) - sock.bind(server_address) - group = socket.inet_aton(multicast_group) - mreq = struct.pack('4sL', group, socket.INADDR_ANY) - sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq) +def setup_socket(): # ソケットのセットアップ + global sock # ソケットをグローバル変数として宣言 + buffer_size = 4096 # バッファサイズを指定 + multicast_group = '224.5.23.1' # マルチキャストグループを指定 + server_address = ('', 10003) # サーバーアドレスを指定 + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # ソケットを作成 + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) # アドレスの再利用を許可 + sock.bind(server_address) # ソケットをバインド + group = socket.inet_aton(multicast_group) # マルチキャストグループを指定 + mreq = struct.pack('4sL', group, socket.INADDR_ANY) # マルチキャストグループへの参加要求を作成 + sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq) # マルチキャストグループに参加 return sock -def receive_packet(udp): - packet = messages_robocup_ssl_wrapper_pb2.SSL_WrapperPacket() - data, _ = udp.recvfrom(buffer) - packet.ParseFromString(data) +def receive_packet(udp): # パケットを受信 + packet = messages_robocup_ssl_wrapper_pb2.SSL_WrapperPacket() # SSL_WrapperPacketを作成 + data, _ = udp.recvfrom(buffer) # データを受信 + packet.ParseFromString(data) # 受信したデータを解析,変換 return packet -def receive_game_controller_signal(sock, stop_event, buffer_size=4096, debug=False): +def receive_game_controller_signal(sock, stop_event, buffer_size=4096, debug=False): # ゲームコントローラーの信号を受信 try: - while not stop_event.is_set(): - data, address = sock.recvfrom(buffer_size) + while not stop_event.is_set(): # 停止イベントがセットされるまでループ + data, address = sock.recvfrom(buffer_size) # データを受信 if debug: - print("データを受信しました from", address) - print("受信データ (バイナリ):", data) + print("データを受信しました from", address) # 受信元アドレスを表示 + print("受信データ (バイナリ):", data) # 受信データ (バイナリ) を表示 try: - referee_message = ssl_gc_referee_message_pb2.Referee() - referee_message.ParseFromString(data) + referee_message = ssl_gc_referee_message_pb2.Referee() # Refereeメッセージを作成 + referee_message.ParseFromString(data) # 受信したデータを解析 if debug: - print("Referee Message (人間が読みやすい形式):") + print("Referee Message (人間が読みやすい形式):") # 人間が読みやすい形式で表示 return referee_message.command - except Exception as e: + except Exception as e: # デコードエラー処理 print("RefereeMessage デコードエラー:", e) print("----------------------") - except KeyboardInterrupt: + except KeyboardInterrupt: # キーボード割り込み処理 print("終了処理を開始します...") \ No newline at end of file diff --git a/robot.py b/robot.py index 19c2e9a..218ef3a 100644 --- a/robot.py +++ b/robot.py @@ -1,44 +1,42 @@ import os import pandas as pd -def track_robot_position(udp, receive_packet, path): +def track_robot_position(udp, receive_packet, path): # ロボットの位置を追跡 if not os.path.isdir(path): - os.mkdir(path) - packet = receive_packet(udp) - robots_yellow = packet.detection.robots_yellow - robots_blue = packet.detection.robots_blue - robot_positions = {"yellow": {}, "blue": {}} + os.mkdir(path) # ディレクトリが存在しない場合は作成 + packet = receive_packet(udp) # パケットを受信 + robots_yellow = packet.detection.robots_yellow # 黄色ロボットの情報 + robots_blue = packet.detection.robots_blue # 青色ロボットの情報 + robot_positions = {"yellow": {}, "blue": {}} # ロボットの位置を格納する辞書 if robots_yellow: - for yellow_robot in robots_yellow: - if 0 <= yellow_robot.robot_id < 17: - robot_positions["yellow"][yellow_robot.robot_id] = (int(yellow_robot.x), int(yellow_robot.y)) + for yellow_robot in robots_yellow: # 黄色ロボットの数だけループ + if 0 <= yellow_robot.robot_id < 17: # ロボットIDが有効な範囲内かチェック + robot_positions["yellow"][yellow_robot.robot_id] = (int(yellow_robot.x), int(yellow_robot.y)) # 黄色ロボットの位置を格納 if robots_blue: - for blue_robot in robots_blue: - if 0 <= blue_robot.robot_id < 17: - robot_positions["blue"][blue_robot.robot_id] = (int(blue_robot.x), int(blue_robot.y)) + for blue_robot in robots_blue: # 青色ロボットの数だけループ + if 0 <= blue_robot.robot_id < 17: # ロボットIDが有効な範囲内かチェック + robot_positions["blue"][blue_robot.robot_id] = (int(blue_robot.x), int(blue_robot.y)) # 青色ロボットの位置を格納 return robot_positions -def store_robot_position(udp, receive_packet, path, stop_event, debug=False): - import os - import pandas as pd - robot_locate = [] - while not stop_event.is_set(): +def store_robot_position(udp, receive_packet, path, stop_event, debug=False): # ロボットの位置を保存 + robot_locate = [] # ロボットの位置を格納するリスト + while not stop_event.is_set(): # スレッドが停止されるまでループ try: - positions = track_robot_position(udp, receive_packet, path) + positions = track_robot_position(udp, receive_packet, path) # ロボットの位置を追跡 if positions: - row = {} - for color in ["yellow", "blue"]: - for robot_id, (x, y) in positions[color].items(): - row[f"{color}_{robot_id}_x"] = x - row[f"{color}_{robot_id}_y"] = y - robot_locate.append(row) - if len(robot_locate) >= 10: - robotPath = os.path.join(path, "robot_position.csv") - df = pd.DataFrame(robot_locate) - write_header = not os.path.exists(robotPath) or os.path.getsize(robotPath) == 0 - df.to_csv(robotPath, mode='a', header=write_header, index=False) - robot_locate.clear() + row = {} # 各フレームのロボットの位置を格納する辞書 + for color in ["yellow", "blue"]: # チームカラーでループ + for robot_id, (x, y) in positions[color].items(): # それぞれの色のロボットを１台ずつ取り出し、IDと位置を取得 + row[f"{color}_{robot_id}_x"] = x # X座標を格納 + row[f"{color}_{robot_id}_y"] = y # Y座標を格納 + robot_locate.append(row) # 各フレームのロボットの位置を格納 + if len(robot_locate) >= 10: # バッファが10フレームに達したら保存 + robotPath = os.path.join(path, "robot_position.csv") # ロボットの位置を保存するCSVファイルのパス + df = pd.DataFrame(robot_locate) # データフレームに変換 + write_header = not os.path.exists(robotPath) or os.path.getsize(robotPath) == 0 # ヘッダーを書き込むかどうか + df.to_csv(robotPath, mode='a', header=write_header, index=False) # CSVファイルに追記 + robot_locate.clear() # バッファをクリア if debug: - print(f"robot_locate (バッファ内): {len(robot_locate)}") - except KeyboardInterrupt: + print(f"robot_locate (バッファ内): {len(robot_locate)}") # デバッグ用にバッファ内のロボット位置データの数を表示 + except KeyboardInterrupt: # キーボード割り込みで終了 break \ No newline at end of file