startup

#!/usr/bin/env python3
import sys
import os
import re
from utils import *

bright(f"[SECURITY] Killing all KidSize running on your computer ...")
os.system(f"killall KidSize")

# Expected team ID for games
expected_team_id: int = 11

# Game controller ip (for filtering)
game_controller_ip: str = "192.168.0.1"

# Expected team ID for drop-in games
drop_in_ids: dict = {"a": 31, "b": 32}
drop_in_field = None

# Fields
# - l/r: grass orientation
fields = {"r": ["r"], "a": ["l"], "b": ["l"], "c": ["l"], "d": ["l"], "e": ["l"]}

# Positions
positions: dict = {
    "A": [
        [-0.75, 3.1, -90],  # Start pos
        [2.5, 1.0, 0],  # Patrol1
        [-1.50, 1.0, 180],  # Patrol2
    ],
    "B": [
        [-0.75, -3.1, 90],
        [2.5, -1.0, 0],
        [-1.5, -1.0, 180],
    ],
    "C": [
        [-3, -3.1, 90],
        [-2.5, 1.5, 90],
        [-2.5, -1.5, -90],
    ],
    "D": [
        [-3, 3.1, -90],
        [0, 0, 0],
        [0, 0, 0],
    ],
}

# Default roles
roles: dict = {"olive": "A", "nova": "B", "arya": "C", "tom": "C", "rush": "D"}

# Retrieving ip and hostname
ip: str = "10.0.0.1" if len(sys.argv) == 1 else sys.argv[1]
hostname: str = rhio_get_value(ip, "/server/hostname")

# Filtering game controller packets only
rhio(ip, f"/referee/ipFilter={game_controller_ip}")

bright(f"[STARTUP] Running a startup to {ip}, hostname is {hostname}")

# Checking devices
while True:
    bright(f"[CHECK] Checking that devices are present...")
    result = rhio(ip, "rhalCheck")
    if "All devices are present" not in result:
        error(result)

        if not yes_no("Devices are missing, try again ?", "y"):
            break
    else:
        success("[CHECK] All devices are present")
        break

# Checking that team is is properly set
is_drop_in: bool = yes_no("[DROP-IN] Is it a drop-in game ?", "n")
if is_drop_in:
    options = ", ".join([f"{key}: {value}" for key, value in zip(drop_in_ids.keys(), drop_in_ids.values())])
    dropin_team: str = question_answers(f"[DROP-IN-ID] What team are we ({options}) ?", list(drop_in_ids.keys()))
    dropin_id: int = drop_in_ids[dropin_team]
    rhio(ip, f"/referee/teamId={dropin_id}")
else:
    team_id: int = int(rhio_get_value(ip, "/referee/teamId"))
    if team_id != expected_team_id:
        warning(f"[TEAM-ID] Team id is {team_id} while expected {expected_team_id}")
        if yes_no(f"Change the team ID back to {expected_team_id} ?"):
            rhio(ip, f"/referee/teamId={expected_team_id}")

if is_drop_in and drop_in_field is not None:
    field: str = drop_in_field
else:
    field: str = question_answers(f"[FIELD] What field are we playing on ?", list(fields.keys()))

if yes_no("[WIFI] Do you want to run the WiFi script ?"):
    network_name: str = f"field_{field}"
    os.system(f"./wifi {ip} {network_name}")

bright("[WIFI] Disabling power save")
os.system(f"ssh rhoban@{ip} sudo iw dev wlan0 set power_save off")

# Pinging the game controller
os.system(f"ssh rhoban@{ip} ping {game_controller_ip} -c 4")

# Checking for the Game Controller to be seen by the robot
while True:
    result = rhio(ip, "infoPlaying")
    last_update = re.match(r"(.+)Referee last update: (.+).", result, re.DOTALL + re.MULTILINE)

    if last_update and float(last_update[2]) > 5:
        error(f"[REFEREE] Update from the referee is outdated ({float(last_update[2])} s)")
        if not yes_no("[REFEREE] Try again ?", "y"):
            break
    else:
        success("[REFEREE] Referee is present")
        break

bright("[INIT] Press ENTER to run init")
input()
rhio(ip, "init")

bright("[WALK] Press ENTER to run walk")
input()
rhio(ip, "walk")
rhio(ip, "/moves/arms/armsState=2")

while True:
    bright("[TARE] Hold me in the air for the tare and press ENTER")
    input()
    bright("[TARE] Taring...")
    result = rhio(ip, "tare")
    if "Error" in result:
        error(result)
        if not yes_no("[TARE] Error while taring, try again ?", "y"):
            break
    else:
        success("[TARE] Tare successful")
        break

