使用moveit2的getCurrentPose()接口函数报错

[1809709119]

首先是提示信息:
[INFO] [1753014822.443208735] [move_group_interface]: Ready to take commands for planning group left_hand.
Planning frame: base_link
[INFO] [1753014822.444405420] [moveit_ros.current_state_monitor]: Listening to joint states on topic 'joint_states'
[INFO] [1753014823.444763207] [moveit_ros.current_state_monitor]: Didn't receive robot state (joint angles) with recent timestamp within 1.000000 seconds. Requested time 1753014822.444444, but latest received state has time 0.000000.
Check clock synchronization if your are running ROS across multiple machines!
[ERROR] [1753014823.444899784] [move_group_interface]: Failed to fetch current robot state

根据官方文档一直查到提示信息在waitForCurrentState()的函数中被打印，函数主要逻辑是使用while阻塞等待回调执行，直到接收到数据的时间戳大于指定的目标时间，但是为了避免一直阻塞，函数会在每次循环求出与初始时间的间隔，然后做一个超时判断，可以看到提示信息第三个浮点数，含义是接收到数据的时间戳，但是它为0，这说明在对象初始化结束后，从未接收到过数据，回调函数没有被执行。检查回调绑定的函数startStateMonitor()，函数接受默认参数主题名joint_states，然后绑定回调然后打印监听中的信息。但是回调似乎不被执行。

已验证主题存在，并且数据正常发布，自已写一个订阅节点也能够正常输出得到的时间戳。

#include <memory>
#include <stdio.h>

#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit_msgs/msg/move_it_error_codes.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>(
    "ib_moveit",
    rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)
  );

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

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

  std::string planning_frame = move_group_interface.getPlanningFrame();
  printf("Planning frame: %s\n", planning_frame.c_str());

  // Set a target Pose
  auto const target_pose = []{
    geometry_msgs::msg::Pose msg;
    msg.orientation.x = 0.00503518;
    msg.orientation.y = 0.707089;
    msg.orientation.z = 0.707089;
    msg.orientation.w = 0.00503523;
    msg.position.x = 0.22454;
    msg.position.y = 0.424263;
    msg.position.z = 0.3421;
    return msg;
  }();
  move_group_interface.setPoseTarget(target_pose);

  //move_group_interface.setRandomTarget();

  auto target_pose_ = move_group_interface.getCurrentPose();

  printf("Target Position: [x=%.3f, y=%.3f, z=%.3f]\n", 
             target_pose_.pose.position.x,
             target_pose_.pose.position.y,
             target_pose_.pose.position.z);

  // Create a plan to that target pose
  auto const [success, plan] = [&move_group_interface]{
    moveit::planning_interface::MoveGroupInterface::Plan msg;
    moveit::core::MoveItErrorCode error_code = move_group_interface.plan(msg);
    if (error_code == moveit::core::MoveItErrorCode::SUCCESS) {
        printf("Planning succeeded\n");
    } else {
      // 输出详细错误信息
      printf("Planning failed with error code: %d\n", error_code.val);
      
      // 检查特定错误类型
      if (error_code == moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION) {
          printf("No IK solution found for target pose!\n");
      } else if (error_code == moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED) {
          printf("General planning failure. Check constraints or environment.\n");
      }
    }
    auto const ok = static_cast<bool>(error_code);
    return std::make_pair(ok, msg);
  }();

  // Execute the plan
  if(success) {
    move_group_interface.execute(plan);
  } else {
    RCLCPP_ERROR(logger, "Planning failed!");
  }

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

[1809709119]

家人们，修好了，只需要在主函数开个线程去spin就可以了。

ccc，现在我宣布个事，我是个sb。

  rclcpp::executors::MultiThreadedExecutor executor;
  executor.add_node(node);
  std::thread spin_thread([&]() {
      executor.spin();
  });

好了，此贴终结。

[1809709119]

好像是因为qos可靠性不一致的过，如何使他们保持一致呢？

