How to get waypoints of a trajectory?

edit

Hello, I am using the code to plan the robot trajectory and i get a effective one. If i want save all the waypoints of this trajectory, how can i do?

@PeteBlackerThe3rd
Sorry, i didn't post the problem clearly and my English is not very good. In Moveit, I want to simulate a feasible trajectory between two points with UR10 robot model and get all waypoints(the end-effector's position and orientation) on the trajectory, just like the rviz plugin, Motion Planning Slider.

1. The path constraint is used in the planning

2. As simulation, the environment with many obstacles is very complicated, so the planning should be circularly run to get a feasible trajectory. Then i want to save this trajectory by exporting all the waypoints to a .txt file

3. Now the trajectory can be obtained and it seems that the code, using Moveit C++ API, cannot export the interpolated waypoints (the end-effector's position and orientation), but the robot's joint states of each point of the trajectory.

I attached the code as followed.

   int main(int argc, char **argv)
    {
       ros::init(argc, argv, "move_group_interface_tutorial");
        ros::NodeHandle node_handle;
        ros::AsyncSpinner spinner(1);
        spinner.start();


        /* This sleep is ONLY to allow Rviz to come up */
        sleep(20.0);

        moveit::planning_interface::MoveGroup group_r("arm");
        moveit_msgs::DisplayTrajectory display_trajectory;
        ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);

        group_r.setPlannerId("RRTConnectkConfigDefault");
        group_r.setPlanningTime(45);

//OrientationConstraint
moveit_msgs::OrientationConstraint ocm;

        ocm.link_name = group_r.getEndEffectorLink();
        ocm.header.frame_id = group_r.getPlanningFrame();



        ocm.absolute_x_axis_tolerance = 0.01;
        ocm.absolute_y_axis_tolerance = 0.01;
        ocm.absolute_z_axis_tolerance = 3.14;
        ocm.weight = 1.0;
        moveit_msgs::Constraints test_constraints;
        test_constraints.orientation_constraints.push_back(ocm);
        group_r.setPathConstraints(test_constraints);



  //start pose

       robot_state::RobotState start_state(*group_r.getCurrentState());
       geometry_msgs::Pose start_pose=group_r.getCurrentPose().pose;

        const robot_state::JointModelGroup *joint_model_group =    start_state.getJointModelGroup(group_r.getName());
        start_state.setFromIK(joint_model_group, start_pose);
        group_r.setStartState(start_state);



  // target  
        geometry_msgs::Pose next_pose;



            next_pose.position.x =-0.0383861;
            next_pose.position.y = -0.70;
           next_pose.position.z =-0.14;             
            next_pose.orientation.x = ocm.orientation.x;
            next_pose.orientation.y = ocm.orientation.y;
            next_pose.orientation.z = ocm.orientation.z;
            next_pose.orientation.w = ocm.orientation.w;



            group_r.setPoseTarget(next_pose, ocm.link_name );


        moveit::planning_interface::MoveGroup::Plan my_plan;
        bool success = false;

        int k = 0;
        int count = 0;

//plan and print the points into the specific file

    while (1)
    {

          success=group_r.plan(my_plan);
          stringstream ss; 
          if(success==true)
       {
          count++;
          moveit_msgs::RobotTrajectory msg;  
          msg =my_plan.trajectory_
          std::vector<trajectory_msgs::MultiDOFJointTrajectoryPoint> trajectory_points;  

         std::vector<int>::size_type size1 = msg.joint_trajectory.points.size();  

          for (unsigned i=0; i<size1; i++)  
          {  


              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; 


         }  

          ofstream outfile ("/home/aicrobo/Downloads/points_save/points.txt",ios::app);
        if(!outfile.is_open())
                    {

                            ROS_INFO("open failed");
                    }
          else
            {  

              outfile<<"trajectory"<<count<<endl;
              outfile<<msg.multi_dof_joint_trajectory.joint_names[0]<<endl;
              outfile<<ss.str()<<endl;
              outfile.close();
            } 
        }


         sleep(5.0);
    }





    group_r.clearPathConstraints();



    }