Reformatted code with black

ff_sim/ff_sim/controller_metrics.py

@@ -90,7 +90,7 @@ def process_new_binary_thrust_cmd(self, msg: ThrusterCommand) -> None:
         self.prev_thruster_sum = np.sum(thrusters)
 
         self.steps += 1
-        if not self.rolled_up: # Ensure valid duty cycles at the beginning
+        if not self.rolled_up:  # Ensure valid duty cycles at the beginning
             self.time_hist.append(dt)
             for i in range(8):
                 self.thrust_hist[i].append(thrusters[i])
@@ -99,7 +99,7 @@ def process_new_binary_thrust_cmd(self, msg: ThrusterCommand) -> None:
                 )
             if self.steps >= self.duty_cycle_window:
                 self.rolled_up = True
-        else: # Once queue is filled up, can just treat this as a list that's constantly being updawted
+        else:  # Once queue is filled up, can just treat this as a list that's constantly being updawted
             self.time_hist.pop(0)
             self.time_hist.append(dt)
             for i in range(8):

ff_sim/ff_sim/simulator_node.py

@@ -403,11 +403,11 @@ def compute_dynamics_dt(self, x_k, u_k, dt, discretization="Euler"):
             print("[FreeFlyerSimulator::compute_dynamics_dt]: Unknown Discretization Scheme.")
 
         # Wrap theta to [-pi, pi]
-        
+
         # x_next[2] = ((x_next[2] % (2 * np.pi)) + 2 * np.pi) % (2 * np.pi)
         # if x_next[2] > np.pi:
         #     x_next[2] -= 2 * np.pi
-        x_next[2] = (x_next[2] + np.pi) % (2*np.pi) - np.pi
+        x_next[2] = (x_next[2] + np.pi) % (2 * np.pi) - np.pi
 
         return x_next
 

testing_files/injectGoalPose.py

@@ -32,8 +32,14 @@
 import math
 import sys
 
-goal_positions = [[0.5,0.5,-math.pi/2], [0.5,0.5,math.pi/2], [1.0,1.0,math.pi/2], [2.0,1.5,math.pi/2]]
-goal_velos = [[0.0,0.0,0.0], [0.0,0.0,0.0], [0.0,0.0,0.0], [0.0,0.0,0.0]]
+goal_positions = [
+    [0.5, 0.5, -math.pi / 2],
+    [0.5, 0.5, math.pi / 2],
+    [1.0, 1.0, math.pi / 2],
+    [2.0, 1.5, math.pi / 2],
+]
+goal_velos = [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]
+
 
 class GoalPublisherNode(Node):
     def __init__(self, index):
@@ -54,10 +60,11 @@ def publish_goal(self, pos, velo):
         goal_pose.state.pose.theta = pos[2]
         goal_pose.state.twist.vx = velo[0]
         goal_pose.state.twist.vy = velo[1]
-        goal_pose.state.twist.wz = velo[2]           
+        goal_pose.state.twist.wz = velo[2]
 
         self.pub_goal.publish(goal_pose)
 
+
 def main():
     index = int(sys.argv[1])
     rclpy.init()
@@ -66,6 +73,6 @@ def main():
     rclpy.spin(publisher)
     rclpy.shutdown()
 
+
 if __name__ == "__main__":
     main()
-