cat > ~/run_panda_pick_place.sh <<'BASH'
#!/usr/bin/env bash
set -euo pipefail

echo "=== Panda MoveIt 抓取演示：一键运行脚本 ==="

# 0) 基础环境
if [ -f /opt/ros/noetic/setup.bash ]; then
  source /opt/ros/noetic/setup.bash
else
  echo "未发现 /opt/ros/noetic，需先安装 ROS Noetic。"; exit 1
fi

ROS_DISTRO_CUR=$(rosversion -d 2>/dev/null || echo "unknown")
echo "当前 ROS 发行版: ${ROS_DISTRO_CUR}"
if [ "${ROS_DISTRO_CUR}" != "noetic" ]; then
  echo "警告：脚本按 Noetic 准备，当前为 ${ROS_DISTRO_CUR}，继续尝试运行…"
fi

# 1) 准备工作空间与依赖
WS=~/ws_arm
PKG=class_demo
mkdir -p "$WS/src"

# 确保必须的包存在（若缺失，尝试安装）
need_pkg() { rospack find "$1" >/dev/null 2>&1 || return 0; return 1; }
if ! rospack find panda_moveit_config >/dev/null 2>&1; then
  echo "未找到 panda_moveit_config，准备安装依赖（需要 sudo 密码）…"
  sudo apt update
  sudo apt install -y ros-noetic-moveit ros-noetic-moveit-resources-panda-moveit-config python3-catkin-tools
fi

# 2) 创建/更新演示包与脚本
if ! rospack find ${PKG} >/dev/null 2>&1; then
  pushd "$WS/src" >/dev/null
  if ! command -v catkin >/dev/null 2>&1; then
    echo "安装 catkin 工具（需要 sudo）…"
    sudo apt update && sudo apt install -y python3-catkin-tools
  fi
  if [ ! -f "$WS/.catkin_tools/profiles/default/config.yaml" ]; then
    catkin init
  fi
  if [ ! -d "${PKG}" ]; then
    catkin_create_pkg ${PKG} rospy geometry_msgs
    mkdir -p ${PKG}/scripts
  fi
  popd >/dev/null
fi

# 写入演示脚本（已修正：组名、cartesian path 参数、无 f-string）
cat > "${WS}/src/${PKG}/scripts/pick_place_demo.py" <<'PY'
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys, time, rospy
import moveit_commander, geometry_msgs.msg

def wait_for_state_update(scene, name, box_is_known=False, box_is_attached=False, timeout=4.0):
    start = time.time()
    while (time.time() - start) < timeout and not rospy.is_shutdown():
        attached = len(scene.get_attached_objects([name])) > 0
        known = name in scene.get_known_object_names()
        if attached == box_is_attached and (known == box_is_known or box_is_attached):
            return True
        rospy.sleep(0.1)
    return False

def open_gripper(hand):
    try:
        hand.set_named_target("open")
        hand.go(wait=True); return
    except Exception:
        pass
    joints = hand.get_active_joints()
    targets = {j: 0.04 for j in joints}
    hand.set_joint_value_target(targets); hand.go(wait=True)

def close_gripper(hand):
    try:
        hand.set_named_target("close")
        hand.go(wait=True); return
    except Exception:
        pass
    joints = hand.get_active_joints()
    targets = {j: 0.005 for j in joints}
    hand.set_joint_value_target(targets); hand.go(wait=True)

