Support exception in callback

include/ddynamic_reconfigure/ddynamic_reconfigure.h

@@ -32,6 +32,7 @@
 #define _DDYNAMIC_RECONFIGURE_
 
 #include <dynamic_reconfigure/server.h>
+#include <ddynamic_reconfigure/exception.h>
 #include <ddynamic_reconfigure/registered_param.h>
 #include <ddynamic_reconfigure/ddynamic_reconfigure_utils.h>
 #include <ros/ros.h>
@@ -187,6 +188,9 @@ class DDynamicReconfigure
 
   virtual void updateConfigData(const dynamic_reconfigure::Config &config);
 
+  virtual void callPreUpdateCallback();
+  virtual void callPostUpdateCallback();
+
   /**
    * @brief setUserCallback Set a function to be called when parameters have changed
    */

include/ddynamic_reconfigure/ddynamic_reconfigure_utils.h

@@ -85,13 +85,14 @@ bool assignValue(const std::vector<T> &v, const std::string &name, const V &valu
 }
 
 template <typename T>
-void attemptGetParam(ros::NodeHandle &nh, const std::string &name, T &param, T default_value)
+void attemptGetParam(ros::NodeHandle &nh, const std::string &name, T &param, const T default_value)
 {
   if (nh.hasParam(name))
   {
     nh.param<T>(name, param, default_value);
   }
 }
+
 template <typename T>
 std::pair<T, T> getMinMax(const std::map<std::string, T> &enum_map)
 {

include/ddynamic_reconfigure/exception.h

@@ -0,0 +1,45 @@
+/**
+ * Copyright 2019 PAL Robotics S.L.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * 3. Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived from this
+ * software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef EXCEPTION_H
+#define EXCEPTION_H
+
+#include <stdexcept>
+
+namespace ddynamic_reconfigure
+{
+  class CallbackException : public std::runtime_error
+  {
+   public:
+    explicit CallbackException(const std::string& msg = "") : std::runtime_error(msg) {}
+    ~CallbackException() {}
+  };
+}
+
+#endif

include/ddynamic_reconfigure/registered_param.h

@@ -36,6 +36,8 @@
 #include <type_traits>
 #include <boost/function.hpp>
 #include <dynamic_reconfigure/ParamDescription.h>
+#include <ddynamic_reconfigure/exception.h>
+
 namespace ddynamic_reconfigure
 {
 template <typename T>
@@ -183,9 +185,19 @@ class PointerRegisteredParam : public RegisteredParam<T>
   }
   void updateValue(T new_value) override
   {
-    *variable_ = new_value;
     if (!callback_.empty())
-      callback_(new_value);
+    {
+      try
+      {
+	callback_(new_value);
+      }
+      catch (const CallbackException& e)
+      {
+	ROS_ERROR("Error in callback: %s", e.what());
+	return;
+      }
+    }
+    *variable_ = new_value;
   }
 
 protected:
@@ -216,7 +228,18 @@ class CallbackRegisteredParam : public RegisteredParam<T>
 
   void updateValue(T new_value) override
   {
-    callback_(new_value);
+    if (!callback_.empty())
+    {
+      try
+      {
+	callback_(new_value);
+      }
+      catch (const CallbackException& e)
+      {
+	ROS_ERROR("Error in callback: %s", e.what());
+	return;
+      }
+    }
     current_value_ = new_value;
   }
 

src/ddynamic_reconfigure.cpp

@@ -94,7 +94,22 @@ void DDynamicReconfigure::registerVariable(const std::string &name, T *variable,
                                            const std::string &description, T min, T max,
                                            const std::string &group)
 {
+  const T initial_variable = *variable;
   attemptGetParam(node_handle_, name, *variable, *variable);
+  if (initial_variable != *variable && callback)
+  {
+    callPreUpdateCallback();
+    try
+    {
+      callback(*variable);
+    }
+    catch (const CallbackException &e)
+    {
+      ROS_ERROR("Error in callback: %s", e.what());
+      *variable = initial_variable;
+    }
+    callPostUpdateCallback();
+  }
   getRegisteredVector<T>().push_back(boost::make_unique<PointerRegisteredParam<T>>(
       name, description, min, max, variable, callback, std::map<std::string, T>(), "", group));
 }
@@ -107,9 +122,24 @@ void DDynamicReconfigure::registerEnumVariable(const std::string &name, T *varia
                                                const std::string &enum_description,
                                                const std::string &group)
 {
+  const T initial_variable = *variable;
+  attemptGetParam(node_handle_, name, *variable, *variable);
+  if (initial_variable != *variable && callback)
+  {
+    callPreUpdateCallback();
+    try
+    {
+      callback(*variable);
+    }
+    catch (const CallbackException& e)
+    {
+      ROS_WARN("Error in callback: %s", e.what());
+      *variable = initial_variable;
+    }
+    callPostUpdateCallback();
+  }
   T min, max;
   std::tie(min, max) = getMinMax(enum_dict);
-  attemptGetParam(node_handle_, name, *variable, *variable);
   getRegisteredVector<T>().push_back(boost::make_unique<PointerRegisteredParam<T>>(
       name, description, min, max, variable, callback, enum_dict, enum_description, group));
 }
@@ -120,7 +150,22 @@ void DDynamicReconfigure::registerVariable(const std::string &name, T current_va
                                            const std::string &description, T min, T max,
                                            const std::string &group)
 {
+  const T initial_value = current_value;
   attemptGetParam(node_handle_, name, current_value, current_value);
+  if (initial_value != current_value && callback)
+  {
+    callPreUpdateCallback();
+    try
+    {
+      callback(current_value);
+    }
+    catch (const CallbackException& e)
+    {
+      ROS_ERROR("Error in callback: %s", e.what());
+      current_value = initial_value;
+    }
+    callPostUpdateCallback();
+  }
   getRegisteredVector<T>().push_back(boost::make_unique<CallbackRegisteredParam<T>>(
       name, description, min, max, current_value, callback, std::map<std::string, T>(), "", group));
 }
@@ -134,9 +179,24 @@ void DDynamicReconfigure::registerEnumVariable(const std::string &name, T curren
                                                const std::string &enum_description,
                                                const std::string &group)
 {
+  const T initial_value = current_value;
   T min, max;
   std::tie(min, max) = getMinMax(enum_dict);
   attemptGetParam(node_handle_, name, current_value, current_value);
+  if (initial_value != current_value && callback)
+  {
+    callPreUpdateCallback();
+    try
+    {
+      callback(current_value);
+    }
+    catch (const CallbackException& e)
+    {
+      ROS_ERROR("Error in callback: %s", e.what());
+      current_value = initial_value;
+    }
+    callPostUpdateCallback();
+  }
   getRegisteredVector<T>().push_back(boost::make_unique<CallbackRegisteredParam<T>>(
       name, description, min, max, current_value, callback, enum_dict, enum_description, group));
 }
@@ -190,21 +250,7 @@ bool DDynamicReconfigure::setConfigCallback(dynamic_reconfigure::Reconfigure::Re
 {
   ROS_DEBUG_STREAM("Called config callback of ddynamic_reconfigure");
 
-  if (pre_update_callback_)
-  {
-    try
-    {
-      pre_update_callback_();
-    }
-    catch (std::exception &e)
-    {
-      ROS_WARN("Reconfigure pre update callback failed with exception %s: ", e.what());
-    }
-    catch (...)
-    {
-      ROS_WARN("Reconfigure pre update callback failed with unprintable exception.");
-    }
-  }
+  callPreUpdateCallback();
 
   updated_config_ = req.config;
   if (auto_update_)
@@ -257,21 +303,7 @@ bool DDynamicReconfigure::setConfigCallback(dynamic_reconfigure::Reconfigure::Re
      */
   // std::cerr<<req.config<<std::endl;
 
-  if (post_update_callback_)
-  {
-    try
-    {
-      post_update_callback_();
-    }
-    catch (std::exception &e)
-    {
-      ROS_WARN("Reconfigure post update callback failed with exception %s: ", e.what());
-    }
-    catch (...)
-    {
-      ROS_WARN("Reconfigure post update callback failed with unprintable exception.");
-    }
-  }
+  callPostUpdateCallback();
 
   dynamic_reconfigure::Config config_msg = generateConfig();
   update_pub_.publish(config_msg);
@@ -343,6 +375,44 @@ void DDynamicReconfigure::clearPostUpdateCallback()
   post_update_callback_.clear();
 }
 
