Inconsistent Robot State Fetching with MoveIt (Failed to fetch current robot state)

[OneOneLiu]

Description

I'm experiencing inconsistent behavior when attempting to fetch the current robot state using MoveIt in ROS2 Humble on Ubuntu 22.04. Specifically, the robot state fetching succeeded in the main function but failed with timestamp errors when called in a service class in the same program, despite the received time being valid (within the 1-second window).

Your environment

• ROS Distro: Humble
• OS Version: Ubuntu 22.04 in Docker
• Moveit2 build from source
• commit tag: faf42b0

Issue Details

I have a class RobotMover that uses MoveIt's move_group_interface_.getCurrentPose().pose to print the current robot pose (sourcecpp file is provided below). I'm calling the printCurrentPose() method in three distinct contexts in the same program:

1. Immediately after node startup ("Public context"): Called inside the constructor of a RobotMoverService class, just after setting up the service.
2. Within the main function ("Main context"): Called directly in the main function after creating the RobotMover and RobotMoverService instances.
3. Inside the service callback ("Private context"): Called when the service is invoked via ros2 service call.

Each call yields different results, with two of the three calls failing to fetch the current robot state.

1. Public Context (Failed, and the received time is 0.000):

[INFO] [1725486364.690316869] [robot_control]: About to call printCurrentPose in Public
[INFO] [1725486364.691598526] [moveit_ros.current_state_monitor]: Listening to joint states on topic 'joint_states'
[INFO] [1725486365.691687685] [moveit_ros.current_state_monitor]: Didn't receive robot state (joint angles) with recent timestamp within 1.000000 seconds. Requested time 1725486364.691609, but latest received state has time 0.000000.
Check clock synchronization if your are running ROS across multiple machines!
[ERROR] [1725486365.691733302] [move_group_interface]: Failed to fetch current robot state
Current Pose:
Position: (0, 0, 0)
Orientation: (0, 0, 0, 1)

2. Main Context (direct call from main, successful):

[INFO] [1725486365.691829284] [robot_control]: About to call printCurrentPose in Main
[WARN] [1725486365.692188894] [moveit_ros.current_state_monitor]: Unable to update multi-DOF joint 'virtual_joint':Failure to lookup transform between 'world'and 'panda_link0' with TF exception: "world" passed to lookupTransform argument target_frame does not exist.
Current Pose:
Position: (0.30702, -5.22133e-12, 0.59027)
Orientation: (0.923956, -0.382499, 1.32493e-12, 3.20041e-12)

3. Private Context (service callback, failed but the received time seems correct):

[INFO] [1725486460.998312142] [robot_control]: Service Callback: About to call printCurrentPose in Private
[INFO] [1725486461.998413735] [moveit_ros.current_state_monitor]: Didn't receive robot state (joint angles) with recent timestamp within 1.000000 seconds. Requested time 1725486460.998349, but latest received state has time 1725486460.997929.
Check clock synchronization if your are running ROS across multiple machines!
[ERROR] [1725486461.998462017] [move_group_interface]: Failed to fetch current robot state
Current Pose:
Position: (0, 0, 0)
Orientation: (0, 0, 0, 1)

The third one is most confusing to me, it said it didn't receive the robot state within 1 second, but the requested time and received time difference is obviously less than 1 sec.

I have checked existing issues and find some might be relevant, moveit/moveit2#775, #587, moveit/moveit2#496, the most relevant one is this issue moveit/moveit2#1399, as the author mentioned:

The problem also only occurs when I call getCurrentState() in my service function, in main it works without problems

Question:

Does anyone know how to resolve this issue, or am I doing something wrong in my implementation?

Thanks in advance!

Steps to reproduce

1. clone the repo to ~/humble_ws/src/ and build the image

mkdir -p ~/humble_ws/src
cd ~/humble_ws/src
git clone https://github.com/OneOneLiu/test_joint_state_fetch.git
cd test_joint_state_fetch/docker
bash build.bash
bash start_container.bash

3. Launch moveit2_tutorial/demo.launch in the container

ros2 launch moveit2_tutorials demo.launch.py

4. In a new terminal, run the test node

ros2 run test_joint_state_fetch test_joint_state

5. In a new terminal, call the /print_current_poseservice

ros2 service call /print_current_pose test_joint_state_fetch/srv/PrintPose

Expected behaviour

All the three calls can fetch and print the current robot states.

Console output

