Skip to content

Commit

Permalink
Publishers:
Browse files Browse the repository at this point in the history 
* Added some checks to prevent `CreateAndAdd` to create a new publisher with same topic #41
* UI now display number of calls and messages sent #40
* Untested code for `UpdateScene` #14
  • Loading branch information
@adeguet1
adeguet1 committed Dec 9, 2022
1 parent 2d58c12 commit 8f5fa23
Show file tree
Hide file tree
Showing 6 changed files with 137 additions and 154 deletions.
23 changes: 11 additions & 12 deletions MRML/vtkMRMLROS2NODENode.cxx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,33 +50,32 @@ void vtkMRMLROS2NODENode::Create(const std::string & nodeName, bool initialize)
mInternals->mNodePointer = std::make_shared<rclcpp::Node>(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
}
}
Expand Down Expand Up @@ -120,4 +119,4 @@ void vtkMRMLROS2NODENode::UpdateScene(vtkMRMLScene *scene)
{
Superclass::UpdateScene(scene);
std::cerr << "ROS2NODENode updated" << std::endl;
}
}
38 changes: 17 additions & 21 deletions MRML/vtkMRMLROS2PublisherInternals.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,16 +5,9 @@
#include <rclcpp/rclcpp.hpp>

#include <vtkMRMLScene.h>
#include <chrono>
#include <functional>
#include <memory>
#include <std_msgs/msg/string.hpp>

#include <vtkMRMLROS2NODENode.h>
#include <vtkMRMLROS2NodeInternals.h>

using namespace std::chrono_literals;

class vtkMRMLROS2PublisherInternals
{
public:
Expand All @@ -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;
Expand All @@ -47,14 +40,15 @@ class vtkMRMLROS2PublisherTemplatedInternals: public vtkMRMLROS2PublisherInterna

protected:
_ros_type mMessageROS;
std::shared_ptr<rclcpp::Publisher<_ros_type>> mPublisher;
std::shared_ptr<rclcpp::Publisher<_ros_type>> 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";
Expand All @@ -67,25 +61,27 @@ class vtkMRMLROS2PublisherTemplatedInternals: public vtkMRMLROS2PublisherInterna
}

std::shared_ptr<rclcpp::Node> 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
Expand All @@ -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++;
}
Expand Down
74 changes: 40 additions & 34 deletions MRML/vtkMRMLROS2PublisherNativeInternals.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
};

Expand All @@ -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<slicer_type, ros_type > \
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<vtkMRMLROS2Publisher##name##Internals *>(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<vtkMRMLROS2Publisher##name##Internals *>(mInternals))->Publish(message); \
mNumberOfMessagesSent += justSent; \
return justSent; \
}

#endif // __vtkMRMLROS2PublisherNativeInternals_h
68 changes: 27 additions & 41 deletions MRML/vtkMRMLROS2PublisherNode.cxx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,45 +10,39 @@ 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;
this->SetName(mMRMLNodeName.c_str());
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
Expand All @@ -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;
}
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);
}
}
}
Loading

0 comments on commit 8f5fa23

Please sign in to comment.