通过ROS控制真实机械臂(5)---Moveit!真实机械臂(move_group界面)

####### 开始控制真实世界的机械臂 #######
我们的目的是使用moveit控制真实的机械臂，而我们真正需要的action是FollowJointTrajectoryAction，这个action是moveit留出来专门控制真实机器人的 。启动刚开始生成的moveit！配置文件中的demo.launch文件后，就能看见FollowJointTrajectoryAction，可以看一下control_msgs/FollowJointTrajectory Action的具体定义，它的内部有一个重要成员 trajectory，当你使用moveit规划了一个机械臂运动路径时，它记录了机械臂运动的各个轨迹点，每个轨迹点以机械臂关节角的形式表述，这个数据也是我们最终控制机械臂所想要的最终数据要想使用moveit来控制真实机械臂，我们需要修改配置文件夹下的几个文件，因为默认生成的moveit配置文件中，所使用的部分参数是针对虚拟机械臂的，可以在rviz环境下观察模型的运动，但真正的控制信号并不会发出来！但真正的控制信号并不会发出来！但真正的控制信号并不会发出来！重要的事情讲三遍

按照上一篇博客的说明,如下操作和修改，让move_group将控制信号发出来。

具体要修改以下几个地方：
##1、demo.launch文件中参数fake_execution的值改为false

##2、修改moveit_controller_manager参数
    move_group.launch文件中，moveit_controller_manager在选择参数值时，“unless”前面那个velue值要修改，写一个自己机器人名称作为前缀

moveit_controller_manager的参数是有选择的，要么等于“redwall_arm”，要么等于“fake”，这要取决于后面的参数fake_execution, 而这个参数我们上一步已经改为了false，即当前moveit_controller_manager应该等于“redwall_arm”接下来，这个参数会传递给trajectory_execution.launch.xml文件，该文件中有下面这句话,按照原先的设置，本应该启动fake_moveit_controller_manager.launch.xml，而现在redwall_arm_moveit_controller_manager.launch.xml将会被启动

##3、修改redwall_arm_moveit_controller_manager.launch.xml文件
    双击打开这个文件，发现里面只写了一对launch标签，基本算是一个空文件，说明moveit早就帮你留好了位置，等着填充,之前已经填写完成了，如下修改

##4、创建controllers.yaml配置文件
上一步的代码最后一句指向了config目录下一个叫做controller.yaml的文件，决定了你所使用的moveit控制器的基本参数。

controller_list:
  - name: redwall_arm
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    joints:
    - Link1
    - Link2
    - Link3
    - Link4
    - Link5

##5、解决真实机械臂与joint_states_publisher消息冲突的问题
当控制真实机械臂时，moveit的配置文件中，demo.launch文件中关于joint_states_publisher的一段代码需要注释掉，
因为该程序也在发布机械臂关节状态，而且是一个模拟的状态，与真实机械臂状态是不相符的，若不注释掉该代码，rviz下显示的机械臂状态总是在不停的跳变,将原本的注释，使用真实的传感器发布的/joint_states来控制。

  
  
    
    
    [/joint_states]
  

##6、解决真实机械臂轨迹执行时间超时的问题。moveit配置文件中有几个参数还需要添加上，否则后续真实机械臂执行轨迹时会报超时的错误。需要在move_group.launch文件中增加几个参数，来延长允许执行轨迹的时间。 找到 所对应的标签组，在其中加入以下参数

      //允许轨迹执行时间的一个放大倍数，可以根据实际情况自行修改
                //超时的一个百分比范围
    或者加入下面的参数，直接关掉监视轨迹执行状况的一个monitoring
    

至此，moveit！已经修改完成，先启动试试看！！！

$ roslaunch redwall_arm_moveit_config demo.launch 

 出现如下报错信息：

[ INFO] [1564730909.628248268]: Constructing new MoveGroup connection for group 'arm_group' in namespace ''
[ INFO] [1564730910.984667942]: MoveItSimpleControllerManager: Waiting for redwall_arm/follow_joint_trajectory to come up
[ERROR] [1564730915.984849668]: MoveItSimpleControllerManager: Action client not connected: redwall_arm/follow_joint_trajectory
[ INFO] [1564730916.087709422]: Returned 0 controllers in list
[ INFO] [1564730916.107154984]: Trajectory execution is managing controllers

 奇怪，明明fake_controllers调用好好的，到真实的controllers咋就不行了，为啥没有action client连接呢。使用rostopic list 查看发布的消息

发现竟然没有找到follow_joint_trajectory消息，难道move_group没有发布吗？ 纠结了一天，终于找到原因了，之前虚拟机械臂有gazebo的ros_control插件自动帮我们获取了follow_joint_trajectory的动作action信息，现在到真实机器人了，需要我们自己写程序了。问题找到了，最重要的是要理解一点，move_group此时是follow_joint_trajectory的client端，动作消息是client端的move_group发布的，听起来有点绕，但是千万不要把报错信息的action client not connect搞混淆了，我们现在client是存在且被感知的(就是move_group)，而之所以报错是因为我缺少了action server端，我们需要添加一个server订阅这个action。

