MoveIt2 Didn't receive robot state (joint angles) with recent timestamp within 1.000000 seconds

Question

I'm trying to set up MoveitGroupInterface for robot manipulator.

Long story short, I'm not able to fetch current robot state.

I manage to initialize MoveGroupInterface and read robot description (so it seems), however, when calling following code snippet:

void m2Iface::getArmState() 
{
  currPose = m_moveGroupPtr->getCurrentPose(EE_LINK_NAME); 
  // current_state_monitor
  m_robotStatePtr = m_moveGroupPtr->getCurrentState();
}

As output of the following command I get:

Didn't receive robot state (joint angles) with recent timestamp within 1.000000 seconds. Requested time 3533.035000, but latest received state has time 0.000000.

I want to initialize moveit2_iface node with the MultiThreadExecutor as follows:

// Create node 
    rclcpp::Node::SharedPtr node = std::make_shared<m2Iface>(); 
    
    // Add multithreaded executor --> Check MoveIt states for multithreaded executor and relation with time 
    rclcpp::executors::MultiThreadedExecutor executor;
    executor.add_node(node); 
    executor.spin(); 

My m2Iface constructor is:

    node_ = std::make_shared<rclcpp::Node>(this->get_name(), 
                                               rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));

    // TODO: Load config path from param
    config = init_config("/root/ws_moveit2/src/arm_api2/config/franka_sim.yaml"); 
    RCLCPP_INFO_STREAM(this->get_logger(), "Loaded config!");

    // TODO: Add as reconfigurable param 
    std::chrono::duration<double> SYSTEM_DT(0.005);
    timer_ = this->create_wall_timer(SYSTEM_DT, std::bind(&m2Iface::run, this));


    PLANNING_GROUP      = config["robot"]["arm_name"].as<std::string>(); 
    EE_LINK_NAME        = config["robot"]["ee_link_name"].as<std::string>();
    ROBOT_DESC          = config["robot"]["robot_desc"].as<std::string>();  
    PLANNING_SCENE      = config["robot"]["planning_scene"].as<std::string>(); 
    MOVE_GROUP_NS       = config["robot"]["move_group_ns"].as<std::string>(); 
    
    ns_ = this->get_namespace();    
    init_publishers(); 
    init_subscribers(); 
    init_moveit(); 

    RCLCPP_INFO_STREAM(this->get_logger(), "Initialized node!"); 

My moveit init methods are:

void m2Iface::init_moveit()
{

    RCLCPP_INFO_STREAM(this->get_logger(), "robot_description: " << ROBOT_DESC); 
    RCLCPP_INFO_STREAM(this->get_logger(), "planning_group: " << PLANNING_GROUP);
    RCLCPP_INFO_STREAM(this->get_logger(), "move_group_ns: " << MOVE_GROUP_NS);  
    // MoveIt related things!
    moveGroupInit       = setMoveGroup(node_, PLANNING_GROUP, MOVE_GROUP_NS); 
    pSceneMonitorInit   = setPlanningSceneMonitor(node_, ROBOT_DESC);
    robotModelInit      = setRobotModel(node_);
}

and

bool m2Iface::setMoveGroup(rclcpp::Node::SharedPtr nodePtr, std::string groupName, std::string moveNs)
{
    // check if moveNs is empty
    if (moveNs == "null") moveNs=""; 

    // set mGroupIface 
    m_moveGroupPtr = new moveit::planning_interface::MoveGroupInterface(nodePtr, 
        moveit::planning_interface::MoveGroupInterface::Options(
            groupName,
            moveit::planning_interface::MoveGroupInterface::ROBOT_DESCRIPTION,
            moveNs));
    RCLCPP_INFO_STREAM(this->get_logger(), "Move group interface set up!"); 
    
    return true; 
}

ROS param use_sim_time is set to True for all running ROS nodes.

I'm thinking it could be related to following:

• Callback group
• Node initializing

But I'm not really able to figure it out.

env:

ROS 2 Humble 
Moveit2 build from source on humble branch 

Answer 1

I've managed to solve it.

It seems that I was wrongly initializing MoveGroupInterface class all along.

I've changed initialization to:

*.hpp:

class m2Iface: public rclcpp::Node
{
    public:
        m2Iface(const rclcpp::NodeOptions &options);  
        //~m2Iface();
        std::string ns_; 
    private: 
        rclcpp::Node::SharedPtr node_;
        rclcpp::Executor::SharedPtr executor_;
        std::thread executor_thread_;

*.cpp:

m2Iface::m2Iface(const rclcpp::NodeOptions &options)
    : Node("moveit2_iface", options), node_(std::make_shared<rclcpp::Node>("moveit2_iface_node")), 
     executor_(std::make_shared<rclcpp::executors::SingleThreadedExecutor>()) 

*_node.cpp:

    rclcpp::NodeOptions node_options;
    node_options.automatically_declare_parameters_from_overrides(true);
    // Create node 
    auto move_group_node = std::make_shared<m2Iface>(node_options); 
    rclcpp::spin(move_group_node);

Move group is initialized in *.cpp as:

 m_moveGroupPtr = std::make_shared<moveit::planning_interface::MoveGroupInterface>(nodePtr, 
        moveit::planning_interface::MoveGroupInterface::Options(
            groupName,
            "robot_description",
            moveNs));
    m_moveGroupPtr->setEndEffectorLink(EE_LINK_NAME); 
    m_moveGroupPtr->setPoseReferenceFrame(PLANNING_FRAME); 
    m_moveGroupPtr->startStateMonitor(); 
    executor_->add_node(node_); 
    executor_thread_ = std::thread([this]() {executor_->spin();});

This link helped me a lot.

Answer 2

There are multiple reasons that this could occur.

One reason is that you're running a (gazebo) simulation of the robot and the clocks are not synchronized. In this case, setting use_sim_time=True is the solution. Add the following to your launch description

def generate_launch_description():
    return launch.LaunchDescription([
        launch_ros.actions.SetParameter(name='use_sim_time', value=True),
        # 'use_sim_time' will be set on all nodes following the line above
        ...
    ])

Another reason could that moveit_ros.current_state_monitor comes up too early before joint_state_broadcaster. The solution (hack) is to simply wait a bit before doing any current state queries.

There could be many more but these are two off the top of my head.