(更正)ROS2_ERROR:Failed to fetch curre === 解决 ROS2 Moveit2 中 MoveGroupInterface::getCurrentState 错误问题

解决 ROS2 Moveit2 中 MoveGroupInterface::getCurrentState 错误问题

在使用 ROS2 和 Moveit2 进行机器人控制时，常见的任务之一是获取机器人当前的状态或位姿。然而，很多用户在使用 moveit::planning_interface::MoveGroupInterface::getCurrentState 或 getCurrentPose 函数时，遇到了一个常见的错误：

ROS2_ERROR: Failed to fetch current robot state

本文将详细介绍这个问题的根源，并提供有效的解决方法，确保 getCurrentState 和 getCurrentPose 能够正确返回机器人的实时状态。

问题的根源：回调函数的多线程问题

在 Moveit2 源码中，回调函数位于 src/moveit2/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp:322，它负责通过 joint_state 话题更新 current_state_time_。

在 Moveit2 中，getCurrentState() 或 getCurrentPose() 依赖于 joint_state 话题的回调函数 (jointStateCallback) 来更新机器人的当前状态。这一回调函数通过 current_state_time_ 记录当前关节状态的时间戳。然而，问题在于回调函数中的 current_state_time_ 已经显示更新，但一旦回调函数结束，回调函数外的状态仍然没有正确更新。

经过深入分析，问题的根源在于：回调函数被触发了，但由于线程阻塞或不同线程间的数据不同步，主线程中的 current_state_time_ 没有被更新。

解决方案

为了解决多线程回调问题，我们可以采取以下措施：

1. 使用多线程执行器：通过多线程执行器可以确保系统能够并行处理多个回调函数，避免回调函数的执行被阻塞或延迟。
2. 异步处理回调：确保在获取机器人状态之前，回调函数有足够的时间完成状态的更新。
3. 合理控制线程数量：根据系统的需求配置合理的线程数，以确保回调函数能够在适当的时机执行。

解决方案代码示例

#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <thread>
using moveit::planning_interface::MoveGroupInterface;
// 异步任务函数，接收一个 MoveGroupInterface 的 shared_ptr
void async_task(std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_interface_arm)
{    
    // 获取当前机器人状态
    auto current_state = move_group_interface_arm->getCurrentPose();
    std::cout<<"----------------"<<current_state.pose.position.x<<std::endl;
}
void exe(std::shared_ptr<rclcpp::Node> node1)
{
    auto move_group_interface_arm1 = std::make_shared<MoveGroupInterface>(node1, "arm1");
    std::thread(std::bind(async_task, move_group_interface_arm1)).detach();
}
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));
    // 创建一个多线程执行器，允许异步处理回调
    rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 24); // 使用 24 个线程
    executor.add_node(node);
    exe(node);
    executor.spin();
    rclcpp::shutdown();
    return 0;
}

！！！确保每个调用getCurrentPose和getCurrentState的节点都放入了多线程执行器

在类中使用多线程执行器

class GetRobotCurrentState
{
	public:
		GetRobotCurrentState(rclcpp::Node::SharedPtr pNH);
	private:
		rclcpp::Node::SharedPtr nh_;
		std::shared_ptr<rclcpp::executors::MultiThreadedExecutor> executor_;
		std::thread executor_thread_;  // 用于运行执行器的线程
	  	.
	  	.
	  	.
	  	.
	  	.
	  	.
}

//构造函数
GetRobotCurrentState::GetRobotCurrentState(rclcpp::Node::SharedPtr pNH) :   nh_(pNH),executor_(std::make_shared<rclcpp::executors::MultiThreadedExecutor>(rclcpp::ExecutorOptions(),16))                                                                   
{
		.
		.
		.
		.
		.
		.
	
	  // 将节点添加到执行器
    executor_->add_node(nh_);
    // 启动执行器线程
    executor_thread_ = std::thread([this]() {
        executor_->spin();
    });
}