Re #15 : Replicated subscriber code to run for Parameters

Updated and newly created parameters for tracked node of name : "/<node_name>" is displayed (minimal implementation)

Logic/vtkSlicerROS2Logic.cxx

@@ -36,6 +36,9 @@
 #include <vtkMRMLROS2SubscriberDefaultNodes.h>
 #include <vtkMRMLROS2PublisherDefaultNodes.h>
 
+//to be changed
+#include <vtkMRMLROS2ParameterNativeNode.h>
+
 #include<vtkMRMLNode.h>
 
 // VTK includes
@@ -186,6 +189,9 @@ void vtkSlicerROS2Logic::RegisterNodes(void)
   // Publishers
   this->GetMRMLScene()->RegisterNodeClass(vtkSmartPointer<vtkMRMLROS2PublisherStringNode>::New());
   this->GetMRMLScene()->RegisterNodeClass(vtkSmartPointer<vtkMRMLROS2PublisherPoseStampedNode>::New());
+
+  // Parameters
+  this->GetMRMLScene()->RegisterNodeClass(vtkSmartPointer<vtkMRMLROS2ParameterStringNode>::New());
 }
 
 
@@ -761,6 +767,35 @@ vtkMRMLROS2PublisherNode * vtkSlicerROS2Logic::CreateAndAddPublisher(const char
   return nullptr;
 }
 
+vtkMRMLROS2ParameterNode * vtkSlicerROS2Logic::CreateAndAddParameter(const char * className, const std::string & trackedNodeName)
+{
+  // First make sure that we have a ROS2Node node
+  if (mROS2Node == nullptr) {
+    vtkErrorMacro(<< "the default ROS node has not been created yet");
+    return nullptr;
+  }
+
+  // CreateNodeByClass
+  vtkSmartPointer<vtkMRMLNode> node = this->GetMRMLScene()->CreateNodeByClass(className);
+
+  // Check that this is a Parameter so we can add it
+  vtkMRMLROS2ParameterNode * parameterNode = vtkMRMLROS2ParameterNode::SafeDownCast(node);
+  if (parameterNode == nullptr) {
+    vtkErrorMacro(<< "\"" << className << "\" is not derived from vtkMRMLROS2ParameterNode");
+    return nullptr;
+  }
+
+  // Add to the scene so the ROS2Node node can find it
+  this->GetMRMLScene()->AddNode(parameterNode);
+  if (parameterNode->AddToROS2Node(mROS2Node->GetID(), trackedNodeName)) {
+    return parameterNode;
+  }
+  // Something went wrong, cleanup
+  this->GetMRMLScene()->RemoveNode(node);
+  node->Delete();
+  return nullptr;
+}
+
 
 void vtkSlicerROS2Logic::AddROS2Node(void)
 {
@@ -795,6 +830,14 @@ void vtkSlicerROS2Logic::AddSomeSubscribers(void)
   this->CreateAndAddSubscriber("vtkMRMLROS2SubscriberStringNode", "/string_sub_2");
 }
 
