Moveit::planning_interface::MoveGroupInterface::move() and execute() methods return before trajectory execution is complete in Noetic

edit

Hello everyone, I am trying to control a KUKA kr10r1100sixx robot with moveit and kuka_eki_hw_interface on ubuntu 20.04 with ROS Noetic. Connection between nodes seems to be correct as I am able to control my real robot from rviz and the moveit API. However, I have a strange behavior with the move() and execute() methods of Moveit::planning_interface::MoveGroupInterface Class. when I call them my robot starts to move and reaches the desire position, BUT the move() and the execute() methods return before the trajectory is completed.

For example with this simple code:

const double tau = 2 * M_PI;

int main(int argc, char **argv){

    ros::init(argc, argv, "move_group_interface");
    ros::NodeHandle node_handle;

    ros::AsyncSpinner spinner(1);
    spinner.start();

    // Setup
    static const std::string PLANNING_GROUP = "kuka_arm";
    moveit::planning_interface::MoveGroupInterface move_group_interface(PLANNING_GROUP);
    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
    const moveit::core::JointModelGroup* joint_model_group = move_group_interface.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

    moveit::core::RobotStatePtr current_state = move_group_interface.getCurrentState();

    moveit::planning_interface::MoveGroupInterface::Plan my_plan;

    // get the current set of joint values for the group.
    std::vector<double> joint_group_positions;
    current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);

    // Now, let's modify one of the joints
    joint_group_positions[0] = tau/6;  // -1/6 turn in radians
    move_group_interface.setJointValueTarget(joint_group_positions);

    move_group_interface.setMaxVelocityScalingFactor(0.05);
    move_group_interface.setMaxAccelerationScalingFactor(0.05);
    move_group_interface.setStartStateToCurrentState();

    bool success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
    std::cout<< "desired position: " << joint_group_positions[0] << std::endl; 
    if(success){
        move_group_interface.execute(my_plan);
    }
    std::cout<< "current position: " << move_group_interface.getCurrentJointValues()[0] << std::endl;

    return 0;
}

my output is :

desired position: 1.0472 
current position: 0.647218

where current position is the position just after execute() has returned.

moreover in the rviz related terminal I have the following output:

You can start planning now!

[ INFO] [1643275596.859554899]: Loading robot model 'kuka_kr10r1100sixx'...
[ INFO] [1643275596.952841080]: Starting planning scene monitor
[ INFO] [1643275596.955281687]: Listening to '/move_group/monitored_planning_scene'
[ INFO] [1643275597.069042839]: Constructing new MoveGroup connection for group 'kuka_arm' in namespace ''
[ INFO] [1643275598.189729895]: Ready to take commands for planning group kuka_arm.
[ INFO] [1643275607.970914515]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1643275607.973524865]: Planner configuration 'kuka_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1643275607.974053888]: kuka_arm/kuka_arm: Starting planning with 1 states already in datastructure
[ INFO] [1643275607.985431161]: kuka_arm/kuka_arm: Created 5 states (2 start + 3 goal)
[ INFO] [1643275607.985503853]: Solution found in 0.011550 seconds
[ INFO] [1643275607.989434298]: SimpleSetup: Path simplification took 0.003820 seconds and changed from 4 to 2 states
[ INFO] [1643275607.990788359]: Execution request received
[ INFO] [1643275611.995134867]: Controller '' successfully finished
[ INFO] [1643275612.050076662]: Completed trajectory execution with status SUCCEEDED ...
[ INFO] [1643275612.050324234]: Execution completed: SUCCEEDED

The last line is printed long before the robot stops.

As the documentation explains that move() and execute() are blocking methods until the traectory is complete, I don't understand the behavior that I'm encountering.

By the way I check the GoalJointTolerance value and it is set to 0.0001.

So,what can be wrong in my code that can explain this behavior? I thought about ... (more)