ROS中控制机械臂抓取目标例程

在上一个博文中介绍了一个简单的目标识别的例子，在这篇博客中，例如是别的结果，完成机械臂的抓取控制，主要进行程序的分析和学习。

包含的头文件：

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
#include <geometry_msgs/PoseStamped.h>
#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_visual_tools/moveit_visual_tools.h>
#include <cv_bridge/cv_bridge.h>
#include <tf/transform_listener.h>

还是从主函数看起：

int main(int argc, char **argv)
{
  //初始化ROS节点，节点名为simple_grasping
  ros::init(argc, argv, "simple_grasping");

  float length, breadth, pregrasp_x, pregrasp_y, pregrasp_z;
  //节点句柄
  ros::NodeHandle n;

  //获取参数，首先是桌子的长
  if (!n.getParam("probot_grasping/table_length", length))
    length = 0.3;
  //桌子的宽
  if (!n.getParam("probot_grasping/table_breadth", breadth))
    breadth = 0.3;
   
  //机械臂的初始位置，不让机械臂挡着视野，影响拍照
  if (!n.getParam("probot_grasping/pregrasp_x", pregrasp_x))
    pregrasp_x = 0.20;
  if (!n.getParam("probot_grasping/pregrasp_y", pregrasp_y))
    pregrasp_y = -0.17;
  if (!n.getParam("probot_grasping/pregrasp_z", pregrasp_z))
    pregrasp_z = 0.28;

  //创建一个对象，将参数传递进去
  GraspingDemo simGrasp(n, pregrasp_x, pregrasp_y, pregrasp_z, length, breadth);
  ROS_INFO_STREAM("Waiting for five seconds..");

  ros::WallDuration(5.0).sleep();
 
  //不断查看图像队列，如果有识别到的图像，则进行抓取
  while (ros::ok())
  {
    // Process image callback
    ros::spinOnce();

    //控制机械臂运动
    simGrasp.initiateGrasping();
  }
  return 0;
}

 先看一下GraspingDemo类的内容：

首先看构造函数当中：

GraspingDemo::GraspingDemo(ros::NodeHandle n_, float pregrasp_x, float pregrasp_y, float pregrasp_z, float length, float breadth) :
    it_(n_), 
    armgroup("manipulator"), 
    grippergroup("gripper"), 
    vMng_(length, breadth)
{
  this->nh_ = n_;

  //获取base_link和camera_link之间的关系，也就是手眼标定的结果
  try
  {
    this->tf_camera_to_robot.waitForTransform("/base_link", "/camera_link", ros::Time(0), ros::Duration(50.0));
  }
  catch (tf::TransformException &ex)
  {
    ROS_ERROR("[adventure_tf]: (wait) %s", ex.what());
    ros::Duration(1.0).sleep();
  }

  //如果查询得到的话，就将结果保存到camera_to_robot_，保存x,y,z和四元数一共7个值
  try
  {
    this->tf_camera_to_robot.lookupTransform("/base_link", "/camera_link", ros::Time(0), (this->camera_to_robot_));
  }
  catch (tf::TransformException &ex)
  {
    ROS_ERROR("[adventure_tf]: (lookup) %s", ex.what());
  }

  grasp_running = false;
  
  this->pregrasp_x = pregrasp_x;
  this->pregrasp_y = pregrasp_y;
  this->pregrasp_z = pregrasp_z;

  //让机械臂运动到初始的位置
  ros::AsyncSpinner spinner(1);
  spinner.start();
  ros::WallDuration(5.0).sleep();
  ROS_INFO_STREAM("Getting into the Grasping Position....");
   //调用该函数控制机械臂运动到设定的位置
  attainPosition(pregrasp_x, pregrasp_y, pregrasp_z);

  // Subscribe to input video feed and publish object location
  //订阅图像话题，一旦收到图像信息，就会进入到callback当中
  image_sub_ = it_.subscribe("/probot_anno/camera/image_raw", 1, &GraspingDemo::imageCb, this);
}

 attainPosition函数：

void GraspingDemo::attainPosition(float x, float y, float z)
{
  // ROS_INFO("The attain position function called");

  // 获取当前位置
  geometry_msgs::PoseStamped currPose = armgroup.getCurrentPose();
  
  //初始化数据类型
  geometry_msgs::Pose target_pose1;
  target_pose1.orientation = currPose.pose.orientation;


  // 设置抓取前的机械臂位置
  target_pose1.position.x = x;
  target_pose1.position.y = y;
  target_pose1.position.z = z;
  armgroup.setPoseTarget(target_pose1);

  //机械臂运动
  armgroup.move();
}