+void DDynamicReconfigure::callPreUpdateCallback()
+{
+  if (pre_update_callback_)
+  {
+    try
+    {
+      pre_update_callback_();
+    }
+    catch (const std::exception &e)
+    {
+      ROS_WARN("Reconfigure pre update callback failed with exception %s: ", e.what());
+    }
+    catch (...)
+    {
+      ROS_WARN("Reconfigure pre update callback failed with unprintable exception.");
+    }
+  }
+}
+
+void DDynamicReconfigure::callPostUpdateCallback()
+{
+  if (post_update_callback_)
+  {
+    try
+    {
+      post_update_callback_();
+    }
+    catch (const std::exception &e)
+    {
+      ROS_WARN("Reconfigure post update callback failed with exception %s: ", e.what());
+    }
+    catch (...)
+    {
+      ROS_WARN("Reconfigure post update callback failed with unprintable exception.");
+    }
+  }
+}
+
 void DDynamicReconfigure::RegisterVariable(double *variable, std::string id, double min, double max)
 {
   registerVariable(id, variable, "", min, max);

test/test_ddynamic_reconfigure.cpp

@@ -315,6 +315,37 @@ TEST_F(DDynamicReconfigureTest, threadTest)
   ASSERT_EQ("bool_param", cfg_msg_->bools[0].name);
   ASSERT_EQ(bool_param, cfg_msg_->bools[0].value);
 }
+
+TEST_F(DDynamicReconfigureTest, callbackExceptionTest)
+{
+  ros::NodeHandle nh("~");
+  DDynamicReconfigure dd(nh);
+
+  int int_value = 0;
+
+  dd.registerVariable<int>(
+    "int_param", &int_value,
+    [](int new_value) { if (new_value < 0) { throw CallbackException("test"); } });
+  dd.PublishServicesTopics();
+  ros::AsyncSpinner spinner(1);
+  spinner.start();
+
+  dynamic_reconfigure::Reconfigure srv;
+  dynamic_reconfigure::IntParameter int_param;
+  int_param.name = "int_param";
+  int_param.value = 10;
+  srv.request.config.ints.push_back(int_param);
+
+  // Update request should be accepted because no exception is raised in the callback
+  EXPECT_EQ(int_value, 0);
+  EXPECT_TRUE(ros::service::call(nh.getNamespace() + "/set_parameters", srv));
+  EXPECT_EQ(int_value, 10);
+
+  // Update request should be rejected because exception is raised in the callback
+  int_param.value = -100;
+  EXPECT_TRUE(ros::service::call(nh.getNamespace() + "/set_parameters", srv));
+  EXPECT_EQ(int_value, 10);  // The value is unchanged
+}
 }