idyntree

idyntree copied to clipboard

As correctly observer by @valegagge , it turns out that yarprobotstatepublisher behaves quite differently from the ROS's RobotStatePublisher:

• the yarprobotstatepublisher always publishes the baseLinkOfTheModel <--> frame transform in tf (for each frame in the model): https://github.com/robotology/idyntree/blob/master/src/tools/yarprobotstatepublisher/src/robotstatepublisher.cpp#L232
• on the other hand, ROS's RobotStatePublisher publishes the parentLink <--> childLink tranform for each non-fixed joint in the model ( https://github.com/ros/robot_state_publisher/blob/melodic-devel/src/robot_state_publisher.cpp#L97 ) and then it separately streams the fixed parentLink <--> childLink tranforms for all the links that are connected by fixed joints (an https://github.com/ros/robot_state_publisher/blob/melodic-devel/src/robot_state_publisher.cpp#L120).

From of the point of view of tf functionalities, the two methods are functionally equivalent, but the ROS's method may be more useful when debugging the tf tree in rqt.

It probably make sense to switch between the existing behavior and the ROS's behaviour with a specific option.

We had a discussion on this issue today with @randaz81 and @elandini84 . I shared with them this pseudocode:

if (mode == SHALLOW) {
    // In shallow mode, we publish the position of each frame of the robot w.r.t. to the base frame of the robot 
    for (size_t frameIdx=0; frameIdx < sizeOfTFFrames; frameIdx++)
    {
        if(m_baseFrameIndex == frameIdx)    // skip self-tranform
            continue;

        iDynTree::Transform base_H_frame = m_kinDynComp.getRelativeTransform(m_baseFrameIndex, frameIdx);
        iDynTree::toYarp(base_H_frame.asHomogeneousTransform(), m_buf4x4);
        m_iframetrans->setTransform(m_tfPrefix + model.getFrameName(frameIdx),
                                    m_tfPrefix + model.getFrameName(m_baseFrameIndex),
                                    m_buf4x4);
    }
} else {
    // mode == DEEP
    // In deep mode, we need to distinguish the following cases:
    // For the frames that are frames of the link, we publish their location w.r.t. to their parent link
    // For the additional frames, we publish their location w.r.t. to the frame of the link to which they are 
    // attached (note that this transform are actually constant)
    
    // The traversal is the data structure that contains information on which link is parent of which other link,
    // as in iDynTree the model is an undirected data structure 
    iDynTree::Traversal traversal;
    
    // We generate a traversal using the base frame index 
    m_kinDynComp.model().ComputeFullTreeTraversal(traversal, m_baseFrameIndex);
    
    // Process links
    for (size_t linkIndex=0; linkIndex < model.getNrOfLinks(); linkIndex++)
    {
        if(m_baseFrameIndex == linkIndex)    // skip self-tranform
            continue;

        LinkIndex parentLinkIndex = traversal.getParentLinkFromLinkIndex(linkIndex)->getIndex();
        iDynTree::Transform parentLink_H_link = m_kinDynComp.getRelativeTransform(parentLinkIndex, linkIndex);
        iDynTree::toYarp(base_H_frame.asHomogeneousTransform(), m_buf4x4);
        m_iframetrans->setTransform(m_tfPrefix + model.getFrameName(linkIndex),
                                    m_tfPrefix + model.getFrameName(parentLinkIndex),
                                    m_buf4x4);
    }
    
    // Process frames, only if the reduced model option is not passed 
    if (!this->reducedModelOption) 
    {
        // Process additional frames (that have all indexes between model.getNrOfLinks()+1 and model.getNrOfFrames()
        for (size_t frameIndex=model.getNrOfLinks()+1; frameIndex < model.getNrOfFrames(); frameIndex++)
        {
                LinkIndex linkIndex = m_kinDynComp.model().getFrameLink(frameIndex);
                iDynTree::Transform link_H_frame = m_kinDynComp.model().getFrameTransform(frameIndex);
                iDynTree::toYarp(base_H_frame.asHomogeneousTransform(), m_buf4x4);
                m_iframetrans->setTransform(m_tfPrefix + model.getFrameName(linkIndex),
                                                  m_tfPrefix + model.getFrameName(frameIndex),
                                                  m_buf4x4);
        }
    }
}