fix move arm issues

src/dual_arm_description/urdf/dual_arm.urdf

@@ -101,7 +101,7 @@
       lower="-3.14"
       upper="3.14"
       effort="30.0"
-      velocity="1.0" />
+      velocity="3.0" />
   </joint>
   <link
     name="left_arm_link_2">
@@ -159,7 +159,7 @@
       lower="-3.14"
       upper="3.14"
       effort="30.0"
-      velocity="1.0" />
+      velocity="3.0" />
   </joint>
   <link
     name="left_arm_link_3">
@@ -217,7 +217,7 @@
       lower="-3.14"
       upper="3.14"
       effort="30.0"
-      velocity="1.0" />
+      velocity="3.0" />
   </joint>
   <link
     name="left_arm_link_4">
@@ -275,7 +275,7 @@
       lower="-3.14"
       upper="3.14"
       effort="30.0"
-      velocity="1.0" />
+      velocity="3.0" />
   </joint>
   <link
     name="left_arm_link_5">
@@ -333,7 +333,7 @@
       lower="-3.14"
       upper="3.14"
       effort="30.0"
-      velocity="1.0" />
+      velocity="3.0" />
   </joint>
   <link
     name="left_arm_link_6">
@@ -391,7 +391,7 @@
       lower="-3.14"
       upper="3.14"
       effort="30.0"
-      velocity="1.0" />
+      velocity="3.0" />
   </joint>
   <link
     name="right_arm_link_1">
@@ -449,7 +449,7 @@
       lower="-3.14"
       upper="3.14"
       effort="30.0"
-      velocity="1.0" />
+      velocity="3.0" />
   </joint>
   <link
     name="right_arm_link_2">
@@ -507,7 +507,7 @@
       lower="-3.14"
       upper="3.14"
       effort="30.0"
-      velocity="1.0" />
+      velocity="3.0" />
   </joint>
   <link
     name="right_arm_link_3">
@@ -565,7 +565,7 @@
       lower="-3.14"
       upper="3.14"
       effort="30.0"
-      velocity="1.0" />
+      velocity="3.0" />
   </joint>
   <link
     name="right_arm_link_4">
@@ -623,7 +623,7 @@
       lower="-3.14"
       upper="3.14"
       effort="30.0"
-      velocity="1.0" />
+      velocity="3.0" />
   </joint>
   <link
     name="right_arm_link_5">
@@ -681,7 +681,7 @@
       lower="-3.14"
       upper="3.14"
       effort="30.0"
-      velocity="1.0" />
+      velocity="3.0" />
   </joint>
   <link
     name="right_arm_link_6">
@@ -739,6 +739,6 @@
       lower="-3.14"
       upper="3.14"
       effort="30.0"
-      velocity="1.0" />
+      velocity="3.0" />
   </joint>
 </robot>

src/dual_arm_moveit_config/config/dual_arm.srdf

@@ -51,7 +51,7 @@
         <joint name="right_arm_joint_6" value="0"/>
     </group_state>
     <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
-    <virtual_joint name="base_joint" type="fixed" parent_frame="world_frame" child_link="base_link"/>
+    <virtual_joint name="base_joint" type="fixed" parent_frame="world" child_link="base_link"/>
     <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
     <disable_collisions link1="base_link" link2="left_arm_link_1" reason="User"/>
     <disable_collisions link1="base_link" link2="left_arm_link_2" reason="User"/>

src/dual_arm_moveit_config/config/joint_limits.yaml

@@ -2,8 +2,8 @@
 
 # For beginners, we downscale velocity and acceleration limits.
 # You can always specify higher scaling factors (<= 1.0) in your motion requests.  # Increase the values below to 1.0 to always move at maximum speed.
-default_velocity_scaling_factor: 0.1
-default_acceleration_scaling_factor: 0.1
+default_velocity_scaling_factor: 0.5
+default_acceleration_scaling_factor: 0.5
 
 # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
 # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
@@ -11,60 +11,60 @@ joint_limits:
   left_arm_joint_1:
     has_velocity_limits: true
     max_velocity: 3.14
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 10.0
   left_arm_joint_2:
     has_velocity_limits: true
     max_velocity: 3.14
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 10.0
   left_arm_joint_3:
     has_velocity_limits: true
     max_velocity: 3.14
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 10.0
   left_arm_joint_4:
     has_velocity_limits: true
     max_velocity: 3.14
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 10.0
   left_arm_joint_5:
     has_velocity_limits: true
     max_velocity: 3.14
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 10.0
   left_arm_joint_6:
     has_velocity_limits: true
     max_velocity: 3.14
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 10.0
   right_arm_joint_1:
     has_velocity_limits: true
     max_velocity: 3.14
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 10.0
   right_arm_joint_2:
     has_velocity_limits: true
     max_velocity: 3.14
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 10.0
   right_arm_joint_3:
     has_velocity_limits: true
     max_velocity: 3.14
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 10.0
   right_arm_joint_4:
     has_velocity_limits: true
     max_velocity: 3.14
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 10.0
   right_arm_joint_5:
     has_velocity_limits: true
     max_velocity: 3.14
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 10.0
   right_arm_joint_6:
     has_velocity_limits: true
     max_velocity: 3.14
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 10.0

src/robot_control/CMakeLists.txt

@@ -1,6 +1,13 @@
 cmake_minimum_required(VERSION 3.16)  # 提升最低版本以匹配ROS 2推荐版本
 project(robot_control)
-
+set(ROS2_HUMBLE_PATH /home/nvidia/disk/ros/humble)
+# 手动指定 eigen_stl_containers 的cmake配置文件路径
+set(eigen_stl_containers_DIR ${ROS2_HUMBLE_PATH}/share/eigen_stl_containers/cmake)
+# 手动指定 eigen3 相关依赖路径 (moveit_core同时依赖这个)
+set(eigen3_cmake_module_DIR ${ROS2_HUMBLE_PATH}/share/eigen3_cmake_module/cmake)
+set(tf2_eigen_DIR ${ROS2_HUMBLE_PATH}/share/tf2_eigen/cmake)
+# 手动指定 moveit_core 路径（双重保险）
+set(moveit_core_DIR ${ROS2_HUMBLE_PATH}/share/moveit_core/cmake)
 # 设置 C++17 标准（std::filesystem 需要）
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
@@ -17,6 +24,7 @@ find_package(geometry_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(trajectory_msgs REQUIRED)
 find_package(control_msgs REQUIRED)
+find_package(controller_manager_msgs REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(action_msgs REQUIRED) 
 find_package(interfaces REQUIRED)
@@ -24,6 +32,15 @@ find_package(moveit_core REQUIRED)
 find_package(moveit_ros_planning_interface REQUIRED)
 find_package(moveit_msgs REQUIRED)
 
+find_package(eigen_stl_containers REQUIRED)
+find_package(eigen3_cmake_module REQUIRED)
+find_package(tf2_eigen REQUIRED)
+find_package(moveit_ros_planning REQUIRED)
+find_package(moveit_ros_planning_interface REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(moveit_msgs REQUIRED)
+
 # 头文件目录
 include_directories(
   include
@@ -62,6 +79,7 @@ ament_target_dependencies(robot_control_node
   sensor_msgs
   trajectory_msgs
   control_msgs
+  controller_manager_msgs
   rclcpp_action
   interfaces
   moveit_core

src/robot_control/include/controllers/arm_control.hpp

@@ -13,6 +13,7 @@
 #include <vector>
 #include <map>
 #include "geometry_msgs/msg/pose.hpp"
+#include "moveit/move_group_interface/move_group_interface.h"
 
 // 前向声明
 namespace rclcpp {
@@ -63,35 +64,39 @@ namespace robot_control
 
         /**
          * @brief 规划关节空间运动（MOVEJ）
+         *
+         * 使用 MoveIt 规划并将完整轨迹存入内部的 TrajectoryState，
+         * 上层通过 GetInterpolatedPoint/HasActiveTrajectory 获取插补点，无需关心具体轨迹点数组。
+         *
          * @param arm_id 手臂ID（0=左臂，1=右臂）
          * @param target_joints 目标关节角度（弧度）
          * @param velocity_scaling 速度缩放因子 [0,1]
          * @param acceleration_scaling 加速度缩放因子 [0,1]
-         * @param trajectory_points 输出：规划好的轨迹点（关节角度，弧度）
          * @return true 规划成功，false 规划失败
          */
         bool PlanJointMotion(
             uint8_t arm_id,
             const std::vector<double>& target_joints,
             double velocity_scaling,
-            double acceleration_scaling,
-            std::vector<std::vector<double>>& trajectory_points);
+            double acceleration_scaling);
 
         /**
          * @brief 规划笛卡尔空间运动（MOVEL）
+         *
+         * 使用 MoveIt 规划并将完整轨迹存入内部的 TrajectoryState，
+         * 上层通过 GetInterpolatedPoint/HasActiveTrajectory 获取插补点。
+         *
          * @param arm_id 手臂ID（0=左臂，1=右臂）
          * @param target_pose 目标位姿
          * @param velocity_scaling 速度缩放因子 [0,1]
          * @param acceleration_scaling 加速度缩放因子 [0,1]
-         * @param trajectory_points 输出：规划好的轨迹点（关节角度，弧度）
          * @return true 规划成功，false 规划失败
          */
         bool PlanCartesianMotion(
             uint8_t arm_id,
             const geometry_msgs::msg::Pose& target_pose,
             double velocity_scaling,
-            double acceleration_scaling,
-            std::vector<std::vector<double>>& trajectory_points);
+            double acceleration_scaling);
 
         /**
          * @brief 检查MoveIt是否已初始化
@@ -133,13 +138,6 @@ namespace robot_control
         bool Stop(std::vector<double>& joint_positions, double dt);
 
         // ==================== MoveIt特定功能 ====================
-        
-        /**
-         * @brief 存储规划好的轨迹
-         * @param trajectory_points 轨迹点（关节角度，弧度）
-         * @param arm_id 手臂ID（0=左臂，1=右臂）
-         */
-        void StoreTrajectory(const std::vector<std::vector<double>>& trajectory_points, uint8_t arm_id);
 
         /**
          * @brief 获取当前插补点位（根据时间周期获取下一个点位）
@@ -157,6 +155,21 @@ namespace robot_control
          */
         bool HasActiveTrajectory(uint8_t arm_id) const;
 
+        /**
+         * @brief 获取该臂当前缓存轨迹的末端关节位置（用于判断是否到达目标）
+         * @param arm_id 手臂ID（0=左臂，1=右臂）
+         * @param end_positions 输出：末端点关节位置（弧度）
+         * @return true 获取成功，false 无轨迹或参数错误
+         */
+        bool GetTrajectoryEndPositions(uint8_t arm_id, std::vector<double>& end_positions) const;
+
+        /**
+         * @brief 获取该臂当前轨迹的时间进度 [0,1]
+         * @param arm_id 手臂ID（0=左臂，1=右臂）
+         * @return 进度（无轨迹返回 1.0；有轨迹按 elapsed_time/total_time 计算并 clamp 到 [0,1]）
+         */
+        double GetTrajectoryProgress(uint8_t arm_id) const;
+
         /**
          * @brief 清除轨迹
          * @param arm_id 手臂ID（0=左臂，1=右臂）
@@ -210,5 +223,45 @@ namespace robot_control
         };
         TrajectoryState left_arm_trajectory_;   ///< 左臂轨迹状态
         TrajectoryState right_arm_trajectory_;  ///< 右臂轨迹状态
+
+        /**
+         * @brief 对 MoveIt 规划结果进行时间参数化 + 固定 dt 重采样，并写入对应臂的 TrajectoryState
+         * @param arm_id 手臂ID（0=左臂，1=右臂）
+         * @param move_group 对应 MoveGroupInterface
+         * @param plan MoveIt 规划结果（会被重采样后的轨迹覆盖）
+         * @param velocity_scaling 速度缩放因子
+         * @param acceleration_scaling 加速度缩放因子
+         * @return true 成功
+         */
+        bool timeParameterizeResampleAndStore_(
+            uint8_t arm_id,
+            moveit::planning_interface::MoveGroupInterface& move_group,
+            moveit::planning_interface::MoveGroupInterface::Plan& plan,
+            double velocity_scaling,
+            double acceleration_scaling);
+
+        /**
+         * @brief 辅助函数：更新 joints_info_map_ 从单个轨迹点
+         * @param arm_id 手臂ID（0=左臂，1=右臂）
+         * @param point 轨迹点（仅包含该臂的自由度）
+         */
+        void UpdateJointsInfo(uint8_t arm_id, const TrajectoryPoint& point);
+        
+        /**
+         * @brief 辅助函数：更新 joints_info_map_ 从插补后的值
+         * @param arm_id 手臂ID（0=左臂，1=右臂）
+         * @param p1 起始轨迹点
+         * @param p2 结束轨迹点
+         * @param alpha 插补系数 [0, 1]
+         * @param num_joints 关节数量（该臂的自由度）
+         * @param joint_positions 插补后的关节位置
+         */
+        void UpdateJointsInfoInterpolated(
+            uint8_t arm_id,
+            const TrajectoryPoint& p1, 
+            const TrajectoryPoint& p2, 
+            double alpha,
+            size_t num_joints,
+            const std::vector<double>& joint_positions);
     };
 }

src/robot_control/include/controllers/control_base.hpp

@@ -16,6 +16,7 @@
 #include <vector>
 #include <memory>
 #include <string>
+#include <map>
 #include "utils/trapezoidal_trajectory.hpp"
 #include "utils/s_curve_trajectory.hpp"
 #include "core/motion_parameters.hpp"
@@ -134,7 +135,8 @@ namespace robot_control
     protected:
         size_t total_joints_;  ///< 关节总数
 
-        std::vector<JointInfo> joints_info_;        ///< 关节信息列表（包含限位、速度、加速度、home位置、当前状态、指令等）
+        std::vector<std::string> joint_names_;      ///< 关节名称列表（保持顺序）
+        std::map<std::string, JointInfo> joints_info_map_;  ///< 关节信息字典（包含限位、速度、加速度、home位置、当前状态、指令等）
         
         // 通用参数（所有控制器共享）
         std::vector<double> lengthParameters_;      ///< 长度参数
@@ -155,7 +157,7 @@ namespace robot_control
 
     private:
         /**
-         * @brief 从 joints_info_ 提取当前位置向量（供轨迹规划器使用）
+         * @brief 从 joints_info_map_ 提取当前位置向量（供轨迹规划器使用）
          */
         std::vector<double> getCurrentPositions() const;
 

src/robot_control/include/core/motion_parameters.hpp

@@ -22,7 +22,7 @@
 
 // ==================== 宏定义 ====================
 
-#define CYCLE_TIME 8    ///< 插补周期（毫秒）
+#define CYCLE_TIME 4    ///< 插补周期（毫秒）
 
 #ifndef DEG2RAD
 #define DEG2RAD(x) ((x)*0.017453293)  ///< 度转弧度

src/robot_control/include/core/robot_control_manager.hpp

@@ -156,24 +156,38 @@ namespace robot_control
         /**
          * @brief 设置手臂关节命令（直接设置关节角度，通过topic发布）
          * @param body_id 手臂ID（0=左臂，1=右臂）
-         * @param joint_angles 关节角度数组（度），大小为6
+         * @param joint_angles 关节角度数组（弧度），大小为 arm_dof_
          * @return true 设置成功，false 设置失败
          */
         bool SetArmJointCommands(int8_t body_id, const std::vector<double>& joint_angles);
 
         /**
          * @brief 规划手臂运动（调用ArmControl的规划方法）
+         *
+         * 规划结果会被存入 ArmControl 内部的轨迹缓存，
+         * 上层通过 GetArmInterpolatedPoint / HasActiveArmTrajectory 获取执行点位。
+         *
          * @param arm_params 手臂运动参数（包含MOVEJ或MOVEL）
          * @param velocity_scaling 速度缩放因子 [0,1]
          * @param acceleration_scaling 加速度缩放因子 [0,1]
-         * @param trajectory_points 输出：规划好的轨迹点（关节角度，弧度）
          * @return true 规划成功，false 规划失败
          */
         bool PlanArmMotion(
             const ArmMotionParams& arm_params,
             double velocity_scaling,
-            double acceleration_scaling,
-            std::vector<std::vector<double>>& trajectory_points);
+            double acceleration_scaling);
+
+        /**
+         * @brief 检查手臂关节目标是否超过限位（MOVEJ 用）
+         * @param arm_id 手臂ID（0=左臂，1=右臂）
+         * @param target_joints 目标关节角（弧度），大小应为 arm_dof_
+         * @param error_msg 输出：失败原因（可为空指针）
+         * @return true 目标在限位内；false 超限或参数无效
+         */
+        bool ValidateArmJointTarget(
+            uint8_t arm_id,
+            const std::vector<double>& target_joints,
+            std::string* error_msg = nullptr) const;
 
         /**
          * @brief 初始化ArmControl的MoveIt（需要在ROS节点创建后调用）
@@ -182,13 +196,6 @@ namespace robot_control
          */
         bool InitializeArmMoveIt(rclcpp::Node* node);
 
-        /**
-         * @brief 存储规划好的轨迹到ArmControl
-         * @param arm_params 手臂运动参数
-         * @param trajectory_points 轨迹点（关节角度，弧度）
-         */
-        void StoreArmTrajectory(const ArmMotionParams& arm_params, const std::vector<std::vector<double>>& trajectory_points);
-
         /**
          * @brief 获取当前插补点位（在主循环中调用）
          * @param arm_id 手臂ID（0=左臂，1=右臂）
@@ -205,6 +212,16 @@ namespace robot_control
          */
         bool HasActiveArmTrajectory(uint8_t arm_id) const;
 
+        /**
+         * @brief 获取该臂轨迹的末端关节位置（弧度）
+         */
+        bool GetArmTrajectoryEndPositions(uint8_t arm_id, std::vector<double>& end_positions) const;
+
+        /**
+         * @brief 获取该臂轨迹的时间进度 [0,1]
+         */
+        double GetArmTrajectoryProgress(uint8_t arm_id) const;
+
         /**
          * @brief 控制手臂运动（在主循环中调用）
          * @param dt 时间步长（秒）

src/robot_control/include/core/robot_control_node.hpp

@@ -31,8 +31,9 @@
 #include "interfaces/msg/motor_cmd.hpp"
 #include "interfaces/srv/motor_param.hpp"
 #include "control_msgs/msg/dynamic_interface_group_values.hpp"
+#include <controller_manager_msgs/srv/switch_controller.hpp>
 
-#define RECORD_FLAG 0  ///< 数据记录标志（0=禁用，1=启用）
+#define RECORD_FLAG 1  ///< 数据记录标志（0=禁用，1=启用）
 
 using MoveHome = interfaces::action::MoveHome;
 using GoalHandleMoveHome = rclcpp_action::ServerGoalHandle<MoveHome>;
@@ -98,6 +99,7 @@ namespace robot_control
         rclcpp::Subscription<MotorPos>::SharedPtr wheelStatesSub_;               ///< 轮子状态订阅器
         rclcpp::Subscription<ImuMsg>::SharedPtr imuMsgSub_;                      ///< IMU消息订阅器
         rclcpp::Client<MotorParam>::SharedPtr motorParamClient_;                 ///< 电机参数客户端
+        rclcpp::Client<controller_manager_msgs::srv::SwitchController>::SharedPtr switch_client_; ///< 控制器切换客户端
 
         // ==================== 核心组件 ====================
         
@@ -116,9 +118,7 @@ namespace robot_control
         rclcpp::Time lastTime_;                      ///< 上次控制循环时间
 
         // ==================== 控制状态 ====================
-        
-        double jogValue_;    ///< 点动值
-        double lastSpeed_;   ///< 上次速度
+        bool is_robot_enabled_;   ///< 机器人是否启用
 
         // ==================== 回调函数 ====================
         
@@ -170,5 +170,19 @@ namespace robot_control
          */
         void motor_enable_all(double enable);
 
+        /**
+         * @brief 设置电机 fault（参考 test_motor.cpp）
+         * @param id 电机/关节序号（0=全部；否则按 all_joint_names_ 的 index+1 语义）
+         * @param fault fault 值（通常 1 表示复位脉冲，0 表示释放）
+         */
+        void motor_fault(int id, double fault);
+
+        /**
+         * @brief 设置电机 enable（参考 test_motor.cpp）
+         * @param id 电机/关节序号（0=全部；否则按 all_joint_names_ 的 index+1 语义）
+         * @param enable 使能值（1=使能，0=禁用）
+         */
+        void motor_enable(int id, double enable);
+
     };
 }

