别人克隆后没有 install，source 会直接失败。把展开后的 install 放进仓库，符号链接和指向本机 build 的 egg-link 收成真实文件，Python 入口改走当前环境的解释器，这样不用先 colcon build 也能启动。

Co-authored-by: Cursor <cursoragent@cursor.com>

install/arm_control/share/ament_index/resource_index/packages/arm_control

@@ -0,0 +1 @@
+

install/arm_control/share/arm_control/config/controllers.yaml

@@ -0,0 +1,34 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 100
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    # 名字与 Flexiv 官方 bringup 对齐
+    rizon_arm_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+rizon_arm_controller:
+  ros__parameters:
+    # 站点机器人 Rizon4-063989（官方 flexiv_description 保留连字符）
+    joints:
+      - Rizon4-063989_joint1
+      - Rizon4-063989_joint2
+      - Rizon4-063989_joint3
+      - Rizon4-063989_joint4
+      - Rizon4-063989_joint5
+      - Rizon4-063989_joint6
+      - Rizon4-063989_joint7
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
+    allow_partial_joints_goal: false
+    # 必须为 false：若为 true，会从最后一次 *命令* 开始插值（在硬件没动
+    # 却报"成功"之后，命令已经落在目标点上），于是机械臂永远不动。
+    open_loop_control: false
+    constraints:
+      stopped_velocity_tolerance: 0.05
+      goal_time: 2.0

install/arm_control/share/arm_control/config/named_poses.yaml

@@ -0,0 +1,10 @@
+# 命名位姿 — 站点机器人 Rizon4-063989（7 自由度），弧度
+# 关节基础名与 flexiv_description 一致：joint1..joint7
+named_pose_server:
+  ros__parameters:
+    joint_prefix: "Rizon4-063989_"
+    joint_names: [joint1, joint2, joint3, joint4, joint5, joint6, joint7]
+    named_poses.home: [0.0, -0.698, 0.0, 1.57, 0.0, 0.698, 0.0]
+    named_poses.ready: [0.0, -0.698, 0.0, 1.57, 0.0, 0.698, 0.0]
+    named_poses.inspect: [0.698, -0.698, 0.698, 1.57, 0.698, 0.698, 0.698]
+    named_poses.pos_left: [-0.698, -0.698, 0.698, 1.57, 0.698, 0.698, 0.698]

install/arm_control/share/arm_control/config/named_poses_89.yaml

@@ -0,0 +1,9 @@
+# Left arm 89 (Rizon4-063989) named poses (radians).
+named_pose_server:
+  ros__parameters:
+    joint_prefix: "Rizon4-063989_"
+    joint_names: [joint1, joint2, joint3, joint4, joint5, joint6, joint7]
+    named_poses.home: [0.0, -0.698, 0.0, 1.57, 0.0, 0.698, 0.0]
+    named_poses.ready: [0.0, -0.698, 0.0, 1.57, 0.0, 0.698, 0.0]
+    named_poses.inspect: [0.698, -0.698, 0.698, 1.57, 0.698, 0.698, 0.698]
+    named_poses.pos_left: [-0.698, -0.698, 0.698, 1.57, 0.698, 0.698, 0.698]

install/arm_control/share/arm_control/config/named_poses_91.yaml

@@ -0,0 +1,10 @@
+# 右臂 91（Rizon4-063991）的命名位姿（弧度）。
+# 起始值与通用 home 一致；在 91 上示教该工位专用的位姿。
+named_pose_server:
+  ros__parameters:
+    joint_prefix: "Rizon4-063991_"
+    joint_names: [joint1, joint2, joint3, joint4, joint5, joint6, joint7]
+    named_poses.home: [0.0, -0.698, 0.0, 1.57, 0.0, 0.698, 0.0]
+    named_poses.ready: [0.0, -0.698, 0.0, 1.57, 0.0, 0.698, 0.0]
+    named_poses.inspect: [0.698, -0.698, 0.698, 1.57, 0.698, 0.698, 0.698]
+    named_poses.pos_left: [-0.698, -0.698, 0.698, 1.57, 0.698, 0.698, 0.698]

install/arm_control/share/arm_control/hook/ament_prefix_path.dsv

@@ -0,0 +1 @@
+prepend-non-duplicate;AMENT_PREFIX_PATH;

install/arm_control/share/arm_control/hook/ament_prefix_path.ps1

@@ -0,0 +1,3 @@
+# generated from colcon_powershell/shell/template/hook_prepend_value.ps1.em
+
+colcon_prepend_unique_value AMENT_PREFIX_PATH "$env:COLCON_CURRENT_PREFIX"

