ROS 控制Innfos机械臂简单例子_LWLGZY的博客_innfos机械臂

#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>



class GraspingDemo
{
private:
  
  ros::NodeHandle nh_;
  
  geometry_msgs::Pose target_pose1;

  moveit::planning_interface::MoveGroupInterface armgroup;

	image_transport::Subscriber image_sub_;
  

  bool grasp_running;
 

  tf::StampedTransform camera_to_robot_;
	
	tf::TransformListener tf_camera_to_robot;
	
	tf::Vector3 obj_camera_frame, obj_robot_frame;

  geometry_msgs::PoseStamped homePose;

  float pregrasp_x, pregrasp_y, pregrasp_z;

public:
  GraspingDemo(ros::NodeHandle n_, float pregrasp_x, float pregrasp_y, float pregrasp_z);

  ~GraspingDemo();

  void attainPosition(float x, float y, float z);

  void imageCb(const sensor_msgs::ImageConstPtr &msg);

  void initiateGrasping();

  void attainObject();

  void grasp();

  void lift();

  void goHome();
};

//+++++++++++++++++++++++++++++++++++++++++++++++
// GraspingDemo::GraspingDemo(/* args */)
// {
// }

GraspingDemo::~GraspingDemo()
{
}


GraspingDemo::GraspingDemo(ros::NodeHandle n_, float pregrasp_x, float pregrasp_y, float pregrasp_z) :
    armgroup("gluon")    
{
  this->nh_ = n_;


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



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)
{
    //调用vision_manager中的函数获取目标的位置，位置坐标是以摄像头中心点的位置作为0坐标
    ROS_INFO("Image Message Received");
}

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

  ROS_INFO_STREAM("Going back to home position....");
  goHome();

  //获取当前的位置
  geometry_msgs::PoseStamped homePose = armgroup.getCurrentPose();

  //调用attainObject()函数使机械臂靠近目标
  ROS_INFO_STREAM("Approaching the Object....");
  attainObject();


  lift();

  grasp_running = false;
}

void GraspingDemo::attainObject()
{
  ROS_INFO("The attain Object function called");
  
  ROS_INFO("位置：%f , %f ,%f ",obj_robot_frame.getX(),obj_robot_frame.getY(),obj_robot_frame.getZ());

  attainPosition(0.20, 0.10, 0.30);


  // 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");
  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();


  
}

void GraspingDemo::goHome()
{
  std::vector<double> joint_home_positions(6, 0.0);
  armgroup.setJointValueTarget(joint_home_positions);
  ROS_INFO("Go to home");
  armgroup.move();
}#include <ros/ros.h>
#include <image_transp