# 底盘选点模块

**Repository Path**: jiang-jinhao998/base-point-selection-module

## Basic Information

- **Project Name**: 底盘选点模块
- **Description**: No description available
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 0
- **Created**: 2026-01-19
- **Last Updated**: 2026-03-01

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# Base Point Selection Module (ROS 2 Foxy)

这是一个专为移动机器人设计的机械臂底座位置优化模块，主要用于解决“在给定目标位姿下，机器人底座应该停在哪里，才能保证机械臂不仅能抓到目标，且避开障碍物”的问题。

## 主要功能

1.  **Reach Space Modeling (可达空间建模)**：
    *   通过对机械臂工作空间进行采样，建立机械臂工作包络的数学模型。
    *   **优化更新**：拟合出的内外椭球参数（12 个参数）会自动持久化保存至 `config/ellipsoid_params.json`，无需重复采样。
2.  **Base Optimization (底座位置优化)**：
    *   利用 PSO (粒子群算法) 在地图中搜索最优的机器人停靠位姿。
    *   **优化更新**：
        *   直接从本地数据文件加载椭球参数，解除了对实时采样服务的依赖。
        *   **高性能点云处理**：弃用 `ros2_numpy`，采用 `sensor_msgs_py` 官方解析库。
        *   **空间裁剪 (CropBox)**：自动裁剪目标点周围的局部障碍物信息，大幅提升优化计算效率。

## 快速开始

### 1. 安装依赖

#### 传统 Pip 方式
```bash
pip install -r requirements_py38.txt -i https://pypi.tuna.tsinghua.edu.cn/simple
```

#### UV 方式 (推荐：极速且隔离)
如果你想尝试更现代、更快速的 Python 环境管理工具 `uv`：
```bash
# 1. 创建虚拟环境 (建议使用系统 site-packages 以兼容 ROS 2)
# uv 本身崇尚隔离，但在 ROS 2 下，我们通常需要访问系统安装的 rclpy 等
uv venv --python 3.8 --system-site-packages
source .venv/bin/activate

# 2. 同步安装第三方依赖
uv pip install -r requirements_py38.txt -i https://pypi.tuna.tsinghua.edu.cn/simple

# 3. 安装 KDL 依赖 (需要编译 KDL Parser)
git clone https://github.com/jvytee/kdl_parser.git
uv pip install kdl_parser/
rm -rf kdl_parser/
```

> [!IMPORTANT]
> **关于系统依赖的特殊说明**：
> 部分依赖项无法通过 `uv` 或 `pip` 完美安装，必须通过系统包管理器 (`apt`) 安装：
> 1.  **ROS 2 核心库**：`rclpy`, `sensor_msgs_py`, `tf2_ros` 等由 ROS 2 发行版提供。
> 2.  **图形界面**：`tkinter` 需要 `sudo apt install python3-tk`。
> 3.  **KDL 基础**：`PyKDL` 虽然在 requirements 中占位，但通常建议使用 `sudo apt install python3-orocos-kdl`。
> 
> **最优实践**：在激活虚拟环境后，确保你已经 `source /opt/ros/foxy/setup.bash`，以便 Python 能找到系统路径下的 ROS 2 包。

### 2. 构建项目

在 ROS 2 工作空间根目录下执行：
```bash
colcon build --symlink-install --cmake-args -DBUILD_TESTING=OFF
source install/setup.bash
```

## 使用说明

### 第一步：生成机械臂可达空间模型 (仅需运行一次)

现在支持通过 Launch 文件一键启动采样与拟合流程，无需手动开启 GUI。

#### 方案 A：参数化全自动模式 (推荐)
1.  **修改参数**：打开 `src/reach_space_modeling/config/reachability_params.yaml`，填入机械臂的 URDF 路径和关节名称。
2.  **启动建模**：
    ```bash
    ros2 launch reach_space_modeling modeling.launch.py
    ```
    该指令会自动完成采样、拟合并将结果保存至 `ellipsoid_params.json`。可视化图片将保存在 `install/share/reach_space_modeling/visualizations/` 下。

#### 方案 B：交互式 GUI 模式
如果你不确定关节名称，可以留空 YAML 参数并直接运行：
```bash
ros2 run reach_space_modeling find_ellips_eq_reach_opt
```
程序将自动打开 **GUI 界面**，允许你手动浏览文件并下拉选择关节。

### 第二步：启动底座位姿优化节点
```bash
ros2 run base_optimization find_opt_pose
```

### （可选）ZMQ 同步接口
如果需要通过 ZMQ 同步阻塞等待 Nav2 完成，可运行：
```bash
ros2 run base_optimization base_align_zmq_server
```
默认绑定地址为 `tcp://0.0.0.0:5568`，可通过环境变量 `BASE_ALIGN_ZMQ_BIND` 覆盖。

### 第三步：触发优化
向 `/des_EE_pose` 话题发布目标位姿即可：
```bash
ros2 topic pub --once /des_EE_pose geometry_msgs/msg/PoseStamped "{header: {frame_id: 'map'}, pose: {position: {x: 1.0, y: 1.0, z: 0.5}, orientation: {w: 1.0}}}"
```

## 工程优势
*   **低耦合**：参数持久化确保了模块可以独立调试和运行。
*   **高效率**：CropBox 过滤和按需解析技术保证了即便在复杂地图下也能秒级响应。
*   **零冗余依赖**：优先使用官方 ROS 2 库进行底层数据交换。