install/arm_control/share/arm_control/hook/ament_prefix_path.sh

@@ -0,0 +1,3 @@
+# generated from colcon_core/shell/template/hook_prepend_value.sh.em
+
+_colcon_prepend_unique_value AMENT_PREFIX_PATH "$COLCON_CURRENT_PREFIX"

install/arm_control/share/arm_control/hook/pythonpath.dsv

@@ -0,0 +1 @@
+prepend-non-duplicate;PYTHONPATH;lib/python3.11/site-packages

install/arm_control/share/arm_control/hook/pythonpath.ps1

@@ -0,0 +1,3 @@
+# generated from colcon_powershell/shell/template/hook_prepend_value.ps1.em
+
+colcon_prepend_unique_value PYTHONPATH "$env:COLCON_CURRENT_PREFIX/lib/python3.11/site-packages"

install/arm_control/share/arm_control/hook/pythonpath.sh

@@ -0,0 +1,3 @@
+# generated from colcon_core/shell/template/hook_prepend_value.sh.em
+
+_colcon_prepend_unique_value PYTHONPATH "$COLCON_CURRENT_PREFIX/lib/python3.11/site-packages"

install/arm_control/share/arm_control/launch/mock_control.launch.py

@@ -0,0 +1,101 @@
+import os
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+from launch_ros.substitutions import FindPackageShare
+
+# 现场机械臂。官方 flexiv_description 要求 SN 前缀里**保留连字符**。
+# 由 scripts/env.sh → load_site_config.sh 提供；不兜底成具体 SN
+# （兜底等于"环境没加载好也照样启动，连哪台看运气"）。
+SITE_ROBOT_SN = os.environ.get('SITE_ROBOT_SN', '').strip()
+SITE_JOINT_PREFIX = os.environ.get('SITE_JOINT_PREFIX', f'{SITE_ROBOT_SN}_')
+SITE_NAMED_POSES = os.environ.get('SITE_NAMED_POSES_FILE', '').strip()
+
+
+def generate_launch_description():
+    use_rviz = LaunchConfiguration('use_rviz')
+    robot_sn = LaunchConfiguration('robot_sn')
+    rizon_type = LaunchConfiguration('rizon_type')
+    load_gripper = LaunchConfiguration('load_gripper')
+
+    urdf_path = PathJoinSubstitution(
+        [FindPackageShare('arm_description'), 'urdf', 'arm.urdf.xacro']
+    )
+    named_poses = SITE_NAMED_POSES if SITE_NAMED_POSES and os.path.isfile(SITE_NAMED_POSES) else None
+    if not named_poses:
+        named_poses = PathJoinSubstitution(
+            [FindPackageShare('arm_control'), 'config', 'named_poses.yaml']
+        )
+    rviz_config = PathJoinSubstitution(
+        [FindPackageShare('flexiv_description'), 'rviz', 'view_rizon.rviz']
+    )
+
+    robot_description = ParameterValue(
+        Command([
+            'xacro ', urdf_path,
+            ' robot_sn:=', robot_sn,
+            ' rizon_type:=', rizon_type,
+            ' load_gripper:=', load_gripper,
+            ' use_fake_hardware:=true',
+            ' ros2_control:=false',
+        ]),
+        value_type=str,
+    )
+
+    return LaunchDescription([
+        DeclareLaunchArgument('use_rviz', default_value='true'),
+        DeclareLaunchArgument(
+            'robot_sn',
+            default_value=SITE_ROBOT_SN,
+            description='Site SN; joints become {SN}_joint1..joint7 (hyphen kept)',
+        ),
+        DeclareLaunchArgument('rizon_type', default_value='Rizon4'),
+        DeclareLaunchArgument(
+            'load_gripper',
+            default_value='false',
+            description='Site has no gripper yet',
+        ),
+
+        Node(
+            package='robot_state_publisher',
+            executable='robot_state_publisher',
+            parameters=[{'robot_description': robot_description}],
+            output='screen',
+        ),
+        Node(
+            package='arm_control',
+            executable='mock_arm_driver',
+            name='mock_arm_driver',
+            parameters=[{'joint_prefix': SITE_JOINT_PREFIX}],
+            output='screen',
+        ),
+        Node(
+            package='arm_control',
+            executable='arm_status_node',
+            name='arm_status_node',
+            parameters=[{
+                'ee_frame': f'{SITE_JOINT_PREFIX}tcp',
+                'base_frame': f'{SITE_JOINT_PREFIX}base_link',
+            }],
+            output='screen',
+        ),
+        Node(
+            package='arm_control',
+            executable='named_pose_server',
+            name='named_pose_server',
+            parameters=[named_poses, {'joint_prefix': SITE_JOINT_PREFIX}],
+            output='screen',
+        ),
+        Node(
+            package='rviz2',
+            executable='rviz2',
+            name='rviz2',
+            arguments=['-d', rviz_config],
+            condition=IfCondition(use_rviz),
+            output='screen',
+        ),
+    ])

