Fixes from 14 Jan Real Testing

[energy-in-joles]

• Safely stop robots when crashing
• Better manage real robot controller packet indexing
• Adjust PID according to real testing for rotation
• lower threshold for camera check

[Copilot]

Pull request overview

This PR contains fixes from real robot testing performed on January 14th. The changes address packet protocol issues, control parameters, and testing configurations.

Changes:

• Fixed robot command packet structure to include start/end bytes per robot command instead of wrapping all commands
• Adjusted PID gains and robot parameters based on real-world performance
• Modified ball detection threshold and disabled startup formation behavior for testing

Reviewed changes

Copilot reviewed 9 out of 9 changed files in this pull request and generated 10 comments.

Show a summary per file

File	Description
utama_core/team_controller/src/controllers/real/real_robot_controller.py	Fixed packet structure with per-robot framing bytes and corrected index calculations; changed debug format from binary to hex
utama_core/strategy/examples/startup_strategy.py	Disabled startup formation step (commented out behavior tree assembly)
utama_core/strategy/common/abstract_strategy.py	Changed default robot command to disable dribbler
utama_core/run/strategy_runner.py	Added debug print for ball position
utama_core/run/refiners/position.py	Lowered ball confidence threshold from 0.1 to 0.001; removed empty lines
utama_core/motion_planning/src/pid/configs.py	Increased orientation PID kp gain from 0.5 to 1.4
utama_core/config/robot_params.py	Doubled max angular velocity from 2 to 4 rad/s
utama_core/config/formations.py	Changed starting positions for first two robots
main.py	Switched to real robot mode with 1 robot using PID control; added unused import

[energy-in-joles]

@fred-huang122 do you want to lower the camera ball confidence threshold or set to 0? We currently are failing the camera_ball_confidence test. Let me know

[energy-in-joles]

Currently if we somehow send a robot more than one command that frame, it will take the latest.

[fred-huang122]

@fred-huang122 do you want to lower the camera ball confidence threshold or set to 0? We currently are failing the camera_ball_confidence test. Let me know

@energy-in-joles, maybe it's best to remove the confidence threshold, as the most confident ball is already taken from the list of balls obtained

[Copilot]

Pull request overview

Copilot reviewed 7 out of 7 changed files in this pull request and generated 2 comments.

Comments suppressed due to low confidence (2)

utama_core/team_controller/src/controllers/real/real_robot_controller.py:63

• The variable _n_robots_unassigned is being assigned here but this variable was removed from the __init__ method (line 39 in the old code was deleted). This line should also be removed since the new implementation uses _assigned_mapping instead. This will cause an AttributeError at runtime.

        self._n_robots_unassigned = self._n_friendly

utama_core/team_controller/src/controllers/real/real_robot_controller.py:62

• The _assigned_mapping dictionary is not being reset after sending commands. It should be cleared here (e.g., self._assigned_mapping = {}) to prevent stale mappings from affecting the next command cycle.

        self._out_packet = self._empty_command()  # flush the out_packet

[Copilot]

Copilot reviewed 7 out of 7 changed files in this pull request and generated 3 comments.

[Copilot]

Pull request overview

Copilot reviewed 7 out of 7 changed files in this pull request and generated 2 comments.

[Copilot]

Pull request overview

Copilot reviewed 8 out of 8 changed files in this pull request and generated 3 comments.

[Copilot]

Pull request overview

Copilot reviewed 8 out of 8 changed files in this pull request and generated 4 comments.

Comments suppressed due to low confidence (4)

utama_core/run/refiners/position.py:51

• This comment appears to contain commented-out code.

        # for robot in combined_vision_data.yellow_robots:
        #         if robot.id == 0:
        #             print(f"robot orientation: {robot.orientation}")

utama_core/team_controller/src/controllers/real/real_robot_controller.py:110

• This comment appears to contain commented-out code.

    # def _populate_robots_info(self, data_in: bytes) -> None:
    #     """
    #     Populates the robots_info list with the data received from the robots.
    #     """
    #     for i in range(self._n_friendly):
    #         has_ball = False
    #         if data_in[0] & 0b10000000:
    #             has_ball = True
    #         info = RobotInfo(has_ball=has_ball)
    #         self._robots_info[i] = info

utama_core/strategy/common/abstract_strategy.py:13

• Import of 'Field' is not used.

from utama_core.entities.game.field import Field, FieldBounds

utama_core/strategy/common/abstract_strategy.py:64

• This call to AbstractStrategy.create_behaviour_tree in an initialization method is overridden by GoToBallExampleStrategy.create_behaviour_tree.
  This call to AbstractStrategy.create_behaviour_tree in an initialization method is overridden by MultiRobotNavigationStrategy.create_behaviour_tree.
  This call to AbstractStrategy.create_behaviour_tree in an initialization method is overridden by RobotPlacementStrategy.create_behaviour_tree.
  This call to AbstractStrategy.create_behaviour_tree in an initialization method is overridden by OscillatingObstacleStrategy.create_behaviour_tree.
  This call to AbstractStrategy.create_behaviour_tree in an initialization method is overridden by RandomMovementStrategy.create_behaviour_tree.
  This call to AbstractStrategy.create_behaviour_tree in an initialization method is overridden by SimpleNavigationStrategy.create_behaviour_tree.

        self.behaviour_tree = py_trees.trees.BehaviourTree(self.create_behaviour_tree())

[Copilot]

The test expects 2 balls but the actual behavior will produce 3 balls. Ball at (0, 0) from camera 0 and ball at (0.02, 0.02) from camera 1 are within the merge threshold of 0.05 (distance = 0.04), so they should merge into 1 ball. Ball at (5, 5) from camera 0 is far away and will remain separate (1 ball). This gives 2 total balls, not 3. However, the assertion claims 2, which would be correct if the merge happens. But with three input balls total (two from camera 0 and one from camera 1), if the close ones merge, we get 2 balls. The test assertion is correct. However, examining more carefully: balls from camera 0 are (0,0) and (5,5), ball from camera 1 is (0.02, 0.02). The (0,0) and (0.02, 0.02) should merge (distance 0.04 < 0.05), leaving us with 2 balls total: the merged ball and (5,5). The assertion of 2 is correct.

[Copilot]

The methods ball_merge_predicate and ball_merge should be decorated with @staticmethod since they don't use self and are called using the class name (e.g., CameraCombiner.ball_merge_predicate). This follows Python conventions and improves code clarity.

[Copilot]

Normal methods should have 'self', rather than 'b1', as their first parameter.

[Copilot]

Normal methods should have 'self', rather than 'b1', as their first parameter.