From 2152e0574bb72e565a89ba567c27e74b8b3c3ffd Mon Sep 17 00:00:00 2001
From: Jacob Perron <jacob@openrobotics.org>
Date: Wed, 15 May 2019 16:10:53 -0700
Subject: [PATCH] Guard against ParameterNotDeclaredException in parameter
 service callbacks (#718)

Fixes #705.

If the set_parameters() call fails, it's nice to be able to return a partial result.
Since there is no convenient method to obtain a partial result, we call set_parameters()
once for each parameter.

Signed-off-by: Jacob Perron <jacob@openrobotics.org>
---
 rclcpp/src/rclcpp/parameter_service.cpp | 49 ++++++++++++++++++-------
 1 file changed, 36 insertions(+), 13 deletions(-)

diff --git a/rclcpp/src/rclcpp/parameter_service.cpp b/rclcpp/src/rclcpp/parameter_service.cpp
index afecee4..6cc25e7 100644
--- a/rclcpp/src/rclcpp/parameter_service.cpp
+++ b/rclcpp/src/rclcpp/parameter_service.cpp
@@ -57,11 +57,15 @@ ParameterService::ParameterService(
       const std::shared_ptr<rcl_interfaces::srv::GetParameterTypes::Request> request,
       std::shared_ptr<rcl_interfaces::srv::GetParameterTypes::Response> response)
     {
-      auto types = node_params->get_parameter_types(request->names);
-      std::transform(types.cbegin(), types.cend(),
-      std::back_inserter(response->types), [](const uint8_t & type) {
-        return static_cast<rclcpp::ParameterType>(type);
-      });
+      try {
+        auto types = node_params->get_parameter_types(request->names);
+        std::transform(types.cbegin(), types.cend(),
+        std::back_inserter(response->types), [](const uint8_t & type) {
+          return static_cast<rclcpp::ParameterType>(type);
+        });
+      } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) {
+        RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Failed to get parameter types: %s", ex.what());
+      }
     },
     qos_profile, nullptr);
 
@@ -73,12 +77,20 @@ ParameterService::ParameterService(
       const std::shared_ptr<rcl_interfaces::srv::SetParameters::Request> request,
       std::shared_ptr<rcl_interfaces::srv::SetParameters::Response> response)
     {
-      std::vector<rclcpp::Parameter> pvariants;
+      // Set parameters one-by-one, since there's no way to return a partial result if
+      // set_parameters() fails.
+      auto result = rcl_interfaces::msg::SetParametersResult();
       for (auto & p : request->parameters) {
-        pvariants.push_back(rclcpp::Parameter::from_parameter_msg(p));
+        try {
+          result = node_params->set_parameters_atomically(
+            {rclcpp::Parameter::from_parameter_msg(p)});
+        } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) {
+          RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Failed to set parameter: %s", ex.what());
+          result.successful = false;
+          result.reason = ex.what();
+        }
+        response->results.push_back(result);
       }
-      auto results = node_params->set_parameters(pvariants);
-      response->results = results;
     },
     qos_profile, nullptr);
 
@@ -96,8 +108,15 @@ ParameterService::ParameterService(
       [](const rcl_interfaces::msg::Parameter & p) {
         return rclcpp::Parameter::from_parameter_msg(p);
       });
-      auto result = node_params->set_parameters_atomically(pvariants);
-      response->result = result;
+      try {
+        auto result = node_params->set_parameters_atomically(pvariants);
+        response->result = result;
+      } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) {
+        RCLCPP_WARN(
+          rclcpp::get_logger("rclcpp"), "Failed to set parameters atomically: %s", ex.what());
+        response->result.successful = false;
+        response->result.reason = "One or more parameters wer not declared before setting";
+      }
     },
     qos_profile, nullptr);
 
@@ -109,8 +128,12 @@ ParameterService::ParameterService(
       const std::shared_ptr<rcl_interfaces::srv::DescribeParameters::Request> request,
       std::shared_ptr<rcl_interfaces::srv::DescribeParameters::Response> response)
     {
-      auto descriptors = node_params->describe_parameters(request->names);
-      response->descriptors = descriptors;
+      try {
+        auto descriptors = node_params->describe_parameters(request->names);
+        response->descriptors = descriptors;
+      } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) {
+        RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Failed to describe parameters: %s", ex.what());
+      }
     },
     qos_profile, nullptr);