install/arm_control/share/arm_control/launch/ros2_control.launch.py

@@ -0,0 +1,66 @@
+# 现场机械臂 SN（由 scripts/env.sh → load_site_config.sh 从 site.yaml 导出）。
+# 这个 launch 以前**根本不传 robot_sn**，靠 arm.urdf.xacro 里写死的默认值
+# 正好等于本机 SN 才跑得起来；换台臂就会去驱动上一台臂的关节名。
+import os
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    use_fake = LaunchConfiguration('use_fake_hardware')
+    robot_sn = LaunchConfiguration('robot_sn')
+
+    urdf_path = PathJoinSubstitution(
+        [FindPackageShare('arm_description'), 'urdf', 'arm.urdf.xacro']
+    )
+    controllers = PathJoinSubstitution(
+        [FindPackageShare('arm_control'), 'config', 'controllers.yaml']
+    )
+
+    robot_description = ParameterValue(
+        Command([
+            'xacro ', urdf_path,
+            ' robot_sn:=', robot_sn,
+            ' use_fake_hardware:=', use_fake,
+        ]),
+        value_type=str,
+    )
+
+    return LaunchDescription([
+        DeclareLaunchArgument('use_fake_hardware', default_value='true'),
+        DeclareLaunchArgument(
+            'robot_sn',
+            default_value=os.environ.get('SITE_ROBOT_SN', '').strip(),
+            description='当前臂 SN；关节名会变成 {SN}_joint1..joint7（连字符保留）',
+        ),
+
+        Node(
+            package='robot_state_publisher',
+            executable='robot_state_publisher',
+            parameters=[{'robot_description': robot_description}],
+            output='screen',
+        ),
+        Node(
+            package='controller_manager',
+            executable='ros2_control_node',
+            parameters=[{'robot_description': robot_description}, controllers],
+            output='screen',
+        ),
+        Node(
+            package='controller_manager',
+            executable='spawner',
+            arguments=['joint_state_broadcaster', '--controller-manager', '/controller_manager'],
+            output='screen',
+        ),
+        Node(
+            package='controller_manager',
+            executable='spawner',
+            arguments=['rizon_arm_controller', '--controller-manager', '/controller_manager'],
+            output='screen',
+        ),
+    ])

install/arm_control/share/arm_control/package.bash

