Moveit classes inside a class

edit

Hi I am trying to wrap MoveIt classes inside another class (which is a backend in a library). I am following the tutorial in http://docs.ros.org/kinetic/api/movei... to figure out how to use all kinematics.

I managed to make the example work with a custom robot. The problem is that when I wrap it inside of a class it seems that is not working. The same piece of code works if I put it in the main.cpp file, but not if I call it from a method of a class. When using the code in the main.cpp it shows the pose of the direct kinematic and it performs the inverse kinematic but when I use it in the class the pose if the direct kinematic is a 4x4 identity matrix and it says that it cannot find the IK (After of each fragment of code there is an example of output)

By now, the class do not store any class member, it is just using the same piece of code. I cannot guess what could be happening. Could it be something scope dependent?

This works: main.cpp

#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>

#include <ros/ros.h>
#include <thread>
#include <chrono>
int main(int _argc, char **_argv)
{
    ros::init(_argc, _argv, "asas");
    ros::AsyncSpinner spinner(1);
    spinner.start();
    std::string mArmPrefix = "";
    ros::NodeHandle n;
    ros::Publisher mJointPublisher = n.advertise<sensor_msgs::JointState>(mArmPrefix + "/joint_states", 1000);

    robot_model_loader::RobotModelLoader robot_model_loader("/robot_description");
    robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();
    ROS_INFO("Model frame: %s", kinematic_model->getModelFrame().c_str());

    robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));
    kinematic_state->setToDefaultValues();
    const robot_state::JointModelGroup *joint_model_group = kinematic_model->getJointModelGroup("arm4dof");

    const std::vector<std::string> &joint_names = joint_model_group->getVariableNames();

    while (ros::ok())
    {
        std::vector<double> joint_values;

        kinematic_state->setToRandomPositions(joint_model_group);
        kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
        for (std::size_t i = 0; i < joint_names.size(); ++i)
        {
            ROS_INFO("Joint %s: %f", joint_names[i].c_str(), joint_values[i]);
        }
        ROS_INFO_STREAM("Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid"));

        /* Enforce the joint limits for this state and check again*/
        kinematic_state->enforceBounds();
        ROS_INFO_STREAM("Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid"));

        kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
        const Eigen::Affine3d &end_effector_state = kinematic_state->getGlobalLinkTransform("arm_2");

        /* Print end-effector pose. Remember that this is in the model frame */
        ROS_INFO_STREAM("Translation: " << end_effector_state.translation());
        ROS_INFO_STREAM("Rotation: " << end_effector_state.rotation());

        bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, 10, 0.1);
        // Now, we can print out the IK solution (if found):
        if (found_ik)
        {
            std::cout << "Inverse kinematic" << std::endl;
            kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
            for (std::size_t i = 0; i < joint_names.size(); ++i)
            {
                ROS_INFO("Joint %s: %f", joint_names[i].c_str(), joint_values[i]);
            }
        }
        else
        {
            ROS_INFO("Did not find IK solution");
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(3000));
    }
    ros::shutdown();
    return 0;
}

Example output:

[ INFO] [1519129629.774489067]: Loading robot model 'arm4DoF'...
[ INFO] [1519129629.793275887]: Model frame: /base_link
[ INFO] [1519129629.793342125]: Joint arm_0_bottom_joint: -1.844674
[ INFO] [1519129629.793351311]: Joint arm_1_joint: 1.262605
[ INFO] [1519129629.793359813]: Joint arm_2_joint: -2.574378
[ INFO] [1519129629.793372435]: Current state is valid
[ INFO] [1519129629.793383459]: Current state is valid
[ INFO] [1519129629 ...