computeCartesianPath() from MoveIt doesn't create a Trajectory

edit

Hello, I'm trying to run the code bellow, but I the computeCartesianPath()-function (inside Solverfunction) won't create a Trajectory. I based this code on the Moveit Cartesian Path Planner Plugin. Running the original code works fine.

Anybody knows what's wrong?

#include <moveit/move_group_interface/move_group.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <ros/ros.h>

#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/planning_interface/planning_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/PlanningScene.h>

#include <boost/function.hpp>
#include <boost/assign/list_of.hpp>
#include <boost/assert.hpp>
#include <math.h>
#include <moveit/robot_state/conversions.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>

#include <tf/tf.h>
#include <tf/LinearMath/Matrix3x3.h>
#include <Eigen/Geometry>
#include <iostream>
#include <stdio.h>

#include <terminal_mocap/terminal_mocap_service.h>
#include <vector>

typedef boost::shared_ptr<move_group_interface::MoveGroup> MoveGroupPtr;
MoveGroupPtr moveit_group_;
const moveit::core::JointModelGroup* joint_model_group;

moveit::core::RobotStatePtr kinematic_state;
bool insidecallback = false;
std::vector<geometry_msgs::Pose> waypoints;
std::vector<double> Trajectoryvector, Velocityvector;

void Solverfunction(std::vector<geometry_msgs::Pose> waypoints_final);


moveit_msgs::RobotTrajectory trajectory;


bool CallBackMocap(terminal_mocap::terminal_mocap_service::Request &req, terminal_mocap::terminal_mocap_service::Response &res)
{

    std::vector<tf::Transform> vector_transform;
    std::vector<geometry_msgs::Pose> waypoints;
    geometry_msgs::Pose puntos;

    printf("Inside bool \n");

    for(int points = 0; points < req.rx.size(); points++)
    {
        tf::Transform singlepoint = tf::Transform(tf::Quaternion(req.rx[points],req.ry[points],req.rz[points],req.rq[points]),tf::Vector3(req.x[points], req.y[points], req.z[points]));

        vector_transform.push_back(singlepoint);
    }

    for (int x = 0; x < vector_transform.size(); x++)
    {
        tf::poseTFToMsg(vector_transform[x], puntos);
        waypoints.push_back(puntos);
    }

    insidecallback = true;
    res.callOK = 1;

}

void Solverfunction(std::vector<geometry_msgs::Pose> waypoints_final)
{
    if (insidecallback)
    {
        double PLAN_TIME_ = 5.0;
        double CART_STEP_SIZE_ = 0.1;
        double CART_JUMP_THRESH_ = 0.1;
        bool MOVEIT_REPLAN_ = true;
        bool AVOID_COLLISIONS_ = false;

        moveit_group_->setPlanningTime(PLAN_TIME_);
        moveit_group_->allowReplanning (MOVEIT_REPLAN_);

        move_group_interface::MoveGroup::Plan plan;
        moveit_msgs::RobotTrajectory trajectory_;
        double fraction = moveit_group_->computeCartesianPath(waypoints,CART_STEP_SIZE_,CART_JUMP_THRESH_,trajectory_,AVOID_COLLISIONS_);
        printf("Calculated %.2f%% \n", fraction * 100);
        sleep(15);
        robot_trajectory::RobotTrajectory rt(kinematic_state->getRobotModel(), "manipulator");

        rt.setRobotTrajectoryMsg(*kinematic_state, trajectory_);

        ROS_INFO_STREAM("Pose reference frame: " << moveit_group_->getPoseReferenceFrame());

        trajectory_processing::IterativeParabolicTimeParameterization iptp;
        bool success = iptp.computeTimeStamps(rt);
        ROS_INFO("Computed time stamp %s \n",success?"SUCCEDED":"FAILED");


        // Get RobotTrajectory_msg from RobotTrajectory
        rt.getRobotTrajectoryMsg(trajectory_);
        // Finally plan and execute the trajectory
        plan.trajectory_ = trajectory_;


        trajectory = trajectory_;


        moveit_group_->execute(plan);

        kinematic_state = moveit_group_->getCurrentState();
    }

}


int main(int argc,char* argv[])
{
    ros::init(argc,argv,"terminal_mocap_node");
    ros::NodeHandle node;
    printf("The node terminal_mocap_node started satisfactorily! \n");

    ros::ServiceServer TerminalMocapService = node.advertiseService("terminal_mocap_service", CallBackMocap);

    moveit_group_ = MoveGroupPtr(new move_group_interface::MoveGroup("manipulator"));
    kinematic_state = moveit::core::RobotStatePtr(moveit_group_->getCurrentState());
    kinematic_state->setToDefaultValues();

    const robot_model::RobotModelConstPtr &kmodel = kinematic_state->getRobotModel();
    joint_model_group = kmodel->getJointModelGroup("manipulator");


    while(ros::ok())
    {

        Solverfunction(waypoints);
        sleep(1);

    ros::spinOnce();
    }


    ros::shutdown();
}