AllocNet 四旋翼避障轨迹规划与键盘遥操作

const double zGoal = mapBound[4] + dilateRadius +
                     fabs(msg->pose.orientation.z) *
                         (mapBound[5] - mapBound[4] - 2 * dilateRadius);
const Eigen::Vector3d goal(msg->pose.position.x, msg->pose.position.y, zGoal);

z_goal = 0.2 + |orientation.z| × (5 − 0.4) = 0.2 + 4.6 × ratio

span  = map_z_size - 2 * dilate              # 4.6
ratio = (z - map_z_origin - dilate) / span   # 反解出归一化比例
msg.pose.orientation.z = ratio

  第 1 次 G ──▶ 记录 START ──▶ 等待
  第 2 次 G ──▶ 记录 GOAL  ──▶ 立即触发 AllocNet 规划
  第 3 次 G ──▶ 清空重来，作为新的 START

  阶段: 已设起点 #1，等待第 2 次 G 触发规划

/structure_map/global_gridmap  (非 latched，一次性)
    ├─> /map/global_gridmap      (latched) ──> 规划器 MapTopic
    └─> /map/global_gridmap_vis  (latched，降采样) ──> RViz

sudo apt update
sudo apt install -y \
    libompl-dev libeigen3-dev \
    libsdl1.2-dev libsdl-image1.2-dev \
    libboost-all-dev cmake build-essential git wget unzip \
    python3-pip python3-rosdep

# osqp —— 版本必须是 release-0.6.3，新版 API 不兼容
cd /tmp && git clone -b release-0.6.3 --depth 1 https://github.com/osqp/osqp.git
cd osqp && git submodule update --init --recursive
mkdir -p build && cd build
cmake .. -DCMAKE_BUILD_TYPE=Release && make -j$(nproc) && sudo make install
sudo ldconfig

# osqp-eigen —— 规划器的 QP 封装 osqp_solver.hpp 依赖它
cd /tmp && git clone --depth 1 https://github.com/robotology/osqp-eigen.git
cd osqp-eigen && mkdir -p build && cd build
cmake .. -DCMAKE_BUILD_TYPE=Release && make -j$(nproc) && sudo make install
sudo ldconfig

cd /tmp
wget -O libtorch.zip \
  https://download.pytorch.org/libtorch/nightly/cpu/libtorch-cxx11-abi-shared-with-deps-2.0.0.dev20230301%2Bcpu.zip
unzip -q libtorch.zip -d /tmp/lt
mkdir -p ~/allocnet_ws/src/AllocNet/src/planner
mv /tmp/lt/libtorch ~/allocnet_ws/src/AllocNet/src/planner/libtorch

mkdir -p ~/allocnet_ws/src && cd ~/allocnet_ws/src

# (1) AllocNet —— 提供 planner 包（本模块对应的分支）
git clone -b feature-keyboard-teleop \
    https://github.com/Xiangyuetang91/AllocNet.git

# (2) kr_param_map —— 提供 param_env 包（★ 不在 AllocNet 仓库内）
git clone --depth 1 https://github.com/KumarRobotics/kr_param_map.git

cd ~/allocnet_ws
catkin_make -DCMAKE_BUILD_TYPE=Release -D_GLIBCXX_USE_CXX11_ABI=1

ls devel/lib/planner/learning_planning   # 规划器
ls devel/lib/param_env/structure_map     # 地图生成

source ~/allocnet_ws/devel/setup.bash

# 1. 把本模块装入工作区的 planner 包
cd ros2/src/air/allocnet_teleop
python3 main.py install --ws ~/allocnet_ws

# 2. 重新编译（脚本需注册进 CMakeLists）
cd ~/allocnet_ws && catkin_make --pkg planner

# 3. 环境自检
python3 main.py check

[1] ROS Noetic: 已安装
[2] AllocNet 工作区 /home/user/allocnet_ws: 已构建
[3] 上游 ROS 包:
     [OK] planner      /home/user/allocnet_ws/src/AllocNet/src/planner
     [OK] param_env    /home/user/allocnet_ws/src/kr_param_map/param_env

     [3b] 包编译产物:
     [OK] planner/learning_planning
     [OK] param_env/structure_map
[4] 本模块脚本:  teleop_keyboard.py / map_republisher.py / record_trajectory.py
[5] launch 文件:  teleop_planning.launch
[6] 可选工具:     rviz / import / Xvfb
 自检结果: 通过，可以运行 `main.py run`

cd src/air/allocnet_teleop
python3 test_teleop_keyboard.py

Ran 18 tests in 0.010s

OK

# 含 RViz
python3 main.py run

# 无图形界面（服务器 / 虚屏）
python3 main.py run --no-gui

# 不记录轨迹
python3 main.py run --no-record

# 用高对比配置启动（点云灰色，轨迹更醒目，适合截图）
python3 main.py run --rviz-config capture_view.rviz

source ~/allocnet_ws/devel/setup.bash
rosrun planner teleop_keyboard.py

# 移动游标 + 下发航点（G = 发送）
rostopic pub -1 /teleop/key std_msgs/String "data: 'wwwd'"
rostopic pub -1 /teleop/key std_msgs/String "data: 'g'"

# 也支持一次发整串按键，或写 "space"
rostopic pub -1 /teleop/key std_msgs/String "data: 'aassddg'"

# 记录节点随 launch 自动启动；也可单独指定输出
rosrun planner record_trajectory.py _output:=$HOME/run1.csv

# 生成图表（plot_trajectory.py 随本模块 install 到工作区，无需额外下载）
python3 src/planner/scripts/plot_trajectory.py --csv $HOME/run1.csv --out-dir docs/assets

sudo apt install -y xvfb imagemagick
Xvfb :99 -screen 0 1280x800x24 -ac +extension GLX +render -noreset &
export DISPLAY=:99
export LIBGL_ALWAYS_SOFTWARE=1
rviz -d $(find planner)/config/capture_view.rviz &
import -display :99 -window root /tmp/f.png

RLException: multiple files named [xxx.launch] in package [planner]