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();
}

  参考博客：古月居