root@sn4622111301:/# ros2 run test_joint_state_fetch test_joint_state
[INFO] [1725486364.674011145] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 1.02956 seconds
[INFO] [1725486364.674071720] [moveit_robot_model.robot_model]: Loading robot model 'panda'...
[WARN] [1725486364.681500165] [moveit_ros.robot_model_loader]: No kinematics plugins defined. Fill and load kinematics.yaml!
[INFO] [1725486364.689950876] [move_group_interface]: Ready to take commands for planning group panda_arm.
[INFO] [1725486364.690316869] [robot_control]: About to call printCurrentPose in Public
[INFO] [1725486364.691598526] [moveit_ros.current_state_monitor]: Listening to joint states on topic 'joint_states'
[INFO] [1725486365.691687685] [moveit_ros.current_state_monitor]: Didn't receive robot state (joint angles) with recent timestamp within 1.000000 seconds. Requested time 1725486364.691609, but latest received state has time 0.000000.
Check clock synchronization if your are running ROS across multiple machines!
[ERROR] [1725486365.691733302] [move_group_interface]: Failed to fetch current robot state
Current Pose:
Position: (0, 0, 0)
Orientation: (0, 0, 0, 1)
[INFO] [1725486365.691829284] [robot_control]: About to call printCurrentPose in Main
[WARN] [1725486365.692188894] [moveit_ros.current_state_monitor]: Unable to update multi-DOF joint 'virtual_joint':Failure to lookup transform between 'world'and 'panda_link0' with TF exception: "world" passed to lookupTransform argument target_frame does not exist.
Current Pose:
Position: (0.30702, -5.22133e-12, 0.59027)
Orientation: (0.923956, -0.382499, 1.32493e-12, 3.20041e-12)
[INFO] [1725486460.998312142] [robot_control]: Service Callback: About to call printCurrentPose in Private
[INFO] [1725486461.998413735] [moveit_ros.current_state_monitor]: Didn't receive robot state (joint angles) with recent timestamp within 1.000000 seconds. Requested time 1725486460.998349, but latest received state has time 1725486460.997929.
Check clock synchronization if your are running ROS across multiple machines!
[ERROR] [1725486461.998462017] [move_group_interface]: Failed to fetch current robot state
Current Pose:
Position: (0, 0, 0)
Orientation: (0, 0, 0, 1)

Source file

#include <rclcpp/rclcpp.hpp>
#include "test_joint_state_fetch/srv/print_pose.hpp"  // Header file for the custom service
#include <moveit/move_group_interface/move_group_interface.h>
#include <iostream>

// Definition of the RobotMover class
class RobotMover {
public:
  RobotMover(const rclcpp::Node::SharedPtr& node)
  : move_group_interface_(node, "panda_arm"), node_(node) {
    // Intentionally left empty
  }

  void printCurrentPose() {
    auto current_pose = move_group_interface_.getCurrentPose().pose;
    std::cout << "Current Pose:" << std::endl;
    std::cout << "Position: (" << current_pose.position.x << ", "
              << current_pose.position.y << ", "
              << current_pose.position.z << ")" << std::endl;
    std::cout << "Orientation: (" << current_pose.orientation.x << ", "
              << current_pose.orientation.y << ", "
              << current_pose.orientation.z << ", "
              << current_pose.orientation.w << ")" << std::endl;
  }

private:
  moveit::planning_interface::MoveGroupInterface move_group_interface_;
  rclcpp::Node::SharedPtr node_;
};

// Definition of the RobotMoverService class
class RobotMoverService {
public:
  RobotMoverService(const rclcpp::Node::SharedPtr& node, RobotMover& robot_mover)
    : node_(node), robot_mover_(robot_mover) {
    // Create a service to print the current pose
    service_ = node_->create_service<test_joint_state_fetch::srv::PrintPose>(
      "print_current_pose", std::bind(&RobotMoverService::handlePrintRequest, this, std::placeholders::_1, std::placeholders::_2)
    );
    // Call in public context
    RCLCPP_INFO(node_->get_logger(), "About to call printCurrentPose in Public");
    robot_mover_.printCurrentPose();
  }

private:
    void handlePrintRequest(const std::shared_ptr<test_joint_state_fetch::srv::PrintPose::Request> /*request*/,
                            std::shared_ptr<test_joint_state_fetch::srv::PrintPose::Response> response) {
        // Call in service callback (private context)
        RCLCPP_INFO(node_->get_logger(), "Service Callback: About to call printCurrentPose in Private");
        robot_mover_.printCurrentPose();
        
        // Set response to success
        response->success = true;
    }
  rclcpp::Node::SharedPtr node_;
  rclcpp::Service<test_joint_state_fetch::srv::PrintPose>::SharedPtr service_;
  RobotMover& robot_mover_;
};

