sequence_move_group Unexpectedly Returns UNKNOWN Code

[maxwelllls]

Description

When testing the sequence_move_group functionality of Pilz for sequential trajectory motion planning, this bug is triggered after running it only a few times.

After sending an async_send_goal() request, the result_callback is immediately invoked, and the returned result.code is UNKNOWN.You can see it on the time stamp.

Therefore, I suspect this might be related to rclcpp_action, but I am unsure how to further locate and troubleshoot the issue.

NOTE:The provided code is a simplified minimal reproducible scenario. During actual testing, this issue occurs with both single-segment and multi-segment trajectories.

ROS Distro

Humble

OS and version

Ubuntu 22.04

Source or binary build?

Source

If binary, which release version?

No response

If source, which branch?

No response

Which RMW are you using?

CycloneDDS

Steps to Reproduce

1. Configure the pilz planning library and launch the robotic arm package.
2. Run the test code ros2 run sequence_move_demo demo

NOTE:
Change the "master_arm" in the code to your robot move group,and set joint at 'home' pose

#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit_msgs/action/move_group_sequence.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/kinematic_constraints/utils.h>
#include <rapidjson/document.h>
#include <chrono>
#include <memory>
#include <string>
#include <map>
#include <vector>
#include <thread>
#include <functional>
#include <cmath>

class SequenceMoveDemo : public rclcpp::Node
{
public:
    SequenceMoveDemo();
    ~SequenceMoveDemo();
    void initialize();
    void run();

private:
    // Member variables
    moveit::core::RobotModelPtr moveit_robot_model_;
    std::shared_ptr<rclcpp_action::Client<moveit_msgs::action::MoveGroupSequence>> sequence_move_group_client_;
    moveit_msgs::msg::MotionSequenceRequest sequence_request_;
    bool last_trajectory_connect_bool_;
    std::thread worker_thread_;
    std::vector<double> last_joint_positions_; // Store last joint positions
};

SequenceMoveDemo::SequenceMoveDemo()
    : Node("sequence_move_demo"), last_trajectory_connect_bool_(false)
{
    // Initialize the action client
    sequence_move_group_client_ = rclcpp_action::create_client<moveit_msgs::action::MoveGroupSequence>(this, "/sequence_move_group");
    RCLCPP_INFO(this->get_logger(), "Created action client: /sequence_move_group");

    // Initialize last_joint_positions_ to zeros (default start state)
    last_joint_positions_ = std::vector<double>(6, 0.0); // Assuming 6 DOF robot
}

SequenceMoveDemo::~SequenceMoveDemo()
{
    if (worker_thread_.joinable())
    {
        worker_thread_.join();
    }
}

void SequenceMoveDemo::initialize()
{
    // Initialize the robot model loader using shared_from_this()
    auto robot_model_loader = std::make_shared<robot_model_loader::RobotModelLoader>(
        shared_from_this(), "robot_description");
    moveit_robot_model_ = robot_model_loader->getModel();

    // Start a thread to run the main loop
    worker_thread_ = std::thread(&SequenceMoveDemo::run, this);
}

