zbw/yiliao2026
1
0
Fork 1
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

零漂数据采集拟合

Browse Source
This commit is contained in:
Orange
2000-01-01 08:38:51 +08:00
parent b14befd1cf
commit 2f51de27e5
6 changed files with 1430 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
scripts/base_feedback_monitor.py
View File

@@ -66,9 +66,9 @@ class BaseFeedbackMonitor(Node):
"robotvel_x",
"robotvel_y",
"robotvel_z",
"gyro_raw_z_rad_s",
"gyro_bias_fit_z_rad_s",
"gyro_corrected_z_rad_s",
"gyro_z_filtered_pre_bias",
"gyro_z_bias_model",
"gyro_z_final_for_yaw",
])
self.start_time = self.get_clock().now()
@@ -169,9 +169,9 @@ class BaseFeedbackMonitor(Node):
"robotvel_x": self.robotvel_msg.x if self.robotvel_msg is not None else None,
"robotvel_y": self.robotvel_msg.y if self.robotvel_msg is not None else None,
"robotvel_z": self.robotvel_msg.z if self.robotvel_msg is not None else None,
"gyro_raw_z_rad_s": self.gyrodebug_msg.x if self.gyrodebug_msg is not None else None,
"gyro_bias_fit_z_rad_s": self.gyrodebug_msg.y if self.gyrodebug_msg is not None else None,
"gyro_corrected_z_rad_s": self.gyrodebug_msg.z if self.gyrodebug_msg is not None else None,
"gyro_z_filtered_pre_bias": self.gyrodebug_msg.x if self.gyrodebug_msg is not None else None,
"gyro_z_bias_model": self.gyrodebug_msg.y if self.gyrodebug_msg is not None else None,
"gyro_z_final_for_yaw": self.gyrodebug_msg.z if self.gyrodebug_msg is not None else None,
}
def log_snapshot(self) -> None:
@@ -270,9 +270,9 @@ class BaseFeedbackMonitor(Node):
gyro = self.gyrodebug_msg
sys.stdout.write(
"gyro debug: "
f"raw_z={self._fmt_float(gyro.x, 6)} "
f"pre_bias={self._fmt_float(gyro.x, 6)} "
f"bias_z={self._fmt_float(gyro.y, 6)} "
f"corr_z={self._fmt_float(gyro.z, 6)} rad/s\n"
f"final_z={self._fmt_float(gyro.z, 6)} rad/s\n"
)
else:
sys.stdout.write("gyro debug: N/A\n")