rcj_soccer_robot.py

import math
import struct
import numpy as np
TIME_STEP = 32
ROBOT_NAMES = ["B1", "B2", "B3", "Y1", "Y2", "Y3"]
N_ROBOTS = len(ROBOT_NAMES)


class RCJSoccerRobot:
    def __init__(self, robot):
        self.robot = robot
        self.name = self.robot.getName()
        self.team = self.name[0]
        self.player_id = int(self.name[1])

        self.receiver = self.robot.getDevice("supervisor receiver")
        self.receiver.enable(TIME_STEP)

        self.team_emitter = self.robot.getDevice("team emitter")
        self.team_receiver = self.robot.getDevice("team receiver")
        self.team_receiver.enable(TIME_STEP)

        self.ball_receiver = self.robot.getDevice("ball receiver")
        self.ball_receiver.enable(TIME_STEP)

        self.gps = self.robot.getDevice("gps")
        self.gps.enable(TIME_STEP)

        self.compass = self.robot.getDevice("compass")
        self.compass.enable(TIME_STEP)

        self.sonar_left = self.robot.getDevice("distancesensor left")
        self.sonar_left.enable(TIME_STEP)
        self.sonar_right = self.robot.getDevice("distancesensor right")
        self.sonar_right.enable(TIME_STEP)
        self.sonar_front = self.robot.getDevice("distancesensor front")
        self.sonar_front.enable(TIME_STEP)
        self.sonar_back = self.robot.getDevice("distancesensor back")
        self.sonar_back.enable(TIME_STEP)

        self.left_motor = self.robot.getDevice("left wheel motor")
        self.right_motor = self.robot.getDevice("right wheel motor")

        self.left_motor.setPosition(float("+inf"))
        self.right_motor.setPosition(float("+inf"))

        self.left_motor.setVelocity(0.0)
        self.right_motor.setVelocity(0.0)

    def parse_supervisor_msg(self, packet: str) -> dict:
        # Parse message received from supervisor

        # Returns:
        #     dict: Location info about each robot and the ball.
        #     Example:
        #         {
        #             'waiting_for_kickoff': False,
        #         }
        # True/False telling whether the goal was scored
        struct_fmt = "?"
        unpacked = struct.unpack(struct_fmt, packet)

        data = {"waiting_for_kickoff": unpacked[0]}
        return data

    def get_new_data(self) -> dict:
        # Read new data from supervisor

        # Returns:
        #     dict: See `parse_supervisor_msg` method
        
        packet = self.receiver.getData()
        self.receiver.nextPacket()
        return self.parse_supervisor_msg(packet)

    def is_new_data(self) -> bool:
        # Check if there is new data from supervisor to be received

        # Returns:
        #     bool: Whether there is new data received from supervisor.
        
        return self.receiver.getQueueLength() > 0

    def parse_team_msg(self, packet: str) -> dict:
        
        # Parse message received from team robot

        # Returns:
        #     dict: Parsed message stored in dictionary.


        # Args:
        #     robot_id (int): ID of the robot
        #     robot_pos_x(double)
        #     robot_pos_y(double)
        #     robot_pos_direction(double)
        #     is_get_ball_date(bool)
        #     get_ball_data_direction(double)
        #     get_ball_data_strength(double)
        
        struct_fmt = "iddd?dd"
        unpacked = struct.unpack(struct_fmt, packet)
        data = {
            "robot_id": unpacked[0],
            "pos_x":unpacked[1],
            "pos_y":unpacked[2],
            "pos_dir":unpacked[3],
            "is_ball_data":unpacked[4],
            "ball_x":unpacked[5],
            "ball_y":unpacked[6],
        }
        return data

    def get_new_team_data(self) -> dict:
        # Read new data from team robot

        # Returns:
        #     dict: See `parse_team_msg` method
        packet = self.team_receiver.getData()
        self.team_receiver.nextPacket()
        return self.parse_team_msg(packet)

    def is_new_team_data(self) -> bool:
        # Check if there is new data from team robots to be received

        # Returns:
        #     bool: Whether there is new data received from team robots.
        
        return self.team_receiver.getQueueLength() > 0

    def send_data_to_team(self, send_data) -> None:
        #
        # Send data to the team

        # Args:
        #     robot_id (int): ID of the robot
        #     robot_pos_x(double)
        #     robot_pos_y(double)
        #     robot_pos_direction(double)
        #     is_get_ball_date(bool)
        #     ball_x(double)
        #     ball_y(double)
            
        #
        struct_fmt = "iddd?dd"
        packet = struct.pack(struct_fmt, *send_data)
        self.team_emitter.send(packet)

    def get_new_ball_data(self) -> dict:
        """
        Read new data from IR sensor

        Returns:
            dict: Direction and strength of the ball signal
            Direction is normalized vector indicating the direction of the
            emitter with respect to the receiver's coordinate system.
            Example:
                {
                    'direction': [0.23, -0.10, 0.96],
                    'strength': 0.1
                }
        """
        _ = self.ball_receiver.getData()
        data = {
            "direction": self.ball_receiver.getEmitterDirection(),
            "strength": self.ball_receiver.getSignalStrength(),
        }
        self.ball_receiver.nextPacket()
        return data

    def is_new_ball_data(self) -> bool:
        """Check if there is new data from ball to be received

        Returns:
            bool: Whether there is new data received from ball.
        """
        return self.ball_receiver.getQueueLength() > 0

    def get_gps_coordinates(self) -> list:
        """Get new GPS coordinates

        Returns:
            List containing x and y values
        """
        gps_values = self.gps.getValues()
        return [gps_values[0], gps_values[1]]

    def get_compass_heading(self) -> float:
        """Get compass heading in radians

        Returns:
            float: Compass value in radians
        """
        compass_values = self.compass.getValues()

        # Subtract math.pi/2 (90) so that the heading 0 means that
        # robot is facing opponent's goal
        rad = math.atan2(compass_values[0], compass_values[1]) - (math.pi / 2)
        if rad < -math.pi:
            rad = rad + (2 * math.pi)

        return rad

    def get_sonar_values(self) -> dict:
        """Get new values from sonars.

        Returns:
            dict: Value for each sonar.
        """
        return {
            "left": self.sonar_left.getValue(),
            "right": self.sonar_right.getValue(),
            "front": self.sonar_front.getValue(),
            "back": self.sonar_back.getValue(),
        }

    def run(self):
        raise NotImplementedError