克隆后还缺仿真安装树和两边的编译目录。把 build、build_sim、install_sim 一并放进仓库；仿真入口不再写死本机 conda，build 里指向源码的链接改成相对路径。

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

install_sim/arm_control/lib/arm_control/arm_status_node

@@ -0,0 +1,33 @@
+#!/usr/bin/env python3
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm_control==0.1.0','console_scripts','arm_status_node'
+import re
+import sys
+
+# for compatibility with easy_install; see #2198
+__requires__ = 'arm_control==0.1.0'
+
+try:
+    from importlib.metadata import distribution
+except ImportError:
+    try:
+        from importlib_metadata import distribution
+    except ImportError:
+        from pkg_resources import load_entry_point
+
+
+def importlib_load_entry_point(spec, group, name):
+    dist_name, _, _ = spec.partition('==')
+    matches = (
+        entry_point
+        for entry_point in distribution(dist_name).entry_points
+        if entry_point.group == group and entry_point.name == name
+    )
+    return next(matches).load()
+
+
+globals().setdefault('load_entry_point', importlib_load_entry_point)
+
+
+if __name__ == '__main__':
+    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
+    sys.exit(load_entry_point('arm_control==0.1.0', 'console_scripts', 'arm_status_node')())

install_sim/arm_control/lib/arm_control/mock_arm_driver

@@ -0,0 +1,33 @@
+#!/usr/bin/env python3
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm_control==0.1.0','console_scripts','mock_arm_driver'
+import re
+import sys
+
+# for compatibility with easy_install; see #2198
+__requires__ = 'arm_control==0.1.0'
+
+try:
+    from importlib.metadata import distribution
+except ImportError:
+    try:
+        from importlib_metadata import distribution
+    except ImportError:
+        from pkg_resources import load_entry_point
+
+
+def importlib_load_entry_point(spec, group, name):
+    dist_name, _, _ = spec.partition('==')
+    matches = (
+        entry_point
+        for entry_point in distribution(dist_name).entry_points
+        if entry_point.group == group and entry_point.name == name
+    )
+    return next(matches).load()
+
+
+globals().setdefault('load_entry_point', importlib_load_entry_point)
+
+
+if __name__ == '__main__':
+    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
+    sys.exit(load_entry_point('arm_control==0.1.0', 'console_scripts', 'mock_arm_driver')())

install_sim/arm_control/lib/arm_control/named_pose_server

@@ -0,0 +1,33 @@
+#!/usr/bin/env python3
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm_control==0.1.0','console_scripts','named_pose_server'
+import re
+import sys
+
+# for compatibility with easy_install; see #2198
+__requires__ = 'arm_control==0.1.0'
+
+try:
+    from importlib.metadata import distribution
+except ImportError:
+    try:
+        from importlib_metadata import distribution
+    except ImportError:
+        from pkg_resources import load_entry_point
+
+
+def importlib_load_entry_point(spec, group, name):
+    dist_name, _, _ = spec.partition('==')
+    matches = (
+        entry_point
+        for entry_point in distribution(dist_name).entry_points
+        if entry_point.group == group and entry_point.name == name
+    )
+    return next(matches).load()
+
+
+globals().setdefault('load_entry_point', importlib_load_entry_point)
+
+
+if __name__ == '__main__':
+    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
+    sys.exit(load_entry_point('arm_control==0.1.0', 'console_scripts', 'named_pose_server')())

install_sim/arm_control/lib/arm_control/real_trajectory_adapter

@@ -0,0 +1,33 @@
+#!/usr/bin/env python3
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm_control==0.1.0','console_scripts','real_trajectory_adapter'
+import re
+import sys
+
+# for compatibility with easy_install; see #2198
+__requires__ = 'arm_control==0.1.0'
+
+try:
+    from importlib.metadata import distribution
+except ImportError:
+    try:
+        from importlib_metadata import distribution
+    except ImportError:
+        from pkg_resources import load_entry_point
+
+
+def importlib_load_entry_point(spec, group, name):
+    dist_name, _, _ = spec.partition('==')
+    matches = (
+        entry_point
+        for entry_point in distribution(dist_name).entry_points
+        if entry_point.group == group and entry_point.name == name
+    )
+    return next(matches).load()
+
+
+globals().setdefault('load_entry_point', importlib_load_entry_point)
+
+
+if __name__ == '__main__':
+    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
+    sys.exit(load_entry_point('arm_control==0.1.0', 'console_scripts', 'real_trajectory_adapter')())

