From cd8b238599941e1099f55ad156e84a399f4bfd22 Mon Sep 17 00:00:00 2001 From: fred-huang122 Date: Tue, 19 Nov 2024 18:25:30 +0000 Subject: [PATCH 1/8] Updated the source location of the repo, fixed imports --- ...eceiver_sim_controller_integration_test.py | 2 +- .../src/tests/referee_receiver_test.py | 2 +- .../src/tests/robot_controller_test.py | 27 +++++++++++++++++++ .../src/tests/sim_controller_test.py | 4 +-- team_controller/src/tests/startup_test.py | 10 +++---- 5 files changed, 36 insertions(+), 9 deletions(-) create mode 100644 team_controller/src/tests/robot_controller_test.py diff --git a/team_controller/src/tests/ref_receiver_sim_controller_integration_test.py b/team_controller/src/tests/ref_receiver_sim_controller_integration_test.py index de216551..e8db7553 100644 --- a/team_controller/src/tests/ref_receiver_sim_controller_integration_test.py +++ b/team_controller/src/tests/ref_receiver_sim_controller_integration_test.py @@ -14,7 +14,7 @@ # TODO: Imcomplete implementation # Add the project root directory to sys.path -project_root = os.path.abspath(os.path.join(os.path.dirname(__file__), "../")) +project_root = os.path.abspath(os.path.join(os.path.dirname(__file__), "../../../")) print(project_root) sys.path.insert(0, project_root) diff --git a/team_controller/src/tests/referee_receiver_test.py b/team_controller/src/tests/referee_receiver_test.py index 9f0631ad..5eb41256 100644 --- a/team_controller/src/tests/referee_receiver_test.py +++ b/team_controller/src/tests/referee_receiver_test.py @@ -13,7 +13,7 @@ # Add the project root directory to sys.path -project_root = os.path.abspath(os.path.join(os.path.dirname(__file__), "../")) +project_root = os.path.abspath(os.path.join(os.path.dirname(__file__), "../../../")) print(project_root) sys.path.insert(0, project_root) diff --git a/team_controller/src/tests/robot_controller_test.py b/team_controller/src/tests/robot_controller_test.py new file mode 100644 index 00000000..e6b6b76b --- /dev/null +++ b/team_controller/src/tests/robot_controller_test.py @@ -0,0 +1,27 @@ +import os +import sys +import threading + +# Add the project root directory to sys.path +project_root = os.path.abspath(os.path.join(os.path.dirname(__file__), "../../../")) +print(project_root) +sys.path.insert(0, project_root) + +from team_controller.src.data.vision_receiver import VisionDataReceiver +from team_controller.src.controllers.robot_startup_controller import StartUpController + +if __name__ == "__main__": + vision_receiver = VisionDataReceiver(debug=True) + decision_maker = StartUpController(vision_receiver, debug=False) + + vision_thread = threading.Thread(target=vision_receiver.pull_game_data) + command_thread = threading.Thread(target=decision_maker.startup) + + vision_thread.start() + command_thread.start() + + try: + vision_thread.join() + command_thread.join() + except KeyboardInterrupt: + print("Exiting...") diff --git a/team_controller/src/tests/sim_controller_test.py b/team_controller/src/tests/sim_controller_test.py index 53f5b155..5ca4f306 100644 --- a/team_controller/src/tests/sim_controller_test.py +++ b/team_controller/src/tests/sim_controller_test.py @@ -2,11 +2,11 @@ import sys # Add the project root directory to sys.path -project_root = os.path.abspath(os.path.join(os.path.dirname(__file__), "../")) +project_root = os.path.abspath(os.path.join(os.path.dirname(__file__), "../../../")) print(project_root) sys.path.insert(0, project_root) -from controllers.sim_controller import SimulatorController +from team_controller.src.controllers.sim_controller import SimulatorController def main(): controller = SimulatorController() diff --git a/team_controller/src/tests/startup_test.py b/team_controller/src/tests/startup_test.py index e1eeaae9..afc58fa9 100644 --- a/team_controller/src/tests/startup_test.py +++ b/team_controller/src/tests/startup_test.py @@ -3,16 +3,16 @@ import threading # Add the project root directory to sys.path -project_root = os.path.abspath(os.path.join(os.path.dirname(__file__), "../")) +project_root = os.path.abspath(os.path.join(os.path.dirname(__file__), "../../../")) print(project_root) sys.path.insert(0, project_root) -from data.vision_receiver import VisionDataReceiver -from controllers.robot_startup_controller import StartUpController +from team_controller.src.data.vision_receiver import VisionDataReceiver +from team_controller.src.controllers.robot_startup_controller import StartUpController if __name__ == "__main__": - vision_receiver = VisionDataReceiver(debug=True) - decision_maker = StartUpController(vision_receiver, debug=False) + vision_receiver = VisionDataReceiver(debug=False) + decision_maker = StartUpController(vision_receiver=vision_receiver, debug=True) vision_thread = threading.Thread(target=vision_receiver.pull_game_data) command_thread = threading.Thread(target=decision_maker.startup) From a84a88e7cf8f85e11477ab5f1c879d14fc7beeb0 Mon Sep 17 00:00:00 2001 From: fred-huang122 Date: Tue, 19 Nov 2024 18:26:09 +0000 Subject: [PATCH 2/8] Typing --- team_controller/src/data/vision_receiver.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/team_controller/src/data/vision_receiver.py b/team_controller/src/data/vision_receiver.py index 4929db9d..95f468f4 100644 --- a/team_controller/src/data/vision_receiver.py +++ b/team_controller/src/data/vision_receiver.py @@ -74,7 +74,7 @@ def __update_team_robots_pos( ) # TODO: When do we not have orientation? - def _print_frame_info(self, t_received: float, detection: object): + def _print_frame_info(self, t_received: float, detection: object) -> None: t_now = time.time() print(f"Time Now: {t_now:.3f}s") print( @@ -91,7 +91,7 @@ def _print_frame_info(self, t_received: float, detection: object): f"{((t_now - t_received) + (detection.t_sent - detection.t_capture)) * 1000.0:.3f}ms" ) - def get_frame_data(self): + def get_frame_data(self) -> FrameData: """ Retrieve all relevant data for this frame. Combination of timestep, ball and robot data. From d015481d95902dd996372a2a9288510c15883c26 Mon Sep 17 00:00:00 2001 From: fred-huang122 Date: Tue, 19 Nov 2024 18:42:37 +0000 Subject: [PATCH 3/8] Added new function to receive response packets (Only used for sim) --- team_controller/src/utils/network_manager.py | 8 +++++--- team_controller/src/utils/network_utils.py | 14 +++++++++++--- 2 files changed, 16 insertions(+), 6 deletions(-) diff --git a/team_controller/src/utils/network_manager.py b/team_controller/src/utils/network_manager.py index 59824464..7bd74036 100644 --- a/team_controller/src/utils/network_manager.py +++ b/team_controller/src/utils/network_manager.py @@ -19,17 +19,19 @@ def __init__(self, address: Tuple[str, int], bind_socket: bool = False): socket.socket(socket.AF_INET, socket.SOCK_DGRAM), address, bind_socket ) - def send_command(self, command: object) -> None: + def send_command(self, command: object, is_sim_robot_cmd: bool = False) -> None: """ Sends a command to the server at the specified address. Args: - command object: An object with in the form of a protocol buffer message to be serialized and sent. + command (object): An object with in the form of a protocol buffer message to be serialized and sent. + is_sim_robot_cmd (bool): If True, the function will attempt to receive a response from the server. (only used when sending robot control cmd) This method relies on a utility function for command transmission. """ # Send a command to the server. - network_utils.send_command(self.address, command) + return network_utils.send_command(self.address, command, is_sim_robot_cmd) + def receive_data(self) -> Optional[bytes]: """ diff --git a/team_controller/src/utils/network_utils.py b/team_controller/src/utils/network_utils.py index d50fd43c..db4b08de 100644 --- a/team_controller/src/utils/network_utils.py +++ b/team_controller/src/utils/network_utils.py @@ -1,7 +1,7 @@ import socket import struct import logging -from typing import Optional, Any, Tuple +from typing import Optional, Tuple from team_controller.src.config.settings import MULTICAST_GROUP, LOCAL_HOST @@ -42,7 +42,7 @@ def setup_socket( mreq = struct.pack("4sL", group, socket.INADDR_ANY) sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq) - sock.settimeout(1.0) # Set timeout to 1 second + sock.settimeout(0.005) # Set timeout to 1 frame period (60 FPS) logging.info( "Socket setup completed with address %s and bind_socket=%s", address, @@ -81,15 +81,20 @@ def receive_data(sock: socket.socket) -> Optional[bytes]: return None -def send_command(address: Tuple[str, int], command: object) -> None: +def send_command(address: Tuple[str, int], command: object, is_sim_robot_cmd: bool = False) -> Optional[bytes]: """ Sends a command to the specified address over a UDP socket. Args: address (Tuple[str, int]): The destination IP address and port. command object: An object with in the form of a protocol buffer message to be serialized and sent. + is_sim_robot_cmd (bool): If True, the function will attempt to receive a response from the server. + Returns: + Optional[bytes]: The data received, or None if no data is received or if an error occurs. + This function creates a temporary UDP socket, serializes the command, and sends it to the specified address. + If the command being sent is a RobotControl packet there will be a response packet which will be received. Errors during serialization or socket operations are logged, with specific handling if the `SerializeToString` method is missing. """ @@ -97,6 +102,9 @@ def send_command(address: Tuple[str, int], command: object) -> None: with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as send_sock: serialized_command = command.SerializeToString() send_sock.sendto(serialized_command, address) + if is_sim_robot_cmd: + data = receive_data(send_sock) + return data logging.info("Command sent to %s", address) except AttributeError: logging.error("Command object has no SerializeToString method") From 9ae1cc5c0f025473a7af5a2b62601be86004fe45 Mon Sep 17 00:00:00 2001 From: fred-huang122 Date: Tue, 19 Nov 2024 18:43:03 +0000 Subject: [PATCH 4/8] added a simulator robot controller --- .../src/controllers/sim_robot_controller.py | 107 ++++++++++++++++++ 1 file changed, 107 insertions(+) create mode 100644 team_controller/src/controllers/sim_robot_controller.py diff --git a/team_controller/src/controllers/sim_robot_controller.py b/team_controller/src/controllers/sim_robot_controller.py new file mode 100644 index 00000000..3723a420 --- /dev/null +++ b/team_controller/src/controllers/sim_robot_controller.py @@ -0,0 +1,107 @@ +import time +import threading +import numpy as np +from typing import Tuple, Optional, Dict, List, Union + +from global_utils.math_utils import rotate_vector +from team_controller.src.data.vision_receiver import VisionDataReceiver +from motion_planning.src.pid.pid import PID +from team_controller.src.config.settings import ( + PID_PARAMS, + LOCAL_HOST, + YELLOW_TEAM_SIM_PORT, + BLUE_TEAM_SIM_PORT, + YELLOW_START, +) +from team_controller.src.utils import network_manager + +from team_controller.src.generated_code.ssl_simulation_robot_control_pb2 import ( + RobotControl, +) +from team_controller.src.generated_code.ssl_simulation_robot_feedback_pb2 import ( + RobotControlResponse, RobotFeedback +) + +# TODO: To be moved to a High-level Descision making repo + + +class SimRobotController: + def __init__( + self, + address=LOCAL_HOST, + port=(YELLOW_TEAM_SIM_PORT, BLUE_TEAM_SIM_PORT), + debug=False, + ): + + self.out_packet = RobotControl() + + self.net_yellow = network_manager.NetworkManager(address=(address, port[0])) + self.net_blue = network_manager.NetworkManager(address=(address, port[1])) + + self.yellow_robot_info = RobotControlResponse() + self.blue_robot_info = RobotControlResponse() + + self.debug = debug + + def send_robot_commands(self, team_is_yellow: bool) -> None: + """ + Sends the robot commands to the appropriate team (yellow or blue). + + Args: + team_is_yellow (bool): True if the team is yellow, False if the team is blue. + """ + if self.debug: + print(f"Sending Robot Commands") + + if team_is_yellow: + data = self.net_yellow.send_command(self.out_packet, is_sim=True) + if data: + self.yellow_robot_info = RobotControlResponse() + self.yellow_robot_info.ParseFromString(data) + self.out_packet.Clear() + else: + data = self.net_blue.send_command(self.out_packet, is_sim=True) + if data: + self.blue_robot_info = RobotControlResponse() + self.blue_robot_info.ParseFromString(data) + self.out_packet.Clear() + + def add_robot_command(self, command: Dict) -> None: + """ + Adds a robot command to the out_packet. + + Args: + command (dict): A dictionary containing the robot command with keys 'id', 'xvel', 'yvel', and 'wvel'. + """ + robot = self.out_packet.robot_commands.add() + robot.id = command["id"] + local_vel = robot.move_command.local_velocity + local_vel.forward = command["xvel"] + local_vel.left = command["yvel"] + local_vel.angular = command["wvel"] + # print(f"Robot {command['id']} command: ({command['xvel']:.3f}, {command['yvel']:.3f}, {command['wvel']:.3f})") + + def robot_has_ball(self, robot_id: int, team_is_yellow: bool) -> bool: + """ + Checks if the specified robot has the ball. + + Args: + robot_id (int): The ID of the robot. + team_is_yellow (bool): True if the team is yellow, False if the team is blue. + + Returns: + bool: True if the robot has the ball, False otherwise. + """ + if team_is_yellow: + response = self.yellow_robot_info + else: + response = self.blue_robot_info + + for robot_feedback in response.feedback: + if robot_feedback.HasField("dribbler_ball_contact") and robot_feedback.id == robot_id: + if robot_feedback.dribbler_ball_contact: + if self.debug: + print(f"Robot: {robot_id}: HAS the Ball") + return True + else: + return False From aa2d36ea16797022a79be1af999cf6b8f5f93453 Mon Sep 17 00:00:00 2001 From: fred-huang122 Date: Tue, 19 Nov 2024 18:43:24 +0000 Subject: [PATCH 5/8] Fully converted to a high level python file --- .../controllers/robot_startup_controller.py | 33 +++++-------------- 1 file changed, 9 insertions(+), 24 deletions(-) diff --git a/team_controller/src/controllers/robot_startup_controller.py b/team_controller/src/controllers/robot_startup_controller.py index 8adc4654..a1ea1b2d 100644 --- a/team_controller/src/controllers/robot_startup_controller.py +++ b/team_controller/src/controllers/robot_startup_controller.py @@ -1,19 +1,16 @@ import time import threading import numpy as np -from typing import Tuple, Optional, Dict, List, Union +from typing import Tuple, Optional, Dict, Union from global_utils.math_utils import rotate_vector from team_controller.src.data.vision_receiver import VisionDataReceiver +from team_controller.src.controllers.sim_robot_controller import SimRobotController from motion_planning.src.pid.pid import PID from team_controller.src.config.settings import ( PID_PARAMS, - LOCAL_HOST, - YELLOW_TEAM_SIM_PORT, - BLUE_TEAM_SIM_PORT, YELLOW_START, ) -from team_controller.src.utils import network_manager from team_controller.src.generated_code.ssl_simulation_robot_control_pb2 import ( RobotControl, @@ -21,23 +18,19 @@ # TODO: To be moved to a High-level Descision making repo - class StartUpController: def __init__( self, vision_receiver: VisionDataReceiver, - address=LOCAL_HOST, - port=(YELLOW_TEAM_SIM_PORT, BLUE_TEAM_SIM_PORT), debug=False, ): self.vision_receiver = vision_receiver - - self.net = network_manager.NetworkManager(address=(address, port[0])) + self.sim_robot_controller = SimRobotController(debug=debug) # TODO: Tune PID parameters further when going from sim to real(it works for Grsim) # potentially have a set tunig parameters for each robot - self.pid_oren = PID(0.0167, 8, -8, 5, 0.01, 0, num_robots=6) - self.pid_trans = PID(0.0167, 1.5, -1.5, 5, 0.01, 0, num_robots=6) + self.pid_oren = PID(0.0167, 8, -8, 4.5, 0, 0, num_robots=6) + self.pid_trans = PID(0.0167, 1.5, -1.5, 4.5, 0, 0, num_robots=6) self.lock = threading.Lock() @@ -58,11 +51,12 @@ def startup(self): command = self._calculate_robot_velocities( robot_id, target_coords, robots, balls, face_ball=True ) - self._add_robot_command(out_packet, command) + self.sim_robot_controller.add_robot_command(command) if self.debug: print(out_packet) - self.net.send_command(out_packet) + self.sim_robot_controller.send_robot_commands(team_is_yellow=True) + self.sim_robot_controller.robot_has_ball(robot_id=3, team_is_yellow=True) time_to_sleep = max(0, 0.0167 - (time.time() - start_time)) time.sleep(time_to_sleep) @@ -91,7 +85,7 @@ def _calculate_robot_velocities( target_coords (Tuple[float, float] | Tuple[float, float, float]): Target coordinates the robot should move towards. Can be a (x, y) or (x, y, orientation) tuple. If `face_ball` is True, the robot will face the ball instead of using the orientation value in target_coords. - robots (Dict[int, Optional[Tuple[float, float, float]]]): All the Current coordinates of the robots sepateated + robots (Dict[int, Optional[Tuple[float, float, float]]]): All the Current coordinates of the robots sepateated by thier robot_id which containts a tuple (x, y, orientation). balls (Dict[int, Tuple[float, float, float]]): All the Coordinates of the detected balls (int) , typically (x, y, z/height in 3D space). face_ball (bool, optional): If True, the robot will orient itself to face the ball's position. Defaults to False. @@ -142,12 +136,3 @@ def _calculate_robot_velocities( ) return out - - def _add_robot_command(self, out_packet, command) -> None: - robot = out_packet.robot_commands.add() - robot.id = command["id"] - local_vel = robot.move_command.local_velocity - local_vel.forward = command["xvel"] - local_vel.left = command["yvel"] - local_vel.angular = command["wvel"] - # print(f"Robot {command['id']} command: ({command['xvel']:.3f}, {command['yvel']:.3f}, {command['wvel']:.3f})") From 168f797cd61e520076eb42ad705800ef14635f95 Mon Sep 17 00:00:00 2001 From: fred-huang122 Date: Tue, 19 Nov 2024 18:44:46 +0000 Subject: [PATCH 6/8] hot fixes + able to check if a robot has the ball (equivalent to IR ball detection for sim) --- team_controller/src/controllers/sim_robot_controller.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/team_controller/src/controllers/sim_robot_controller.py b/team_controller/src/controllers/sim_robot_controller.py index 3723a420..ddbaa194 100644 --- a/team_controller/src/controllers/sim_robot_controller.py +++ b/team_controller/src/controllers/sim_robot_controller.py @@ -54,13 +54,13 @@ def send_robot_commands(self, team_is_yellow: bool) -> None: print(f"Sending Robot Commands") if team_is_yellow: - data = self.net_yellow.send_command(self.out_packet, is_sim=True) + data = self.net_yellow.send_command(self.out_packet, is_sim_robot_cmd=True) if data: self.yellow_robot_info = RobotControlResponse() self.yellow_robot_info.ParseFromString(data) self.out_packet.Clear() else: - data = self.net_blue.send_command(self.out_packet, is_sim=True) + data = self.net_blue.send_command(self.out_packet, is_sim_robot_cmd=True) if data: self.blue_robot_info = RobotControlResponse() self.blue_robot_info.ParseFromString(data) From 50fe5cbd7dad5035547605b795d1ceda9a9f0d41 Mon Sep 17 00:00:00 2001 From: Han Date: Wed, 20 Nov 2024 14:28:55 +0000 Subject: [PATCH 7/8] vision receiver updates + new functions --- entities/data/vision.py | 1 + team_controller/src/data/vision_receiver.py | 80 ++++++++++++++++++- .../src/tests/vision_receiver_test.py | 6 ++ 3 files changed, 86 insertions(+), 1 deletion(-) diff --git a/entities/data/vision.py b/entities/data/vision.py index 2332b6a1..1eaeab18 100644 --- a/entities/data/vision.py +++ b/entities/data/vision.py @@ -3,3 +3,4 @@ BallData = namedtuple("BallData", ["x", "y", "z"]) RobotData = namedtuple("RobotData", ["x", "y", "orientation"]) FrameData = namedtuple("FrameData", ["ts", "yellow_robots", "blue_robots", "ball"]) +TeamRobotCoords = namedtuple("TeamRobotCoords", ["team_color", "robots"]) diff --git a/team_controller/src/data/vision_receiver.py b/team_controller/src/data/vision_receiver.py index 4929db9d..e93bbad1 100644 --- a/team_controller/src/data/vision_receiver.py +++ b/team_controller/src/data/vision_receiver.py @@ -1,7 +1,7 @@ import threading import time from typing import List -from entities.data.vision import BallData, RobotData, FrameData +from entities.data.vision import BallData, RobotData, FrameData, TeamRobotCoords from team_controller.src.utils import network_manager from team_controller.src.config.settings import MULTICAST_GROUP, VISION_PORT, NUM_ROBOTS from team_controller.src.generated_code.ssl_vision_wrapper_pb2 import SSL_WrapperPacket @@ -174,3 +174,81 @@ def pull_game_data(self) -> None: print(f"Robots: {self.get_robots_pos(True)}\n") print(f"Ball: {self.get_ball_pos()}\n") time.sleep(0.0083) + + def get_robot_coords(self, is_yellow: bool) -> TeamRobotCoords: + """ + Retrieves the current positions of all robots on the specified team. + + Args: + is_yellow (bool): If True, retrieves data for the yellow team; otherwise, for the blue team. + + Returns: + TeamRobotCoords: A named tuple containing the team color and a list of RobotData. + """ + with self.lock: + team_color = "yellow" if is_yellow else "blue" + robots = self.robots_yellow_pos if is_yellow else self.robots_blue_pos + return TeamRobotCoords(team_color=team_color, robots=robots) + + def get_robot_by_id(self, is_yellow: bool, robot_id: int) -> RobotData: + """ + Retrieves the position data for a specific robot by ID. + + Args: + is_yellow (bool): If True, retrieves data for the yellow team; otherwise, for the blue team. + robot_id (int): The ID of the robot. + + Returns: + RobotData: The position data of the specified robot. + """ + with self.lock: + robots = self.robots_yellow_pos if is_yellow else self.robots_blue_pos + if 0 <= robot_id < len(robots) and robots[robot_id] is not None: + return robots[robot_id] + else: + return None # Or raise an exception. TODO + + def get_closest_robot_to_point(self, is_yellow: bool, x: float, y: float) -> RobotData: + """ + Finds the robot closest to a given point. + + Args: + is_yellow (bool): If True, searches within the yellow team; otherwise, within the blue team. + x (float): The x-coordinate of the point. + y (float): The y-coordinate of the point. + + Returns: + RobotData: The position data of the closest robot. + """ + with self.lock: + robots = self.robots_yellow_pos if is_yellow else self.robots_blue_pos + min_distance = float('inf') + closest_robot = None + for robot in robots: + if robot is not None: + distance = ((robot.x - x) ** 2 + (robot.y - y) ** 2) ** 0.5 + if distance < min_distance: + min_distance = distance + closest_robot = robot + # Haven't been tested TODO + return closest_robot + + def get_ball_velocity(self) -> tuple: + """ + Calculates the ball's velocity based on position changes over time. + + Returns: + tuple: The velocity components (vx, vy). + """ + # TODO Find a method to store the data and get velocity. --> self.previour_ball_pos + # with self.lock: + # if self.previous_ball_pos and self.ball_pos: + # dt = self.time_received - self.previous_time_received + # if dt > 0: + # vx = (self.ball_pos.x - self.previous_ball_pos.x) / dt + # vy = (self.ball_pos.y - self.previous_ball_pos.y) / dt + # return (vx, vy) + # return (0.0, 0.0) + pass + + diff --git a/team_controller/src/tests/vision_receiver_test.py b/team_controller/src/tests/vision_receiver_test.py index ade1678f..b053e4db 100644 --- a/team_controller/src/tests/vision_receiver_test.py +++ b/team_controller/src/tests/vision_receiver_test.py @@ -32,10 +32,16 @@ def main(): ball_pos = receiver.get_ball_pos() robots_yellow_pos = receiver.get_robots_pos(is_yellow=True) robots_blue_pos = receiver.get_robots_pos(is_yellow=False) + robot_coords = receiver.get_robot_coords(is_yellow=False) print("Updated Ball Position:", ball_pos) + print() print("Updated Yellow Robots Positions:", robots_yellow_pos) + print() print("Updated Blue Robots Positions:", robots_blue_pos) + print() + print("Update Blue Robots Coords:", robot_coords) + print() else: print("No data update received within the timeout period.") From a48cda859d2e108adaf7e8c1966f96dd3740929b Mon Sep 17 00:00:00 2001 From: Han Date: Wed, 20 Nov 2024 15:06:29 +0000 Subject: [PATCH 8/8] vision_receiver update --- team_controller/src/data/vision_receiver.py | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/team_controller/src/data/vision_receiver.py b/team_controller/src/data/vision_receiver.py index 5eaf2052..1ec4bb19 100644 --- a/team_controller/src/data/vision_receiver.py +++ b/team_controller/src/data/vision_receiver.py @@ -67,12 +67,13 @@ def __update_team_robots_pos( ) -> None: # Generic method to update robots for both teams. for robot in robots_data: - robots[robot.robot_id] = RobotData( - robot.x, - robot.y, - robot.orientation if robot.HasField("orientation") else 0, - ) - # TODO: When do we not have orientation? + if 0 <= robot.robot_id < len(robots): + robots[robot.robot_id] = RobotData( + robot.x, + robot.y, + robot.orientation if robot.HasField("orientation") else 0, + ) + # TODO: When do we not have orientation? def _print_frame_info(self, t_received: float, detection: object) -> None: t_now = time.time()