ROS中Moveit生成轨迹如何作用于实际的机械臂(一)

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代码分析

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

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

1. action_based_controller_handle.h
2. follow_joint_trajectory_controller_handle.h
3. gripper_controller_handle.h

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

class ActionBasedControllerHandle
{
	ActionBasedControllerHandle(const std::string& name, const std::string& ns);
	bool isConnected();
	bool cancelExecution()
	bool waitForExecution(const ros::Duration& timeout = ros::Duration(0));
	ExecutionStatus getLastExecutionStatus();
	void addJoint(const std::string& name);
	void getJoints(std::vector<std::string>& joints);
protected:
	std::string getActionName(void);
	void finishControllerExecution(const actionlib::SimpleClientGoalState& state);
}

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

class FollowJointTrajectoryControllerHandle
: public ActionBasedControllerHandle<control_msgs::FollowJointTrajectoryAction>
{
FollowJointTrajectoryControllerHandle(const std::string& name, const std::string& action_ns):ActionBasedControllerHandle<control_msgs::FollowJointTrajectoryAction>(name, action_ns) ;
bool sendTrajectory(const moveit_msgs::RobotTrajectory& trajectory);
void configure(XmlRpc::XmlRpcValue& config);
}

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

1. follow_joint_trajectory_controller_handle.cpp，对应FollowJointTrajectoryControllerHandle类中成员函数的实现。这里面最重要的函数为sendTrajectory，其传入的参数为moveit_msgs::RobotTrajectory。RobotTrajectory是一个包裹JointTrajectory类型的消息结构体。然后将JointTrajectory作为Action的goal发送给Action Server。Action Server收到该goal后就控制电机来让机器人运动起来。
2. moveit_simple_controller_mana