MoveIt Servo 与自己编写的 Action Server 通信

MoveIt Servo 与自己编写的 Action Server 通信需要通过 控制器管理器配置文件 和 MoveIt 配置 来建立连接。让我详细说明整个通信链路：

一、通信架构图

text

复制代码

MoveIt Servo (C++ node)
    ↓
MoveIt Controller Manager
    ↓
FollowJointTrajectoryControllerHandle
    ↓
Action Client (自动生成)
    ↓ [ROS 2 Action 通信]
您的 Action Server (myrobot_driver)
    ↓
真实硬件

二、关键配置文件

1. controllers.yaml - 控制器配置

yaml

复制代码

# config/controllers.yaml
moveit_simple_controller_manager:
  controller_names:
    - arm_controller  # 必须与 servo 配置中的 controller_name 匹配
  
  arm_controller:
    action_ns: arm_controller/follow_joint_trajectory  # 您的 Action Server 名称
    type: FollowJointTrajectory
    joints:
      - joint1
      - joint2
      - joint3
      - joint4
      - joint5
      - joint6
    default: true

2. servo.yaml - Servo 配置文件

yaml

复制代码

# config/servo.yaml
# 控制器配置
controller_name: arm_controller  # 必须与 controllers.yaml 中的名称匹配

# 关节配置
joints:
  - joint1
  - joint2
  - joint3
  - joint4
  - joint5
  - joint6

# 运动学参数
move_group_name: arm_group
planning_group: arm_group

# Servo 参数
command_in_type: "unitless"  # 或 "speed_units"
publish_period: 0.01  # 10ms

3. moveit_controllers.yaml - MoveIt 控制器配置

yaml

复制代码

# config/moveit_controllers.yaml
controller_manager_name: MoveItSimpleControllerManager

controller_names:
  - arm_controller

arm_controller:
  action_ns: arm_controller/follow_joint_trajectory
  type: FollowJointTrajectory
  joints:
    - joint1
    - joint2
    - joint3
    - joint4
    - joint5
    - joint6

三、启动文件配置

launch/servo.launch.py

python

复制代码

from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
import os

def generate_launch_description():
    # 1. 启动您的 Action Server
    action_server = Node(
        package='myrobot_driver',
        executable='myrobot_driver_node',  # 您的 Action Server 可执行文件
        name='myrobot_driver',
        output='screen'
    )
    
    # 2. 启动 MoveIt Servo
    servo_node = Node(
        package='moveit_servo',
        executable='servo_node',
        name='servo_node',
        parameters=[
            {'use_sim_time': False},
            os.path.join(get_package_share_directory('your_config_pkg'), 'config', 'servo.yaml'),
            os.path.join(get_package_share_directory('your_config_pkg'), 'config', 'controllers.yaml')
        ],
        output='screen'
    )
    
    # 3. 启动 MoveIt 控制器管理器
    controller_manager = Node(
        package='moveit_ros_control_interface',
        executable='moveit_controller_manager',
        parameters=[os.path.join(get_package_share_directory('your_config_pkg'), 'config', 'moveit_controllers.yaml')],
        output='screen'
    )
    
    return LaunchDescription([
        action_server,
        servo_node,
        controller_manager,
    ])

四、代码层面的连接

您的 Action Server 需要实现的标准接口

cpp

复制代码

// myrobot_driver_ros2.cpp (您的代码)
class FollowJointTrajectoryAction : public rclcpp::Node
{
public:
    FollowJointTrajectoryAction() : Node("myrobot_driver")
    {
        // Action Server 名称必须与配置文件中的 action_ns 匹配
        action_server_ = rclcpp_action::create_server<FollowJointTrajectory>(
            this,
            "arm_controller/follow_joint_trajectory",  // ← 关键：名称必须匹配
            // ... 回调函数
        );
    }
};

MoveIt Servo 内部工作流程

cpp

复制代码

// moveit_servo 内部（简化版）
class ServoNode {
    void sendTrajectory() {
        // 1. 获取控制器句柄
        auto controller_handle = controller_manager_->getControllerHandle("arm_controller");
        
        // 2. 转换为轨迹控制器句柄
        auto traj_handle = std::dynamic_pointer_cast<FollowJointTrajectoryControllerHandle>(controller_handle);
        
        // 3. 发送轨迹（这会调用您的 Action Server）
        traj_handle->sendTrajectory(current_trajectory_);
        
        // 4. 内部会创建 Action Client 并调用：
        // client->async_send_goal(goal, send_goal_options);
        // 发送到 "arm_controller/follow_joint_trajectory" Action Server
    }
};