while True:
    try:
        vision_last_update: int = float(rhio_get_value(ip, "/Vision/lastUpdate"))
        if vision_last_update > 200:
            error(f"[VISION] Last update from the vision is old ({vision_last_update} ms)")
            if not yes_no("[VISION] Try again ?", "y"):
                break
        else:
            success("[VISION] Vision seems good")
            break
    except ValueError:
        error(f"[VISION] Can't retrieve last update from vision")
        if not yes_no("[VISION] Try again ?", "y"):
            break

# Setting the grass orientation
side: str = question_answers("[SIDE] What side are we attacking ? ", ["l", "r"])
(grass_orientation,) = fields[field]

if side == grass_orientation:
    bright("[GRASS] We play with grass")
    rhio(ip, "/strategy/with_grass=1")
else:
    bright("[GRASS] We play against the grass")
    rhio(ip, "/strategy/with_grass=0")


field_view_r: str = """
                                                   
   ╔═════ D ════════ A ═══╦══════════════════════╗ 
   ║                      ║                      ║ 
   ║                      ║                      ║ 
   ╟──┐              A    ║                      ╢ 
   ║  │                   ║                      ║ 
   ║  │                 .-╬-.                    ║ 
   ║ D│   +          B (  ╬  )     opponents     ║ 
   ║  │                 .-╬-.                    ║ 
   ║  │   C               ║                      ║ 
   ╟──┘                   ║                      ╢ 
   ║                      ║                      ║ 
   ║                      ║                      ║ 
   ╚═════ C ════════ B ═══╩══════════════════════╝ 
                                                   
"""

field_view_l: str = """
                                                   
   ╔══════════════════════╦═══ B ═══════ C ══════╗ 
   ║                      ║                      ║ 
   ║                      ║                      ║ 
   ╟                      ║                   ┌──╢ 
   ║                      ║              C    │  ║ 
   ║                    .-╬-.                 │  ║ 
   ║      opponents    (  ╬  ) B         +    │D ║ 
   ║                    .-╬-.                 │  ║ 
   ║                      ║    A              │  ║ 
   ╟                      ║                   └──╢ 
   ║                      ║                      ║ 
   ║                      ║                      ║ 
   ╚══════════════════════╩═══ A ═══════ D ══════╝ 
                                                   
"""

field_view: str = field_view_r if side == "r" else field_view_l

# Setting the background to be green
field_view = Back.GREEN + Fore.WHITE + field_view + Style.RESET_ALL

if hostname in roles:
    role: str = roles[hostname]
else:
    print(field_view)
    role: str = question_answers("[ROLE] What is this robot role ?", list(positions.keys()))


field_view = field_view.replace(role, Fore.BLACK + Style.BRIGHT + role + Style.NORMAL + Fore.WHITE)

# Goal keeper / free kicker
is_goal_keeper: int = 1 if role == "D" else 0
is_free_kicker: int = 1 if role == "A" else 0

rhio(ip, f"/moves/robocup/goalKeeper={is_goal_keeper}")

# Positions on the field
initial_position, patrol1, patrol2 = positions[role]
configuration: str = ""

# Setting initial position on the field
configuration += f"/moves/robocup/autoStartX={initial_position[0]}\n"
configuration += f"/moves/robocup/autoStartY={initial_position[1]}\n"
configuration += f"/moves/robocup/autoStartAzimuth={initial_position[2]}\n"

# Setting patrols target
configuration += f"/moves/search/P1X={patrol1[0]}\n"
configuration += f"/moves/search/P1Y={patrol1[1]}\n"
configuration += f"/moves/search/P1Azimuth={patrol1[2]}\n"
configuration += f"/moves/search/P2X={patrol2[0]}\n"
configuration += f"/moves/search/P2Y={patrol2[1]}\n"
configuration += f"/moves/search/P2Azimuth={patrol2[2]}\n"

# XXX: To investigate, not sure why this is here
configuration += f"/moves/search/beginY=1.50\n"
configuration += f"/moves/search/beginY=-1.50\n"
configuration += "exit\n"

rhio_commands(ip, configuration)

# Logging the game
if yes_no("[LOG] Should we log ?"):
    rhio(ip, "/Vision/autologGames=true")
else:
    rhio(ip, "/Vision/autologGames=false")

if yes_no("[CAMPING] Enable camping mode ?"):
  rhio(ip, "setPlayersPlacer camping")

bright("[REMINDER] Place the robot accordingly to this view:")
print(field_view)

success("[YODA] May the force be with you")
rhio(ip, "robocup")