New metrics node is working!

StanfordASL · Feb 16, 2024 · 92cbb27 · 92cbb27
1 parent 1bd3a8f
commit 92cbb27
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 26 deletions.
diff --git a/ff_sim/ff_sim/controller_metrics.py b/ff_sim/ff_sim/controller_metrics.py
@@ -53,7 +53,7 @@ class ControllerMetricsPublisher(Node):
 
     def __init__(self):
         super().__init__("ff_ctrl_metrics")
-        self.curr_time = self.get_clock().now().to_msg().sec
+        self.curr_time = self.get_clock().now().to_msg()
         self.steps = 0
         self.running_total_gas = 0
         self.prev_thruster_sum = 0
@@ -79,8 +79,10 @@ def process_new_wheel_cmd(self, msg: WheelVelCommand) -> None:
 
     def process_new_binary_thrust_cmd(self, msg: ThrusterCommand) -> None:
         """ Process binary thrusters """
-        now = self.get_clock().now().to_msg().sec
-        dt = now - self.curr_time
+        now = self.get_clock().now().to_msg()
+        dtsec = now.sec - self.curr_time.sec
+        dtnsec = now.nanosec - self.curr_time.nanosec
+        dt = dtsec + dtnsec/1e9
 
         thrusters = np.array(msg.switches, dtype=float)
         self.running_total_gas += self.prev_thruster_sum * dt
@@ -90,7 +92,7 @@ def process_new_binary_thrust_cmd(self, msg: ThrusterCommand) -> None:
         if not self.rolled_up:
             self.time_hist.append(dt)
             for i in range(8):
-                self.thrust_hist[i].append(self.thrusters[i])
+                self.thrust_hist[i].append(thrusters[i])
                 self.thrust_duty_cycles[i] = np.dot(self.thrust_hist[i],self.time_hist) / np.sum(self.time_hist)
             if self.steps >= self.duty_cycle_window:
                 self.rolled_up = True
@@ -99,7 +101,7 @@ def process_new_binary_thrust_cmd(self, msg: ThrusterCommand) -> None:
             self.time_hist.append(dt)
             for i in range(8):
                 self.thrust_hist[i].pop(0)
-                self.thrust_hist[i].append(self.thrusters[i])
+                self.thrust_hist[i].append(thrusters[i])
                 self.thrust_duty_cycles[i] = np.dot(self.thrust_hist[i],self.time_hist) / np.sum(self.time_hist)
 
         metrics = ControllerMetrics()

diff --git a/ff_sim/ff_sim/simulator_node.py b/ff_sim/ff_sim/simulator_node.py
@@ -269,29 +269,9 @@ def sim_loop(self) -> None:
         mocap.pose.orientation.y = 0.0
         mocap.pose.orientation.z = np.sin(theta / 2)
 
-        # Metrics
-        # self.running_total_gas += np.sum(self.thrusters) * self.SIM_DT
-        # self.steps += 1
-        # metrics = ControllerMetrics()
-        # metrics.header.stamp = now
-        # metrics.total_gas_time = self.running_total_gas
-        # if not self.rolled_up:
-        #     for i in range(8):
-        #         self.thrust_hist[i].append(self.thrusters[i])
-        #         self.thrust_duty_cycles[i] = np.sum(self.thrust_hist[i]) / len(self.thrust_hist[i])
-        #     # if (now.sec > self.start_time.sec and now.nanosec > self.start_time.nanosec):
-        #     if self.steps >= self.duty_cycle_window:
-        #         self.rolled_up = True
-        # else:
-        #     for i in range(8):
-        #         self.thrust_duty_cycles[i] -= self.thrust_hist[i].pop(0) / self.duty_cycle_window
-        #         self.thrust_duty_cycles[i] += self.thrusters[i] / self.duty_cycle_window
-        #         self.thrust_hist[i].append(self.thrusters[i])
-        # metrics.running_duty_cycles = self.thrust_duty_cycles
-        # # Publish
+        # Publish
         self.pub_state.publish(state)
         self.pub_mocap.publish(mocap)
-        # self.pub_controller_metrics.publish(metrics)
 
     def update_wheel_cmd_vel_cb(self, msg: WheelVelCommand) -> None:
         self.wheel_vel_cmd = msg.velocity