How to plan the Cartesian path with multiple waypoints with MoveItCpp API

edit

Hello,

I'm following the tutorial of MoveIt to learning the MoveItCpp API in ROS Melodic. Before switching to MoveItCpp API, I followed the demo of of Move Group C++ Interface and implemented the Cartesian path, now I want to make an analog Caresian path by MoveItCpp API.

But I can't find way to plan the path after the successfull return of "computeCartesianPath" which will plan the path simultaneously in Move Group C++ Interface. I tried to iteratively setGoal and plan on each result state returned from "computeCartesianPath", but this looks ungraceful and probably time-cosuming. Is there a way, analog to Move Group C++ interface, to plan such Cartesian path?

Here are my code sippets:

// plan05
// 
// restore the start state
planningComponents->setStartState(*robotStartState);
// set the state to the one which satisfies the constraints of Cartesian
auto startState05 = *(moveitCppPtr->getCurrentState());
geometry_msgs::Pose startPose05;
if (dof > 6)
{
    // DOF 7
    startPose05.orientation.w = 1.0;
}
else
{
    // DOF 6
    startPose05.orientation = robotStartPose.orientation;
}
startPose05.position.x = paramStartPose05[0];
startPose05.position.y = paramStartPose05[1];
startPose05.position.z = paramStartPose05[2];
if (startState05.setFromIK(jointModelGroupPtr, startPose05, paramIkTimeout05))
{
    planningComponents->setStartState(startState05);

    // organize waypoints
    EigenSTL::vector_Isometry3d waypoints;
    Eigen::Isometry3d eigenPose;
    tf::poseMsgToEigen(startPose05, eigenPose);
    waypoints.push_back(eigenPose);

    geometry_msgs::Pose wayPose = startPose05;
    // up and back
    for (std::size_t i = 0; i < paramPoseIndex01.size(); ++i)
    {
        if (paramPoseIndex01[i] == "x")
        {
            wayPose.position.x += paramPoseDelta01[i];
        }
        else if (paramPoseIndex01[i] == "y")
        {
            wayPose.position.y += paramPoseDelta01[i];
        }
        else
        {
            wayPose.position.z += paramPoseDelta01[i];
        }
    }
    tf::poseMsgToEigen(wayPose, eigenPose);
    waypoints.push_back(eigenPose);
    // left
    for (std::size_t i = 0; i < paramPoseIndex02.size(); ++i)
    {
        if (paramPoseIndex02[i] == "x")
        {
            wayPose.position.x += paramPoseDelta02[i];
        }
        else if (paramPoseIndex02[i] == "y")
        {
            wayPose.position.y += paramPoseDelta02[i];
        }
        else
        {
            wayPose.position.z += paramPoseDelta02[i];
        }
    }
    tf::poseMsgToEigen(wayPose, eigenPose);
    waypoints.push_back(eigenPose);
    // down and right
    for (std::size_t i = 0; i < paramPoseIndex03.size(); ++i)
    {
        if (paramPoseIndex03[i] == "x")
        {
            wayPose.position.x += paramPoseDelta03[i];
        }
        else if (paramPoseIndex03[i] == "y")
        {
            wayPose.position.y += paramPoseDelta03[i];
        }
        else
        {
            wayPose.position.z += paramPoseDelta03[i];
        }
    }
    tf::poseMsgToEigen(wayPose, eigenPose);
    waypoints.push_back(eigenPose);

    std::vector<moveit::core::RobotStatePtr> resTraj;
    double fraction = startState05.computeCartesianPath(jointModelGroupPtr, resTraj, jointModelGroupPtr->getLinkModel(paramVisualLink), waypoints,
        true, paramEndEffectorStep, paramJumpThreshold);
    ROS_INFO_NAMED("demo", "visualizing plan 05 (Cartesian path) (%.2f%% achieved)", fraction * 100.0);

    visualTools.publishText(textPose, "Cartesian path", rvt::WHITE, rvt::XLARGE);
    for (std::size_t i = 0; i < waypoints.size(); ++i)
    {
        visualTools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL);
    }
    visualTools.trigger();

    for (auto it : resTraj)
    {
        planningComponents->setGoal(*it);
        auto planSolution05 = planningComponents->plan();
        // set the previous state to current
        planningComponents->setStartState(*it);

        std::this_thread::sleep_for(std::chrono::milliseconds(200));
    }
}
else
{
    visualTools.publishText(textPose, "failed to get the IK from pre-set start state", rvt::WHITE, rvt::XLARGE);
    visualTools.publishAxisLabeled(startPose05, "target_pose");
    visualTools.trigger();
}

// start the next plan
visualTools.deleteAllMarkers();
visualTools.prompt("Press 'next' to continue with plan 06");

iteratively setGoal and plan behavior:

Thanks for your insights!