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