install_sim/arm_control/lib/arm_control/trajectory_bridge

@@ -0,0 +1,33 @@
+#!/usr/bin/env python3
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm_control==0.1.0','console_scripts','trajectory_bridge'
+import re
+import sys
+
+# for compatibility with easy_install; see #2198
+__requires__ = 'arm_control==0.1.0'
+
+try:
+    from importlib.metadata import distribution
+except ImportError:
+    try:
+        from importlib_metadata import distribution
+    except ImportError:
+        from pkg_resources import load_entry_point
+
+
+def importlib_load_entry_point(spec, group, name):
+    dist_name, _, _ = spec.partition('==')
+    matches = (
+        entry_point
+        for entry_point in distribution(dist_name).entry_points
+        if entry_point.group == group and entry_point.name == name
+    )
+    return next(matches).load()
+
+
+globals().setdefault('load_entry_point', importlib_load_entry_point)
+
+
+if __name__ == '__main__':
+    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
+    sys.exit(load_entry_point('arm_control==0.1.0', 'console_scripts', 'trajectory_bridge')())

install_sim/arm_control/lib/python3.11/site-packages/arm_control-0.1.0-py3.11.egg-info/PKG-INFO

@@ -0,0 +1,13 @@
+Metadata-Version: 2.4
+Name: arm_control
+Version: 0.1.0
+Summary: Arm control nodes for mock and real hardware.
+Maintainer: arm_dev
+Maintainer-email: dev@example.com
+License: Apache-2.0
+Requires-Dist: setuptools
+Dynamic: license
+Dynamic: maintainer
+Dynamic: maintainer-email
+Dynamic: requires-dist
+Dynamic: summary

install_sim/arm_control/lib/python3.11/site-packages/arm_control-0.1.0-py3.11.egg-info/SOURCES.txt

@@ -0,0 +1,23 @@
+package.xml
+setup.cfg
+setup.py
+../../build_sim/arm_control/arm_control.egg-info/PKG-INFO
+../../build_sim/arm_control/arm_control.egg-info/SOURCES.txt
+../../build_sim/arm_control/arm_control.egg-info/dependency_links.txt
+../../build_sim/arm_control/arm_control.egg-info/entry_points.txt
+../../build_sim/arm_control/arm_control.egg-info/requires.txt
+../../build_sim/arm_control/arm_control.egg-info/top_level.txt
+../../build_sim/arm_control/arm_control.egg-info/zip-safe
+arm_control/__init__.py
+arm_control/arm_status_node.py
+arm_control/mock_arm_driver.py
+arm_control/named_pose_server.py
+arm_control/real_trajectory_adapter.py
+arm_control/trajectory_bridge.py
+config/controllers.yaml
+config/named_poses.yaml
+config/named_poses_89.yaml
+config/named_poses_91.yaml
+launch/mock_control.launch.py
+launch/ros2_control.launch.py
+resource/arm_control

install_sim/arm_control/lib/python3.11/site-packages/arm_control-0.1.0-py3.11.egg-info/dependency_links.txt

@@ -0,0 +1 @@
+

install_sim/arm_control/lib/python3.11/site-packages/arm_control-0.1.0-py3.11.egg-info/entry_points.txt

@@ -0,0 +1,6 @@
+[console_scripts]
+arm_status_node = arm_control.arm_status_node:main
+mock_arm_driver = arm_control.mock_arm_driver:main
+named_pose_server = arm_control.named_pose_server:main
+real_trajectory_adapter = arm_control.real_trajectory_adapter:main
+trajectory_bridge = arm_control.trajectory_bridge:main

install_sim/arm_control/lib/python3.11/site-packages/arm_control-0.1.0-py3.11.egg-info/requires.txt

