arm_dev
c1cd862eea
这批是 09-24 那轮仿真工作的收尾，一直没提交。内容按目的分四块： 1) 仿真真能「夹住」工件 sim_grasp_lock：给规与夹爪之间加接触锁（默认关，run_gauge_assembly --sim 才开）。 没有它时仿真里规只靠摩擦，S08 棘轮一转就滑出指间，于是把「仿真不真」误读成 「力判据不对」。新增 test_sim_grasp_lock.py。 2) 腕部力传感器进仿真 sim_wrist_ft（+ config/wrist_ft.yaml）：S07 的首选判据是腕部 FT 的轴向突变， 而仿真里以前根本没有腕部力，S07 只能退到触觉兜底，两套判据从没在同一场景下比过。 现在两条通道都在，接触判定可对照。 3) 真值位姿可查（不再靠猜） sim_live_pose / sim_pose_service：把 Gazebo 里刚体的实测位姿发布出来， stage2 的真值路径按**实测**位姿算孔位，而不是按场景文件里的标称位姿。 顺带把真值孔位的推导（truth_holes）独立出来并补测。 4) 工具几何成为可配置项 config/sim_tool_geometry.yaml + stage2 的 tool_tip_below_flange_m 启动参数： 仿真里规会在指间下滑，工具长每次装夹都不同（实测见过 243/254mm）， 悬停高度与 S07 下探预算必须按实测值算，不能再写死。 应用层同时跟进：gauge_fsm / gauge_config 把「悬停时工具尖端入没入工件」的判据 （hover_z_guard）与拖件判定（drag_verdict）收成纯函数并补测；stage3 搜孔、 site 导出、run_gauge_assembly 与若干文档同步。 新增 test_gauge_exit_lift / test_gauge_gripper_client / test_node_handler_collisions。 测试：全仓库 1085 passed。
153 lines
8.3 KiB
Bash
153 lines
8.3 KiB
Bash
#!/usr/bin/env bash
|
||
# =============================================================================
|
||
# 激活**仿真**环境（Gazebo Fortress + 本工作区的仿真构建产物）。
|
||
#
|
||
# 用法: source scripts/sim_env.sh
|
||
#
|
||
# 与 scripts/env.sh（真机环境）的关系：
|
||
# 两者互斥，各自用一套 conda 环境 + 一套构建产物。
|
||
# 真机: ros_flexiv → build/ install/
|
||
# 仿真: ros_gz_sim → build_sim/ install_sim/
|
||
# 切换时**最好开新的 shell**。同一 shell 里来回切会残留 PATH/PYTHONPATH，
|
||
# 症状详见 scripts/lib/conda_switch.sh 顶部的说明（实测踩过）。
|
||
# 这里会先退到 base 再切以尽量规避，但开新 shell 才是稳妥做法。
|
||
#
|
||
# 为什么仿真用独立的 build/install 目录：
|
||
# 工作区只有一份源码。如果仿真也往 install/ 里写，就会把真机环境的构建
|
||
# 产物覆盖掉（两边的依赖路径不同）。分目录后互不干扰。
|
||
# =============================================================================
|
||
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
||
|
||
SIM_ENV_NAME="${SIM_ENV_NAME:-ros_gz_sim}"
|
||
REAL_ENV_NAME="${REAL_ENV_NAME:-ros_flexiv}"
|
||
CONDA_SH="${HOME}/anaconda3/etc/profile.d/conda.sh"
|
||
|
||
# shellcheck disable=SC1091
|
||
source "${ROOT}/scripts/lib/conda_switch.sh"
|
||
|
||
if [[ ! -f "${CONDA_SH}" ]]; then
|
||
echo "找不到 conda: ${CONDA_SH}" >&2
|
||
return 1 2>/dev/null || exit 1
|
||
fi
|
||
|
||
# shellcheck disable=SC1090
|
||
source "${CONDA_SH}"
|
||
|
||
# 关掉 `set -u`：ROS 的 activate.d 脚本会引用未绑定的 CONDA_BUILD，
|
||
# 调用方常带 set -u，那样会直接把 source 打断，表现为"脚本毫无输出就退出"。
|
||
_sim_had_u=0
|
||
case "$-" in *u*) _sim_had_u=1; set +u ;; esac
|
||
|
||
if ! conda_switch_clean "${SIM_ENV_NAME}" "${REAL_ENV_NAME}"; then
|
||
[[ "${_sim_had_u}" -eq 1 ]] && set -u
|
||
unset _sim_had_u
|
||
echo "无法激活 conda 环境 ${SIM_ENV_NAME}。" >&2
|
||
echo "先创建: bash ${ROOT}/scripts/install_sim_env.sh" >&2
|
||
return 1 2>/dev/null || exit 1
|
||
fi
|
||
|
||
# 仿真构建产物（由 scripts/build_sim.sh 用 --install-base 生成，与真机隔离）
|
||
if [[ -f "${ROOT}/install_sim/setup.bash" ]]; then
|
||
# shellcheck disable=SC1091
|
||
source "${ROOT}/install_sim/setup.bash"
|
||
else
|
||
echo "提示: install_sim/ 还不存在，先跑 bash scripts/build_sim.sh" >&2
|
||
fi
|
||
|
||
# --- 清掉真机环境留下的路径残留 ---------------------------------------------
|
||
# 同一个 shell 里先 source 过 scripts/env.sh（真机）再 source 本文件时，install/
|
||
# 与 ros_flexiv 的路径不会被 conda deactivate 撤掉，会长在 install_sim/ 前面，
|
||
# 导致 import 到真机那份包、dlopen 到真机那份库。详见 conda_switch.sh 里
|
||
# path_drop_patterns 的注释（那里记了 sim_tactile_adapter 崩溃的实测症状）。
|
||
path_drop_patterns PYTHONPATH "${ROOT}/install/*" "${ROOT}/build/*" "*/envs/${REAL_ENV_NAME}/*"
|
||
path_drop_patterns LD_LIBRARY_PATH "${ROOT}/install/*" "*/envs/${REAL_ENV_NAME}/*"
|
||
path_drop_patterns PATH "${ROOT}/install/*" "*/envs/${REAL_ENV_NAME}/*"
|
||
path_drop_patterns AMENT_PREFIX_PATH "${ROOT}/install" "*/envs/${REAL_ENV_NAME}"
|
||
path_drop_patterns CMAKE_PREFIX_PATH "${ROOT}/install" "*/envs/${REAL_ENV_NAME}"
|
||
|
||
# --- 补上 conda 环境自己的 lib ----------------------------------------------
|
||
# conda 激活**不会**把 $CONDA_PREFIX/lib 放进 LD_LIBRARY_PATH（见下面 GZ 插件
|
||
# 路径那段长注释）。对 gz 我们靠 GZ_SIM_SYSTEM_PLUGIN_PATH 兜住了，但
|
||
# rosidl 的 typesupport 是靠 dlopen("lib<包名>__rosidl_typesupport_*.so") 找的，
|
||
# 只认 ld.so 的搜索路径。于是 conda 里带的（不在本工作区的）消息包会在
|
||
# create_publisher/create_subscription 时抛
|
||
# Type support not from this implementation ... cannot open shared object file
|
||
# 追加在**末尾**（优先级最低），避免压过系统/工作区里已有的同名库。
|
||
if [[ -n "${CONDA_PREFIX:-}" && -d "${CONDA_PREFIX}/lib" ]]; then
|
||
case ":${LD_LIBRARY_PATH:-}:" in
|
||
*":${CONDA_PREFIX}/lib:"*) ;;
|
||
*) export LD_LIBRARY_PATH="${LD_LIBRARY_PATH:+${LD_LIBRARY_PATH}:}${CONDA_PREFIX}/lib" ;;
|
||
esac
|
||
fi
|
||
|
||
[[ "${_sim_had_u}" -eq 1 ]] && set -u
|
||
unset _sim_had_u
|
||
|
||
export YOLO_AUTOINSTALL=false
|
||
export ROS2_WS="${ROOT}"
|
||
# 仿真单独一个 DDS domain：避免与正在跑的真机节点互相抢话题/污染 TF。
|
||
# 真机用的是默认 domain，两者天然隔离。
|
||
#
|
||
# ⚠️ 这里**强制** 77，不是 `${ROS_DOMAIN_ID:-77}`（2026-09-24 修的）。
|
||
# 写法上"尊重已有值"看起来更礼貌，实际会把隔离**交给运气**：实测本机 shell 里
|
||
# 就 export 着 ROS_DOMAIN_ID=0（真机默认），于是 `${ROS_DOMAIN_ID:-77}` 当场取到 0，
|
||
# 仿真栈和真机落在**同一个 domain** 上 —— 仿真的运动指令可能被真机控制器订阅到。
|
||
# 这是最坏的一类错误（动真机），而现场只会看到"脚本打印了 77、实际是 0"。
|
||
# 要换 domain 请显式设 SIM_ROS_DOMAIN_ID（名字里带 SIM，不会被真机环境的 export 撞上）。
|
||
if [[ -n "${SIM_ROS_DOMAIN_ID:-}" ]]; then
|
||
export ROS_DOMAIN_ID="${SIM_ROS_DOMAIN_ID}"
|
||
elif [[ "${ROS_DOMAIN_ID:-77}" != "77" ]]; then
|
||
echo "[sim_env] ⚠ 环境里已有 ROS_DOMAIN_ID=${ROS_DOMAIN_ID}（真机通常用 0）——" >&2
|
||
echo "[sim_env] 已**强制**改成 77：隔离不能靠「环境里恰好没设」。要另指定请设 SIM_ROS_DOMAIN_ID。" >&2
|
||
export ROS_DOMAIN_ID=77
|
||
else
|
||
export ROS_DOMAIN_ID=77
|
||
fi
|
||
|
||
# =============================================================================
|
||
# Gazebo 的插件与资源搜索路径 —— 必须在这里设，不能只在 launch 里设
|
||
# =============================================================================
|
||
# ros_gz_sim 的 gz_sim.launch.py 用的是 `from os import environ`，即在**生成
|
||
# launch 描述时**读进程环境，然后把它算出来的值作为 additional_env 传给 gz。
|
||
# 所以用 launch 的 SetEnvironmentVariable 补是没用的：那个值会被 gz_sim.launch.py
|
||
# 自己算的那份字典覆盖掉。必须在 ros2 launch 之前就进到进程环境里。
|
||
#
|
||
# 1) 插件搜索路径
|
||
# 缺了它，Gazebo 会在加载 gz_ros2_control 时报：
|
||
# [Err] [SystemLoader.cc:94] Failed to load system plugin
|
||
# [libgz_ros2_control-system.so] : couldn't find shared library.
|
||
# 而库其实就在 $CONDA_PREFIX/lib 下（`ldd` 也全齐）。原因是 gz 的
|
||
# SystemLoader 先在搜索路径里找文件，找不到就直接放弃，**不会**去
|
||
# dlopen 一个裸文件名（所以 conda 二进制的 RPATH 兜不住）。
|
||
# 又因为 gz_sim.launch.py 只把 LD_LIBRARY_PATH 拼进搜索路径，而 conda 激活
|
||
# 并不往 LD_LIBRARY_PATH 里加 $CONDA_PREFIX/lib，于是这条路径永远缺失。
|
||
#
|
||
# 2) 资源搜索路径
|
||
# gz 的 URDF 解析器会把 mesh 的 package:// 改写成 model://，
|
||
# 而 model://<名> 是去"资源路径下的子目录"里找的。因此这里放的必须是
|
||
# **各包 share 目录的上一层**（该层的子目录名就是包名），不是 share 目录本身。
|
||
# 缺了它的表现是一堆非致命但很吵的报错（视觉网格加载不出来），
|
||
# 并且**碰撞网格同样加载不出来** —— 后者会让装配任务的接触判据失效。
|
||
if [[ -n "${CONDA_PREFIX:-}" ]]; then
|
||
export GZ_SIM_SYSTEM_PLUGIN_PATH="${CONDA_PREFIX}/lib${GZ_SIM_SYSTEM_PLUGIN_PATH:+:${GZ_SIM_SYSTEM_PLUGIN_PATH}}"
|
||
# IGN_* 是 Fortress（改名之前）用的名字，两个都设，兼容新旧插件查找逻辑
|
||
export IGN_GAZEBO_SYSTEM_PLUGIN_PATH="${CONDA_PREFIX}/lib${IGN_GAZEBO_SYSTEM_PLUGIN_PATH:+:${IGN_GAZEBO_SYSTEM_PLUGIN_PATH}}"
|
||
fi
|
||
|
||
_res_paths=()
|
||
for _d in "${ROOT}"/install_sim/*/share; do
|
||
[[ -d "${_d}" ]] && _res_paths+=("${_d}")
|
||
done
|
||
_res_paths+=("${ROOT}/devices/sim/gazebo/models") # 以后往这里放自定义模型
|
||
_res_joined="$(IFS=:; echo "${_res_paths[*]}")"
|
||
|
||
export GZ_SIM_RESOURCE_PATH="${_res_joined}${GZ_SIM_RESOURCE_PATH:+:${GZ_SIM_RESOURCE_PATH}}"
|
||
export IGN_GAZEBO_RESOURCE_PATH="${_res_joined}${IGN_GAZEBO_RESOURCE_PATH:+:${IGN_GAZEBO_RESOURCE_PATH}}"
|
||
unset _res_paths _res_joined _d
|
||
|
||
# 仿真与真机共用同一份 site.yaml —— 这样"仿真里调好的参数搬到真机也生效"。
|
||
# shellcheck disable=SC1091
|
||
source "${ROOT}/scripts/lib/load_site_config.sh" 2>/dev/null || true
|
||
|
||
echo "[sim_env] CONDA=${CONDA_DEFAULT_ENV} ROS_DISTRO=${ROS_DISTRO} ROS_DOMAIN_ID=${ROS_DOMAIN_ID}"
|