src/robot_control/launch/robot_control.launch.py

@@ -1,9 +1,46 @@
 from launch import LaunchDescription
 from launch_ros.actions import Node
 from launch.actions import TimerAction
-from launch.actions import IncludeLaunchDescription
+from launch.actions import IncludeLaunchDescription, OpaqueFunction
 from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.substitutions import FindPackageShare
+from launch.substitutions import PathJoinSubstitution
 from ament_index_python.packages import get_package_share_directory
+import os
+
+def load_urdf_srdf(context, *args, **kwargs):
+    """加载URDF和SRDF文件内容"""
+    nodes = []
+    
+    # 获取包路径
+    dual_arm_description_path = FindPackageShare(package='dual_arm_description').perform(context)
+    dual_arm_moveit_config_path = FindPackageShare(package='dual_arm_moveit_config').perform(context)
+    
+    # 读取URDF文件
+    urdf_path = os.path.join(dual_arm_description_path, 'urdf', 'dual_arm.urdf')
+    with open(urdf_path, 'r') as f:
+        urdf_content = f.read()
+    
+    # 读取SRDF文件
+    srdf_path = os.path.join(dual_arm_moveit_config_path, 'config', 'dual_arm.srdf')
+    with open(srdf_path, 'r') as f:
+        srdf_content = f.read()
+    
+    # 创建robot_control_node，并传递URDF和SRDF参数
+    robot_control_node = Node(
+        package='robot_control',
+        executable='robot_control_node',
+        name='robot_control_node',
+        output='screen',
+        parameters=[{
+            'use_sim_time': False,
+            'robot_description': urdf_content,
+            'robot_description_semantic': srdf_content
+        }]
+    )
+    nodes.append(robot_control_node)
+    
+    return nodes
 
 def generate_launch_description():
     # 获取dual_arm_moveit_config包的路径
@@ -18,23 +55,12 @@ def generate_launch_description():
         ])
     )
 
-    # 创建节点启动描述
-    robot_control_node = Node(
-        package='robot_control',  # 功能包名称
-        executable='robot_control_node',  # 可执行文件名称
-        name='robot_control_node',  # 节点名称（可自定义）
-        output='screen',  # 输出到屏幕
-        parameters=[{"use_sim_time": False}]  # 启用模拟时间
-    )
-
-    start_robot_control_node = TimerAction(
-        period=1.0,
-        actions=[robot_control_node],
-    )
+    # 使用OpaqueFunction来加载URDF和SRDF并创建节点
+    robot_control_nodes = OpaqueFunction(function=load_urdf_srdf)
     
     # 组装launch描述
     # move_group需要在robot_control_node之前启动
     return LaunchDescription([
         move_group_launch,  # 先启动move_group
-        start_robot_control_node  # 然后启动robot_control_node
+        robot_control_nodes  # 然后启动robot_control_node
     ])

src/robot_control/package.xml

@@ -16,6 +16,7 @@
   <build_depend>sensor_msgs</build_depend>
   <build_depend>trajectory_msgs</build_depend>
   <build_depend>control_msgs</build_depend>
+  <build_depend>controller_manager_msgs</build_depend>
   <build_depend>Eigen3</build_depend>  <!-- 对应CMake中的Eigen3依赖 -->
   <build_depend>interfaces</build_depend>
   <build_depend>moveit_core</build_depend>
@@ -31,6 +32,7 @@
   <exec_depend>sensor_msgs</exec_depend>
   <exec_depend>trajectory_msgs</exec_depend>
   <exec_depend>control_msgs</exec_depend>
+  <exec_depend>controller_manager_msgs</exec_depend>
   <exec_depend>nav_msgs</exec_depend>
   <exec_depend>interfaces</exec_depend>
   <exec_depend>moveit_core</exec_depend>

src/robot_control/scripts/dual_arm_action_client.py

@@ -0,0 +1,383 @@
+#!/usr/bin/env python3
+"""
+DualArm Action Client - 通过ROS2 action控制双臂运动
+
+该程序实现了DualArm action客户端，可以向robot_control节点发送双臂运动指令。
+支持左右臂分别做关节运动（MOVEJ）或笛卡尔空间运动（MOVEL）。
+"""
+
+import rclpy
+from rclpy.node import Node
+from rclpy.action import ActionClient
+from interfaces.action import DualArm
+from interfaces.msg import ArmMotionParams
+import sys
+from rclpy.task import Future
+import time
+
+
+class DualArmActionClient(Node):
+    """DualArm Action客户端类"""
+    
+    def __init__(self):
+        super().__init__('dual_arm_action_client')
+        self._action_client = ActionClient(self, DualArm, 'DualArm')
+        self._goal_handle = None
+        self._send_goal_future = None
+        self._get_result_future = None
+        
+    def wait_for_server(self, timeout_sec=10.0):
+        """等待action server可用"""
+        self.get_logger().info('等待DualArm action server...')
+        if not self._action_client.wait_for_server(timeout_sec=timeout_sec):
+            self.get_logger().error(f'Action server不可用，超时时间: {timeout_sec}秒')
+            return False
+        self.get_logger().info('Action server已就绪')
+        return True
+    
+    def send_goal(self, arm_type, arm_motion_params_list, velocity_scaling=0.5, 
+                  acceleration_scaling=0.5, cartesian_step=0.01):
+        """
+        发送运动目标
+        
+        Args:
+            arm_type: 运动臂类型 (DualArm.Goal.ARM_LEFT=0, ARM_RIGHT=1, ARM_DUAL=2)
+            arm_motion_params_list: ArmMotionParams消息列表
+            velocity_scaling: 速度缩放因子 [0,1]
+            acceleration_scaling: 加速度缩放因子 [0,1]
+            cartesian_step: MOVEL步长 [0,0.5] (单位: 米)
+        """
+        # reset per-goal futures
+        self._goal_handle = None
+        self._send_goal_future = None
+        self._get_result_future = None
+
+        goal_msg = DualArm.Goal()
+        goal_msg.arm_type = arm_type
+        goal_msg.arm_motion_params = arm_motion_params_list
+        goal_msg.velocity_scaling = velocity_scaling
+        goal_msg.acceleration_scaling = acceleration_scaling
+        goal_msg.cartesian_step = cartesian_step
+        
+        self.get_logger().info(f'发送目标: arm_type={arm_type}, '
+                              f'arm_count={len(arm_motion_params_list)}, '
+                              f'velocity_scaling={velocity_scaling}, '
+                              f'acceleration_scaling={acceleration_scaling}')
+        
+        # 发送goal（带 feedback 回调）
+        self._send_goal_future = self._action_client.send_goal_async(
+            goal_msg,
+            feedback_callback=self._feedback_callback
+        )
+        self._send_goal_future.add_done_callback(self._goal_response_callback)
+        return self._send_goal_future
+    
+    def _goal_response_callback(self, future):
+        """Goal响应回调"""
+        goal_handle = future.result()
+        if not goal_handle:
+            self.get_logger().error('目标被拒绝')
+            return
+        
+        if not goal_handle.accepted:
+            self.get_logger().error('目标被拒绝')
+            return
+        
+        self._goal_handle = goal_handle
+        self.get_logger().info('目标已接受')
+        
+        # 获取结果future并设置回调
+        self._get_result_future = goal_handle.get_result_async()
+        self._get_result_future.add_done_callback(self._result_callback)
+        
+        # 注意：在Python中，feedback回调需要在goal_handle上设置
+        # 但ROS2 Python ActionClient的feedback需要通过goal_handle.get_result_async()的结果来获取
+        # 实际上，feedback应该在action执行过程中通过其他方式获取
+    
+    def _feedback_callback(self, feedback_msg):
+        """Feedback回调"""
+        feedback = feedback_msg.feedback
+        status_names = ['规划中', '执行中', '完成']
+        status_name = status_names[feedback.status] if feedback.status < len(status_names) else '未知'
+        
+        self.get_logger().info(
+            f'反馈 - 状态: {status_name} ({feedback.status}), '
+            f'进度: {feedback.progress:.2%}'
+        )
+        
+        if len(feedback.joints_left) > 0:
+            self.get_logger().info(f'  左臂关节: {[f"{j:.3f}" for j in feedback.joints_left]}')
+        if len(feedback.joints_right) > 0:
+            self.get_logger().info(f'  右臂关节: {[f"{j:.3f}" for j in feedback.joints_right]}')
+    
+    def _result_callback(self, future):
+        """Result回调"""
+        wrapped_result = future.result()
+        result = wrapped_result.result
+        if result.success:
+            self.get_logger().info(f'执行成功: {result.message}')
+            self.get_logger().info(f'最终进度: {result.final_progress:.2%}')
+        else:
+            self.get_logger().error(f'执行失败: {result.message}')
+    
+    def cancel_goal(self):
+        """取消当前目标"""
+        if self._goal_handle is not None:
+            self.get_logger().info('取消目标...')
+            future = self._action_client.cancel_goal_async(self._goal_handle)
+            return future
+        return None
+
+    def send_goal_and_wait(
+        self,
+        arm_type,
+        arm_motion_params_list,
+        velocity_scaling=0.5,
+        acceleration_scaling=0.5,
+        cartesian_step=0.01,
+        result_timeout_sec=30.0,
+    ):
+        """
+        发送 goal 并阻塞等待结果返回（确保上一段运动真正执行完成后再继续）。
+        返回 (success: bool, result_message: str)
+        """
+        future = self.send_goal(
+            arm_type=arm_type,
+            arm_motion_params_list=arm_motion_params_list,
+            velocity_scaling=velocity_scaling,
+            acceleration_scaling=acceleration_scaling,
+            cartesian_step=cartesian_step,
+        )
+        rclpy.spin_until_future_complete(self, future)
+
+        # 等待 result future 被 goal response callback 设置
+        start = time.time()
+        while self._get_result_future is None and rclpy.ok():
+            rclpy.spin_once(self, timeout_sec=0.05)
+            if result_timeout_sec is not None and (time.time() - start) > result_timeout_sec:
+                self.get_logger().error('等待 result future 超时，尝试取消')
+                self.cancel_goal()
+                return False, 'wait result future timeout'
+
+        if self._get_result_future is None:
+            return False, 'result future not set'
+
+        rclpy.spin_until_future_complete(self, self._get_result_future, timeout_sec=result_timeout_sec)
+        if not self._get_result_future.done():
+            self.get_logger().error('等待执行结果超时，尝试取消')
+            self.cancel_goal()
+            return False, 'result timeout'
+
+        wrapped_result = self._get_result_future.result()
+        result = wrapped_result.result
+        return bool(result.success), str(result.message)
+
+
+def create_arm_motion_param(arm_id, motion_type, target_joints=None, target_pose=None):
+    """
+    创建ArmMotionParams消息
+    
+    Args:
+        arm_id: 机械臂ID (ArmMotionParams.ARM_LEFT=0, ARM_RIGHT=1)
+        motion_type: 运动模式 (ArmMotionParams.MOVEJ=0, MOVEL=1)
+        target_joints: 目标关节角列表（弧度），用于MOVEJ
+        target_pose: 目标位姿（geometry_msgs.Pose），用于MOVEL
+    
+    Returns:
+        ArmMotionParams消息对象
+    """
+    param = ArmMotionParams()
+    param.arm_id = arm_id
+    param.motion_type = motion_type
+    
+    if motion_type == ArmMotionParams.MOVEJ:
+        if target_joints is not None:
+            param.target_joints = target_joints
+        else:
+            raise ValueError("MOVEJ模式需要提供target_joints")
+    elif motion_type == ArmMotionParams.MOVEL:
+        if target_pose is not None:
+            param.target_pose = target_pose
+        else:
+            raise ValueError("MOVEL模式需要提供target_pose")
+    
+    return param
+
+
+def example_left_and_right_arm_joint_motion():
+    """示例：循环测试（类似 right_arm_joint_test.cpp 的关节逐个 up/down 测试）"""
+    rclpy.init()
+    
+    client = DualArmActionClient()
+    
+    # 等待action server
+    if not client.wait_for_server(timeout_sec=10.0):
+        client.get_logger().error('无法连接到action server')
+        client.destroy_node()
+        rclpy.shutdown()
+        return
+    
+    try:
+        # ========= 配置测试参数（按需修改）=========
+        arm_type = DualArm.Goal.ARM_LEFT
+        arm_id = ArmMotionParams.ARM_LEFT
+
+        # 6关节 home（rad）
+        home = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+        home2 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+        amplitude = 1.0     # 单关节摆动幅度（rad）
+        cycles = 1         # 每个关节 up/down 循环次数
+        velocity_scaling = 0.5
+        acceleration_scaling = 0.5
+        result_timeout_sec = 30.0
+        sleep_between_sec = 5.0
+
+        dof = 6
+
+        client.get_logger().info('=' * 60)
+        client.get_logger().info(f'循环关节测试开始: arm_type={arm_type}, dof={dof}, cycles={cycles}, amplitude={amplitude}rad')
+        client.get_logger().info('=' * 60)
+
+        for j in range(dof):
+            client.get_logger().info(f'测试关节 {j} ...')
+
+            for c in range(cycles):
+                # Move up
+                if j == 0:
+                    up = list(home)
+                else:
+                    up = list(home2)
+
+                up[j] += amplitude / 2.0
+                up_param = create_arm_motion_param(
+                    arm_id=arm_id,
+                    motion_type=ArmMotionParams.MOVEJ,
+                    target_joints=up
+                )
+
+                ok, msg = client.send_goal_and_wait(
+                    arm_type=arm_type,
+                    arm_motion_params_list=[up_param],
+                    velocity_scaling=velocity_scaling,
+                    acceleration_scaling=acceleration_scaling,
+                    result_timeout_sec=result_timeout_sec,
+                )
+                if not ok:
+                    client.get_logger().error(f'UP 执行失败: joint={j}, cycle={c}, msg={msg}')
+                    break
+
+                time.sleep(sleep_between_sec)
+
+                # Move down
+                if j == 0:
+                    down = list(home)
+                else:
+                    down = list(home2)
+                down[j] -= amplitude / 2.0
+                down_param = create_arm_motion_param(
+                    arm_id=arm_id,
+                    motion_type=ArmMotionParams.MOVEJ,
+                    target_joints=down
+                )
+
+                ok, msg = client.send_goal_and_wait(
+                    arm_type=arm_type,
+                    arm_motion_params_list=[down_param],
+                    velocity_scaling=velocity_scaling,
+                    acceleration_scaling=acceleration_scaling,
+                    result_timeout_sec=result_timeout_sec,
+                )
+                if not ok:
+                    client.get_logger().error(f'DOWN 执行失败: joint={j}, cycle={c}, msg={msg}')
+                    break
+
+                time.sleep(sleep_between_sec)
+
+        client.get_logger().info('循环关节测试结束')
+        
+        # 等待一段时间后执行下一个目标
+        # import time
+        # time.sleep(2.0)
+        
+        # # 示例2: 右臂关节运动
+        # right_target_joints = [-0.1, -0.2, 0.3, -0.4, 0.2, -0.1]  # 弧度
+        
+        # right_param = create_arm_motion_param(
+        #     arm_id=ArmMotionParams.ARM_RIGHT,
+        #     motion_type=ArmMotionParams.MOVEJ,
+        #     target_joints=right_target_joints
+        # )
+        
+        # client.get_logger().info('=' * 60)
+        # client.get_logger().info('示例2: 发送右臂关节运动指令')
+        # client.get_logger().info('=' * 60)
+        
+        # future = client.send_goal(
+        #     arm_type=DualArm.Goal.ARM_RIGHT,
+        #     arm_motion_params_list=[right_param],
+        #     velocity_scaling=0.5,
+        #     acceleration_scaling=0.5
+        # )
+        
+        # # 等待goal响应
+        # rclpy.spin_until_future_complete(client, future)
+        
+        # # 等待执行完成
+        # if client._get_result_future is not None:
+        #     rclpy.spin_until_future_complete(client, client._get_result_future, timeout_sec=30.0)
+        
+        # # 示例3: 双臂同时运动
+        # client.get_logger().info('=' * 60)
+        # client.get_logger().info('示例3: 发送双臂同时运动指令')
+        # client.get_logger().info('=' * 60)
+        
+        # # 重新设置目标关节角度
+        # left_target_joints_dual = [0.2, 0.3, -0.4, 0.5, -0.3, 0.2]
+        # right_target_joints_dual = [-0.2, -0.3, 0.4, -0.5, 0.3, -0.2]
+        
+        # left_param_dual = create_arm_motion_param(
+        #     arm_id=ArmMotionParams.ARM_LEFT,
+        #     motion_type=ArmMotionParams.MOVEJ,
+        #     target_joints=left_target_joints_dual
+        # )
+        
+        # right_param_dual = create_arm_motion_param(
+        #     arm_id=ArmMotionParams.ARM_RIGHT,
+        #     motion_type=ArmMotionParams.MOVEJ,
+        #     target_joints=right_target_joints_dual
+        # )
+        
+        # future = client.send_goal(
+        #     arm_type=DualArm.Goal.ARM_DUAL,
+        #     arm_motion_params_list=[left_param_dual, right_param_dual],
+        #     velocity_scaling=0.5,
+        #     acceleration_scaling=0.5
+        # )
+        
+        # # 等待goal响应
+        # rclpy.spin_until_future_complete(client, future)
+        
+        # # 等待执行完成
+        # if client._get_result_future is not None:
+        #     rclpy.spin_until_future_complete(client, client._get_result_future, timeout_sec=30.0)
+        
+    except KeyboardInterrupt:
+        client.get_logger().info('收到中断信号，取消目标...')
+        client.cancel_goal()
+    except Exception as e:
+        client.get_logger().error(f'发生错误: {e}')
+    finally:
+        client.destroy_node()
+        rclpy.shutdown()
+
+
+def main(args=None):
+    """主函数"""
+    if args is None:
+        args = sys.argv
+    
+    example_left_and_right_arm_joint_motion()
+
+
+if __name__ == '__main__':
+    main()

