# rasp_imu_hat_6dof

这是树莓派 6DOF IMU HAT 的 ROS2 Humble 版本代码，包含两个驱动包：

- `rasp_imu_hat_6dof`：通过 IIC 读取 IMU 数据，使用 Python `rclpy` 发布话题和服务。
- `serial_imu_hat_6dof`：通过串口读取 IMU 数据，使用 C++ `rclcpp` 发布话题和服务。

旧版 ROS1 第三方源码 `imu_tools/` 已删除。ROS2 Humble 下如需滤波或 RViz IMU 插件，请安装发行版包：

```bash
sudo apt install ros-humble-imu-tools
```

## 构建

```bash
source /opt/ros/humble/setup.bash
colcon build --packages-select rasp_imu_hat_6dof serial_imu_hat_6dof
source install/setup.bash
```

IIC 版本依赖树莓派 SMBus：

```bash
sudo apt install python3-smbus
```

## 使用串口发布 IMU 数据

启动串口节点：

```bash
ros2 launch serial_imu_hat_6dof serial_imu_hat.launch.py
```

默认话题和服务名可以在 `serial_imu_hat_6dof/cfg/param.yaml` 中修改：

- `/imu_data`：发布 `sensor_msgs/msg/Imu`
- `/yaw_data`：发布 yaw，单位弧度
- `/pitch_data`：发布 pitch，单位弧度
- `/roll_data`：发布 roll，单位弧度
- `/yaw_zero_topic`：发布空消息将 yaw 归零
- `/yaw_zero_service`：服务方式将 yaw 归零
- `/baud_update_service`：修改 IMU 串口波特率
- `/pin_outHL_service`：控制 D0/D1/D2/D3 引脚输出高低电平
- `/imu_node/GetYawData`：获取当前 yaw

常用命令：

```bash
ros2 topic pub --once /yaw_zero_topic std_msgs/msg/Empty "{}"
ros2 service call /yaw_zero_service serial_imu_hat_6dof/srv/SetYawZero
ros2 service call /baud_update_service serial_imu_hat_6dof/srv/SetBaudRate "{index: 1}"
ros2 service call /pin_outHL_service serial_imu_hat_6dof/srv/SetPinOutHL "{d0: 1, d1: 0, d2: 0, d3: 0}"
ros2 service call /imu_node/GetYawData serial_imu_hat_6dof/srv/GetYawData
```

`SetBaudRate.index` 的含义：`1` 为 9600，`2` 为 57600，`3` 为 115200。修改 IMU 模块波特率后，需要同步修改参数文件中的 `baud_rate` 并重新连接模块。

## 使用 IIC 发布 IMU 数据

启动 IIC 节点：

```bash
ros2 launch rasp_imu_hat_6dof imu_data_pub.launch.py
```

获取当前 yaw：

```bash
ros2 service call /imu_node/GetYawData rasp_imu_hat_6dof/srv/GetYawData
```

ROS1 的 `dynamic_reconfigure` 已替换为 ROS2 参数。运行时修正 yaw：

```bash
ros2 param set /imu_node yaw_calibration 10.0
```

## RViz2 显示

```bash
ros2 launch rasp_imu_hat_6dof imu_rviz_display.launch.py
```