Possible issue with KDL kinematics plugin in getPositionFK

edit

Dear ROS users,

for personal research, I was investigating how KDL is provided through ROS plugins, and I noticed this code in getPositionFK (it is the current status of the master branch as well as melodic-devel):

bool valid = true;
for (unsigned int i = 0; i < poses.size(); i++)
{
  if (fk_solver_->JntToCart(jnt_pos_in, p_out) >= 0)
  {
    poses[i] = tf2::toMsg(p_out);
  }
  else
  {
    ROS_ERROR_NAMED("kdl", "Could not compute FK for %s", link_names[i].c_str());
    valid = false;
  }
}
return valid;

It seems to me that, at each iteration, poses[i] always takes the same value, regardless of i (regardless of the specific links in the link_names parameter). I think that the purpose of this for loop is to give each pose[i] the pose of the link we are computing FK for. I think that the code should be something like this:

bool valid = true;

for (unsigned int i = 0; i < poses.size(); i++)
{
    KDL::Frame p_out;

    if (fk_solver_->JntToCart(jnt_pos_in, p_out, link_indices[i]+1) >= 0)
    {
        poses[i] = tf2::toMsg(p_out);
    }
    else
    {
        ROS_ERROR_NAMED("nsp", "Could not compute FK for %s", link_names[i].c_str());
        valid = false;
    }
}

return valid;

I tested this piece of code by providing link_names = getLinkNames() (all links) and the output parameter poses only contains one pose repeated link_names.size() times.

Is this a bug, or is there something else I'm not able to get?

UPDATE: I opened an issue here