@@ -0,0 +1 @@
+setuptools

install_sim/arm_control/lib/python3.11/site-packages/arm_control-0.1.0-py3.11.egg-info/top_level.txt

@@ -0,0 +1 @@
+arm_control

install_sim/arm_control/lib/python3.11/site-packages/arm_control-0.1.0-py3.11.egg-info/zip-safe

@@ -0,0 +1 @@
+

install_sim/arm_control/lib/python3.11/site-packages/arm_control/__init__.py

@@ -0,0 +1 @@
+"""arm_control 的包初始化。"""

install_sim/arm_control/lib/python3.11/site-packages/arm_control/arm_status_node.py

@@ -0,0 +1,124 @@
+#!/usr/bin/env python3
+"""根据 joint_states、TF 与 Flexiv 的 tcp_pose 发布 ArmStatus，并提供 GetArmState 服务。"""
+
+from __future__ import annotations
+
+from typing import Optional
+
+import rclpy
+from geometry_msgs.msg import PoseStamped
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+from tf2_ros import Buffer, TransformListener
+
+from arm_msgs.msg import ArmStatus
+from arm_msgs.srv import GetArmState
+
+
+class ArmStatusNode(Node):
+    def __init__(self) -> None:
+        super().__init__('arm_status_node')
+        self.declare_parameter('base_frame', 'base_link')
+        self.declare_parameter('ee_frame', 'tcp')
+        self.declare_parameter('tcp_pose_topic', '')
+        self.declare_parameter('publish_rate', 20.0)
+
+        self.base_frame = str(self.get_parameter('base_frame').value)
+        self.ee_frame = str(self.get_parameter('ee_frame').value)
+        self.tcp_pose_topic = str(self.get_parameter('tcp_pose_topic').value).strip()
+        self._last_js: Optional[JointState] = None
+        self._last_tcp: Optional[PoseStamped] = None
+        self._mode = 'idle'
+
+        self._tf = Buffer()
+        TransformListener(self._tf, self)
+        self._pub = self.create_publisher(ArmStatus, 'arm_status', 10)
+        self.create_subscription(JointState, 'joint_states', self._on_js, 10)
+        if self.tcp_pose_topic:
+            self.create_subscription(
+                PoseStamped, self.tcp_pose_topic, self._on_tcp, 10
+            )
+        self.create_service(GetArmState, 'get_arm_state', self._on_get)
+        rate = float(self.get_parameter('publish_rate').value)
+        self.create_timer(1.0 / rate, self._tick)
+        self.get_logger().info(
+            f'ArmStatus: base={self.base_frame} ee={self.ee_frame} '
+            f'tcp_topic={self.tcp_pose_topic or "（无）"}'
+        )
+
+    def _on_js(self, msg: JointState) -> None:
+        self._last_js = msg
+        moving = any(abs(v) > 1e-3 for v in msg.velocity) if msg.velocity else False
+        self._mode = 'moving' if moving else 'idle'
+
+    def _on_tcp(self, msg: PoseStamped) -> None:
+        self._last_tcp = msg
+
+    def _ee(self) -> Optional[PoseStamped]:
+        # 优先用 Flexiv 发布的 TCP 位姿（真机栈上更可靠）
+        if self._last_tcp is not None:
+            return self._last_tcp
+        try:
+            tf = self._tf.lookup_transform(
+                self.base_frame, self.ee_frame, rclpy.time.Time()
+            )
+        except Exception:  # noqa: BLE001
+            return None
+        pose = PoseStamped()
+        pose.header = tf.header
+        pose.pose.position.x = tf.transform.translation.x
+        pose.pose.position.y = tf.transform.translation.y
+        pose.pose.position.z = tf.transform.translation.z
+        pose.pose.orientation = tf.transform.rotation
+        return pose
+
+    def _build(self) -> ArmStatus:
+        s = ArmStatus()
+        s.header.stamp = self.get_clock().now().to_msg()
+        s.header.frame_id = self.base_frame
+        s.mode = self._mode
+        s.is_moving = self._mode == 'moving'
+        s.is_homed = True
+        if self._last_js is not None:
+            s.joint_positions = list(self._last_js.position)
+            s.joint_velocities = list(self._last_js.velocity)
+        ee = self._ee()
+        if ee is not None:
+            s.ee_pose = ee
+        return s
+
+    def _tick(self) -> None:
+        self._pub.publish(self._build())
+
+    def _on_get(self, _req, response):
+        s = self._build()
+        response.success = True
+        response.mode = s.mode
+        response.joint_positions = s.joint_positions
+        response.ee_pose = s.ee_pose
+        if (
+            abs(s.ee_pose.pose.position.x) < 1e-9
+            and abs(s.ee_pose.pose.position.y) < 1e-9
+            and abs(s.ee_pose.pose.position.z) < 1e-9
+        ):
+            response.message = 'ok（末端位姿不可用 —— 请检查 TF/tcp_pose）'
+        else:
+            response.message = 'ok'
+        return response
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = ArmStatusNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

