Remap joint_states on C++ (Failed to fetch current robot state)

[Mat198]

Description

How can I remap the joint_states topic to use move group interface getCurrentState() on ROS2 C++?

It's asked here without answer for C++.

Environment

• ROS Distro: Humble
• OS Version: Ubuntu 22.04
• Moveit Source Build - main

Code

 // ROS2 client library
#include <rclcpp/rclcpp.hpp>

// ROS2 msg includes
#include <moveit/move_group_interface/move_group_interface.h>

using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface;

int main(int argc, char **argv){

    rclcpp::init(argc,argv);
    rclcpp::NodeOptions node_options;
    node_options.automatically_declare_parameters_from_overrides(true);
    auto move_group_node = rclcpp::Node::make_shared("move_group_interface_tutorial", node_options);

    // We spin up a SingleThreadedExecutor for the current state monitor to get information
    // about the robot's state.
    rclcpp::executors::SingleThreadedExecutor executor;
    executor.add_node(move_group_node);
    std::thread([&executor]() { executor.spin(); }).detach();

    RCLCPP_INFO(move_group_node->get_logger(), "Moveit node started!");

    auto move_group_interface = MoveGroupInterface(move_group_node, "robot_arm");

    RCLCPP_INFO(move_group_node->get_logger(), "Move Group interface created!");

    move_group_interface.getCurrentJointValues();

    while (rclcpp::ok()){
        /* code */
    }

    rclcpp::shutdown();
    return 0;
}

Expected behaviour

This code should be able to get the current Joint Values.

Backtrace or Console output

[INFO] [moveit_ros.current_state_monitor]: Listening to joint states on topic 'joint_states' [INFO] [moveit_ros.current_state_monitor]: Didn't received robot state (joint angles) with recent timestamp within 1.000000 seconds. Check clock synchronization if your are running ROS across multiple machines! [ERROR] [move_group_interface]: Failed to fetch current robot state

[navdotnetreqs]

Hi! I am stuck in the same position. I can run gazebo nicely for my arm, and Moveit+rviz works nicely. I can move the arm from rviz and from c++ (setPoseTarget), but when I try to get the position it always fails with:

[moveit_ros.current_state_monitor]: Didn't receive robot state (joint angles) with recent timestamp within 1.000000 seconds. Requested time 1724488072.980249, but latest received state has time 0.000000.

Spent a whole day looking for solutions, made sure to have use_sim_time:=true everywhere, but still no luck.

• Since Moveit (gui) and Gazebo talk nicely to one another, the model should be fine, and the issue must be somewhere in the c++ code.. right?

The code is very concise; am I missing something obvious?

#include <memory>

#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>

int main(int argc, char * argv[])
{
  // Initialize ROS and create the Node
  rclcpp::init(argc, argv);
  auto const node = std::make_shared<rclcpp::Node>(
    "hello_moveit",
    rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)
  );

  // Create a ROS logger
  auto const logger = rclcpp::get_logger("hello_moveit");

  // Create the MoveIt MoveGroup Interface
  using moveit::planning_interface::MoveGroupInterface;
  auto move_group_interface = MoveGroupInterface(node, "arm_manipulator");

  RCLCPP_INFO(logger, "Reference frame: %s", move_group_interface.getEndEffectorLink().c_str());

  std::vector<double> joints;
  joints = move_group_interface.getCurrentJointValues();

  // Shutdown ROS
  rclcpp::shutdown();
  return 0;
}

[navdotnetreqs]

For anyone else wandering here, I figured out based on the error message that the state request is sent in real time, but the previous states are in simulated time. By testing the different nodes with (ros2 param get /nodename use_sim_time), I found out that my hello_moveit did not get this parameter, even if I gave it as a command line option.

Creating the node by

      // Create NodeOptions object
    rclcpp::NodeOptions node_options;
    node_options.automatically_declare_parameters_from_overrides(true);

    // Set use_sim_time to true using Parameter
    node_options.parameter_overrides(
        {rclcpp::Parameter("use_sim_time", true)}
    );

    // Create the node with the options
    auto node = rclcpp::Node::make_shared("hello_moveit", node_options);

partially solved this issue. Sometimes I got the states, however most of the time it still failed.

The example here works for getting the pose https://robotics.stackexchange.com/questions/111430/failed-to-fetch-current-robot-state-when-using-mgi-in-wall-timer-callback-functi

not sure why..