Fawkes API Fawkes Development Version
dynamic_reconfigure_thread.cpp
1
2/***************************************************************************
3 * dynamic_reconfigure_thread.cpp - Robotino ROS Dynamic Reconfigure Thread
4 *
5 * Created: Fri May 05 20:07:27 2017
6 * Copyright 2017 Christoph Henke
7 ****************************************************************************/
8
9/* This program is free software; you can redistribute it and/or modify
10 * it under the terms of the GNU General Public License as published by
11 * the Free Software Foundation; either version 2 of the License, or
12 * (at your option) any later version.
13 *
14 * This program is distributed in the hope that it will be useful,
15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17 * GNU Library General Public License for more details.
18 *
19 * Read the full text in the LICENSE.GPL file in the doc directory.
20 */
21
22#include "dynamic_reconfigure_thread.h"
23
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>
30
31using namespace fawkes;
32
33/** @class RosDynamicReconfigureThread "dynamic_reconfigure_thread.h"
34 * Performing dynamic reconfiguration between Fawkes and ROS
35 * @author Christoph Henke
36 */
37
38/** Contructor. */
40: Thread("RosDynamicReconfigureThread", Thread::OPMODE_WAITFORWAKEUP),
42{
43}
44
45/** Method for initialization
46 */
47void
49{
50 try {
51 dynrec_if_ = blackboard->open_for_writing<DynamicReconfigureInterface>("DynamicReconfigure");
52 } catch (Exception &e) {
53 e.append("%s initialization failed, could not open dynamic reconfigure "
54 "interface for writing",
55 name());
56 logger->log_error(name(), e);
57 throw;
58 }
59
60 // Initialize interface
61 dynrec_if_->set_last_service("");
62 dynrec_if_->set_last_parameter("");
63 dynrec_if_->set_last_msg_id(0);
64 dynrec_if_->set_last_bool_value(false);
65 dynrec_if_->set_last_str_value("");
66 dynrec_if_->set_last_uint32_value(0);
67 dynrec_if_->set_last_uint64_value(0);
68 dynrec_if_->set_last_float_value(0.0);
69 dynrec_if_->write();
70}
71
72/** Method for finalization
73 */
74void
76{
77 // close interfaces
78 try {
79 blackboard->close(dynrec_if_);
80 } catch (Exception &e) {
81 logger->log_error(name(), "Closing interface failed!");
82 logger->log_error(name(), e);
83 }
84}
85
86/** Method for setting an dynamic reconfigure of type bool
87 * @param service the service to call for dynamic reconfiguration
88 * @param path path of the ROS parameter
89 * @param value value for the ROS parameter
90 */
91bool
92RosDynamicReconfigureThread::set_dynreconf_value(const std::string &service,
93 const std::string &parameter,
94 const bool value)
95{
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;
100
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;
105
106 if (!ros::service::exists(service, false)) {
108 "Error in setting dynreconf value %s to %s - "
109 "service %s does not exists",
110 parameter.c_str(),
111 value ? "true" : "false",
112 service.c_str());
113 dynrec_if_->set_last_msg_status(fawkes::DynamicReconfigureInterface::LastMsgStatus::Failed);
114 dynrec_if_->write();
115 dynreconf_conf.bools.clear();
116 return false;
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",
120 parameter.c_str(),
121 value ? "true" : "false",
122 service.c_str());
123 dynrec_if_->set_last_msg_status(fawkes::DynamicReconfigureInterface::LastMsgStatus::Failed);
124 dynrec_if_->write();
125 dynreconf_conf.bools.clear();
126 return false;
127 } else {
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);
130 dynrec_if_->write();
131 dynreconf_conf.bools.clear();
132 return true;
133 }
134}
135
136/** Method for setting an dynamic reconfigure of type string
137 * @param service the service to call for dynamic reconfiguration
138 * @param path path of the ROS parameter
139 * @param value value for the ROS parameter
140 */
141bool
142RosDynamicReconfigureThread::set_dynreconf_value(const std::string &service,
143 const std::string &parameter,
144 const std::string &value)
145{
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;
150
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;
155
156 if (!ros::service::exists(service, false)) {
158 "Error in setting dynreconf value %s to %s - "
159 "service %s does not exists",
160 parameter.c_str(),
161 value.c_str(),
162 service.c_str());
163 dynrec_if_->set_last_msg_status(fawkes::DynamicReconfigureInterface::LastMsgStatus::Failed);
164 dynrec_if_->write();
165 dynreconf_conf.strs.clear();
166 return false;
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",
170 parameter.c_str(),
171 value.c_str(),
172 service.c_str());
173 dynrec_if_->set_last_msg_status(fawkes::DynamicReconfigureInterface::LastMsgStatus::Failed);
174 dynrec_if_->write();
175 dynreconf_conf.strs.clear();
176 return false;
177 } else {
178 logger->log_info(name(), "Setting %s to %s", parameter.c_str(), value.c_str());
179 dynrec_if_->set_last_msg_status(fawkes::DynamicReconfigureInterface::LastMsgStatus::Succeeded);
180 dynrec_if_->write();
181 dynreconf_conf.strs.clear();
182 return true;
183 }
184}
185
186/** Method for setting an dynamic reconfigure of type int
187 * @param service the service to call for dynamic reconfiguration
188 * @param path path of the ROS parameter
189 * @param value value for the ROS parameter
190 */
191bool
192RosDynamicReconfigureThread::set_dynreconf_value(const std::string &service,
193 const std::string &parameter,
194 const int value)
195{
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;
200
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;
205
206 if (!ros::service::exists(service, false)) {
208 "Error in setting dynreconf value %s to %d - "
209 "service %s does not exists",
210 parameter.c_str(),
211 value,
212 service.c_str());
213 dynrec_if_->set_last_msg_status(fawkes::DynamicReconfigureInterface::LastMsgStatus::Failed);
214 dynrec_if_->write();
215 dynreconf_conf.ints.clear();
216 return false;
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",
220 parameter.c_str(),
221 value,
222 service.c_str());
223 dynrec_if_->set_last_msg_status(fawkes::DynamicReconfigureInterface::LastMsgStatus::Failed);
224 dynrec_if_->write();
225 dynreconf_conf.ints.clear();
226 return false;
227 } else {
228 logger->log_info(name(), "Setting %s to %d", parameter.c_str(), value);
229 dynrec_if_->set_last_msg_status(fawkes::DynamicReconfigureInterface::LastMsgStatus::Succeeded);
230 dynrec_if_->write();
231 dynreconf_conf.ints.clear();
232 return true;
233 }
234}
235
236/** Method for setting an dynamic reconfigure of type float
237 * @param service the service to call for dynamic reconfiguration
238 * @param path path of the ROS parameter
239 * @param value value for the ROS parameter
240 */
241bool
242RosDynamicReconfigureThread::set_dynreconf_value(const std::string &service,
243 const std::string &parameter,
244 const double value)
245{
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;
250
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;
255
256 if (!ros::service::exists(service, false)) {
258 "Error in setting dynreconf value %s to %f - "
259 "service %s does not exists",
260 parameter.c_str(),
261 value,
262 service.c_str());
263 dynrec_if_->set_last_msg_status(fawkes::DynamicReconfigureInterface::LastMsgStatus::Failed);
264 dynrec_if_->write();
265 dynreconf_conf.doubles.clear();
266 return false;
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",
270 parameter.c_str(),
271 value,
272 service.c_str());
273 dynrec_if_->set_last_msg_status(fawkes::DynamicReconfigureInterface::LastMsgStatus::Failed);
274 dynrec_if_->write();
275 dynreconf_conf.doubles.clear();
276 return false;
277 } else {
278 logger->log_info(name(), "Setting %s to %f", parameter.c_str(), value);
279 dynrec_if_->set_last_msg_status(fawkes::DynamicReconfigureInterface::LastMsgStatus::Succeeded);
280 dynrec_if_->write();
281 dynreconf_conf.doubles.clear();
282 return true;
283 }
284}
285
286/** Method for resetting all interface values
287 */
288void
289RosDynamicReconfigureThread::reset_dynamic_reconfigure_interface()
290{
291 dynrec_if_->set_last_service("");
292 dynrec_if_->set_last_parameter("");
293 dynrec_if_->set_last_msg_id(0);
294 dynrec_if_->set_last_bool_value(false);
295 dynrec_if_->set_last_str_value("");
296 dynrec_if_->set_last_uint32_value(0);
297 dynrec_if_->set_last_uint64_value(0);
298 dynrec_if_->set_last_float_value(0.0);
299}
300
301void
303{
304 while (!dynrec_if_->msgq_empty()) {
306 logger->log_info(name(), "Bool message received");
307 reset_dynamic_reconfigure_interface();
308 // Writing the last called service and the according parameters into the blackboard
309 dynrec_if_->set_last_service(msg->service());
310 dynrec_if_->set_last_parameter(msg->parameter());
311 dynrec_if_->set_last_msg_id(msg->id());
312 dynrec_if_->set_last_bool_value(msg->is_value());
313 dynrec_if_->write();
314 set_dynreconf_value(msg->service(), msg->parameter(), msg->is_value());
316 dynrec_if_->msgq_first_safe(msg)) {
317 logger->log_info(name(), "String message received");
318 reset_dynamic_reconfigure_interface();
319 // Writing the last called service and the according parameters into the blackboard
320 dynrec_if_->set_last_service(msg->service());
321 dynrec_if_->set_last_parameter(msg->parameter());
322 dynrec_if_->set_last_msg_id(msg->id());
323 dynrec_if_->set_last_str_value(msg->value());
324 dynrec_if_->write();
325 set_dynreconf_value(msg->service(), msg->parameter(), msg->value());
327 dynrec_if_->msgq_first_safe(msg)) {
328 logger->log_info(name(), "Uint32 message received");
329 reset_dynamic_reconfigure_interface();
330 // Writing the last called service and the according parameters into the blackboard
331 dynrec_if_->set_last_service(msg->service());
332 dynrec_if_->set_last_parameter(msg->parameter());
333 dynrec_if_->set_last_msg_id(msg->id());
334 dynrec_if_->set_last_uint32_value(msg->value());
335 dynrec_if_->write();
336 set_dynreconf_value(msg->service(), msg->parameter(), (int)msg->value());
338 dynrec_if_->msgq_first_safe(msg)) {
339 logger->log_info(name(), "Uint64 message received");
340 reset_dynamic_reconfigure_interface();
341 // Writing the last called service and the according parameters into the blackboard
342 dynrec_if_->set_last_service(msg->service());
343 dynrec_if_->set_last_parameter(msg->parameter());
344 dynrec_if_->set_last_msg_id(msg->id());
345 dynrec_if_->set_last_uint64_value(msg->value());
346 dynrec_if_->write();
347 set_dynreconf_value(msg->service(), msg->parameter(), (int)msg->value());
349 dynrec_if_->msgq_first_safe(msg)) {
350 logger->log_info(name(), "Float message received");
351 reset_dynamic_reconfigure_interface();
352 // Writing the last called service and the according parameters into the blackboard
353 dynrec_if_->set_last_service(msg->service());
354 dynrec_if_->set_last_parameter(msg->parameter());
355 dynrec_if_->set_last_msg_id(msg->id());
356 dynrec_if_->set_last_float_value(msg->value());
357 dynrec_if_->write();
358 set_dynreconf_value(msg->service(), msg->parameter(), (double)msg->value());
359 }
360
361 dynrec_if_->msgq_pop();
362 } // while
363}
virtual void loop()
Code to execute in the thread.
virtual void finalize()
Method for finalization.
virtual void init()
Method for initialization.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
SetBoolMessage Fawkes BlackBoard Interface Message.
SetFloatMessage Fawkes BlackBoard Interface Message.
SetStringMessage Fawkes BlackBoard Interface Message.
SetUint32Message Fawkes BlackBoard Interface Message.
SetUint64Message Fawkes BlackBoard Interface Message.
DynamicReconfigureInterface Fawkes BlackBoard Interface.
void set_last_uint64_value(const uint64_t new_last_uint64_value)
Set last_uint64_value value.
void set_last_bool_value(const bool new_last_bool_value)
Set last_bool_value value.
void set_last_parameter(const char *new_last_parameter)
Set last_parameter value.
void set_last_msg_status(const LastMsgStatus new_last_msg_status)
Set last_msg_status value.
void set_last_str_value(const char *new_last_str_value)
Set last_str_value value.
void set_last_float_value(const float new_last_float_value)
Set last_float_value value.
void set_last_service(const char *new_last_service)
Set last_service value.
void set_last_msg_id(const uint64_t new_last_msg_id)
Set last_msg_id value.
void set_last_uint32_value(const uint32_t new_last_uint32_value)
Set last_uint32_value value.
Base class for exceptions in Fawkes.
Definition: exception.h:36
void append(const char *format,...) noexcept
Append messages to the message list.
Definition: exception.cpp:333
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1215
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:501
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1062
MessageType * msgq_first_safe(MessageType *&msg) noexcept
Get first message casted to the desired type without exceptions.
Definition: interface.h:340
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
Fawkes library namespace.