【Ros控制机械臂学习笔记】move_group C++interface接口学习

       在完成机器人URDF模型建立，利用moveit_setup_assistant配置生成robot_moveit_config文件夹之后，接下来就是要的学习方向有两个。

        一个是向下位机走，即上图的右面，只是简单的修改配置文件（demo.launch\ros_controllers.yaml等文件），以此来控制机械臂运动，重点是编写Follow_joint_trajectory 这个Action的server端，因为moveit已经帮我们实现了.action文件和client端的编写，然后在编写自己机械臂的joint_state_pblisher节点，与机械臂的控制器程序相配合，就能实现ros对机械臂的控制 。

         另一个学习方向就行向上走，即图中move_group的左面，目的是实现更复杂的控制功能，这里ROS提供了RVIZ中的GUI、C++以及python三种接口让我们来对机械臂进行更复杂的运动控制。根据自己的理解，其中，gui只是在调试机械臂的时候用到的，因为他是在rviz中拖动坐标球进行目标点位设置的，要想让机械臂末端到达准确的坐标点位这种方式是无法实现的。所以要达到准确的目标点位设置和姿态设置，还是需要使用C++接口和python接口。由于没有python基础，所以打算从C++开始。

【学习课堂小笔记】

1、要用到哪些头文件呢？

头文件的功能目前我还没全部摸透，但是，看了几篇示例代码下来，我感觉，学习阶段，可以简单粗暴把以下头文件全部包括进来，编写程序的过程中，在慢慢摸索每个头文件的功能（因为头文件内部的东西实在太多了，真的是不想看，有几千行）

#include<moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <vector>
#include <iostream>

2、准备工作

这部分要设定在C++中要操作运动的规划组（就是在用配置助手生成包的时候设定的规划组），然后实例化一个PlanningSceneInterface类，用于操作rviz中的碰撞对象

//配置规划组  调用PlanningSceneInterface
//方式一 
 static const std::string PLANNING_GROUP = "arm";
 moveit::planning_interface::MoveGroupInterface group(PLANNING_GROUP);
//方式二
 moveit::planning_interface::MoveGroupInterface group("arm");

//用PlanningSceneInterface类在“虚拟世界”场景中添加和删除碰撞对象
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;  

3、操作机械臂动起来

这部分工作意图就很明显了，直接看代码实现方式

//-----------------------------------------------------------------------
 // 设置一个末端位置
  geometry_msgs::Pose target_pose1;
  target_pose1.orientation.w = 0.726282;
  target_pose1.orientation.x= 4.04423e-07;
  target_pose1.orientation.y = -0.687396;
  target_pose1.orientation.z = 4.81813e-07;  //用四元数确定末端的姿态

  target_pose1.position.x = 0.0261186;
  target_pose1.position.y = 4.50972e-07;
  target_pose1.position.z = 0.573659;
  group.setPoseTarget(target_pose1);        //设定末端的坐标


//这步操作相当于在rviz中点击plan 但是不执行（execute）
  moveit::planning_interface::MoveGroupInterface::Plan my_plan;
  moveit::planning_interface::MoveItErrorCode success = group.plan(my_plan); 
  ROS_INFO("Visualizing plan 1 (pose goal) %s",success.val ? "":"FAILED"); 
 
// 简单理解 就是点rviz里面的  execute
	group.move();	 
//-------------------------------------------------------------------------


//***************************************************************************************
 //创建一个PlanningSceneInterface对象，用于访问moveit中的场景  并对场景进行操作
   moveit::planning_interface::PlanningSceneInterface current_scene;
 //等待5S 等待对象实例化
   sleep(5.0);
 //在moveit中添加一个圆柱体 碰撞对象 名称是对象的ID   
   moveit_msgs::CollisionObject cylinder;

   cylinder.id = "seven_dof_arm_cylinder";//这个名字可以随便定义
 //设置碰撞对象的参数  定义了一个primitive的结构体
    shape_msgs::SolidPrimitive primitive;
    primitive.type = primitive.CYLINDER;
    primitive.dimensions.resize(3);
    primitive.dimensions[0] = 0.6;
    primitive.dimensions[1] = 0.2;
    primitive.dimensions[2] = 0.2;
 //设在碰撞对象的位置姿态
    geometry_msgs::Pose pose;
    pose.orientation.w = 1.0;
    pose.position.x =  0.0;
    pose.position.y = -0.4;
    pose.position.z =  0.4;
 //添加相对于碰撞检测对象的姿态——添加规划场景  就是把定义好的碰撞物体加载道规划场景中
    cylinder.primitives.push_back(primitive);
    cylinder.primitive_poses.push_back(pose);
    cylinder.operation = cylinder.ADD;
 //创建一个向量  将碰撞类型加入到该向量中
    cylinder.header.frame_id = "base_link";
    std::vector<moveit_msgs::CollisionObject> collision_objects;
    collision_objects.push_back(cylinder);
 //将添加的对象添加到当清的规划场景中
    current_scene.addCollisionObjects(collision_objects);
 //代码块功能：将碰撞对象加载到规划场景中