五、完整的通信流程

步骤1：启动顺序

bash

复制代码

# 1. 启动您的 Action Server
ros2 run myrobot_driver myrobot_driver_node

# 2. 启动 MoveIt Servo（会自动加载控制器管理器）
ros2 launch your_config servo.launch.py

步骤2：连接建立

cpp

复制代码

// MoveIt Servo 启动时会：
// 1. 读取 controllers.yaml
// 2. 创建 Action Client 连接到您的 Server
// 3. 等待连接建立

步骤3：轨迹发送

cpp

复制代码

// 当用户通过 Teleop 或规划器发送命令时：
// 1. Servo 计算轨迹点
// 2. 打包成 FollowJointTrajectory Goal
// 3. 通过 Action Client 发送
// 4. 您的 Action Server 接收并执行

六、验证通信的命令

检查 Action Server 是否可见

bash

复制代码

# 列出所有 Action Server
ros2 action list

# 应该看到：
# /arm_controller/follow_joint_trajectory  # 您的 Action Server

手动测试通信

bash

复制代码

# 发送测试轨迹
ros2 action send_goal /arm_controller/follow_joint_trajectory \
    control_msgs/action/FollowJointTrajectory \
    "{trajectory: {joint_names: ['joint1','joint2','joint3','joint4','joint5','joint6'], 
                   points: [{positions: [0.1,0.2,0.3,0.4,0.5,0.6], 
                            time_from_start: {sec: 1}}]}}"

七、常见问题排查

问题1：找不到 Action Server

cpp

复制代码

// 错误信息
// [ERROR] Action client not connected to action server: arm_controller/follow_joint_trajectory

// 解决方案：检查命名空间
// 您的 Server 名称：/arm_controller/follow_joint_trajectory
// 配置中的名称：arm_controller/follow_joint_trajectory

问题2：关节名称不匹配

cpp

复制代码

// 您的 Server 期望的关节名称
joint_state_.name = {"joint1", "joint2", ...};

// controllers.yaml 中的关节名称
joints: ["joint1", "joint2", ...]  // 必须完全一致！

八、完整的配置示例

文件结构

text

复制代码

your_moveit_config/
├── config/
│   ├── controllers.yaml
│   ├── servo.yaml
│   └── moveit_controllers.yaml
├── launch/
│   └── servo.launch.py
└── config/
    └── ros2_controllers.yaml  # 您的 Server 配置

测试通信的完整代码

python

复制代码

# test_servo_communication.py
import rclpy
from control_msgs.action import FollowJointTrajectory
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from rclpy.action import ActionClient

def test_communication():
    rclpy.init()
    node = rclpy.create_node('test_client')
    
    # 创建 Action Client
    client = ActionClient(node, FollowJointTrajectory, 
                         'arm_controller/follow_joint_trajectory')
    
    # 等待 Server
    if not client.wait_for_server(timeout_sec=5.0):
        print("Server not available!")
        return
    
    # 发送测试目标
    goal = FollowJointTrajectory.Goal()
    goal.trajectory.joint_names = ['joint1', 'joint2', 'joint3', 
                                   'joint4', 'joint5', 'joint6']
    
    point = JointTrajectoryPoint()
    point.positions = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]
    point.time_from_start.sec = 1
    goal.trajectory.points.append(point)
    
    # 发送
    client.send_goal_async(goal)
    print("Goal sent!")
    
    rclpy.spin_once(node)

if __name__ == '__main__':
    test_communication()

总结

MoveIt Servo 通过以下文件与您的 Action Server 通信：

controllers.yaml - 定义控制器和 Action 命名空间
servo.yaml - 指定使用的控制器名称
moveit_controllers.yaml - MoveIt 的控制器配置

关键连接点是 Action 名称：

您的 Server 必须创建：arm_controller/follow_joint_trajectory
配置文件中必须指定相同的 action_ns

只要 Action 名称匹配，MoveIt Servo 会自动创建 Action Client 并与您的 Server 通信，无需修改 MoveIt Servo 的源代码！