Repository: robojackets/robocup-software
Issue: During theirRestart, marking is not assigning bots correctly to mark enemies.

Seems to work correctly for 1 bot however the other two bots running marking are inactive.

Changed files:
soccer/gameplay/skills/mark.py, soccer/gameplay/plays/restarts/their_restart.py

diff --git a/soccer/gameplay/plays/restarts/their_restart.py b/soccer/gameplay/plays/restarts/their_restart.py
index 88bdbca106..eb935175c5 100644
--- a/soccer/gameplay/plays/restarts/their_restart.py
+++ b/soccer/gameplay/plays/restarts/their_restart.py
@@ -46,41 +46,16 @@ class TheirRestart(standard_play.StandardPlay):
         their_kicker = min(main.their_robots(),
                            key=lambda opp: opp.pos.dist_to(ball_pos))
 
-        # we build an array of (OpponentRobot, float distToClosestOfOurBots) tuples
-        # we'll use these in a sec to assign our marking robots
-        open_opps_and_dists = []
-        for opp in main.their_robots():
-            # ignore their kicker
-            if opp == their_kicker: continue
-
-            ball_dist = opp.pos.dist_to(ball_pos)
-
-            # see if the opponent is close enough to the ball for us to care
-            # if it is, we record the closest distance from one of our robots to it
-            if ball_dist < 3.0:
-                # which of our robots is closest to this opponent
-                closest_self_dist = min([bot.pos.dist_to(opp.pos)
-                                         for bot in main.our_robots()])
-                open_opps_and_dists.append((opp, closest_self_dist))
+        # we build an array of OpponentRobots sorted rudimentarily by their threat
+        # Right now, this is (inverse of) distance to the ball * 2 + y position.
+        # Needs tuning/improvement. Right now this is excessively defensive
+        sorted_opponents = sorted(
+            filter(lambda robot: robot != their_kicker, main.their_robots()),
+            key=lambda robot: robot.pos.dist_to(ball_pos) * 2 + robot.pos.y)
 
         # Decide what each marking robot should do
-        # @open_opps contains the robots we want to mark (if any)
+        # @sorted_opponents contains the robots we want to mark by priority
         # any robot that isn't assigned a mark_robot will move towards the ball
         for i, mark_i in enumerate(self.marks):
-            if mark_i.robot != None:
-                if i < len(open_opps_and_dists):
-                    # mark the opponent
-                    mark_i.mark_robot = open_opps_and_dists[i][0]
-                else:
-                    pass
-                    # NOTE: the old code ran these motion commands INSTEAD of running the mark command
-                    # we could do that, but it'd require removing the subbehavior and re-adding a move or something...
-
-                    # # move towards the ball and face it, but don't get within field center's radius
-                    # pos = mark_i.robot.pos
-                    # target = pos + (ball_pos - pos).normalized() * (ball_pos.dist_to(pos) - constants.Field.CenterRadius)
-
-                    # mark_i.robot.move(target)
-                    # mark_i.face(ball_pos)
-
-                    # tell the marking robots to avoid eachother more than normal
+            if i < len(sorted_opponents):
+                mark_i.mark_robot = sorted_opponents[i]
diff --git a/soccer/gameplay/skills/mark.py b/soccer/gameplay/skills/mark.py
index 438b3d0590..71eb738f0a 100644
--- a/soccer/gameplay/skills/mark.py
+++ b/soccer/gameplay/skills/mark.py
@@ -3,6 +3,7 @@ import behavior
 import robocup
 import main
 import constants
+import role_assignment
 
 
 class Mark(single_robot_behavior.SingleRobotBehavior):
@@ -12,6 +13,7 @@ class Mark(single_robot_behavior.SingleRobotBehavior):
         self._mark_line_thresh = 0.9
         self._mark_robot = None
         self._mark_point = None
+        self._target_point = None
 
         self.add_transition(behavior.Behavior.State.start,
                             behavior.Behavior.State.running, lambda: True,
@@ -36,11 +38,11 @@ class Mark(single_robot_behavior.SingleRobotBehavior):
         main.system_state().draw_line(ball_mark_line, (0, 0, 255), "Mark")
 
         mark_line_dist = ball_mark_line.dist_to(pos)
-        target_point = None
+        self._target_point = None
         if mark_line_dist > self.mark_line_thresh:
-            target_point = ball_mark_line.nearest_point(pos)
+            self._target_point = ball_mark_line.nearest_point(pos)
         else:
-            target_point = ball_pos + (
+            self._target_point = ball_pos + (
                 mark_pos -
                 ball_pos).normalized() * self.ratio * ball_mark_line.length()
 
@@ -49,7 +51,7 @@ class Mark(single_robot_behavior.SingleRobotBehavior):
 
         if self.mark_robot is not None:
             self.robot.approach_opponent(self.mark_robot.shell_id(), True)
-        self.robot.move_to(target_point)
+        self.robot.move_to(self._target_point)
         self.robot.face(ball_pos)
 
     @property
@@ -86,3 +88,10 @@ class Mark(single_robot_behavior.SingleRobotBehavior):
     @mark_robot.setter
     def mark_robot(self, value):
         self._mark_robot = value
+
+    # Choose a robot close to the mark point
+    def role_requirements(self):
+        req = super().role_requirements()
+        if self._target_point is not None:
+            req.destination_shape = self._target_point
+        return req