// Main function
int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  rclcpp::executors::SingleThreadedExecutor executor;
  auto node = rclcpp::Node::make_shared("robot_control");
  RobotMover robot_mover(node);
  RobotMoverService robot_mover_service(node, robot_mover);

  executor.add_node(node);
  std::thread executor_thread([&executor]() { executor.spin(); });
  // Call from main
  RCLCPP_INFO(node->get_logger(), "About to call printCurrentPose in Main");
  robot_mover.printCurrentPose();
  
  executor_thread.join();
  rclcpp::shutdown();
  return 0;
}

[OneOneLiu]

I managed to resolve the issue based on this comment. The solution involved setting up a separate executor within the class for spinning the MoveGroup interface, while keeping the main program's node running in a separate spin. This allowed me to fetch the robot's current state successfully from a service request.

However, I'm still somewhat confused about why this resolves the issue. I'll continue digging into the ROS 2 internals to better understand what's going on. In the meantime, here's the updated version of the code that works:

#include <rclcpp/rclcpp.hpp>
#include "test_joint_state_fetch/srv/print_pose.hpp"  // Header file for the custom service
#include <moveit/move_group_interface/move_group_interface.h>
#include <iostream>

// Combined RobotMover class, which includes service and pose printing functionality
class RobotMover : public rclcpp::Node {
public:
  // Constructor to initialize the node, MoveGroupInterface, and executor
  RobotMover(const rclcpp::NodeOptions &options)
  : rclcpp::Node("robot_control", options), // Initialize the node with the name "robot_control"
    node_(std::make_shared<rclcpp::Node>("move_group_interface")), // Create an additional ROS node
    move_group_interface_(node_, "panda_arm"), // Initialize MoveGroupInterface for controlling the arm
    executor_(std::make_shared<rclcpp::executors::SingleThreadedExecutor>()) // Create a single-threaded executor
  {
    // Create the service for printing the current pose
    service_ = this->create_service<test_joint_state_fetch::srv::PrintPose>(
      "print_current_pose", 
      std::bind(&RobotMover::handlePrintRequest, this, std::placeholders::_1, std::placeholders::_2)
    );

    // Add the node to the executor and start the executor thread
    executor_->add_node(node_);
    executor_thread_ = std::thread([this]() {
      RCLCPP_INFO(node_->get_logger(), "Starting executor thread"); // Log message indicating the thread start
      executor_->spin(); // Run the executor to process callbacks
    });
  }

  // Function to print the current end-effector pose
  void printCurrentPose() {
    auto current_pose = move_group_interface_.getCurrentPose().pose; // Get the current pose
    std::cout << "Current Pose:" << std::endl;
    std::cout << "Position: (" << current_pose.position.x << ", "
              << current_pose.position.y << ", "
              << current_pose.position.z << ")" << std::endl;
    std::cout << "Orientation: (" << current_pose.orientation.x << ", "
              << current_pose.orientation.y << ", "
              << current_pose.orientation.z << ", "
              << current_pose.orientation.w << ")" << std::endl;
  }

private:
  // Service callback function to handle pose printing requests
  void handlePrintRequest(const std::shared_ptr<test_joint_state_fetch::srv::PrintPose::Request> /*request*/,
                          std::shared_ptr<test_joint_state_fetch::srv::PrintPose::Response> response) {
    // Print the current pose in the service callback
    RCLCPP_INFO(node_->get_logger(), "Service Callback: About to call printCurrentPose in Private");
    printCurrentPose(); // Print the robot's current pose

    // Set the response to indicate success
    response->success = true;
  }

  // Member variables
  rclcpp::Node::SharedPtr node_; // Additional ROS node pointer
  moveit::planning_interface::MoveGroupInterface move_group_interface_;  // MoveIt interface for controlling the arm
  rclcpp::Service<test_joint_state_fetch::srv::PrintPose>::SharedPtr service_;  // Service pointer for pose requests
  std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> executor_;  // Single-threaded executor
  std::thread executor_thread_;  // Thread to run the executor
};

// Main function - Entry point of the program
int main(int argc, char** argv) {
  rclcpp::init(argc, argv); // Initialize ROS 2

  rclcpp::NodeOptions node_options;
  node_options.automatically_declare_parameters_from_overrides(true); // Allow automatic parameter declaration
  node_options.use_intra_process_comms(false); // Disable intra-process communication

  auto node = std::make_shared<RobotMover>(node_options); // Create the RobotMover object and start the node

  rclcpp::spin(node); // Spin the main thread to process callbacks

  rclcpp::shutdown(); // Shutdown the ROS 2 system
  return 0; // Exit the program
}

[CVF2]

Did you ever figure out what causes this issue? I am currently having the exact same issue.

[SteMuc]

I'm still having the same issue. I launched the ur_sim_moveit.launch.py with use_sim_time:=True, and try the test_fetch test, but I'm not able to set the movegroup planning interface, and get the CurrentState() as well. Is anyone has experienced the same issue?