void SequenceMoveDemo::run()
{
    std::vector<std::vector<double>> positions = {
        {0.0, 0.0, 0.1, 0.0, 0.0, 0.0},  // Position A
        {0.0, 0.0, 0.5, 0.0, 0.0, 0.0}   // Position B
    };

    while (rclcpp::ok())
    {
        for (const auto& target_joint : positions)
        {
            // Basic configuration data acquisition
            auto current_state = std::make_shared<moveit::core::RobotState>(moveit_robot_model_);
            current_state->setToDefaultValues();
            const moveit::core::JointModelGroup* joint_model_group = current_state->getJointModelGroup("master_arm");
            const std::vector<std::string>& joint_names = joint_model_group->getActiveJointModelNames();

            moveit_msgs::msg::MotionSequenceItem item;
            item.req.start_state.joint_state.name = joint_names;

            // Use the last_joint_positions_ for the start state
            item.req.start_state.joint_state.position = last_joint_positions_;
            last_joint_positions_ = target_joint;
            RCLCPP_INFO(this->get_logger(), "Set start state to last joint positions.");

            // Goal constraints
            item.req.goal_constraints.clear();
            auto joint_goal_state = std::make_shared<moveit::core::RobotState>(moveit_robot_model_);
            joint_goal_state->setJointGroupPositions(joint_model_group, target_joint);
            joint_goal_state->update();
            item.req.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints(*joint_goal_state, joint_model_group));

            item.req.group_name = "master_arm";
            item.req.planner_id = "PTP"; // or "LIN" or "CIRC"
            item.req.allowed_planning_time = 5.0;
            item.req.max_velocity_scaling_factor = 0.5;
            item.req.max_acceleration_scaling_factor = 0.5;
            item.blend_radius = 0.0;
            sequence_request_.items.push_back(item);

            // Wait for action server
            if (!sequence_move_group_client_->wait_for_action_server(std::chrono::seconds(10))) {
                RCLCPP_ERROR(this->get_logger(), "Error waiting for action server /sequence_move_group");
                return;
            }

            // Create action goal
            auto goal_msg = moveit_msgs::action::MoveGroupSequence::Goal();
            goal_msg.request = sequence_request_;

            // Configure planning options
            goal_msg.planning_options.planning_scene_diff.is_diff = true;
            goal_msg.planning_options.planning_scene_diff.robot_state.is_diff = true;

            // Plan only
            goal_msg.planning_options.plan_only = false;
            bool done = false;
            rclcpp_action::ResultCode code = rclcpp_action::ResultCode::UNKNOWN;

            auto send_goal_options = rclcpp_action::Client<moveit_msgs::action::MoveGroupSequence>::SendGoalOptions();
            send_goal_options.goal_response_callback =
                [&](const rclcpp_action::ClientGoalHandle<moveit_msgs::action::MoveGroupSequence>::SharedPtr& goal_handle) {
                    if (!goal_handle)
                    {
                        done = true;
                        RCLCPP_INFO(this->get_logger(), "Planning request rejected");
                    }
                    else
                        RCLCPP_INFO(this->get_logger(), "Planning request accepted");
                };
            send_goal_options.result_callback =
                [&](const rclcpp_action::ClientGoalHandle<moveit_msgs::action::MoveGroupSequence>::WrappedResult& result) {
                    code = result.code;

                    RCLCPP_INFO(this->get_logger(), "Planning request result code= %d", static_cast<int>(code));

                    // Print result information
                    if (result.result)
                    {
                        RCLCPP_INFO(this->get_logger(), "Error code: %d", result.result->response.error_code.val);
                        RCLCPP_INFO(this->get_logger(), "Planning time: %f seconds", result.result->response.planning_time);
                    }
                    else
                    {
                        RCLCPP_WARN(this->get_logger(), "Result information is empty.");
                    }

                    switch (result.code)
                    {
                        case rclcpp_action::ResultCode::SUCCEEDED:
                            RCLCPP_INFO(this->get_logger(), "Planning request complete!");
                            done = true;
                            break;
                        case rclcpp_action::ResultCode::ABORTED:
                            RCLCPP_INFO(this->get_logger(), "Planning request aborted");
                            done = true;
                            return;
                        case rclcpp_action::ResultCode::CANCELED:
                            RCLCPP_INFO(this->get_logger(), "Planning request canceled");
                            done = true;
                            return;
                        default:
                            RCLCPP_INFO(this->get_logger(), "Planning request unknown result code");
                            return;
                    }
                };

            RCLCPP_INFO(this->get_logger(), "Number of items in request: %zu", goal_msg.request.items.size());

            // Send the goal
            RCLCPP_INFO(this->get_logger(), "Sending goal with plan_only=false");
            auto goal_handle_future = sequence_move_group_client_->async_send_goal(goal_msg, send_goal_options);
            while (!done)
            {
                std::this_thread::sleep_for(std::chrono::milliseconds(1));
            }

            sequence_request_.items.clear();
            RCLCPP_INFO(this->get_logger(), "Cleared sequence request items.");

            // Wait for 0.5 seconds before switching to the next position
            std::this_thread::sleep_for(std::chrono::milliseconds(1000));
        }
    }
}

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<SequenceMoveDemo>();
    node->initialize(); // Ensure the instance is fully constructed
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

Expected behavior

The robot runs back and forth between position A and B

Actual behavior

It stops after 5-10 runs

Backtrace or Console output

