Motion Planner unable to retreive the actual Robot State from the Planning Scene

edit

I am having trouble executing a simple Point-To-Point movement-plan which was calculated by a planning_interface::MotionPlanRequest. The trajectory calculation succeeds, but when trying to execute it nothing is working. Apparently I am fetching an incorrect robot_state, even though the actual robot in Rviz is in valid, different position. My guess is, that it is an issue with the planning_scene I am refering. My other applications working with joint space goals as well as cartesian paths work fine, the issue seems to be in absolute cartesian space as my trajectory seems to be computed from a static pose of the robot which is never updated subsequently (see my update at the bottom)

Environment

• ROS Distro: Kinetic
• OS Version: Ubuntu 16.04
• Binary build, latest

Steps to reproduce

The code is quite similar to the one in the Motion-Planning-API tutorials, the only difference is that I am using a different robot, try it with your own robot: https://docs.ros.org/kinetic/api/move...

Expected behaviour

The planner (I am using OMPL RRT::geometric) is given a request, which should contain the start state of my robot, derived from the planningScene in use, as is explained here: http://docs.ros.org/melodic/api/movei...

Actual behaviour

The robot is supposed to execute the calculated trajectory, but when asked to do so, I am given the following Error-Message in the terminal, where my planning_execution.launch (rviz) is running:

Invalid Trajectory: start point deviates from current robot state more than 0.01 joint 'joint_b': expected: 0, current: -1.81088

As I already said, this might be an issue with the planning_scene, as it is apparently not containing the robot_state which is displayed in rviz, but this is just my guess. It's worth noting that I have two planning scene's in the rviz dropdown menu being displayed, maybe this is causing the issue:

• /move_group/monitored_planning_scene
• planning_scene

Another info I can give is that I am using the trac_ik kinematics solver

Code

` planning_interface::MotionPlanRequest req; planning_interface::MotionPlanResponse res;

moveit::planning_interface::MoveGroupInterface move_group("gripper");
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();
robot_state::RobotStatePtr robot_state(new robot_state::RobotState(robot_model));
const robot_state::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP);

/* robot_state::RobotState start_state(*move_group.getCurrentState());
start_state.setFromIK(joint_model_group, move_group.getCurrentPose().pose);
move_group.setStartState(start_state); */

// Refering the robot's environment from different topics through Moveit! messages:
planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model));
planning_scene->getCurrentStateNonConst().setToDefaultValues(joint_model_group, "ready");

// Loading a valid planner from the parameter sever:
boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager>> planner_plugin_loader;
planning_interface::PlannerManagerPtr planner_instance;

try 
{
    planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>("moveit_core", "planning_interface::PlannerManager"));
}
catch (pluginlib::PluginlibException& ex)
{
    ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what());
}
try
{
    planner_instance.reset(planner_plugin_loader->createUnmanagedInstance("ompl_interface/OMPLPlanner"));

    if (!planner_instance->initialize(robot_model, nh_.getNamespace()))
    {
        ROS_FATAL_STREAM("Could not initialize planner instance");
        ROS_INFO_STREAM("Using planning interface '" << planner_instance->getDescription() << "'");
    }
}    
catch (pluginlib::PluginlibException& ex)
{
    const std::vector<std::string>& classes = planner_plugin_loader->getDeclaredClasses();
    std::stringstream ss;

    for (std::size_t i = 0; i < classes.size(); ++i)
    {
        ss << classes[i] << " ";
        ROS_ERROR_STREAM("Exception while loading planner "  ": " << ex.what() << std::endl ...