此更改更改了imu数据处理方式，使得静止状态yaw不会发生偏移，后续应优化陀螺仪数据处理：低通滤波会导致相位延迟、滑动窗口会导致数据有微量误差。
无法避免运动过程中的yaw偏移。 后续应在运动时也加上零漂偏移
This commit is contained in:
298
scripts/base_feedback_monitor.py
Normal file
298
scripts/base_feedback_monitor.py
Normal file
@@ -0,0 +1,298 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import csv
|
||||
import math
|
||||
import sys
|
||||
from datetime import datetime
|
||||
from pathlib import Path
|
||||
from typing import Optional
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import QoSProfile
|
||||
|
||||
from nav_msgs.msg import Odometry
|
||||
from origincar_msg.msg import Data
|
||||
from sensor_msgs.msg import Imu
|
||||
from std_msgs.msg import Float32
|
||||
|
||||
|
||||
def quaternion_to_yaw(x: float, y: float, z: float, w: float) -> float:
|
||||
siny_cosp = 2.0 * (w * z + x * y)
|
||||
cosy_cosp = 1.0 - 2.0 * (y * y + z * z)
|
||||
return math.atan2(siny_cosp, cosy_cosp)
|
||||
|
||||
|
||||
class BaseFeedbackMonitor(Node):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("base_feedback_monitor")
|
||||
qos = QoSProfile(depth=10)
|
||||
self.workspace_root = Path.cwd()
|
||||
self.log_dir = self.workspace_root / "feedback_logs" / datetime.now().strftime("%Y%m%d_%H%M%S")
|
||||
self.log_dir.mkdir(parents=True, exist_ok=True)
|
||||
self.csv_path = self.log_dir / "feedback.csv"
|
||||
self.csv_file = self.csv_path.open("w", newline="", encoding="utf-8")
|
||||
self.csv_writer = csv.writer(self.csv_file)
|
||||
self.csv_writer.writerow([
|
||||
"wall_time",
|
||||
"elapsed_s",
|
||||
"imu_ready",
|
||||
"odom_ready",
|
||||
"voltage_ready",
|
||||
"robotpose_ready",
|
||||
"robotvel_ready",
|
||||
"gyrodebug_ready",
|
||||
"voltage_v",
|
||||
"imu_orientation_x",
|
||||
"imu_orientation_y",
|
||||
"imu_orientation_z",
|
||||
"imu_orientation_w",
|
||||
"imu_yaw_rad",
|
||||
"imu_angular_velocity_x",
|
||||
"imu_angular_velocity_y",
|
||||
"imu_angular_velocity_z",
|
||||
"imu_linear_acceleration_x",
|
||||
"imu_linear_acceleration_y",
|
||||
"imu_linear_acceleration_z",
|
||||
"odom_pose_x",
|
||||
"odom_pose_y",
|
||||
"odom_yaw_rad",
|
||||
"odom_twist_vx",
|
||||
"odom_twist_vy",
|
||||
"odom_twist_wz",
|
||||
"robotpose_x",
|
||||
"robotpose_y",
|
||||
"robotpose_z",
|
||||
"robotvel_x",
|
||||
"robotvel_y",
|
||||
"robotvel_z",
|
||||
"gyro_raw_z_rad_s",
|
||||
"gyro_bias_fit_z_rad_s",
|
||||
"gyro_corrected_z_rad_s",
|
||||
])
|
||||
|
||||
self.start_time = self.get_clock().now()
|
||||
self.imu_msg: Optional[Imu] = None
|
||||
self.odom_msg: Optional[Odometry] = None
|
||||
self.voltage_msg: Optional[Float32] = None
|
||||
self.robotpose_msg: Optional[Data] = None
|
||||
self.robotvel_msg: Optional[Data] = None
|
||||
self.gyrodebug_msg: Optional[Data] = None
|
||||
|
||||
self.create_subscription(Imu, "imu/data_raw", self.imu_callback, qos)
|
||||
self.create_subscription(Odometry, "odom", self.odom_callback, qos)
|
||||
self.create_subscription(Float32, "PowerVoltage", self.voltage_callback, qos)
|
||||
self.create_subscription(Data, "robotpose", self.robotpose_callback, qos)
|
||||
self.create_subscription(Data, "robotvel", self.robotvel_callback, qos)
|
||||
self.create_subscription(Data, "gyro_debug", self.gyrodebug_callback, qos)
|
||||
|
||||
self.create_timer(0.2, self.render)
|
||||
self.create_timer(0.2, self.log_snapshot)
|
||||
self.get_logger().info(f"Logging feedback to {self.csv_path}")
|
||||
|
||||
def imu_callback(self, msg: Imu) -> None:
|
||||
self.imu_msg = msg
|
||||
|
||||
def odom_callback(self, msg: Odometry) -> None:
|
||||
self.odom_msg = msg
|
||||
|
||||
def voltage_callback(self, msg: Float32) -> None:
|
||||
self.voltage_msg = msg
|
||||
|
||||
def robotpose_callback(self, msg: Data) -> None:
|
||||
self.robotpose_msg = msg
|
||||
|
||||
def robotvel_callback(self, msg: Data) -> None:
|
||||
self.robotvel_msg = msg
|
||||
|
||||
def gyrodebug_callback(self, msg: Data) -> None:
|
||||
self.gyrodebug_msg = msg
|
||||
|
||||
def _fmt_float(self, value: Optional[float], digits: int = 4) -> str:
|
||||
if value is None:
|
||||
return "N/A"
|
||||
return f"{value:.{digits}f}"
|
||||
|
||||
def _topic_state(self, msg: object) -> str:
|
||||
return "OK" if msg is not None else "WAIT"
|
||||
|
||||
def _get_snapshot(self) -> dict:
|
||||
imu_yaw = None
|
||||
odom_yaw = None
|
||||
|
||||
if self.imu_msg is not None:
|
||||
imu_yaw = quaternion_to_yaw(
|
||||
self.imu_msg.orientation.x,
|
||||
self.imu_msg.orientation.y,
|
||||
self.imu_msg.orientation.z,
|
||||
self.imu_msg.orientation.w,
|
||||
)
|
||||
|
||||
if self.odom_msg is not None:
|
||||
odom_yaw = quaternion_to_yaw(
|
||||
self.odom_msg.pose.pose.orientation.x,
|
||||
self.odom_msg.pose.pose.orientation.y,
|
||||
self.odom_msg.pose.pose.orientation.z,
|
||||
self.odom_msg.pose.pose.orientation.w,
|
||||
)
|
||||
|
||||
return {
|
||||
"wall_time": datetime.now().isoformat(timespec="milliseconds"),
|
||||
"elapsed_s": (self.get_clock().now() - self.start_time).nanoseconds / 1e9,
|
||||
"imu_ready": int(self.imu_msg is not None),
|
||||
"odom_ready": int(self.odom_msg is not None),
|
||||
"voltage_ready": int(self.voltage_msg is not None),
|
||||
"robotpose_ready": int(self.robotpose_msg is not None),
|
||||
"robotvel_ready": int(self.robotvel_msg is not None),
|
||||
"gyrodebug_ready": int(self.gyrodebug_msg is not None),
|
||||
"voltage_v": self.voltage_msg.data if self.voltage_msg is not None else None,
|
||||
"imu_orientation_x": self.imu_msg.orientation.x if self.imu_msg is not None else None,
|
||||
"imu_orientation_y": self.imu_msg.orientation.y if self.imu_msg is not None else None,
|
||||
"imu_orientation_z": self.imu_msg.orientation.z if self.imu_msg is not None else None,
|
||||
"imu_orientation_w": self.imu_msg.orientation.w if self.imu_msg is not None else None,
|
||||
"imu_yaw_rad": imu_yaw,
|
||||
"imu_angular_velocity_x": self.imu_msg.angular_velocity.x if self.imu_msg is not None else None,
|
||||
"imu_angular_velocity_y": self.imu_msg.angular_velocity.y if self.imu_msg is not None else None,
|
||||
"imu_angular_velocity_z": self.imu_msg.angular_velocity.z if self.imu_msg is not None else None,
|
||||
"imu_linear_acceleration_x": self.imu_msg.linear_acceleration.x if self.imu_msg is not None else None,
|
||||
"imu_linear_acceleration_y": self.imu_msg.linear_acceleration.y if self.imu_msg is not None else None,
|
||||
"imu_linear_acceleration_z": self.imu_msg.linear_acceleration.z if self.imu_msg is not None else None,
|
||||
"odom_pose_x": self.odom_msg.pose.pose.position.x if self.odom_msg is not None else None,
|
||||
"odom_pose_y": self.odom_msg.pose.pose.position.y if self.odom_msg is not None else None,
|
||||
"odom_yaw_rad": odom_yaw,
|
||||
"odom_twist_vx": self.odom_msg.twist.twist.linear.x if self.odom_msg is not None else None,
|
||||
"odom_twist_vy": self.odom_msg.twist.twist.linear.y if self.odom_msg is not None else None,
|
||||
"odom_twist_wz": self.odom_msg.twist.twist.angular.z if self.odom_msg is not None else None,
|
||||
"robotpose_x": self.robotpose_msg.x if self.robotpose_msg is not None else None,
|
||||
"robotpose_y": self.robotpose_msg.y if self.robotpose_msg is not None else None,
|
||||
"robotpose_z": self.robotpose_msg.z if self.robotpose_msg is not None else None,
|
||||
"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,
|
||||
}
|
||||
|
||||
def log_snapshot(self) -> None:
|
||||
snapshot = self._get_snapshot()
|
||||
self.csv_writer.writerow(snapshot.values())
|
||||
self.csv_file.flush()
|
||||
|
||||
def render(self) -> None:
|
||||
snapshot = self._get_snapshot()
|
||||
sys.stdout.write("\033[2J\033[H")
|
||||
sys.stdout.write("origincar base feedback monitor\n")
|
||||
sys.stdout.write("=" * 40 + "\n")
|
||||
sys.stdout.write(
|
||||
f"imu/data_raw: {self._topic_state(self.imu_msg)} | "
|
||||
f"odom: {self._topic_state(self.odom_msg)} | "
|
||||
f"PowerVoltage: {self._topic_state(self.voltage_msg)} | "
|
||||
f"robotpose: {self._topic_state(self.robotpose_msg)} | "
|
||||
f"robotvel: {self._topic_state(self.robotvel_msg)} | "
|
||||
f"gyro_debug: {self._topic_state(self.gyrodebug_msg)}\n\n"
|
||||
)
|
||||
sys.stdout.write(f"log file: {self.csv_path}\n\n")
|
||||
|
||||
if self.voltage_msg is not None:
|
||||
sys.stdout.write(f"voltage: {self._fmt_float(self.voltage_msg.data, 3)} V\n")
|
||||
else:
|
||||
sys.stdout.write("voltage: N/A\n")
|
||||
|
||||
if self.imu_msg is not None:
|
||||
imu = self.imu_msg
|
||||
sys.stdout.write(
|
||||
"imu orientation: "
|
||||
f"x={self._fmt_float(imu.orientation.x)} "
|
||||
f"y={self._fmt_float(imu.orientation.y)} "
|
||||
f"z={self._fmt_float(imu.orientation.z)} "
|
||||
f"w={self._fmt_float(imu.orientation.w)} "
|
||||
f"yaw={self._fmt_float(snapshot['imu_yaw_rad'])} rad\n"
|
||||
)
|
||||
sys.stdout.write(
|
||||
"imu angular vel: "
|
||||
f"x={self._fmt_float(imu.angular_velocity.x)} "
|
||||
f"y={self._fmt_float(imu.angular_velocity.y)} "
|
||||
f"z={self._fmt_float(imu.angular_velocity.z)} rad/s\n"
|
||||
)
|
||||
sys.stdout.write(
|
||||
"imu linear acc: "
|
||||
f"x={self._fmt_float(imu.linear_acceleration.x)} "
|
||||
f"y={self._fmt_float(imu.linear_acceleration.y)} "
|
||||
f"z={self._fmt_float(imu.linear_acceleration.z)} m/s^2\n"
|
||||
)
|
||||
else:
|
||||
sys.stdout.write("imu orientation: N/A\n")
|
||||
sys.stdout.write("imu angular vel: N/A\n")
|
||||
sys.stdout.write("imu linear acc: N/A\n")
|
||||
|
||||
if self.odom_msg is not None:
|
||||
odom = self.odom_msg
|
||||
sys.stdout.write(
|
||||
"odom pose: "
|
||||
f"x={self._fmt_float(odom.pose.pose.position.x)} "
|
||||
f"y={self._fmt_float(odom.pose.pose.position.y)} "
|
||||
f"yaw={self._fmt_float(snapshot['odom_yaw_rad'])}\n"
|
||||
)
|
||||
sys.stdout.write(
|
||||
"odom twist: "
|
||||
f"vx={self._fmt_float(odom.twist.twist.linear.x)} "
|
||||
f"vy={self._fmt_float(odom.twist.twist.linear.y)} "
|
||||
f"wz={self._fmt_float(odom.twist.twist.angular.z)}\n"
|
||||
)
|
||||
else:
|
||||
sys.stdout.write("odom pose: N/A\n")
|
||||
sys.stdout.write("odom twist: N/A\n")
|
||||
|
||||
if self.robotpose_msg is not None:
|
||||
pose = self.robotpose_msg
|
||||
sys.stdout.write(
|
||||
"robotpose: "
|
||||
f"x={self._fmt_float(pose.x)} "
|
||||
f"y={self._fmt_float(pose.y)} "
|
||||
f"z={self._fmt_float(pose.z)}\n"
|
||||
)
|
||||
else:
|
||||
sys.stdout.write("robotpose: N/A\n")
|
||||
|
||||
if self.robotvel_msg is not None:
|
||||
vel = self.robotvel_msg
|
||||
sys.stdout.write(
|
||||
"robotvel: "
|
||||
f"x={self._fmt_float(vel.x)} "
|
||||
f"y={self._fmt_float(vel.y)} "
|
||||
f"z={self._fmt_float(vel.z)}\n"
|
||||
)
|
||||
else:
|
||||
sys.stdout.write("robotvel: N/A\n")
|
||||
|
||||
if self.gyrodebug_msg is not None:
|
||||
gyro = self.gyrodebug_msg
|
||||
sys.stdout.write(
|
||||
"gyro debug: "
|
||||
f"raw_z={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"
|
||||
)
|
||||
else:
|
||||
sys.stdout.write("gyro debug: N/A\n")
|
||||
|
||||
sys.stdout.write("\nCtrl+C to exit\n")
|
||||
sys.stdout.flush()
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = BaseFeedbackMonitor()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.csv_file.close()
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Reference in New Issue
Block a user