看一下订阅图像的回调函数：

void GraspingDemo::imageCb(const sensor_msgs::ImageConstPtr &msg)
{
  if (!grasp_running)
  {
    ROS_INFO_STREAM("Processing the Image to locate the Object...");
    //将图像变换到opencv当中
    try
    {
      cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception &e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }
 
    //调用vision_manager中的函数获取目标的位置，位置坐标是以摄像头中心点的位置作为0坐标
    // ROS_INFO("Image Message Received");
    float obj_x, obj_y;
    vMng_.get2DLocation(cv_ptr->image, obj_x, obj_y);

    // Temporary Debugging
    std::cout<< " X-Co-ordinate in Camera Frame :" << obj_x << std::endl;
    std::cout<< " Y-Co-ordinate in Camera Frame :" << obj_y << std::endl;

    //通过坐标变换，将二维坐标变换为相机坐标系下的三维坐标，在本程序中与URDF建模有关系
    obj_camera_frame.setZ(-obj_y);
    obj_camera_frame.setY(-obj_x);
    obj_camera_frame.setX(0.45);

    //关键的一行代码，将相机坐标系下的位置转化为base_link坐标系下的坐标
    obj_robot_frame = camera_to_robot_ * obj_camera_frame;
    grasp_running = true;

    // Temporary Debugging
    std::cout<< " X-Co-ordinate in Robot Frame :" << obj_robot_frame.getX() << std::endl;
    std::cout<< " Y-Co-ordinate in Robot Frame :" << obj_robot_frame.getY() << std::endl;
    std::cout<< " Z-Co-ordinate in Robot Frame :" << obj_robot_frame.getZ() << std::endl;
  }
}

然后机械臂开始根据这个目标位置进行运动：

void GraspingDemo::initiateGrasping()
{
  //开启新的线程
  ros::AsyncSpinner spinner(1);
  spinner.start();
  ros::WallDuration(3.0).sleep();

  //获取当前的位置
  homePose = armgroup.getCurrentPose();
  
  //调用attainObject()函数使机械臂靠近目标
  ROS_INFO_STREAM("Approaching the Object....");
  attainObject();

  //夹取物体
  ROS_INFO_STREAM("Attempting to Grasp the Object now..");
  grasp();

  //夹住物体做一个小范围移动
  ROS_INFO_STREAM("Lifting the Object....");
  lift();

  //机械臂返回到初始状态
  ROS_INFO_STREAM("Going back to home position....");
  goHome();

  grasp_running = false;
}

看一下涉及到的三个函数： 

靠近物体：

void GraspingDemo::attainObject()
{
  // ROS_INFO("The attain Object function called");
  attainPosition(obj_robot_frame.getX(), obj_robot_frame.getY(), obj_robot_frame.getZ() + 0.04);

  // Open Gripper
  ros::WallDuration(1.0).sleep();
  grippergroup.setNamedTarget("open");
  grippergroup.move();

  // Slide down the Object
  geometry_msgs::PoseStamped currPose = armgroup.getCurrentPose();
  geometry_msgs::Pose target_pose1;

  target_pose1.orientation = currPose.pose.orientation;
  target_pose1.position = currPose.pose.position;

  target_pose1.position.z = obj_robot_frame.getZ() - 0.02;
  armgroup.setPoseTarget(target_pose1);
  armgroup.move();
}

抓取：

void GraspingDemo::grasp()
{
  // ROS_INFO("The Grasping function called");

  ros::WallDuration(1.0).sleep();
  grippergroup.setNamedTarget("close");
  grippergroup.move();
}

夹爪离开：

void GraspingDemo::lift()
{
  // ROS_INFO("The lift function called");

  // For getting the pose
  geometry_msgs::PoseStamped currPose = armgroup.getCurrentPose();

  geometry_msgs::Pose target_pose1;
  target_pose1.orientation = currPose.pose.orientation;
  target_pose1.position = currPose.pose.position;

  // Starting Postion after picking
  //target_pose1.position.z = target_pose1.position.z + 0.06;

  if(rand() % 2)
  {
    target_pose1.position.y = target_pose1.position.y + 0.02;
  }
  else
  {
    target_pose1.position.y = target_pose1.position.y - 0.02;
  }
  
  armgroup.setPoseTarget(target_pose1);
  armgroup.move();

  // Open Gripper
  ros::WallDuration(1.0).sleep();
  grippergroup.setNamedTarget("open");
  grippergroup.move();

  target_pose1.position.z = target_pose1.position.z + 0.06;
  armgroup.setPoseTarget(target_pose1);
  armgroup.move();
}

参考博客：古月居