⚠️ Server Race Condition Fix

Important Fix - GO2 High Five Simulation Update (Action Required Before Submission)

---

You may have noticed your high five action works in isolation but the robot seems to fight itself - the leg moves but keeps snapping back, or the joints don't hold position.

Why this happens: The simulator's built-in controller runs at 60 times per second publishing joint positions. Your action server publishes joint commands too - and they overwrite each other. Neither wins cleanly.

The fix: Very soon, I will push two new commands to the sim (my repo fork) - freeze and unfreeze. Calling freeze pauses the built-in controller so your server has exclusive control of the joints. Unfreeze hands control back when you're done.

---

Step 1 - Pull the sim update and rebuild

Open your WSL2 terminal:

cd ~/go_sim/src/go2_ros2_sim_py
git pull

cd ~/go_sim
colcon build --symlink-install
source install/setup.bash

---

Step 2 - Add freeze/unfreeze to your _execute_callback

In go2_high_five/go2_high_five/high_five_server.py, find your _execute_callback method and wrap the joint-publishing section in a try/finally block like this:

def _execute_callback(self, goal_handle):
    self.get_logger().info('Executing high five...')
    feedback_msg = HighFive.Feedback()

    try:
        # Freeze the sim controller so we have exclusive joint control
        self._controller._call_behavior('freeze')
        time.sleep(0.05)  # let any in-flight command finish

        # --- YOUR EXISTING PHASE 1 / 2 / 3 JOINT CODE STAYS HERE ---
        # (raise leg, hold, lower - unchanged)

        result = HighFive.Result()
        result.success = True
        result.message = 'High five complete!'
        goal_handle.succeed()
        return result

    finally:
        # Always unfreeze - even if something goes wrong or goal is cancelled
        self._controller._call_behavior('unfreeze')

Make sure you import time

---

Step 3 - Rebuild your package

cd ~/go_sim
colcon build --packages-select go2_high_five --symlink-install
source install/setup.bash

Your existing joint commands and phases don't change - you're just wrapping them so the sim controller steps aside while you run.

If you had a spam loop (publishing the same joint command many times per frame to "win" the fight), you can remove it - a single publish per frame is enough once frozen.

Because of this issue everyone is granted an an extension for submission until 22/06/2026.

Please submit a short video with your code to blackboard as this will speed up marking if I have not see your high five in class.

---