install_sim/arm_control/lib/python3.11/site-packages/arm_control/mock_arm_driver.py

@@ -0,0 +1,172 @@
+#!/usr/bin/env python3
+"""Mock 7-DOF Rizon4 driver: joint_states + FollowJointTrajectory + JointCommand."""
+
+from __future__ import annotations
+
+import time
+from typing import List
+
+import rclpy
+from rclpy.action import ActionServer, CancelResponse, GoalResponse
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.executors import MultiThreadedExecutor
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+
+from arm_msgs.action import FollowJointTrajectory
+from arm_msgs.msg import JointCommand
+
+DEFAULT_JOINTS = [
+    'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7',
+]
+
+
+class MockArmDriver(Node):
+    def __init__(self) -> None:
+        super().__init__('mock_arm_driver')
+        self.declare_parameter('joint_names', DEFAULT_JOINTS)
+        self.declare_parameter('joint_prefix', '')
+        self.declare_parameter('publish_rate', 50.0)
+        self.declare_parameter(
+            'default_positions', [0.0, -0.698, 0.0, 1.57, 0.0, 0.698, 0.0]
+        )
+
+        prefix = str(self.get_parameter('joint_prefix').value or '')
+        base = list(
+            self.get_parameter('joint_names').get_parameter_value().string_array_value
+            or DEFAULT_JOINTS
+        )
+        self.joint_names: List[str] = [f'{prefix}{n}' for n in base]
+        rate = float(self.get_parameter('publish_rate').value)
+        defaults = list(
+            self.get_parameter('default_positions').get_parameter_value().double_array_value
+            or [0.0] * len(self.joint_names)
+        )
+        while len(defaults) < len(self.joint_names):
+            defaults.append(0.0)
+
+        self.positions = defaults[: len(self.joint_names)]
+        self.velocities = [0.0] * len(self.joint_names)
+        self._busy = False
+        self._cb = ReentrantCallbackGroup()
+
+        self._js_pub = self.create_publisher(JointState, 'joint_states', 10)
+        self.create_subscription(JointCommand, 'joint_command', self._on_cmd, 10)
+        self.create_timer(1.0 / rate, self._publish)
+
+        ActionServer(
+            self,
+            FollowJointTrajectory,
+            'follow_joint_trajectory',
+            execute_callback=self._execute,
+            goal_callback=self._goal_cb,
+            cancel_callback=self._cancel_cb,
+            callback_group=self._cb,
+        )
+        self.get_logger().info(f'Mock Rizon4 ready: {self.joint_names}')
+
+    def _goal_cb(self, _req):
+        if self._busy:
+            self.get_logger().warn('拒绝：上一个目标还在执行')
+            return GoalResponse.REJECT
+        return GoalResponse.ACCEPT
+
+    def _cancel_cb(self, _gh):
+        return CancelResponse.ACCEPT
+
+    def _on_cmd(self, msg: JointCommand) -> None:
+        if self._busy:
+            return
+        for i, name in enumerate(msg.joint_names):
+            if name in self.joint_names and i < len(msg.positions):
+                idx = self.joint_names.index(name)
+                self.positions[idx] = float(msg.positions[i])
+                self.velocities[idx] = 0.0
+
+    def _publish(self) -> None:
+        msg = JointState()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.name = self.joint_names
+        msg.position = list(self.positions)
+        msg.velocity = list(self.velocities)
+        self._js_pub.publish(msg)
+
+    def _execute(self, goal_handle):
+        self._busy = True
+        goal = goal_handle.request
+        result = FollowJointTrajectory.Result()
+        feedback = FollowJointTrajectory.Feedback()
+
+        n_j = len(goal.joint_names)
+        n_p = int(goal.num_points)
+        if n_j == 0 or n_p == 0 or len(goal.positions) < n_j * n_p:
+            result.success = False
+            result.message = 'Invalid / empty trajectory'
+            goal_handle.abort()
+            self._busy = False
+            return result
+
+        start = time.monotonic()
+        times = list(goal.time_from_start) if goal.time_from_start else [
+            (i + 1) * 0.5 for i in range(n_p)
+        ]
+        try:
+            for p in range(n_p):
+                if goal_handle.is_cancel_requested:
+                    goal_handle.canceled()
+                    result.success = False
+                    result.message = 'Canceled'
+                    result.final_positions = list(self.positions)
+                    self._busy = False
+                    return result
+                target = goal.positions[p * n_j:(p + 1) * n_j]
+                deadline = start + times[p]
+                while time.monotonic() < deadline:
+                    for name, tval in zip(goal.joint_names, target):
+                        if name not in self.joint_names:
+                            continue
+                        idx = self.joint_names.index(name)
+                        err = tval - self.positions[idx]
+                        self.velocities[idx] = err * 5.0
+                        self.positions[idx] += 0.15 * err
+                    feedback.current_point = p
+                    feedback.progress = (p + 1) / n_p
+                    feedback.status = 'moving'
+                    goal_handle.publish_feedback(feedback)
+                    time.sleep(0.02)
+                for name, tval in zip(goal.joint_names, target):
+                    if name in self.joint_names:
+                        self.positions[self.joint_names.index(name)] = tval
+                self.velocities = [0.0] * len(self.joint_names)
+
+            goal_handle.succeed()
+            result.success = True
+            result.message = 'Trajectory completed'
+            result.final_positions = list(self.positions)
+        except Exception as exc:  # noqa: BLE001
+            result.success = False
+            result.message = str(exc)
+            goal_handle.abort()
+        finally:
+            self._busy = False
+            self.velocities = [0.0] * len(self.joint_names)
+        return result
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = MockArmDriver()
+    executor = MultiThreadedExecutor()
+    executor.add_node(node)
+    try:
+        executor.spin()
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

