Go to the documentation of this file.00001 #ifdef __clang__
00002 #pragma clang diagnostic push
00003 #pragma ide diagnostic ignored "OCDFAInspection"
00004 #pragma clang diagnostic ignored "-Wunknown-pragmas"
00005 #pragma ide diagnostic ignored "modernize-loop-convert"
00006 #pragma ide diagnostic ignored "modernize-use-auto"
00007 #endif
00008
00009
00010
00011 #include <ddynamic_reconfigure/ddynamic_reconfigure.h>
00012 #include <boost/foreach.hpp>
00013
00014 using namespace boost;
00015 namespace ddynamic_reconfigure {
00016
00017 DDynamicReconfigure::DDynamicReconfigure(ros::NodeHandle &nh) {
00018 nh_ = nh;
00019 };
00020
00021 void DDynamicReconfigure::add(DDPtr param) {
00022 params_[param->getName()] = param;
00023 };
00024
00025 void DDynamicReconfigure::add(DDParam *param) {
00026 add(DDPtr(param));
00027 };
00028
00029 void DDynamicReconfigure::remove(DDPtr param) {
00030 remove(param->getName());
00031 };
00032
00033 void DDynamicReconfigure::remove(DDParam *param) {
00034 remove(param->getName());
00035 };
00036
00037 void DDynamicReconfigure::remove(string param_name) {
00038 params_.erase(param_name);
00039 };
00040
00041 void DDynamicReconfigure::start() {
00042 ConfigDescription conf_desc = makeDescription();
00043 Config conf = makeConfig();
00044
00045 function<bool(Reconfigure::Request& req, Reconfigure::Response& rsp)> callback = bind(&internalCallback,this,_1,_2);
00046
00047 set_service_ = nh_.advertiseService("set_parameters", callback);
00048
00049
00050 desc_pub_ = nh_.advertise<ConfigDescription>("parameter_descriptions", 1, true);
00051 desc_pub_.publish(conf_desc);
00052
00053
00054 update_pub_ = nh_.advertise<Config>("parameter_updates", 1, true);
00055 update_pub_.publish(conf);
00056 }
00057
00058 Config DDynamicReconfigure::makeConfig() {
00059 Config conf;
00060 GroupState group_state;
00061
00062
00063 group_state.name = "Default";
00064 group_state.state = (unsigned char)true;
00065
00066
00067 conf.groups.push_back(group_state);
00068 for(DDMap::const_iterator it = params_.begin(); it != params_.end(); it++) {it->second->prepConfig(conf);}
00069 return conf;
00070 }
00071
00072 ConfigDescription DDynamicReconfigure::makeDescription() {
00073 ConfigDescription conf_desc;
00074 Group group;
00075
00076
00077 group.name = "default";
00078 for(DDMap::const_iterator it = params_.begin(); it != params_.end(); it++) {it->second->prepGroup(group);}
00079
00080
00081 for(DDMap::const_iterator it = params_.begin(); it != params_.end(); it++) {it->second->prepConfigDescription(conf_desc);}
00082 conf_desc.groups.push_back(group);
00083 return conf_desc;
00084 };
00085
00086 void DDynamicReconfigure::start(DDFunc callback) {
00087 start();
00088 setCallback(callback);
00089 };
00090
00091 void DDynamicReconfigure::start(void(*callback)(const DDMap&,int)) {
00092 DDFunc f(callback);
00093 start(f);
00094 };
00095
00096
00097
00098
00099
00100
00101
00102 void DDynamicReconfigure::setCallback(DDFunc callback) {
00103 callback_ = make_shared<function<void(const DDMap&,int)> >(callback);
00104 };
00105
00106 void defaultCallback(const DDMap&,int) {};
00107
00108 void DDynamicReconfigure::clearCallback() {
00109 callback_ = make_shared<DDFunc>(&defaultCallback);
00110 };
00111
00112
00113 bool DDynamicReconfigure::internalCallback(DDynamicReconfigure *obj, Reconfigure::Request& req, Reconfigure::Response& rsp) {
00114 ROS_DEBUG_STREAM("Called config callback of ddynamic_reconfigure");
00115
00116 int level = obj->getUpdates(req, obj->params_);
00117
00118 if (obj->callback_) {
00119 try {
00120 (*obj->callback_)(obj->params_,level);
00121 } catch (std::exception &e) {
00122 ROS_WARN("Reconfigure callback failed with exception %s: ", e.what());
00123 } catch (...) {
00124 ROS_WARN("Reconfigure callback failed with unprintable exception.");
00125 }
00126 }
00127
00128 obj->update_pub_.publish(obj->makeConfig());
00129
00130 return true;
00131 }
00132
00133 int DDynamicReconfigure::getUpdates(const Reconfigure::Request &req, DDMap &config) {
00134 int level = 0;
00135
00136 BOOST_FOREACH(const IntParameter i,req.config.ints) {
00137 int new_level = reassign(config, i.name, i.value);
00138 if(new_level == -1) {
00139 ROS_ERROR_STREAM("Variable [" << i.name << "] is not registered");
00140 } else {
00141 level |= new_level;
00142 }
00143 }
00144 BOOST_FOREACH(const DoubleParameter i,req.config.doubles) {
00145 int new_level = reassign(config, i.name, i.value);
00146 if(new_level == -1) {
00147 ROS_ERROR_STREAM("Variable [" << i.name << "] is not registered");
00148 } else {
00149 level |= new_level;
00150 }
00151 }
00152 BOOST_FOREACH(const BoolParameter i,req.config.bools) {
00153 int new_level = reassign(config, i.name, (bool)i.value);
00154 if(new_level == -1) {
00155 ROS_ERROR_STREAM("Variable [" << i.name << "] is not registered");
00156 } else {
00157 level |= new_level;
00158 }
00159 }
00160 BOOST_FOREACH(const StrParameter i,req.config.strs) {
00161 int new_level = reassign(config, i.name, i.value);
00162 if(new_level == -1) {
00163 ROS_ERROR_STREAM("Variable [" << i.name << "] is not registered");
00164 } else {
00165 level |= new_level;
00166 }
00167 }
00168 return level;
00169 }
00170
00171 template <class T>
00172 int DDynamicReconfigure::reassign(DDMap& map, const string &name, T value) {
00173 Value val(value);
00174 if(map.find(name) != map.end() && map[name]->sameType(val)) {
00175 DDPtr old = map[name];
00176 if(old->sameValue(val)) { return 0;} else {
00177 old->setValue(val);
00178 return old->getLevel();
00179 }
00180 } else {
00181 return -1;
00182 }
00183 }
00184
00185 Value DDynamicReconfigure::get(const char *name) {
00186 return ddynamic_reconfigure::get(params_,name);
00187 }
00188
00189 DDPtr DDynamicReconfigure::at(const char *name) {
00190 return ddynamic_reconfigure::at(params_,name);
00191 }
00192
00193 ostream &operator<<(ostream &os, const DDynamicReconfigure &dd) {
00194 os << "{" << *dd.params_.begin()->second;
00195 for(DDMap::const_iterator it = ++dd.params_.begin(); it != dd.params_.end(); it++) {
00196 os << "," << *it->second;
00197 }
00198 os << "}";
00199 return os;
00200 }
00201
00202 DDPtr at(const DDMap& map, const char *name) {
00203 DDMap::const_iterator it = map.find(name);
00204 if(it == map.end()) {
00205 return DDPtr();
00206 } else { return it->second;}
00207 }
00208
00209 Value get(const DDMap& map, const char *name) {
00210 DDMap::const_iterator it = map.find(name);
00211 if(it == map.end()) {
00212 return Value("\000");
00213 } else { return it->second->getValue();}
00214 }
00215 }
00216 #ifdef __clang__
00217 #pragma clang diagnostic pop
00218 #endif