src/robot_control/src/actions/action_manager.cpp

@@ -513,6 +513,21 @@ rclcpp_action::GoalResponse ActionManager::handle_dual_arm_goal(
         RCLCPP_ERROR(node_->get_logger(), "Jog mode is enabled");
         return rclcpp_action::GoalResponse::REJECT;
     }
+
+    // 在规划之前检查 MOVEJ 的关节目标是否超过限位（单位：rad）
+    for (const auto& arm_param : goal->arm_motion_params) {
+        if (arm_param.motion_type == arm_param.MOVEJ) {
+            std::string err;
+            if (!robot_control_manager_.ValidateArmJointTarget(
+                    arm_param.arm_id, arm_param.target_joints, &err))
+            {
+                RCLCPP_ERROR(node_->get_logger(),
+                    "DualArm goal rejected: MOVEJ target_joints exceed limits for arm_id %d: %s",
+                    arm_param.arm_id, err.c_str());
+                return rclcpp_action::GoalResponse::REJECT;
+            }
+        }
+    }
     
     return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
 }
@@ -527,7 +542,6 @@ rclcpp_action::CancelResponse ActionManager::handle_dual_arm_cancel(
 
 void ActionManager::handle_dual_arm_accepted(const std::shared_ptr<GoalHandleDualArm> goal_handle)
 {
-    dual_arm_executing_.store(true);
     using namespace std::placeholders;
     std::thread{std::bind(&ActionManager::dual_arm_execute, this, _1), goal_handle}.detach();
 }
@@ -573,21 +587,41 @@ void ActionManager::dual_arm_execute(const std::shared_ptr<GoalHandleDualArm> go
                     dual_arm_executing_.store(false);
                     return;
                 }
-            }
 
+                // 再次校验关节限位（防止执行阶段被直接调用/绕过 goal 阶段）
+                std::string err;
+                if (!robot_control_manager_.ValidateArmJointTarget(
+                        arm_param.arm_id, arm_param.target_joints, &err))
+                {
+                    RCLCPP_ERROR(node_->get_logger(),
+                        "MOVEJ target_joints exceed limits for arm_id %d: %s",
+                        arm_param.arm_id, err.c_str());
+                    result->message = std::string("MOVEJ target_joints exceed limits: ") + err;
+                    goal_handle->abort(result);
+                    dual_arm_executing_.store(false);
+                    return;
+                }
+            }
+            std::cout << "Plan Arm Motion" << std::endl;
+            std::cout << "arm_param.arm_id: " << arm_param.arm_id << std::endl;
+            std::cout << "arm_param.motion_type: " << arm_param.motion_type << std::endl;
+            std::cout << "goal->velocity_scaling: " << goal->velocity_scaling << std::endl;
+            std::cout << "goal->acceleration_scaling: " << goal->acceleration_scaling << std::endl;
+            std::cout << "arm_param.target_joints: " << arm_param.target_joints.size() << std::endl;
+            std::cout << "arm_param.target_joints: " << arm_param.target_joints[0] << std::endl;
+            std::cout << "arm_param.target_joints: " << arm_param.target_joints[1] << std::endl;
+            std::cout << "arm_param.target_joints: " << arm_param.target_joints[2] << std::endl;
+            std::cout << "arm_param.target_joints: " << arm_param.target_joints[3] << std::endl;
+            std::cout << "arm_param.target_joints: " << arm_param.target_joints[4] << std::endl;
+            std::cout << "arm_param.target_joints: " << arm_param.target_joints[5] << std::endl;
             // 调用RobotControlManager的规划接口（统一管理）
-            std::vector<std::vector<double>> trajectory_points;
             if (robot_control_manager_.PlanArmMotion(
                 arm_param,
                 goal->velocity_scaling,
-                goal->acceleration_scaling,
-                trajectory_points)) {
+                goal->acceleration_scaling)) {
                 RCLCPP_INFO(node_->get_logger(), 
-                    "Arm motion planning successful for arm_id %d, motion_type %d, trajectory points: %zu",
-                    arm_param.arm_id, arm_param.motion_type, trajectory_points.size());
-                
-                // 存储轨迹到ArmControl，在主循环中依次下发
-                robot_control_manager_.StoreArmTrajectory(arm_param, trajectory_points);
+                    "Arm motion planning successful for arm_id %d, motion_type %d",
+                    arm_param.arm_id, arm_param.motion_type);
                 
                 result->success = true;
                 result->message = "DualArm planning succeeded";
