From 68e3a12ffe66a86dc1667330012106c0014f7e56 Mon Sep 17 00:00:00 2001
From: Antoine H <harle@isir.upmc.fr>
Date: Wed, 25 May 2022 12:42:23 +0200
Subject: [PATCH] rtt_ros2_params:  add declareParameter operation

---
 .../orocos/rtt_ros2_params/rosparam.hpp       |  5 +++
 .../rtt_ros2_params_service.hpp               |  3 ++
 .../src/rtt_ros2_params_service.cpp           | 41 +++++++++++++++++++
 3 files changed, 49 insertions(+)

diff --git a/rtt_ros2_params/include/orocos/rtt_ros2_params/rosparam.hpp b/rtt_ros2_params/include/orocos/rtt_ros2_params/rosparam.hpp
index f765016..7218774 100644
--- a/rtt_ros2_params/include/orocos/rtt_ros2_params/rosparam.hpp
+++ b/rtt_ros2_params/include/orocos/rtt_ros2_params/rosparam.hpp
@@ -34,12 +34,14 @@ class RosParam : public RTT::ServiceRequester
   : RTT::ServiceRequester("rosparam", owner),
     getParameter("getParameter"),
     setParameter("setParameter"),
+    declareParameter("declareParameter"),
     setOrDeclareParameter("setOrDeclareParameter"),
     loadProperty("loadProperty"),
     storeProperty("storeProperty")
   {
     this->addOperationCaller(getParameter);
     this->addOperationCaller(setParameter);
+    this->addOperationCaller(declareParameter);
     this->addOperationCaller(setOrDeclareParameter);
 
     // Operations loadProperty and storeProperty are only available if the requester is connected
@@ -61,6 +63,9 @@ class RosParam : public RTT::ServiceRequester
   RTT::OperationCaller<bool(
       const std::string & name,
       const rclcpp::ParameterValue & value)> setParameter;
+  RTT::OperationCaller<bool(
+      const std::string & name,
+      const rclcpp::ParameterValue & default_value)> declareParameter;
   RTT::OperationCaller<bool(
       const std::string & name,
       const rclcpp::ParameterValue & value)> setOrDeclareParameter;
diff --git a/rtt_ros2_params/include/orocos/rtt_ros2_params/rtt_ros2_params_service.hpp b/rtt_ros2_params/include/orocos/rtt_ros2_params/rtt_ros2_params_service.hpp
index dc3f00e..935a2cd 100644
--- a/rtt_ros2_params/include/orocos/rtt_ros2_params/rtt_ros2_params_service.hpp
+++ b/rtt_ros2_params/include/orocos/rtt_ros2_params/rtt_ros2_params_service.hpp
@@ -40,6 +40,9 @@ class RosParamService : public RTT::Service
   bool setParameter(
     const std::string & name,
     const rclcpp::ParameterValue & value);
+  bool declareParameter(
+    const std::string & name,
+    const rclcpp::ParameterValue & default_value);
   bool setOrDeclareParameter(
     const std::string & name,
     const rclcpp::ParameterValue & value);
diff --git a/rtt_ros2_params/src/rtt_ros2_params_service.cpp b/rtt_ros2_params/src/rtt_ros2_params_service.cpp
index a2eb933..78089b7 100644
--- a/rtt_ros2_params/src/rtt_ros2_params_service.cpp
+++ b/rtt_ros2_params/src/rtt_ros2_params_service.cpp
@@ -63,6 +63,12 @@ RosParamService::RosParamService(RTT::TaskContext * owner)
   .arg("name", "Name of the parameter to set")
   .arg("value", "The value of the parameter to set");
 
+  addOperation(
+    "declareParameter", &RosParamService::declareParameter, this, RTT::ClientThread)
+  .doc("Try to declare a parameter to the node parameter server and set its value.")
+  .arg("name", "Name of the parameter to set")
+  .arg("default_value", "The default value of the parameter to declare");
+
   addOperation(
     "setOrDeclareParameter", &RosParamService::setOrDeclareParameter, this, RTT::ClientThread)
   .doc("Sets a parameter to the node parameter server and eventually declares it")
@@ -147,6 +153,41 @@ bool RosParamService::setParameter(
   return true;
 }
 
+bool RosParamService::declareParameter(
+  const std::string & name,
+  const rclcpp::ParameterValue & default_value=rclcpp::ParameterValue())
+{
+  RTT::Logger::In in(getName());
+
+  RTT::log(RTT::Debug) <<
+    "Declaring the parameter \"" << name << "\"" <<
+    RTT::endlog();
+
+  auto rosnode = rtt_ros2_node::getNode(getOwner());
+  if (nullptr == rosnode) {
+    RTT::log(RTT::Error) <<
+      "No ROS node service from package rtt_ros2_node loaded into this "
+      "component or as a global service." <<
+      RTT::endlog();
+    return false;
+  }
+
+  if (!rosnode->has_parameter(name)) {
+    // Create new parameter
+    RTT::log(RTT::Info) <<
+      "The parameter did not exist" <<
+      RTT::endlog();
+    rosnode->declare_parameter(name, default_value);
+    return true;
+  }
+  else{
+    RTT::log(RTT::Error) <<
+      "Failed to declare ROS parameter " << name << ": " <<
+      "has already been declared" << RTT::endlog();
+    return false;
+  }
+}
+
 bool RosParamService::setOrDeclareParameter(
   const std::string & name,
   const rclcpp::ParameterValue & value)