ros2/moveit2 报错IKConstraintSampler received dirty robot state

问题

当以ros-humle-moveit应用双臂规划时，给定两个笛卡尔目标规划运行如：

move_group.setPoseTarget(another_pose_L, "L_panda_link8");
move_group.setPoseTarget(another_pose_R, "R_panda_link8");
bool success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);

会大量重复报错,如：

[ERROR] [1646681894.535920515, 851.070000000]: IKConstraintSampler received dirty robot state, but valid transforms are required. Failing.

解决

首先我的报错为ros-humle-moveit(2.5.5版本)，再此之前的moveit2可能都有此问题。幸运的是此问题已在2.6.0及以后版本修复，所以可以对比ros-jazzy-moveit(2.10.0版本)发现UnionConstraintSampler::sample的实现有变化。其他版本同理，故针对我的版本提取cpp所在的moveit_core：

cd /your/ws/path/src
git clone https://github.com/moveit/moveit2.git -b 2.5.5 
mv moveit2/moveit_core moveit_core
rm -rf moveit2
cd ..

修改moveit2/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp中的UnionConstraintSampler::sample的实现为以下，其他保持不变

bool UnionConstraintSampler::sample(moveit::core::RobotState& state, const moveit::core::RobotState& reference_state,
                                    unsigned int max_attempts)
{
  state = reference_state;
  for (ConstraintSamplerPtr& sampler : samplers_)
  {
    // ConstraintSampler::sample returns states with dirty link transforms (because it only writes values)
    // but requires a state with clean link transforms as input. This means that we need to clean the link
    // transforms between calls to ConstraintSampler::sample.
    state.updateLinkTransforms();
    if (!sampler->sample(state, max_attempts))
      return false;
  }
  return true;
}

最后重新编译记得再. install/setup.bash一下运行即可。