install_sim/arm_control/lib/python3.11/site-packages/arm_control/named_pose_server.py

@@ -0,0 +1,134 @@
+#!/usr/bin/env python3
+"""服务端：SetNamedPose -> FollowJointTrajectory（与底层后端无关）。"""
+
+from __future__ import annotations
+
+import time
+from typing import Dict, List
+
+import rclpy
+from rclpy.action import ActionClient
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.executors import MultiThreadedExecutor
+from rclpy.node import Node
+
+from arm_msgs.action import FollowJointTrajectory
+from arm_msgs.srv import SetNamedPose
+
+DEFAULT_JOINTS = [
+    'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7',
+]
+DEFAULT_POSES: Dict[str, List[float]] = {
+    '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],
+}
+
+
+def _wait_future(future, timeout_sec: float) -> bool:
+    deadline = time.monotonic() + timeout_sec
+    while not future.done():
+        if time.monotonic() > deadline:
+            return False
+        time.sleep(0.01)
+    return True
+
+
+class NamedPoseServer(Node):
+    def __init__(self) -> None:
+        super().__init__('named_pose_server')
+        self.declare_parameter('joint_names', DEFAULT_JOINTS)
+        self.declare_parameter('joint_prefix', '')
+        for name, vals in DEFAULT_POSES.items():
+            self.declare_parameter(f'named_poses.{name}', vals)
+
+        prefix = str(self.get_parameter('joint_prefix').value or '')
+        base = list(
+            self.get_parameter('joint_names').get_parameter_value().string_array_value
+            or DEFAULT_JOINTS
+        )
+        self.joint_names = [f'{prefix}{n}' for n in base]
+        self.poses = {
+            name: list(self.get_parameter(f'named_poses.{name}').value)
+            for name in DEFAULT_POSES
+        }
+
+        self._cb = ReentrantCallbackGroup()
+        self._client = ActionClient(
+            self,
+            FollowJointTrajectory,
+            'follow_joint_trajectory',
+            callback_group=self._cb,
+        )
+        self.create_service(
+            SetNamedPose, 'set_named_pose', self._on_set, callback_group=self._cb
+        )
+        self.get_logger().info(
+            f'命名位姿 {list(self.poses)} joints={self.joint_names}'
+        )
+
+    def _on_set(self, request: SetNamedPose.Request, response: SetNamedPose.Response):
+        name = request.pose_name
+        if name not in self.poses:
+            response.success = False
+            response.message = f'未知位姿"{name}"。可用: {list(self.poses)}'
+            return response
+        if not self._client.wait_for_server(timeout_sec=3.0):
+            response.success = False
+            response.message = 'follow_joint_trajectory action 不可用'
+            return response
+
+        goal = FollowJointTrajectory.Goal()
+        goal.joint_names = self.joint_names
+        goal.num_points = 1
+        goal.positions = list(self.poses[name])
+        duration = max(0.5, 1.0 / max(float(request.velocity_scale), 0.1))
+        goal.time_from_start = [duration]
+        goal.velocity_scale = float(request.velocity_scale)
+
+        self.get_logger().info(f'-> 位姿 [{name}] T={duration:.2f}s')
+        send_fut = self._client.send_goal_async(goal)
+        if not _wait_future(send_fut, 5.0):
+            response.success = False
+            response.message = '等待目标被接受超时'
+            return response
+        handle = send_fut.result()
+        if handle is None or not handle.accepted:
+            response.success = False
+            response.message = '目标被拒绝'
+            return response
+        res_fut = handle.get_result_async()
+        if not _wait_future(res_fut, 60.0):
+            response.success = False
+            response.message = '等待轨迹结果超时'
+            return response
+        wrapped = res_fut.result()
+        if wrapped is None:
+            response.success = False
+            response.message = '结果为空'
+            return response
+        result = wrapped.result
+        response.success = bool(result.success)
+        response.message = result.message or f'已移动到位姿 {name}'
+        return response
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = NamedPoseServer()
+    executor = MultiThreadedExecutor(num_threads=4)
+    executor.add_node(node)
+    try:
+        executor.spin()
+    except KeyboardInterrupt:
+        pass
+    finally:
+        executor.shutdown()
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