redtem@LAPTOP-SNFSRH5A:~/downloads$ ros2 topic info /joint_states -v
Type: sensor_msgs/msg/JointState

Publisher count: 2

Node name: joint_state_publisher
Node namespace: /
Topic type: sensor_msgs/msg/JointState
Endpoint type: PUBLISHER
GID: 01.0f.40.e9.6e.1c.8a.c1.00.00.00.00.00.00.12.03.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RELIABLE
  History (Depth): UNKNOWN
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

Node name: joint_state_publisher
Node namespace: /
Topic type: sensor_msgs/msg/JointState
Endpoint type: PUBLISHER
GID: 01.0f.40.e9.84.fe.50.bb.00.00.00.00.00.00.12.03.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RELIABLE
  History (Depth): UNKNOWN
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

Subscription count: 2

Node name: robot_state_publisher
Node namespace: /
Topic type: sensor_msgs/msg/JointState
Endpoint type: SUBSCRIPTION
GID: 01.0f.40.e9.78.fe.0f.a5.00.00.00.00.00.00.15.04.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: BEST_EFFORT
  History (Depth): UNKNOWN
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

Node name: move_group_private_103588141987792
Node namespace: /
Topic type: sensor_msgs/msg/JointState
Endpoint type: SUBSCRIPTION
GID: 01.0f.40.e9.7a.fe.37.7f.00.00.00.00.00.00.27.04.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: BEST_EFFORT
  History (Depth): UNKNOWN
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

[1809709119]

不是qos的原因

[1809709119]

萌新已经蒙b了，来个大佬救一下

我把moveit代码上加了更多的调试信息，并且在执行获取state时，不判断超时，重新编译，重新运行。不执行回调，是因为什么的原因啊，难道是创建订阅函数的原因？

void CurrentStateMonitor::startStateMonitor(const std::string& joint_states_topic)
{
  RCLCPP_INFO(LOGGER, "startStateMonitor start!!!\n");
  if (!state_monitor_started_ && robot_model_)
  {
    joint_time_.clear();
    if (joint_states_topic.empty())
    {
      RCLCPP_ERROR(LOGGER, "The joint states topic cannot be an empty string");
    }
    else
    {
      RCLCPP_INFO(LOGGER, "startStateMonitor start bind cb!!!\n");
      middleware_handle_->createJointStateSubscription(
          joint_states_topic, [this](const sensor_msgs::msg::JointState::ConstSharedPtr& joint_state) {
            RCLCPP_INFO(LOGGER, "will cb!!!\n");
            return jointStateCallback(joint_state);
          });
    }
    if (tf_buffer_ && !robot_model_->getMultiDOFJointModels().empty())
    {
      tf_buffer_->setUsingDedicatedThread(true);
      middleware_handle_->createDynamicTfSubscription([this](const auto& msg) { return transformCallback(msg, false); });
      middleware_handle_->createStaticTfSubscription([this](const auto& msg) { return transformCallback(msg, true); });
    }
    state_monitor_started_ = true;
    monitor_start_time_ = middleware_handle_->now();
    RCLCPP_INFO(LOGGER, "Listening to joint states on topic '%s'", joint_states_topic.c_str());
  }
}

[INFO] [1753077504.501639652] [moveit_ros.current_state_monitor]: startStateMonitor start!!!

[INFO] [1753077504.501667601] [moveit_ros.current_state_monitor]: startStateMonitor start bind cb!!!

[INFO] [1753077504.503177932] [moveit_ros.current_state_monitor]: Listening to joint states on topic 'joint_states'
[INFO] [1753077504.503223672] [moveit_ros.current_state_monitor]: waitForCurrentState start!!!

[1809709119]

家人们，修好了，只需要在主函数开个线程去spin就可以了。

ccc，现在我宣布个事，我是个sb。

  rclcpp::executors::MultiThreadedExecutor executor;
  executor.add_node(node);
  std::thread spin_thread([&]() {
      executor.spin();
  });

好了，此贴终结。