Hi everyone, I am facing an issue while trying to use cartesian paths with moveit. I am developing a GUI for my robot, which includes functionalities to achieve stop pause and continue for robot control.

I am using a staubli arm(tx260). I can stop active motions by using /stop_motion service provided by industrial_core. It works well when I use joint space methods. But when I use this service to stop Cartesian paths it cracks the program. I have tried to understand the reason but failed so far.

Right after calling /stop_motion service in log I sometimes see;

[ERROR] [1597395259.432551613]: Failed to initialize message
[ WARN] [1597395259.432589447]: Failed sent joint point, will try again

This most probably occurs because Cartesian paths are denser and I call service /stop_motion right away cutting the message initialization.

After I call /stop_motion service I also call move_group.stop() which results in;

[ INFO] [1597395259.441869902]: Received event 'stop'
[ INFO] [1597395259.441915581]: Cancelling execution for 
[ INFO] [1597395259.441944588]: Stopped trajectory execution.
[ INFO] [1597395259.442181566]: Controller  successfully finished
[ INFO] [1597395259.442224900]: Completed trajectory execution with status PREEMPTED ...
[ INFO] [1597395259.442394479]: Execution completed: PREEMPTED

which looks ok. But the problem occurs when I want to continue the path. In every motion command I keep a copy of initial goal, so the continue function just tries to move robot to there. But when I call move_group.execute(myplan); Trajectory wont be executed and I have to restart the program to control robot. I will be glad to hear if someone can show me what are possible mistakes I am doing here;

more information can be provided in case it is needed.

Edit for answer of @fvd;

There is really not much of error outputs excepts those two lines of my original question.

Now the exact function that I use is below Whenever pause is called(Through GUI) /stop_motion service is utilized to stop robot immediately and the 2 ros params are used to define the logic of pause, continue, After pause , the robot goal is reset and preempted, with the continue a new plan is being planned from the current pose to original goal(in baselink frame), so I think it should be not related to it start pose being to far from the current one. Please note that `robottcpgoalintoolspace` is not actually a pose , but distances to travel in x,y,z coordinates in tool space,

bool RobotController::moveEndEffectortoGoalinToolSpace(geometry_msgs::Pose robot_tcp_goal_in_tool_space,
                                                       moveit::planning_interface::MoveGroupInterface *move_group_ptr_, tf::TransformListener *listener_) {

    bool returnMoveitResult = false;
    // get given goal position(in tool_link frame)
    geometry_msgs::Pose msg_robot_goal_in_tool_link;
    msg_robot_goal_in_tool_link.position = robot_tcp_goal_in_tool_space.position;

    // we will trasnform the given goal from tool_link to base_link
    tf::Pose tf_robot_goal_in_tool_link;
    tf::poseMsgToTF(msg_robot_goal_in_tool_link, tf_robot_goal_in_tool_link);

    // get transform between tool_link and base_link
    tf::StampedTransform tool_to_base_link_transform;
    // lookup transform (this should be cached, since it’s probably static)
    listener_->lookupTransform(robot_base_link, robot_eef_link, ros::Time(0.0f), tool_to_base_link_transform);

    // do the transform by multiplying transform matrices with goal pose in tool_link
    tf::Pose tf_robot_goal_in_base_link;
    tf_robot_goal_in_base_link = tool_to_base_link_transform * tf_robot_goal_in_tool_link;

    // MOVEIT uses geometry_msgs type , so convert transformed goal pose to geometry_msgs
    geometry_msgs::Pose msg_robot_goal_in_base_link;
    tf::poseTFToMsg(tf_robot_goal_in_base_link, msg_robot_goal_in_base_link);

    // we will do a linear plan between current pose and given target pose, so push the current pose to waypoint vector
    std::vector<geometry_msgs::Pose> waypoints;
    waypoints.push_back(move_group_ptr_->getCurrentPose().pose);

    // Since we are moving along tool_link , the orintation of robot tcp shall not change
    msg_robot_goal_in_base_link.orientation = move_group_ptr_->getCurrentPose().pose.orientation;
    waypoints.push_back(msg_robot_goal_in_base_link);

    // compute a cartesian path between target pose(in base_link) and current pose
    moveit_msgs::RobotTrajectory trajectory;

    // fraction is double between 0.0 and 1.0. It indicates the percentage of sucessfully planed path
    double fraction = move_group_ptr_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);

    // stream how much percent of this cartesian path can be achieved
    ROS_INFO("Visualizing plan for (Cartesian path) (%.2f%% acheived)", fraction * 100.0);

    // We dont actually plan, but we need to set the trajectory of plan to calculated cartesian path
    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
    my_plan.trajectory_ = trajectory;

    // publish a path to visualize planned path
    publishRobotTrajectory(move_group_ptr_, my_plan.trajectory_);

    // Make sure that the planned path does not violate any joint rotation limit
    isValidPlan(my_plan);

    // Move the manipulator according to calculated cartesian(linear) path
    returnMoveitResult = (move_group_ptr_->execute(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

    bool is_pause_motion_service_called;
    node_handle_ptr_->getParam("is_pause_motion_service_called", is_pause_motion_service_called);

    if (is_pause_motion_service_called) {
        node_handle_ptr_->setParam("is_pause_motion_service_called", false);
        volatile bool is_resume_motion_service_called;
        bool temp;
        node_handle_ptr_->getParam("is_resume_motion_service_called", temp);
        is_resume_motion_service_called = temp;
        while (!is_resume_motion_service_called) {
            node_handle_ptr_->getParam("is_resume_motion_service_called", temp);
            is_resume_motion_service_called = temp;
            ROS_WARN("YOU HAVE PAUSED ROBOT IN MIDDLE OF CARTESIAN PATH EXECUTION, WAITING FOR CONTINUE BUTTON TO BE PRESSED !!!");
            sleep(1.0);
        }
        {
             waypoints.clear();
             waypoints.push_back(move_group_ptr_->getCurrentPose().pose);
             // Since we are moving along tool_link , the orintation of robot tcp shall not change
             msg_robot_goal_in_base_link.orientation = move_group_ptr_->getCurrentPose().pose.orientation;
             waypoints.push_back(msg_robot_goal_in_base_link);
             // fraction is double between 0.0 and 1.0. It indicates the percentage of sucessfully planed path
             fraction = move_group_ptr_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
             my_plan.trajectory_ = trajectory;
             // publish a path to visualize planned path
             publishRobotTrajectory(move_group_ptr_, my_plan.trajectory_);
             // Make sure that the planned path does not violate any joint rotation limit
             isValidPlan(my_plan);
             ROS_WARN("PLANNED A VALID PLAN AND GONNA CALL EXECUTE !!!");
             // Move the manipulator according to calculated cartesian(linear) path
             returnMoveitResult = (move_group_ptr_->execute(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
             ROS_WARN("CONTINUE HANDLED !!! ");

            node_handle_ptr_->setParam("is_resume_motion_service_called", false);
        }
    }

    return returnMoveitResult;
}