move_group log.txt

1732012197.378579 [69] demo: determined enp0s31f6 (udp/192.168.99.69) as highest quality interface, selected for automatic interface.
[INFO] [1732012197.428778175] [sequence_move_demo]: Created action client: /sequence_move_group
[INFO] [1732012197.911398975] [moveit_4087791879.moveit.ros.rdf_loader]: Loaded robot model in 0.473234 seconds
[INFO] [1732012197.911444098] [moveit_4087791879.moveit.core.robot_model]: Loading robot model 'rml_63_description'...
[INFO] [1732012197.911463227] [moveit_4087791879.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ERROR] [1732012198.146835897] [pluginlib.ClassLoader]: Skipped loading plugin with error: XML Document '/home/boomy/ws_moveit2/install/moveit_chomp_optimizer_adapter/share/moveit_chomp_optimizer_adapter/chomp_optimizer_adapter_plugin_description.xml' has no Root Element. This likely means the XML is malformed or missing..
[WARN] [1732012198.152957095] [moveit_4087791879.moveit.ros.robot_model_loader]: No kinematics plugins defined. Fill and load kinematics.yaml!
[INFO] [1732012198.153569905] [sequence_move_demo]: Set start state to last joint positions.
[INFO] [1732012198.156182744] [sequence_move_demo]: Number of items in request: 1
[INFO] [1732012198.156243076] [sequence_move_demo]: Sending goal with plan_only=false
[INFO] [1732012198.163143472] [sequence_move_demo]: Planning request accepted
[INFO] [1732012198.508954695] [sequence_move_demo]: Planning request result code= 4
[INFO] [1732012198.509003203] [sequence_move_demo]: Error code: 1
[INFO] [1732012198.509218862] [sequence_move_demo]: Planning time: 0.000000 seconds
[INFO] [1732012198.509281222] [sequence_move_demo]: Planning request complete!
[INFO] [1732012198.509559068] [sequence_move_demo]: Cleared sequence request items.
[INFO] [1732012203.509717511] [sequence_move_demo]: Set start state to last joint positions.
[INFO] [1732012203.510228768] [sequence_move_demo]: Number of items in request: 1
[INFO] [1732012203.510266151] [sequence_move_demo]: Sending goal with plan_only=false
[INFO] [1732012203.512238240] [sequence_move_demo]: Planning request accepted
[INFO] [1732012204.174324262] [sequence_move_demo]: Planning request result code= 4
[INFO] [1732012204.174396365] [sequence_move_demo]: Error code: 1
[INFO] [1732012204.174414847] [sequence_move_demo]: Planning time: 0.000000 seconds
[INFO] [1732012204.174432389] [sequence_move_demo]: Planning request complete!
[INFO] [1732012204.175300527] [sequence_move_demo]: Cleared sequence request items.
[INFO] [1732012209.175479874] [sequence_move_demo]: Set start state to last joint positions.
[INFO] [1732012209.175857108] [sequence_move_demo]: Number of items in request: 1
[INFO] [1732012209.175896998] [sequence_move_demo]: Sending goal with plan_only=false
[INFO] [1732012209.176965139] [sequence_move_demo]: Planning request accepted
[INFO] [1732012209.823131012] [sequence_move_demo]: Planning request result code= 4
[INFO] [1732012209.823174214] [sequence_move_demo]: Error code: 1
[INFO] [1732012209.823189162] [sequence_move_demo]: Planning time: 0.000000 seconds
[INFO] [1732012209.823204600] [sequence_move_demo]: Planning request complete!
[INFO] [1732012209.823775718] [sequence_move_demo]: Cleared sequence request items.
[INFO] [1732012214.823932331] [sequence_move_demo]: Set start state to last joint positions.
[INFO] [1732012214.824249063] [sequence_move_demo]: Number of items in request: 1
[INFO] [1732012214.824292035] [sequence_move_demo]: Sending goal with plan_only=false
[INFO] [1732012214.825031389] [sequence_move_demo]: Planning request accepted
[INFO] [1732012215.473047477] [sequence_move_demo]: Planning request result code= 4
[INFO] [1732012215.473095251] [sequence_move_demo]: Error code: 1
[INFO] [1732012215.473112264] [sequence_move_demo]: Planning time: 0.000000 seconds
[INFO] [1732012215.473128071] [sequence_move_demo]: Planning request complete!
[INFO] [1732012215.473412593] [sequence_move_demo]: Cleared sequence request items.
[INFO] [1732012220.473593633] [sequence_move_demo]: Set start state to last joint positions.
[INFO] [1732012220.474040410] [sequence_move_demo]: Number of items in request: 1
[INFO] [1732012220.474071576] [sequence_move_demo]: Sending goal with plan_only=false
[INFO] [1732012220.474931430] [sequence_move_demo]: Planning request accepted
[INFO] [1732012221.133010556] [sequence_move_demo]: Planning request result code= 4
[INFO] [1732012221.133076824] [sequence_move_demo]: Error code: 1
[INFO] [1732012221.133095239] [sequence_move_demo]: Planning time: 0.000000 seconds
[INFO] [1732012221.133111218] [sequence_move_demo]: Planning request complete!
[INFO] [1732012221.133229187] [sequence_move_demo]: Cleared sequence request items.
[INFO] [1732012226.133374048] [sequence_move_demo]: Set start state to last joint positions.
[INFO] [1732012226.133698616] [sequence_move_demo]: Number of items in request: 1
[INFO] [1732012226.133729362] [sequence_move_demo]: Sending goal with plan_only=false
[INFO] [1732012226.135707183] [sequence_move_demo]: Planning request accepted
[INFO] [1732012226.782810248] [sequence_move_demo]: Planning request result code= 4
[INFO] [1732012226.782856312] [sequence_move_demo]: Error code: 1
[INFO] [1732012226.782873915] [sequence_move_demo]: Planning time: 0.000000 seconds
[INFO] [1732012226.782891302] [sequence_move_demo]: Planning request complete!
[INFO] [1732012226.783230136] [sequence_move_demo]: Cleared sequence request items.
[INFO] [1732012231.783368712] [sequence_move_demo]: Set start state to last joint positions.
[INFO] [1732012231.783669544] [sequence_move_demo]: Number of items in request: 1
[INFO] [1732012231.783699079] [sequence_move_demo]: Sending goal with plan_only=false
[INFO] [1732012231.784493893] [sequence_move_demo]: Planning request accepted
[INFO] [1732012232.443191784] [sequence_move_demo]: Planning request result code= 4
[INFO] [1732012232.443242262] [sequence_move_demo]: Error code: 1
[INFO] [1732012232.443257714] [sequence_move_demo]: Planning time: 0.000000 seconds
[INFO] [1732012232.443322554] [sequence_move_demo]: Planning request complete!
[INFO] [1732012232.448162755] [sequence_move_demo]: Cleared sequence request items.
[INFO] [1732012237.448327356] [sequence_move_demo]: Set start state to last joint positions.
[INFO] [1732012237.448659886] [sequence_move_demo]: Number of items in request: 1
[INFO] [1732012237.448686806] [sequence_move_demo]: Sending goal with plan_only=false
[INFO] [1732012237.449232138] [sequence_move_demo]: Planning request accepted
[INFO] [1732012238.102866545] [sequence_move_demo]: Planning request result code= 4
[INFO] [1732012238.102929447] [sequence_move_demo]: Error code: 1
[INFO] [1732012238.102946452] [sequence_move_demo]: Planning time: 0.000000 seconds
[INFO] [1732012238.102964450] [sequence_move_demo]: Planning request complete!
[INFO] [1732012238.103834674] [sequence_move_demo]: Cleared sequence request items.
[INFO] [1732012243.104020065] [sequence_move_demo]: Set start state to last joint positions.
[INFO] [1732012243.104996102] [sequence_move_demo]: Number of items in request: 1
[INFO] [1732012243.105047145] [sequence_move_demo]: Sending goal with plan_only=false
[INFO] [1732012243.105816297] [sequence_move_demo]: Planning request accepted
[INFO] [1732012243.106202476] [sequence_move_demo]: Planning request result code= 0
[INFO] [1732012243.106232580] [sequence_move_demo]: Error code: 0
[INFO] [1732012243.106248451] [sequence_move_demo]: Planning time: 0.000000 seconds
[INFO] [1732012243.106264605] [sequence_move_demo]: Planning request unknown result code