joint_trajectory_client Blocks After Sending a Command

[Khouloud-Hamrouni]

I'm still struggling with the joint_trajectory_client demo lol.
When running joint_trajectory_client.cpp, the node blocks after sending the first trajectory command. Although the action server accepts the goal, the node never returns from spin_until_future_complete(), and subsequent commands (e.g., resetting to default) are not executed.
In the first terminal, when launching:

ros2 launch lbr_bringup gazebo.launch.py \
    ctrl:=joint_trajectory_controller \
    model:=iiwa14

This is what i get in the end:

[ruby $(which ign) gazebo-2] [INFO] [1743072843.525970997] [lbr.joint_trajectory_controller]: Received new action goal
[ruby $(which ign) gazebo-2] [INFO] [1743072843.526142370] [lbr.joint_trajectory_controller]: Accepted new action goal
[ruby $(which ign) gazebo-2] [INFO] [1743072860.413727070] [lbr.joint_trajectory_controller]: Goal reached, success!

In the second terminal, when launching:
ros2 run lbr_demos_cpp joint_trajectory_client --ros-args -r __ns:=/lbr

I've got:

[INFO] [1743072843.206167323] [lbr.joint_trajectory_client]: Waiting for action server to become available...
[INFO] [1743072843.524862162] [lbr.joint_trajectory_client]: Action server available.
[INFO] [1743072843.524973612] [lbr.joint_trajectory_client]: Rotating odd joints.
[INFO] [1743072843.526586564] [lbr.joint_trajectory_client]: Goal was accepted by server.

Observations:

• The first command (e.g., "Rotating odd joints") is accepted and executed.
• The node blocks waiting for a result; no result is published on the topic /joint_trajectory_controller/follow_joint_trajectory/result.
• Attempting to list controllers or contact /controller_manager/list_controllers fails, indicating that the controller manager is unresponsive.
• The log "Moving to zero position" is never shown

Thank you for your help and support!

[mhubii]

thanks for sharing. Can confirm this occurs occasionally. This can e.g. be reproduced via:

1. Launch Gazebo

   ros2 launch lbr_bringup gazebo.launch.py

2. Run client

   ros2 run lbr_demos_cpp joint_trajectory_client --ros-args -r __ns:=/lbr

3. Pause Gazebo for a couple of seconds:
   

The potential cause is the timeout in spin_until_future_complete:

lbr_fri_ros2_stack/lbr_demos/lbr_demos_cpp/src/joint_trajectory_client.cpp

Line 66 in e16d118

std::chrono::seconds(sec_from_start + goal_sec_tolerance));

Which exits early and then possibly gets stuck below waiting for a result that is never received because nothing spins the node anymore

lbr_fri_ros2_stack/lbr_demos/lbr_demos_cpp/src/joint_trajectory_client.cpp

Line 67 in e16d118

if (result_future.get().result->error_code !=

[mhubii]

The easiest fix is to just remove the timeout:

rclcpp::spin_until_future_complete(this->get_node_base_interface(), result_future,
                                   std::chrono::seconds(sec_from_start + goal_sec_tolerance));

to

rclcpp::spin_until_future_complete(this->get_node_base_interface(), result_future);

This turns execute into a blocking function

lbr_fri_ros2_stack/lbr_demos/lbr_demos_cpp/src/joint_trajectory_client.cpp

Line 85 in e16d118

joint_trajectory_client->execute({

One would also have to handle SIGINT. Because currently the process just gets stuck.

Other fixes would include adding a simple bool somewhere to execute, a la

while (!ready) {
    // sleep
    if (!rclcpp::ok()) return false;
}

[Khouloud-Hamrouni]

thank you for your response, I will try to fix it and keep you updated

[Khouloud-Hamrouni]

I tried to fix the problem by handling SIGINT:
I replaced:

  // wait for result
rclcpp::spin_until_future_complete(this->get_node_base_interface(), result_future,
                                   std::chrono::seconds(sec_from_start + goal_sec_tolerance));

in line 85
by:

// wait for result with SIGINT handling
    auto result_future = joint_trajectory_action_client_->async_get_result(goal_handle);
    bool ready = false;
    while(!ready){
      if(!rclcpp::ok()){ //handle ctrl+c
        RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for result.");
        return;
      }
      auto future_status = rclcpp::spin_until_future_complete(this->get_node_base_interface(), result_future,std::chrono::seconds(sec_from_start + goal_sec_tolerance));
      if (future_status == rclcpp::FutureReturnCode::SUCCESS) {
        ready = true;
      } else if (future_status == rclcpp::FutureReturnCode::TIMEOUT) {
        // Keep waiting
        continue;
      } else {
        RCLCPP_ERROR(this->get_logger(), "Error while waiting for result.");
        return;
      }
    }

And it works correctly.
If you're okay with the solution, I can open a PR and update it.
Thanks for your help tho!

[mhubii]

thanks for the suggestion, maybe easier to just remove the timeout all together?

[mhubii]

the sigint handling requires something like

#include <csignal>
std::signal(SIGINT, handler);

where handler shuts down

[Khouloud-Hamrouni]

Yeah you're right. I think we can simply remove timeout as you first suggested!