Closed traversaro closed 1 year ago
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);
}
}
}
(I moved the issue to the idyntree-yarp-tools repo from the idyntree repo).
I think this is already implemented, isn't it? https://github.com/robotology/idyntree-yarp-tools/blob/759b69b4e023f11ac1c261d4f71813b929f4b549/src/modules/yarprobotstatepublisher/src/robotstatepublisher.cpp#L395-L484
Right, this was fixed by https://github.com/robotology/idyntree/pull/724 .
As correctly observer by @valegagge , it turns out that
yarprobotstatepublisher
behaves quite differently from the ROS'sRobotStatePublisher
:yarprobotstatepublisher
always publishes thebaseLinkOfTheModel
<-->frame
transform intf
(for each frame in the model): https://github.com/robotology/idyntree/blob/master/src/tools/yarprobotstatepublisher/src/robotstatepublisher.cpp#L232RobotStatePublisher
publishes theparentLink
<-->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 fixedparentLink
<-->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.