diff --git a/include/ddynamic_reconfigure/ddynamic_reconfigure.h b/include/ddynamic_reconfigure/ddynamic_reconfigure.h index 1822c38..062d0c3 100644 --- a/include/ddynamic_reconfigure/ddynamic_reconfigure.h +++ b/include/ddynamic_reconfigure/ddynamic_reconfigure.h @@ -32,6 +32,7 @@ #define _DDYNAMIC_RECONFIGURE_ #include +#include #include #include #include @@ -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 */ diff --git a/include/ddynamic_reconfigure/ddynamic_reconfigure_utils.h b/include/ddynamic_reconfigure/ddynamic_reconfigure_utils.h index 666524f..d0dec7c 100644 --- a/include/ddynamic_reconfigure/ddynamic_reconfigure_utils.h +++ b/include/ddynamic_reconfigure/ddynamic_reconfigure_utils.h @@ -85,13 +85,14 @@ bool assignValue(const std::vector &v, const std::string &name, const V &valu } template -void attemptGetParam(ros::NodeHandle &nh, const std::string &name, T ¶m, T default_value) +void attemptGetParam(ros::NodeHandle &nh, const std::string &name, T ¶m, const T default_value) { if (nh.hasParam(name)) { nh.param(name, param, default_value); } } + template std::pair getMinMax(const std::map &enum_map) { diff --git a/include/ddynamic_reconfigure/exception.h b/include/ddynamic_reconfigure/exception.h new file mode 100644 index 0000000..11f85b8 --- /dev/null +++ b/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 + +namespace ddynamic_reconfigure +{ + class CallbackException : public std::runtime_error + { + public: + explicit CallbackException(const std::string& msg = "") : std::runtime_error(msg) {} + ~CallbackException() {} + }; +} + +#endif diff --git a/include/ddynamic_reconfigure/registered_param.h b/include/ddynamic_reconfigure/registered_param.h index a469395..c5b1384 100644 --- a/include/ddynamic_reconfigure/registered_param.h +++ b/include/ddynamic_reconfigure/registered_param.h @@ -36,6 +36,8 @@ #include #include #include +#include + namespace ddynamic_reconfigure { template @@ -183,9 +185,19 @@ class PointerRegisteredParam : public RegisteredParam } 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 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; } diff --git a/src/ddynamic_reconfigure.cpp b/src/ddynamic_reconfigure.cpp index d46644b..d510b05 100644 --- a/src/ddynamic_reconfigure.cpp +++ b/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().push_back(boost::make_unique>( name, description, min, max, variable, callback, std::map(), "", 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().push_back(boost::make_unique>( 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().push_back(boost::make_unique>( name, description, min, max, current_value, callback, std::map(), "", 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().push_back(boost::make_unique>( 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<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_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 +} }