Modulo 5.0.0
All Classes Namespaces Functions Variables Enumerations Pages Concepts
BaseControllerInterface.cpp
1#include "modulo_controllers/BaseControllerInterface.hpp"
2
3#include <chrono>
4
5#include <lifecycle_msgs/msg/state.hpp>
6
7#include <modulo_core/translators/message_readers.hpp>
8
9template<class... Ts>
10struct overloaded : Ts... {
11 using Ts::operator()...;
12};
13template<class... Ts>
14overloaded(Ts...) -> overloaded<Ts...>;
15
16using namespace modulo_core;
17using namespace state_representation;
18using namespace std::chrono_literals;
19
20namespace modulo_controllers {
21
23 : controller_interface::ControllerInterface(), input_validity_period_(std::numeric_limits<double>::quiet_NaN()) {}
24
25rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn BaseControllerInterface::on_init() {
26 // registering set_parameter callbacks is only possible on_init since the lifecycle node is not yet initialized
27 // on construction. This means we might not be able to validate parameter overrides - if they are provided.
28 pre_set_parameter_cb_handle_ = get_node()->add_pre_set_parameters_callback(
29 [this](std::vector<rclcpp::Parameter>& parameters) { return this->pre_set_parameters_callback(parameters); });
30 on_set_parameter_cb_handle_ = get_node()->add_on_set_parameters_callback(
31 [this](const std::vector<rclcpp::Parameter>& parameters) -> rcl_interfaces::msg::SetParametersResult {
32 return this->on_set_parameters_callback(parameters);
33 });
34 add_parameter<double>("predicate_publishing_rate", 10.0, "The rate at which to publish controller predicates");
36 "input_validity_period", 1.0, "The maximum age of an input state before discarding it as expired");
37 return CallbackReturn::SUCCESS;
38}
39
40rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
41BaseControllerInterface::on_configure(const rclcpp_lifecycle::State&) {
42 input_validity_period_ = get_parameter_value<double>("input_validity_period");
43 add_inputs();
44 add_outputs();
45
46 if (predicates_.size()) {
47 predicate_publisher_ =
48 get_node()->create_publisher<modulo_interfaces::msg::PredicateCollection>("/predicates", qos_);
49 predicate_message_.node = get_node()->get_fully_qualified_name();
50 predicate_message_.type = modulo_interfaces::msg::PredicateCollection::CONTROLLER;
51
52 predicate_timer_ = get_node()->create_wall_timer(
53 std::chrono::nanoseconds(static_cast<int64_t>(1e9 / get_parameter_value<double>("predicate_publishing_rate"))),
54 [this]() { this->publish_predicates(); });
55 }
56
57 RCLCPP_DEBUG(get_node()->get_logger(), "Configuration of BaseControllerInterface successful");
58 return CallbackReturn::SUCCESS;
59}
60
62 const std::shared_ptr<ParameterInterface>& parameter, const std::string& description, bool read_only) {
63 rclcpp::Parameter ros_param;
64 try {
65 ros_param = translators::write_parameter(parameter);
67 throw modulo_core::exceptions::ParameterException("Failed to add parameter: " + std::string(ex.what()));
68 }
69 if (!get_node()->has_parameter(parameter->get_name())) {
70 RCLCPP_DEBUG(get_node()->get_logger(), "Adding parameter '%s'.", parameter->get_name().c_str());
71 parameter_map_.set_parameter(parameter);
72 read_only_parameters_.insert_or_assign(parameter->get_name(), false);
73 try {
74 rcl_interfaces::msg::ParameterDescriptor descriptor;
75 descriptor.description = description;
76 descriptor.read_only = read_only;
77 // since the pre_set_parameters_callback is not called on parameter declaration, this has to be true
78 set_parameters_result_.successful = true;
79 set_parameters_result_.reason = "";
80 if (parameter->is_empty()) {
81 descriptor.dynamic_typing = true;
82 descriptor.type = translators::get_ros_parameter_type(parameter->get_parameter_type());
83 get_node()->declare_parameter(parameter->get_name(), rclcpp::ParameterValue{}, descriptor);
84 } else {
85 get_node()->declare_parameter(parameter->get_name(), ros_param.get_parameter_value(), descriptor);
86 }
87 std::vector<rclcpp::Parameter> ros_parameters{get_node()->get_parameters({parameter->get_name()})};
88 pre_set_parameters_callback(ros_parameters);
89 auto result = on_set_parameters_callback(ros_parameters);
90 if (!result.successful) {
91 get_node()->undeclare_parameter(parameter->get_name());
93 }
94 read_only_parameters_.at(parameter->get_name()) = read_only;
95 } catch (const std::exception& ex) {
96 parameter_map_.remove_parameter(parameter->get_name());
97 read_only_parameters_.erase(parameter->get_name());
98 throw modulo_core::exceptions::ParameterException("Failed to add parameter: " + std::string(ex.what()));
99 }
100 } else {
101 RCLCPP_DEBUG(get_node()->get_logger(), "Parameter '%s' already exists.", parameter->get_name().c_str());
102 }
103}
104
105std::shared_ptr<ParameterInterface> BaseControllerInterface::get_parameter(const std::string& name) const {
106 try {
107 return parameter_map_.get_parameter(name);
108 } catch (const state_representation::exceptions::InvalidParameterException& ex) {
109 throw modulo_core::exceptions::ParameterException("Failed to get parameter '" + name + "': " + ex.what());
110 }
111}
112
113void BaseControllerInterface::pre_set_parameters_callback(std::vector<rclcpp::Parameter>& parameters) {
114 if (pre_set_parameter_callback_called_) {
115 pre_set_parameter_callback_called_ = false;
116 return;
117 }
118 rcl_interfaces::msg::SetParametersResult result;
119 result.successful = true;
120 for (auto& ros_parameter : parameters) {
121 try {
122 auto parameter = parameter_map_.get_parameter(ros_parameter.get_name());
123 if (read_only_parameters_.at(ros_parameter.get_name())) {
124 RCLCPP_DEBUG(get_node()->get_logger(), "Parameter '%s' is read only.", ros_parameter.get_name().c_str());
125 continue;
126 }
127
128 // convert the ROS parameter into a ParameterInterface without modifying the original
129 auto new_parameter = translators::read_parameter_const(ros_parameter, parameter);
130 if (!this->validate_parameter(new_parameter)) {
131 result.successful = false;
132 result.reason += "Validation of parameter '" + ros_parameter.get_name() + "' returned false!";
133 } else if (!new_parameter->is_empty()) {
134 // update the value of the parameter in the map
135 translators::copy_parameter_value(new_parameter, parameter);
136 ros_parameter = translators::write_parameter(new_parameter);
137 }
138 } catch (const std::exception& ex) {
139 result.successful = false;
140 result.reason += ex.what();
141 }
142 }
143 set_parameters_result_ = result;
144}
145
146rcl_interfaces::msg::SetParametersResult
147BaseControllerInterface::on_set_parameters_callback(const std::vector<rclcpp::Parameter>&) {
148 auto result = set_parameters_result_;
149 set_parameters_result_.successful = true;
150 set_parameters_result_.reason = "";
151 return result;
152}
153
154bool BaseControllerInterface::validate_parameter(const std::shared_ptr<ParameterInterface>& parameter) {
155 if (parameter->get_name() == "predicate_publishing_rate" || parameter->get_name() == "input_validity_period") {
156 auto value = parameter->get_parameter_value<double>();
157 if (value < 0.0 || value > std::numeric_limits<double>::max()) {
158 RCLCPP_ERROR(
159 get_node()->get_logger(), "Parameter value of parameter '%s' should be a positive finite number",
160 parameter->get_name().c_str());
161 return false;
162 }
163 }
164 return on_validate_parameter_callback(parameter);
165}
166
167bool BaseControllerInterface::on_validate_parameter_callback(const std::shared_ptr<ParameterInterface>&) {
168 return true;
169}
170
171void BaseControllerInterface::add_predicate(const std::string& predicate_name, bool predicate_value) {
172 add_predicate(predicate_name, [predicate_value]() { return predicate_value; });
173}
174
176 const std::string& predicate_name, const std::function<bool(void)>& predicate_function) {
177 if (predicate_name.empty()) {
178 RCLCPP_ERROR(get_node()->get_logger(), "Failed to add predicate: Provide a non empty string as a name.");
179 return;
180 }
181 if (predicates_.find(predicate_name) != predicates_.end()) {
182 RCLCPP_WARN(
183 get_node()->get_logger(), "Predicate with name '%s' already exists, overwriting.", predicate_name.c_str());
184 } else {
185 RCLCPP_DEBUG(get_node()->get_logger(), "Adding predicate '%s'.", predicate_name.c_str());
186 }
187 try {
188 this->predicates_.insert_or_assign(predicate_name, Predicate(predicate_function));
189 } catch (const std::exception& ex) {
190 RCLCPP_ERROR(
191 get_node()->get_logger(), "Failed to evaluate callback of predicate '%s', returning false: %s",
192 predicate_name.c_str(), ex.what());
193 }
194}
195
196bool BaseControllerInterface::get_predicate(const std::string& predicate_name) const {
197 auto predicate_it = predicates_.find(predicate_name);
198 if (predicate_it == predicates_.end()) {
199 RCLCPP_ERROR_THROTTLE(
200 get_node()->get_logger(), *get_node()->get_clock(), 1000,
201 "Failed to get predicate '%s': Predicate does not exists, returning false.", predicate_name.c_str());
202 return false;
203 }
204 try {
205 return predicate_it->second.get_value();
206 } catch (const std::exception& ex) {
207 RCLCPP_ERROR_THROTTLE(
208 get_node()->get_logger(), *get_node()->get_clock(), 1000,
209 "Failed to evaluate callback of predicate '%s', returning false: %s", predicate_name.c_str(), ex.what());
210 }
211 return false;
212}
213
214void BaseControllerInterface::set_predicate(const std::string& predicate_name, bool predicate_value) {
215 set_predicate(predicate_name, [predicate_value]() { return predicate_value; });
216}
217
219 const std::string& predicate_name, const std::function<bool(void)>& predicate_function) {
220 auto predicate_it = predicates_.find(predicate_name);
221 if (predicate_it == predicates_.end()) {
222 RCLCPP_ERROR_THROTTLE(
223 get_node()->get_logger(), *get_node()->get_clock(), 1000,
224 "Failed to set predicate '%s': Predicate does not exist.", predicate_name.c_str());
225 return;
226 }
227 predicate_it->second.set_predicate(predicate_function);
228 if (auto new_predicate = predicate_it->second.query(); new_predicate) {
229 publish_predicate(predicate_name, *new_predicate);
230 }
231}
232
233void BaseControllerInterface::add_trigger(const std::string& trigger_name) {
234 if (trigger_name.empty()) {
235 RCLCPP_ERROR(get_node()->get_logger(), "Failed to add trigger: Provide a non empty string as a name.");
236 return;
237 }
238 if (std::find(triggers_.cbegin(), triggers_.cend(), trigger_name) != triggers_.cend()) {
239 RCLCPP_ERROR(
240 get_node()->get_logger(), "Failed to add trigger: there is already a trigger with name '%s'.",
241 trigger_name.c_str());
242 return;
243 }
244 if (predicates_.find(trigger_name) != predicates_.end()) {
245 RCLCPP_ERROR(
246 get_node()->get_logger(), "Failed to add trigger: there is already a predicate with name '%s'.",
247 trigger_name.c_str());
248 return;
249 }
250 triggers_.push_back(trigger_name);
251 add_predicate(trigger_name, false);
252}
253
254void BaseControllerInterface::trigger(const std::string& trigger_name) {
255 if (std::find(triggers_.cbegin(), triggers_.cend(), trigger_name) == triggers_.cend()) {
256 RCLCPP_ERROR(
257 get_node()->get_logger(), "Failed to trigger: could not find trigger with name '%s'.", trigger_name.c_str());
258 return;
259 }
260 set_predicate(trigger_name, true);
261 // reset the trigger to be published on the next step
262 predicates_.at(trigger_name).set_predicate([]() { return false; });
263}
264
265modulo_interfaces::msg::Predicate
266BaseControllerInterface::get_predicate_message(const std::string& name, bool value) const {
267 modulo_interfaces::msg::Predicate message;
268 message.predicate = name;
269 message.value = value;
270 return message;
271}
272
273void BaseControllerInterface::publish_predicate(const std::string& predicate_name, bool value) const {
274 auto message(predicate_message_);
275 message.predicates.push_back(get_predicate_message(predicate_name, value));
276 predicate_publisher_->publish(message);
277}
278
279void BaseControllerInterface::publish_predicates() {
280 auto message(predicate_message_);
281 for (auto predicate_it = predicates_.begin(); predicate_it != predicates_.end(); ++predicate_it) {
282 if (auto new_predicate = predicate_it->second.query(); new_predicate) {
283 message.predicates.push_back(get_predicate_message(predicate_it->first, *new_predicate));
284 }
285 }
286 if (message.predicates.size()) {
287 predicate_publisher_->publish(message);
288 }
289}
290
291std::string BaseControllerInterface::validate_and_declare_signal(
292 const std::string& signal_name, const std::string& type, const std::string& default_topic, bool fixed_topic) {
293 auto parsed_signal_name = modulo_utils::parsing::parse_topic_name(signal_name);
294 if (parsed_signal_name.empty()) {
295 RCLCPP_WARN(
296 get_node()->get_logger(),
297 "The parsed signal name for %s '%s' is empty. Provide a string with valid characters for the signal name "
298 "([a-zA-Z0-9_]).",
299 type.c_str(), signal_name.c_str());
300 return "";
301 }
302 if (signal_name != parsed_signal_name) {
303 RCLCPP_WARN(
304 get_node()->get_logger(),
305 "The parsed signal name for %s '%s' is '%s'. Use the parsed signal name to refer to this %s and its topic "
306 "parameter.",
307 type.c_str(), signal_name.c_str(), parsed_signal_name.c_str(), type.c_str());
308 }
309 if (inputs_.find(parsed_signal_name) != inputs_.end()) {
310 RCLCPP_WARN(get_node()->get_logger(), "Signal '%s' already exists as input.", parsed_signal_name.c_str());
311 return "";
312 }
313 if (outputs_.find(parsed_signal_name) != outputs_.end()) {
314 RCLCPP_WARN(get_node()->get_logger(), "Signal '%s' already exists as output", parsed_signal_name.c_str());
315 return "";
316 }
317 auto topic = default_topic.empty() ? "~/" + parsed_signal_name : default_topic;
318 auto parameter_name = parsed_signal_name + "_topic";
319 if (get_node()->has_parameter(parameter_name) && get_parameter(parameter_name)->is_empty()) {
320 set_parameter_value<std::string>(parameter_name, topic);
321 } else {
323 parameter_name, topic, "Signal topic name of " + type + " '" + parsed_signal_name + "'", fixed_topic);
324 }
325 RCLCPP_DEBUG(
326 get_node()->get_logger(), "Declared %s '%s' and parameter '%s' with value '%s'.", type.c_str(),
327 parsed_signal_name.c_str(), parameter_name.c_str(), topic.c_str());
328 return parsed_signal_name;
329}
330
331void BaseControllerInterface::create_input(
332 const ControllerInput& input, const std::string& name, const std::string& topic_name) {
333 auto parsed_name = validate_and_declare_signal(name, "input", topic_name);
334 if (!parsed_name.empty()) {
335 inputs_.insert_or_assign(parsed_name, input);
336 }
337}
338
339void BaseControllerInterface::add_inputs() {
340 for (auto& [name, input] : inputs_) {
341 try {
342 auto topic = get_parameter_value<std::string>(name + "_topic");
343 std::visit(
344 overloaded{
345 [&](const realtime_tools::RealtimeBuffer<std::shared_ptr<EncodedState>>&) {
346 subscriptions_.push_back(create_subscription<EncodedState>(name, topic));
347 },
348 [&](const realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Bool>>&) {
349 subscriptions_.push_back(create_subscription<std_msgs::msg::Bool>(name, topic));
350 },
351 [&](const realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Float64>>&) {
352 subscriptions_.push_back(create_subscription<std_msgs::msg::Float64>(name, topic));
353 },
354 [&](const realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Float64MultiArray>>&) {
355 subscriptions_.push_back(create_subscription<std_msgs::msg::Float64MultiArray>(name, topic));
356 },
357 [&](const realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Int32>>&) {
358 subscriptions_.push_back(create_subscription<std_msgs::msg::Int32>(name, topic));
359 },
360 [&](const realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::String>>&) {
361 subscriptions_.push_back(create_subscription<std_msgs::msg::String>(name, topic));
362 },
363 [&](const std::any&) {
364 custom_input_configuration_callables_.at(name)(name, topic);
365 }},
366 input.buffer);
367 } catch (const std::exception& ex) {
368 RCLCPP_ERROR(get_node()->get_logger(), "Failed to add input '%s': %s", name.c_str(), ex.what());
369 }
370 }
371}
372
373void BaseControllerInterface::create_output(
374 const PublisherVariant& publishers, const std::string& name, const std::string& topic_name) {
375 auto parsed_name = validate_and_declare_signal(name, "output", topic_name);
376 if (!parsed_name.empty()) {
377 outputs_.insert_or_assign(parsed_name, publishers);
378 }
379}
380
381void BaseControllerInterface::add_outputs() {
382 for (auto& [name, publishers] : outputs_) {
383 try {
384 auto topic = get_parameter_value<std::string>(name + "_topic");
385 std::visit(
386 overloaded{
387 [&](EncodedStatePublishers& pub) {
388 std::get<1>(pub) = get_node()->create_publisher<EncodedState>(topic, qos_);
389 std::get<2>(pub) = std::make_shared<realtime_tools::RealtimePublisher<EncodedState>>(std::get<1>(pub));
390 },
391 [&](BoolPublishers& pub) {
392 pub.first = get_node()->create_publisher<std_msgs::msg::Bool>(topic, qos_);
393 pub.second = std::make_shared<realtime_tools::RealtimePublisher<std_msgs::msg::Bool>>(pub.first);
394 },
395 [&](DoublePublishers& pub) {
396 pub.first = get_node()->create_publisher<std_msgs::msg::Float64>(topic, qos_);
397 pub.second = std::make_shared<realtime_tools::RealtimePublisher<std_msgs::msg::Float64>>(pub.first);
398 },
399 [&](DoubleVecPublishers& pub) {
400 pub.first = get_node()->create_publisher<std_msgs::msg::Float64MultiArray>(topic, qos_);
401 pub.second =
402 std::make_shared<realtime_tools::RealtimePublisher<std_msgs::msg::Float64MultiArray>>(pub.first);
403 },
404 [&](IntPublishers& pub) {
405 pub.first = get_node()->create_publisher<std_msgs::msg::Int32>(topic, qos_);
406 pub.second = std::make_shared<realtime_tools::RealtimePublisher<std_msgs::msg::Int32>>(pub.first);
407 },
408 [&](StringPublishers& pub) {
409 pub.first = get_node()->create_publisher<std_msgs::msg::String>(topic, qos_);
410 pub.second = std::make_shared<realtime_tools::RealtimePublisher<std_msgs::msg::String>>(pub.first);
411 },
412 [&](CustomPublishers& pub) {
413 custom_output_configuration_callables_.at(name)(pub, name);
414 }},
415 publishers);
416 } catch (const std::bad_any_cast& ex) {
417 RCLCPP_ERROR(get_node()->get_logger(), "Failed to add custom output '%s': %s", name.c_str(), ex.what());
418 } catch (const std::exception& ex) {
419 RCLCPP_ERROR(get_node()->get_logger(), "Failed to add output '%s': %s", name.c_str(), ex.what());
420 }
421 }
422}
423
424void BaseControllerInterface::set_input_validity_period(double input_validity_period) {
425 input_validity_period_ = input_validity_period;
426}
427
428bool BaseControllerInterface::check_input_valid(const std::string& name) const {
429 if (inputs_.find(name) == inputs_.end()) {
430 RCLCPP_WARN_THROTTLE(
431 get_node()->get_logger(), *get_node()->get_clock(), 1000, "Could not find input '%s'", name.c_str());
432 return false;
433 }
434 if (static_cast<double>(std::chrono::duration_cast<std::chrono::nanoseconds>(
435 std::chrono::steady_clock::now() - inputs_.at(name).timestamp)
436 .count())
437 / 1e9
438 >= input_validity_period_) {
439 return false;
440 }
441 return true;
442}
443
444std::string
445BaseControllerInterface::validate_service_name(const std::string& service_name, const std::string& type) const {
446 std::string parsed_service_name = modulo_utils::parsing::parse_topic_name(service_name);
447 if (parsed_service_name.empty()) {
448 RCLCPP_WARN(
449 get_node()->get_logger(),
450 "The parsed service name for %s service '%s' is empty. Provide a string with valid characters for the service "
451 "name "
452 "([a-zA-Z0-9_]).",
453 type.c_str(), service_name.c_str());
454 return "";
455 }
456 if (service_name != parsed_service_name) {
457 RCLCPP_WARN(
458 get_node()->get_logger(),
459 "The parsed name for '%s' service '%s' is '%s'. Use the parsed name to refer to this service.", type.c_str(),
460 service_name.c_str(), parsed_service_name.c_str());
461 }
462 if (empty_services_.find(parsed_service_name) != empty_services_.cend()) {
463 RCLCPP_WARN(
464 get_node()->get_logger(), "Service with name '%s' already exists as empty service.",
465 parsed_service_name.c_str());
466 return "";
467 }
468 if (string_services_.find(parsed_service_name) != string_services_.cend()) {
469 RCLCPP_WARN(
470 get_node()->get_logger(), "Service with name '%s' already exists as string service.",
471 parsed_service_name.c_str());
472 return "";
473 }
474 RCLCPP_DEBUG(get_node()->get_logger(), "Adding %s service '%s'.", type.c_str(), parsed_service_name.c_str());
475 return parsed_service_name;
476}
477
479 const std::string& service_name, const std::function<ControllerServiceResponse(void)>& callback) {
480 auto parsed_service_name = validate_service_name(service_name, "empty");
481 if (!parsed_service_name.empty()) {
482 try {
483 auto service = get_node()->create_service<modulo_interfaces::srv::EmptyTrigger>(
484 "~/" + parsed_service_name,
485 [this, callback](
486 const std::shared_ptr<modulo_interfaces::srv::EmptyTrigger::Request>,
487 std::shared_ptr<modulo_interfaces::srv::EmptyTrigger::Response> response) {
488 try {
489 if (this->command_mutex_.try_lock_for(100ms)) {
490 auto callback_response = callback();
491 this->command_mutex_.unlock();
492 response->success = callback_response.success;
493 response->message = callback_response.message;
494 } else {
495 response->success = false;
496 response->message = "Unable to acquire lock for command interface within 100ms";
497 }
498 } catch (const std::exception& ex) {
499 response->success = false;
500 response->message = ex.what();
501 }
502 },
503 qos_);
504 empty_services_.insert_or_assign(parsed_service_name, service);
505 } catch (const std::exception& ex) {
506 RCLCPP_ERROR(get_node()->get_logger(), "Failed to add service '%s': %s", parsed_service_name.c_str(), ex.what());
507 }
508 }
509}
510
512 const std::string& service_name,
513 const std::function<ControllerServiceResponse(const std::string& string)>& callback) {
514 auto parsed_service_name = validate_service_name(service_name, "string");
515 if (!parsed_service_name.empty()) {
516 try {
517 auto service = get_node()->create_service<modulo_interfaces::srv::StringTrigger>(
518 "~/" + parsed_service_name,
519 [this, callback](
520 const std::shared_ptr<modulo_interfaces::srv::StringTrigger::Request> request,
521 std::shared_ptr<modulo_interfaces::srv::StringTrigger::Response> response) {
522 try {
523 if (this->command_mutex_.try_lock_for(100ms)) {
524 auto callback_response = callback(request->payload);
525 this->command_mutex_.unlock();
526 response->success = callback_response.success;
527 response->message = callback_response.message;
528 } else {
529 response->success = false;
530 response->message = "Unable to acquire lock for command interface within 100ms";
531 }
532 } catch (const std::exception& ex) {
533 response->success = false;
534 response->message = ex.what();
535 }
536 },
537 qos_);
538 string_services_.insert_or_assign(parsed_service_name, service);
539 } catch (const std::exception& ex) {
540 RCLCPP_ERROR(get_node()->get_logger(), "Failed to add service '%s': %s", parsed_service_name.c_str(), ex.what());
541 }
542 }
543}
544
546 return qos_;
547}
548
549void BaseControllerInterface::set_qos(const rclcpp::QoS& qos) {
550 qos_ = qos;
551}
552
554 return get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE;
555}
556
558 return command_mutex_;
559}
560
561}// namespace modulo_controllers
std::shared_ptr< state_representation::ParameterInterface > get_parameter(const std::string &name) const
Get a parameter by name.
CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state) override
Add signals.
rclcpp::QoS get_qos() const
Getter of the Quality of Service attribute.
bool is_active() const
Check if the controller is currently in state active or not.
void set_parameter_value(const std::string &name, const T &value)
Set the value of a parameter.
T get_parameter_value(const std::string &name) const
Get a parameter value by name.
void set_input_validity_period(double input_validity_period)
Set the input validity period of input signals.
CallbackReturn on_init() override
Declare parameters and register the on_set_parameters callback.
void add_parameter(const std::shared_ptr< state_representation::ParameterInterface > &parameter, const std::string &description, bool read_only=false)
Add a parameter.
void trigger(const std::string &trigger_name)
Latch the trigger with the provided name.
void set_qos(const rclcpp::QoS &qos)
Set the Quality of Service for ROS publishers and subscribers.
void set_predicate(const std::string &predicate_name, bool predicate_value)
Set the value of the predicate given as parameter, if the predicate is not found does not do anything...
void add_predicate(const std::string &predicate_name, bool predicate_value)
Add a predicate to the map of predicates.
bool get_predicate(const std::string &predicate_name) const
Get the logical value of a predicate.
virtual bool on_validate_parameter_callback(const std::shared_ptr< state_representation::ParameterInterface > &parameter)
Parameter validation function to be redefined by derived controller classes.
void add_service(const std::string &service_name, const std::function< ControllerServiceResponse(void)> &callback)
Add a service to trigger a callback function with no input arguments.
std::timed_mutex & get_command_mutex()
Get the reference to the command mutex.
void add_trigger(const std::string &trigger_name)
Add a trigger to the controller.
An exception class to notify errors with parameters in modulo classes.
An exception class to notify incompatibility when translating parameters from different sources.
rclcpp::Parameter write_parameter(const std::shared_ptr< state_representation::ParameterInterface > &parameter)
Write a ROS Parameter from a ParameterInterface pointer.
rclcpp::ParameterType get_ros_parameter_type(const state_representation::ParameterType &parameter_type)
Given a state representation parameter type, get the corresponding ROS parameter type.
void copy_parameter_value(const std::shared_ptr< const state_representation::ParameterInterface > &source_parameter, const std::shared_ptr< state_representation::ParameterInterface > &parameter)
Copy the value of one parameter interface into another.
std::shared_ptr< state_representation::ParameterInterface > read_parameter_const(const rclcpp::Parameter &ros_parameter, const std::shared_ptr< const state_representation::ParameterInterface > &parameter)
Update the parameter value of a ParameterInterface from a ROS Parameter object only if the two parame...
Modulo Core.
Response structure to be returned by controller services.