通过imu传感器4元数的地平线模拟

3轴模拟转动

之前算法节点订阅过四元数，直接用之前的节点把4元数给rviz2，进行世界坐标和imu坐标的转换， 但是rviz2好像有局限，显示只能到30帧

校准

    def _handle_calibrate(self, request, response):
        with self._lock:
            if self._latest_q is None:
                response.success = False
                response.message = 'No IMU data yet'
                return response
            # 记录当前四元数并计算逆用于后续补偿
            self._calib_q = tuple(self._latest_q)
            self._calib_q_inv = self._quat_inverse(self._calib_q)

    def _quat_inverse(self, q):
        x, y, z, w = q
        norm_sq = x*x + y*y + z*z + w*w
        if norm_sq == 0:
            return (0.0, 0.0, 0.0, 1.0)
        return (-x / norm_sq, -y / norm_sq, -z / norm_sq, w / norm_sq)

手动记录此时的4元数数据，并把之后的4元数与之计算即可实现校准

ros2 service call /exo/imu/calibrate std_srvs/srv/Trigger "{}"

地平线模拟

拿过来画好的地hud代码，我订阅校准后的4元数，即可进行模拟 但是模拟的帧率也到不了120，查看数据频率没有问题

---

本文章使用limfx的vscode插件快速发布