//***************************************************************************************



//***************************************************************************************
 //创建一个PlanningSceneInterface对象，用于访问moveit中的场景  并对场景进行操作
    moveit::planning_interface::PlanningSceneInterface current_scene;
 //简单理解  就是调用下面这段话，可以将上面定义好的碰撞对象删除
    std::vector<std::string> object_ids;
    object_ids.push_back("seven_dof_arm_cylinder");//这里面的名字是在添加碰撞对象的时候定义的
    current_scene.removeCollisionObjects(object_ids);
 //代码块功能：将碰撞对象删除
//***************************************************************************************

//***************************************************************************************
 //向机械臂连杆上添加一个碰撞对象———实现抓取
 //开始 初始化启动ros
 	ros::init(argc, argv, "seven_dof_arm_planner");
	ros::NodeHandle node_handle;
	ros::AsyncSpinner spinner(1);
	spinner.start();
 //创建一个PlanningSceneInterface对象，用于访问moveit中的场景  并对场景进行操作
 	moveit::planning_interface::MoveGroupInterface group("arm");
	moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
	moveit::planning_interface::PlanningSceneInterface current_scene;
    sleep(2);
 //定义抓取对象  这部分 就和添加碰撞对象时的操作一样了
    moveit_msgs::CollisionObject grasping_object;
	grasping_object.id = "grasping_object";
    
	shape_msgs::SolidPrimitive primitive;
	primitive.type = primitive.BOX;
	primitive.dimensions.resize(3);
	primitive.dimensions[0] = 0.03;
	primitive.dimensions[1] = 0.03;
	primitive.dimensions[2] = 0.08;
 //将抓取对象的在环境中的空间位置设定在机械臂末端的位置上
	geometry_msgs::Pose pose;
	pose.orientation.w = 1.0;
	pose.position.x =  0.03;
	pose.position.y =  0.0;
	pose.position.z =  0.65;
    grasping_object.primitives.push_back(primitive);
	grasping_object.primitive_poses.push_back(pose);
	grasping_object.operation = grasping_object.ADD;
 //确定选用的参考坐标系，因为后面对对象的操作可能选用的参考系不一样
    grasping_object.header.frame_id = "base_link";
 //创建一个向量  将碰撞类型加入到该向量中
    std::vector<moveit_msgs::CollisionObject> collision_objects;
	collision_objects.push_back(grasping_object);
 //将添加的对象添加到当清的规划场景中	
    current_scene.addCollisionObjects(collision_objects);
	
	sleep(4);

 //*将被抓取的物体附着在机械臂的抓手上
 	ROS_INFO("Attaching object grasping_object to robot's body");
	//这个类里面有两个成员变量link_name  object
    moveit_msgs::AttachedCollisionObject attacched_object;   
    //link_name 确定末端执行器连杆名称
	attacched_object.link_name = "grasping_frame";
	attacched_object.object = grasping_object;
    //将附加的碰撞对象同步应用于move_group节点的规划场景。其他PlanningSceneMonitors将不会收到更新，除非他们订阅move_group的监控场景
	current_scene.applyAttachedCollisionObject( attacched_object );
	sleep(4);
  
    //将grasping_object 的参数改成remove 就是实现了将物体从抓手上释放
	ROS_INFO("Detaching object grasping_object to robot's body");
	grasping_object.operation = grasping_object.REMOVE;
	attacched_object.link_name = "grasping_frame";
	attacched_object.object = grasping_object;
	current_scene.applyAttachedCollisionObject( attacched_object );

	sleep(1);

	ros::shutdown();
    // 代码块的功能：添加抓取和放置的功能
//***************************************************************************************

心酸的学习历程，不知道哪些地方是错的，欢迎大家一起讨论啊