22 #include "dynamic_reconfigure_thread.h"
24 #include <dynamic_reconfigure/BoolParameter.h>
25 #include <dynamic_reconfigure/Config.h>
26 #include <dynamic_reconfigure/DoubleParameter.h>
27 #include <dynamic_reconfigure/IntParameter.h>
28 #include <dynamic_reconfigure/Reconfigure.h>
29 #include <dynamic_reconfigure/StrParameter.h>
40 :
Thread(
"RosDynamicReconfigureThread",
Thread::OPMODE_WAITFORWAKEUP),
53 e.
append(
"%s initialization failed, could not open dynamic reconfigure "
54 "interface for writing",
92 RosDynamicReconfigureThread::set_dynreconf_value(
const std::string &service,
93 const std::string ¶meter,
96 dynamic_reconfigure::BoolParameter dynreconf_bool_param;
97 dynamic_reconfigure::Config dynreconf_conf;
98 dynamic_reconfigure::ReconfigureRequest dynreconf_srv_req;
99 dynamic_reconfigure::ReconfigureResponse dynreconf_srv_resp;
101 dynreconf_bool_param.name = parameter;
102 dynreconf_bool_param.value = value;
103 dynreconf_conf.bools.push_back(dynreconf_bool_param);
104 dynreconf_srv_req.config = dynreconf_conf;
106 if (!ros::service::exists(service,
false)) {
108 "Error in setting dynreconf value %s to %s - "
109 "service %s does not exists",
111 value ?
"true" :
"false",
113 dynrec_if_->
set_last_msg_status(fawkes::DynamicReconfigureInterface::LastMsgStatus::Failed);
115 dynreconf_conf.bools.clear();
117 }
else if (!ros::service::call(service, dynreconf_srv_req, dynreconf_srv_resp)) {
119 "Error in setting dynreconf value %s to %s with service %s",
121 value ?
"true" :
"false",
123 dynrec_if_->
set_last_msg_status(fawkes::DynamicReconfigureInterface::LastMsgStatus::Failed);
125 dynreconf_conf.bools.clear();
128 logger->
log_info(
name(),
"Setting %s to %s", parameter.c_str(), value ?
"true" :
"false");
129 dynrec_if_->
set_last_msg_status(fawkes::DynamicReconfigureInterface::LastMsgStatus::Succeeded);
131 dynreconf_conf.bools.clear();
142 RosDynamicReconfigureThread::set_dynreconf_value(
const std::string &service,
143 const std::string ¶meter,
144 const std::string &value)
146 dynamic_reconfigure::StrParameter dynreconf_str_param;
147 dynamic_reconfigure::Config dynreconf_conf;
148 dynamic_reconfigure::ReconfigureRequest dynreconf_srv_req;
149 dynamic_reconfigure::ReconfigureResponse dynreconf_srv_resp;
151 dynreconf_str_param.name = parameter;
152 dynreconf_str_param.value = value;
153 dynreconf_conf.strs.push_back(dynreconf_str_param);
154 dynreconf_srv_req.config = dynreconf_conf;
156 if (!ros::service::exists(service,
false)) {
158 "Error in setting dynreconf value %s to %s - "
159 "service %s does not exists",
163 dynrec_if_->
set_last_msg_status(fawkes::DynamicReconfigureInterface::LastMsgStatus::Failed);
165 dynreconf_conf.strs.clear();
167 }
else if (!ros::service::call(service, dynreconf_srv_req, dynreconf_srv_resp)) {
169 "Error in setting dynreconf value %s to %s with service %s",
173 dynrec_if_->
set_last_msg_status(fawkes::DynamicReconfigureInterface::LastMsgStatus::Failed);
175 dynreconf_conf.strs.clear();
179 dynrec_if_->
set_last_msg_status(fawkes::DynamicReconfigureInterface::LastMsgStatus::Succeeded);
181 dynreconf_conf.strs.clear();
192 RosDynamicReconfigureThread::set_dynreconf_value(
const std::string &service,
193 const std::string ¶meter,
196 dynamic_reconfigure::IntParameter dynreconf_int_param;
197 dynamic_reconfigure::Config dynreconf_conf;
198 dynamic_reconfigure::ReconfigureRequest dynreconf_srv_req;
199 dynamic_reconfigure::ReconfigureResponse dynreconf_srv_resp;
201 dynreconf_int_param.name = parameter;
202 dynreconf_int_param.value = value;
203 dynreconf_conf.ints.push_back(dynreconf_int_param);
204 dynreconf_srv_req.config = dynreconf_conf;
206 if (!ros::service::exists(service,
false)) {
208 "Error in setting dynreconf value %s to %d - "
209 "service %s does not exists",
213 dynrec_if_->
set_last_msg_status(fawkes::DynamicReconfigureInterface::LastMsgStatus::Failed);
215 dynreconf_conf.ints.clear();
217 }
else if (!ros::service::call(service, dynreconf_srv_req, dynreconf_srv_resp)) {
219 "Error in setting dynreconf value %s to %d with service %s",
223 dynrec_if_->
set_last_msg_status(fawkes::DynamicReconfigureInterface::LastMsgStatus::Failed);
225 dynreconf_conf.ints.clear();
229 dynrec_if_->
set_last_msg_status(fawkes::DynamicReconfigureInterface::LastMsgStatus::Succeeded);
231 dynreconf_conf.ints.clear();
242 RosDynamicReconfigureThread::set_dynreconf_value(
const std::string &service,
243 const std::string ¶meter,
246 dynamic_reconfigure::DoubleParameter dynreconf_double_param;
247 dynamic_reconfigure::Config dynreconf_conf;
248 dynamic_reconfigure::ReconfigureRequest dynreconf_srv_req;
249 dynamic_reconfigure::ReconfigureResponse dynreconf_srv_resp;
251 dynreconf_double_param.name = parameter;
252 dynreconf_double_param.value = value;
253 dynreconf_conf.doubles.push_back(dynreconf_double_param);
254 dynreconf_srv_req.config = dynreconf_conf;
256 if (!ros::service::exists(service,
false)) {
258 "Error in setting dynreconf value %s to %f - "
259 "service %s does not exists",
263 dynrec_if_->
set_last_msg_status(fawkes::DynamicReconfigureInterface::LastMsgStatus::Failed);
265 dynreconf_conf.doubles.clear();
267 }
else if (!ros::service::call(service, dynreconf_srv_req, dynreconf_srv_resp)) {
269 "Error in setting dynreconf value %s to %f with service %s",
273 dynrec_if_->
set_last_msg_status(fawkes::DynamicReconfigureInterface::LastMsgStatus::Failed);
275 dynreconf_conf.doubles.clear();
279 dynrec_if_->
set_last_msg_status(fawkes::DynamicReconfigureInterface::LastMsgStatus::Succeeded);
281 dynreconf_conf.doubles.clear();
289 RosDynamicReconfigureThread::reset_dynamic_reconfigure_interface()
307 reset_dynamic_reconfigure_interface();
314 set_dynreconf_value(msg->service(), msg->parameter(), msg->is_value());
318 reset_dynamic_reconfigure_interface();
325 set_dynreconf_value(msg->service(), msg->parameter(), msg->value());
329 reset_dynamic_reconfigure_interface();
336 set_dynreconf_value(msg->service(), msg->parameter(), (
int)msg->value());
340 reset_dynamic_reconfigure_interface();
347 set_dynreconf_value(msg->service(), msg->parameter(), (
int)msg->value());
351 reset_dynamic_reconfigure_interface();
358 set_dynreconf_value(msg->service(), msg->parameter(), (
double)msg->value());