如下添加一个server节点启动即可

#include 
#include 
#include 
#include 
#include 

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

using namespace std;

typedef actionlib::SimpleActionServer Server;

std::basic_string Link1;
std::basic_string Link2;
std::basic_string Link3;
std::basic_string Link4;
std::basic_string Link5;
std::basic_string Link6;

/* 目标位置 */
double cp1;
double cp2;
double cp3;
double cp4;
double cp5;
double cp6;

/* 目标位置 */
double tp1;
double tp2;
double tp3;
double tp4;
double tp5;
double tp6;

/* 收到action的goal后调用的回调函数 */
void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server* as)
{
	//ros::Rate rate(1);

    Link1 = goal->trajectory.joint_names[0];
    Link2 = goal->trajectory.joint_names[1];
    Link3 = goal->trajectory.joint_names[2];
    Link4 = goal->trajectory.joint_names[3];
    Link5 = goal->trajectory.joint_names[4];
    Link6 = goal->trajectory.joint_names[5];

    cp1 = goal->trajectory.points[0].positions[0];
    cp2 = goal->trajectory.points[0].positions[1];
    cp3 = goal->trajectory.points[0].positions[2];
    cp4 = goal->trajectory.points[0].positions[3];
    cp5 = goal->trajectory.points[0].positions[4];
    cp6 = goal->trajectory.points[0].positions[5];

    tp1 = goal->trajectory.points[1].positions[0];
    tp2 = goal->trajectory.points[1].positions[1];
    tp3 = goal->trajectory.points[1].positions[2];
    tp4 = goal->trajectory.points[1].positions[3];
    tp5 = goal->trajectory.points[1].positions[4];
    tp6 = goal->trajectory.points[1].positions[5];

	// 并且按照1hz的频率发布进度feedback
	// control_msgs::FollowJointTrajectoryFeedback feedback;
    //feedback = NULL;
    //as->publishFeedback(feedback);

    // 当action完成后，向客户端返回结果
    //printf("goal=[%f,%f,%f,%f,%f,%f]\n",tp1,tp2,tp3,tp4,tp5,tp6);
    ROS_INFO("Recieve action successful!");
    as->setSucceeded();
}


/* 主函数主要用于动作订阅和套接字通信 */
int main(int argc, char** argv)
{
	ros::init(argc, argv, "trajectory_server");
	ros::NodeHandle nh;
	// 定义一个服务器
	Server server(nh, "redwall_arm/follow_joint_trajectory", boost::bind(&execute, _1, &server), false);
	// 服务器开始运行
	server.start();
	ros::spin();

}

再次启动demo.launch和server节点：

 点击 plan and execute之后查看上述action内容：

 $ rostopic echo /redwall_arm/follow_joint_trajectory/goal

涉及到底层的控制：

1、机械臂底层驱动已经完成。
    预先完成好底层驱动控制部分，如之前的文章通过手柄控制的机械臂。

2、机械臂能够按照路点（轨迹）运动
    这应该是一般机械臂的一项基本功能。这里的轨迹由一个个路点组成，路点可以理解为机械臂在空间中运动时必须通过的点，一个个路  点串起来就形成了轨迹。路点的数据结构一般是一组关节角度，比如5自由度机械臂，有5个独立关节，其确定了机械臂在空间的唯一位置，那么，就可以用 [ joint1，joint2，joint3，joint4，joint5] 来表示一个路点。因为moveit最终发出来的控制指令的实质就是一连串的路点，只要机械臂能够执行这一串路点 就达到了机械臂按照规划的轨迹运动的目的。

3、ros_control除了作为action的服务端，还需要通过joint_states话题发布自身的关节角move_group订阅了该话题，就能够获取机械臂当前的状态了，joint_states话题存储了机械臂的当前真实的关节角。因此，构造该话题时，要通过机械臂开放给用户的编程接口，一般机械臂都允许用户获取关节角的~

4、 关于action服务端写在哪的问题，action服务端的编写需要结合你自己的机械臂来进行，因为它要将moveit的控制指令翻译给机械臂的驱动,action的一个优点就是避免了繁杂的话题处理，使用action处理两个节点的连接时客户端向服务端发送请求，服务端会在相应的回调函数里通过参数直接得到这个请求数据，数据的传递过程被action这种机制隐藏了，虽然底层也是话题传递，但没必要再找话题订阅者或发布者了

5、 moveit配置的时候movegroup注意名称,因为trajectory_gen中使用的是arm_group，名称要统一

6、出现如下报错，将URDF中base_link的inertia属性去除即可

The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF