ROS使用Moveit来对机械臂进行轨迹规划，而Moveit使用ControllerManager插件接口的形式来发布轨迹信息给机械臂的控制器。为了能让机械臂（无论是真实的还是虚拟的）按照计算的轨迹动起来，就必须实现这一个接口。在Moveit中提供了两个具体的接口实现，即simple_controller_manager和fake_controller_manager。fake_controller_manager就是一个假的ControllerManager，只用于仿真使用。这里不做讨论，仿真时直接使用就好了。simple_controller_manager则是一个可以与实际硬件关联的ControllerManager，所以为了能够很好地理解Moveit如何对真实机械臂进行轨迹运动控制，分析一下simple_controller_manager是很有必要的。

在这个我们对simple_controller_manager的代码进行简要的分析。因为我们的目的并不是要写一个ControllerManager，使用simple_controller_manager来操控机械臂已经足够了。

simple_controller_manager的代码可在Moveit的github中查看。 在其include目录中，有三个文件：

第一个头文件定义用于action相关操作的类ActionBasedControllerHandle:

第二个头文件中以ActionBasedControllerHandle为基类，定义一个关节轨迹Controller的handle类：

第三个头文件仍以ActionBasedControllerHandle为基类，定义一个手抓controller的handle类GripperControllerHandle。 在src目录中，有两个cpp源文件:

JointTrajectory作为轨迹规划中用来描述轨迹点信息的重要消息类型，值得介绍一下。其定义在include/trajectory_msgs/JointTrajectory.h中，它包含了如下的内容：

frame_id并没有什么意义，不去管它。joint_names是一系列关节名的list，应该和URDF中对应的关节名称一致。另外，每次发送的命令中不必包含所有的关节，因为有些关节在当时可能是不动的。

该消息的核心部分是trajectory_msgs/JointTrajectoryPoint，对应于轨迹中的每个点，它包含了四个浮点数list和一个duration类型。对于前四个浮点数，可以使用其中的一个或多个。例如一个最小的用法就是仅指定positions。

为了控制一个七自由度机械臂的七个关节，应该提供N个包含相近positions的points。另一种方式是，points仅包含粗糙的路径点，然后再交给插值器产生精细的运动命令。

每个点中的time_from_start值指示该路径（或轨迹）点对应的时间。

尽管轨迹message可以是精细的且冗长的。通常还是将轨迹message做成粗略的，再由动作服务器（action server）来完成插值。

在使用Setup Assistant配置机器人之后，在demo.launch文件中包含move_group.launch部分，fake_execution默认是为true的，这就意味着并不执行真正操作而只是仿真。为了控制实际的机器人，我们需要将其设为false。

在move_group.launch可以看到在包含trajectory_excution.launch.xml部分，会根据fake_execution的值将moveit_controller_manager的值设为panda或fake。fake_execution的值若为false则使用panda。

再在trajectory_execution.launch.xml中包含panda_moveit_controller_manager.launch.xml或fake_moveit_controller_manager.launch.xml。

在panda_moveit_controller_manager.launch.xml文件中，即使用了moveit_simple_controller_manager/MoveItSimpleControllerManager。同时使用ros_controllers.yaml作为配置文件放入ROS参数服务器。

在ros_controllers.yaml中，可以看到如下内容：

当moveit启动后，根据panda_moveit_controller_manager.launch.xml加载moveIt_simple_ controller_manager，然后根据controller_list中指定的FollowJointTrajectory类型实例化FollowJointTrajectoryControllerHandle来完成关节轨迹指令的发布。

那么，现在我们经过理清了Moveit控制机械臂的流程了。

为了论述的简洁性，这里只编写一个简单的框架性的Action Server节点。具体的轨迹执行并不做讨论。

在工作空间创建一个名为trajectory_test的package。在CMakeLists.txt的find_package中添加actionlib和actionlib_msgs。取消add_executable对应的行，并修改为正确的源文件。取消target_link_libraries对应的行。

然后在src目录中添加 main.cpp。其内容如下：