diff --git a/MRML/vtkMRMLROS2NODENode.cxx b/MRML/vtkMRMLROS2NODENode.cxx index ac2a3b3..c4fe9fa 100644 --- a/MRML/vtkMRMLROS2NODENode.cxx +++ b/MRML/vtkMRMLROS2NODENode.cxx @@ -50,33 +50,32 @@ void vtkMRMLROS2NODENode::Create(const std::string & nodeName, bool initialize) mInternals->mNodePointer = std::make_shared(nodeName); } -vtkMRMLROS2SubscriberNode* vtkMRMLROS2NODENode::GetSubscriberNodeByTopic(const std::string & topic){ - +vtkMRMLROS2SubscriberNode* vtkMRMLROS2NODENode::GetSubscriberNodeByTopic(const std::string & topic) +{ int subscriberRefs = this->GetNumberOfNodeReferences("subscriber"); - for (int j = 0; j < subscriberRefs; j ++){ - + for (int j = 0; j < subscriberRefs; j ++) { vtkMRMLROS2SubscriberNode * node = vtkMRMLROS2SubscriberNode::SafeDownCast(this->GetNthNodeReference("subscriber", j)); - if (!node){ + if (!node) { vtkWarningMacro(<< "Node referenced by role 'subscriber' is not a subscriber"); } std::string topicName = node->GetTopic(); - if (topicName == topic){ // check if an existing nodes name matches the topic provided + if (topicName == topic) { // check if an existing nodes name matches the topic provided return node; // if so return the node } } return nullptr; // otherwise return a null ptr } -vtkMRMLROS2PublisherNode* vtkMRMLROS2NODENode::GetPublisherNodeByTopic(const std::string & topic){ - +vtkMRMLROS2PublisherNode* vtkMRMLROS2NODENode::GetPublisherNodeByTopic(const std::string & topic) +{ int publisherRefs = this->GetNumberOfNodeReferences("publisher"); - for (int j = 0; j < publisherRefs; j ++){ + for (int j = 0; j < publisherRefs; j ++) { vtkMRMLROS2PublisherNode * node = vtkMRMLROS2PublisherNode::SafeDownCast(this->GetNthNodeReference("publisher", j)); - if (!node){ + if (!node) { vtkWarningMacro(<< "Node referenced by role 'subscriber' is not a subscriber"); } std::string topicName = node->GetTopic(); - if (topicName == topic){ // check if an existing nodes name matches the topic provided + if (topicName == topic) { // check if an existing nodes name matches the topic provided return node; // if so return the node } } @@ -120,4 +119,4 @@ void vtkMRMLROS2NODENode::UpdateScene(vtkMRMLScene *scene) { Superclass::UpdateScene(scene); std::cerr << "ROS2NODENode updated" << std::endl; -} \ No newline at end of file +} diff --git a/MRML/vtkMRMLROS2PublisherInternals.h b/MRML/vtkMRMLROS2PublisherInternals.h index ca07a93..cf9f4a6 100644 --- a/MRML/vtkMRMLROS2PublisherInternals.h +++ b/MRML/vtkMRMLROS2PublisherInternals.h @@ -5,16 +5,9 @@ #include #include -#include -#include -#include -#include - #include #include -using namespace std::chrono_literals; - class vtkMRMLROS2PublisherInternals { public: @@ -25,11 +18,11 @@ class vtkMRMLROS2PublisherInternals virtual bool AddToROS2Node(vtkMRMLScene * scene, const char * nodeId, const std::string & topic, std::string & errorMessage) = 0; + virtual bool IsAddedToROS2Node(void) const = 0; virtual const char * GetROSType(void) const = 0; virtual const char * GetSlicerType(void) const = 0; virtual size_t GetNumberOfMessagesSent(vtkMRMLScene * scene, const char * nodeId, const std::string & topic) = 0; vtkMRMLROS2NODENode * rosNodePtr; - int nthRef = 0; protected: vtkMRMLROS2PublisherNode * mMRMLNode; @@ -47,14 +40,15 @@ class vtkMRMLROS2PublisherTemplatedInternals: public vtkMRMLROS2PublisherInterna protected: _ros_type mMessageROS; - std::shared_ptr> mPublisher; + std::shared_ptr> mPublisher = nullptr; /** * Add the Publisher to the ROS2 node. This methods searched the * vtkMRMLROS2NODENode by Id to locate the rclcpp::node */ bool AddToROS2Node(vtkMRMLScene * scene, const char * nodeId, - const std::string & topic, std::string & errorMessage) { + const std::string & topic, std::string & errorMessage) override + { vtkMRMLNode * rosNodeBasePtr = scene->GetNodeByID(nodeId); if (!rosNodeBasePtr) { errorMessage = "unable to locate node"; @@ -67,25 +61,27 @@ class vtkMRMLROS2PublisherTemplatedInternals: public vtkMRMLROS2PublisherInterna } std::shared_ptr nodePointer = rosNodePtr->mInternals->mNodePointer; - // vtkMRMLNode * pub = rosNodePtr->GetPublisherNodeByTopic(topic); - // if (pub == nullptr){ + vtkMRMLNode * pub = rosNodePtr->GetPublisherNodeByTopic(topic); + if (pub != nullptr) { + errorMessage = "there is already a publisher for topic \"" + topic + "\""; + return false; + } mPublisher = nodePointer->create_publisher<_ros_type>(topic, 10); rosNodePtr->SetNthNodeReferenceID("publisher", - rosNodePtr->GetNumberOfNodeReferences("publisher"), - mMRMLNode->GetID()); + rosNodePtr->GetNumberOfNodeReferences("publisher"), + mMRMLNode->GetID()); mMRMLNode->SetNodeReferenceID("node", nodeId); - // } - // Otherwise state that there is already a subscriber for that topic - // else{ - // return false; - // } return true; } + bool IsAddedToROS2Node(void) const override + { + return (mPublisher != nullptr); + } + const char * GetROSType(void) const override { return rosidl_generator_traits::name<_ros_type>(); - // return typeid(_ros_type).name(); } const char * GetSlicerType(void) const override @@ -103,7 +99,7 @@ class vtkMRMLROS2PublisherTemplatedInternals: public vtkMRMLROS2PublisherInterna if (!rosNodePtr) { return false; } - if (rosNodePtr->GetPublisherNodeByTopic(topic) && rosNodePtr->GetSubscriberNodeByTopic(topic)) + if (rosNodePtr->GetPublisherNodeByTopic(topic) && rosNodePtr->GetSubscriberNodeByTopic(topic)) { mMRMLNode->mNumberOfMessagesSent++; } diff --git a/MRML/vtkMRMLROS2PublisherNativeInternals.h b/MRML/vtkMRMLROS2PublisherNativeInternals.h index 819c8b4..0fe2c63 100644 --- a/MRML/vtkMRMLROS2PublisherNativeInternals.h +++ b/MRML/vtkMRMLROS2PublisherNativeInternals.h @@ -16,11 +16,14 @@ class vtkMRMLROS2PublisherNativeInternals: _slicer_type mLastMessageSlicer; - void Publish(const _slicer_type & msg) + size_t Publish(const _slicer_type & msg) { - vtkSlicerToROS2(msg, this->mMessageROS); - this->mPublisher->publish(this->mMessageROS); - this->mMRMLNode->mNumberOfCalls++; + const auto nbSubscriber = this->mPublisher->get_subscription_count(); + if (nbSubscriber != 0) { + vtkSlicerToROS2(msg, this->mMessageROS); + this->mPublisher->publish(this->mMessageROS); + } + return nbSubscriber; } }; @@ -31,49 +34,52 @@ class vtkMRMLROS2PublisherNativeInternals: { \ public: \ typedef vtkMRMLROS2Publisher##name##Node SelfType; \ - vtkTypeMacro(vtkMRMLROS2Publisher##name##Node, vtkMRMLROS2PublisherNode); \ + vtkTypeMacro(vtkMRMLROS2Publisher##name##Node, vtkMRMLROS2PublisherNode); \ \ static SelfType * New(void); \ vtkMRMLNode * CreateNodeInstance(void) override; \ const char * GetNodeTagName(void) override; \ - void Publish( const slicer_type & message); \ - \ + size_t Publish(const slicer_type & message); \ + \ protected: \ - vtkMRMLROS2Publisher##name##Node(); \ + vtkMRMLROS2Publisher##name##Node(); \ ~vtkMRMLROS2Publisher##name##Node(); \ } #define VTK_MRML_ROS_PUBLISHER_NATIVE_CXX(slicer_type, ros_type, name) \ - \ + \ vtkStandardNewMacro(vtkMRMLROS2Publisher##name##Node); \ - \ + \ typedef vtkMRMLROS2PublisherNativeInternals \ vtkMRMLROS2Publisher##name##Internals; \ \ - vtkMRMLROS2Publisher##name##Node::vtkMRMLROS2Publisher##name##Node() \ - { \ - mInternals = new vtkMRMLROS2Publisher##name##Internals(this); \ - } \ - \ - vtkMRMLROS2Publisher##name##Node::~vtkMRMLROS2Publisher##name##Node() \ - { \ - delete mInternals; \ -} \ - \ - vtkMRMLNode * vtkMRMLROS2Publisher##name##Node::CreateNodeInstance(void) \ - { \ - return SelfType::New(); \ - } \ - \ - const char * vtkMRMLROS2Publisher##name##Node::GetNodeTagName(void) \ - { \ - return "ROS2Publisher"#name; \ - } \ - \ - void vtkMRMLROS2Publisher##name##Node::Publish(const slicer_type & message) \ - { \ - (dynamic_cast(mInternals))->Publish(message); \ - } + vtkMRMLROS2Publisher##name##Node::vtkMRMLROS2Publisher##name##Node() \ + { \ + mInternals = new vtkMRMLROS2Publisher##name##Internals(this); \ + } \ + \ + vtkMRMLROS2Publisher##name##Node::~vtkMRMLROS2Publisher##name##Node() \ + { \ + delete mInternals; \ + } \ + \ + vtkMRMLNode * vtkMRMLROS2Publisher##name##Node::CreateNodeInstance(void) \ + { \ + return SelfType::New(); \ + } \ + \ + const char * vtkMRMLROS2Publisher##name##Node::GetNodeTagName(void) \ + { \ + return "ROS2Publisher"#name; \ + } \ + \ + size_t vtkMRMLROS2Publisher##name##Node::Publish(const slicer_type & message) \ + { \ + mNumberOfCalls++; \ + const auto justSent = (reinterpret_cast(mInternals))->Publish(message); \ + mNumberOfMessagesSent += justSent; \ + return justSent; \ + } #endif // __vtkMRMLROS2PublisherNativeInternals_h diff --git a/MRML/vtkMRMLROS2PublisherNode.cxx b/MRML/vtkMRMLROS2PublisherNode.cxx index 5b11e1d..39a6a39 100644 --- a/MRML/vtkMRMLROS2PublisherNode.cxx +++ b/MRML/vtkMRMLROS2PublisherNode.cxx @@ -10,18 +10,18 @@ vtkMRMLROS2PublisherNode::~vtkMRMLROS2PublisherNode() { } -void vtkMRMLROS2PublisherNode::PrintSelf(ostream& os, vtkIndent indent) +void vtkMRMLROS2PublisherNode::PrintSelf(std::ostream& os, vtkIndent indent) { Superclass::PrintSelf(os,indent); os << indent << "Topic: " << mTopic << "\n"; os << indent << "ROS type: " << mInternals->GetROSType() << "\n"; os << indent << "Slicer type: " << mInternals->GetSlicerType() << "\n"; // This is scrambled - os << indent << "Number of calls: " << mNumberOfCalls << "\n"; - os << indent << "Number of messages sent:" << mNumberOfMessagesSent << "\n"; // This should be replaced with GetLastMessage + os << indent << "Number of calls: " << mNumberOfCalls << "\n"; + os << indent << "Number of messages sent:" << mNumberOfMessagesSent << "\n"; } bool vtkMRMLROS2PublisherNode::AddToROS2Node(const char * nodeId, - const std::string & topic) + const std::string & topic) { mTopic = topic; mMRMLNodeName = "ros2:pub:" + topic; @@ -29,26 +29,20 @@ bool vtkMRMLROS2PublisherNode::AddToROS2Node(const char * nodeId, vtkMRMLScene * scene = this->GetScene(); if (!this->GetScene()) { - vtkWarningMacro(<< "AddToROS2Node, Publisher MRML node for topic \"" << topic << "\" needs to be added to the scene first"); + vtkErrorMacro(<< "AddToROS2Node, publisher MRML node for topic \"" << topic << "\" needs to be added to the scene first"); return false; } - - parentNodeID.assign(nodeId); - std::string errorMessage; - if (mInternals->AddToROS2Node(scene, nodeId, topic, errorMessage)) { - return true; - } - else{ - vtkWarningMacro(<< "Publisher for this topic: \"" << topic << "\" is already in the scene."); + if (!mInternals->AddToROS2Node(scene, nodeId, topic, errorMessage)) { + vtkErrorMacro(<< "AddToROS2Node, " << errorMessage); + return false; } - vtkWarningMacro(<< "AddToROS2Node, looking for ROS2 node: " << errorMessage); - return false; + return true; } -const char * vtkMRMLROS2PublisherNode::GetTopic(void) const +bool vtkMRMLROS2PublisherNode::IsAddedToROS2Node(void) const { - return mTopic.c_str(); + return mInternals->IsAddedToROS2Node(); } const char * vtkMRMLROS2PublisherNode::GetROSType(void) const @@ -61,43 +55,35 @@ const char * vtkMRMLROS2PublisherNode::GetSlicerType(void) const return mInternals->GetSlicerType(); } -size_t vtkMRMLROS2PublisherNode::GetNumberOfCalls(void) const -{ - return mNumberOfCalls; -} - -size_t vtkMRMLROS2PublisherNode::GetNumberOfMessagesSent(const char * nodeId, const std::string & topic) -{ - vtkMRMLScene * scene = this->GetScene(); - return mInternals->GetNumberOfMessagesSent(scene, nodeId, topic); -} - -void vtkMRMLROS2PublisherNode::WriteXML( ostream& of, int nIndent ) +void vtkMRMLROS2PublisherNode::WriteXML(ostream& of, int nIndent) { Superclass::WriteXML(of, nIndent); // This will take care of referenced nodes - vtkIndent indent(nIndent); - + // vtkIndent indent(nIndent); vtkMRMLWriteXMLBeginMacro(of); - vtkMRMLWriteXMLStdStringMacro(topicName, mTopic); - vtkMRMLWriteXMLStdStringMacro(parentNodeID, parentNodeID); + vtkMRMLWriteXMLStdStringMacro(topicName, Topic); vtkMRMLWriteXMLEndMacro(); } -//------------------------------------------------------------------------------ -void vtkMRMLROS2PublisherNode::ReadXMLAttributes( const char** atts ) +void vtkMRMLROS2PublisherNode::ReadXMLAttributes(const char** atts) { int wasModifying = this->StartModify(); Superclass::ReadXMLAttributes(atts); // This will take care of referenced nodes vtkMRMLReadXMLBeginMacro(atts); - vtkMRMLReadXMLStdStringMacro(topicName, mTopic); - vtkMRMLReadXMLStdStringMacro(parentNodeID, parentNodeID); + vtkMRMLReadXMLStdStringMacro(topicName, Topic); vtkMRMLReadXMLEndMacro(); this->EndModify(wasModifying); } void vtkMRMLROS2PublisherNode::UpdateScene(vtkMRMLScene *scene) { - Superclass::UpdateScene(scene); - this->AddToROS2Node(parentNodeID.c_str(), mTopic); - std::cerr << "Publisher updated. " << std::endl; -} \ No newline at end of file + Superclass::UpdateScene(scene); + if (!IsAddedToROS2Node()) { + int nbNodeRefs = this->GetNumberOfNodeReferences("node"); + if (nbNodeRefs != 1) { + vtkErrorMacro(<< "No ROS2 node reference defined for publisher \"" << GetName() << "\""); + } else { + this->AddToROS2Node(this->GetNthNodeReference("node", 0)->GetID(), + mTopic); + } + } +} diff --git a/MRML/vtkMRMLROS2PublisherNode.h b/MRML/vtkMRMLROS2PublisherNode.h index 17f2554..43a3267 100644 --- a/MRML/vtkMRMLROS2PublisherNode.h +++ b/MRML/vtkMRMLROS2PublisherNode.h @@ -12,6 +12,7 @@ class vtkMRMLROS2PublisherInternals; class VTK_SLICER_ROS2_MODULE_MRML_EXPORT vtkMRMLROS2PublisherNode: public vtkMRMLNode { + // friend declarations friend class vtkMRMLROS2PublisherInternals; template @@ -22,35 +23,29 @@ class VTK_SLICER_ROS2_MODULE_MRML_EXPORT vtkMRMLROS2PublisherNode: public vtkMRM bool AddToROS2Node(const char * nodeId, const std::string & topic); + bool IsAddedToROS2Node(void) const; - const char * GetTopic(void) const; + const std::string & GetTopic(void) const { + return mTopic; + } const char * GetROSType(void) const; const char * GetSlicerType(void) const; - size_t GetNumberOfCalls(void) const; + size_t GetNumberOfCalls(void) const { + return mNumberOfCalls; + } - size_t GetNumberOfMessagesSent(const char * nodeId, const std::string & topic); + size_t GetNumberOfMessagesSent(void) const { + return mNumberOfMessagesSent; + } - size_t mNumberOfCalls = 0; - size_t mNumberOfMessagesSent = 0; + void PrintSelf(std::ostream& os, vtkIndent indent) override; - void PrintSelf(ostream& os, vtkIndent indent) override; - - // - // /** - // * Get the latest message as a vtkVariant. This method will use the - // * latest ROS message received and convert it to the internal - // * Slicer/VTK type if needed. The result of the conversion is - // * cached so future calls to GetLastMessage don't require converting - // * again - // */ - // virtual vtkVariant GetLastMessageVariant(void) = 0; - - // Save and load - virtual void ReadXMLAttributes( const char** atts ) override; - virtual void WriteXML( ostream& of, int indent ) override; + // Save and load + virtual void ReadXMLAttributes(const char** atts) override; + virtual void WriteXML(std::ostream& of, int indent) override; void UpdateScene(vtkMRMLScene *scene) override; protected: @@ -60,12 +55,14 @@ class VTK_SLICER_ROS2_MODULE_MRML_EXPORT vtkMRMLROS2PublisherNode: public vtkMRM vtkMRMLROS2PublisherInternals * mInternals; std::string mTopic = "undefined"; std::string mMRMLNodeName = "ros2:sub:undefined"; - std::string parentNodeID = "undefined"; - vtkGetMacro(mTopic, std::string); - vtkSetMacro(mTopic, std::string); - vtkGetMacro(parentNodeID, std::string); - vtkSetMacro(parentNodeID, std::string); + size_t mNumberOfCalls = 0; + size_t mNumberOfMessagesSent = 0; + + // For ReadXMLAttributes + inline void SetTopic(const std::string & topic) { + mTopic = topic; + } }; #endif // __vtkMRMLROS2PublisherNode_h diff --git a/MRML/vtkMRMLROS2PublisherVTKInternals.h b/MRML/vtkMRMLROS2PublisherVTKInternals.h index 3c7b2a4..ee1b86e 100644 --- a/MRML/vtkMRMLROS2PublisherVTKInternals.h +++ b/MRML/vtkMRMLROS2PublisherVTKInternals.h @@ -18,18 +18,14 @@ class vtkMRMLROS2PublisherVTKInternals: vtkSmartPointer<_slicer_type> mLastMessageSlicer; - void Publish(_slicer_type * msg) + size_t Publish(_slicer_type * msg) { - vtkSlicerToROS2(msg, this->mMessageROS); - this->mPublisher->publish(this->mMessageROS); - this->mMRMLNode->mNumberOfCalls++; - } - - // overload Publish to support pointers - void Publish(vtkSmartPointer<_slicer_type> msg) - { - this->Publish(msg.GetPointer()); - this->mMRMLNode->mNumberOfCalls++; + const auto nbSubscriber = this->mPublisher->get_subscription_count(); + if (nbSubscriber != 0) { + vtkSlicerToROS2(msg, this->mMessageROS); + this->mPublisher->publish(this->mMessageROS); + } + return nbSubscriber; } }; @@ -45,8 +41,8 @@ class vtkMRMLROS2PublisherVTKInternals: static SelfType * New(void); \ vtkMRMLNode * CreateNodeInstance(void) override; \ const char * GetNodeTagName(void) override; \ - void Publish(slicer_type* msg); \ - void Publish( vtkSmartPointer message); \ + size_t Publish(slicer_type* msg); \ + size_t Publish(vtkSmartPointer message); \ \ protected: \ vtkMRMLROS2Publisher##name##Node(); \ @@ -81,14 +77,17 @@ class vtkMRMLROS2PublisherVTKInternals: return "ROS2Publisher"#name; \ } \ \ - void vtkMRMLROS2Publisher##name##Node::Publish(slicer_type * message) \ + size_t vtkMRMLROS2Publisher##name##Node::Publish(slicer_type * message) \ { \ - (dynamic_cast(mInternals))->Publish(message); \ - } \ - \ - void vtkMRMLROS2Publisher##name##Node::Publish(vtkSmartPointer message) \ + mNumberOfCalls++; \ + const auto justSent = (reinterpret_cast(mInternals))->Publish(message); \ + mNumberOfMessagesSent += justSent; \ + return justSent; \ + } \ + \ + size_t vtkMRMLROS2Publisher##name##Node::Publish(vtkSmartPointer message) \ { \ - (dynamic_cast(mInternals))->Publish(message); \ - } - + return this->Publish(message.GetPointer()); \ + } + #endif // __vtkMRMLROS2PublisherVTKInternals_h