@@ -615,6 +649,19 @@ void ActionManager::dual_arm_execute(const std::shared_ptr<GoalHandleDualArm> go
     // 执行阶段：循环发送反馈，直到轨迹执行完成
     feedback->status = 1;  // STATUS_EXECUTING = 1
     
+    dual_arm_executing_.store(true);
+
+    // 哪些手臂需要达成目标（根据本次 goal 的 arm_motion_params 判断）
+    bool need_left = false;
+    bool need_right = false;
+    for (const auto& arm_param : goal->arm_motion_params) {
+        if (arm_param.arm_id == 0) need_left = true;
+        if (arm_param.arm_id == 1) need_right = true;
+    }
+    const double kJointTolRad = 0.002;  // 目标到达阈值（弧度）
+    const rclcpp::Time exec_start_time = node_->now();
+    const double kMaxWaitSec = 15.0;   // 安全超时，防止永不结束
+
     while (dual_arm_executing_.load() && rclcpp::ok())
     {
         // 检查取消请求
@@ -631,15 +678,14 @@ void ActionManager::dual_arm_execute(const std::shared_ptr<GoalHandleDualArm> go
         bool left_arm_active = robot_control_manager_.HasActiveArmTrajectory(0);
         bool right_arm_active = robot_control_manager_.HasActiveArmTrajectory(1);
         
-        // 计算进度（简单实现：如果还有轨迹在执行，进度为0.5，否则为1.0）
-        if (left_arm_active || right_arm_active) {
-            feedback->progress = 0.5;
-        } else {
-            feedback->progress = 1.0;
-            feedback->status = 2;  // STATUS_DONE = 2
-        }
+        // 基于时间的进度：读取 ArmControl 内部 elapsed_time / total_time
+        double p_sum = 0.0;
+        int p_cnt = 0;
+        if (need_left) { p_sum += robot_control_manager_.GetArmTrajectoryProgress(0); ++p_cnt; }
+        if (need_right){ p_sum += robot_control_manager_.GetArmTrajectoryProgress(1); ++p_cnt; }
+        feedback->progress = (p_cnt > 0) ? (p_sum / static_cast<double>(p_cnt)) : 1.0;
         
-        // 获取关节反馈并提取左右臂关节（单位：度）
+        // 获取关节反馈并提取左右臂关节（单位：弧度）
         auto joint_feedback = robot_control_manager_.GetJointFeedback();
         feedback->joints_left.clear();
         feedback->joints_right.clear();
@@ -650,8 +696,7 @@ void ActionManager::dual_arm_execute(const std::shared_ptr<GoalHandleDualArm> go
                 const std::string& joint_name = motionParams.dual_arm_joint_names_[i];
                 auto it = motionParams.joint_name_to_index_.find(joint_name);
                 if (it != motionParams.joint_name_to_index_.end() && it->second < joint_feedback.data.size()) {
-                    double angle_deg = joint_feedback.data[it->second];
-                    double angle_rad = angle_deg * M_PI / 180.0;  // 度转弧度
+                    double angle_rad = joint_feedback.data[it->second];
                     feedback->joints_left.push_back(angle_rad);
                 } else {
                     feedback->joints_left.push_back(0.0);
@@ -665,8 +710,7 @@ void ActionManager::dual_arm_execute(const std::shared_ptr<GoalHandleDualArm> go
                 const std::string& joint_name = motionParams.dual_arm_joint_names_[i];
                 auto it = motionParams.joint_name_to_index_.find(joint_name);
                 if (it != motionParams.joint_name_to_index_.end() && it->second < joint_feedback.data.size()) {
-                    double angle_deg = joint_feedback.data[it->second];
-                    double angle_rad = angle_deg * M_PI / 180.0;  // 度转弧度
+                    double angle_rad = joint_feedback.data[it->second];
                     feedback->joints_right.push_back(angle_rad);
                 } else {
                     feedback->joints_right.push_back(0.0);
@@ -676,9 +720,42 @@ void ActionManager::dual_arm_execute(const std::shared_ptr<GoalHandleDualArm> go
         
         // 发布反馈
         goal_handle->publish_feedback(feedback);
-        
-        // 如果轨迹执行完成，退出循环
-        if (!left_arm_active && !right_arm_active) {
+
+        // 判断是否“接近目标值才结束”
+        auto all_close = [&](uint8_t arm_id, const std::vector<double>& current, double tol) -> bool {
+            std::vector<double> target;
+            if (!robot_control_manager_.GetArmTrajectoryEndPositions(arm_id, target)) {
+                return true;  // 没有轨迹，认为不需要判断
+            }
+            if (current.size() != target.size()) {
+                return false;
+            }
+            for (size_t i = 0; i < current.size(); ++i) {
+                if (std::abs(current[i] - target[i]) > tol) return false;
+            }
+            return true;
+        };
+
+        const bool left_close = (!need_left) ? true : all_close(0, feedback->joints_left, kJointTolRad);
+        const bool right_close = (!need_right) ? true : all_close(1, feedback->joints_right, kJointTolRad);
+
+        // 安全超时：避免永远等不到反馈到位
+        if ((node_->now() - exec_start_time).seconds() > kMaxWaitSec) {
+            RCLCPP_ERROR(node_->get_logger(), "DualArm execute timeout (%.1fs), aborting", kMaxWaitSec);
+            result->success = false;
+            result->message = "DualArm execute timeout";
+            goal_handle->abort(result);
+            dual_arm_executing_.store(false);
+            return;
+        }
+
+        // 当轨迹已结束（不再 active）且反馈已接近目标，才结束
+        const bool traj_done = (!left_arm_active && !right_arm_active);
+        if (traj_done && left_close && right_close) {
+            feedback->progress = 1.0;
+            feedback->status = 2;  // STATUS_DONE = 2
+            goal_handle->publish_feedback(feedback);
+            std::cout << "轨迹执行完成且到位 , action internal finished" << std::endl;
             break;
         }
         

src/robot_control/src/controllers/arm_control.cpp

@@ -9,6 +9,9 @@
 #include "moveit/move_group_interface/move_group_interface.h"
 #include "moveit/planning_interface/planning_interface.h"
 #include "moveit_msgs/msg/move_it_error_codes.hpp"
+#include "moveit/robot_trajectory/robot_trajectory.h"
+#include "moveit/robot_state/robot_state.h"
+#include "moveit/trajectory_processing/iterative_time_parameterization.h"
 
 #ifndef M_PI
 #define M_PI 3.14159265358979323846
@@ -49,6 +52,11 @@ bool ArmControl::InitializeMoveIt(rclcpp::Node* node)
 
     node_ = node;
 
+    // 先重置所有move group，确保初始状态干净
+    move_group_left_.reset();
+    move_group_right_.reset();
+    move_group_dual_.reset();
+
     try {
         // 创建一个shared_ptr包装node（MoveGroupInterface需要shared_ptr）
         auto node_shared = std::shared_ptr<rclcpp::Node>(node, [](rclcpp::Node*) {
@@ -56,35 +64,83 @@ bool ArmControl::InitializeMoveIt(rclcpp::Node* node)
         });
 
         // 初始化左臂move group
-        move_group_left_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(
-            node_shared, "arm_left");
-        if (!move_group_left_) {
-            std::cerr << "ArmControl::InitializeMoveIt: Failed to create MoveGroupInterface for arm_left" << std::endl;
+        try {
+            move_group_left_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(
+                node_shared, "arm_left");
+            if (!move_group_left_) {
+                std::cerr << "ArmControl::InitializeMoveIt: Failed to create MoveGroupInterface for arm_left" << std::endl;
+                return false;
+            }
+            std::cout << "ArmControl::InitializeMoveIt: MoveGroupInterface initialized for arm_left" << std::endl;
+        } catch (const std::exception& e) {
+            std::cerr << "ArmControl::InitializeMoveIt: Exception creating arm_left MoveGroupInterface: " << e.what() << std::endl;
+            move_group_left_.reset();
+            return false;
+        } catch (...) {
+            std::cerr << "ArmControl::InitializeMoveIt: Unknown exception creating arm_left MoveGroupInterface" << std::endl;
+            move_group_left_.reset();
             return false;
         }
-        std::cout << "ArmControl::InitializeMoveIt: MoveGroupInterface initialized for arm_left" << std::endl;
 
         // 初始化右臂move group
-        move_group_right_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(
-            node_shared, "arm_right");
-        if (!move_group_right_) {
-            std::cerr << "ArmControl::InitializeMoveIt: Failed to create MoveGroupInterface for arm_right" << std::endl;
+        try {
+            move_group_right_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(
+                node_shared, "arm_right");
+            if (!move_group_right_) {
+                std::cerr << "ArmControl::InitializeMoveIt: Failed to create MoveGroupInterface for arm_right" << std::endl;
+                move_group_left_.reset();
+                return false;
+            }
+            std::cout << "ArmControl::InitializeMoveIt: MoveGroupInterface initialized for arm_right" << std::endl;
+        } catch (const std::exception& e) {
+            std::cerr << "ArmControl::InitializeMoveIt: Exception creating arm_right MoveGroupInterface: " << e.what() << std::endl;
+            move_group_left_.reset();
+            move_group_right_.reset();
+            return false;
+        } catch (...) {
+            std::cerr << "ArmControl::InitializeMoveIt: Unknown exception creating arm_right MoveGroupInterface" << std::endl;
+            move_group_left_.reset();
+            move_group_right_.reset();
             return false;
         }
-        std::cout << "ArmControl::InitializeMoveIt: MoveGroupInterface initialized for arm_right" << std::endl;
 
         // 初始化双臂move group
-        move_group_dual_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(
-            node_shared, "dual_arm");
-        if (!move_group_dual_) {
-            std::cerr << "ArmControl::InitializeMoveIt: Failed to create MoveGroupInterface for dual_arm" << std::endl;
+        try {
+            move_group_dual_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(
+                node_shared, "dual_arm");
+            if (!move_group_dual_) {
+                std::cerr << "ArmControl::InitializeMoveIt: Failed to create MoveGroupInterface for dual_arm" << std::endl;
+                move_group_left_.reset();
+                move_group_right_.reset();
+                return false;
+            }
+            std::cout << "ArmControl::InitializeMoveIt: MoveGroupInterface initialized for dual_arm" << std::endl;
+        } catch (const std::exception& e) {
+            std::cerr << "ArmControl::InitializeMoveIt: Exception creating dual_arm MoveGroupInterface: " << e.what() << std::endl;
+            move_group_left_.reset();
+            move_group_right_.reset();
+            move_group_dual_.reset();
+            return false;
+        } catch (...) {
+            std::cerr << "ArmControl::InitializeMoveIt: Unknown exception creating dual_arm MoveGroupInterface" << std::endl;
+            move_group_left_.reset();
+            move_group_right_.reset();
+            move_group_dual_.reset();
             return false;
         }
-        std::cout << "ArmControl::InitializeMoveIt: MoveGroupInterface initialized for dual_arm" << std::endl;
 
         return true;
     } catch (const std::exception& e) {
         std::cerr << "ArmControl::InitializeMoveIt: Exception: " << e.what() << std::endl;
+        move_group_left_.reset();
+        move_group_right_.reset();
+        move_group_dual_.reset();
+        return false;
+    } catch (...) {
+        std::cerr << "ArmControl::InitializeMoveIt: Unknown exception" << std::endl;
+        move_group_left_.reset();
+        move_group_right_.reset();
+        move_group_dual_.reset();
         return false;
     }
 }
@@ -115,16 +171,21 @@ bool ArmControl::PlanJointMotion(
     uint8_t arm_id,
     const std::vector<double>& target_joints,
     double velocity_scaling,
-    double acceleration_scaling,
-    std::vector<std::vector<double>>& trajectory_points)
+    double acceleration_scaling)
 {
     auto move_group = GetMoveGroup(arm_id);
     if (!move_group) {
-        std::cerr << "ArmControl::PlanJointMotion: Invalid arm_id or MoveIt not initialized" << std::endl;
         return false;
     }
 
     try {
+
+        auto cs = move_group->getCurrentState();
+        RCLCPP_INFO(node_->get_logger(), "current state received: %s", cs ? "yes" : "no");
+
+        // 确保规划起点与当前机器人状态一致（否则 t=0 可能会出现“跳变”）
+        move_group->setStartStateToCurrentState();
+
         // 设置速度和加速度缩放因子
         move_group->setMaxVelocityScalingFactor(velocity_scaling);
         move_group->setMaxAccelerationScalingFactor(acceleration_scaling);
@@ -137,61 +198,12 @@ bool ArmControl::PlanJointMotion(
         moveit::core::MoveItErrorCode error_code = move_group->plan(plan);
 
         if (error_code != moveit::core::MoveItErrorCode::SUCCESS) {
-            std::cerr << "ArmControl::PlanJointMotion: Planning failed with error code: " 
+            std::cerr << " Planning failed with error code: " 
                       << error_code.val << " for arm_id " << static_cast<int>(arm_id) << std::endl;
             return false;
         }
-
-        // 提取轨迹点（为了向后兼容，仍返回位置信息）
-        trajectory_points.clear();
-        const auto& trajectory = plan.trajectory_.joint_trajectory;
-        if (trajectory.points.size() > 0) {
-            // 存储完整轨迹信息到TrajectoryState
-            TrajectoryState* traj_state = (arm_id == 0) ? &left_arm_trajectory_ : &right_arm_trajectory_;
-            traj_state->trajectory_points.clear();
-            size_t num_joints = trajectory.points[0].positions.size();
-            
-            for (const auto& point : trajectory.points) {
-                // 提取位置信息（用于返回）
-                trajectory_points.push_back(point.positions);
-                
-                // 存储完整轨迹信息（位置、速度、加速度、时间）
-                TrajectoryPoint traj_point;
-                traj_point.positions = point.positions;
-                
-                // 提取速度信息（如果存在）
-                if (point.velocities.size() == num_joints) {
-                    traj_point.velocities = point.velocities;
-                } else {
-                    traj_point.velocities.resize(num_joints, 0.0);
-                }
-                
-                // 提取加速度信息（如果存在）
-                if (point.accelerations.size() == num_joints) {
-                    traj_point.accelerations = point.accelerations;
-                } else {
-                    traj_point.accelerations.resize(num_joints, 0.0);
-                }
-                
-                // 提取时间信息
-                traj_point.time_from_start = point.time_from_start.sec + 
-                                            point.time_from_start.nanosec * 1e-9;
-                
-                traj_state->trajectory_points.push_back(traj_point);
-            }
-            
-            traj_state->current_index = 0;
-            traj_state->elapsed_time = 0.0;
-            traj_state->is_active = true;
-        } else {
-            std::cerr << "ArmControl::PlanJointMotion: Trajectory is empty" << std::endl;
-            return false;
-        }
-
-        std::cout << "ArmControl::PlanJointMotion: Planning successful for arm_id " 
-                  << static_cast<int>(arm_id) << ", " << trajectory_points.size() 
-                  << " trajectory points generated" << std::endl;
-        return true;
+        return timeParameterizeResampleAndStore_(
+            arm_id, *move_group, plan, velocity_scaling, acceleration_scaling);
     } catch (const std::exception& e) {
         std::cerr << "ArmControl::PlanJointMotion: Exception: " << e.what() << std::endl;
         return false;
@@ -202,8 +214,7 @@ bool ArmControl::PlanCartesianMotion(
     uint8_t arm_id,
     const geometry_msgs::msg::Pose& target_pose,
     double velocity_scaling,
-    double acceleration_scaling,
-    std::vector<std::vector<double>>& trajectory_points)
+    double acceleration_scaling)
 {
     auto move_group = GetMoveGroup(arm_id);
     if (!move_group) {
@@ -212,6 +223,12 @@ bool ArmControl::PlanCartesianMotion(
     }
 
     try {
+        auto cs = move_group->getCurrentState();
+        RCLCPP_INFO(node_->get_logger(), "current state received: %s", cs ? "yes" : "no");
+
+        // 确保规划起点与当前机器人状态一致（否则 t=0 可能会出现“跳变”）
+        move_group->setStartStateToCurrentState();
+
         // 设置速度和加速度缩放因子
         move_group->setMaxVelocityScalingFactor(velocity_scaling);
         move_group->setMaxAccelerationScalingFactor(acceleration_scaling);
@@ -228,96 +245,188 @@ bool ArmControl::PlanCartesianMotion(
                       << error_code.val << " for arm_id " << static_cast<int>(arm_id) << std::endl;
             return false;
         }
-
-        // 提取轨迹点（为了向后兼容，仍返回位置信息）
-        trajectory_points.clear();
-        const auto& trajectory = plan.trajectory_.joint_trajectory;
-        if (trajectory.points.size() > 0) {
-            // 存储完整轨迹信息到TrajectoryState
-            TrajectoryState* traj_state = (arm_id == 0) ? &left_arm_trajectory_ : &right_arm_trajectory_;
-            traj_state->trajectory_points.clear();
-            size_t num_joints = trajectory.points[0].positions.size();
-            
-            for (const auto& point : trajectory.points) {
-                // 提取位置信息（用于返回）
-                trajectory_points.push_back(point.positions);
-                
-                // 存储完整轨迹信息（位置、速度、加速度、时间）
-                TrajectoryPoint traj_point;
-                traj_point.positions = point.positions;
-                
-                // 提取速度信息（如果存在）
-                if (point.velocities.size() == num_joints) {
-                    traj_point.velocities = point.velocities;
-                } else {
-                    traj_point.velocities.resize(num_joints, 0.0);
-                }
-                
-                // 提取加速度信息（如果存在）
-                if (point.accelerations.size() == num_joints) {
-                    traj_point.accelerations = point.accelerations;
-                } else {
-                    traj_point.accelerations.resize(num_joints, 0.0);
-                }
-                
-                // 提取时间信息
-                traj_point.time_from_start = point.time_from_start.sec + 
-                                            point.time_from_start.nanosec * 1e-9;
-                
-                traj_state->trajectory_points.push_back(traj_point);
-            }
-            
-            traj_state->current_index = 0;
-            traj_state->elapsed_time = 0.0;
-            traj_state->is_active = true;
-        } else {
-            std::cerr << "ArmControl::PlanCartesianMotion: Trajectory is empty" << std::endl;
-            return false;
-        }
-
-        std::cout << "ArmControl::PlanCartesianMotion: Planning successful for arm_id " 
-                  << static_cast<int>(arm_id) << ", " << trajectory_points.size() 
-                  << " trajectory points generated" << std::endl;
-        return true;
+        return timeParameterizeResampleAndStore_(
+            arm_id, *move_group, plan, velocity_scaling, acceleration_scaling);
     } catch (const std::exception& e) {
         std::cerr << "ArmControl::PlanCartesianMotion: Exception: " << e.what() << std::endl;
         return false;
     }
 }
 
-void ArmControl::StoreTrajectory(const std::vector<std::vector<double>>& trajectory_points, uint8_t arm_id)
+bool ArmControl::timeParameterizeResampleAndStore_(
+    uint8_t arm_id,
+    moveit::planning_interface::MoveGroupInterface& move_group,
+    moveit::planning_interface::MoveGroupInterface::Plan& plan,
+    double velocity_scaling,
+    double acceleration_scaling)
 {
-    // 注意：此函数保留用于向后兼容，但现在主要使用 StoreTrajectoryFromPlan
-    // 如果只提供位置信息，速度和加速度将被设置为0
+    // 固定采样时间间隔（建议与你的控制周期一致或为其整数倍）
+    // CYCLE_TIME 单位是毫秒(ms)，这里转换为秒(s)
+    constexpr double kFixedTimeStep = static_cast<double>(CYCLE_TIME) / 1000.0;
+
+    // 1) RobotTrajectory：从 plan.trajectory_ 构建
+    robot_trajectory::RobotTrajectory original_traj(
+        move_group.getRobotModel(), move_group.getName());
+    original_traj.setRobotTrajectoryMsg(*move_group.getCurrentState(), plan.trajectory_);
+
+    // 2) 先时间参数化，得到合理的 total_time
+    trajectory_processing::IterativeParabolicTimeParameterization time_param;
+    if (!time_param.computeTimeStamps(original_traj, velocity_scaling, acceleration_scaling))
+    {
+        std::cerr << "ArmControl: computeTimeStamps failed for arm_id "
+                  << static_cast<int>(arm_id) << std::endl;
+        return false;
+    }
+
+    const size_t waypoint_count = original_traj.getWayPointCount();
+    if (waypoint_count == 0)
+    {
+        std::cerr << "ArmControl: Empty trajectory after time parameterization for arm_id "
+                  << static_cast<int>(arm_id) << std::endl;
+        return false;
+    }
+    const double total_time = original_traj.getWayPointDurationFromStart(waypoint_count - 1);
+
+    // 3) 固定 dt 重采样：注意 addSuffixWayPoint 的 dt 参数是“相邻点间隔”，不是绝对时间
+    robot_trajectory::RobotTrajectory resampled_traj(
+        move_group.getRobotModel(), move_group.getName());
+
+    moveit::core::RobotStatePtr interpolated_state =
+        std::make_shared<moveit::core::RobotState>(move_group.getRobotModel());
+
+    double prev_t = 0.0;
+    bool first = true;
+    for (double current_time = 0.0; current_time <= total_time + 1e-9; current_time += kFixedTimeStep)
+    {
+        const double t = std::min(current_time, total_time);
+        if (!original_traj.getStateAtDurationFromStart(t, interpolated_state))
+        {
+            std::cerr << "ArmControl: getStateAtDurationFromStart failed at t=" << t
+                      << " for arm_id " << static_cast<int>(arm_id) << std::endl;
+            return false;
+        }
+
+        const double dt_from_prev = first ? 0.0 : (t - prev_t);
+        resampled_traj.addSuffixWayPoint(*interpolated_state, dt_from_prev);
+        first = false;
+        prev_t = t;
+
+        if (t >= total_time - 1e-9) break;
+    }
+
+    // 4) 写回 plan.trajectory_
+    resampled_traj.getRobotTrajectoryMsg(plan.trajectory_);
+
+    // 5) 存入内部 TrajectoryState（使用 msg 内的 time_from_start，应该已经是均匀 dt）
+    const auto& jt = plan.trajectory_.joint_trajectory;
+    if (jt.points.empty())
+    {
+        std::cerr << "ArmControl: Trajectory is empty after resampling for arm_id "
+                  << static_cast<int>(arm_id) << std::endl;
+        return false;
+    }
+
     TrajectoryState* traj_state = (arm_id == 0) ? &left_arm_trajectory_ : &right_arm_trajectory_;
-    
     traj_state->trajectory_points.clear();
-    
-    if (trajectory_points.empty()) {
-        traj_state->is_active = false;
-        return;
-    }
-    
-    size_t num_joints = trajectory_points[0].size();
-    double time_step = 0.0;  // 如果没有时间信息，使用索引作为时间
-    
-    // 将位置向量转换为完整的轨迹点（速度和加速度设为0）
-    for (size_t i = 0; i < trajectory_points.size(); ++i) {
+
+    const size_t num_joints = jt.points[0].positions.size();
+    traj_state->trajectory_points.reserve(jt.points.size());
+
+    for (const auto& point : jt.points)
+    {
         TrajectoryPoint traj_point;
-        traj_point.positions = trajectory_points[i];
-        traj_point.velocities.resize(num_joints, 0.0);
-        traj_point.accelerations.resize(num_joints, 0.0);
-        traj_point.time_from_start = time_step * i;  // 简单的时间估计
-        
-        traj_state->trajectory_points.push_back(traj_point);
+        traj_point.positions = point.positions;
+
+        if (point.velocities.size() == num_joints)
+            traj_point.velocities = point.velocities;
+        else
+            traj_point.velocities.assign(num_joints, 0.0);
+
+        if (point.accelerations.size() == num_joints)
+            traj_point.accelerations = point.accelerations;
+        else
+            traj_point.accelerations.assign(num_joints, 0.0);
+
+        traj_point.time_from_start =
+            static_cast<double>(point.time_from_start.sec) +
+            static_cast<double>(point.time_from_start.nanosec) * 1e-9;
+
+        traj_state->trajectory_points.push_back(std::move(traj_point));
     }
-    
+
     traj_state->current_index = 0;
     traj_state->elapsed_time = 0.0;
     traj_state->is_active = true;
-    
-    std::cout << "ArmControl::StoreTrajectory: Stored " << trajectory_points.size() 
-              << " trajectory points (positions only) for arm_id " << static_cast<int>(arm_id) << std::endl;
+
+    std::cout << "ArmControl::timeParameterizeResampleAndStore_: Resampled points: " << traj_state->trajectory_points.size() << std::endl;
+    return true;
+}
+
+// 辅助函数：更新 joints_info_map_ 从单个轨迹点（区分左右臂）
+void ArmControl::UpdateJointsInfo(uint8_t arm_id, const TrajectoryPoint& point)
+{
+    // 假设 joint_names_ 中前一半是左臂，后一半是右臂
+    const size_t arm_dof = total_joints_ / 2;
+    const size_t offset = (arm_id == 0) ? 0 : arm_dof;
+
+    for (size_t i = 0; i < point.positions.size(); ++i) {
+        const size_t idx = offset + i;
+        if (idx >= joint_names_.size()) {
+            break;
+        }
+        auto& joint_info = joints_info_map_[joint_names_[idx]];
+        joint_info.current_position = point.positions[i];
+        if (i < point.velocities.size()) {
+            joint_info.current_velocity = point.velocities[i];
+            joint_info.command_velocity = point.velocities[i];
+        }
+        if (i < point.accelerations.size()) {
+            joint_info.current_acceleration = point.accelerations[i];
+            joint_info.command_acceleration = point.accelerations[i];
+        }
+    }
+}
+
+// 辅助函数：更新 joints_info_map_ 从插补后的值（区分左右臂）
+void ArmControl::UpdateJointsInfoInterpolated(
+    uint8_t arm_id,
+    const TrajectoryPoint& p1, 
+    const TrajectoryPoint& p2, 
+    double alpha,
+    size_t num_joints,
+    const std::vector<double>& joint_positions)
+{
+    const size_t arm_dof = total_joints_ / 2;
+    const size_t offset = (arm_id == 0) ? 0 : arm_dof;
+
+    for (size_t i = 0; i < num_joints; ++i) {
+        const size_t idx = offset + i;
+        if (idx >= joint_names_.size()) {
+            break;
+        }
+        auto& joint_info = joints_info_map_[joint_names_[idx]];
+        joint_info.current_position = joint_positions[i];
+        
+        // 插补速度
+        if (i < p1.velocities.size() && i < p2.velocities.size()) {
+            double vel = p1.velocities[i] + alpha * (p2.velocities[i] - p1.velocities[i]);
+            joint_info.current_velocity = vel;
+            joint_info.command_velocity = vel;
+        } else if (i < p1.velocities.size()) {
+            joint_info.current_velocity = p1.velocities[i];
+            joint_info.command_velocity = p1.velocities[i];
+        }
+        
+        // 插补加速度
+        if (i < p1.accelerations.size() && i < p2.accelerations.size()) {
+            double acc = p1.accelerations[i] + alpha * (p2.accelerations[i] - p1.accelerations[i]);
+            joint_info.current_acceleration = acc;
+            joint_info.command_acceleration = acc;
+        } else if (i < p1.accelerations.size()) {
+            joint_info.current_acceleration = p1.accelerations[i];
+            joint_info.command_acceleration = p1.accelerations[i];
+        }
+    }
 }
 
 bool ArmControl::GetInterpolatedPoint(uint8_t arm_id, double dt, std::vector<double>& joint_positions)
@@ -328,42 +437,66 @@ bool ArmControl::GetInterpolatedPoint(uint8_t arm_id, double dt, std::vector<dou
         return false;
     }
 
-    if (traj_state->current_index >= traj_state->trajectory_points.size()) {
-        // 轨迹执行完成
-        traj_state->is_active = false;
-        return false;
-    }
-
-    // 获取当前轨迹点
-    const TrajectoryPoint& current_point = traj_state->trajectory_points[traj_state->current_index];
-    
-    // 提取位置、速度、加速度信息
-    joint_positions = current_point.positions;
-    
-    // 更新 joints_info_ 中的速度和加速度
-    for (size_t i = 0; i < joints_info_.size() && i < current_point.positions.size(); i++)
-    {
-        joints_info_[i].current_position = current_point.positions[i];
-        if (i < current_point.velocities.size())
-        {
-            joints_info_[i].current_velocity = current_point.velocities[i];
-            joints_info_[i].command_velocity = current_point.velocities[i];
-        }
-        if (i < current_point.accelerations.size())
-        {
-            joints_info_[i].current_acceleration = current_point.accelerations[i];
-            joints_info_[i].command_acceleration = current_point.accelerations[i];
-        }
-    }
-    
-    // 更新索引和时间
-    traj_state->current_index++;
+    // 累加已执行时间
     traj_state->elapsed_time += dt;
+    // std::cout << "ArmControl::GetInterpolatedPoint: elapsed_time = " << traj_state->elapsed_time << std::endl;
     
-    // 检查是否还有更多点位
-    if (traj_state->current_index >= traj_state->trajectory_points.size()) {
+    // 获取轨迹的总时间（最后一个点的时间）
+    const double total_time = traj_state->trajectory_points.back().time_from_start;
+    // std::cout << "ArmControl::GetInterpolatedPoint: total_time = " << total_time << std::endl;
+    
+    // 如果已执行时间超过总时间，轨迹执行完成
+    if (traj_state->elapsed_time >= total_time - 1e-6) {
+        const TrajectoryPoint& last_point = traj_state->trajectory_points.back();
+        joint_positions = last_point.positions;
+        UpdateJointsInfo(arm_id, last_point);
         traj_state->is_active = false;
-        return false;  // 最后一个点位，下次调用返回false
+        return false;  // 轨迹执行完成
+    }
+    
+    // 利用重采样后的密集点特性，从 current_index 开始查找（时间递增）
+    size_t num_points = traj_state->trajectory_points.size();
+    size_t start_idx = traj_state->current_index;
+    
+    // 从 current_index 开始向前查找，找到包含 elapsed_time 的区间
+    while (start_idx < num_points - 1 && 
+           traj_state->elapsed_time > traj_state->trajectory_points[start_idx + 1].time_from_start) {
+        ++start_idx;
+    }
+    
+    // 如果时间小于第一个点，使用第一个点
+    if (traj_state->elapsed_time < traj_state->trajectory_points[0].time_from_start) {
+        start_idx = 0;
+    }
+    
+    // 更新 current_index 以便下次查找更快
+    traj_state->current_index = start_idx;
+    
+    const TrajectoryPoint& p1 = traj_state->trajectory_points[start_idx];
+    const size_t end_idx = (start_idx < num_points - 1) ? start_idx + 1 : start_idx;
+    const TrajectoryPoint& p2 = traj_state->trajectory_points[end_idx];
+    
+    // 如果两个点的时间相同或已到最后一个点，直接使用当前点
+    if (start_idx == end_idx || 
+        std::abs(p2.time_from_start - p1.time_from_start) < 1e-9) {
+        joint_positions = p1.positions;
+        UpdateJointsInfo(arm_id, p1);
+    } else {
+        // 在两个点之间进行线性插补
+        const double t1 = p1.time_from_start;
+        const double t2 = p2.time_from_start;
+        const double t = traj_state->elapsed_time;
+        const double alpha = (t - t1) / (t2 - t1);  // 插补系数 [0, 1]
+        
+        const size_t num_joints = std::min(p1.positions.size(), p2.positions.size());
+        joint_positions.resize(num_joints);
+        
+        // 对每个关节进行位置、速度、加速度插补
+        for (size_t i = 0; i < num_joints; ++i) {
+            joint_positions[i] = p1.positions[i] + alpha * (p2.positions[i] - p1.positions[i]);
+        }
+        
+        UpdateJointsInfoInterpolated(arm_id, p1, p2, alpha, num_joints, joint_positions);
     }
     
     return true;  // 还有更多点位
@@ -375,6 +508,36 @@ bool ArmControl::HasActiveTrajectory(uint8_t arm_id) const
     return traj_state->is_active && !traj_state->trajectory_points.empty();
 }
 
+bool ArmControl::GetTrajectoryEndPositions(uint8_t arm_id, std::vector<double>& end_positions) const
+{
+    const TrajectoryState* traj_state = (arm_id == 0) ? &left_arm_trajectory_ : &right_arm_trajectory_;
+    if (traj_state->trajectory_points.empty()) {
+        return false;
+    }
+    end_positions = traj_state->trajectory_points.back().positions;
+    return !end_positions.empty();
+}
+
+double ArmControl::GetTrajectoryProgress(uint8_t arm_id) const
+{
+    const TrajectoryState* traj_state = (arm_id == 0) ? &left_arm_trajectory_ : &right_arm_trajectory_;
+    if (traj_state->trajectory_points.empty()) {
+        return 1.0;
+    }
+    const double total_time = traj_state->trajectory_points.back().time_from_start;
+    if (total_time <= 1e-9) {
+        return 1.0;
+    }
+    double p = traj_state->elapsed_time / total_time;
+    if (p < 0.0) p = 0.0;
+    if (p > 1.0) p = 1.0;
+    // 若轨迹已结束但未清空，直接认为完成
+    if (!traj_state->is_active) {
+        p = 1.0;
+    }
+    return p;
+}
+
 void ArmControl::ClearTrajectory(uint8_t arm_id)
 {
     TrajectoryState* traj_state = (arm_id == 0) ? &left_arm_trajectory_ : &right_arm_trajectory_;
@@ -405,14 +568,12 @@ bool ArmControl::MoveToTargetJoint(
     // 如果当前没有活动轨迹，进行规划
     if (!is_moving_)
     {
-        std::vector<std::vector<double>> trajectory_points;
-        
         // 使用默认的arm_id（左臂，如果需要可以根据target_joint判断）
         uint8_t arm_id = current_arm_id_;
         
         // 规划关节空间运动
-        // 注意：PlanJointMotion 内部已经调用了 StoreTrajectoryFromPlan 存储完整轨迹信息
-        if (PlanJointMotion(arm_id, target_joint, 0.5, 0.5, trajectory_points))
+        // 注意：PlanJointMotion 内部已经存入内部 TrajectoryState
+        if (PlanJointMotion(arm_id, target_joint, 0.5, 0.5))
         {
             is_moving_ = true;
             movingDurationTime_ = 0.0;
@@ -430,11 +591,12 @@ bool ArmControl::MoveToTargetJoint(
     if (GetInterpolatedPoint(current_arm_id_, dt, interpolated_joints))
     {
         joint_positions = interpolated_joints;
-        // 更新 joints_info_ 中的指令位置和状态
-        for (size_t i = 0; i < joints_info_.size() && i < interpolated_joints.size(); i++)
+        // 更新 joints_info_map_ 中的指令位置和状态
+        for (size_t i = 0; i < joint_names_.size() && i < interpolated_joints.size(); i++)
         {
-            joints_info_[i].command_position = interpolated_joints[i];
-            joints_info_[i].current_position = interpolated_joints[i];
+            auto& joint_info = joints_info_map_[joint_names_[i]];
+            joint_info.command_position = interpolated_joints[i];
+            joint_info.current_position = interpolated_joints[i];
         }
         // 注意：GetInterpolatedPoint 会更新速度加速度，但需要通过其他方式获取并更新到 joints_info_
         movingDurationTime_ += dt;
@@ -444,15 +606,16 @@ bool ArmControl::MoveToTargetJoint(
     {
         // 轨迹执行完成
         joint_positions = target_joint;
-        // 更新 joints_info_ 中的指令位置和状态
-        for (size_t i = 0; i < joints_info_.size() && i < target_joint.size(); i++)
+        // 更新 joints_info_map_ 中的指令位置和状态
+        for (size_t i = 0; i < joint_names_.size() && i < target_joint.size(); i++)
         {
-            joints_info_[i].command_position = target_joint[i];
-            joints_info_[i].current_position = target_joint[i];
-            joints_info_[i].current_velocity = 0.0;
-            joints_info_[i].command_velocity = 0.0;
-            joints_info_[i].current_acceleration = 0.0;
-            joints_info_[i].command_acceleration = 0.0;
+            auto& joint_info = joints_info_map_[joint_names_[i]];
+            joint_info.command_position = target_joint[i];
+            joint_info.current_position = target_joint[i];
+            joint_info.current_velocity = 0.0;
+            joint_info.command_velocity = 0.0;
+            joint_info.current_acceleration = 0.0;
+            joint_info.command_acceleration = 0.0;
         }
         is_moving_ = false;
         movingDurationTime_ = 0.0;
@@ -464,10 +627,10 @@ bool ArmControl::MoveToTargetJoint(
 bool ArmControl::GoHome(std::vector<double>& joint_positions, double dt)
 {
     // 使用基类的home位置
-    std::vector<double> home_positions(joints_info_.size());
-    for (size_t i = 0; i < joints_info_.size(); i++)
+    std::vector<double> home_positions(joint_names_.size());
+    for (size_t i = 0; i < joint_names_.size(); i++)
     {
-        home_positions[i] = joints_info_[i].home_position;
+        home_positions[i] = joints_info_map_[joint_names_[i]].home_position;
     }
     return MoveToTargetJoint(joint_positions, home_positions, dt);
 }

src/robot_control/src/controllers/control_base.cpp

@@ -13,6 +13,8 @@ ControlBase::ControlBase(
     const std::vector<double>& lengthParameters
 ): 
     total_joints_(joint_names.size()),
+    joint_names_(joint_names),
+    joints_info_map_(joints_info_map),
     lengthParameters_(lengthParameters),
     is_target_set_(false),
     planner_type_(TrajectoryPlannerType::TRAPEZOIDAL)  // 默认使用梯形轨迹规划器
@@ -20,8 +22,7 @@ ControlBase::ControlBase(
     if (total_joints_ <= 0)
         throw std::invalid_argument("Total joints must be positive");
     
-    // 从 joints_info_map 中提取信息，存储到 joints_info_ 中（状态和指令参数已在 JointInfo 构造函数中初始化为0）
-    joints_info_.resize(total_joints_);
+    // 验证所有关节名称都在 joints_info_map 中
     std::vector<double> maxSpeed(total_joints_);
     std::vector<double> maxAcc(total_joints_);
     
@@ -31,13 +32,12 @@ ControlBase::ControlBase(
         auto it = joints_info_map.find(name);
         if (it != joints_info_map.end())
         {
-            joints_info_[i] = it->second;
-            maxSpeed[i] = joints_info_[i].max_velocity;
-            maxAcc[i] = joints_info_[i].max_acceleration;
+            maxSpeed[i] = it->second.max_velocity;
+            maxAcc[i] = it->second.max_acceleration;
         }
         else
         {
-            throw std::invalid_argument("Joint name not found in joints_info_map");
+            throw std::invalid_argument("Joint name not found in joints_info_map: " + name);
         }
     }
     
@@ -63,15 +63,15 @@ ControlBase::~ControlBase()
 
 void ControlBase::SetHomePositions(const std::vector<double>& home_positions)
 {
-    for (size_t i = 0; i < joints_info_.size() && i < home_positions.size(); i++)
+    for (size_t i = 0; i < joint_names_.size() && i < home_positions.size(); i++)
     {
-        joints_info_[i].home_position = home_positions[i];
+        joints_info_map_[joint_names_[i]].home_position = home_positions[i];
     }
 }
 
 bool ControlBase::MoveToTargetJoint(std::vector<double>& joint_positions, const std::vector<double>& target_joint, double dt)
 {
-    if (joints_info_.size() != target_joint.size())
+    if (joint_names_.size() != target_joint.size())
     {
         throw std::invalid_argument("Joint position and target joint size do not match.");
     }
@@ -81,11 +81,11 @@ bool ControlBase::MoveToTargetJoint(std::vector<double>& joint_positions, const
         if (IsReached(target_joint))
         {
             joint_positions = target_joint;
-            // 更新 joints_info_ 中的位置
-            for (size_t i = 0; i < joints_info_.size() && i < target_joint.size(); i++)
+            // 更新 joints_info_map_ 中的位置
+            for (size_t i = 0; i < joint_names_.size() && i < target_joint.size(); i++)
             {
-                joints_info_[i].current_position = target_joint[i];
-                joints_info_[i].command_position = target_joint[i];
+                joints_info_map_[joint_names_[i]].current_position = target_joint[i];
+                joints_info_map_[joint_names_[i]].command_position = target_joint[i];
             }
             return true; 
         }
@@ -128,15 +128,16 @@ bool ControlBase::MoveToTargetJoint(std::vector<double>& joint_positions, const
     if (is_finished)
     {
         joint_positions = target_joint;
-        // 更新 joints_info_ 中的状态
-        for (size_t i = 0; i < joints_info_.size() && i < target_joint.size(); i++)
+        // 更新 joints_info_map_ 中的状态
+        for (size_t i = 0; i < joint_names_.size() && i < target_joint.size(); i++)
         {
-            joints_info_[i].current_position = target_joint[i];
-            joints_info_[i].command_position = target_joint[i];
-            if (i < velocities.size()) joints_info_[i].current_velocity = velocities[i];
-            if (i < velocities.size()) joints_info_[i].command_velocity = velocities[i];
-            if (i < accelerations.size()) joints_info_[i].current_acceleration = accelerations[i];
-            if (i < accelerations.size()) joints_info_[i].command_acceleration = accelerations[i];
+            auto& joint_info = joints_info_map_[joint_names_[i]];
+            joint_info.current_position = target_joint[i];
+            joint_info.command_position = target_joint[i];
+            if (i < velocities.size()) joint_info.current_velocity = velocities[i];
+            if (i < velocities.size()) joint_info.command_velocity = velocities[i];
+            if (i < accelerations.size()) joint_info.current_acceleration = accelerations[i];
+            if (i < accelerations.size()) joint_info.command_acceleration = accelerations[i];
         }
         is_moving_ = false;
         movingDurationTime_ = 0.0;
@@ -144,15 +145,16 @@ bool ControlBase::MoveToTargetJoint(std::vector<double>& joint_positions, const
         return true;
     }
 
-    // 更新 joints_info_ 中的状态
-    for (size_t i = 0; i < joints_info_.size() && i < desired_pos.size(); i++)
+    // 更新 joints_info_map_ 中的状态
+    for (size_t i = 0; i < joint_names_.size() && i < desired_pos.size(); i++)
     {
-        joints_info_[i].current_position = desired_pos[i];
-        joints_info_[i].command_position = desired_pos[i];
-        if (i < velocities.size()) joints_info_[i].current_velocity = velocities[i];
-        if (i < velocities.size()) joints_info_[i].command_velocity = velocities[i];
-        if (i < accelerations.size()) joints_info_[i].current_acceleration = accelerations[i];
-        if (i < accelerations.size()) joints_info_[i].command_acceleration = accelerations[i];
+        auto& joint_info = joints_info_map_[joint_names_[i]];
+        joint_info.current_position = desired_pos[i];
+        joint_info.command_position = desired_pos[i];
+        if (i < velocities.size()) joint_info.current_velocity = velocities[i];
+        if (i < velocities.size()) joint_info.command_velocity = velocities[i];
+        if (i < accelerations.size()) joint_info.current_acceleration = accelerations[i];
+        if (i < accelerations.size()) joint_info.command_acceleration = accelerations[i];
     }
     joint_positions = desired_pos;
 
@@ -162,10 +164,10 @@ bool ControlBase::MoveToTargetJoint(std::vector<double>& joint_positions, const
 bool ControlBase::GoHome(std::vector<double>& joint_positions, double dt)
 {
     // 构建 home 位置向量
-    std::vector<double> home_positions(joints_info_.size());
-    for (size_t i = 0; i < joints_info_.size(); i++)
+    std::vector<double> home_positions(joint_names_.size());
+    for (size_t i = 0; i < joint_names_.size(); i++)
     {
-        home_positions[i] = joints_info_[i].home_position;
+        home_positions[i] = joints_info_map_[joint_names_[i]].home_position;
     }
     
     // 无论是否在运动中，都移动到 home 位置
@@ -201,11 +203,12 @@ bool ControlBase::Stop(std::vector<double>& joint_positions, double dt)
             is_stop_finished = s_curveTrajectory_->isStopFinished(stopDurationTime_);
         }
         
-        // 更新 joints_info_ 中的位置
-        for (size_t i = 0; i < joints_info_.size() && i < stop_positions.size(); i++)
+        // 更新 joints_info_map_ 中的位置
+        for (size_t i = 0; i < joint_names_.size() && i < stop_positions.size(); i++)
         {
-            joints_info_[i].current_position = stop_positions[i];
-            joints_info_[i].command_position = stop_positions[i];
+            auto& joint_info = joints_info_map_[joint_names_[i]];
+            joint_info.current_position = stop_positions[i];
+            joint_info.command_position = stop_positions[i];
         }
         joint_positions = stop_positions;
         
@@ -227,26 +230,27 @@ void ControlBase::UpdateJointStates(
     const std::vector<double>& joint_velocities,
     const std::vector<double>& joint_torques)
 {
-    // 更新 joints_info_ 中的当前状态
-    for (size_t i = 0; i < joints_info_.size() && i < joint_positions.size(); i++)
+    // 更新 joints_info_map_ 中的当前状态
+    for (size_t i = 0; i < joint_names_.size() && i < joint_positions.size(); i++)
     {
-        joints_info_[i].current_position = joint_positions[i];
+        auto& joint_info = joints_info_map_[joint_names_[i]];
+        joint_info.current_position = joint_positions[i];
         if (i < joint_velocities.size())
         {
-            joints_info_[i].current_velocity = joint_velocities[i];
+            joint_info.current_velocity = joint_velocities[i];
         }
         if (i < joint_torques.size())
         {
-            joints_info_[i].current_torque = joint_torques[i];
+            joint_info.current_torque = joint_torques[i];
         }
     }
 
     if( !is_joint_position_initialized_)
     {
         // 初始化命令位置为当前位置
-        for (size_t i = 0; i < joints_info_.size() && i < joint_positions.size(); i++)
+        for (size_t i = 0; i < joint_names_.size() && i < joint_positions.size(); i++)
         {
-            joints_info_[i].command_position = joint_positions[i];
+            joints_info_map_[joint_names_[i]].command_position = joint_positions[i];
         }
         is_joint_position_initialized_ = true;
         std::cout << "joint commands initialized" << std::endl;
@@ -262,9 +266,9 @@ bool ControlBase::IsReached(const std::vector<double>& target_joint)
 {
     bool result = true;
 
-    for (size_t i = 0; i < target_joint.size() && i < joints_info_.size(); i++)
+    for (size_t i = 0; i < target_joint.size() && i < joint_names_.size(); i++)
     {
-        if (std::abs(joints_info_[i].current_position - target_joint[i]) > POSITION_TOLERANCE)
+        if (std::abs(joints_info_map_[joint_names_[i]].current_position - target_joint[i]) > POSITION_TOLERANCE)
         {
             result = false;
             break;
@@ -281,15 +285,16 @@ void ControlBase::SetTrajectoryPlannerType(TrajectoryPlannerType planner_type)
 
 bool ControlBase::checkJointLimits(std::vector<double>& joint_positions)
 {
-    for (size_t i = 0; i < joint_positions.size() && i < joints_info_.size(); i++)
+    for (size_t i = 0; i < joint_positions.size() && i < joint_names_.size(); i++)
     {
-        if (joint_positions[i] < joints_info_[i].min_limit)
+        const auto& joint_info = joints_info_map_[joint_names_[i]];
+        if (joint_positions[i] < joint_info.min_limit)
         {
-            joint_positions[i] = joints_info_[i].min_limit;
+            joint_positions[i] = joint_info.min_limit;
         }
-        else if (joint_positions[i] > joints_info_[i].max_limit)
+        else if (joint_positions[i] > joint_info.max_limit)
         {
-            joint_positions[i] = joints_info_[i].max_limit;
+            joint_positions[i] = joint_info.max_limit;
         }
     }
 
@@ -298,10 +303,10 @@ bool ControlBase::checkJointLimits(std::vector<double>& joint_positions)
 
 std::vector<double> ControlBase::getCurrentPositions() const
 {
-    std::vector<double> positions(joints_info_.size());
-    for (size_t i = 0; i < joints_info_.size(); i++)
+    std::vector<double> positions(joint_names_.size());
+    for (size_t i = 0; i < joint_names_.size(); i++)
     {
-        positions[i] = joints_info_[i].current_position;
+        positions[i] = joints_info_map_.at(joint_names_[i]).current_position;
     }
     return positions;
 }

src/robot_control/src/controllers/leg_control.cpp

@@ -90,9 +90,9 @@ bool LegControl::SetMoveLegParametersInternal(double moveDistance)
     tempPosition.resize(total_joints_, 0.0);
 
     // 使用默认零位置 0.0（已删除 joint_zero_positions_）
-    double backJoint1 = DEG2RAD(joints_info_[0].current_position);
-    double frountJoint1 = DEG2RAD(joints_info_[1].current_position);
-    double frountJoint2 = DEG2RAD(joints_info_[2].current_position);
+    double backJoint1 = DEG2RAD(joints_info_map_[joint_names_[0]].current_position);
+    double frountJoint1 = DEG2RAD(joints_info_map_[joint_names_[1]].current_position);
+    double frountJoint2 = DEG2RAD(joints_info_map_[joint_names_[2]].current_position);
 
     double currentBackLegHeight = lengthParameters_[0] * abs(std::sin(backJoint1));
     double currentFrountLegHeight = lengthParameters_[1] * abs(std::sin(frountJoint1)) + lengthParameters_[2] * abs(std::sin(frountJoint1 + frountJoint2));
@@ -153,7 +153,7 @@ bool LegControl::SetMoveLegParametersInternal(double moveDistance)
     }
 
     // 使用默认零位置 0.0（已删除 joint_zero_positions_）
-    for (size_t i = 0; i < joints_info_.size() && i < tempPosition.size(); i++)
+    for (size_t i = 0; i < joint_names_.size() && i < tempPosition.size(); i++)
     {
         target_joint_positions_[i] = tempPosition[i];
     }

src/robot_control/src/controllers/waist_control.cpp

@@ -81,8 +81,8 @@ bool WaistControl::SetMoveWaistParametersInternal(double movepitch, double movey
     std::vector<double> tempPosition;
     tempPosition.resize(total_joints_, 0.0);
 
-    tempPosition[0] = joints_info_[0].command_position - joints_info_[0].home_position + movepitch;
-    tempPosition[1] = joints_info_[1].command_position - joints_info_[1].home_position + moveyaw;
+    tempPosition[0] = joints_info_map_[joint_names_[0]].command_position - joints_info_map_[joint_names_[0]].home_position + movepitch;
+    tempPosition[1] = joints_info_map_[joint_names_[1]].command_position - joints_info_map_[joint_names_[1]].home_position + moveyaw;
 
     if (!checkJointLimits(tempPosition))
     {
@@ -90,9 +90,9 @@ bool WaistControl::SetMoveWaistParametersInternal(double movepitch, double movey
         return false;
     }
 
-    for (size_t i = 0; i < joints_info_.size() && i < tempPosition.size(); i++)
+    for (size_t i = 0; i < joint_names_.size() && i < tempPosition.size(); i++)
     {
-        target_joint_positions_[i] = tempPosition[i] + joints_info_[i].home_position;
+        target_joint_positions_[i] = tempPosition[i] + joints_info_map_[joint_names_[i]].home_position;
     }
     
     std::cout << "Waist Target Joint Position: " << target_joint_positions_[0] << ", " << target_joint_positions_[1] << std::endl;

src/robot_control/src/controllers/wheel_control.cpp

@@ -38,7 +38,7 @@ bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, doub
 
     for (size_t i = 0; i < total_joints_; i++)
     {
-        tempHomePositions[i] = joints_info_[i].home_position;
+        tempHomePositions[i] = joints_info_map_[joint_names_[i]].home_position;
         tempDesiredPositions[i] = target_joint_positions_[i];
     }
     
@@ -52,11 +52,11 @@ bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, doub
     {
         if (beta > 0)
         {
-            tempHomePositions[0] = joints_info_[0].home_position - beta - 8;
+            tempHomePositions[0] = joints_info_map_[joint_names_[0]].home_position - beta - 8;
         }
         else
         {
-            tempHomePositions[0] = joints_info_[0].home_position - beta + 8;
+            tempHomePositions[0] = joints_info_map_[joint_names_[0]].home_position - beta + 8;
         }
     }
 
@@ -65,12 +65,12 @@ bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, doub
         tempAdjustCount ++; 
         if (beta > 0)
         {
-            tempHomePositions[0] = joints_info_[0].home_position - beta * 2.0;
+            tempHomePositions[0] = joints_info_map_[joint_names_[0]].home_position - beta * 2.0;
             
         }
         else
         {
-            tempHomePositions[0] = joints_info_[0].home_position - beta;
+            tempHomePositions[0] = joints_info_map_[joint_names_[0]].home_position - beta;
         }
 
          if (tempAdjustCount == 1)
@@ -105,7 +105,7 @@ bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, doub
         // 使用默认方向 1（正方向）
         for (size_t i = 0; i < total_joints_; i++)
         {
-            target_joint_positions_[i] = joints_info_[i].home_position + theta;
+            target_joint_positions_[i] = joints_info_map_[joint_names_[i]].home_position + theta;
         }
     }
     else
@@ -114,7 +114,7 @@ bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, doub
         for (size_t i = 0; i < total_joints_; i++)
         {
            target_joint_positions_[i] = tempDesiredPositions[i];
-           joints_info_[i].home_position = tempHomePositions[i];
+           joints_info_map_[joint_names_[i]].home_position = tempHomePositions[i];
         }
     }
 
@@ -145,7 +145,7 @@ bool WheelControl::SetMoveWheelParametersInternalJog(double moveWheelDistance, d
 
     for (size_t i = 0; i < total_joints_; i++)
     {
-        tempHomePositions[i] = joints_info_[i].home_position;
+        tempHomePositions[i] = joints_info_map_[joint_names_[i]].home_position;
         tempDesiredPositions[i] = target_joint_positions_[i];
     }
     
@@ -156,8 +156,8 @@ bool WheelControl::SetMoveWheelParametersInternalJog(double moveWheelDistance, d
     double beta = moveWheelAngle / 360.0 * M_PI * 2 * lengthParameters_[1] / lengthParameters_[0] * 180.0 / M_PI;
 
     // 使用默认方向 1（正方向）
-    tempHomePositions[0] = joints_info_[0].home_position + beta;
-    tempHomePositions[1] = joints_info_[1].home_position + beta;
+    tempHomePositions[0] = joints_info_map_[joint_names_[0]].home_position + beta;
+    tempHomePositions[1] = joints_info_map_[joint_names_[1]].home_position + beta;
 
     for (size_t i = 0; i < total_joints_; i++)
     {
@@ -170,7 +170,7 @@ bool WheelControl::SetMoveWheelParametersInternalJog(double moveWheelDistance, d
     for (size_t i = 0; i < total_joints_; i++)
     {
         target_joint_positions_[i] = tempDesiredPositions[i];
-        joints_info_[i].home_position = tempHomePositions[i];
+        joints_info_map_[joint_names_[i]].home_position = tempHomePositions[i];
     }
 
 
@@ -201,8 +201,8 @@ bool WheelControl::MoveWheel(std::vector<double>& joint_positions)
         }
     }
 
-    joint_positions.resize(joints_info_.size());
-    for (size_t i = 0; i < joints_info_.size(); i++)
+    joint_positions.resize(joint_names_.size());
+    for (size_t i = 0; i < joint_names_.size(); i++)
     {
         joint_positions[i] = target_joint_positions_[i];
     }

src/robot_control/src/core/motion_parameters.cpp

@@ -55,14 +55,14 @@ MotionParameters::MotionParameters()
     leg_joint_names_ = {"left_rear_hip_joint", "left_rear_knee_joint", "right_rear_hip_joint", 
                         "right_rear_knee_joint", "left_front_hip_joint", "left_front_knee_joint"};  // 腿部关节名称列表
     // 双臂关节名称列表（合并左右臂）
-    // dual_arm_joint_names_ = {"left_arm_joint_1", "left_arm_joint_2", "left_arm_joint_3", 
-    //                           "left_arm_joint_4", "left_arm_joint_5", "left_arm_joint_6",
-    //                           "right_arm_joint_1", "right_arm_joint_2", "right_arm_joint_3", 
-    //                           "right_arm_joint_4", "right_arm_joint_5", "right_arm_joint_6"};
-    dual_arm_joint_names_ = {"joint_1", "joint_2", " joint_3", 
-                              "joint_4", "joint_5", "joint_6",
-                              "joint_7", "joint_8", "joint_9", 
-                              "joint_10", "joint_11", "joint_12"};
+    dual_arm_joint_names_ = {"left_arm_joint_1", "left_arm_joint_2", "left_arm_joint_3", 
+                              "left_arm_joint_4", "left_arm_joint_5", "left_arm_joint_6",
+                              "right_arm_joint_1", "right_arm_joint_2", "right_arm_joint_3", 
+                              "right_arm_joint_4", "right_arm_joint_5", "right_arm_joint_6"};
+    // dual_arm_joint_names_ = {"joint_1", "joint_2", " joint_3", 
+    //                           "joint_4", "joint_5", "joint_6",
+    //                           "joint_7", "joint_8", "joint_9", 
+    //                           "joint_10", "joint_11", "joint_12"};
 
     // ==================== 手臂参数 ====================
     // 每条手臂的自由度数（关节数）
@@ -99,13 +99,31 @@ MotionParameters::MotionParameters()
         joints_info_map_[leg_joint_names_[i]] = JointInfo(max_limit, min_limit, LimitType::POSITION, 50.0, 100.0, home_pos);
     }
     
-    // 双臂关节：默认限位±180度，速度30度/秒，加速度80度/秒²，Home位置0.0（弧度）
+    // 双臂关节（单位：弧度）
+    // 按每个关节单独设置限位（建议与 dual_arm.urdf 中的 lower/upper 保持一致）。
     std::vector<double> dual_arm_home = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0,  // 左臂Home位置
                                          0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; // 右臂Home位置
+
+    // 12个关节的限位（左臂6 + 右臂6），单位：rad
+    // 当前值来自 dual_arm_description/urdf/dual_arm.urdf（全部为 [-3.14, 3.14]），
+    // 后续你可以按关节逐一改成更真实的机械限位（例如某些关节是非对称范围）。
+    const std::vector<double> dual_arm_min_limits = {
+        -3.14, -1.57, -1.57, -3.14, -1.57, -3.14,   // left_arm_joint_1..6
+        -3.14, -1.57, -1.57, -3.14, -1.57, -3.14    // right_arm_joint_1..6
+    };
+    const std::vector<double> dual_arm_max_limits = {
+         3.14,  1.57,  1.57,  3.14,  1.57,  3.14,   // left_arm_joint_1..6
+         3.14,  1.57,  1.57,  3.14,  1.57,  3.14    // right_arm_joint_1..6
+    };
+
     for (size_t i = 0; i < dual_arm_joint_names_.size(); i++)
     {
         double home_pos = (i < dual_arm_home.size()) ? dual_arm_home[i] : 0.0;
-        joints_info_map_[dual_arm_joint_names_[i]] = JointInfo(180.0, -180.0, LimitType::POSITION, 30.0, 80.0, home_pos);
+        const double min_limit = (i < dual_arm_min_limits.size()) ? dual_arm_min_limits[i] : -M_PI;
+        const double max_limit = (i < dual_arm_max_limits.size()) ? dual_arm_max_limits[i] :  M_PI;
+        const double max_vel = DEG2RAD(30.0);    // 30 deg/s -> rad/s
+        const double max_acc = DEG2RAD(80.0);    // 80 deg/s^2 -> rad/s^2
+        joints_info_map_[dual_arm_joint_names_[i]] = JointInfo(max_limit, min_limit, LimitType::POSITION, max_vel, max_acc, home_pos);
     }
 
     total_joints_ = wheel_joint_names_.size() + waist_joint_names_.size() + leg_joint_names_.size() + dual_arm_joint_names_.size();

src/robot_control/src/core/robot_control_manager.cpp

@@ -6,7 +6,9 @@
  */
 
 #include "core/robot_control_manager.hpp"
+#include <cmath>
 #include <limits>
+#include <string>
 
 using namespace robot_control;
 using namespace std;
@@ -92,6 +94,52 @@ RobotControlManager::~RobotControlManager()
     // 智能指针会自动释放，无需手动 delete
 }
 
+bool RobotControlManager::ValidateArmJointTarget(
+    uint8_t arm_id,
+    const std::vector<double>& target_joints,
+    std::string* error_msg) const
+{
+    const size_t ARM_DOF = motionParams_.arm_dof_;
+
+    if (arm_id != 0 && arm_id != 1) {
+        if (error_msg) *error_msg = "Invalid arm_id (expected 0=left, 1=right)";
+        return false;
+    }
+    if (target_joints.size() != ARM_DOF) {
+        if (error_msg) {
+            *error_msg = "target_joints size mismatch (expected " + std::to_string(ARM_DOF) +
+                         ", got " + std::to_string(target_joints.size()) + ")";
+        }
+        return false;
+    }
+    if (motionParams_.dual_arm_joint_names_.size() < 2 * ARM_DOF) {
+        if (error_msg) *error_msg = "dual_arm_joint_names_ size is invalid";
+        return false;
+    }
+
+    const size_t start_idx = (arm_id == 0) ? 0 : ARM_DOF;
+    for (size_t i = 0; i < ARM_DOF; ++i) {
+        const std::string& joint_name = motionParams_.dual_arm_joint_names_[start_idx + i];
+        auto it = motionParams_.joints_info_map_.find(joint_name);
+        if (it == motionParams_.joints_info_map_.end()) {
+            if (error_msg) *error_msg = "JointInfo not found for joint: " + joint_name;
+            return false;
+        }
+        const auto& info = it->second;
+        const double v = target_joints[i];
+        if (v < info.min_limit || v > info.max_limit) {
+            if (error_msg) {
+                *error_msg = "Joint " + joint_name + " out of limits: value=" + std::to_string(v) +
+                             ", min=" + std::to_string(info.min_limit) +
+                             ", max=" + std::to_string(info.max_limit);
+            }
+            return false;
+        }
+    }
+
+    return true;
+}
+
 void RobotControlManager::SetJogWheel(bool value)
 {
     is_wheel_jog_ = value;
@@ -148,7 +196,7 @@ bool RobotControlManager::SetArmJointCommands(int8_t body_id, const std::vector<
         jointCommands_.data.resize(motionParams_.total_joints_, 0.0);
     }
     
-    // 设置关节命令（单位：度），通过关节名称获取索引
+    // 设置关节命令（内部统一单位：弧度）
     for (size_t i = 0; i < ARM_DOF; ++i) {
         const std::string& joint_name = motionParams_.dual_arm_joint_names_[start_idx + i];
         auto it = motionParams_.joint_name_to_index_.find(joint_name);
@@ -178,8 +226,7 @@ bool RobotControlManager::InitializeArmMoveIt(rclcpp::Node* node)
 bool RobotControlManager::PlanArmMotion(
     const ArmMotionParams& arm_params,
     double velocity_scaling,
-    double acceleration_scaling,
-    std::vector<std::vector<double>>& trajectory_points)
+    double acceleration_scaling)
 {
     if (!arm_controller_enabled_ || !arm_controller_) {
         return false;
@@ -192,43 +239,41 @@ bool RobotControlManager::PlanArmMotion(
     if (arm_params.motion_type == arm_params.MOVEJ) {
         // 关节空间运动
         if (arm_params.target_joints.empty()) {
+            std::cout << "Target joints empty" << std::endl;
             return false;
         }
+        {
+            std::string err;
+            if (!ValidateArmJointTarget(arm_params.arm_id, arm_params.target_joints, &err)) {
+                std::cout << "PlanArmMotion: target_joints exceed limits: " << err << std::endl;
+                return false;  // 在规划之前直接失败
+            }
+        }
+        std::cout << "Plan Joint Motion" << std::endl;
         return arm_controller_->PlanJointMotion(
             arm_params.arm_id,
             arm_params.target_joints,  // 已经是弧度
             velocity_scaling,
-            acceleration_scaling,
-            trajectory_points);
+            acceleration_scaling);
     } else if (arm_params.motion_type == arm_params.MOVEL) {
         // 笛卡尔空间运动
+        std::cout << "Plan Cartesian Motion" << std::endl;
         return arm_controller_->PlanCartesianMotion(
             arm_params.arm_id,
             arm_params.target_pose,
             velocity_scaling,
-            acceleration_scaling,
-            trajectory_points);
+            acceleration_scaling);
     }
 
     return false;
 }
 
-void RobotControlManager::StoreArmTrajectory(
-    const ArmMotionParams& arm_params,
-    const std::vector<std::vector<double>>& trajectory_points)
-{
-    if (!arm_controller_enabled_ || !arm_controller_) {
-        return;
-    }
-
-    arm_controller_->StoreTrajectory(trajectory_points, arm_params.arm_id);
-}
-
 bool RobotControlManager::GetArmInterpolatedPoint(
     uint8_t arm_id,
     double dt,
     std::vector<double>& joint_positions)
 {
+
     if (!arm_controller_enabled_ || !arm_controller_) {
         return false;
     }
@@ -245,6 +290,22 @@ bool RobotControlManager::HasActiveArmTrajectory(uint8_t arm_id) const
     return arm_controller_->HasActiveTrajectory(arm_id);
 }
 
+bool RobotControlManager::GetArmTrajectoryEndPositions(uint8_t arm_id, std::vector<double>& end_positions) const
+{
+    if (!arm_controller_enabled_ || !arm_controller_) {
+        return false;
+    }
+    return arm_controller_->GetTrajectoryEndPositions(arm_id, end_positions);
+}
+
+double RobotControlManager::GetArmTrajectoryProgress(uint8_t arm_id) const
+{
+    if (!arm_controller_enabled_ || !arm_controller_) {
+        return 1.0;
+    }
+    return arm_controller_->GetTrajectoryProgress(arm_id);
+}
+
 bool RobotControlManager::MoveArm(double dt)
 {
     if (!arm_controller_enabled_ || !arm_controller_) {
@@ -263,13 +324,7 @@ bool RobotControlManager::MoveArm(double dt)
     if (left_arm_active) {
         std::vector<double> left_joint_positions;
         if (GetArmInterpolatedPoint(0, dt, left_joint_positions)) {
-            // 还有更多点位，转换并设置关节命令
-            std::vector<double> left_joint_degrees;
-            left_joint_degrees.reserve(left_joint_positions.size());
-            for (double rad : left_joint_positions) {
-                left_joint_degrees.push_back(rad * 180.0 / M_PI);  // 弧度转度
-            }
-            SetArmJointCommands(0, left_joint_degrees);
+            SetArmJointCommands(0, left_joint_positions);
         }
     }
 
@@ -277,13 +332,7 @@ bool RobotControlManager::MoveArm(double dt)
     if (right_arm_active) {
         std::vector<double> right_joint_positions;
         if (GetArmInterpolatedPoint(1, dt, right_joint_positions)) {
-            // 还有更多点位，转换并设置关节命令
-            std::vector<double> right_joint_degrees;
-            right_joint_degrees.reserve(right_joint_positions.size());
-            for (double rad : right_joint_positions) {
-                right_joint_degrees.push_back(rad * 180.0 / M_PI);  // 弧度转度
-            }
-            SetArmJointCommands(1, right_joint_degrees);
+            SetArmJointCommands(1, right_joint_positions);
         }
     }
 
@@ -516,6 +565,19 @@ void RobotControlManager::UpdateWheelStates(const MotorPos::SharedPtr msg)
 
 void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::SharedPtr msg) 
 {
+
+    if (!msg) {
+        std::cout << "UpdateJointStates: null msg" << std::endl;
+        return;
+    }
+
+    // JointState 基本一致性检查：name/position 必须等长
+    if (msg->name.size() != msg->position.size()) {
+        std::cout << "UpdateJointStates: name.size(" << msg->name.size()
+                  << ") != position.size(" << msg->position.size() << ")" << std::endl;
+        return;
+    }
+
     jointStates_ = *msg;
     
     // 首次收到 JointState 消息时，初始化关节名称映射
@@ -526,64 +588,56 @@ void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::
         jointInited_.resize(motionParams_.total_joints_, false);
         jointCommands_.data.resize(motionParams_.total_joints_, 0.0);
         std::cout << "Joint name mapping initialized with " << motionParams_.total_joints_ << " joints" << std::endl;
-    }
-    
-    jointPositions_.resize(motionParams_.total_joints_);
-    jointVelocities_.resize(motionParams_.total_joints_);
-    jointEfforts_.resize(motionParams_.total_joints_);
-
-    // 使用关节名称匹配来更新状态
-    for (size_t i = 0; i < jointStates_.position.size() && i < jointStates_.name.size(); i++)
-    {
-        const std::string& joint_name = jointStates_.name[i];
-        
-        // 如果名称映射已初始化，通过名称找到内部索引
-        if (joint_name_mapping_initialized_ && motionParams_.joint_name_to_index_.find(joint_name) != motionParams_.joint_name_to_index_.end())
-        {
-            size_t internal_index = motionParams_.joint_name_to_index_[joint_name];
-            if (internal_index < jointPositions_.size())
-            {
-                // pulse_to_degree 已删除，使用默认值 1.0
-                double pulse_to_degree = 1.0;
-                
-                jointPositions_[internal_index] = jointStates_.position[i] * pulse_to_degree;
-                jointVelocities_[internal_index] = (i < jointStates_.velocity.size()) ? jointStates_.velocity[i] * pulse_to_degree : 0.0;
-                jointEfforts_[internal_index] = (i < jointStates_.effort.size()) ? jointStates_.effort[i] : 0.0;
-            }
-        }
-        else
-        {
-            std::cout << "joint name mapping not initialized" << std::endl;
-            return;
-        }
+        jointPositions_.resize(motionParams_.total_joints_);
+        jointVelocities_.resize(motionParams_.total_joints_);
+        jointEfforts_.resize(motionParams_.total_joints_);
     }
 
-    // TODO: This block can be deleted
-    for (size_t i = 0; i < motionParams_.total_joints_; i++)
+    for (size_t i = 0; i < jointStates_.name.size(); i++)
     {
-       if(!jointInited_[i])
-       {
-            if(jointPositions_[i] != 0)
-            {
-                jointInited_[i] = true;
-                std::cout << "get joint feedback, joint" << i + 1 << "  " << jointPositions_[i] << std::endl;
+        if (joint_name_mapping_initialized_ && motionParams_.joint_name_to_index_.find(jointStates_.name[i]) != motionParams_.joint_name_to_index_.end())
+        {
+            size_t internal_index = motionParams_.joint_name_to_index_[jointStates_.name[i]];
+
+            const double pos = jointStates_.position[i];
+            // 过滤 NaN/Inf，避免传播脏数据
+            jointPositions_[internal_index] = std::isfinite(pos) ? pos : 0.0;
+
+            // velocity/effort 允许为空或更短，必须做边界保护
+            if (i < jointStates_.velocity.size() && std::isfinite(jointStates_.velocity[i])) {
+                jointVelocities_[internal_index] = jointStates_.velocity[i];
+            } else {
+                jointVelocities_[internal_index] = 0.0;
             }
-       }
+
+            if (i < jointStates_.effort.size() && std::isfinite(jointStates_.effort[i])) {
+                jointEfforts_[internal_index] = jointStates_.effort[i];
+            } else {
+                jointEfforts_[internal_index] = 0.0;
+            }
+        }
     }
 
     static int count = 0;
-    if (isAllTrueManual(jointInited_) && !isJointInitValueSet_)
+    if (!isJointInitValueSet_)
     {
-        if (count > 50)
+        if (count > 500)
         {
+            for (size_t i = 0; i<jointPositions_.size(); i++)
+            {
+                jointPositions_[i] = 0.0;
+            }   
+            // 用已映射后的 jointPositions_ 初始化 command，并裁剪到关节限位内
             jointCommands_.data = jointPositions_;
+            CheckJointLimits(jointCommands_);
+
             isJointInitValueSet_ = true;
             count = 0;
             std::cout << "Joint commands set to joint positions" << std::endl;
             std::cout << "All joints are initialized" << std::endl;
             for (size_t i = 0; i < motionParams_.total_joints_; i++)
             {
-                std::cout << "Joint positions: " << i + 1 << "  " << jointCommands_.data[i] << std::endl;
+                std::cout << jointStates_.name[i] << "  " << jointStates_.position[i] << std::endl;
             }
         }
         count++;

src/robot_control/src/core/robot_control_node.cpp

@@ -4,6 +4,8 @@
 #include "actions/action_manager.hpp"
 #include "core/motion_parameters.hpp"
 #include "core/common_enum.hpp"
+#include <chrono>
+#include <future>
 
 using namespace robot_control;
 
@@ -15,11 +17,11 @@ namespace fs = std::filesystem;
 RobotControlNode::RobotControlNode() : Node("robot_control_node")
 {
     RCLCPP_INFO(this->get_logger(), "robot_control_node Node is creating");
-    lastSpeed_ = 51;
+    is_robot_enabled_ = false;
  
     // 初始化数据文件（设置路径，确保目录存在）
     #if RECORD_FLAG
-        data_file_path_ = "/home/demo/ros2_joint_data.txt";  // 数据保存路径
+        data_file_path_ = "/home/nvidia/disk/ros2_joint_data.txt";  // 数据保存路径
         fs::path file_path(data_file_path_);
         fs::create_directories(file_path.parent_path());  // 自动创建父目录（如果不存在）
         data_file_.open(data_file_path_, std::ios::trunc);
@@ -36,6 +38,7 @@ RobotControlNode::RobotControlNode() : Node("robot_control_node")
     motorParamClient_ = this->create_client<MotorParam>("/motor_param");
     jointTrajectoryPub_ = this->create_publisher<trajectory_msgs::msg::JointTrajectory>("/trajectory_controller/joint_trajectory", 10);
     gpioPub_ = this->create_publisher<control_msgs::msg::DynamicInterfaceGroupValues>("/gpio_command_controller/commands", 10);
+    switch_client_ = create_client<controller_manager_msgs::srv::SwitchController>("/controller_manager/switch_controller");
     imuMsgSub_ = this->create_subscription<ImuMsg>("/openzen/imu_msg", 10,std::bind(&RobotControlNode::ImuMsgCallback, this, std::placeholders::_1));
     jointStatesSub_ = this->create_subscription<sensor_msgs::msg::JointState>("/joint_states", 10,std::bind(&RobotControlNode::JointStatesCallback, this, std::placeholders::_1));
     wheelStatesSub_ = this->create_subscription<MotorPos>("/motor_pos", 10,std::bind(&RobotControlNode::WheelStatesCallback, this, std::placeholders::_1));
@@ -60,6 +63,24 @@ RobotControlNode::RobotControlNode() : Node("robot_control_node")
         motorParamClient_);
     action_manager_->initialize();
 
+    bool ret=activateController("trajectory_controller");
+    if(ret)
+    {
+        std::cout << "切换控制器成功!" << std::endl;
+    }
+    else
+    {
+        std::cout << "切换控制器失败!" << std::endl;
+    }
+    motor_enable(0, 1);
+
+    // 初始化MoveIt（需要在ROS节点创建后调用）
+    if (robotControlManager_.InitializeArmMoveIt(this)) {
+        RCLCPP_INFO(this->get_logger(), "MoveIt initialized successfully");
+    } else {
+        RCLCPP_WARN(this->get_logger(), "MoveIt initialization failed or arm controller not enabled");
+    }
+
     lastTime_ = this->now();  // 初始化时间
     std::cout << "RobotFsm Node is created finished!" << std::endl;
 }
@@ -134,14 +155,22 @@ void RobotControlNode::ControlLoop()
     {
         if(robotControlManager_.MoveArm(dt_sec))
         {
-            // 轨迹执行完成
             action_manager_->set_dual_arm_executing(false);
         }
     }
 
     if (robotControlManager_.RobotInitFinished())
     {
-        Publish_joint_trajectory();
+        if(!is_robot_enabled_)
+        {
+            motor_reset_fault_all();
+            is_robot_enabled_ = true;
+            motor_enable(0, 1);
+        }
+        else
+        {
+            Publish_joint_trajectory();
+        }
     }
 }
 
@@ -174,8 +203,10 @@ void RobotControlNode::Publish_joint_trajectory()
 
     // 创建 JointTrajectory 消息
     trajectory_msgs::msg::JointTrajectory trajectory_msg;
-    trajectory_msg.header.stamp = this->now();
-    trajectory_msg.header.frame_id = "";
+    // joint_trajectory_controller 要求：要么 header.stamp=0 表示立即开始，
+    // 要么 header.stamp 是未来时间；同时 points[0].time_from_start 必须为正且单调递增。
+    trajectory_msg.header.stamp.sec = 0;
+    trajectory_msg.header.stamp.nanosec = 0;  // start ASAP
     
     const MotionParameters& motion_params = robotControlManager_.GetMotionParameters();
     size_t total_joints = motion_params.total_joints_;
@@ -200,11 +231,30 @@ void RobotControlNode::Publish_joint_trajectory()
     {
         point.positions[i] = cmd_msg.data[i];
     }
-    
-    point.time_from_start.sec = 0;
-    point.time_from_start.nanosec = 0;
+
+    // 单点轨迹也需要一个 >0 的 time_from_start（用控制周期）
+    const double dt = static_cast<double>(CYCLE_TIME) / 1000.0;
+    point.time_from_start.sec = static_cast<int32_t>(dt);
+    point.time_from_start.nanosec =
+        static_cast<uint32_t>((dt - static_cast<double>(point.time_from_start.sec)) * 1e9);
     
     trajectory_msg.points.push_back(point);
+
+    // std::cout << "Publish Joint Trajectory" << std::endl;
+    // std::cout << "trajectory_msg.points.size(): " << trajectory_msg.points.size() << std::endl;
+    // std::cout << "trajectory_msg.points[0].positions.size(): " << trajectory_msg.points[0].positions.size() << std::endl;
+    // std::cout << "trajectory_msg.points[0].positions[0]: " << trajectory_msg.points[0].positions[0] << std::endl;
+    // std::cout << "trajectory_msg.points[0].positions[1]: " << trajectory_msg.points[0].positions[1] << std::endl;
+    // std::cout << "trajectory_msg.points[0].positions[2]: " << trajectory_msg.points[0].positions[2] << std::endl;
+    // std::cout << "trajectory_msg.points[0].positions[3]: " << trajectory_msg.points[0].positions[3] << std::endl;
+    // std::cout << "trajectory_msg.points[0].positions[4]: " << trajectory_msg.points[0].positions[4] << std::endl;
+    // std::cout << "trajectory_msg.points[0].positions[5]: " << trajectory_msg.points[0].positions[5] << std::endl;
+    // std::cout << "trajectory_msg.points[0].positions[6]: " << trajectory_msg.points[0].positions[6] << std::endl;
+    // std::cout << "trajectory_msg.points[0].positions[7]: " << trajectory_msg.points[0].positions[7] << std::endl;
+    // std::cout << "trajectory_msg.points[0].positions[8]: " << trajectory_msg.points[0].positions[8] << std::endl;
+    // std::cout << "trajectory_msg.points[0].positions[9]: " << trajectory_msg.points[0].positions[9] << std::endl;
+    // std::cout << "trajectory_msg.points[0].positions[10]: " << trajectory_msg.points[0].positions[10] << std::endl;
+    // std::cout << "trajectory_msg.points[0].positions[11]: " << trajectory_msg.points[0].positions[11] << std::endl;
     
     // 发布关节轨迹
     jointTrajectoryPub_->publish(trajectory_msg);
@@ -250,42 +300,99 @@ void RobotControlNode::WheelStatesCallback(const MotorPos::SharedPtr cmd_msg)
 
 bool RobotControlNode::activateController(const std::string& controller_name)
 {
-    // TODO: Implement controller activation if needed
-    // This function is currently not used in the codebase
-    (void)controller_name;  // Suppress unused parameter warning
+    // 通过 controller_manager 的 switch_controller 服务激活指定控制器
+    if (!switch_client_)
+    {
+        RCLCPP_ERROR(this->get_logger(), "switch_client_ is null");
+        return false;
+    }
+
+    if (controller_name.empty())
+    {
+        RCLCPP_ERROR(this->get_logger(), "activateController: controller_name is empty");
+        return false;
+    }
+
+    if (!switch_client_->wait_for_service(std::chrono::seconds(2)))
+    {
+        RCLCPP_ERROR(
+            this->get_logger(),
+            "activateController: service /controller_manager/switch_controller not available");
+        return false;
+    }
+
+    auto request = std::make_shared<controller_manager_msgs::srv::SwitchController::Request>();
+    request->activate_controllers = {controller_name};
+    request->deactivate_controllers.clear();
+    request->strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT;
+    request->activate_asap = true;
+    request->timeout.sec = 5;
+    request->timeout.nanosec = 0;
+
+    auto future = switch_client_->async_send_request(request);
+
     return true;
 }
 
+void RobotControlNode::motor_fault(int id, double fault)
+{
+    if (!gpioPub_) return;
+
+    control_msgs::msg::DynamicInterfaceGroupValues msg;
+    const MotionParameters& motion_params = robotControlManager_.GetMotionParameters();
+
+    // id == 0: apply to all joints
+    // id != 0: apply only to that joint index (1-based), others set to 0
+    for (size_t i = 0; i < motion_params.all_joint_names_.size(); ++i) {
+        const std::string& group = motion_params.all_joint_names_[i];
+        msg.interface_groups.push_back(group);
+
+        control_msgs::msg::InterfaceValue v;
+        v.interface_names = {"fault"};
+        if (id == 0) {
+            v.values = {fault};
+        } else {
+            v.values = {(static_cast<int>(i) + 1 == id) ? fault : 0.0};
+        }
+        msg.interface_values.push_back(v);
+    }
+
+    gpioPub_->publish(msg);
+}
+
+void RobotControlNode::motor_enable(int id, double enable)
+{
+    if (!gpioPub_) return;
+
+    control_msgs::msg::DynamicInterfaceGroupValues msg;
+    const MotionParameters& motion_params = robotControlManager_.GetMotionParameters();
+
+    for (size_t i = 0; i < motion_params.all_joint_names_.size(); ++i) {
+        const std::string& group = motion_params.all_joint_names_[i];
+        msg.interface_groups.push_back(group);
+
+        control_msgs::msg::InterfaceValue v;
+        v.interface_names = {"enable"};
+        if (id == 0) {
+            v.values = {enable};
+        } else {
+            v.values = {(static_cast<int>(i) + 1 == id) ? enable : 0.0};
+        }
+        msg.interface_values.push_back(v);
+    }
+
+    gpioPub_->publish(msg);
+}
+
 void RobotControlNode::motor_reset_fault_all()
 {
-    control_msgs::msg::DynamicInterfaceGroupValues faultMsg_;
-    const MotionParameters& motion_params = robotControlManager_.GetMotionParameters();
-    for(size_t i=0;i<motion_params.all_joint_names_.size();i++){
-        std::string tempInterfaceGroup = motion_params.all_joint_names_[i];
-        faultMsg_.interface_groups.push_back(tempInterfaceGroup);
-        control_msgs::msg::InterfaceValue tempValue;
-        //reset
-        tempValue.interface_names = {"fault"};
-        tempValue.values = {1};
-        faultMsg_.interface_values.push_back(tempValue);
-    }
-    gpioPub_->publish(faultMsg_);
-    usleep(20000);
+    motor_fault(0, 1);
+    usleep(500);
+    motor_fault(0, 0);
 }
 
 void RobotControlNode::motor_enable_all(double enable)
 {
-    control_msgs::msg::DynamicInterfaceGroupValues enableMsg_;
-    const MotionParameters& motion_params = robotControlManager_.GetMotionParameters();
-    for(size_t i=0;i<motion_params.all_joint_names_.size();i++){
-        std::string tempInterfaceGroup = motion_params.all_joint_names_[i]; 
-        enableMsg_.interface_groups.push_back(tempInterfaceGroup);
-        control_msgs::msg::InterfaceValue tempValue;
-        //enable
-        tempValue.interface_names = {"enable"};
-        tempValue.values = {enable};
-        enableMsg_.interface_values.push_back(tempValue);
-    }
-    gpioPub_->publish(enableMsg_);
+    motor_enable(0, enable);
     usleep(20000);
 }

src/robot_control/src/utils/plot_joint_trajectory.py

@@ -0,0 +1,70 @@
+import numpy as np
+import matplotlib.pyplot as plt
+import os
+
+def plot_joint_trajectories(file_path):
+    # 检查文件是否存在
+    if not os.path.exists(file_path):
+        print(f"错误：文件不存在 - {file_path}")
+        return
+
+    # 读取数据
+    timestamps = []
+    joint_data = []  # 存储所有关节数据，格式：[ [关节1数据], [关节2数据], ... ]
+
+    with open(file_path, 'r') as f:
+        for line in f:
+            line = line.strip()
+            if not line:
+                continue
+            
+            # 解析一行数据（时间戳 + 所有关节值）
+            line=line[:-1]
+            parts = list(map(float, line.split(',')))
+            timestamp = parts[0]
+            joints = parts[1:]  # 第0个是时间戳，后面是关节数据
+            ###parts = line.split(',')
+            ###timestamp = parts[0]
+            ####joints = parts[1:]  # 第0个是时间戳，后面是关节数据
+            joints=parts
+            timestamps.append(timestamp)
+            
+            # 初始化关节数据列表（首次读取时）
+            if not joint_data:
+                joint_data = [[] for _ in range(len(joints))]
+            
+            # 按关节索引存储数据
+            for i, val in enumerate(joints):
+                if i < len(joint_data):  # 避免索引越界
+                    joint_data[i].append(val)
+
+    # 转换为相对时间（以第一个时间戳为起点）
+    if not timestamps:
+        print("警告：未读取到有效数据")
+        return
+    start_time = timestamps[0]
+    ###relative_times = [t - start_time for t in timestamps]
+
+    # 绘制所有关节轨迹（最多显示12个关节，避免图过于拥挤）
+    num_joints = len(joint_data)
+    num_plots = min(num_joints, 12)  # 超过12个关节只显示前12个
+
+    plt.figure(figsize=(12, 8))
+    for i in range(num_plots):
+        plt.plot(joint_data[i], label=f'关节 {i+1}')
+
+    # 图形配置
+    plt.xlabel('时间 (秒)')
+    plt.ylabel('关节位置')
+    plt.title('所有关节轨迹曲线')
+    plt.grid(True, linestyle='--', alpha=0.7)
+    plt.legend(bbox_to_anchor=(1.05, 1), loc='upper left')  # 图例放右侧
+    plt.tight_layout()  # 自动调整布局
+    plt.show()
+
+if __name__ == "__main__":
+    # 数据文件路径（与C++代码中的路径一致）
+    data_file = "/home/nvidia/disk/ros2_joint_data.txt"
+    # data_file = "/home/nvidia/log.txt"
+    plot_joint_trajectories(data_file)
+