From 2d58c12fb3bce0b8593b009db9053f1b736ed351 Mon Sep 17 00:00:00 2001 From: Anton Deguet Date: Fri, 9 Dec 2022 00:14:28 -0500 Subject: [PATCH] Logic: cleanup --- Logic/vtkSlicerROS2Logic.cxx | 96 ++++++++++++++++-------------------- Logic/vtkSlicerROS2Logic.h | 19 +++++-- 2 files changed, 58 insertions(+), 57 deletions(-) diff --git a/Logic/vtkSlicerROS2Logic.cxx b/Logic/vtkSlicerROS2Logic.cxx index 2ae86b0..81a1b6e 100644 --- a/Logic/vtkSlicerROS2Logic.cxx +++ b/Logic/vtkSlicerROS2Logic.cxx @@ -77,6 +77,7 @@ auto const MM_TO_M_CONVERSION = 1000.00; //---------------------------------------------------------------------------- vtkStandardNewMacro(vtkSlicerROS2Logic); + //---------------------------------------------------------------------------- vtkSlicerROS2Logic::vtkSlicerROS2Logic() { @@ -96,16 +97,8 @@ vtkSlicerROS2Logic::vtkSlicerROS2Logic() mTfListener = std::make_shared(*mTfBuffer); mTfBroadcaster = std::make_unique(*mNodePointer); - - //vtkMRMLROS2SubscriberNode* subNode = vtkMRMLROS2SubscriberNode::SafeDownCast(node); } -// vtkMRMLROS2SubscriberNode* vtkSlicerROS2Logic::CreateSubscriberNode(){ -// -// vtkSmartPointer< vtkMRMLROS2SubscriberNode > subNode = vtkSmartPointer< vtkMRMLROS2SubscriberNode >::New(); -// return subNode; -// -// } //---------------------------------------------------------------------------- vtkSlicerROS2Logic::~vtkSlicerROS2Logic() @@ -163,7 +156,7 @@ void vtkSlicerROS2Logic::SetModelFile(const std::string & selectedFile) mModel.URDF = std::string((std::istreambuf_iterator(input_file)), std::istreambuf_iterator()); if (mModel.URDF == "") { std::cerr << "The URDF file is empty! Please select a valid urdf file." << std::endl; - return; + return; } loadRobotSTLModels(); } @@ -180,7 +173,6 @@ void vtkSlicerROS2Logic::SetMRMLSceneInternal(vtkMRMLScene * newScene) this->SetAndObserveMRMLSceneEventsInternal(newScene, events.GetPointer()); } - //----------------------------------------------------------------------------- void vtkSlicerROS2Logic::RegisterNodes(void) { @@ -209,18 +201,9 @@ void vtkSlicerROS2Logic::OnMRMLSceneNodeAdded(vtkMRMLNode* vtkNotUsed(node)) { } -// bool vtkSlicerROS2Logic::testSubNode(vtkMRMLNode* node){ -// vtkMRMLROS2SubscriberNode* subNode = vtkMRMLROS2SubscriberNode::SafeDownCast(node); -// subNode->SubscriberCallBack(); -// // if (subNode != NULL){ -// // subNode->SubscriberCallBack(); -// // } -// // return false; -// } //--------------------------------------------------------------------------- -void vtkSlicerROS2Logic -::OnMRMLSceneNodeRemoved(vtkMRMLNode* vtkNotUsed(node)) +void vtkSlicerROS2Logic::OnMRMLSceneNodeRemoved(vtkMRMLNode* vtkNotUsed(node)) { } @@ -371,7 +354,6 @@ void vtkSlicerROS2Logic::loadRobotSTLModels(void) tnode->SetMatrixTransformToParent(initialPositionMatrix); tnode->Modified(); - // vtkMRMLTransformNode *transformNode = vtkMRMLTransformNode::SafeDownCast(this->GetMRMLScene()->GetFirstNodeByName((link_names_vector[k] + "_transform").c_str())); tnode->SetAndObserveTransformNodeID(mChainNodeTransforms[k]->GetID()); // transformNode->GetID()); @@ -719,9 +701,10 @@ void vtkSlicerROS2Logic::initializeFkSolver(void) mKDLSolver = new KDL::ChainFkSolverPos_recursive(*kdl_chain); } + vtkMRMLROS2SubscriberNode * vtkSlicerROS2Logic::CreateAndAddSubscriber(const char * className, const std::string & topic) { - // Provide this as python method for adding new subscribers to the scene + // First make sure that we have a ROS2Node node if (mROS2Node == nullptr) { vtkErrorMacro(<< "the default ROS node has not been created yet"); return nullptr; @@ -737,14 +720,21 @@ vtkMRMLROS2SubscriberNode * vtkSlicerROS2Logic::CreateAndAddSubscriber(const cha return nullptr; } + // Add to the scene so the ROS2Node node can find it this->GetMRMLScene()->AddNode(subscriberNode); - subscriberNode->AddToROS2Node(mROS2Node->GetID(), topic); - return subscriberNode; + if (subscriberNode->AddToROS2Node(mROS2Node->GetID(), topic)) { + return subscriberNode; + } + // Something went wrong, cleanup + this->GetMRMLScene()->RemoveNode(node); + node->Delete(); + return nullptr; } + vtkMRMLROS2PublisherNode * vtkSlicerROS2Logic::CreateAndAddPublisher(const char * className, const std::string & topic) { - // Provide this as python method for adding new subscribers to the scene + // First make sure that we have a ROS2Node node if (mROS2Node == nullptr) { vtkErrorMacro(<< "the default ROS node has not been created yet"); return nullptr; @@ -753,30 +743,25 @@ vtkMRMLROS2PublisherNode * vtkSlicerROS2Logic::CreateAndAddPublisher(const char // CreateNodeByClass vtkSmartPointer node = this->GetMRMLScene()->CreateNodeByClass(className); - // Check that this is a subscriber so we can add it + // Check that this is a publisher so we can add it vtkMRMLROS2PublisherNode * publisherNode = vtkMRMLROS2PublisherNode::SafeDownCast(node); if (publisherNode == nullptr) { vtkErrorMacro(<< "\"" << className << "\" is not derived from vtkMRMLROS2PublisherNode"); return nullptr; } + // Add to the scene so the ROS2Node node can find it this->GetMRMLScene()->AddNode(publisherNode); - publisherNode->AddToROS2Node(mROS2Node->GetID(), topic); - return publisherNode; + if (publisherNode->AddToROS2Node(mROS2Node->GetID(), topic)) { + return publisherNode; + } + // Something went wrong, cleanup + this->GetMRMLScene()->RemoveNode(node); + node->Delete(); + return nullptr; } -void vtkSlicerROS2Logic::AddPublisher(void) -{ - vtkSmartPointer stringPub = vtkMRMLROS2PublisherStringNode::New(); - this->GetMRMLScene()->AddNode(stringPub); - stringPub->AddToROS2Node(mROS2Node->GetID(), "/string_pub"); - - vtkSmartPointer stringPub2 = vtkMRMLROS2PublisherStringNode::New(); - this->GetMRMLScene()->AddNode(stringPub2); - stringPub2->AddToROS2Node(mROS2Node->GetID(), "/publisher"); -} - void vtkSlicerROS2Logic::AddROS2Node(void) { mROS2Node = vtkMRMLROS2NODENode::New(); @@ -784,31 +769,33 @@ void vtkSlicerROS2Logic::AddROS2Node(void) mROS2Node->Create("testNode"); } -void vtkSlicerROS2Logic::AddToScene(void) + +void vtkSlicerROS2Logic::AddSomePublishers(void) +{ + // the long way + vtkSmartPointer stringPub = vtkMRMLROS2PublisherStringNode::New(); + this->GetMRMLScene()->AddNode(stringPub); + stringPub->AddToROS2Node(mROS2Node->GetID(), "/string_pub"); + // the fast way + this->CreateAndAddPublisher("vtkMRMLROS2PublisherStringNode", "/string_pub_2"); +} + + +void vtkSlicerROS2Logic::AddSomeSubscribers(void) { + // the long way vtkSmartPointer subString = vtkMRMLROS2SubscriberStringNode::New(); this->GetMRMLScene()->AddNode(subString); subString->AddToROS2Node(mROS2Node->GetID(), "/string_sub"); - + // with VTK type vtkSmartPointer subPose = vtkMRMLROS2SubscriberPoseStampedNode::New(); this->GetMRMLScene()->AddNode(subPose); subPose->AddToROS2Node(mROS2Node->GetID(), "/pose_sub"); - + // the fast way this->CreateAndAddSubscriber("vtkMRMLROS2SubscriberStringNode", "/string_sub_2"); - - - // vtkSmartPointer sub = vtkNew(); - // std::string sub_name = "/blah_blah"; - // sub->SetTopic(sub_name); - // sub->SetSubscriber(mNodePointer); - // vtkSmartPointer mat = vtkNew(); - // sub->GetLastMessage(mat); - // AddTransformForMatrix(mat, sub_name); - // mat->PrintSelf(std::cerr, vtkIndent(0)); - // this->GetMRMLScene()->AddNode(sub); - // mSubs.push_back(sub); } + void vtkSlicerROS2Logic::AddTransformForMatrix(vtkSmartPointer mat, std::string name){ // Add a transform for subscriber type 1 (PoseStamped) vtkSmartPointer poseTransform = vtkSmartPointer::Take(vtkMRMLLinearTransformNode::New()); @@ -818,6 +805,7 @@ void vtkSlicerROS2Logic::AddTransformForMatrix(vtkSmartPointer mat this->GetMRMLScene()->AddNode(poseTransform); } + void vtkSlicerROS2Logic::updateMRMLSceneFromSubs(void){ // This should be activated on spin and should update that's attached to the vtkMRMLROS2SubscriberNode // Get lastMessage needs to work for this though diff --git a/Logic/vtkSlicerROS2Logic.h b/Logic/vtkSlicerROS2Logic.h index ab2a939..6f42160 100644 --- a/Logic/vtkSlicerROS2Logic.h +++ b/Logic/vtkSlicerROS2Logic.h @@ -82,12 +82,25 @@ class VTK_SLICER_ROS2_MODULE_LOGIC_EXPORT vtkSlicerROS2Logic: void Spin(void); void Clear(); void BroadcastTransform(); - void AddToScene(void); - void AddROS2Node(void); - void AddPublisher(void); + + /*! Helper method to create a subscriber given a subscriber type and + a topic. This method will create the corresponding MRML node if + there is no existing subscriber for the given topic and add it to + the default ROS2 node for this logic. It will return a nullptr a + new subscriber was not created. */ vtkMRMLROS2SubscriberNode * CreateAndAddSubscriber(const char * className, const std::string & topic); + + /*! Helper method to create a publisher given a publisher type and + a topic. This method will create the corresponding MRML node if + there is no existing publisher for the given topic and add it to + the default ROS2 node for this logic. It will return a nullptr a + new publisher was not created. */ vtkMRMLROS2PublisherNode * CreateAndAddPublisher(const char * className, const std::string & topic); + void AddROS2Node(void); + void AddSomePublishers(void); + void AddSomeSubscribers(void); + // std::vector> mSubs; // This is a list of the subscribers to update the widget vtkSmartPointer mROS2Node; // proper MRML node // Moved to public which might be wrong!! void AddTransformForMatrix(vtkSmartPointer mat, std::string name);