[DRAFT] Goalie utility function

launch/vision_filter.launch.py

@@ -10,10 +10,11 @@ def generate_launch_description():
     config = os.path.join(get_package_share_directory('rj_robocup'), 'config',
                           'sim.yaml')
     return LaunchDescription([
-        Node(package="rj_robocup",
-             executable="rj_vision_filter",
-             output="screen",
-             parameters=[config],
-             on_exit=Shutdown(),
+        Node(
+            package="rj_robocup",
+            executable="rj_vision_filter",
+            output="screen",
+            parameters=[config],
+            on_exit=Shutdown(),
         )
     ])

launch/vision_receiver.launch.py

@@ -8,17 +8,19 @@
 
 def generate_launch_description():
     sim_vision_port = 10020
-    config = os.path.join(get_package_share_directory('rj_robocup'), 'config', 'sim.yaml')
+    config = os.path.join(get_package_share_directory('rj_robocup'), 'config',
+                          'sim.yaml')
     shared_vision_port_single_primary = 10002
 
     hz = 120.0
     port = sim_vision_port
 
     return LaunchDescription([
-        Node(package="rj_robocup",
-             executable="vision_receiver",
-             output="screen",
-             parameters=[config],
-             on_exit=Shutdown(),
+        Node(
+            package="rj_robocup",
+            executable="vision_receiver",
+            output="screen",
+            parameters=[config],
+            on_exit=Shutdown(),
         )
     ])

rj_common/include/rj_common/transforms.hpp

@@ -13,6 +13,7 @@ namespace rj_common {
  * of the field) by this matrix yields a point in team coordinates (origin at our goal).
  * @param defend_plus_x Whether our (defended) goal has positive x-coordinate in the vision frame
  */
-[[nodiscard]] rj_geometry::TransformMatrix world_to_team(const FieldDimensions& dimensions, bool defend_plus_x);
+[[nodiscard]] rj_geometry::TransformMatrix world_to_team(const FieldDimensions& dimensions,
+                                                         bool defend_plus_x);
 
 }  // namespace rj_common

rj_gameplay/rj_gameplay/MAX_KICK_SPEED.py

@@ -1,2 +1,2 @@
 # file solely to limit the number of "kick too fast" penalties we get in a 10-sec change
-MAX_KICK_SPEED = 5.0 # m/s
+MAX_KICK_SPEED = 5.0  # m/s

rj_gameplay/rj_gameplay/action/capture.py

@@ -19,13 +19,14 @@ def __init__(self, robot_id: int = None):
 
     def tick(self, intent) -> None:
         collect_command = CollectMotionCommand()
-        intent.motion_command.collect_command = [collect_command] 
+        intent.motion_command.collect_command = [collect_command]
         intent.dribbler_speed = 1.0
         intent.is_active = True
         return intent
 
     def is_done(self, world_state) -> bool:
-        if self.robot_id is not None and world_state.our_robots[self.robot_id].has_ball_sense:
+        if self.robot_id is not None and world_state.our_robots[
+                self.robot_id].has_ball_sense:
             self.ticks_done += 1
         else:
             self.ticks_done -= 5

rj_gameplay/rj_gameplay/action/kick.py

@@ -14,6 +14,7 @@
 KICK_BALL_SPEED_THRESHOLD = 0.9
 from rj_gameplay.MAX_KICK_SPEED import *
 
+
 class IKick(action.IAction, ABC):
     def done(self) -> bool:
         pass

rj_gameplay/rj_gameplay/action/line_kick.py

@@ -14,12 +14,17 @@
 
 from rj_gameplay.MAX_KICK_SPEED import *
 
+
 class LineKickAction(action.IFiniteAction):
     """
     Activates kicker. Intended to be used after an aim/drive action in a skill.
     """
-
-    def __init__(self, robot_id: int, target: np.ndarray, priority: int = 0, chip: bool = False, kick_speed: float = 6.0) -> None:
+    def __init__(self,
+                 robot_id: int,
+                 target: np.ndarray,
+                 priority: int = 0,
+                 chip: bool = False,
+                 kick_speed: float = 6.0) -> None:
         self.robot_id = robot_id
         self.priority = priority
 

rj_gameplay/rj_gameplay/action/move.py

@@ -20,7 +20,6 @@ class Move(action.IFiniteAction):
     """
     Move Action
     """
-
     def __init__(self,
                  robot_id: int,
                  target_point: np.ndarray,
@@ -42,23 +41,29 @@ def is_done(self, world_state: rc.WorldState) -> bool:
         threshold = 0.3
         if self.robot_id is None or world_state is None:
             return False
-        elif (math.sqrt((world_state.our_robots[self.robot_id].pose[0] - self.target_point[0]) ** 2 + (
-                world_state.our_robots[self.robot_id].pose[1] - self.target_point[1]) ** 2) < threshold):
+        elif (math.sqrt((world_state.our_robots[self.robot_id].pose[0] -
+                         self.target_point[0])**2 +
+                        (world_state.our_robots[self.robot_id].pose[1] -
+                         self.target_point[1])**2) < threshold):
             return True
         else:
             return False
 
     def tick(self, intent: msg.RobotIntent) -> msg.RobotIntent:
         path_command = PathTargetMotionCommand()
-        path_command.target.position = Point(x=self.target_point[0], y=self.target_point[1])
-        path_command.target.velocity = Point(x=self.target_vel[0], y=self.target_vel[1])
+        path_command.target.position = Point(x=self.target_point[0],
+                                             y=self.target_point[1])
+        path_command.target.velocity = Point(x=self.target_vel[0],
+                                             y=self.target_vel[1])
         path_command.ignore_ball = self.ignore_ball
 
         if (self.face_angle is not None):
             path_command.override_angle = [self.face_angle]
 
         if (self.face_point is not None):
-            path_command.override_face_point = [Point(x=self.face_point[0], y=self.face_point[1])]
+            path_command.override_face_point = [
+                Point(x=self.face_point[0], y=self.face_point[1])
+            ]
 
         intent.motion_command.path_target_command = [path_command]
         intent.is_active = True

rj_gameplay/rj_gameplay/action/pivot.py

@@ -12,8 +12,13 @@
 
 class Pivot(action.IFiniteAction):
 
-    def __init__(self, robot_id: int, pivot_point: np.ndarray, target_point: np.ndarray, dribble_speed: float,
-                 threshold: float = 0.02, priority: int = 1):
+    def __init__(self,
+                 robot_id: int,
+                 pivot_point: np.ndarray,
+                 target_point: np.ndarray,
+                 dribble_speed: float,
+                 threshold: float = 0.02,
+                 priority: int = 1):
         self.robot_id = robot_id
         self.pivot_point = pivot_point
         self.target_point = target_point
@@ -35,12 +40,13 @@ def is_done(self, world_state: rc.WorldState) -> bool:
         # TODO: Change this when we get action state feedback
         angle_threshold = self.threshold
         # TODO: Make these local params
-        stopped_threshold = 5 * self.threshold # We don't _really_ care about this when we're kicking, if not for latency
+        stopped_threshold = 5 * self.threshold  # We don't _really_ care about this when we're kicking, if not for latency
         if self.robot_id is None:
             return False
         robot = world_state.our_robots[self.robot_id]
         robot_pos_to_target = self.target_point - robot.pose[0:2]
-        robot_to_target_unit = robot_pos_to_target / np.linalg.norm(robot_pos_to_target)
+        robot_to_target_unit = robot_pos_to_target / np.linalg.norm(
+            robot_pos_to_target)
         heading_vect = np.array([np.cos(robot.pose[2]), np.sin(robot.pose[2])])
         dot_product = np.dot(heading_vect, robot_to_target_unit)
         angle = np.arccos(dot_product)