From dc5e09583acfbc18e748c3412a0f65830d9859ce Mon Sep 17 00:00:00 2001 From: Flim de Jong Date: Wed, 6 Nov 2024 11:09:40 +0100 Subject: [PATCH] Added resetReferee functionality + slight reorganisation --- roboteam_ai/src/RL/RL_Ray/env_ray.py | 2 +- roboteam_ai/src/RL/env.py | 12 +- roboteam_ai/src/RL/src/changeGameState.py | 177 ++++++++++++++++++++ roboteam_ai/src/RL/src/getState.py | 2 +- roboteam_ai/src/RL/src/resetRefereeAPI.py | 86 +++------- roboteam_ai/src/RL/src/sentActionCommand.py | 2 +- 6 files changed, 209 insertions(+), 72 deletions(-) create mode 100644 roboteam_ai/src/RL/src/changeGameState.py diff --git a/roboteam_ai/src/RL/RL_Ray/env_ray.py b/roboteam_ai/src/RL/RL_Ray/env_ray.py index d49161246..f1c2cea5b 100644 --- a/roboteam_ai/src/RL/RL_Ray/env_ray.py +++ b/roboteam_ai/src/RL/RL_Ray/env_ray.py @@ -9,7 +9,7 @@ from ..src.sentActionCommand import send_action_command from ..src.getState import get_ball_state, get_robot_state from ..src.teleportBall import teleport_ball -from ..src.resetReferee import send_referee_reset +from ..src.resetRefereeAPI import send_referee_reset @ray.remote class RoboTeamEnv(gymnasium.Env): diff --git a/roboteam_ai/src/RL/env.py b/roboteam_ai/src/RL/env.py index ecde5f5e0..dde3f17c3 100644 --- a/roboteam_ai/src/RL/env.py +++ b/roboteam_ai/src/RL/env.py @@ -7,9 +7,10 @@ # Now import the functions from src.sentActionCommand import send_action_command -from src.getState import get_ball_state, get_robot_state +from src.getState import get_ball_state, get_robot_state, get_referee_state from src.teleportBall import teleport_ball -from src.resetReferee import send_referee_reset +from roboteam_ai.src.RL.src.resetRefereeAPI import reset_referee_state +from src.changeGameState import start_game """ This environment file is in the form of a gymnasium environment. @@ -164,7 +165,6 @@ def step(self, action): return observation_space, reward, done, truncated - def is_terminated(self): """ Activates when the task has been completed (or it failed because of opponent scoring a goal) @@ -181,7 +181,6 @@ def is_truncated(self): # Implement logic to reset the game if no goal is scored pass - def reset(self, seed=None): """ The reset function resets the environment when a game is ended @@ -191,7 +190,10 @@ def reset(self, seed=None): teleport_ball(0,0) # Reset referee state - send_referee_reset() # This resets the cards, goals and initiates a kickoff. + reset_referee_state() + + # Set blue team on right side + initiates kickoff + start_game() # Reset shaped_reward_given boolean self.shaped_reward_given = False diff --git a/roboteam_ai/src/RL/src/changeGameState.py b/roboteam_ai/src/RL/src/changeGameState.py new file mode 100644 index 000000000..d68b8bd27 --- /dev/null +++ b/roboteam_ai/src/RL/src/changeGameState.py @@ -0,0 +1,177 @@ +import sys +import os +import websockets +import asyncio +import json +import time + +async def set_team_state(team, on_positive_half): + """ + Set team state including which half they play on. + + Args: + team (str): Either "BLUE" or "YELLOW" + on_positive_half (bool): True if team should play on positive half + """ + uri = "ws://localhost:8081/api/control" + + try: + async with websockets.connect(uri) as websocket: + # Similar structure to first_kickoff_team + state_msg = { + "change": { + "update_team_state_change": { + "for_team": team, + "on_positive_half": on_positive_half + } + } + } + + print(f"Setting {team} team to play on {'positive' if on_positive_half else 'negative'} half...") + await websocket.send(json.dumps(state_msg)) + + try: + response = await asyncio.wait_for(websocket.recv(), timeout=2.0) + print("Received response:", json.loads(response)) + except asyncio.TimeoutError: + print("No response received in 2 seconds") + + except websockets.exceptions.ConnectionClosed as e: + print(f"WebSocket connection closed: {e}") + except Exception as e: + print(f"Error: {e}") + +async def send_referee_command(command_type, team=None): + """ + Send a referee command using websockets. + + Args: + command_type (str): One of: + HALT, STOP, NORMAL_START, FORCE_START, DIRECT, + KICKOFF, PENALTY, TIMEOUT, BALL_PLACEMENT + team (str, optional): For team-specific commands, either "YELLOW" or "BLUE" + """ + uri = "ws://localhost:8081/api/control" + + try: + async with websockets.connect(uri) as websocket: + command_msg = { + "change": { + "new_command_change": { + "command": { + "type": command_type, + "for_team": team if team else "UNKNOWN" + } + } + } + } + + print(f"Sending referee command: {command_type} {'for ' + team if team else ''}...") + await websocket.send(json.dumps(command_msg)) + + try: + response = await asyncio.wait_for(websocket.recv(), timeout=2.0) + print("Received response:", json.loads(response)) + except asyncio.TimeoutError: + print("No response received in 2 seconds") + + except websockets.exceptions.ConnectionClosed as e: + print(f"WebSocket connection closed: {e}") + except Exception as e: + print(f"Error: {e}") + +async def set_first_kickoff_team(team): + """ + Set which team takes the first kickoff. + + Args: + team (str): Either "BLUE" or "YELLOW" + """ + uri = "ws://localhost:8081/api/control" + + try: + async with websockets.connect(uri) as websocket: + config_msg = { + "change": { + "update_config_change": { + "first_kickoff_team": team + } + } + } + + print(f"Setting first kickoff team to {team}...") + await websocket.send(json.dumps(config_msg)) + + try: + response = await asyncio.wait_for(websocket.recv(), timeout=2.0) + print("Received response:", json.loads(response)) + except asyncio.TimeoutError: + print("No response received in 2 seconds") + + except websockets.exceptions.ConnectionClosed as e: + print(f"WebSocket connection closed: {e}") + except Exception as e: + print(f"Error: {e}") + +async def halt(): + """Send HALT referee command - stops all robots immediately""" + await send_referee_command("HALT") + +async def stop(): + """Send STOP referee command - stops game but robots can move""" + await send_referee_command("STOP") + +async def force_start(): + """Send FORCE_START referee command - starts game immediately""" + await send_referee_command("FORCE_START") + +async def normal_start(): + """Send NORMAL_START referee command - starts game normally""" + await send_referee_command("NORMAL_START") + +async def direct_free_kick(team): + """Send DIRECT referee command - awards direct free kick to team""" + await send_referee_command("DIRECT", team) + +async def kickoff(team): + """Send KICKOFF referee command - starts kickoff for team""" + await send_referee_command("KICKOFF", team) + +async def penalty(team): + """Send PENALTY referee command - starts penalty for team""" + await send_referee_command("PENALTY", team) + +async def ball_placement(team): + """Send BALL_PLACEMENT referee command - starts ball placement for team""" + await send_referee_command("BALL_PLACEMENT", team) + +async def timeout(team): + """Send TIMEOUT referee command - starts timeout for team""" + await send_referee_command("TIMEOUT", team) + +async def start_game(): + """Set Blue team as first kickoff, on positive half, and start the game properly""" + # Set sides for teams + await set_team_state("BLUE", True) # Blue on positive half + await set_team_state("YELLOW", False) # Yellow on negative half + + # Set Blue for first kickoff + await set_first_kickoff_team("BLUE") + + # Start sequence + await halt() # First halt to ensure safe state + await stop() # Then stop to prepare for start + await asyncio.sleep(10) + await kickoff("BLUE") # Set up kickoff for Blue team + # await normal_start() # Start the game normally + +if __name__ == "__main__": + print("Connecting to game controller...") + + # To set up and start a game with Blue kickoff and on positive half: + asyncio.get_event_loop().run_until_complete(start_game()) + + # Or use individual commands as needed: + # asyncio.get_event_loop().run_until_complete(set_team_state("BLUE", True)) + # asyncio.get_event_loop().run_until_complete(halt()) + # asyncio.get_event_loop().run_until_complete(normal_start()) \ No newline at end of file diff --git a/roboteam_ai/src/RL/src/getState.py b/roboteam_ai/src/RL/src/getState.py index ed81750df..d281fbd54 100644 --- a/roboteam_ai/src/RL/src/getState.py +++ b/roboteam_ai/src/RL/src/getState.py @@ -9,7 +9,7 @@ # Make sure to go back to the main roboteam directory current_dir = os.path.dirname(os.path.abspath(__file__)) -roboteam_path = os.path.abspath(os.path.join(current_dir, "..", "..", "..")) +roboteam_path = os.path.abspath(os.path.join(current_dir, "..", "..", "..", "..")) # Add to sys.path sys.path.append(roboteam_path) diff --git a/roboteam_ai/src/RL/src/resetRefereeAPI.py b/roboteam_ai/src/RL/src/resetRefereeAPI.py index b2b81d1d2..39384d059 100644 --- a/roboteam_ai/src/RL/src/resetRefereeAPI.py +++ b/roboteam_ai/src/RL/src/resetRefereeAPI.py @@ -1,75 +1,33 @@ import sys import os -import socket -import struct -import time -from google.protobuf.timestamp_pb2 import Timestamp -from roboteam_ai.src.RL.src.getState import get_referee_state +import websockets +import asyncio +import json -current_dir = os.path.dirname(os.path.abspath(__file__)) -roboteam_path = os.path.abspath(os.path.join(current_dir, "..", "..", "..")) - -# Add to sys.path -sys.path.append(roboteam_path) - -from roboteam_networking.proto.ssl_gc_api_pb2 import Input - -def send_reset_match_command(host="0.0.0.0", port=10003): - """ - Send a reset_match command to the SSL Game Controller - - Args: - host (str): The hostname where the game controller is running - port (int): The port number the game controller is listening on - """ - # Create the input message - input_msg = Input() - - # Set the reset_match flag to True - input_msg.reset_match = True +async def reset_referee_state(): + uri = "ws://localhost:8081/api/control" try: - # Create a TCP socket - with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as sock: - # Set a timeout of 5 seconds - sock.settimeout(5) + async with websockets.connect(uri) as websocket: + # Create JSON message + reset_msg = { + "reset_match": True + } - # Connect to the game controller - sock.connect((host, port)) + print("Sending JSON reset command...") + await websocket.send(json.dumps(reset_msg)) - # Serialize the message - serialized_msg = input_msg.SerializeToString() - msg_length = len(serialized_msg) - - print(f"\nMessage length: {msg_length} bytes") - print(f"Length bytes: {msg_length.to_bytes(4, byteorder='big').hex()}") - print(f"Message bytes: {serialized_msg.hex()}") - - # Send the message length - length_bytes = msg_length.to_bytes(4, byteorder='big') - bytes_sent = sock.send(length_bytes) - print(f"\nSent {bytes_sent} length bytes") - - # Send the actual message - bytes_sent = sock.send(serialized_msg) - print(f"Sent {bytes_sent} message bytes") - - # Wait for response try: - response = sock.recv(3000) - if response: - print("Reset match command sent and response received") - else: - print("Reset match command sent but no response") - except socket.timeout: - print("Reset match command sent but timed out waiting for response") - - except ConnectionRefusedError: - print("Could not connect to the game controller. Is it running?") + response = await asyncio.wait_for(websocket.recv(), timeout=2.0) + print("Received response:", json.loads(response)) + except asyncio.TimeoutError: + print("No response received in 2 seconds") + + except websockets.exceptions.ConnectionClosed as e: + print(f"WebSocket connection closed: {e}") except Exception as e: - print(f"An error occurred: {e}") + print(f"Error: {e}") -# Example usage if __name__ == "__main__": - # You can customize the host and port if needed - send_reset_match_command() \ No newline at end of file + print("Connecting to game controller...") + asyncio.get_event_loop().run_until_complete(reset_referee_state()) \ No newline at end of file diff --git a/roboteam_ai/src/RL/src/sentActionCommand.py b/roboteam_ai/src/RL/src/sentActionCommand.py index 145442458..854ea8548 100644 --- a/roboteam_ai/src/RL/src/sentActionCommand.py +++ b/roboteam_ai/src/RL/src/sentActionCommand.py @@ -4,7 +4,7 @@ # Make sure to go back to the main roboteam directory current_dir = os.path.dirname(os.path.abspath(__file__)) -roboteam_path = os.path.abspath(os.path.join(current_dir, "..", "..", "..")) +roboteam_path = os.path.abspath(os.path.join(current_dir, "..", "..", "..", "..")) # Add to sys.path sys.path.append(roboteam_path)