moveit规划路径joint_path_command数据提取与导出

源码

#include <iostream>       //handeye_calibration#include <Eigen/Eigen>#include <stdlib.h>#include <Eigen/Geometry>#include <Eigen/Core>#include <moveit/move_group_interface/move_group.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 <vector>#include <math.h>#include <control_msgs/JointTrajectoryAction.h>#include <trajectory_msgs/JointTrajectoryPoint.h>#include <stdio.h>#include <stdlib.h>#include <time.h>#include "ros/ros.h"#include "std_msgs/String.h"#include <sstream>using namespace std;int success_times = 0;bool flag = true;int i = 0;string my_joint_trajectory = "";void Specified_pos(float x,float y,float z,float w,float roll,float pitch,float yaw){    move_group_interface::MoveGroup group("SRD6");    geometry_msgs::Pose Specified_pose;    Specified_pose.position.x = x;    Specified_pose.position.y = y;    Specified_pose.position.z = z;    Specified_pose.orientation.w = w;    Specified_pose.orientation.x = roll;    Specified_pose.orientation.y = pitch;    Specified_pose.orientation.z = yaw;    //group.setMaxVelocityScalingFactor(0.3);  //速度系数    //group.setMaxAccelerationScalingFactor(0.3);//加速度系数    group.setPoseTarget(Specified_pose);    //moveit::planning_interface::MoveGroup::Plan planner;    moveit::planning_interface::MoveGroup::Plan planner;    bool is_success=group.plan(planner);    if(is_success)    {      moveit_msgs::RobotTrajectory msg;      msg = planner.trajectory_;// get the trajectory from the path [1]      std::vector<trajectory_msgs::JointTrajectoryPoint> trajectory_points;      trajectory_points = msg.joint_trajectory.points;  //Can be seen in [2]      std::vector<int>::size_type size1 = msg.joint_trajectory.points.size();      ROS_INFO("The amount of points = %ld", size1);      std::vector<float> position;      //position.resize(vector1);      int k = 0;      string joint_trajectory_pt;      for (unsigned i=0; i<size1; i++)      {          std::vector<int>::size_type size2 = msg.joint_trajectory.points[i].positions.size();          for (unsigned j=0; j<size2; j++)          {              k++;              position.push_back(msg.joint_trajectory.points[i]. positions[j]); // positions[j]was eerst [0]          }          ros::Duration duration;          duration = msg.joint_trajectory.points[i].time_from_start;          //ROS_INFO("time_from_start: [ %d, %d ]", duration.sec,duration.nsec);          float nsec_to_sec;          float time_from_start;          nsec_to_sec = duration.nsec*0.000000001;          time_from_start = duration.sec + nsec_to_sec;          stringstream ss;          ss << "point_index: " << i << endl             << "positions: "             << "[" << msg.joint_trajectory.points[i].positions[0]             << "," << msg.joint_trajectory.points[i].positions[1]             << "," << msg.joint_trajectory.points[i].positions[2]             << "," << msg.joint_trajectory.points[i].positions[3]             << "," << msg.joint_trajectory.points[i].positions[4]             << "," << msg.joint_trajectory.points[i].positions[5]             << "]" << endl             << "velocities: "             << "[" << msg.joint_trajectory.points[i].velocities[0]             << "," << msg.joint_trajectory.points[i].velocities[1]             << "," << msg.joint_trajectory.points[i].velocities[2]             << "," << msg.joint_trajectory.points[i].velocities[3]             << "," << msg.joint_trajectory.points[i].velocities[4]             << "," << msg.joint_trajectory.points[i].velocities[5]             << "]" << endl             << "accelerations: "             << "[" << msg.joint_trajectory.points[i].accelerations[0]             << "," << msg.joint_trajectory.points[i].accelerations[1]             << "," << msg.joint_trajectory.points[i].accelerations[2]             << "," << msg.joint_trajectory.points[i].accelerations[3]             << "," << msg.joint_trajectory.points[i].accelerations[4]             << "," << msg.joint_trajectory.points[i].accelerations[5]             << "]" << endl             << "time_from_start: "             << "[" << time_from_start             << "]" << endl;          joint_trajectory_pt = joint_trajectory_pt + ss.str();      }      cout << joint_trajectory_pt << endl;      my_joint_trajectory = joint_trajectory_pt;      joint_trajectory_pt = "";      group.move();      success_times++;    }    else    {        cout<<"Planning fail!"<<endl;    }}void hand_eye_calibration(float center_x,float center_y,float center_z,float sphere_radius){   move_group_interface::MoveGroup group("SRD6");   Eigen::Matrix<double,3,1> matrix_effect;   Eigen::Matrix<double,3,1> matrix_sphere_center;   Eigen::Matrix<double,3,1> matrix_obj_pt;   Eigen::Matrix<double,3,3> ee2camera;   Eigen::Matrix<double,3,3> matrix_rotation;   Eigen::Vector3d base_Z(0,0,1);    int i;    float x,y,z;    float azimuth,pitch;    double obj_x,obj_y,obj_z;    //float x_ca,y_ca,z_ca;    Specified_pos(0.2,-0.2,1.4,0,1,0,0);    //Fixed_pos    srand(time(NULL));    for(i = 0;i<50;i++)    {      LOOP:        cout << "i = " << i << endl;//        cout << "success_times = " << success_times << endl;        azimuth = (rand()/(RAND_MAX+0.1))*(2*M_PI);        pitch = (rand()/(RAND_MAX+0.1))*(M_PI/4);        obj_x = sphere_radius * sin(pitch)*cos(azimuth) + center_x;   //球面上的点在UR5基坐标系中的x坐标        obj_y = sphere_radius * sin(pitch)*sin(azimuth) + center_y;   //球面上的点在UR5基坐标系中的y坐标        obj_z = sphere_radius * cos(pitch) + center_z ;   //球面上的点在UR5基坐标系中的z坐标        matrix_sphere_center << center_x,center_y,center_z;        matrix_obj_pt << obj_x,obj_y,obj_z;        //求出z轴        Eigen::Vector3d z_axiz;        z_axiz =  matrix_sphere_center - matrix_obj_pt;        z_axiz.normalize(); //单位化z轴        Eigen::Vector3d x_axiz_assit;        x_axiz_assit << sphere_radius*cos(azimuth),sphere_radius*sin(azimuth),0;        x_axiz_assit.normalize();        Eigen::Vector3d x_axiz(x_axiz_assit.cross(z_axiz));        x_axiz.normalize();        // Terminal y axis        Eigen::Vector3d y_axiz ;        y_axiz = (z_axiz.cross(x_axiz));        y_axiz.normalize();        //得到旋转矩阵        matrix_rotation << x_axiz,y_axiz,z_axiz;        ee2camera << 0,-1,0,0.866026,0,-0.5,0.5,0,0.866026;        matrix_rotation =  matrix_rotation *  ee2camera ;        Eigen::Quaterniond q = Eigen::Quaterniond(matrix_rotation);        q.normalize();        matrix_effect << obj_x,obj_y,obj_z;        //cout << "quaternion =\n" << q.coeffs() << endl; // Eigen 的存储顺序为(x,y,z,w) ,实部为w                                                 // -q.coeffs()本来是没有负号的，但是求出来跟那个软件相反，所以加了个负号，就一样了        //cout << "siyuanshu" << q.w() << endl;        //Specified_pos(obj_x,obj_y,obj_z,q.w(),q.x(),q.y(),q.z());        //***joint_path_command excute and export***//        ros::NodeHandle n;        ros::Publisher joint_trajectory_pub = n.advertise<std_msgs::String>("joint_path_command", 10000);        ros::Rate loop_rate(10);        //int count = 0;        while(ros::ok())        {          Specified_pos(obj_x,obj_y,obj_z,q.w(),q.x(),q.y(),q.z());          std_msgs::String joint_path_command;          for(int n=0;n<10;n++)          {            joint_path_command.data = my_joint_trajectory;            joint_trajectory_pub.publish(joint_path_command);            ros::spinOnce();            loop_rate.sleep();            //++count;          }          if(i<50)          {            i++;            goto LOOP;          }          else          {            break;          }        }    }}void ur5_line(float org_x,float org_y,float org_z,float goal_x,float goal_y,float goal_z){  const int times = 5;  float x_add,y_add,z_add;  float cur_x,cur_y,cur_z;  x_add = (goal_x-org_x)/times;  y_add = (goal_y-org_y)/times;  z_add = (goal_z-org_z)/times;  for(int i=0;i<times+1;i++)  {    cur_x = org_x + x_add*i;    cur_y = org_y + y_add*i;    cur_z = org_z + z_add*i;    Specified_pos(cur_x,cur_y,cur_z,1,0,0,0);    cout << "cur_x =" << cur_x << endl;    cout << "cur_y =" << cur_y << endl;    cout << "cur_z =" << cur_z << endl;  }}int main(int argc, char **argv){    ros::init(argc, argv, "move_group_interface_demo", ros::init_options::AnonymousName);    // 创建一个异步的自旋线程（spinning thread）    ros::AsyncSpinner spinner(1);    spinner.start();    hand_eye_calibration(0.2,0.2,0.7,0.5);    //Specified_pos(0.5,0.3,1.0,0,1,0,0);//    ur5_line(-0.5,0.5,1.0,0.5,0.5,1.0);  ros::shutdown();  return 0;}