+void vtkSlicerROS2Logic::AddSomeParameters(void)
+{
+  // the long way
+  vtkSmartPointer<vtkMRMLROS2ParameterStringNode> subString = vtkMRMLROS2ParameterStringNode::New();
+  this->GetMRMLScene()->AddNode(subString);
+  subString->AddToROS2Node(mROS2Node->GetID(), "/string_param");
+}
+
 
 void vtkSlicerROS2Logic::AddTransformForMatrix(vtkSmartPointer<vtkMatrix4x4> mat, std::string name){
   // Add a transform for subscriber type 1 (PoseStamped)

Logic/vtkSlicerROS2Logic.h

@@ -36,6 +36,7 @@ namespace rclcpp {
 class vtkMRMLTransformNode;
 class vtkMRMLROS2SubscriberNode;
 class vtkMRMLROS2PublisherNode;
+class vtkMRMLROS2ParameterNode;
 
 // Slicer includes
 #include <vtkSlicerModuleLogic.h>
@@ -97,9 +98,12 @@ class VTK_SLICER_ROS2_MODULE_LOGIC_EXPORT vtkSlicerROS2Logic:
     new publisher was not created. */
   vtkMRMLROS2PublisherNode * CreateAndAddPublisher(const char * className, const std::string & topic);
 
+  vtkMRMLROS2ParameterNode * CreateAndAddParameter(const char * className, const std::string & topic);
+
   void AddROS2Node(void);
   void AddSomePublishers(void);
   void AddSomeSubscribers(void);
+  void AddSomeParameters(void);
 
   // std::vector<vtkSmartPointer<vtkMRMLROS2SubscriberNode>> mSubs; // This is a list of the subscribers to update the widget
   vtkSmartPointer<vtkMRMLROS2NODENode> mROS2Node; // proper MRML node // Moved to public which might be wrong!!

MRML/CMakeLists.txt

@@ -21,6 +21,7 @@ set(_SRCS_INTERNAL
   vtkMRMLROS2NodeInternals.h
   vtkMRMLROS2PublisherInternals.h
   vtkMRMLROS2SubscriberInternals.h
+  vtkMRMLROS2ParameterInternals.h
   )
 
 set_source_files_properties(
@@ -43,6 +44,10 @@ set(${KIT}_SRCS
   vtkMRMLROS2PublisherNode.cxx
   vtkMRMLROS2PublisherDefaultNodes.h
   vtkMRMLROS2PublisherDefaultNodes.cxx
+  vtkMRMLROS2ParameterNode.h
+  vtkMRMLROS2ParameterNode.cxx 
+  vtkMRMLROS2ParameterNativeNode.h
+  vtkMRMLROS2ParameterNativeNode.cxx 
   )
 
 SET (${KIT}_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR} CACHE INTERNAL "" FORCE)

MRML/vtkMRMLROS2NODENode.cxx

@@ -3,6 +3,7 @@
 #include "vtkCommand.h"
 #include <vtkMRMLROS2SubscriberNode.h>
 #include <vtkMRMLROS2PublisherNode.h> 
+#include <vtkMRMLROS2ParameterNode.h> 
 
 vtkStandardNewMacro(vtkMRMLROS2NODENode);
 
@@ -89,6 +90,21 @@ vtkMRMLROS2PublisherNode* vtkMRMLROS2NODENode::GetPublisherNodeByTopic(const std
   return nullptr; // otherwise return a null ptr
 }
 
+vtkMRMLROS2ParameterNode* vtkMRMLROS2NODENode::GetParameterNodeByTopic(const std::string & topic)
+{
+  int parameterRefs = this->GetNumberOfNodeReferences("parameter");
+  for (int j = 0; j < parameterRefs; j ++) {
+    vtkMRMLROS2ParameterNode * node = vtkMRMLROS2ParameterNode::SafeDownCast(this->GetNthNodeReference("parameter", j));
+    if (!node) {
+      vtkWarningMacro(<< "Node referenced by role 'parameter' is not a parameter");
+    }
+    std::string topicName = node->GetTopic(); 
+    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
+}
 
 void vtkMRMLROS2NODENode::Spin(void)
 {

MRML/vtkMRMLROS2NODENode.h

@@ -10,12 +10,14 @@
 class vtkMRMLROS2NodeInternals;
 class vtkMRMLROS2SubscriberNode;
 class vtkMRMLROS2PublisherNode;
+class vtkMRMLROS2ParameterNode;
 
 class VTK_SLICER_ROS2_MODULE_MRML_EXPORT vtkMRMLROS2NODENode: public vtkMRMLNode
 {
 
   template <typename _ros_type, typename _slicer_type> friend class vtkMRMLROS2SubscriberTemplatedInternals;
   template <typename _slicer_type, typename _ros_type> friend class vtkMRMLROS2PublisherTemplatedInternals;
+  template <typename _slicer_type, typename _ros_type> friend class vtkMRMLROS2ParameterTemplatedInternals;
 
  public:
   typedef vtkMRMLROS2NODENode SelfType;
@@ -32,6 +34,7 @@ class VTK_SLICER_ROS2_MODULE_MRML_EXPORT vtkMRMLROS2NODENode: public vtkMRMLNode
   void Spin(void);
   vtkMRMLROS2SubscriberNode* GetSubscriberNodeByTopic(const std::string & topic);
   vtkMRMLROS2PublisherNode* GetPublisherNodeByTopic(const std::string & topic);
+  vtkMRMLROS2ParameterNode* GetParameterNodeByTopic(const std::string & topic);
 
   // Save and load
   virtual void ReadXMLAttributes(const char** atts) override;

MRML/vtkMRMLROS2ParameterInternals.h

@@ -0,0 +1,143 @@
+#ifndef __vtkMRMLROS2ParameterInternals_h
+#define __vtkMRMLROS2ParameterInternals_h
+
+// ROS2 includes
+#include <rclcpp/rclcpp.hpp>
+
+#include <vtkMRMLScene.h>
+#include <vtkMRMLROS2NODENode.h>
+#include <vtkMRMLROS2NodeInternals.h>
+
+class vtkMRMLROS2ParameterInternals
+{
+public:
+  vtkMRMLROS2ParameterInternals(vtkMRMLROS2ParameterNode * mrmlNode):
+    mMRMLNode(mrmlNode)
+  {}
+  virtual ~vtkMRMLROS2ParameterInternals() = default;
+
+  virtual bool AddToROS2Node(vtkMRMLScene * scene, const char * nodeId,
+			     const std::string & trackedNodeName, 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 std::string GetLastMessageYAML(void) const = 0;
+
+protected:
+  vtkMRMLROS2ParameterNode * mMRMLNode;
+};
+
+
+template <typename _ros_type, typename _slicer_type>
+class vtkMRMLROS2ParameterTemplatedInternals: public vtkMRMLROS2ParameterInternals
+{
+public:
+  typedef vtkMRMLROS2ParameterTemplatedInternals<_ros_type, _slicer_type> SelfType;
+
+  vtkMRMLROS2ParameterTemplatedInternals(vtkMRMLROS2ParameterNode *  mrmlNode):
+    vtkMRMLROS2ParameterInternals(mrmlNode)
+  {}
+
+protected:
+  _ros_type mLastMessageROS;
+  std::shared_ptr<rclcpp::Subscription<_ros_type>> mSubscription = nullptr;
+  std::shared_ptr<rclcpp::ParameterEventHandler> param_subscriber_ = nullptr;
+  std::shared_ptr<rclcpp::ParameterEventCallbackHandle> cb_handle;
+
+  /**
+   * This is the ROS callback for the subscription.  This methods
+   * saves the ROS message as-is and set the modified flag for the
+   * MRML node
+   */
+  void ParameterCallback(const _ros_type & message) {
+    // \todo is there a timestamp in MRML nodes we can update from the ROS message?
+    mLastMessageROS = message;
+    mMRMLNode->mNumberOfMessages++;
+    mMRMLNode->Modified();
+  }
+
+  /**
+   * Add the subscriber 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 & trackedNodeName, std::string & errorMessage) {
+
+    vtkMRMLNode * rosNodeBasePtr = scene->GetNodeByID(nodeId);
+
+    if (!rosNodeBasePtr) {
+      errorMessage = "unable to locate node";
+      return false;
+    }
+
+    vtkMRMLROS2NODENode * rosNodePtr = dynamic_cast<vtkMRMLROS2NODENode *>(rosNodeBasePtr);
+
+    if (!rosNodePtr) {
+      errorMessage = std::string(rosNodeBasePtr->GetName()) + " doesn't seem to be a vtkMRMLROS2NODENode";
+      return false;
+    }
+
+    std::shared_ptr<rclcpp::Node> nodePointer = rosNodePtr->mInternals->mNodePointer;
+
+    param_subscriber_ = std::make_shared<rclcpp::ParameterEventHandler>(nodePointer);
+
+    auto cb = [trackedNodeName, nodePointer](const rcl_interfaces::msg::ParameterEvent &event)
+    {
+        // Obtain list of parameters that changed
+        std::cerr << "Event node :"<< event.node.c_str() << std::endl;
+        if(!strcmp(event.node.c_str(),trackedNodeName.c_str())){
+          auto params = rclcpp::ParameterEventHandler::get_parameters_from_event(event);
+
+          // Iterate through every parameter in the list
+          for (auto &p : params)
+          {
+              // Create a string message from the info received.
+              std::string msgString = "Param Name :" + p.get_name() + " | Param Type : " + p.get_type_name() + " | Param Value : " + p.value_to_string();
+              std::cerr << "Received an update to a parameter"  << std::endl;
+              std::cerr << msgString << "\n" <<std::endl;
+          }
+        }
+    };
+
+    cb_handle = param_subscriber_->add_parameter_event_callback(cb);
+
+    // mSubscription
+    //   = nodePointer->create_subscription<_ros_type>(topic, 100,
+		// 				    std::bind(&SelfType::ParameterCallback, this, std::placeholders::_1));
+
+    rosNodePtr->SetNthNodeReferenceID("parameter",
+				      rosNodePtr->GetNumberOfNodeReferences("parameter"),
+				      mMRMLNode->GetID());
+
+    mMRMLNode->SetNodeReferenceID("node", nodeId);
+
+    return true;
+
+  }
+
+  bool IsAddedToROS2Node(void) const override
+  {
+    return (mSubscription != nullptr);
+  }
+
+  const char * GetROSType(void) const override
+  {
+    return rosidl_generator_traits::name<_ros_type>();
+  }
+
+  const char * GetSlicerType(void) const override
+  {
+    return typeid(_slicer_type).name();
+  }
+
+  std::string GetLastMessageYAML(void) const override
+  {
+    std::stringstream out;
+    rosidl_generator_traits::to_yaml(mLastMessageROS, out);
+    return out.str();
+  }
+};
+
+#endif
+// __vtkMRMLROS2ParameterInternals_h
+

MRML/vtkMRMLROS2ParameterNativeInternals.h

@@ -0,0 +1,32 @@
+#ifndef __vtkMRMLROS2ParameterNativeInternals_h
+#define __vtkMRMLROS2ParameterNativeInternals_h
+
+#include <vtkMRMLROS2ParameterInternals.h>
+
+template <typename _ros_type, typename _slicer_type>
+class vtkMRMLROS2ParameterNativeInternals:
+  public vtkMRMLROS2ParameterTemplatedInternals<_ros_type, _slicer_type>
+{
+public:
+  typedef vtkMRMLROS2ParameterTemplatedInternals<_ros_type, _slicer_type> BaseType;
+
+  vtkMRMLROS2ParameterNativeInternals(vtkMRMLROS2ParameterNode * mrmlNode):
+    BaseType(mrmlNode)
+  {}
+
+  _slicer_type mLastMessageSlicer;
+
+  void GetLastMessage(_slicer_type & result)
+  {
+    // todo maybe add some check that we actually received a message?
+    vtkROS2ToSlicer(this->mLastMessageROS, result);
+  }
+
+  vtkVariant GetLastMessageVariant(void)
+  {
+    GetLastMessage(mLastMessageSlicer);
+    return vtkVariant(mLastMessageSlicer);
+  }
+};
+
+#endif // __vtkMRMLROS2ParameterNativeInternals_h