ROS 控制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 <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 "GraspingDemo.hpp"


int main(int argc, char **argv)

{

  setlocale(LC_CTYPE, "zh_CN.utf8");

  ros::init(argc, argv, "move_group_interface_tutorial");

  ros::NodeHandle node_handle;

  ros::AsyncSpinner spinner(1);

  spinner.start();





  // Visualization

  // ^^^^^^^^^^^^^

  //

  // The package MoveItVisualTools provides many capabilities for visualizing objects, robots,

  // and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script.

  namespace rvt = rviz_visual_tools;

  moveit_visual_tools::MoveItVisualTools visual_tools("dummy");

  visual_tools.deleteAllMarkers();



  // Remote control is an introspection tool that allows users to step through a high level script

  // via buttons and keyboard shortcuts in RViz

  visual_tools.loadRemoteControl();



  // RViz provides many types of markers, in this demo we will use text, cylinders, and spheres

  Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();

  text_pose.translation().z() = 1.75;

  visual_tools.publishText(text_pose, "MoveGroupInterface Demo", rvt::WHITE, rvt::XLARGE);



  // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations

  visual_tools.trigger();



  float pregrasp_x = 0.20;

  float pregrasp_y = 0.20;

  float pregrasp_z = 0.20;



  //创建一个对象，将参数传递进去

  GraspingDemo simGrasp(node_handle, pregrasp_x, pregrasp_y, pregrasp_z);

  ROS_INFO_STREAM("Waiting for five seconds..");



  ros::WallDuration(5.0).sleep();





  

  while (ros::ok())

  {

    // visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to go to run");

    // Process callback

    ros::spinOnce();

    //控制机械臂运动

    simGrasp.initiateGrasping();

    ros::WallDuration(3.0).sleep();

  }

  ros::shutdown();

  return 0;
}

运行步骤：

roslaunch gluon_moveit_config cm_demo.launch

roslaunch moveit_tutorials move_group_interface_tutorial.launch