How to Figure out Moveit2 Motion Execution Succeed on Real Robot

Question

For the pick and place operation with real Fanuc robot, I am using the following framework, libraries:

• Motion Planning: Moveit2
• Control: ros2_control
• Task Management: Smach State Machine

To be able to check whether the motion is planned successfully , I am using the moveit::core::MoveItErrorCode class

Related section from my Moveit2 action server:

auto arm_move_group = moveit::planning_interface::MoveGroupInterface(shared_from_this(), "arm");
std::vector<double> arm_joint_goal = {j1, j2, j3, j4, j5, j6};
arm_move_group.setJointValueTarget(arm_joint_goal);

moveit::planning_interface::MoveGroupInterface::Plan arm_plan;
auto success = arm_move_group.plan(arm_plan);

if (success == moveit::core::MoveItErrorCode::SUCCESS)
{
    arm_move_group.move();
    result->success = true;
    goal_handle->succeed(result);
    RCLCPP_INFO(get_logger(), "Arm Goal succeeded");
}

else{
    RCLCPP_ERROR(this->get_logger(),"Arm Goal Failed Because: %s" ,moveit::core::error_code_to_string(success).c_str());
    result->success = false;
    goal_handle->succeed(result);
}

However the result->success = true (line 9) is executed before arm_move_group.move() (line 8) is fully completed which causes synchronization problem for pick and place operation because after action sends the True result, picking process is triggered for the gripper while robot is not reached to gripping pose.

Answer 1

We solved the problem with the following code:

const std::shared_ptr<rclcpp_action::ServerGoalHandle<fanuc_msgs::action::MultiGroup>> goal_handle;
auto result = std::make_shared<fanuc_msgs::action::MultiGroup::Result>();
moveit::planning_interface::MoveGroupInterface arm_move_group = moveit::planning_interface::MoveGroupInterface(shared_from_this(), "arm");
moveit::planning_interface::MoveGroupInterface::Plan moveit_plan;
std::vector<double> arm_joint_goal = {j1, j2, j3, j4, j5, j6};

bool pose_valid = arm_move_group.setJointValueTarget(arm_joint_goal);

if (pose_valid)
{
    RCLCPP_INFO(get_logger(), "Joint Target is valid for ARM");

    moveit::core::MoveItErrorCode plan_success = move_group.plan(moveit_plan);

    if (plan_success == moveit::core::MoveItErrorCode::SUCCESS)
    {
        RCLCPP_INFO(get_logger(), "Joint Target plan is successful for ARM");

        moveit::core::MoveItErrorCode execution_success = move_group.execute(moveit_plan);

        if (execution_success == moveit::core::MoveItErrorCode::SUCCESS)
        {
            RCLCPP_INFO(get_logger(), "Joint Target execution is successful for ARM");
            result->success = true;
        }
        else
        {
            RCLCPP_ERROR(get_logger(), "Joint Target execution is failed for ARM. Because: %s" ,moveit::core::error_code_to_string(execution_success).c_str());
            result->success = false;
        }
    }

    else
    {
        RCLCPP_ERROR(get_logger(), "Joint Target plan is failed for ARM. Because: %s" ,moveit::core::error_code_to_string(plan_success).c_str());
        result->success = false;
    }
}

else
{
    RCLCPP_ERROR(get_logger(), "Joint Target is not valid for ARM");
    result->success = false;
}

goal_handle->succeed(result);