主界面一屏要同时看臂、夹爪、相机和触觉，按钮再被拉高就会叠到画面上。相机按原分辨率走回调取最新帧，场景一动不再等红外齐帧把彩色丢掉。模型和相机都留在工位窗口里，启动默认不再弹 RViz。
验收：test_listed_arms 与 test_camera_preview 共 20 项通过。D435 1280×720 发布约 29 帧/秒。 Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
+14
-12
@@ -11,23 +11,24 @@ elif [[ -f /opt/ros/jazzy/setup.bash ]]; then
|
||||
source /opt/ros/jazzy/setup.bash
|
||||
fi
|
||||
|
||||
if [[ -f "${HOME}/anaconda3/etc/profile.d/conda.sh" ]]; then
|
||||
# shellcheck disable=SC1091
|
||||
source "${HOME}/anaconda3/etc/profile.d/conda.sh"
|
||||
# conda 可能在 ~/anaconda3 或 ~/miniconda3。不写死用户名。
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/lib/dev_tools.sh"
|
||||
dev_source_conda || true
|
||||
if command -v conda >/dev/null 2>&1; then
|
||||
# 干净切换：先把堆叠的激活退到 base，再激活真机环境。
|
||||
# 同一 shell 里从仿真环境(ros_gz_sim)切过来时，不清干净会残留 PATH/PYTHONPATH，
|
||||
# 导致 python/pip 解析到另一个环境的包，构建报 "--editable not recognized"
|
||||
# 之类与真实原因无关的错。详见 scripts/lib/conda_switch.sh。
|
||||
# 同一 shell 里从仿真环境切过来时，不清干净会残留 PATH/PYTHONPATH。
|
||||
_env_helper="${ROOT}/scripts/lib/conda_switch.sh"
|
||||
_real_env="${REAL_ENV_NAME:-ros_flexiv}"
|
||||
if [[ -f "${_env_helper}" ]]; then
|
||||
# shellcheck disable=SC1090
|
||||
source "${_env_helper}"
|
||||
conda_switch_clean "ros_flexiv" "${SIM_ENV_NAME:-ros_gz_sim}" || \
|
||||
conda activate ros_flexiv 2>/dev/null || true
|
||||
conda_switch_clean "${_real_env}" "${SIM_ENV_NAME:-ros_gz_sim}" || \
|
||||
conda activate "${_real_env}" 2>/dev/null || true
|
||||
else
|
||||
conda activate ros_flexiv 2>/dev/null || true
|
||||
conda activate "${_real_env}" 2>/dev/null || true
|
||||
fi
|
||||
unset _env_helper
|
||||
unset _env_helper _real_env
|
||||
fi
|
||||
|
||||
if [[ -f "${ROOT}/install/setup.bash" ]]; then
|
||||
@@ -38,13 +39,14 @@ else
|
||||
fi
|
||||
export LD_LIBRARY_PATH="${HOME}/flexiv_install/lib:${LD_LIBRARY_PATH:-}"
|
||||
# pip 版 TensorRT（tensorrt-cu12）—— 加载 YOLO `.engine` 需要它
|
||||
_TRT_SITE="${CONDA_PREFIX:-${HOME}/anaconda3/envs/ros_flexiv}/lib/python3.11/site-packages"
|
||||
_pyver="$(python3 -c 'import sys; print(f"{sys.version_info[0]}.{sys.version_info[1]}")' 2>/dev/null || echo 3.11)"
|
||||
_TRT_SITE="${CONDA_PREFIX:-${HOME}/anaconda3/envs/${REAL_ENV_NAME:-ros_flexiv}}/lib/python${_pyver}/site-packages"
|
||||
for _d in "${_TRT_SITE}/tensorrt" "${_TRT_SITE}/tensorrt_libs" "${_TRT_SITE}/nvidia/tensorrt/lib"; do
|
||||
if [[ -d "${_d}" ]]; then
|
||||
export LD_LIBRARY_PATH="${_d}:${LD_LIBRARY_PATH}"
|
||||
fi
|
||||
done
|
||||
unset _d _TRT_SITE
|
||||
unset _d _TRT_SITE _pyver
|
||||
export YOLO_AUTOINSTALL=false
|
||||
export ROS2_WS="${ROOT}"
|
||||
|
||||
|
||||
@@ -106,11 +106,11 @@ Done. Next steps (Ubuntu 22.04 / Humble + Rizon):
|
||||
source install/setup.bash
|
||||
|
||||
4) Real robot (site SN, no gripper yet):
|
||||
ros2 launch arm_bringup real.launch.py
|
||||
ros2 launch arm_bringup real.launch.py start_rviz:=false
|
||||
# or explicitly:
|
||||
ros2 launch arm_bringup real.launch.py robot_sn:=<site.yaml 里该臂的 robot.sn> load_gripper:=false
|
||||
ros2 launch arm_bringup real.launch.py robot_sn:=<site.yaml 里该臂的 robot.sn> load_gripper:=false start_rviz:=false
|
||||
|
||||
Official fake hardware:
|
||||
ros2 launch flexiv_bringup rizon.launch.py robot_sn:=<site.yaml 里该臂的 robot.sn> use_fake_hardware:=true load_gripper:=false
|
||||
ros2 launch flexiv_bringup rizon.launch.py robot_sn:=<site.yaml 里该臂的 robot.sn> use_fake_hardware:=true load_gripper:=false start_rviz:=false
|
||||
|
||||
EOF
|
||||
|
||||
+57
-14
@@ -117,10 +117,48 @@ def classify_state(
|
||||
)
|
||||
|
||||
|
||||
def _probe(sn: str, iface: str, enable: bool) -> Dict[str, object]:
|
||||
"""连一次机器人并读状态。返回给 classify_state 的关键字参数。"""
|
||||
import flexivrdk # 延迟导入：让本模块在没有 flexivrdk 的环境里也能被单测导入
|
||||
def pack_status(state: Dict[str, object]) -> Dict[str, object]:
|
||||
"""把探针结果收成给界面的 JSON。
|
||||
|
||||
``q`` 不能传进 ``classify_state``：那个函数只认使能字段。
|
||||
曾经用 ``classify_state(**state)``，关节角一读到就抛 TypeError，
|
||||
JSON 根本没打出来，界面就永远停在“还没连上”。
|
||||
"""
|
||||
fields = dict(state)
|
||||
q = fields.pop("q", None)
|
||||
level, summary, actions = classify_state(**fields) # type: ignore[arg-type]
|
||||
payload: Dict[str, object] = {
|
||||
**fields,
|
||||
"level": level,
|
||||
"summary": summary,
|
||||
"actions": actions,
|
||||
}
|
||||
if isinstance(q, list) and len(q) == 7:
|
||||
payload["q"] = q
|
||||
return payload
|
||||
|
||||
|
||||
def _open_robot(sn: str, iface: str, pc_ip: str):
|
||||
"""按 IPv4 白名单连机器人。网卡名不能直接交给 RDK。"""
|
||||
import flexivrdk
|
||||
from rdk_helpers import rdk_whitelist_lists
|
||||
|
||||
# 空名单才退回不指定网卡。指定了 eth0 这种名字时，FastDDS 会把真地址滤掉，
|
||||
# 表现是 ping 得通、这里一直搜到超时。
|
||||
candidates = rdk_whitelist_lists(iface, pc_ip) or [[]]
|
||||
last = ""
|
||||
for listen in candidates:
|
||||
try:
|
||||
if listen:
|
||||
return flexivrdk.Robot(sn, list(listen), True, False), ""
|
||||
return flexivrdk.Robot(sn), ""
|
||||
except Exception as exc: # noqa: BLE001
|
||||
last = str(exc)
|
||||
return None, last or "连不上 RDK"
|
||||
|
||||
|
||||
def _probe(sn: str, iface: str, enable: bool, pc_ip: str = "") -> Dict[str, object]:
|
||||
"""连一次机器人并读状态。返回给 classify_state 的关键字参数，另加关节角 ``q``。"""
|
||||
out: Dict[str, object] = {
|
||||
"connected": False,
|
||||
"operational": False,
|
||||
@@ -130,13 +168,9 @@ def _probe(sn: str, iface: str, enable: bool) -> Dict[str, object]:
|
||||
"estop_released": True,
|
||||
"error": "",
|
||||
}
|
||||
try:
|
||||
if iface:
|
||||
robot = flexivrdk.Robot(sn, [iface], True, False)
|
||||
else:
|
||||
robot = flexivrdk.Robot(sn)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
out["error"] = str(exc)
|
||||
robot, error = _open_robot(sn, iface, pc_ip)
|
||||
if robot is None:
|
||||
out["error"] = error
|
||||
return out
|
||||
|
||||
out["connected"] = True
|
||||
@@ -157,6 +191,13 @@ def _probe(sn: str, iface: str, enable: bool) -> Dict[str, object]:
|
||||
out["op_status"] = str(_safe(robot.operational_status, ""))
|
||||
out["fault"] = bool(_safe(robot.fault, False))
|
||||
out["estop_released"] = bool(_safe(robot.estop_released, True))
|
||||
# 只读关节角，给界面画姿态。读不到就空着，绝不在这里 Enable。
|
||||
try:
|
||||
q = [float(v) for v in list(robot.states().q)[:7]]
|
||||
except Exception: # noqa: BLE001
|
||||
q = []
|
||||
if len(q) == 7 and all(v == v for v in q):
|
||||
out["q"] = q
|
||||
return out
|
||||
|
||||
|
||||
@@ -164,18 +205,20 @@ def main(argv: Optional[Sequence[str]] = None) -> int:
|
||||
ap = argparse.ArgumentParser(description="读一次机械臂状态并给结论")
|
||||
ap.add_argument("--sn", required=True)
|
||||
ap.add_argument("--iface", default="")
|
||||
ap.add_argument("--pc-ip", default="", help="本机机器人网卡的 IPv4，用来做 RDK 白名单")
|
||||
ap.add_argument("--enable", action="store_true", help="顺手尝试 Enable（会真的使能）")
|
||||
ap.add_argument("--out", default="", help="把 KEY=VALUE 写到这个文件")
|
||||
ap.add_argument("--json", action="store_true", help="把结果打到 stdout（JSON）")
|
||||
args = ap.parse_args(argv)
|
||||
|
||||
state = _probe(args.sn, args.iface, args.enable)
|
||||
level, summary, actions = classify_state(**state) # type: ignore[arg-type]
|
||||
payload = {**state, "level": level, "summary": summary, "actions": actions}
|
||||
state = _probe(args.sn, args.iface, args.enable, args.pc_ip)
|
||||
payload = pack_status(state)
|
||||
level = str(payload["level"])
|
||||
actions = list(payload["actions"])
|
||||
|
||||
if args.out:
|
||||
lines = [f"{k}={v}" for k, v in payload.items() if k != "actions"]
|
||||
lines.append("actions=" + " | ".join(actions))
|
||||
lines.append("actions=" + " | ".join(str(item) for item in actions))
|
||||
Path(args.out).write_text("\n".join(lines) + "\n", encoding="utf-8")
|
||||
if args.json:
|
||||
print(json.dumps(payload, ensure_ascii=False, indent=2))
|
||||
|
||||
+11
-5
@@ -1,5 +1,5 @@
|
||||
#!/usr/bin/env bash
|
||||
# 三阶段装配流程的完整入口：一阶段跟踪 → 二阶段对孔 → 三阶段搜孔轻压。
|
||||
# 跟踪 → 对孔。搜孔轻压已取消，本脚本不再进入那一步，也不能用参数打开。
|
||||
#
|
||||
# 为什么要单独有这个脚本：
|
||||
# pipeline.launch.py 只管两件事 —— 发布手眼静态 TF、起 assembly_pipeline 串联节点。
|
||||
@@ -13,10 +13,10 @@
|
||||
# 4) exec pipeline.launch.py，把下面的 CLI 参数映射成它的 launch 参数
|
||||
#
|
||||
# 用法：
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 # 全流程
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 # 跟踪 + 对孔
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 --hole k2 # 指定对 k2 孔
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 --skip-stage1 # 臂已在工件上方，跳过跟踪
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 --depth 6 --pitch 1.0
|
||||
# 旋入不在这里。用 run_stage3_search.sh --arm 89 --screw
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 --camera-preview # 相机带 Qt 预览窗
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 --no-camera # 相机已在别处拉起了
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 --no-tactile # 不起触觉服务
|
||||
@@ -37,7 +37,7 @@ ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
||||
# ---- 默认值：全部沿用 site.yaml / launch 默认，CLI 只做覆盖 ----
|
||||
SKIP_STAGE1=false
|
||||
SKIP_STAGE2=false
|
||||
SKIP_STAGE3=false
|
||||
SKIP_STAGE3=true # 搜孔已取消。--skip-stage3 仍然接受，但没有开关能把它重新打开。
|
||||
HOLE=""
|
||||
DEPTH=""
|
||||
PITCH=""
|
||||
@@ -83,6 +83,12 @@ while [[ $# -gt 0 ]]; do
|
||||
esac
|
||||
done
|
||||
|
||||
if [[ -n "${DEPTH}" || -n "${PITCH}" ]]; then
|
||||
echo "ERROR: 搜孔轻压已取消，整体流程不再搜孔，也不在这里旋入。" >&2
|
||||
echo " 旋入请用: bash ~/ros2/scripts/run_stage3_search.sh --arm ${SITE_ROBOT_ARM:-89} --screw" >&2
|
||||
exit 1
|
||||
fi
|
||||
|
||||
if [[ -n "${FINAL_GAP}" && "${NO_DESCENT}" == true ]]; then
|
||||
echo "ERROR: 同时写了 --final-gap 和 --no-descent，不知道听谁的。" >&2
|
||||
exit 1
|
||||
@@ -241,7 +247,7 @@ fi
|
||||
# 每次 echo 吃满超时 × 60 次 ≈ 3 分钟，终端上没有任何新行（2026-10-09 真机）。
|
||||
_SKIP_TACTILE_REASON=""
|
||||
if [[ "${SKIP_STAGE3}" == true && "${WITH_TACTILE}" == true ]]; then
|
||||
echo "=== --skip-stage3：一/二阶段不用触觉，跳过拉起 ==="
|
||||
echo "=== 搜孔已取消：跟踪和对孔不用触觉，跳过拉起 ==="
|
||||
WITH_TACTILE=false
|
||||
_SKIP_TACTILE_REASON="stage3"
|
||||
fi
|
||||
|
||||
@@ -31,6 +31,8 @@ done
|
||||
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/env.sh"
|
||||
# 用源码里的采集逻辑，不走 install 里那份旧的。install 的入口仍由 ros2 run 找到。
|
||||
export PYTHONPATH="${ROOT}/src/arm_application${PYTHONPATH:+:${PYTHONPATH}}"
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/lib/load_site_config.sh"
|
||||
|
||||
@@ -127,12 +129,14 @@ DO_PREVIEW="${SITE_CAM_PREVIEW:-1}"
|
||||
if [[ "${PREVIEW}" -ge 0 ]]; then
|
||||
DO_PREVIEW="${PREVIEW}"
|
||||
fi
|
||||
if [[ "${SITE_CAM_ENABLE_INFRA:-1}" == "1" ]]; then
|
||||
# 程序界面只看彩色和深度。红外是旧的四窗格查看器才要的。
|
||||
# 彩色要等红外齐帧时，场景一动，整组帧对不齐，彩色就被丢掉。
|
||||
if [[ "${DO_PREVIEW}" == "1" && "${SITE_CAM_ENABLE_INFRA:-1}" == "1" ]]; then
|
||||
CAM_ARGS+=(-p enable_infra:=true)
|
||||
else
|
||||
CAM_ARGS+=(-p enable_infra:=false)
|
||||
fi
|
||||
if [[ "${SITE_CAM_PREVIEW_STEREO:-1}" == "1" ]]; then
|
||||
if [[ "${DO_PREVIEW}" == "1" && "${SITE_CAM_PREVIEW_STEREO:-1}" == "1" ]]; then
|
||||
CAM_ARGS+=(-p preview_stereo:=true)
|
||||
else
|
||||
CAM_ARGS+=(-p preview_stereo:=false)
|
||||
|
||||
@@ -17,6 +17,8 @@ while [[ $# -gt 0 ]]; do
|
||||
done
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/env.sh"
|
||||
# 用源码里的采集逻辑，不走 install 里那份旧的。install 的入口仍由 ros2 run 找到。
|
||||
export PYTHONPATH="${ROOT}/src/arm_application${PYTHONPATH:+:${PYTHONPATH}}"
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/lib/load_site_config.sh"
|
||||
SN="${SITE_D435_SERIAL:-}"
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
#!/usr/bin/env bash
|
||||
# 只看 URDF（joint_state_publisher_gui + RViz）
|
||||
# 发布 URDF。模型在工位程序里看，不再弹出 RViz 和关节滑条窗。
|
||||
set -euo pipefail
|
||||
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/install/setup.bash"
|
||||
ros2 launch arm_description display.launch.py
|
||||
ros2 launch arm_description display.launch.py use_rviz:=false use_gui:=false
|
||||
|
||||
@@ -61,6 +61,107 @@ fi
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/env.sh"
|
||||
|
||||
# 平行爪节点起来之后串口是它的。再开一次会报占用，开合就失效。
|
||||
# 这时把命令交给节点已经在听的 /gripper/ctrl。
|
||||
pgea_node_up() {
|
||||
local line
|
||||
while IFS= read -r line; do
|
||||
[[ "${line}" == *pgrep* ]] && continue
|
||||
[[ -n "${line}" ]] && return 0
|
||||
done < <(pgrep -af 'dh_pgea_node.py|lib/arm_application/dh_pgea' 2>/dev/null || true)
|
||||
return 1
|
||||
}
|
||||
|
||||
# 控制前把平行爪驱动拉起来。不 pkill 已经在跑的节点，也不写 0xA5。
|
||||
# auto_init 用 site 里这一臂的值（现场是 false，回零会把夹着的件丢掉）。
|
||||
# setsid：命令脚本退出后驱动还在，不会跟着会话一起被挂断。
|
||||
ensure_pgea_node() {
|
||||
if pgea_node_up; then
|
||||
return 0
|
||||
fi
|
||||
local port="${SITE_GRIP_PORT:-/dev/ttyUSB0}"
|
||||
if [[ ! -e "${port}" ]]; then
|
||||
echo "ERROR: ${port} 不在。USB–RS485 没插上，夹爪驱动起不来。" >&2
|
||||
return 1
|
||||
fi
|
||||
local node_py="${ROOT}/src/arm_application/arm_application/dh_pgea_node.py"
|
||||
local short="${SITE_ROBOT_SHORT_ID:-89}"
|
||||
local log_dir="/tmp/orisys_console"
|
||||
local log="${log_dir}/gripper_node_${short}.log"
|
||||
local auto="${SITE_GRIP_AUTO_INIT:-true}"
|
||||
mkdir -p "${log_dir}"
|
||||
echo "=== 平行爪驱动没在跑，先拉起来（auto_init=${auto}，不另写回零） ${port} ==="
|
||||
setsid python3 "${node_py}" --ros-args \
|
||||
-p port:="${port}" \
|
||||
-p baud:="${SITE_GRIP_BAUD:-115200}" \
|
||||
-p slave_id:="${SITE_GRIP_SLAVE:-1}" \
|
||||
-p stroke_mm:="${SITE_GRIP_STROKE_MM:-20.0}" \
|
||||
-p force_pct:="${SITE_GRIP_FORCE:-50}" \
|
||||
-p speed_pct:="${SITE_GRIP_SPEED:-50}" \
|
||||
-p auto_init:="${auto}" \
|
||||
-p model:="${MODEL}" \
|
||||
-p "serial_number:='${SITE_GRIP_SN:-}'" \
|
||||
-p "rs485_rts:='${SITE_GRIP_RS485_RTS:-}'" \
|
||||
>>"${log}" 2>&1 < /dev/null &
|
||||
local pid=$!
|
||||
local i
|
||||
for i in $(seq 1 24); do
|
||||
if ! kill -0 "${pid}" 2>/dev/null; then
|
||||
echo "ERROR: 夹爪驱动起来就退出了。" >&2
|
||||
tail -n 30 "${log}" >&2 || true
|
||||
return 1
|
||||
fi
|
||||
if timeout 2 ros2 topic echo /gripper/states --once >/dev/null 2>&1; then
|
||||
echo "=== 夹爪驱动已在发 /gripper/states（pid ${pid}）==="
|
||||
return 0
|
||||
fi
|
||||
done
|
||||
echo "ERROR: 夹爪驱动没有发出 /gripper/states。" >&2
|
||||
tail -n 30 "${log}" >&2 || true
|
||||
kill "${pid}" 2>/dev/null || true
|
||||
return 1
|
||||
}
|
||||
|
||||
if [[ ${#CLI[@]} -gt 0 && "${IS_ROTARY}" -eq 0 ]]; then
|
||||
ensure_pgea_node || exit 1
|
||||
fi
|
||||
|
||||
if [[ ${#CLI[@]} -gt 0 && "${IS_ROTARY}" -eq 0 ]] && pgea_node_up; then
|
||||
export PYTHONPATH="${ROOT}/src/arm_application${PYTHONPATH:+:${PYTHONPATH}}"
|
||||
export GRIP_CMD="${CLI[0]}"
|
||||
export GRIP_VAL="${CLI[1]:-}"
|
||||
export GRIP_FORCE="${GRIP_FORCE:-${SITE_GRIP_FORCE:-50}}"
|
||||
export GRIP_SPEED="${GRIP_SPEED:-${SITE_GRIP_SPEED:-50}}"
|
||||
echo "=== 平行爪节点已占 ${SITE_GRIP_PORT:-/dev/ttyUSB0}，命令走 /gripper/ctrl ==="
|
||||
CTRL="$(python3 - <<'PY'
|
||||
import os
|
||||
from arm_application.dh_pgea import ctrl_values
|
||||
vals = ctrl_values(
|
||||
os.environ.get("GRIP_CMD", ""),
|
||||
os.environ.get("GRIP_VAL", ""),
|
||||
int(os.environ.get("GRIP_FORCE") or "50"),
|
||||
int(os.environ.get("GRIP_SPEED") or "50"),
|
||||
)
|
||||
if vals is None:
|
||||
raise SystemExit(2)
|
||||
print(", ".join(str(v) for v in vals))
|
||||
PY
|
||||
)" && rc=0 || rc=$?
|
||||
if [[ "${rc}" -eq 2 ]]; then
|
||||
exec timeout 4 ros2 topic echo /gripper/states --once
|
||||
fi
|
||||
if [[ "${rc}" -ne 0 ]]; then
|
||||
echo "ERROR: 无法把夹爪命令转成 /gripper/ctrl" >&2
|
||||
exit 1
|
||||
fi
|
||||
exec timeout 8 ros2 topic pub --once /gripper/ctrl std_msgs/msg/Float32MultiArray "{data: [${CTRL}]}"
|
||||
fi
|
||||
|
||||
if [[ ${#CLI[@]} -gt 0 && "${IS_ROTARY}" -eq 0 ]]; then
|
||||
echo "ERROR: 平行爪命令没有送到 /gripper/ctrl。" >&2
|
||||
exit 1
|
||||
fi
|
||||
|
||||
if [[ ${#CLI[@]} -gt 0 ]]; then
|
||||
if [[ "${IS_ROTARY}" -eq 1 ]]; then
|
||||
CMD="${CLI[0]}"
|
||||
|
||||
+3
-2
@@ -19,7 +19,8 @@ while [[ $# -gt 0 ]]; do
|
||||
--list|-l) LIST_ONLY=true; shift ;;
|
||||
-h|--help)
|
||||
sed -n '2,8p' "$0"
|
||||
echo " Extra args are passed to ros2 launch (e.g. use_rviz:=false)."
|
||||
echo " 机械臂模型在工位程序里看，这里默认不开 RViz。"
|
||||
echo " 其余参数传给 ros2 launch。要临时打开 RViz：use_rviz:=true"
|
||||
exit 0
|
||||
;;
|
||||
*) break ;;
|
||||
@@ -127,4 +128,4 @@ if ! "${RDK_PYTHON}" "${ROOT}/scripts/rdk_switch_tool.py" "${RDK_TOOL_ARGS[@]}";
|
||||
exit 1
|
||||
fi
|
||||
|
||||
exec ros2 launch arm_bringup stack.launch.py backend:=real robot_sn:="${ROBOT_SN}" "$@"
|
||||
exec ros2 launch arm_bringup stack.launch.py backend:=real robot_sn:="${ROBOT_SN}" use_rviz:=false "$@"
|
||||
|
||||
+2
-2
@@ -1,5 +1,5 @@
|
||||
#!/usr/bin/env bash
|
||||
# 快速启动：mock 机械臂 + RViz
|
||||
# 快速启动 mock 机械臂。模型在工位程序里看，不再弹出 RViz。
|
||||
set -euo pipefail
|
||||
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
||||
# shellcheck disable=SC1091
|
||||
@@ -16,4 +16,4 @@ source "${ROOT}/install/setup.bash"
|
||||
# shellcheck source=lib/ros_domain.sh
|
||||
source "${ROOT}/scripts/lib/ros_domain.sh"
|
||||
ros_domain_force_real
|
||||
ros2 launch arm_bringup sim.launch.py
|
||||
ros2 launch arm_bringup sim.launch.py use_rviz:=false
|
||||
|
||||
@@ -13,7 +13,7 @@
|
||||
# bash scripts/run_sim_gz.sh --no-gripper --no-sensors # 最省资源的运动学验证
|
||||
#
|
||||
# 与 scripts/run_sim.sh 的区别（两个名字容易混，这里说清）：
|
||||
# run_sim.sh ← 走 arm_bringup 的 **mock** 驱动：假数据 + RViz，无物理
|
||||
# run_sim.sh ← 走 arm_bringup 的 **mock** 驱动：假数据，无物理，模型在工位程序里看
|
||||
# run_sim_gz.sh ← 走 Gazebo：真物理、真接触
|
||||
# 两者都不需要真机。要验证力控/接触类逻辑只能用后者。
|
||||
#
|
||||
|
||||
@@ -1,18 +1,16 @@
|
||||
#!/usr/bin/env bash
|
||||
# 三阶段柔顺搜孔：触面 → 力闭环螺旋搜孔 → 十字定心 → 力限插入/旋入 → 锁定。
|
||||
# 旋转装配入口。不带 --screw 会直接拒绝，不再单独做搜孔轻压。
|
||||
# 前置（各占一个终端）：
|
||||
# T1 bash ~/ros2/scripts/run_real.sh --arm 89 # 控制器
|
||||
# —— 二阶段已完成（run_stage2_locate.sh --arm 89），臂保持悬停不动
|
||||
# 本脚本会自动拉起触觉传感器服务（仅左臂 89 装了 Orisys 传感器）。
|
||||
#
|
||||
# 用法：
|
||||
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 # 搜孔+落座（site 默认深度）
|
||||
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --hole k2 # 对 k2 孔
|
||||
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 0 # 只搜孔定心，不插入
|
||||
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 6 --pitch 1.0 # 螺纹规旋入(M6=1.0)
|
||||
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --r-max 5 # 临时改搜索半径
|
||||
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --resnap # 搜前用 D405 复拍对准
|
||||
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --no-tactile # 不起触觉服务（调试用）
|
||||
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --screw # 深度和螺距只读 site
|
||||
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --hole k2 --screw
|
||||
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --r-max 5 --screw # 临时改搜索半径
|
||||
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --resnap --screw # 旋入前用 D405 复拍
|
||||
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --no-tactile --screw # 不起触觉服务（调试用）
|
||||
#
|
||||
# 参数固定接口：config/site.yaml → hole_detect.stage3（力阈值/螺旋/插入全部可调）
|
||||
# 力阈值标定：先 bash ~/ros2/scripts/run_tactile.sh --cli 看 Orisys 读数量级
|
||||
@@ -22,6 +20,7 @@ ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
||||
HOLE=""
|
||||
DEPTH=""
|
||||
PITCH=""
|
||||
SCREW=false
|
||||
RMAX=""
|
||||
RESNAP=false
|
||||
WITH_TACTILE=true
|
||||
@@ -39,7 +38,8 @@ while [[ $# -gt 0 ]]; do
|
||||
--r-max) RMAX="${2:?}"; shift 2 ;;
|
||||
--resnap) RESNAP=true; shift ;;
|
||||
--no-tactile) WITH_TACTILE=false; shift ;;
|
||||
-h|--help) sed -n '2,24p' "$0"; exit 0 ;;
|
||||
--screw) SCREW=true; shift ;;
|
||||
-h|--help) sed -n '2,19p' "$0"; exit 0 ;;
|
||||
*) echo "Unknown: $1" >&2; exit 1 ;;
|
||||
esac
|
||||
done
|
||||
@@ -49,6 +49,44 @@ source "${ROOT}/scripts/env.sh"
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/lib/load_site_config.sh"
|
||||
|
||||
if [[ "${SCREW}" != true ]]; then
|
||||
echo "ERROR: 搜孔轻压已取消。这一步不再单独跑，也不再跟在跟踪和对孔后面。" >&2
|
||||
echo " 要旋入请加 --screw，深度和螺距只读 site.yaml。" >&2
|
||||
exit 1
|
||||
fi
|
||||
|
||||
if [[ "${SCREW}" == true ]]; then
|
||||
if [[ -n "${DEPTH}" || -n "${PITCH}" ]]; then
|
||||
echo "ERROR: --screw 不要再写 --depth 或 --pitch。深度和螺距只在 site.yaml。" >&2
|
||||
exit 1
|
||||
fi
|
||||
_screw_line="$(python3 - "${SITE_CONFIG}" << 'PY'
|
||||
import sys
|
||||
import yaml
|
||||
|
||||
path = sys.argv[1]
|
||||
with open(path, encoding="utf-8") as handle:
|
||||
data = yaml.safe_load(handle) or {}
|
||||
stage3 = ((data.get("hole_detect") or {}).get("stage3") or {})
|
||||
depth = stage3.get("screw_stage_depth_mm")
|
||||
pitch = stage3.get("screw_stage_pitch_mm")
|
||||
if not isinstance(depth, (int, float)) or not isinstance(pitch, (int, float)):
|
||||
sys.stderr.write(
|
||||
"ERROR: site.yaml 的 hole_detect.stage3 缺少 screw_stage_depth_mm 或 screw_stage_pitch_mm。\n"
|
||||
)
|
||||
sys.exit(1)
|
||||
if float(depth) <= 0 or float(pitch) == 0:
|
||||
sys.stderr.write(
|
||||
f"ERROR: 旋转装配深度必须大于 0、螺距不能为 0，现在是 depth={depth} pitch={pitch}，这样旋不进去。\n"
|
||||
)
|
||||
sys.exit(1)
|
||||
print(f"{float(depth)} {float(pitch)}")
|
||||
PY
|
||||
)"
|
||||
DEPTH="${_screw_line%% *}"
|
||||
PITCH="${_screw_line#* }"
|
||||
fi
|
||||
|
||||
if [[ -n "${SITE_FASTDDS_PROFILE:-}" && -f "${SITE_FASTDDS_PROFILE}" ]]; then
|
||||
export FASTRTPS_DEFAULT_PROFILES_FILE="${SITE_FASTDDS_PROFILE}"
|
||||
export RMW_IMPLEMENTATION="${RMW_IMPLEMENTATION:-rmw_fastrtps_cpp}"
|
||||
|
||||
Reference in New Issue
Block a user