# ws_618

**Repository Path**: wty-yy/ws_618

## Basic Information

- **Project Name**: ws_618
- **Description**: 618项目上位机代码，建图，定位，导航
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 1
- **Created**: 2025-05-30
- **Last Updated**: 2025-11-13

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

此仓库用于乐聚Kuavo第四代机器人的上位机，机器人版本为45（长臂），代码架构如下

![Framework](./assets/leju_618_framework.drawio.png)

# 目录
- [安装](#安装)
  - [Livox-SDK2](#livox-sdk2)
  - [livox_ros_driver2 + FastLIO2](#livox_ros_driver2--fastlio2)
  - [ros1_bridge](#ros1_bridge)
  - [Conda环境](#conda环境)
  - [LightGlue-ONNX](#lightglue-onnx)
  - [仓库更新后编译命令](#仓库更新后编译命令)
- [启动](#启动)
  - [方便快捷的可视化UI界面](#方便快捷的可视化ui界面)
  - [建图](#建图)
    - [FAST LIO2 3D建图](#fast-lio2-3d建图)
    - [2D建图](#2d建图)
    - [一个PCL点云数据读取的DEMO](#一个pcl点云数据读取的demo)
  - [导航](#导航)
    - [启动ros1_bridge](#启动ros1_bridge)
    - [一个/cmd_vel控制移动的DEMO](#一个cmd_vel控制移动的demo)
    - [完整导航功能](#完整导航功能)
- [可能的问题](#可能的问题)
  - [编译报错](#编译报错)

# 安装
在Ubuntu设置当中配置主机静态IP为`192.168.1.50`
## Livox-SDK2
```bash
cd pkgs/Livox-SDK2
mkdir build && cd build
cmake .. 
make -j${nproc}
sudo make install
```
修改[mid360_config.json](pkgs/Livox-SDK2/samples/livox_lidar_quick_start/mid360_config.json)中的
```bash
"host_ip"        : "192.168.1.50",
```
测试是否安装成功
```bash
cd pkgs/Livox-SDK2/build/samples/livox_lidar_quick_start
./livox_lidar_quick_start ../../../samples/livox_lidar_quick_start/mid360_config.json
```
出现`point cloud handle:...`一直滚动的数据就说明成功了

## livox_ros_driver2 + FastLIO2
```bash
cd /ws_618
# 自动安装依赖包 (可能无法连上服务器)
sudo apt update
sudo apt install python3-rosdep
rosdep install --from-paths src --ignore-src -y

# 或者手动安装依赖包
sudo apt install ros-humble-pcl-ros

source /opt/ros/humble/setup.sh
./src/livox_ros_driver2/build.sh humble
```

修改[MID360_config.json](src/livox_ros_driver2/config/MID360_config.json)中主机ip(MID360['host_net_info']['cmd_data_ip'])和雷达ip(lidar_configs[0]['ip'])，其中雷达ip192.168.1.1xx中的xx为雷达S/N码最后两位（可以在雷达包装盒或雷达侧面二维码下方找到，或者通过wireshark），可以通过ping 192.168.1.1xx判断ip是否正确
```bash
vim src/livox_ros_driver2/config/MID360_config.json

# 主机ip
"cmd_data_ip" : "192.168.1.50",

# 雷达ip
"ip" : "192.168.1.1xx",
```

上述配置完成后启动测试文件，从rviz2中能看到实时的雷达图像即可：
```bash
source install/setup.sh
ros2 launch livox_ros_driver2 rviz_MID360_launch.py
```

## nav2
```bash
sudo apt install ros-humble-navigation2
sudo apt install ros-humble-nav2-bringup
```

## ros1_bridge
由于`source`带有ros1_bridge的项目时，会同时将ROS1, ROS2添加到环境变量，无法启动rviz2，因此单独作为一个项目放在[wty-yy/ws_618_ros1_bridge](https://gitee.com/wty-yy/ws_618_ros1_bridge)，安装方法如下
```bash
cd ~
git clone https://gitee.com/wty-yy/ws_618_ros1_bridge.git ros1_bridge_ws
cd ~/ros1_bridge_ws

source /opt/ros/noetic/setup.sh
source /opt/ros/humble/setup.sh
# 第一次编译可能需要16分钟左右
colcon build --symlink-install --packages-select ros1_bridge --cmake-force-configure
```

## Conda环境
> 参考[MyBlog - ROS2 Conda安装](https://wty-yy.xyz/posts/30945/#conda%E5%AE%89%E8%A3%85)
```bash
# 必须为python 3.10
conda create -n ros2 python=3.10  # 安装的python版本一定要和ros2默认的版本一致
conda activate ros2
conda install colcon-common-extensions  # 仅需安装

# 一些默认会安装的包
conda install numpy lark matplotlib opencv pyqt
# onnxruntime, intel的cpu支持openvino推理加速
pip install onnxruntime-openvino
# lightglue-onnx还需包
pip install tqdm typer strenum
# yolo所需包
pip install ultralytics
# torch安装参考官网, 这只举个例子 https://pytorch.org/get-started/locally/
pip install torch torchvision --index-url https://download.pytorch.org/whl/cu126
```
**后续编译一定先进环境后编译**

## LightGlue-ONNX
下载[superpoint_lightglue_pipeline.onnx](https://github.com/fabio-sim/LightGlue-ONNX/releases/download/v2.0/superpoint_lightglue_pipeline.onnx)(48.81Mb)文件并放到[pkgs/LightGlue-ONNX/weights](pkgs/LightGlue-ONNX/weights)文件夹下

进入`ros2`的conda环境，测试是否安装成功
```bash
conda activate ros2
cd pkgs/LightGlue-ONNX
# 测试安装是否成功 (如果使用AGX的tensorrt一定要先运行一次tensorrt的推理, 预先编译)
python3 dynamo.py infer \
  weights/superpoint_lightglue_pipeline.onnx \
  assets/sacre_coeur1.jpg assets/sacre_coeur2.jpg \
  superpoint \
  -h 512 -w 512 \
  --profile \
  -d cpu  # 可选: cpu, openvino, cuda, tensorrt
```
Jetson AGX Orin (MAXN) 速度测试:
| CPU | CUDA | TensorRT |
| - | - | - |
| 1.8s/it | 11.5it/s | 22.4it/s |

测试完成后, 选择速度最快的方式, 例如agx就是用`cuda`(`tensorrt`有时候无法启动还在调试), 修改[nav.launch.py#73](src/navigation/launch/nav.launch.py#73)的73为`default_value='cuda'`

## 仓库更新后编译命令
```bash
# 进环境
conda activate ros2
# 编译全部包
colcon build --symlink-install
# 或者指定更新的pkg名称
colcon build --symlink-install --packages-select <pkg_name>
```

# 启动
```bash
# 下述命令执行前都请先source本项目
source install/setup.sh
```


## 方便快捷的可视化UI界面
包含下述的建图、导航功能，能自动启动ros2命令，方便快捷
```bash
# 启动ros1_bridge, 参考wty-yy/ws_618_ros1_bridge的README, https://gitee.com/wty-yy/ws_618_ros1_bridge
# 下位机, 加载刚刚拷贝的yaml
rosparam load bridge.yaml

# 上位机, 在项目根目录下, 启动转发
source install/setup.zsh
ros2 run ros1_bridge parameter_bridge  # 指定转发的topic

# 启动GUI
ros2 run gui nav_gui.py

ros2 run manipulation mani_core.py
```
![GUI](./assets/gui.png)


## 建图
### FAST LIO2 3D建图
```bash
ros2 launch livox_ros_driver2 msg_MID360_launch.py  # 启动MID360通讯节点
ros2 launch fast_lio mapping.launch.py config_file:=mid360.yaml  # 启动建图
# 启用yaml中的pcd_save_en: true, path_save_en: true来保存地图和轨迹

# 一键启动建图+MID360脚本
ros2 launch map_manager realtime_fast_lio2.launch.py
```
[保存生成的map](https://wty-yy.github.io/posts/43386/#%E4%BF%9D%E5%AD%98map)

### 2D建图
完成上述3D建图后，将`fast lio2`建图[`save_data/`](src/FAST_LIO/save_data)下的`scans.pcd`, `path.csv`文件移动到[`maps/`](src/map_manager/maps)文件夹下，后续的2D PNG平面图生成同样生成在[`maps/`](src/map_manager/maps)文件夹下`2d_map_**_**.png`（其中`**`分别表示x最小值和y最小值`*100`的结果，用于定位2D图的原点位置）
```bash
# 注意: 所有没有以`/`开头的路径均为相对`ws_618/src/map_manager/`的相对路径
# launch文件自动启动rviz2并执行build_2d_map程序
ros2 launch map_manager build_2d_map.launch.py

# 支持自定义参数包含
ros2 launch map_manager build_2d_map.launch.py \
    pcl_file_path:=/path/to/your/file.pcd \
    path_file_path:=/path/to/your/file.csv \
    min_pz_gain:=0.0 max_pz_gain:=0.5 \
    grid_value_threshold:=90.0

# 读取PNG中的图片并可视化
ros2 launch map_manager build_2d_map.launch.py \
    flag_build_2d_map:=false \
    map_file_path:=maps/2d_map_-612_-582.png
```

### 一个PCL点云数据读取的DEMO
```bash
# 利用轨迹对地图z轴进行裁剪
ros2 run map_manager demo_load_pcl --ros-args -p pcl_file_path:=/ws_618/src/map_manager/maps/scans_0.pcd \
    -p path_file_path:=/ws_618/src/map_manager/maps/path_0.csv \
    -p min_pz_gain:=0.3 -p max_pz_gain:=1.8
```

## 导航
### 启动ros1_bridge
由于所有的电机控制都在下位机的ROS1上，因此需要启动`ros1_bridge`来连接ROS1和上位机的ROS2，需先对`ROS_IP, ROS_MASTER_URI`两个全局变量进行配置确保上下位机的ROS1可以多机通讯（可参考[MyBlog/ROS1的多机通讯](https://wty-yy.xyz/posts/62745/#ros1%E7%9A%84%E5%A4%9A%E6%9C%BA%E9%80%9A%E8%AE%AF)）

首先将[bridge.yaml](src/ros1_bridge/config/bridge.yaml)从上位机拷贝到下位机上，或者直接用`wget`下载文件
```bash
# 下位机（每次roscore重启后都需要重新加载一次配置）
cd ~/Downloads
wget https://gitee.com/wty-yy/ws_618_ros1_bridge/raw/master/src/ros1_bridge/config/bridge.yaml  # 直接下载
rosparam load bridge.yaml  # 加载配置

# 上位机（本机）
source ~/ros1_bridge_ws/install/setup.zsh
ros2 run ros1_bridge parameter_bridge  # 启动桥接

# 下位机（启动机器人电机）
sudo su  # 进入管理员
cd /home/lab/kuavo-ros-opensource-xjtu
roslaunch humanoid_controllers load_kuavo_real.launch cali:=true cali_arm:=true joystick_type:=bt2pro 
```
> 下位机控制代码为[wty-yy/kuavo-ros-opensource-xjtu](https://gitee.com/wty-yy/kuavo-ros-opensource-xjtu)，基于官方的1.1.6修改

### 一个/cmd_vel控制移动的DEMO
```bash
ros2 run navigation demo_twist
```

### 完整导航功能
需要基于建图得到的地图文件
```bash
# device为lightglue推理的设备 (用于2d图校准), 可选: cpu, cuda, openvino, tensorrt
ros2 launch navigation nav.launch.py map_file_path:=maps/your/map_name.png device:=cpu
```

# 可能的问题
## 编译报错
```bash
# 报错: ModuleNotFoundError: No module named 'ament_package'
sudo apt install python3-ament-package

# 报错: No 'rosidl_typesupport_c' found
sudo apt install ros-humble-rosidl-typesupport-c
```

# 完成项目
2025.10月加入新功能
- [x] 使用1.2.2支持上肢零点标定, 优化抓取, 交换手动作, 成功率达到100%, 支持抓取完成后移动到左/右放置物体
- [x] 导航到抓取工作台时，还需根据apriltag进行对齐矫正机器人位置
- [x] 抓取完毕后，由于避障策略的问题，机器人无法离开工作台，需要修正避障算法逻辑，当前方出现障碍需要给一个向后的短距离导航点（或向后移动一点），不要直接停止控制