install_sim/arm_control/lib/python3.11/site-packages/arm_control/real_trajectory_adapter.py

@@ -0,0 +1,136 @@
+#!/usr/bin/env python3
+"""对应用侧暴露 arm_msgs/FollowJointTrajectory；实际驱动 Flexiv JTC（control_msgs）。"""
+
+from __future__ import annotations
+
+import time
+
+import rclpy
+from control_msgs.action import FollowJointTrajectory as CtrlFJT
+from rclpy.action import ActionClient, ActionServer, CancelResponse, GoalResponse
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.executors import MultiThreadedExecutor
+from rclpy.node import Node
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+
+from arm_msgs.action import FollowJointTrajectory as ArmFJT
+
+
+def _wait(fut, timeout: float) -> bool:
+    end = time.monotonic() + timeout
+    while not fut.done():
+        if time.monotonic() > end:
+            return False
+        time.sleep(0.01)
+    return True
+
+
+class RealTrajectoryAdapter(Node):
+    def __init__(self) -> None:
+        super().__init__('real_trajectory_adapter')
+        self.declare_parameter('arm_action', 'follow_joint_trajectory')
+        self.declare_parameter(
+            'control_action', 'rizon_arm_controller/follow_joint_trajectory'
+        )
+        arm = str(self.get_parameter('arm_action').value)
+        ctrl = str(self.get_parameter('control_action').value)
+        self._cb = ReentrantCallbackGroup()
+        self._ctrl = ActionClient(self, CtrlFJT, ctrl, callback_group=self._cb)
+        ActionServer(
+            self,
+            ArmFJT,
+            arm,
+            execute_callback=self._execute,
+            goal_callback=lambda _g: GoalResponse.ACCEPT,
+            cancel_callback=lambda _g: CancelResponse.ACCEPT,
+            callback_group=self._cb,
+        )
+        self.get_logger().info(f'真机适配器 {arm} -> {ctrl}')
+
+    def _execute(self, goal_handle):
+        result = ArmFJT.Result()
+        req = goal_handle.request
+        if not req.joint_names or req.num_points < 1:
+            result.success = False
+            result.message = '目标非法'
+            goal_handle.abort()
+            return result
+        if not self._ctrl.wait_for_server(timeout_sec=5.0):
+            result.success = False
+            result.message = f'控制器 action 不可用'
+            goal_handle.abort()
+            return result
+
+        n_j = len(req.joint_names)
+        traj = JointTrajectory()
+        traj.joint_names = list(req.joint_names)
+        pos = list(req.positions)
+        times = list(req.time_from_start) or [1.0]
+        idx = 0
+        n_pts = min(int(req.num_points), len(times))
+        for i, t in enumerate(times[:n_pts]):
+            pt = JointTrajectoryPoint()
+            pt.positions = pos[idx : idx + n_j]
+            idx += n_j
+            # 点到点的命名位姿必须在静止状态下结束 —— 末速度非零
+            # 再叠加开环 JTC，会让机械臂到位后滑行/抖动。
+            pt.velocities = [0.0] * n_j
+            pt.accelerations = [0.0] * n_j
+            sec = int(t)
+            pt.time_from_start.sec = sec
+            pt.time_from_start.nanosec = int((t - sec) * 1e9)
+            traj.points.append(pt)
+
+        ctrl_goal = CtrlFJT.Goal()
+        ctrl_goal.trajectory = traj
+        send = self._ctrl.send_goal_async(ctrl_goal)
+        if not _wait(send, 5.0):
+            result.success = False
+            result.message = '发送给控制器超时'
+            goal_handle.abort()
+            return result
+        handle = send.result()
+        if handle is None or not handle.accepted:
+            result.success = False
+            result.message = '控制器拒绝了目标'
+            goal_handle.abort()
+            return result
+        traj_duration = 0.0
+        if traj.points:
+            last = traj.points[-1].time_from_start
+            traj_duration = last.sec + last.nanosec * 1e-9
+        res_fut = handle.get_result_async()
+        if not _wait(res_fut, max(120.0, traj_duration * 3.0)):
+            result.success = False
+            result.message = '等待控制器超时'
+            goal_handle.abort()
+            return result
+        ctrl_res = res_fut.result().result
+        ok = ctrl_res.error_code == CtrlFJT.Result.SUCCESSFUL
+        result.success = ok
+        result.message = 'OK' if ok else f'控制器 error_code={ctrl_res.error_code}'
+        if ok:
+            goal_handle.succeed()
+        else:
+            goal_handle.abort()
+        return result
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = RealTrajectoryAdapter()
+    ex = MultiThreadedExecutor(num_threads=4)
+    ex.add_node(node)
+    try:
+        ex.spin()
+    except KeyboardInterrupt:
+        pass
+    finally:
+        ex.shutdown()
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

