commit 59b058344476c7148374dfbc7952e74a012aaa38 Author: robocon <19589917063@163.com> Date: Thu Aug 13 12:52:06 2026 +0800 Initialize odin1 workspace diff --git a/catkin_ws/README.md b/catkin_ws/README.md new file mode 100644 index 0000000..66373f1 --- /dev/null +++ b/catkin_ws/README.md @@ -0,0 +1,143 @@ +# catkin_ws + +## 简介说明 +16dof采用留形科技的odin1空间记忆模组作为视觉方案 +采用的odin1固件版本为0.12.0 +本目录按 ROS 2 工作空间组织，工作空间根目录为当前目录，源码目录为 `src/`。 +固件下载链接：https://vvcazjv268.feishu.cn/file/AAsOba7nSoGj22xclcKcoadE +Odin1驱动0.11.0获取链接：https://github.com/manifoldsdk/odin_ros_driver.git +本目录仅仅附带 +```text +catkin_ws/ +├── src/ 空 +├── runros.sh +├── readme.md 本文档 +└── odin1的使用手册.md +``` +所附带链接为留形科技官方下载链接 +并默认已配置 官方 ROS 2 功能包： + +- `src/odin_ros_driver` +以及参照官方教程完成系统规则写入 +附带官方github链接：https://github.com/manifoldsdk/odin_ros_driver + +- 注意 odin1需配套对应 **odin1固件版本**， **驱动版本** 以及 **mindcloud软件版本**，详情请查阅官方说明 + +## 目录结构 + +```text +catkin_ws/ +├── src/ +│ └── odin_ros_driver/ +├── build/ +├── install/ +├── runros.sh +├── readme.md +└── log/ +``` + +说明： + +- `src/` 用于存放 ROS 2 功能包源码。 +- `build/`、`install/`、`log/` 为 `colcon build` 后生成的工作空间产物。 + +## 扫图相关（需先完成control_command.yaml配置） + +fire：~/catkin_ws/src/odin_ros_driver/config/control_command.yaml +```yaml +#存颜色 + recorddata: 1 # 0: off; 1: on +#存地图 + custom_map_mode: 1 # 0: Odometry mode 1: SLAM mode 2: Relocalization mode +``` + +### 运行odin +```bash +# 启动官方历程 +./runros.sh +#新建终端 保存地图 +cd ~/catkin_ws/src/odin_ros_driver/ +./set_param.sh save_map 1 +``` +将生成的文件导入留形官方后处理软件mindcloud +下载地址：https://version.manifoldtech.cn/download/mcs + + + +## 使用方法 + +1. 进入工作空间根目录： + +```bash +cd ~/catkin_ws +``` +## 提供一个一键构建脚本runros.sh +```bash +#请先赋予可执行权限，此脚本可代替ros2空间构建命令 +./runros.sh +``` +2. 先加载 ROS 2 环境： + +```bash +source /opt/ros/<你的发行版>/setup.bash +``` + +3. 构建工作空间： + +```bash +colcon build +``` + +4. 加载工作空间环境： + +```bash +source /home/lcf/catkin_ws/install/setup.bash +``` + +5. 查看工作空间中的包： + +```bash +colcon list +``` + +6. 在当前终端加载环境： + +```bash +source ~/catkin_ws/runros.sh +``` + +7. 一键启动默认 ROS 2 驱动： + +```bash +~/catkin_ws/runros.sh +``` + +默认等价于： + +```bash +source ~/catkin_ws/install/setup.bash +ros2 launch odin_ros_driver odin1_ros2.launch.py +``` + +8. 如需只打开一个已加载环境的干净交互 bash： + +```bash +~/catkin_ws/runros.sh --shell +``` + +9. 直接通过脚本执行 `ros2` 或 `rviz2` 命令： + +```bash +~/catkin_ws/runros.sh ros2 topic list +~/catkin_ws/runros.sh rviz2 -d /home/lcf/catkin_ws/src/odin_ros_driver/config/odin_ros2.rviz +``` + +## 说明 + +- 当前根目录已经是 ROS 2 工作空间根目录，无需额外执行 `ros2 workspace create` 之类命令。 +- 若构建失败，请先确认系统已安装 ROS 2、`colcon` 以及包依赖。 +- `runros.sh` 会自动查找 `/opt/ros` 下可用的 ROS 2 发行版，并在存在 `install/setup.bash` 时自动叠加当前工作空间环境。 +- `runros.sh` 会将 ROS 2 运行日志写入工作空间下的 `log/ros2/`，避免依赖用户主目录中的默认 `~/.ros/log`。 +- 直接执行 `runros.sh` 且不带参数时，会默认执行 `ros2 launch odin_ros_driver odin1_ros2.launch.py`，用于一键启动驱动。 +- 如需只进入 shell，请显式使用 `runros.sh --shell`；该模式会进入一个不读取用户 `~/.bashrc`/`~/.profile` 的干净交互 bash，避免用户 shell 初始化脚本覆盖或污染当前 ROS 环境。 +- 直接执行 `runros.sh` 且带参数时，会在完成环境加载后直接执行该命令，适合一键启动 `ros2` 或 `rviz2`。 diff --git a/catkin_ws/odin1的使用手册.md b/catkin_ws/odin1的使用手册.md new file mode 100644 index 0000000..c87ceea --- /dev/null +++ b/catkin_ws/odin1的使用手册.md @@ -0,0 +1,609 @@ +# Odin1 的使用手册 + +## 1. 文档来源与适用范围 + +本文根据 Odin1 官方仓库内容整理，官方仓库克隆位置为： + +- `/home/lcf/catkin_ws/ziliao/odin_ros_driver` + +本手册重点面向你当前这台机器上的 **ROS2 使用方式**，并结合你当前工作空间实际路径整理为可直接操作的版本： + +- 工作空间根目录：`/home/lcf/catkin_ws` +- 实际运行包目录：`/home/lcf/catkin_ws/src/odin_ros_driver` +- 官方参考仓库目录：`/home/lcf/catkin_ws/ziliao/odin_ros_driver` + +## 2. Odin1 是什么 + +Odin1 官方 ROS 驱动 `odin_ros_driver` 是一个同时支持 ROS1 和 ROS2 的驱动包，核心用途包括： + +- 连接 Odin1 设备并发布点云、IMU、RGB、里程计、TF 等数据 +- 提供里程计模式、SLAM 建图模式、重定位模式 +- 支持 RViz 可视化 +- 支持在线调节 AE/AWB +- 支持地图保存与重定位 + +官方说明里明确推荐： + +- ROS2 推荐使用 `Humble` +- Ubuntu 推荐使用 `22.04` +- 设备固件版本要求 `v0.11.11` + +## 3. 官方仓库里与 ROS2 最相关的文件 + +- `README.md` + - 官方主说明，包含安装、运行、建图、常见问题 +- `RELOCALIZATION_GUIDE.md` + - 重定位专门说明 +- `config/control_command.yaml` + - 运行参数总配置，建图和重定位主要看这个文件 +- `launch_ROS2/odin1_ros2.launch.py` + - ROS2 启动入口 +- `script/build_ros2.sh` + - 官方 ROS2 构建脚本 +- `set_param.sh` + - 用于在运行时发送参数命令，保存地图时会用到 + +## 4. ROS2 启动流程总结 + +官方 README 的 ROS2 逻辑本质上是两步： + +```bash +source /home/lcf/catkin_ws/install/setup.bash +ros2 launch odin_ros_driver odin1_ros2.launch.py +``` + +你当前环境里已经额外有一个一键脚本： + +```bash +/home/lcf/catkin_ws/runros.sh +``` + +这条命令现在默认就等价于上面两条命令，所以日常使用时建议优先用它。 + +## 5. ROS2 启动时实际会拉起什么 + +根据官方 `launch_ROS2/odin1_ros2.launch.py`，启动时会拉起以下节点： + +- `host_sdk_sample` + - 主驱动节点，负责设备连接、数据流和核心逻辑 +- `pcd2depth_ros2_node` + - 深度图相关节点 +- `cloud_reprojection_ros2_node` + - 点云投影相关节点 +- `image_overlay_node` + - 图像叠加相关节点 +- `rviz2` + - 默认会同时启动 RViz2，并加载 `config/odin_ros2.rviz` + +所以官方 ROS2 launch 不是只起一个驱动进程，而是“驱动 + 辅助处理节点 + RViz2”的完整演示链路。 + +## 6. 先决条件 + +### 6.1 系统与依赖 + +官方建议： + +- Ubuntu 22.04 +- ROS2 Humble +- OpenCV 只保留一个版本 +- 已安装 `yaml-cpp`、`libusb`、`Eigen3`、OpenSSL 等依赖 + +### 6.2 Udev 规则 + +官方要求添加 udev 规则，否则可能遇到 USB 访问权限问题： + +```bash +sudo vim /etc/udev/rules.d/99-odin-usb.rules +``` + +写入： + +```bash +SUBSYSTEM=="usb", ATTR{idVendor}=="2207", ATTR{idProduct}=="0019", MODE="0666", GROUP="plugdev" +``` + +然后执行： + +```bash +sudo udevadm control --reload +sudo udevadm trigger +``` + +### 6.3 可能遇到的 USB 权限问题 + +如果启动时报： + +- `LIBUSB_ERROR_ACCESS` +- `LIBUSB_ERROR_BUSY` + +优先检查： + +1. 是否已经添加 udev 规则 +2. 当前用户是否在 `plugdev` 组 +3. 是否有旧的 `host_sdk_sample` 进程还在占用设备 + +可用命令： + +```bash +ps aux | grep host_sdk_sample +killall host_sdk_sample +``` + +## 7. 你当前环境下的推荐使用方式 + +### 7.1 一键启动 + +```bash +/home/lcf/catkin_ws/runros.sh +``` + +该命令会自动： + +1. 加载 `/opt/ros/humble/setup.bash` +2. 加载 `/home/lcf/catkin_ws/install/setup.bash` +3. 执行： + +```bash +ros2 launch odin_ros_driver odin1_ros2.launch.py +``` + +### 7.2 只加载环境 + +```bash +source /home/lcf/catkin_ws/runros.sh +``` + +### 7.3 只开一个干净 shell + +```bash +/home/lcf/catkin_ws/runros.sh --shell +``` + +## 8. Odin1 的三种工作模式 + +官方通过 `config/control_command.yaml` 中的 `custom_map_mode` 控制工作模式： + +- `0`：里程计模式 +- `1`：SLAM 建图模式 +- `2`：重定位模式 + +### 8.1 里程计模式 + +```yaml +custom_map_mode: 0 +``` + +特点： + +- `map` 和 `odom` 视为同一位姿 +- 不进行回环建图 +- 适合只看实时位姿和点云 + +如果里程计漂移，官方建议在运行过程中执行： + +```bash +cd /home/lcf/catkin_ws/src/odin_ros_driver +./set_param.sh algo_reset 1 +``` + +### 8.2 SLAM 建图模式 + +```yaml +custom_map_mode: 1 +``` + +特点： + +- 在里程计模式基础上增加回环检测和地图保存能力 +- 启动后自动开始建图并缓存地图数据 +- 地图不会自动保存，必须手动触发保存 + +### 8.3 重定位模式 + +```yaml +custom_map_mode: 2 +``` + +特点： + +- 基于已有地图重新定位 +- 必须指定地图文件路径 +- 成功后会发布 `map -> odom` 的 TF + +## 9. ROS2 下怎么建图 + +这是你最关心的部分，按下面流程做。 + +### 9.1 第一步：修改配置为建图模式 + +编辑实际运行中的配置文件： + +- `/home/lcf/catkin_ws/src/odin_ros_driver/config/control_command.yaml` + +至少确认这些字段： + +```yaml +register_keys: + custom_map_mode: 1 + relocalization_map_abs_path: "" +``` + +建议同时关注： + +- `strict_usb3.0_check` + - 默认 `0`，允许非严格 USB3.0；但官方强调建图最好使用 USB3.0 +- `senddtof` + - 点云开关 +- `sendodom` + - 里程计开关 +- `sendcloudslam` + - SLAM 点云开关 +- `sendcloudrender` + - 渲染点云开关 +- `showpath` + - 是否显示路径 + +如果你想更明显地看路径，可考虑： + +```yaml +showpath: 1 +``` + +### 9.2 第二步：启动驱动 + +推荐直接执行： + +```bash +/home/lcf/catkin_ws/runros.sh +``` + +或者手动执行： + +```bash +source /home/lcf/catkin_ws/install/setup.bash +ros2 launch odin_ros_driver odin1_ros2.launch.py +``` + +### 9.3 第三步：移动设备开始建图 + +驱动启动后，Odin1 会自动开始建图并缓存地图数据。 + +建议操作方式： + +- 使用 USB3.0 连接 +- 启动后缓慢移动设备 +- 尽量覆盖你希望建图的区域 +- 尽量回到已走过区域，帮助回环闭合 +- 场景中应有足够几何/纹理特征 + +### 9.4 第四步：保存地图 + +建图完成后，进入包目录： + +```bash +cd /home/lcf/catkin_ws/src/odin_ros_driver +``` + +执行官方命令： + +```bash +./set_param.sh save_map 1 +``` + +`set_param.sh` 的本质是向 `/tmp/odin_command.txt` 写入： + +```bash +set save_map 1 +``` + +主驱动会读取这个命令并调用 SDK 保存地图。 + +### 9.5 第五步：找到保存出来的地图文件 + +配置文件里写明： + +```yaml +mapping_result_dest_dir: "" +mapping_result_file_name: "" +``` + +如果这两个都为空，则官方默认保存到： + +- `{ws}/src/odin_ros_driver/map/{driver_start_time}/` + +结合你当前工作空间，默认可按下面路径去找： + +- `/home/lcf/catkin_ws/src/odin_ros_driver/map/` + +实际生成的地图文件通常是 `.bin` 文件。 + +### 9.6 第六步：重复保存 + +官方说明允许重复执行： + +```bash +./set_param.sh save_map 1 +``` + +但两次保存之间建议至少间隔 **5 秒**。 + +## 10. 重定位怎么用 + +官方重定位文档单独给了更详细说明，这里按实用方式整理。 + +### 10.1 自动重定位 + +修改配置： + +```yaml +register_keys: + custom_map_mode: 2 + relocalization_map_abs_path: "/home/lcf/catkin_ws/src/odin_ros_driver/map/你的地图目录/xxx.bin" + custom_init_pos: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0] +``` + +推荐条件： + +- 起始位置距离原始建图轨迹 1 米以内 +- 起始朝向与建图时偏差不超过 ±10° +- 场景特征明显 + +启动后行为： + +1. 尝试将当前观测和已有地图匹配 +2. 成功后发布 `map -> odom` TF +3. 失败时进入后备模式并继续尝试 + +### 10.2 指定初始位姿重定位 + +如果你知道大概起点位置，可设置： + +```yaml +register_keys: + custom_map_mode: 2 + relocalization_map_abs_path: "/绝对路径/xxx.bin" + custom_init_pos: [x, y, z, qx, qy, qz, qw] +``` + +含义是： + +- `x y z`：地图坐标系中的位置 +- `qx qy qz qw`：四元数姿态 + +例如绕 Z 轴 90°： + +```yaml +custom_init_pos: [5.2, -3.1, 0.0, 0.0, 0.0, 0.707, 0.707] +``` + +### 10.3 与官方文档的一个小冲突 + +官方主 README 的参数表里写过： + +- `custom_init_pos`：currently unused + +但官方单独的 `RELOCALIZATION_GUIDE.md` 已经把它作为重定位功能来详细说明，并给了示例，因此更应以重定位指南为准。 + +如果你实际测试发现 `custom_init_pos` 不生效，优先检查： + +1. `custom_map_mode` 是否为 `2` +2. `custom_init_pos` 是否正好 7 个值 +3. 四元数是否归一化 +4. 地图文件路径是否为绝对路径 + +## 11. 建图与重定位的推荐操作套路 + +### 11.1 首次建图 + +1. 把 `custom_map_mode` 设为 `1` +2. 启动： + +```bash +/home/lcf/catkin_ws/runros.sh +``` + +3. 缓慢移动 Odin1，采完整个区域 +4. 返回包目录保存地图： + +```bash +cd /home/lcf/catkin_ws/src/odin_ros_driver +./set_param.sh save_map 1 +``` + +5. 到 `map/` 目录下找到 `.bin` 地图文件 + +### 11.2 下次基于已有地图定位 + +1. 把 `custom_map_mode` 改为 `2` +2. 把 `relocalization_map_abs_path` 指到上一步生成的 `.bin` +3. 若知道起点，补 `custom_init_pos` +4. 再执行： + +```bash +/home/lcf/catkin_ws/runros.sh +``` + +## 12. 常用参数解释 + +以下参数来自官方 `control_command.yaml`，是 ROS2 下最常用的一批。 + +### 12.1 时间与同步 + +- `use_host_ros_time` + - `0`：使用设备时间，官方推荐 + - `1`：收到数据时用主机 ROS 时间 + - `2`：对齐到主机时间轴 + +建议： + +- 一般先保持 `0` + +### 12.2 传感器输出 + +- `sendimu` + - 是否发布 IMU +- `sendodom` + - 是否发布里程计 +- `senddtof` + - 是否发布原始点云 +- `sendcloudslam` + - 是否发布 SLAM 点云 +- `sendcloudrender` + - 是否发布渲染后的点云 +- `sendrgb` + - 是否发布 RGB 图像 +- `sendrgbcompressed` + - 是否发布压缩 RGB 图像 + +### 12.3 点云质量相关 + +- `cloud_raw_confidence_threshold` + - 原始点云置信度阈值，官方示例是 `35` +- `dtof_fps` + - 官方注释给出 `100`、`145`、`290` 等档位 + +### 12.4 其他 + +- `recorddata` + - 是否录制官方私有格式数据，空间占用很大 +- `devstatuslog` + - 是否记录设备状态日志 +- `showpath` + - 是否显示路径 +- `showcamerapose` + - 是否显示相机位姿 + +## 13. ROS2 下常用话题 + +官方 README 给出的常用话题包括： + +- `/odin1/imu` +- `/odin1/image` +- `/odin1/image_undistort` +- `/odin1/image/compressed` +- `/odin1/cloud_raw` +- `/odin1/cloud_render` +- `/odin1/cloud_slam` +- `/odin1/odometry` +- `/odin1/odometry_high` +- `/odin1/path` +- `/tf` +- `/odin1/reprojected_image` + +重定位成功后，`/tf` 中会出现 `map -> odom` 关系。 + +## 14. 在线调参能力 + +官方驱动提供四个 ROS2 服务，可在驱动运行时调整自动曝光和自动白平衡： + +- `/odin1/get_ae` +- `/odin1/get_awb` +- `/odin1/set_ae` +- `/odin1/set_awb` + +示例： + +```bash +source /home/lcf/catkin_ws/install/setup.bash +ros2 service call /odin1/get_ae odin_ros_driver/srv/GetAe +ros2 service call /odin1/get_awb odin_ros_driver/srv/GetAwb +ros2 service call /odin1/set_ae odin_ros_driver/srv/SetAe "{mode: 0}" +ros2 service call /odin1/set_awb odin_ros_driver/srv/SetAwb "{mode: 0}" +``` + +## 15. 常见问题总结 + +### 15.1 启动后设备很快断开 + +官方认为 ROS2 在复杂网络环境下容易因为广播导致阻塞，从而出现设备断开。 + +如果不需要跨设备通信，可先尝试： + +```bash +export ROS_LOCALHOST_ONLY=1 +``` + +### 15.2 RViz 卡死或长时间无响应 + +官方建议先给 Odin1 重新上电。 + +### 15.3 重启设备后 RViz 出现 `TF_OLD_DATA` + +官方说明这是时间戳冲突导致的，点击 RViz 底部 reset 按钮可清掉旧状态。 + +### 15.4 图像相关节点一启动就崩 + +官方提示这通常与系统里安装了多个 OpenCV 版本有关。 + +### 15.5 `Missing camera node 'cam_0'` + +官方建议重新插拔 USB。 + +### 15.6 录 bag 时 IMU 或高频里程计丢帧 + +官方仓库提供了： + +- `script/rosbag2_qos.yaml` + +录制时建议： + +```bash +ros2 bag record -a \ + --qos-profile-overrides-path src/odin_ros_driver/script/rosbag2_qos.yaml \ + -o my_bag +``` + +## 16. 你当前环境下的最简建图命令 + +### 16.1 建图前 + +编辑： + +- `/home/lcf/catkin_ws/src/odin_ros_driver/config/control_command.yaml` + +设置： + +```yaml +custom_map_mode: 1 +``` + +### 16.2 启动建图 + +```bash +/home/lcf/catkin_ws/runros.sh +``` + +### 16.3 保存地图 + +```bash +cd /home/lcf/catkin_ws/src/odin_ros_driver +./set_param.sh save_map 1 +``` + +### 16.4 做重定位 + +编辑： + +- `/home/lcf/catkin_ws/src/odin_ros_driver/config/control_command.yaml` + +设置： + +```yaml +custom_map_mode: 2 +relocalization_map_abs_path: "/home/lcf/catkin_ws/src/odin_ros_driver/map/你的目录/你的地图.bin" +``` + +然后再次启动： + +```bash +/home/lcf/catkin_ws/runros.sh +``` + +## 17. 我对官方文档的实用化结论 + +如果只用一句话总结官方仓库的 ROS2 使用方式： + +- **建图**：把 `custom_map_mode` 设为 `1`，启动驱动，移动设备采图，最后执行 `./set_param.sh save_map 1` +- **重定位**：把 `custom_map_mode` 设为 `2`，设置 `relocalization_map_abs_path` 指向 `.bin` 地图，再重新启动驱动 +- **启动**：在你当前环境里，最省事的方式就是直接运行 `/home/lcf/catkin_ws/runros.sh` + diff --git a/catkin_ws/project.log b/catkin_ws/project.log new file mode 100644 index 0000000..afb9a4c --- /dev/null +++ b/catkin_ws/project.log @@ -0,0 +1,25 @@ +2026-06-19 15:19:22 CST +- 初始化 `/home/lcf/catkin_ws2` 为 ROS 2 工作空间根目录。 +- 确认根目录已存在 `src/`，其中包含 ROS 2 包 `src/odin_ros_driver`。 +- 新增工作空间根目录 `README.md`，补充目录结构、构建命令与环境加载说明。 +- 新增工作空间根目录 `.gitignore`，忽略 `build/`、`install/`、`log/` 目录。 +- 检查本机环境，确认已安装 `colcon`，且存在 ROS 2 发行版 `/opt/ros/humble`。 +- 使用 `source /opt/ros/humble/setup.bash && colcon list` 校验工作空间，已成功识别 `odin_ros_driver`。 +2026-06-19 15:19:22 CST +- 新增工作空间根目录脚本 `runros.sh`，支持一键加载 ROS 2 基础环境与当前工作空间环境。 +- `runros.sh` 同时支持 `source ./runros.sh` 和直接执行 `./runros.sh` 两种使用方式。 +- 更新根目录 `README.md`，补充 `runros.sh` 的使用说明与行为说明。 +2026-06-19 15:35:12 CST +- 为 `runros.sh` 增加调试日志落盘，记录脚本调用方式、ROS 环境加载结果、工作空间覆盖层加载结果与 shell 交接链路。 +- 修复 `runros.sh` 直接执行时拉起交互 shell 会读取用户 `~/.bashrc` 的问题，改为启动不读取用户 rc 文件的干净交互 bash。 +- 扩展 `runros.sh`，支持直接执行传入命令，例如 `ros2`、`rviz2`，以便一键在已加载环境中启动目标程序。 +- 修正用户 `~/.bashrc` 中会自我追加 `source` 语句的错误配置，并将重复加载工作空间环境的逻辑收敛为一次且带存在性判断。 +- 更新根目录 `README.md`，修正工作空间路径为 `/home/lcf/catkin_ws`，并补充直接执行 `ros2`/`rviz2` 的示例。 +2026-06-19 15:52:00 CST +- 调整 `runros.sh` 默认行为：直接执行脚本时不再进入交互 shell，而是默认执行 `ros2 launch odin_ros_driver odin1_ros2.launch.py`，满足一键启动需求。 +- 为 `runros.sh` 新增 `--shell` 和 `--help` 入口，分别用于显式打开干净交互 shell 与查看脚本使用说明。 +- 更新根目录 `README.md`，补充 `runros.sh` 默认一键启动、`--shell` 模式及其与 `source install/setup.bash + ros2 launch` 的对应关系。 +- 为 `runros.sh` 新增工作空间内 `ROS_LOG_DIR` 设置，将 ROS 2 运行日志固定写入 `/home/lcf/catkin_ws/log/ros2`，避免依赖用户主目录默认日志目录导致的一键启动失败。 +2026-06-19 16:15:56 CST +- 阅读 `/home/lcf/catkin_ws/ziliao/odin_ros_driver` 中的 Odin1 官方仓库资料，重点整理 ROS2 相关的 README、重定位指南、启动文件、配置文件和运行时命令接口。 +- 新增根目录说明文档 `/home/lcf/catkin_ws/odin1的使用手册.md`，总结 Odin1 在当前工作空间下的 ROS2 启动、建图、保存地图、重定位、常用参数、话题与常见问题。 diff --git a/catkin_ws/runros.sh b/catkin_ws/runros.sh new file mode 100644 index 0000000..d51049b --- /dev/null +++ b/catkin_ws/runros.sh @@ -0,0 +1,359 @@ +#!/bin/bash + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +WORKSPACE_ROOT="${SCRIPT_DIR}" +DEBUG_LOG_FILE="${WORKSPACE_ROOT}/trae-debug-log-runros-shell-loop.ndjson" +ROS_LOG_ROOT="${WORKSPACE_ROOT}/log/ros2" +DEFAULT_LAUNCH_PACKAGE="odin_ros_driver" +DEFAULT_LAUNCH_FILE="odin1_ros2.launch.py" + +# Function: print_info +# Input: +# - $1: message text. +# Output: +# - Prints an informational message to stdout. +# Description: +# - Standardizes normal runtime messages for this script. +# References: +# - None. +print_info() { + echo "[INFO] $1" +} + +# Function: print_warn +# Input: +# - $1: warning text. +# Output: +# - Prints a warning message to stdout. +# Description: +# - Standardizes warning messages for this script. +# References: +# - None. +print_warn() { + echo "[WARN] $1" +} + +# Function: print_error +# Input: +# - $1: error text. +# Output: +# - Prints an error message to stderr. +# Description: +# - Standardizes error messages for this script. +# References: +# - None. +print_error() { + echo "[ERROR] $1" >&2 +} + +# Function: write_debug_log +# Input: +# - $1: event name. +# - $2: event detail text. +# Output: +# - Appends one NDJSON debug record to the local debug log file. +# Description: +# - Records runtime evidence for script invocation mode, environment loading, +# and interactive shell handoff without changing business behavior. +# References: +# - Uses variables `DEBUG_LOG_FILE`, `WORKSPACE_ROOT`, and `ROS_DISTRO` +# defined in this file. +# - Called by `load_ros2_environment()`, `load_workspace_environment()`, +# `start_interactive_shell()`, and `main()` in this file. +write_debug_log() { + local event_name="$1" + local event_detail="$2" + local invoke_mode="executed" + + if is_script_sourced; then + invoke_mode="sourced" + fi + + printf '{"ts":"%s","event":"%s","detail":"%s","pid":"%s","ppid":"%s","mode":"%s","shell":"%s","workspace":"%s","ros_distro":"%s"}\n' \ + "$(date '+%Y-%m-%dT%H:%M:%S%z')" \ + "${event_name}" \ + "${event_detail}" \ + "$$" \ + "$PPID" \ + "${invoke_mode}" \ + "${SHELL:-/bin/bash}" \ + "${WORKSPACE_ROOT}" \ + "${ROS_DISTRO:-unset}" >> "${DEBUG_LOG_FILE}" +} + +# Function: is_script_sourced +# Input: +# - None. +# Output: +# - Returns 0 when the script is sourced. +# - Returns 1 when the script is executed directly. +# Description: +# - Detects whether the current script is loaded into the caller shell or +# started as a standalone process. +# References: +# - Uses bash built-in variables `${BASH_SOURCE[0]}` and `${0}`. +is_script_sourced() { + [[ "${BASH_SOURCE[0]}" != "${0}" ]] +} + +# Function: find_ros2_setup +# Input: +# - None. +# Output: +# - Prints the absolute path of the detected ROS 2 `setup.bash`. +# - Returns 1 if no ROS 2 installation is found. +# Description: +# - Prefers the current `ROS_DISTRO` when available, otherwise probes common +# ROS 2 distributions from newer to older. +# References: +# - Uses environment variable `ROS_DISTRO`. +# - Searches under `/opt/ros//setup.bash`. +find_ros2_setup() { + local ros2_setup="" + local distros=("jazzy" "iron" "humble" "galactic" "foxy" "rolling") + local distro="" + + if [ -n "${ROS_DISTRO}" ] && [ -f "/opt/ros/${ROS_DISTRO}/setup.bash" ]; then + echo "/opt/ros/${ROS_DISTRO}/setup.bash" + return 0 + fi + + for distro in "${distros[@]}"; do + if [ -f "/opt/ros/${distro}/setup.bash" ]; then + ros2_setup="/opt/ros/${distro}/setup.bash" + echo "${ros2_setup}" + return 0 + fi + done + + return 1 +} + +# Function: load_ros2_environment +# Input: +# - None. +# Output: +# - Returns 0 when the ROS 2 environment is sourced successfully. +# - Returns 1 when no valid ROS 2 environment is found. +# Description: +# - Locates and loads the base ROS 2 environment required by the workspace. +# References: +# - Calls `find_ros2_setup()` in this file. +# - Sources `/opt/ros//setup.bash`. +load_ros2_environment() { + local ros2_setup="" + + ros2_setup="$(find_ros2_setup)" || { + write_debug_log "load_ros2_environment_failed" "no_ros2_setup_found" + print_error "No ROS 2 installation was found under /opt/ros." + return 1 + } + + # shellcheck disable=SC1090 + source "${ros2_setup}" + write_debug_log "load_ros2_environment" "${ros2_setup}" + print_info "Loaded ROS 2 environment: ${ros2_setup}" + return 0 +} + +# Function: load_workspace_environment +# Input: +# - None. +# Output: +# - Returns 0 after attempting to load the workspace environment. +# Description: +# - Loads the current workspace overlay when `install/setup.bash` exists. +# When the workspace has not been built yet, it keeps only the base ROS 2 +# environment and prints the recommended next step. +# References: +# - Uses variable `WORKSPACE_ROOT` defined in this file. +# - Sources `${WORKSPACE_ROOT}/install/setup.bash`. +load_workspace_environment() { + local workspace_setup="${WORKSPACE_ROOT}/install/setup.bash" + + cd "${WORKSPACE_ROOT}" || return 1 + + if [ -f "${workspace_setup}" ]; then + # shellcheck disable=SC1090 + source "${workspace_setup}" + write_debug_log "load_workspace_environment" "${workspace_setup}" + print_info "Loaded workspace environment: ${workspace_setup}" + else + write_debug_log "load_workspace_environment_missing" "${workspace_setup}" + print_warn "Workspace overlay not found: ${workspace_setup}" + print_warn "Run 'colcon build' first if you need package overlays." + fi + + export ROS_WORKSPACE="${WORKSPACE_ROOT}" + print_info "Workspace root: ${WORKSPACE_ROOT}" + return 0 +} + +# Function: ensure_ros_log_directory +# Input: +# - None. +# Output: +# - Returns 0 when the ROS 2 log directory is ready for use. +# - Returns 1 when the log directory cannot be created. +# Description: +# - Ensures ROS 2 launch logs are written into the workspace-local log +# directory instead of relying on the user's home directory. +# References: +# - Uses variables `ROS_LOG_ROOT` and `WORKSPACE_ROOT` defined in this file. +# - Called by `main()` in this file. +ensure_ros_log_directory() { + mkdir -p "${ROS_LOG_ROOT}" || { + write_debug_log "ensure_ros_log_directory_failed" "${ROS_LOG_ROOT}" + print_error "Failed to create ROS log directory: ${ROS_LOG_ROOT}" + return 1 + } + + export ROS_LOG_DIR="${ROS_LOG_ROOT}" + write_debug_log "ensure_ros_log_directory" "${ROS_LOG_DIR}" + print_info "ROS log directory: ${ROS_LOG_DIR}" + return 0 +} + +# Function: run_command_with_environment +# Input: +# - $@: command and arguments to execute. +# Output: +# - Replaces the current process with the provided command. +# Description: +# - Executes a user-specified command after the ROS 2 and workspace +# environment have been loaded successfully. +# References: +# - Called by `main()` in this file. +run_command_with_environment() { + write_debug_log "run_command_with_environment" "$*" + print_info "Running command with ROS 2 environment loaded: $*" + exec "$@" +} + +# Function: launch_default_ros2_stack +# Input: +# - None. +# Output: +# - Replaces the current process with the default ROS 2 launch command. +# Description: +# - Starts the default `odin_ros_driver` ROS 2 launch file after the +# workspace environment has been loaded successfully. +# References: +# - Uses variables `DEFAULT_LAUNCH_PACKAGE` and `DEFAULT_LAUNCH_FILE` +# defined in this file. +# - Calls `run_command_with_environment()` in this file. +launch_default_ros2_stack() { + write_debug_log "launch_default_ros2_stack" "${DEFAULT_LAUNCH_PACKAGE} ${DEFAULT_LAUNCH_FILE}" + print_info "Starting default ROS 2 launch: ${DEFAULT_LAUNCH_PACKAGE} ${DEFAULT_LAUNCH_FILE}" + run_command_with_environment ros2 launch "${DEFAULT_LAUNCH_PACKAGE}" "${DEFAULT_LAUNCH_FILE}" +} + +# Function: start_interactive_shell +# Input: +# - None. +# Output: +# - Replaces the current process with a clean interactive bash shell. +# Description: +# - Keeps the loaded ROS 2 and workspace environment in a new interactive +# shell when the script is executed directly, while avoiding user shell +# rc files that may recursively modify configuration. +# References: +# - Uses `/bin/bash --noprofile --norc -i`. +start_interactive_shell() { + write_debug_log "start_interactive_shell" "exec_clean_interactive_bash" + print_info "Starting a clean interactive bash shell with ROS 2 environment loaded." + exec /bin/bash --noprofile --norc -i +} + +# Function: print_usage +# Input: +# - None. +# Output: +# - Prints the script usage text to stdout. +# Description: +# - Documents the default one-click launch behavior, the custom command mode, +# and the explicit shell mode for this script. +# References: +# - Uses variables `DEFAULT_LAUNCH_PACKAGE` and `DEFAULT_LAUNCH_FILE` +# defined in this file. +print_usage() { + cat < [args...] + +Behavior: + - source runros.sh + Load ROS 2 and workspace environment into the current shell. + - runros.sh + Launch: ros2 launch ${DEFAULT_LAUNCH_PACKAGE} ${DEFAULT_LAUNCH_FILE} + - runros.sh --shell + Open a clean interactive bash shell with the environment loaded. + - runros.sh [args...] + Run the provided command with the environment loaded. +EOF +} + +# Function: main +# Input: +# - None. +# Output: +# - Returns 0 on success. +# - Returns 1 when required environment loading fails. +# Description: +# - Coordinates ROS 2 base environment loading, workspace overlay loading, +# and chooses behavior for sourced vs executed usage. +# References: +# - Calls `is_script_sourced()` in this file. +# - Calls `load_ros2_environment()` in this file. +# - Calls `load_workspace_environment()` in this file. +# - Calls `ensure_ros_log_directory()` in this file. +# - Calls `launch_default_ros2_stack()` in this file. +# - Calls `run_command_with_environment()` in this file. +# - Calls `start_interactive_shell()` in this file. +# - Calls `print_usage()` in this file. +main() { + write_debug_log "main_enter" "argv:$*" + + if ! is_script_sourced; then + case "$1" in + -h|--help) + print_usage + return 0 + ;; + esac + fi + + load_ros2_environment || return 1 + load_workspace_environment || return 1 + ensure_ros_log_directory || return 1 + + if is_script_sourced; then + if [ "$#" -gt 0 ]; then + write_debug_log "main_warn" "arguments_ignored_when_sourced" + print_warn "Arguments are ignored when the script is sourced." + fi + write_debug_log "main_exit" "current_shell_ready" + print_info "Environment is ready in the current shell." + return 0 + fi + + if [ "$#" -eq 0 ]; then + write_debug_log "main_handoff" "default_launch_requested" + launch_default_ros2_stack + fi + + if [ "$1" = "--shell" ]; then + write_debug_log "main_handoff" "interactive_shell_requested" + start_interactive_shell + fi + + if [ "$#" -gt 0 ]; then + write_debug_log "main_handoff" "command_execution_requested" + run_command_with_environment "$@" + fi +} + +main "$@" diff --git a/catkin_ws/src/odin_ros_driver/.gitignore b/catkin_ws/src/odin_ros_driver/.gitignore new file mode 100644 index 0000000..5829d8f --- /dev/null +++ b/catkin_ws/src/odin_ros_driver/.gitignore @@ -0,0 +1,4 @@ +recorddata/ +/config/calib.yaml +/log +/map \ No newline at end of file diff --git a/catkin_ws/src/odin_ros_driver/RELOCALIZATION_GUIDE.md b/catkin_ws/src/odin_ros_driver/RELOCALIZATION_GUIDE.md new file mode 100644 index 0000000..e6e12c2 --- /dev/null +++ b/catkin_ws/src/odin_ros_driver/RELOCALIZATION_GUIDE.md @@ -0,0 +1,642 @@ +# Relocalization Guide / 重定位使用指南 + +This guide explains how to use the relocalization feature in Odin ROS Driver, including automatic relocalization and init position relocalization modes. + +本指南介绍如何使用 Odin ROS Driver 的重定位功能，包括自动重定位和指定初始位置重定位两种模式。 + +--- + +## Table of Contents / 目录 + +1. [Overview / 概述](#overview--概述) +2. [Prerequisites / 前提条件](#prerequisites--前提条件) +3. [Mode 1: Auto Relocalization / 自动重定位](#mode-1-auto-relocalization--自动重定位) +4. [Mode 2: Init Position Relocalization / 指定初始位置重定位](#mode-2-init-position-relocalization--指定初始位置重定位) +5. [init_pos Format / init_pos 格式说明](#init_pos-format--init_pos-格式说明) +6. [Configuration Examples / 配置示例](#configuration-examples--配置示例) +7. [Programmatic API / 编程接口](#programmatic-api--编程接口) +8. [Troubleshooting / 故障排除](#troubleshooting--故障排除) + +--- + +## Overview / 概述 + +### English + +Relocalization mode (`custom_map_mode: 2`) allows Odin to localize itself within a pre-built map. There are two approaches: + +| Mode | Description | Use Case | +|------|-------------|----------| +| **Auto Relocalization** | Algorithm automatically searches for position in the map | Starting position is unknown or within recommended range | +| **Init Position Relocalization** | User provides an initial pose estimate via `init_pos` | Starting position is known, faster convergence needed | + +### 中文 + +重定位模式（`custom_map_mode: 2`）允许 Odin 在预先构建的地图中进行自我定位。有两种方式： + +| 模式 | 描述 | 适用场景 | +|------|------|----------| +| **自动重定位** | 算法自动在地图中搜索位置 | 起始位置未知，或在推荐范围内 | +| **指定初始位置重定位** | 用户通过 `init_pos` 提供初始位姿估计 | 起始位置已知，需要更快收敛 | + +--- + +## Prerequisites / 前提条件 + +### English + +1. **Pre-built map file**: A `.bin` map file created in SLAM mode (`custom_map_mode: 1`) +2. **Map file path**: Know the absolute path to your map file +3. **Starting position**: For init position mode, know the approximate starting pose in map coordinates + +### 中文 + +1. **预构建的地图文件**：在 SLAM 模式（`custom_map_mode: 1`）下创建的 `.bin` 地图文件 +2. **地图文件路径**：知道地图文件的绝对路径 +3. **起始位置**：对于指定初始位置模式，需要知道在地图坐标系中的大致起始位姿 + +--- + +## Mode 1: Auto Relocalization / 自动重定位 + +### English + +In auto relocalization mode, the algorithm automatically searches for the device's position within the map based on current sensor observations. + +**Configuration** (`config/control_command.yaml`): + +```yaml +register_keys: + custom_map_mode: 2 + relocalization_map_abs_path: "/path/to/your/map.bin" + # custom_init_pos is NOT set or uses default [0,0,0,0,0,0,1] +``` + +**Recommended Starting Conditions**: +- Within **1 meter** of a position on the original SLAM trajectory +- Within **±10 degrees** of the original orientation +- In a visually distinctive area of the map + +**Behavior**: +1. On startup, Odin attempts to match current observations with the map +2. If successful, TF between `map` and `odom` frames is published +3. If unsuccessful, system operates in fallback SLAM mode (map saving disabled) +4. Relocalization attempts continue in background until successful + +**Tips**: +- Gently shaking or moving the device after startup can improve relocalization accuracy +- Highly distinctive scenes may allow successful matching beyond the 1m/10° range + +### 中文 + +在自动重定位模式下，算法根据当前传感器观测自动在地图中搜索设备位置。 + +**配置** (`config/control_command.yaml`): + +```yaml +register_keys: + custom_map_mode: 2 + relocalization_map_abs_path: "/path/to/your/map.bin" + # custom_init_pos 不设置或使用默认值 [0,0,0,0,0,0,1] +``` + +**推荐起始条件**： +- 距离原始 SLAM 轨迹上某点 **1 米**以内 +- 朝向与原始方向偏差在 **±10 度**以内 +- 位于地图中视觉特征明显的区域 + +**行为**： +1. 启动时，Odin 尝试将当前观测与地图匹配 +2. 如果成功，发布 `map` 和 `odom` 坐标系之间的 TF +3. 如果失败，系统进入后备 SLAM 模式（地图保存功能禁用） +4. 后台持续尝试重定位直到成功 + +**提示**： +- 启动后轻轻晃动或移动设备可以提高重定位精度 +- 在特征明显的场景中，可能在超出 1m/10° 范围时也能成功匹配 + +--- + +## Mode 2: Init Position Relocalization / 指定初始位置重定位 + +### English + +In init position relocalization mode, you provide an initial pose estimate to help the algorithm converge faster. + +**Configuration** (`config/control_command.yaml`): + +```yaml +register_keys: + custom_map_mode: 2 + relocalization_map_abs_path: "/path/to/your/map.bin" + custom_init_pos: [x, y, z, qx, qy, qz, qw] +``` + +**When to Use**: +- You know the approximate starting position (e.g., from external localization system) +- Starting position is far from the recommended 1m/10° range +- You need faster relocalization convergence +- Deploying in a fixed docking station with known pose + +**Behavior**: +1. Algorithm uses provided `init_pos` as initial pose estimate +2. Searches for matches in the vicinity of the provided position +3. Faster convergence compared to auto mode when estimate is accurate + +### 中文 + +在指定初始位置重定位模式下，您提供初始位姿估计以帮助算法更快收敛。 + +**配置** (`config/control_command.yaml`): + +```yaml +register_keys: + custom_map_mode: 2 + relocalization_map_abs_path: "/path/to/your/map.bin" + custom_init_pos: [x, y, z, qx, qy, qz, qw] +``` + +**适用场景**： +- 您知道大致的起始位置（例如，来自外部定位系统） +- 起始位置远离推荐的 1m/10° 范围 +- 需要更快的重定位收敛速度 +- 部署在已知位姿的固定充电桩 + +**行为**： +1. 算法使用提供的 `init_pos` 作为初始位姿估计 +2. 在提供位置的附近搜索匹配 +3. 当估计准确时，比自动模式收敛更快 + +--- + +## init_pos Format / init_pos 格式说明 + +### English + +`init_pos` is an array of **7 float values** representing position and orientation: + +```yaml +custom_init_pos: [x, y, z, qx, qy, qz, qw] +``` + +| Index | Parameter | Description | Unit | +|-------|-----------|-------------|------| +| 0 | x | X position in map frame | meters | +| 1 | y | Y position in map frame | meters | +| 2 | z | Z position in map frame | meters | +| 3 | qx | Quaternion X component | - | +| 4 | qy | Quaternion Y component | - | +| 5 | qz | Quaternion Z component | - | +| 6 | qw | Quaternion W component | - | + +**Important Notes**: +- The quaternion must be normalized: `sqrt(qx² + qy² + qz² + qw²) ≈ 1.0` +- Coordinates are relative to the **map frame** (world frame at SLAM start) +- Default value `[0, 0, 0, 0, 0, 0, 1]` represents origin with no rotation + +**Common Quaternion Values**: + +| Orientation | qx | qy | qz | qw | +|-------------|----|----|----|----| +| No rotation (identity) | 0 | 0 | 0 | 1 | +| 90° around Z-axis | 0 | 0 | 0.707 | 0.707 | +| 180° around Z-axis | 0 | 0 | 1 | 0 | +| -90° around Z-axis | 0 | 0 | -0.707 | 0.707 | + +### 中文 + +`init_pos` 是一个包含 **7 个 float 值**的数组，表示位置和朝向： + +```yaml +custom_init_pos: [x, y, z, qx, qy, qz, qw] +``` + +| 索引 | 参数 | 描述 | 单位 | +|------|------|------|------| +| 0 | x | 地图坐标系中的 X 位置 | 米 | +| 1 | y | 地图坐标系中的 Y 位置 | 米 | +| 2 | z | 地图坐标系中的 Z 位置 | 米 | +| 3 | qx | 四元数 X 分量 | - | +| 4 | qy | 四元数 Y 分量 | - | +| 5 | qz | 四元数 Z 分量 | - | +| 6 | qw | 四元数 W 分量 | - | + +**重要说明**： +- 四元数必须归一化：`sqrt(qx² + qy² + qz² + qw²) ≈ 1.0` +- 坐标相对于**地图坐标系**（SLAM 启动时的世界坐标系） +- 默认值 `[0, 0, 0, 0, 0, 0, 1]` 表示原点且无旋转 + +**常用四元数值**： + +| 朝向 | qx | qy | qz | qw | +|------|----|----|----|----| +| 无旋转（单位四元数） | 0 | 0 | 0 | 1 | +| 绕 Z 轴旋转 90° | 0 | 0 | 0.707 | 0.707 | +| 绕 Z 轴旋转 180° | 0 | 0 | 1 | 0 | +| 绕 Z 轴旋转 -90° | 0 | 0 | -0.707 | 0.707 | + +--- + +## Configuration Examples / 配置示例 + +### Example 1: Auto Relocalization / 自动重定位示例 + +```yaml +register_keys: + custom_map_mode: 2 + relocalization_map_abs_path: "/home/user/maps/office_map.bin" +``` + +### Example 2: Init Position at Origin / 在原点指定初始位置 + +```yaml +register_keys: + custom_map_mode: 2 + relocalization_map_abs_path: "/home/user/maps/office_map.bin" + custom_init_pos: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0] +``` + +### Example 3: Init Position with Offset / 带偏移的初始位置 + +Position at (5.2, -3.1, 0) with 90° rotation around Z-axis: + +位置在 (5.2, -3.1, 0)，绕 Z 轴旋转 90°： + +```yaml +register_keys: + custom_map_mode: 2 + relocalization_map_abs_path: "/home/user/maps/warehouse_map.bin" + custom_init_pos: [5.2, -3.1, 0.0, 0.0, 0.0, 0.707, 0.707] +``` + +### Example 4: Docking Station Pose / 充电桩位置 + +Known docking station at (10.5, 2.3, 0) facing -X direction (180° rotation): + +已知充电桩位置在 (10.5, 2.3, 0)，朝向 -X 方向（旋转 180°）： + +```yaml +register_keys: + custom_map_mode: 2 + relocalization_map_abs_path: "/home/user/maps/factory_map.bin" + custom_init_pos: [10.5, 2.3, 0.0, 0.0, 0.0, 1.0, 0.0] +``` + +--- + +## Programmatic API / 编程接口 + +### English + +You can also set `init_pos` programmatically using the `lidar_set_custom_parameter` API. This is useful for: +- Dynamic relocalization during runtime +- Integration with external localization systems +- Setting initial pose from robot's last known position + +#### API Function + +```cpp +#include "lidar_api.h" + +/** + * @brief Set a custom parameter on the device + * @param device Device handle obtained from lidar_open_device() + * @param param_name Parameter name (e.g., "init_pos") + * @param value_data Pointer to the parameter data + * @param value_length Size of the data in bytes + * @return 0 on success, -1 on error, -2 if file transfer in progress + */ +int lidar_set_custom_parameter(device_handle device, + const char* param_name, + const void* value_data, + size_t value_length); +``` + +#### Complete Example + +```cpp +#include "lidar_api.h" +#include +#include + +// Helper function to create quaternion from yaw angle (rotation around Z-axis) +void yaw_to_quaternion(float yaw_rad, float* qx, float* qy, float* qz, float* qw) { + *qx = 0.0f; + *qy = 0.0f; + *qz = sinf(yaw_rad / 2.0f); + *qw = cosf(yaw_rad / 2.0f); +} + +int set_init_position(device_handle device, + float x, float y, float z, + float qx, float qy, float qz, float qw) { + // init_pos format: [x, y, z, qx, qy, qz, qw] - 7 floats + float init_pos[7] = {x, y, z, qx, qy, qz, qw}; + + int result = lidar_set_custom_parameter( + device, + "init_pos", // Parameter name + init_pos, // Data pointer + sizeof(init_pos) // 7 * sizeof(float) = 28 bytes + ); + + if (result == 0) { + printf("Successfully set init_pos: [%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f]\n", + x, y, z, qx, qy, qz, qw); + } else { + printf("Failed to set init_pos, error code: %d\n", result); + } + + return result; +} + +// Usage examples: + +// Example 1: Set position at origin with no rotation +void example_origin(device_handle device) { + set_init_position(device, + 0.0f, 0.0f, 0.0f, // x, y, z + 0.0f, 0.0f, 0.0f, 1.0f // qx, qy, qz, qw (identity) + ); +} + +// Example 2: Set position with 90° yaw rotation +void example_with_rotation(device_handle device) { + float qx, qy, qz, qw; + float yaw_degrees = 90.0f; + float yaw_rad = yaw_degrees * M_PI / 180.0f; + + yaw_to_quaternion(yaw_rad, &qx, &qy, &qz, &qw); + + set_init_position(device, + 5.2f, -3.1f, 0.0f, // x, y, z + qx, qy, qz, qw // quaternion from yaw + ); +} + +// Example 3: Set position from external localization system +void example_from_external_localization(device_handle device, + double ext_x, double ext_y, double ext_yaw) { + float qx, qy, qz, qw; + yaw_to_quaternion((float)ext_yaw, &qx, &qy, &qz, &qw); + + set_init_position(device, + (float)ext_x, (float)ext_y, 0.0f, + qx, qy, qz, qw + ); +} +``` + +#### ROS Integration Example + +```cpp +#include "lidar_api.h" +#include // ROS1 +// or +#include // ROS2 + +// Callback for /initialpose topic (from RViz "2D Pose Estimate" tool) +void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg, + device_handle device) { + float init_pos[7] = { + (float)msg->pose.pose.position.x, + (float)msg->pose.pose.position.y, + (float)msg->pose.pose.position.z, + (float)msg->pose.pose.orientation.x, + (float)msg->pose.pose.orientation.y, + (float)msg->pose.pose.orientation.z, + (float)msg->pose.pose.orientation.w + }; + + int result = lidar_set_custom_parameter(device, "init_pos", init_pos, sizeof(init_pos)); + + if (result == 0) { + ROS_INFO("Set init_pos from RViz: [%.2f, %.2f, %.2f]", + init_pos[0], init_pos[1], init_pos[2]); + } else { + ROS_ERROR("Failed to set init_pos: %d", result); + } +} +``` + +#### Important Notes + +1. **Call timing**: Set `init_pos` **before** starting the stream with `lidar_start_stream()` +2. **Map mode**: Ensure `custom_map_mode` is set to `2` (relocalization mode) +3. **Map file**: The relocalization map must be set via `lidar_set_relocalization_map()` or YAML config +4. **Thread safety**: `lidar_set_custom_parameter` is thread-safe but blocks until response received + +### 中文 + +您也可以使用 `lidar_set_custom_parameter` API 以编程方式设置 `init_pos`。适用于： +- 运行时动态重定位 +- 与外部定位系统集成 +- 从机器人上次已知位置设置初始位姿 + +#### API 函数 + +```cpp +#include "lidar_api.h" + +/** + * @brief 在设备上设置自定义参数 + * @param device 从 lidar_open_device() 获取的设备句柄 + * @param param_name 参数名称（如 "init_pos"） + * @param value_data 指向参数数据的指针 + * @param value_length 数据大小（字节） + * @return 成功返回 0，错误返回 -1，文件传输中返回 -2 + */ +int lidar_set_custom_parameter(device_handle device, + const char* param_name, + const void* value_data, + size_t value_length); +``` + +#### 完整示例 + +```cpp +#include "lidar_api.h" +#include +#include + +// 辅助函数：从偏航角（绕 Z 轴旋转）创建四元数 +void yaw_to_quaternion(float yaw_rad, float* qx, float* qy, float* qz, float* qw) { + *qx = 0.0f; + *qy = 0.0f; + *qz = sinf(yaw_rad / 2.0f); + *qw = cosf(yaw_rad / 2.0f); +} + +int set_init_position(device_handle device, + float x, float y, float z, + float qx, float qy, float qz, float qw) { + // init_pos 格式: [x, y, z, qx, qy, qz, qw] - 7 个 float + float init_pos[7] = {x, y, z, qx, qy, qz, qw}; + + int result = lidar_set_custom_parameter( + device, + "init_pos", // 参数名 + init_pos, // 数据指针 + sizeof(init_pos) // 7 * sizeof(float) = 28 字节 + ); + + if (result == 0) { + printf("成功设置 init_pos: [%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f]\n", + x, y, z, qx, qy, qz, qw); + } else { + printf("设置 init_pos 失败，错误码: %d\n", result); + } + + return result; +} + +// 使用示例： + +// 示例 1：在原点设置位置，无旋转 +void example_origin(device_handle device) { + set_init_position(device, + 0.0f, 0.0f, 0.0f, // x, y, z + 0.0f, 0.0f, 0.0f, 1.0f // qx, qy, qz, qw（单位四元数） + ); +} + +// 示例 2：设置带 90° 偏航旋转的位置 +void example_with_rotation(device_handle device) { + float qx, qy, qz, qw; + float yaw_degrees = 90.0f; + float yaw_rad = yaw_degrees * M_PI / 180.0f; + + yaw_to_quaternion(yaw_rad, &qx, &qy, &qz, &qw); + + set_init_position(device, + 5.2f, -3.1f, 0.0f, // x, y, z + qx, qy, qz, qw // 从偏航角计算的四元数 + ); +} + +// 示例 3：从外部定位系统设置位置 +void example_from_external_localization(device_handle device, + double ext_x, double ext_y, double ext_yaw) { + float qx, qy, qz, qw; + yaw_to_quaternion((float)ext_yaw, &qx, &qy, &qz, &qw); + + set_init_position(device, + (float)ext_x, (float)ext_y, 0.0f, + qx, qy, qz, qw + ); +} +``` + +#### ROS 集成示例 + +```cpp +#include "lidar_api.h" +#include // ROS1 +// 或 +#include // ROS2 + +// /initialpose 话题的回调函数（来自 RViz 的 "2D Pose Estimate" 工具） +void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg, + device_handle device) { + float init_pos[7] = { + (float)msg->pose.pose.position.x, + (float)msg->pose.pose.position.y, + (float)msg->pose.pose.position.z, + (float)msg->pose.pose.orientation.x, + (float)msg->pose.pose.orientation.y, + (float)msg->pose.pose.orientation.z, + (float)msg->pose.pose.orientation.w + }; + + int result = lidar_set_custom_parameter(device, "init_pos", init_pos, sizeof(init_pos)); + + if (result == 0) { + ROS_INFO("从 RViz 设置 init_pos: [%.2f, %.2f, %.2f]", + init_pos[0], init_pos[1], init_pos[2]); + } else { + ROS_ERROR("设置 init_pos 失败: %d", result); + } +} +``` + +#### 重要说明 + +1. **调用时机**：在调用 `lidar_start_stream()` 启动数据流**之前**设置 `init_pos` +2. **地图模式**：确保 `custom_map_mode` 设置为 `2`（重定位模式） +3. **地图文件**：必须通过 `lidar_set_relocalization_map()` 或 YAML 配置设置重定位地图 +4. **线程安全**：`lidar_set_custom_parameter` 是线程安全的，但会阻塞直到收到响应 + +--- + +## Troubleshooting / 故障排除 + +### Relocalization Fails / 重定位失败 + +**English**: +- Ensure starting position is within recommended range (1m/10°) +- Check that the map file path is correct and file exists +- Verify the environment hasn't changed significantly since mapping +- Try gently moving the device to provide more observations + +**中文**： +- 确保起始位置在推荐范围内（1m/10°） +- 检查地图文件路径是否正确且文件存在 +- 验证环境自建图以来没有显著变化 +- 尝试轻轻移动设备以提供更多观测 + +### init_pos Not Taking Effect / init_pos 未生效 + +**English**: +- Verify `custom_map_mode` is set to `2` +- Check that `custom_init_pos` has exactly 7 values +- Ensure quaternion is normalized (sum of squares ≈ 1) +- Restart the driver after modifying configuration + +**中文**： +- 验证 `custom_map_mode` 设置为 `2` +- 检查 `custom_init_pos` 是否恰好有 7 个值 +- 确保四元数已归一化（平方和 ≈ 1） +- 修改配置后重启驱动程序 + +### TF Not Published / TF 未发布 + +**English**: +- Relocalization may still be in progress +- Check ROS logs for relocalization status messages +- System operates in fallback mode until relocalization succeeds + +**中文**： +- 重定位可能仍在进行中 +- 检查 ROS 日志中的重定位状态消息 +- 系统在重定位成功前以后备模式运行 + +### Map File Not Found / 地图文件未找到 + +**English**: +- Use absolute path (starting with `/`) +- Check file permissions +- Verify file extension is `.bin` + +**中文**： +- 使用绝对路径（以 `/` 开头） +- 检查文件权限 +- 验证文件扩展名为 `.bin` + +--- + +## Related Topics / 相关话题 + +| Topic | Description | +|-------|-------------| +| `/odin1/odometry` | Odometry in odom frame | +| `/odin1/odometry_highfreq` | High-frequency odometry | +| `/odin1/cloud_slam` | SLAM point cloud in odom frame | +| `/tf` | Transform tree (includes map→odom after successful relocalization) | + +--- + +## See Also / 参见 + +- [README.md](README.md) - Main documentation +- [config/control_command.yaml](config/control_command.yaml) - Configuration file + diff --git a/readme.md b/readme.md new file mode 100644 index 0000000..1fb0673 --- /dev/null +++ b/readme.md @@ -0,0 +1,33 @@ +# RC_WheelLeg / Odin1 ROS 工作区说明 + +本目录是一个 ROS 工作区归档，当前根目录为 `D:\桌面\catkin_ws`，主要内容位于子目录 `catkin_ws`。 + +## 目录内容 + +```text +D:\桌面\catkin_ws +└── catkin_ws + ├── README.md + ├── odin1的使用手册.md + ├── project.log + ├── runros.sh + └── src + └── odin_ros_driver + ├── .gitignore + └── RELOCALIZATION_GUIDE.md +``` + +## 文件说明 + +- `catkin_ws/README.md`：原工作区说明文档，描述 Odin1 与 ROS 工作区的基本使用方式。 +- `catkin_ws/odin1的使用手册.md`：Odin1 使用手册。 +- `catkin_ws/project.log`：项目日志文件。 +- `catkin_ws/runros.sh`：ROS 环境构建/运行相关脚本。 +- `catkin_ws/src/odin_ros_driver/`：Odin ROS 驱动目录。该目录内部当前保留了独立 Git 仓库元数据，外层仓库仅记录其中当前可见的普通文件。 +- `catkin_ws/src/odin_ros_driver/RELOCALIZATION_GUIDE.md`：重定位相关说明文档。 + +## 注意事项 + +- `catkin_ws/src/odin_ros_driver` 本身是一个独立 Git 仓库；在外层仓库提交时，应避免把其内部 `.git` 目录作为外层仓库内容提交。 +- 当前归档按磁盘现有内容整理，未包含 ROS 构建产物目录，例如 `build/`、`install/`、`log/`。 +- 如需完整恢复 `odin_ros_driver` 的驱动源码，应以其内部 Git 仓库或官方远程仓库状态为准。