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.