install_sim/arm_control/lib/python3.11/site-packages/arm_control/trajectory_bridge.py

@@ -0,0 +1,116 @@
+#!/usr/bin/env python3
+"""把 control_msgs/FollowJointTrajectory 桥接到 arm_msgs/FollowJointTrajectory。"""
+
+from __future__ import annotations
+
+import time
+
+import rclpy
+from control_msgs.action import FollowJointTrajectory as CtrlFJT
+from rclpy.action import ActionClient, ActionServer, CancelResponse, GoalResponse
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.executors import MultiThreadedExecutor
+from rclpy.node import Node
+
+from arm_msgs.action import FollowJointTrajectory as ArmFJT
+
+
+def _wait(fut, timeout: float) -> bool:
+    end = time.monotonic() + timeout
+    while not fut.done():
+        if time.monotonic() > end:
+            return False
+        time.sleep(0.01)
+    return True
+
+
+class TrajectoryBridge(Node):
+    def __init__(self) -> None:
+        super().__init__('trajectory_bridge')
+        self.declare_parameter(
+            'control_action', 'rizon_arm_controller/follow_joint_trajectory'
+        )
+        self.declare_parameter('arm_action', 'follow_joint_trajectory')
+        ctrl = str(self.get_parameter('control_action').value)
+        arm = str(self.get_parameter('arm_action').value)
+        self._cb = ReentrantCallbackGroup()
+        self._arm = ActionClient(self, ArmFJT, arm, callback_group=self._cb)
+        ActionServer(
+            self,
+            CtrlFJT,
+            ctrl,
+            execute_callback=self._execute,
+            goal_callback=lambda _g: GoalResponse.ACCEPT,
+            cancel_callback=lambda _g: CancelResponse.ACCEPT,
+            callback_group=self._cb,
+        )
+        self.get_logger().info(f'桥接 {ctrl} -> {arm}')
+
+    def _execute(self, goal_handle):
+        result = CtrlFJT.Result()
+        traj = goal_handle.request.trajectory
+        if not traj.joint_names or not traj.points:
+            result.error_code = CtrlFJT.Result.INVALID_GOAL
+            goal_handle.abort()
+            return result
+        if not self._arm.wait_for_server(timeout_sec=3.0):
+            result.error_code = CtrlFJT.Result.INVALID_GOAL
+            goal_handle.abort()
+            return result
+
+        n_j = len(traj.joint_names)
+        goal = ArmFJT.Goal()
+        goal.joint_names = list(traj.joint_names)
+        goal.num_points = len(traj.points)
+        goal.velocity_scale = 1.0
+        positions = []
+        times = []
+        for pt in traj.points:
+            positions.extend(list(pt.positions)[:n_j])
+            t = pt.time_from_start.sec + pt.time_from_start.nanosec * 1e-9
+            times.append(max(t, 0.05))
+        goal.positions = positions
+        goal.time_from_start = times
+
+        send = self._arm.send_goal_async(goal)
+        if not _wait(send, 5.0):
+            result.error_code = CtrlFJT.Result.GOAL_TOLERANCE_VIOLATED
+            goal_handle.abort()
+            return result
+        handle = send.result()
+        if handle is None or not handle.accepted:
+            result.error_code = CtrlFJT.Result.INVALID_GOAL
+            goal_handle.abort()
+            return result
+        res_fut = handle.get_result_async()
+        if not _wait(res_fut, 120.0):
+            result.error_code = CtrlFJT.Result.GOAL_TOLERANCE_VIOLATED
+            goal_handle.abort()
+            return result
+        if res_fut.result().result.success:
+            result.error_code = CtrlFJT.Result.SUCCESSFUL
+            goal_handle.succeed()
+        else:
+            result.error_code = CtrlFJT.Result.GOAL_TOLERANCE_VIOLATED
+            goal_handle.abort()
+        return result
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = TrajectoryBridge()
+    ex = MultiThreadedExecutor(num_threads=4)
+    ex.add_node(node)
+    try:
+        ex.spin()
+    except KeyboardInterrupt:
+        pass
+    finally:
+        ex.shutdown()
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

install_sim/arm_control/share/ament_index/resource_index/packages/arm_control

@@ -0,0 +1 @@
+

install_sim/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_sim/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_sim/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_sim/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_sim/arm_control/share/arm_control/hook/ament_prefix_path.dsv

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

install_sim/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_sim/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_sim/arm_control/share/arm_control/hook/pythonpath.dsv

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

install_sim/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_sim/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_sim/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_sim/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_sim/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_sim/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_sim/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_sim/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_sim/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_sim/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_sim/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_sim/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