问题描述:
在使用Moveit获取机械臂关节角度时,有时会遇到如下错误信息:
原因分析:
出现这一错误的原因主要在于Moveit的状态监视器在处理回调函数 planning_scene_monitor::CurrentStateMonitor::jointStateCallback
中传入的联合状态时存在问题。
planning_scene_monitor::CurrentStateMonitor
负责监控机器人当前的状态,并通过回调函数 jointStateCallback
获取并更新机器人各个关节的状态。这个回调函数会处理从ROS话题 /joint_states
中接收到的联合状态消息。每当收到新消息时,jointStateCallback
会解析消息内容并更新内部的机器人状态表示。
在某些情况下,如果这个回调函数未能及时处理消息,或消息的处理是同步进行的,那么在执行 waitForCurrentRobotState
方法时就会出现问题。具体表现为,waitForCurrentRobotState
方法会等待当前状态更新到允许的最大时间(默认是1秒)。如果在这个时间内未能获取到最新的联合状态,方法就会超时从而打印出“Failed to fetch current robot state“。
解决办法:
要解决这个问题,我们需要确保在功能函数中对ROS消息回调进行异步处理。以下是具体的解决步骤:
-
初始化节点和MoveIt相关组件: 在ROS中,确保你已经正确初始化了节点和MoveIt相关的组件,如
move_group
等。 -
启用异步Spinner: 使用异步spinner来处理ROS消息回调。可以在初始化代码中添加如下代码:
#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
int main(int argc, char** argv) {
ros::init(argc, argv, "moveit_async_example");
ros::NodeHandle nh;
// 创建异步spinner
ros::AsyncSpinner spinner(1); // 使用1个线程
spinner.start();
// 初始化MoveIt!接口
moveit::planning_interface::MoveGroupInterface move_group("arm");
// 其他初始化代码...
// 主循环
ros::waitForShutdown();
return 0;
}