chore: 框架科学优化前的完整基线
在开始'设备契约层 + 目录重构 + 通止规装配'改造之前，先钉住当前可运行状态， 使后续任何删除/重构都可回退。 本基线包含： - 现有 ROS 2 工作区全部源码（arm_application / arm_control / arm_bringup 等） - 官方 flexiv_ros2 栈（含 flexiv_hardware / flexiv_controllers 等符号链接） - 设备契约层 src/arm_interfaces（新增，46 项单测通过） - devices/ 设备选型与品牌清单（新增） - scripts/gen_site_example.py 与由它生成的 config/site.example.yaml 按 .gitignore 排除：build/ / install/ / log/ / config/site.yaml / 模型权重二进制。
This commit is contained in:
arm_dev
Executable
+283
@@ -0,0 +1,283 @@
|
||||
#!/usr/bin/env bash
|
||||
# Move Rizon4 to a named pose (default: home) and FORCE stop.
|
||||
#
|
||||
# Critical: visual_servo.launch / servo_node must be killed first, otherwise
|
||||
# they keep streaming JointTrajectory and the arm never settles.
|
||||
#
|
||||
# Usage:
|
||||
# bash ~/ros2/scripts/go_home.sh
|
||||
# bash ~/ros2/scripts/go_home.sh --pose ready --speed 0.3
|
||||
|
||||
set -eo pipefail
|
||||
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
||||
POSE="${POSE:-}"
|
||||
SPEED="${SPEED:-}"
|
||||
KEEP_SERVO=0
|
||||
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/lib/parse_arm_args.sh"
|
||||
parse_arm_args "$@"
|
||||
set -- "${PARSE_ARM_REST[@]}"
|
||||
|
||||
usage() {
|
||||
cat <<'EOF'
|
||||
Move arm to a named pose (default: home), then hold with zero velocity.
|
||||
|
||||
Default: kill perception/servo/launch first (required to stop shaking).
|
||||
|
||||
Options:
|
||||
--pose NAME home | ready | inspect | pos_left
|
||||
--speed FLOAT velocity_scale (0,1] default 0.25
|
||||
--arm 89|91 与 run_real 同一臂
|
||||
--keep-servo do NOT kill servo (arm may keep shaking)
|
||||
-h, --help
|
||||
EOF
|
||||
}
|
||||
|
||||
while [[ $# -gt 0 ]]; do
|
||||
case "$1" in
|
||||
--pose) POSE="${2:?}"; shift 2 ;;
|
||||
--speed) SPEED="${2:?}"; shift 2 ;;
|
||||
--keep-servo) KEEP_SERVO=1; shift ;;
|
||||
--stop-track) shift ;; # legacy
|
||||
-h|--help) usage; exit 0 ;;
|
||||
*) echo "Unknown: $1" >&2; usage >&2; exit 1 ;;
|
||||
esac
|
||||
done
|
||||
|
||||
if ! command -v ros2 >/dev/null 2>&1 || [[ -z "${ROS_DISTRO:-}" ]]; then
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/env.sh"
|
||||
fi
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/lib/load_site_config.sh"
|
||||
POSE="${POSE:-${SITE_GO_HOME_POSE:-home}}"
|
||||
SPEED="${SPEED:-${SITE_GO_HOME_SPEED:-0.25}}"
|
||||
|
||||
kill_streamers() {
|
||||
echo "=== killing visual-servo streamers ==="
|
||||
# Prefer disable control before kill (if node still up)
|
||||
ros2 param set /servo_node enable_control false >/dev/null 2>&1 || true
|
||||
|
||||
# Kill launch parent first, then executables (SIGTERM then SIGKILL)
|
||||
local patterns=(
|
||||
'visual_servo.launch.py'
|
||||
'/lib/arm_application/servo_node'
|
||||
'/lib/arm_application/perception_node'
|
||||
)
|
||||
local p
|
||||
for p in "${patterns[@]}"; do
|
||||
if pgrep -f "$p" >/dev/null 2>&1; then
|
||||
echo " TERM $p"
|
||||
pkill -TERM -f "$p" 2>/dev/null || true
|
||||
fi
|
||||
done
|
||||
sleep 1.0
|
||||
for p in "${patterns[@]}"; do
|
||||
if pgrep -f "$p" >/dev/null 2>&1; then
|
||||
echo " KILL $p"
|
||||
pkill -KILL -f "$p" 2>/dev/null || true
|
||||
fi
|
||||
done
|
||||
sleep 0.5
|
||||
|
||||
if pgrep -f '/lib/arm_application/servo_node' >/dev/null 2>&1; then
|
||||
echo "ERROR: servo_node still alive — refuse to move (would keep shaking)" >&2
|
||||
pgrep -af '/lib/arm_application/servo_node' >&2 || true
|
||||
exit 2
|
||||
fi
|
||||
echo " servo_node gone OK"
|
||||
}
|
||||
|
||||
print_joints() {
|
||||
python3 <<'PY'
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import JointState
|
||||
rclpy.init()
|
||||
n = Node("go_home_joints")
|
||||
box = [None]
|
||||
n.create_subscription(JointState, "joint_states", lambda m: box.__setitem__(0, m), 10)
|
||||
for _ in range(40):
|
||||
rclpy.spin_once(n, timeout_sec=0.05)
|
||||
if box[0] is not None:
|
||||
break
|
||||
if box[0] is None:
|
||||
print(" (no joint_states)")
|
||||
else:
|
||||
m = dict(zip(box[0].name, box[0].position))
|
||||
v = dict(zip(box[0].name, box[0].velocity)) if box[0].velocity else {}
|
||||
for i in range(1, 8):
|
||||
k = f"{__import__('os').environ.get('SITE_JOINT_PREFIX','Rizon4-063989_')}joint{i}"
|
||||
if k in m:
|
||||
print(f" joint{i}: {m[k]:+.4f} qd={v.get(k, 0.0):+.4f}")
|
||||
n.destroy_node(); rclpy.shutdown()
|
||||
PY
|
||||
}
|
||||
|
||||
# Strong settle via controller ACTION (not topic — matches JTC reliably)
|
||||
settle_hold_action() {
|
||||
local pose="$1"
|
||||
python3 - "$pose" <<'PY'
|
||||
import sys, time
|
||||
import rclpy
|
||||
from rclpy.action import ActionClient
|
||||
from rclpy.node import Node
|
||||
from control_msgs.action import FollowJointTrajectory
|
||||
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
|
||||
from sensor_msgs.msg import JointState
|
||||
|
||||
POSES = {
|
||||
"home": [0.0, -0.698, 0.0, 1.57, 0.0, 0.698, 0.0],
|
||||
"ready": [0.0, -0.698, 0.0, 1.57, 0.0, 0.698, 0.0],
|
||||
"inspect": [0.698, -0.698, 0.698, 1.57, 0.698, 0.698, 0.698],
|
||||
"pos_left": [-0.698, -0.698, 0.698, 1.57, 0.698, 0.698, 0.698],
|
||||
}
|
||||
name = sys.argv[1]
|
||||
target = POSES.get(name)
|
||||
if target is None:
|
||||
raise SystemExit(f"unknown pose {name}")
|
||||
import os
|
||||
PREFIX = os.environ.get("SITE_JOINT_PREFIX", "Rizon4-063989_")
|
||||
JOINTS = [f"{PREFIX}joint{i}" for i in range(1, 8)]
|
||||
|
||||
rclpy.init()
|
||||
n = Node("go_home_hold_action")
|
||||
js_box = [None]
|
||||
n.create_subscription(JointState, "joint_states", lambda m: js_box.__setitem__(0, m), 10)
|
||||
cli = ActionClient(n, FollowJointTrajectory, "rizon_arm_controller/follow_joint_trajectory")
|
||||
if not cli.wait_for_server(timeout_sec=5.0):
|
||||
print(" WARN: JTC action not available — skip action hold")
|
||||
n.destroy_node(); rclpy.shutdown(); raise SystemExit(0)
|
||||
|
||||
def spin_until(pred, timeout=3.0):
|
||||
t0 = time.monotonic()
|
||||
while time.monotonic() - t0 < timeout:
|
||||
rclpy.spin_once(n, timeout_sec=0.05)
|
||||
if pred():
|
||||
return True
|
||||
return False
|
||||
|
||||
spin_until(lambda: js_box[0] is not None, 3.0)
|
||||
|
||||
# Two-point: current -> target (or target hold), BOTH with zero velocity
|
||||
q_now = list(target)
|
||||
if js_box[0] is not None:
|
||||
m = dict(zip(js_box[0].name, js_box[0].position))
|
||||
try:
|
||||
q_now = [float(m[j]) for j in JOINTS]
|
||||
except KeyError:
|
||||
pass
|
||||
|
||||
def send_hold(q, T=0.40):
|
||||
traj = JointTrajectory()
|
||||
traj.joint_names = list(JOINTS)
|
||||
# start
|
||||
p0 = JointTrajectoryPoint()
|
||||
p0.positions = [float(v) for v in q]
|
||||
p0.velocities = [0.0] * 7
|
||||
p0.accelerations = [0.0] * 7
|
||||
p0.time_from_start.sec = 0
|
||||
p0.time_from_start.nanosec = 50_000_000
|
||||
# end same pose, still zero vel (pin)
|
||||
p1 = JointTrajectoryPoint()
|
||||
p1.positions = [float(v) for v in q]
|
||||
p1.velocities = [0.0] * 7
|
||||
p1.accelerations = [0.0] * 7
|
||||
ns = int(round(T * 1e9))
|
||||
p1.time_from_start.sec = ns // 1_000_000_000
|
||||
p1.time_from_start.nanosec = ns % 1_000_000_000
|
||||
traj.points = [p0, p1]
|
||||
goal = FollowJointTrajectory.Goal()
|
||||
goal.trajectory = traj
|
||||
fut = cli.send_goal_async(goal)
|
||||
spin_until(lambda: fut.done(), 3.0)
|
||||
if not fut.done():
|
||||
print(" hold goal send timeout")
|
||||
return False
|
||||
gh = fut.result()
|
||||
if gh is None or not gh.accepted:
|
||||
print(" hold goal rejected")
|
||||
return False
|
||||
rfut = gh.get_result_async()
|
||||
spin_until(lambda: rfut.done(), T + 3.0)
|
||||
return True
|
||||
|
||||
# 1) snap toward named pose with zero end vel
|
||||
print(f" action hold → {name} (zero vel)")
|
||||
send_hold(target, T=0.60)
|
||||
time.sleep(0.4)
|
||||
# 2–4) re-pin measured pose several times (align JTC open-loop cmd to encoder)
|
||||
for i in range(3):
|
||||
spin_until(lambda: js_box[0] is not None, 1.0)
|
||||
if js_box[0] is None:
|
||||
break
|
||||
m = dict(zip(js_box[0].name, js_box[0].position))
|
||||
q_pin = [float(m[j]) for j in JOINTS]
|
||||
print(f" action re-pin #{i+1} @ measured joints (zero vel)")
|
||||
send_hold(q_pin, T=0.45)
|
||||
time.sleep(0.25)
|
||||
|
||||
# Wait until |qd| small
|
||||
print(" waiting for joints to stop ...")
|
||||
t0 = time.monotonic()
|
||||
while time.monotonic() - t0 < 5.0:
|
||||
rclpy.spin_once(n, timeout_sec=0.05)
|
||||
if js_box[0] is None or not js_box[0].velocity:
|
||||
continue
|
||||
vmap = dict(zip(js_box[0].name, js_box[0].velocity))
|
||||
vmax = max(abs(float(vmap.get(j, 0.0))) for j in JOINTS)
|
||||
if vmax < 0.015:
|
||||
print(f" settled |qd|_max={vmax:.4f} rad/s")
|
||||
break
|
||||
else:
|
||||
print(" WARN: still some residual velocity")
|
||||
|
||||
n.destroy_node()
|
||||
rclpy.shutdown()
|
||||
PY
|
||||
}
|
||||
|
||||
if [[ "${KEEP_SERVO}" -eq 0 ]]; then
|
||||
kill_streamers
|
||||
else
|
||||
echo "WARNING: --keep-servo — shaking likely"
|
||||
fi
|
||||
|
||||
if ! ros2 service list 2>/dev/null | grep -q '/set_named_pose'; then
|
||||
echo "ERROR: /set_named_pose missing — start run_real.sh" >&2
|
||||
exit 1
|
||||
fi
|
||||
|
||||
echo "=== joints before ==="
|
||||
print_joints
|
||||
|
||||
echo "=== move: ${POSE} speed=${SPEED} ==="
|
||||
RESP="$(
|
||||
ros2 service call /set_named_pose arm_msgs/srv/SetNamedPose \
|
||||
"{pose_name: '${POSE}', velocity_scale: ${SPEED}, acceleration_scale: 0.0}" 2>&1
|
||||
)"
|
||||
echo "${RESP}"
|
||||
if ! echo "${RESP}" | grep -q 'success=True'; then
|
||||
echo "ERROR: move failed" >&2
|
||||
exit 1
|
||||
fi
|
||||
|
||||
WAIT_SEC="$(python3 -c "print(max(2.5, 1.0 / max(float('${SPEED}'), 0.05) + 2.0))")"
|
||||
echo "Waiting ${WAIT_SEC}s ..."
|
||||
sleep "${WAIT_SEC}"
|
||||
|
||||
echo "=== settle (action, vel=0) ==="
|
||||
settle_hold_action "${POSE}"
|
||||
|
||||
# Final check: servo must still be dead
|
||||
if pgrep -f '/lib/arm_application/servo_node' >/dev/null 2>&1; then
|
||||
echo "WARNING: servo_node came back — killing again"
|
||||
kill_streamers
|
||||
settle_hold_action "${POSE}"
|
||||
fi
|
||||
|
||||
echo "=== joints after ==="
|
||||
print_joints
|
||||
echo "Done. Arm should be still."
|
||||
echo "Tracking later: bash ~/ros2/scripts/run_visual_servo_rt.sh --control"
|
||||
Reference in New Issue
Block a user