def main():
    rospy.init_node("class_pick_place")
    moveit_commander.roscpp_initialize(sys.argv)

    rospy.loginfo("初始化 MoveIt 接口…")
    robot = moveit_commander.RobotCommander()
    scene = moveit_commander.PlanningSceneInterface(synchronous=True)
    arm = moveit_commander.MoveGroupCommander("panda_arm")
    hand = moveit_commander.MoveGroupCommander("panda_hand")
    eef_link = arm.get_end_effector_link()

    arm.set_planner_id("RRTConnectkConfigDefault")
    arm.set_planning_time(5.0)
    arm.set_num_planning_attempts(10)
    arm.set_max_acceleration_scaling_factor(0.5)
    arm.set_max_velocity_scaling_factor(0.5)
    rospy.sleep(1.0)

    # 1) 添加方块
    rospy.loginfo("添加方块到场景…")
    box_name = "demo_box"
    box_pose = geometry_msgs.msg.PoseStamped()
    box_pose.header.frame_id = robot.get_planning_frame()
    box_pose.pose.position.x, box_pose.pose.position.y, box_pose.pose.position.z = 0.40, 0.00, 0.02
    box_pose.pose.orientation.w = 1.0
    scene.add_box(box_name, box_pose, size=(0.04, 0.04, 0.04))
    wait_for_state_update(scene, box_name, box_is_known=True)

    # 2) 张开夹爪
    rospy.loginfo("张开夹爪…")
    open_gripper(hand)

    # 3) 移动到方块上方
    rospy.loginfo("移动到方块上方（预抓取位）…")
    pre = geometry_msgs.msg.Pose()
    pre.position.x, pre.position.y, pre.position.z = 0.40, 0.00, 0.25
    pre.orientation.x, pre.orientation.y, pre.orientation.z, pre.orientation.w = 0.0, 1.0, 0.0, 0.0
    arm.set_pose_target(pre)
    arm.go(wait=True); arm.stop(); arm.clear_pose_targets()

    # 4) 直线下探到抓取高度
    rospy.loginfo("直线下探…")
    grasp = geometry_msgs.msg.Pose()
    grasp.position.x, grasp.position.y, grasp.position.z = 0.40, 0.00, 0.12
    grasp.orientation = pre.orientation
    start_pose = arm.get_current_pose().pose
    plan, fraction = arm.compute_cartesian_path([start_pose, grasp], 0.01, True)
    rospy.loginfo("下探轨迹覆盖率: {:.2f}".format(fraction))
    arm.execute(plan, wait=True)

    # 5) 闭合夹爪并附着方块
    rospy.loginfo("闭合夹爪并附着方块…")
    close_gripper(hand)
    touch_links = robot.get_link_names(group="panda_hand")
    scene.attach_box(eef_link, box_name, touch_links=touch_links)
    wait_for_state_update(scene, box_name, box_is_attached=True)

    # 6) 直线抬起
    rospy.loginfo("直线抬起…")
    start_pose = arm.get_current_pose().pose
    up_pose = geometry_msgs.msg.Pose()
    up_pose.position.x = start_pose.position.x
    up_pose.position.y = start_pose.position.y
    up_pose.position.z = start_pose.position.z + 0.12
    up_pose.orientation = start_pose.orientation
    plan, fraction = arm.compute_cartesian_path([start_pose, up_pose], 0.01, True)
    rospy.loginfo("抬起轨迹覆盖率: {:.2f}".format(fraction))
    arm.execute(plan, wait=True)

    # 7) 移动到放置位置上方
    rospy.loginfo("移动到放置位置上方…")
    place_top = geometry_msgs.msg.Pose()
    place_top.position.x, place_top.position.y, place_top.position.z = 0.30, -0.20, 0.25
    place_top.orientation = pre.orientation
    arm.set_pose_target(place_top)
    arm.go(wait=True); arm.stop(); arm.clear_pose_targets()

    # 8) 下放
    rospy.loginfo("下放…")
    place = geometry_msgs.msg.Pose()
    place.position.x, place.position.y, place.position.z = 0.30, -0.20, 0.12
    place.orientation = place_top.orientation
    start_pose = arm.get_current_pose().pose
    plan, fraction = arm.compute_cartesian_path([start_pose, place], 0.01, True)
    rospy.loginfo("下放轨迹覆盖率: {:.2f}".format(fraction))
    arm.execute(plan, wait=True)

    # 9) 张开夹爪，释放方块
    rospy.loginfo("张开夹爪，释放方块…")
    open_gripper(hand)
    scene.remove_attached_object(eef_link, name=box_name)
    wait_for_state_update(scene, box_name, box_is_known=True, box_is_attached=False)

    # 10) 移除世界中的方块（可选）
    rospy.loginfo("从场景移除方块（可选）…")
    scene.remove_world_object(box_name)

    # 11) 回到安全位
    rospy.loginfo("回到安全位…")
    safe = geometry_msgs.msg.Pose()
    safe.position.x, safe.position.y, safe.position.z = 0.35, 0.0, 0.35
    safe.orientation = pre.orientation
    arm.set_pose_target(safe)
    arm.go(wait=True); arm.stop(); arm.clear_pose_targets()

    rospy.loginfo("演示完成 ✅")

if __name__ == "__main__":
    try:
        main()
    finally:
        try:
            moveit_commander.roscpp_shutdown()
        except Exception:
            pass
PY

chmod +x "${WS}/src/${PKG}/scripts/pick_place_demo.py"

# 3) 构建（保证可被 rospack/rosrun 找到）
pushd "$WS" >/dev/null
if ! command -v catkin >/dev/null 2>&1; then
  echo "安装 catkin 工具（需要 sudo）…"
  sudo apt update && sudo apt install -y python3-catkin-tools
fi
catkin build
popd >/dev/null

# 4) 启动 MoveIt + RViz（若未运行）
source "${WS}/devel/setup.bash"

if ! rosnode list 2>/dev/null | grep -q "^/move_group$"; then
  echo "启动 MoveIt demo（后台运行，日志在 ~/.ros/panda_demo.launch.log）…"
  roslaunch panda_moveit_config demo.launch > ~/.ros/panda_demo.launch.log 2>&1 &
  echo -n "等待 move_group 就绪"
  for i in $(seq 1 60); do
    if rosnode list 2>/dev/null | grep -q "^/move_group$"; then echo " …OK"; break; fi
    echo -n "."; sleep 1
    if [ "$i" -eq 60 ]; then echo " 超时未就绪，请查看 ~/.ros/panda_demo.launch.log"; exit 1; fi
  done
else
  echo "检测到 move_group 已在运行，跳过启动。"
fi

# 5) 运行抓取演示
echo "执行抓取脚本…"
rosrun ${PKG} pick_place_demo.py || { echo "脚本运行失败，查看 ~/.ros/log/latest/ 下日志"; exit 1; }

echo
echo "演示已完成。"
echo "若想让 RViz 画面更干净："
echo "  - MotionPlanning → Trajectory：取消勾选 Show Trail、Loop Animation；"
echo "  - MotionPlanning → Planning Request：取消勾选 Query Start/Goal State；"
echo "  - 如仍有重复机器人，可关闭单独的 RobotModel 显示项。"
echo "你也可以手动 File → Save Config As… 保存一个清爽配置以便下次直接使用。"
BASH

chmod +x ~/run_panda_pick_place.sh

bash ~/run_panda_pick_place.sh