@@ -0,0 +1,31 @@
+# generated from colcon_bash/shell/template/package.bash.em
+
+# This script extends the environment for this package.
+
+# a bash script is able to determine its own path if necessary
+if [ -z "$COLCON_CURRENT_PREFIX" ]; then
+  # the prefix is two levels up from the package specific share directory
+  _colcon_package_bash_COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`/../.." > /dev/null && pwd)"
+else
+  _colcon_package_bash_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
+fi
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+# additional arguments: arguments to the script
+_colcon_package_bash_source_script() {
+  if [ -f "$1" ]; then
+    if [ -n "$COLCON_TRACE" ]; then
+      echo "# . \"$1\""
+    fi
+    . "$@"
+  else
+    echo "not found: \"$1\"" 1>&2
+  fi
+}
+
+# source sh script of this package
+_colcon_package_bash_source_script "$_colcon_package_bash_COLCON_CURRENT_PREFIX/share/arm_control/package.sh"
+
+unset _colcon_package_bash_source_script
+unset _colcon_package_bash_COLCON_CURRENT_PREFIX

install/arm_control/share/arm_control/package.dsv

@@ -0,0 +1,6 @@
+source;share/arm_control/hook/pythonpath.ps1
+source;share/arm_control/hook/pythonpath.dsv
+source;share/arm_control/hook/pythonpath.sh
+source;share/arm_control/hook/ament_prefix_path.ps1
+source;share/arm_control/hook/ament_prefix_path.dsv
+source;share/arm_control/hook/ament_prefix_path.sh

install/arm_control/share/arm_control/package.ps1

@@ -0,0 +1,116 @@
+# generated from colcon_powershell/shell/template/package.ps1.em
+
+# function to append a value to a variable
+# which uses colons as separators
+# duplicates as well as leading separators are avoided
+# first argument: the name of the result variable
+# second argument: the value to be prepended
+function colcon_append_unique_value {
+  param (
+    $_listname,
+    $_value
+  )
+
+  # get values from variable
+  if (Test-Path Env:$_listname) {
+    $_values=(Get-Item env:$_listname).Value
+  } else {
+    $_values=""
+  }
+  $_duplicate=""
+  # start with no values
+  $_all_values=""
+  # iterate over existing values in the variable
+  if ($_values) {
+    $_values.Split(":") | ForEach {
+      # not an empty string
+      if ($_) {
+        # not a duplicate of _value
+        if ($_ -eq $_value) {
+          $_duplicate="1"
+        }
+        if ($_all_values) {
+          $_all_values="${_all_values}:$_"
+        } else {
+          $_all_values="$_"
+        }
+      }
+    }
+  }
+  # append only non-duplicates
+  if (!$_duplicate) {
+    # avoid leading separator
+    if ($_all_values) {
+      $_all_values="${_all_values}:${_value}"
+    } else {
+      $_all_values="${_value}"
+    }
+  }
+
+  # export the updated variable
+  Set-Item env:\$_listname -Value "$_all_values"
+}
+
+# function to prepend a value to a variable
+# which uses colons as separators
+# duplicates as well as trailing separators are avoided
+# first argument: the name of the result variable
+# second argument: the value to be prepended
+function colcon_prepend_unique_value {
+  param (
+    $_listname,
+    $_value
+  )
+
+  # get values from variable
+  if (Test-Path Env:$_listname) {
+    $_values=(Get-Item env:$_listname).Value
+  } else {
+    $_values=""
+  }
+  # start with the new value
+  $_all_values="$_value"
+  # iterate over existing values in the variable
+  if ($_values) {
+    $_values.Split(":") | ForEach {
+      # not an empty string
+      if ($_) {
+        # not a duplicate of _value
+        if ($_ -ne $_value) {
+          # keep non-duplicate values
+          $_all_values="${_all_values}:$_"
+        }
+      }
+    }
+  }
+  # export the updated variable
+  Set-Item env:\$_listname -Value "$_all_values"
+}
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+# additional arguments: arguments to the script
+function colcon_package_source_powershell_script {
+  param (
+    $_colcon_package_source_powershell_script
+  )
+  # source script with conditional trace output
+  if (Test-Path $_colcon_package_source_powershell_script) {
+    if ($env:COLCON_TRACE) {
+      echo ". '$_colcon_package_source_powershell_script'"
+    }
+    . "$_colcon_package_source_powershell_script"
+  } else {
+    Write-Error "not found: '$_colcon_package_source_powershell_script'"
+  }
+}
+
+
+# a powershell script is able to determine its own path
+# the prefix is two levels up from the package specific share directory
+$env:COLCON_CURRENT_PREFIX=(Get-Item $PSCommandPath).Directory.Parent.Parent.FullName
+
+colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX/share/arm_control/hook/pythonpath.ps1"
+colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX/share/arm_control/hook/ament_prefix_path.ps1"
+
+Remove-Item Env:\COLCON_CURRENT_PREFIX

install/arm_control/share/arm_control/package.sh

@@ -0,0 +1,87 @@
+# generated from colcon_core/shell/template/package.sh.em
+
+# This script extends the environment for this package.
+
+# function to prepend a value to a variable
+# which uses colons as separators
+# duplicates as well as trailing separators are avoided
+# first argument: the name of the result variable
+# second argument: the value to be prepended
+_colcon_prepend_unique_value() {
+  # arguments
+  _listname="$1"
+  _value="$2"
+
+  # get values from variable
+  eval _values=\"\$$_listname\"
+  # backup the field separator
+  _colcon_prepend_unique_value_IFS=$IFS
+  IFS=":"
+  # start with the new value
+  _all_values="$_value"
+  # workaround SH_WORD_SPLIT not being set in zsh
+  if [ "$(command -v colcon_zsh_convert_to_array)" ]; then
+    colcon_zsh_convert_to_array _values
+  fi
+  # iterate over existing values in the variable
+  for _item in $_values; do
+    # ignore empty strings
+    if [ -z "$_item" ]; then
+      continue
+    fi
+    # ignore duplicates of _value
+    if [ "$_item" = "$_value" ]; then
+      continue
+    fi
+    # keep non-duplicate values
+    _all_values="$_all_values:$_item"
+  done
+  unset _item
+  # restore the field separator
+  IFS=$_colcon_prepend_unique_value_IFS
+  unset _colcon_prepend_unique_value_IFS
+  # export the updated variable
+  eval export $_listname=\"$_all_values\"
+  unset _all_values
+  unset _values
+
+  unset _value
+  unset _listname
+}
+
+# since a plain shell script can't determine its own path when being sourced
+# either use the provided COLCON_CURRENT_PREFIX
+# or fall back to the build time prefix (if it exists)
+_colcon_package_sh_COLCON_CURRENT_PREFIX="/home/cc/ros2/install/arm_control"
+if [ -z "$COLCON_CURRENT_PREFIX" ]; then
+  if [ ! -d "$_colcon_package_sh_COLCON_CURRENT_PREFIX" ]; then
+    echo "The build time path \"$_colcon_package_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2
+    unset _colcon_package_sh_COLCON_CURRENT_PREFIX
+    return 1
+  fi
+  COLCON_CURRENT_PREFIX="$_colcon_package_sh_COLCON_CURRENT_PREFIX"
+fi
+unset _colcon_package_sh_COLCON_CURRENT_PREFIX
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+# additional arguments: arguments to the script
+_colcon_package_sh_source_script() {
+  if [ -f "$1" ]; then
+    if [ -n "$COLCON_TRACE" ]; then
+      echo "# . \"$1\""
+    fi
+    . "$@"
+  else
+    echo "not found: \"$1\"" 1>&2
+  fi
+}
+
+# source sh hooks
+_colcon_package_sh_source_script "$COLCON_CURRENT_PREFIX/share/arm_control/hook/pythonpath.sh"
+_colcon_package_sh_source_script "$COLCON_CURRENT_PREFIX/share/arm_control/hook/ament_prefix_path.sh"
+
+unset _colcon_package_sh_source_script
+unset COLCON_CURRENT_PREFIX
+
+# do not unset _colcon_prepend_unique_value since it might be used by non-primary shell hooks

install/arm_control/share/arm_control/package.xml

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>arm_control</name>
+  <version>0.1.0</version>
+  <description>Control nodes for Flexiv Rizon 4 mock and ros2_control.</description>
+  <maintainer email="dev@example.com">arm_dev</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>trajectory_msgs</depend>
+  <depend>control_msgs</depend>
+  <depend>arm_msgs</depend>
+  <depend>tf2_ros</depend>
+  <exec_depend>controller_manager</exec_depend>
+  <exec_depend>joint_trajectory_controller</exec_depend>
+  <exec_depend>joint_state_broadcaster</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>ros2_control</exec_depend>
+  <exec_depend>ros2_controllers</exec_depend>
+  <exec_depend>xacro</exec_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

install/arm_control/share/arm_control/package.zsh

@@ -0,0 +1,42 @@
+# generated from colcon_zsh/shell/template/package.zsh.em
+
+# This script extends the environment for this package.
+
+# a zsh script is able to determine its own path if necessary
+if [ -z "$COLCON_CURRENT_PREFIX" ]; then
+  # the prefix is two levels up from the package specific share directory
+  _colcon_package_zsh_COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`/../.." > /dev/null && pwd)"
+else
+  _colcon_package_zsh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
+fi
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+# additional arguments: arguments to the script
+_colcon_package_zsh_source_script() {
+  if [ -f "$1" ]; then
+    if [ -n "$COLCON_TRACE" ]; then
+      echo "# . \"$1\""
+    fi
+    . "$@"
+  else
+    echo "not found: \"$1\"" 1>&2
+  fi
+}
+
+# function to convert array-like strings into arrays
+# to workaround SH_WORD_SPLIT not being set
+colcon_zsh_convert_to_array() {
+  local _listname=$1
+  local _dollar="$"
+  local _split="{="
+  local _to_array="(\"$_dollar$_split$_listname}\")"
+  eval $_listname=$_to_array
+}
+
+# source sh script of this package
+_colcon_package_zsh_source_script "$_colcon_package_zsh_COLCON_CURRENT_PREFIX/share/arm_control/package.sh"
+unset convert_zsh_to_array
+
+unset _colcon_package_zsh_source_script
+unset _colcon_package_zsh_COLCON_CURRENT_PREFIX

install/arm_control/share/colcon-core/packages/arm_control

@@ -0,0 +1 @@
+arm_msgs:control_msgs:controller_manager:geometry_msgs:joint_state_broadcaster:joint_trajectory_controller:rclpy:robot_state_publisher:ros2_control:ros2_controllers:sensor_msgs:std_msgs:tf2_ros:trajectory_msgs:xacro