TF2
For TF2, there is no need to support multiple data types since TF2’s
API exclusively uses geometry_msgs::msg::TransformStamped. On the
Slicer side, the classes vtkMRMLROS2Tf2BroadcasterNode and
vtkMRMLROS2Tf2LookupNode support both vtkMatrix4x4 and
vtkMRMLTransformNode.
TF2 lookups use a TF2 buffer to store all the TF2 messages
(broadcasts) sent by all the ROS nodes. For the SlicerROS2 module, we
decided to add a TF2 buffer as a private data member of the
vtkMRMLROS2NodeNode since most users will never need a direct
access to the TF2 buffer. The TF2 lookups are performed when the node
node is spun.
Broadcasts
To create a new TF2 broadcaster, one should use the MRML ROS 2 node
method vtkMRMLROS2NodeNode::CreateAndAddTf2BroadcasterNode. This
method takes two parameters:
the parent ID (
std::string)the child ID (
std::string)
Broadcasters are triggered by calling the Broadcast method. It is
also possible to set the TF2 broadcast as an observer for an existing
vtkMRMLTransformNode using the method ObserveTransformNode.
The broadcast will then automatically occur when the observed transform
node is modified.
rosLogic = slicer.util.getModuleLogic('ROS2')
rosNode = rosLogic.GetDefaultROS2Node()
broadcaster = rosNode.CreateAndAddTf2BroadcasterNode('Parent', 'Child')
# Broadcast a 4x4 matrix
broadcastedMat = vtk.vtkMatrix4x4()
broadcastedMat.SetElement(0, 3, 66.0) # Set a default value
broadcaster.Broadcast(broadcastedMat)
auto broadcaster = rosNode.CreateAndAddTf2BroadcasterNode("Parent", "Child");
# Broadcast a 4x4 matrix
vtkSmartPointer<vtkMatrix4x4> broadcastedMat = vtkMatrix4x4::New();
broadcastedMat->SetElement(0, 3, 66.0);
broadcaster->Broadcast(broadcastedMat);
To remove the broadcaster node, use the method
vtkMRMLROS2NodeNode::RemoveAndDeleteTf2BroadcasterNode. This
method takes two parameters:
the parent ID (
std::string)the child ID (
std::string)
Lookups
To create a new TF2 lookup, one should use the MRML ROS 2 node method
vtkMRMLROS2NodeNode::CreateAndAddTf2LookupNode. This method takes
two parameters:
the parent ID (
std::string)the child ID (
std::string)
The class vtkMRMLROS2Tf2LookupNode is derived from
vtkMRMLTransformNode so it can be used as any other transformation
in the MRML scene.
Lookup nodes get updated when the ROS 2 node is spun. Users can set
their own callback to act on updated transformations using an observer
on the MRML ROS subscriber node. The last transformation received can
be retrieved using GetMatrixTransformToParent.
rosLogic = slicer.util.getModuleLogic('ROS2')
rosNode = rosLogic.GetDefaultROS2Node()
# Note that this next line will give you a repeated error if there is nothing broadcasting to Parent -> Child
lookupNode = rosNode.CreateAndAddTf2LookupNode('Parent', 'Child')
# get the transform "manually"
lookupMat = lookupNode.GetMatrixTransformToParent()
# or use an observer
observerId = lookupNode.AddObserver(slicer.vtkMRMLTransformNode.TransformModifiedEvent, observer.Callback)
auto lookup = rosNode.CreateAndAddTf2LookupNode("Parent", "Child");
# Broadcast a 4x4 matrix
vtkSmartPointer<vtkMatrix4x4> lookupMat = vtkMatrix4x4::New();
lookupMat->GetMatrixTransformToParent(lookupMat);
To remove the lookup node, the method
vtkMRMLROS2NodeNode::RemoveAndDeleteTf2LookupNode. This method
takes two parameters:
the parent ID (
std::string)the child ID (
std::string)