Merge pull request #133 from GIRA/relocation-bug

Workaround for the relocation bug
robocup-junior · Apr 30, 2022 · 0eec6ed · 0eec6ed
2 parents 2278b6d + 8153a04
commit 0eec6ed
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 0 deletions.
diff --git a/controllers/rcj_soccer_referee_supervisor/referee/referee.py b/controllers/rcj_soccer_referee_supervisor/referee/referee.py
@@ -377,6 +377,8 @@ def kickoff(self, team: Optional[str] = None):
         )
 
     def tick(self) -> bool:
+        self.sv.check_reset_physics_counters()
+
         # On the very first tick, note that the match has started
         if self.time == self.match_time:
             self.eventer.event(

diff --git a/controllers/rcj_soccer_referee_supervisor/referee/supervisor.py b/controllers/rcj_soccer_referee_supervisor/referee/supervisor.py
@@ -30,6 +30,7 @@ def __init__(self):
         self.robot_rotation_fields = {}
         self.robot_translation = {}
         self.robot_rotation = {}
+        self.robot_reset_physics = {}
         for robot in ROBOT_NAMES:
             robot_node = self.getFromDef(robot)
             self.robot_nodes[robot] = robot_node
@@ -42,6 +43,17 @@ def __init__(self):
             self.robot_rotation_fields[robot] = field
             self.robot_rotation[robot] = field.getSFRotation()
 
+            self.robot_reset_physics[robot] = 0
+
+    def check_reset_physics_counters(self):
+        # HACK(Richo): Workaround for the following issue
+        # https://github.com/RoboCupJuniorTC/rcj-soccer-sim/issues/130
+        for robot in ROBOT_NAMES:
+            reset_physics_counter = self.robot_reset_physics[robot]
+            if reset_physics_counter > 0:
+                self.robot_nodes[robot].resetPhysics()
+                self.robot_reset_physics[robot] = reset_physics_counter - 1
+
     def update_positions(self):
         """Update the positions of robots and the ball"""
         self.ball_translation = self.ball_translation_field.getSFVec3f()
@@ -83,6 +95,7 @@ def set_robot_position(self, robot_name: str, position: List[float]):
         """
         tr_field = self.robot_translation_fields[robot_name]
         tr_field.setSFVec3f(position)
+        self.robot_reset_physics[robot_name] = 1
         self.robot_nodes[robot_name].resetPhysics()
         self.robot_translation[robot_name] = position