diff --git a/versions/main/_controller_interface_8cpp_source.html b/versions/main/_controller_interface_8cpp_source.html new file mode 100644 index 000000000..ab5f40c02 --- /dev/null +++ b/versions/main/_controller_interface_8cpp_source.html @@ -0,0 +1,949 @@ + + + + + + + +Modulo: /github/workspace/source/modulo_controllers/src/ControllerInterface.cpp Source File + + + + + + + + + +
+
+ + + + + + +
+
Modulo 4.1.0 +
+
+
+ + + + + + + + + +
+
+ + +
+
+
+
+
+
Loading...
+
Searching...
+
No Matches
+
+
+
+
+ + +
+
+
ControllerInterface.cpp
+
+
+
1#include "modulo_controllers/ControllerInterface.hpp"
+
2
+
3#include <chrono>
+
4
+
5#include <modulo_core/translators/message_readers.hpp>
+
6
+
7#include "modulo_controllers/utils/utilities.hpp"
+
8
+
9template<class... Ts>
+
10struct overloaded : Ts... {
+
11 using Ts::operator()...;
+
12};
+
13template<class... Ts>
+
14overloaded(Ts...) -> overloaded<Ts...>;
+
15
+
16using namespace state_representation;
+
17using namespace std::chrono_literals;
+
18
+
19namespace modulo_controllers {
+
20
+
+
21ControllerInterface::ControllerInterface(bool claim_all_state_interfaces)
+
22 : controller_interface::ControllerInterface(),
+
23 input_validity_period_(1.0),
+
24 claim_all_state_interfaces_(claim_all_state_interfaces),
+
25 on_init_called_(false) {}
+
+
26
+
+
27rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn ControllerInterface::on_init() {
+
28 on_init_called_ = true;
+
29 parameter_cb_handle_ = get_node()->add_on_set_parameters_callback(
+
30 [this](const std::vector<rclcpp::Parameter>& parameters) -> rcl_interfaces::msg::SetParametersResult {
+
31 return on_set_parameters_callback(parameters);
+
32 });
+
33
+
34 try {
+
35 add_parameter(std::make_shared<Parameter<std::string>>("hardware_name"), "The name of the hardware interface");
+
36 add_parameter(
+
37 std::make_shared<Parameter<std::string>>("robot_description"),
+
38 "The string formatted content of the controller's URDF description");
+
39 add_parameter(
+
40 std::make_shared<Parameter<std::vector<std::string>>>("joints"),
+
41 "A vector of joint names that the controller will claim");
+
42 add_parameter<double>(
+
43 "activation_timeout", 1.0, "The seconds to wait for valid data on the state interfaces before activating");
+
44 add_parameter<double>(
+
45 "input_validity_period", input_validity_period_,
+
46 "The maximum age of an input state before discarding it as expired");
+
47 add_parameter<int>("predicate_publishing_rate", 10, "The rate at which to publish controller predicates");
+
48 } catch (const std::exception& e) {
+
49 RCLCPP_ERROR(get_node()->get_logger(), "Exception thrown during on_init stage with message: %s \n", e.what());
+
50 return CallbackReturn::ERROR;
+
51 }
+
52 return CallbackReturn::SUCCESS;
+
53}
+
+
54
+
55rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+
+
56ControllerInterface::on_configure(const rclcpp_lifecycle::State&) {
+
57 if (!on_init_called_) {
+
58 RCLCPP_ERROR(
+
59 get_node()->get_logger(),
+
60 "The controller has not been properly initialized! Derived controller classes must call "
+
61 "'ControllerInterface::on_init()' during their own initialization before being configured.");
+
62 return CallbackReturn::ERROR;
+
63 }
+
64 auto hardware_name = get_parameter("hardware_name");
+
65 if (hardware_name->is_empty()) {
+
66 RCLCPP_ERROR(get_node()->get_logger(), "Parameter 'hardware_name' cannot be empty");
+
67 return CallbackReturn::ERROR;
+
68 }
+
69 hardware_name_ = hardware_name->get_parameter_value<std::string>();
+
70
+
71 input_validity_period_ = get_parameter_value<double>("input_validity_period");
+
72 add_inputs();
+
73 add_outputs();
+
74
+
75 if (predicates_.size()) {
+
76 // TODO: topic
+
77 predicate_publisher_ =
+
78 get_node()->create_publisher<modulo_interfaces::msg::PredicateCollection>("/predicates2", qos_);
+
79 predicate_message_.node = get_node()->get_fully_qualified_name();
+
80 predicate_message_.type = modulo_interfaces::msg::PredicateCollection::CONTROLLER;
+
81
+
82 predicate_timer_ = get_node()->create_wall_timer(
+
83 std::chrono::nanoseconds(static_cast<int64_t>(1e9 / get_parameter_value<int>("predicate_publishing_rate"))),
+
84 [this]() { this->publish_predicates(); });
+
85 }
+
86
+
87 RCLCPP_DEBUG(get_node()->get_logger(), "Configuration of ControllerInterface successful");
+
88 return on_configure();
+
89}
+
+
90
+
+
91rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn ControllerInterface::on_configure() {
+
92 return CallbackReturn::SUCCESS;
+
93}
+
+
94
+
+
95void ControllerInterface::add_state_interface(const std::string& name, const std::string& interface) {
+
96 add_interface(name, interface, state_interface_names_, "state");
+
97}
+
+
98
+
+
99void ControllerInterface::add_command_interface(const std::string& name, const std::string& interface) {
+
100 add_interface(name, interface, command_interface_names_, "command");
+
101}
+
+
102
+
103void ControllerInterface::add_interface(
+
104 const std::string& name, const std::string& interface, std::vector<std::string>& list, const std::string& type) {
+
105 if (get_node()->get_current_state().label() != "configuring") {
+
106 throw std::runtime_error("Interfaces can only be added when the controller is in state 'configuring'");
+
107 }
+
108 auto full_name = name + "/" + interface;
+
109 if (std::find(list.cbegin(), list.cend(), full_name) == list.cend()) {
+
110 list.push_back(full_name);
+
111 RCLCPP_DEBUG(
+
112 get_node()->get_logger(), "Adding interface '%s' to the list of desired %s interfaces", full_name.c_str(),
+
113 type.c_str());
+
114 } else {
+
115 RCLCPP_WARN(
+
116 get_node()->get_logger(), "Interface '%s' is already in the list of desired %s interfaces", full_name.c_str(),
+
117 type.c_str());
+
118 }
+
119}
+
120
+
+
121controller_interface::InterfaceConfiguration ControllerInterface::command_interface_configuration() const {
+
122 controller_interface::InterfaceConfiguration command_interfaces_config;
+
123 if (command_interface_names_.empty()) {
+
124 RCLCPP_DEBUG(get_node()->get_logger(), "List of command interfaces is empty, not claiming any interfaces.");
+
125 command_interfaces_config.type = controller_interface::interface_configuration_type::NONE;
+
126 return command_interfaces_config;
+
127 }
+
128
+
129 command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+
130 for (const auto& interface : command_interface_names_) {
+
131 RCLCPP_DEBUG(get_node()->get_logger(), "Claiming command interface '%s'", interface.c_str());
+
132 command_interfaces_config.names.push_back(interface);
+
133 }
+
134
+
135 return command_interfaces_config;
+
136}
+
+
137
+
+
138controller_interface::InterfaceConfiguration ControllerInterface::state_interface_configuration() const {
+
139 controller_interface::InterfaceConfiguration state_interfaces_config;
+
140 if (claim_all_state_interfaces_) {
+
141 RCLCPP_DEBUG(get_node()->get_logger(), "Claiming all state interfaces.");
+
142 state_interfaces_config.type = controller_interface::interface_configuration_type::ALL;
+
143 return state_interfaces_config;
+
144 }
+
145
+
146 state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+
147 for (const auto& interface : state_interface_names_) {
+
148 RCLCPP_DEBUG(get_node()->get_logger(), "Claiming state interface '%s'", interface.c_str());
+
149 state_interfaces_config.names.push_back(interface);
+
150 }
+
151
+
152 return state_interfaces_config;
+
153}
+
+
154
+
155rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+
+
156ControllerInterface::on_activate(const rclcpp_lifecycle::State&) {
+
157 // initialize the map of command data from all available interfaces
+
158 command_interface_data_ = std::vector<double>(command_interfaces_.size());
+
159 for (unsigned int i = 0; i < command_interfaces_.size(); ++i) {
+
160 const auto& command_interface = command_interfaces_.at(i);
+
161 if (command_interface_indices_.find(command_interface.get_prefix_name()) == command_interface_indices_.cend()) {
+
162 command_interface_indices_.insert_or_assign(
+
163 command_interface.get_prefix_name(), std::unordered_map<std::string, unsigned int>());
+
164 }
+
165 command_interface_indices_.at(command_interface.get_prefix_name())
+
166 .insert_or_assign(command_interface.get_interface_name(), i);
+
167 command_interface_data_.at(i) = command_interface.get_value();
+
168 }
+
169
+
170 // initialize the map of state data from all available interfaces
+
171 for (const auto& state_interface : state_interfaces_) {
+
172 if (state_interface_data_.find(state_interface.get_prefix_name()) == state_interface_data_.cend()) {
+
173 state_interface_data_.insert_or_assign(
+
174 state_interface.get_prefix_name(), std::unordered_map<std::string, double>());
+
175 }
+
176 state_interface_data_.at(state_interface.get_prefix_name())
+
177 .insert_or_assign(state_interface.get_interface_name(), state_interface.get_value());
+
178 }
+
179
+
180 auto status = on_activate();
+
181 if (status != CallbackReturn::SUCCESS) {
+
182 return status;
+
183 }
+
184
+
185 auto start_time = get_node()->get_clock()->now();
+
186 auto activation_timeout = rclcpp::Duration::from_seconds(get_parameter_value<double>("activation_timeout"));
+
187 while (read_state_interfaces() != controller_interface::return_type::OK) {
+
188 RCLCPP_DEBUG_THROTTLE(
+
189 get_node()->get_logger(), *get_node()->get_clock(), 1000,
+
190 "Activation is not possible yet; the controller did not receive valid states from hardware");
+
191 if ((get_node()->get_clock()->now() - start_time) > activation_timeout) {
+
192 release_interfaces();
+
193 RCLCPP_ERROR(
+
194 get_node()->get_logger(),
+
195 "Activation was not successful; the controller did not receive valid states from hardware");
+
196 return CallbackReturn::FAILURE;
+
197 }
+
198 }
+
199
+
200 RCLCPP_DEBUG(get_node()->get_logger(), "Activation of ControllerInterface successful");
+
201 return CallbackReturn::SUCCESS;
+
202}
+
+
203
+
+
204rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn ControllerInterface::on_activate() {
+
205 return CallbackReturn::SUCCESS;
+
206}
+
+
207
+
208rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+
+
209ControllerInterface::on_deactivate(const rclcpp_lifecycle::State&) {
+
210 return on_deactivate();
+
211}
+
+
212
+
+
213rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn ControllerInterface::on_deactivate() {
+
214 return CallbackReturn::SUCCESS;
+
215}
+
+
216
+
217controller_interface::return_type
+
+
218ControllerInterface::update(const rclcpp::Time& time, const rclcpp::Duration& period) {
+
219 auto status = read_state_interfaces();
+
220 if (status != controller_interface::return_type::OK) {
+
221 return status;
+
222 }
+
223
+
224 try {
+
225 status = evaluate(time, period.to_chrono<std::chrono::nanoseconds>());
+
226 } catch (const std::exception& e) {
+
227 RCLCPP_ERROR_THROTTLE(
+
228 get_node()->get_logger(), *get_node()->get_clock(), 1000, "Exception during evaluate(): %s \n", e.what());
+
229 return controller_interface::return_type::ERROR;
+
230 }
+
231 if (status != controller_interface::return_type::OK) {
+
232 return status;
+
233 }
+
234
+
235 if (command_interface_data_.empty()) {
+
236 return controller_interface::return_type::OK;
+
237 }
+
238
+
239 controller_interface::return_type ret;
+
240 if (command_mutex_.try_lock()) {
+
241 missed_locks_ = 0;
+
242 ret = write_command_interfaces(period);
+
243 command_mutex_.unlock();
+
244 } else {
+
245 if (missed_locks_ > 2) {
+
246 RCLCPP_ERROR(
+
247 get_node()->get_logger(),
+
248 "Controller is unable to acquire lock for command interfaces, returning an error now");
+
249 ret = controller_interface::return_type::ERROR;
+
250 }
+
251 ++missed_locks_;
+
252 RCLCPP_WARN(get_node()->get_logger(), "Unable to acquire lock for command interfaces (%u/3)", missed_locks_);
+
253 }
+
254
+
255 return ret;
+
256}
+
+
257
+
+
258controller_interface::return_type ControllerInterface::read_state_interfaces() {
+
259 for (const auto& state_interface : state_interfaces_) {
+
260 state_interface_data_.at(state_interface.get_prefix_name()).at(state_interface.get_interface_name()) =
+
261 state_interface.get_value();
+
262 }
+
263
+
264 return controller_interface::return_type::OK;
+
265}
+
+
266
+
+
267controller_interface::return_type ControllerInterface::write_command_interfaces(const rclcpp::Duration&) {
+
268 for (auto& command_interface : command_interfaces_) {
+
269 command_interface.set_value(command_interface_data_.at(
+
270 command_interface_indices_.at(command_interface.get_prefix_name()).at(command_interface.get_interface_name())));
+
271 }
+
272 return controller_interface::return_type::OK;
+
273}
+
+
274
+
+
275std::unordered_map<std::string, double> ControllerInterface::get_state_interfaces(const std::string& name) const {
+
276 return state_interface_data_.at(name);
+
277}
+
+
278
+
+
279double ControllerInterface::get_state_interface(const std::string& name, const std::string& interface) const {
+
280 return state_interface_data_.at(name).at(interface);
+
281}
+
+
282
+
+
283double ControllerInterface::get_command_interface(const std::string& name, const std::string& interface) const {
+
284 return command_interfaces_.at(command_interface_indices_.at(name).at(interface)).get_value();
+
285}
+
+
286
+
+
287void ControllerInterface::set_command_interface(const std::string& name, const std::string& interface, double value) {
+
288 try {
+
289 command_interface_data_.at(command_interface_indices_.at(name).at(interface)) = value;
+
290 } catch (const std::out_of_range&) {
+
291 RCLCPP_WARN_THROTTLE(
+
292 get_node()->get_logger(), *get_node()->get_clock(), 1000,
+
293 "set_command_interface called with an unknown name/interface: %s/%s", name.c_str(), interface.c_str());
+
294 }
+
295}
+
+
296
+
297void ControllerInterface::add_parameter(
+
298 const std::shared_ptr<ParameterInterface>& parameter, const std::string& description, bool read_only) {
+
299 set_parameter_callback_called_ = false;
+
300 rclcpp::Parameter ros_param;
+
301 try {
+
302 ros_param = modulo_core::translators::write_parameter(parameter);
+
303 if (!get_node()->has_parameter(parameter->get_name())) {
+
304 RCLCPP_DEBUG(get_node()->get_logger(), "Adding parameter '%s'.", parameter->get_name().c_str());
+
305 parameter_map_.set_parameter(parameter);
+
306 rcl_interfaces::msg::ParameterDescriptor descriptor;
+
307 descriptor.description = description;
+
308 descriptor.read_only = read_only;
+
309 if (parameter->is_empty()) {
+
310 descriptor.dynamic_typing = true;
+
311 descriptor.type = modulo_core::translators::get_ros_parameter_type(parameter->get_parameter_type());
+
312 get_node()->declare_parameter(parameter->get_name(), rclcpp::ParameterValue{}, descriptor);
+
313 } else {
+
314 get_node()->declare_parameter(parameter->get_name(), ros_param.get_parameter_value(), descriptor);
+
315 }
+
316 if (!set_parameter_callback_called_) {
+
317 auto result = on_set_parameters_callback({get_node()->get_parameters({parameter->get_name()})});
+
318 if (!result.successful) {
+
319 get_node()->undeclare_parameter(parameter->get_name());
+
320 throw std::runtime_error(result.reason);
+
321 }
+
322 }
+
323 } else {
+
324 RCLCPP_DEBUG(get_node()->get_logger(), "Parameter '%s' already exists.", parameter->get_name().c_str());
+
325 }
+
326 } catch (const std::exception& ex) {
+
327 RCLCPP_ERROR(
+
328 get_node()->get_logger(), "Failed to add parameter '%s': %s", parameter->get_name().c_str(), ex.what());
+
329 }
+
330}
+
331
+
+
332std::shared_ptr<ParameterInterface> ControllerInterface::get_parameter(const std::string& name) const {
+
333 return parameter_map_.get_parameter(name);
+
334}
+
+
335
+
336rcl_interfaces::msg::SetParametersResult
+
337ControllerInterface::on_set_parameters_callback(const std::vector<rclcpp::Parameter>& parameters) {
+
338 rcl_interfaces::msg::SetParametersResult result;
+
339 result.successful = true;
+
340 for (const auto& ros_parameter : parameters) {
+
341 try {
+
342 if (ros_parameter.get_name().substr(0, 17) == "qos_overrides./tf") {
+
343 continue;
+
344 }
+
345 // get the associated parameter interface by name
+
346 auto parameter = parameter_map_.get_parameter(ros_parameter.get_name());
+
347
+
348 // convert the ROS parameter into a ParameterInterface without modifying the original
+
349 auto new_parameter = modulo_core::translators::read_parameter_const(ros_parameter, parameter);
+
350 if (!validate_parameter(new_parameter)) {
+
351 result.successful = false;
+
352 result.reason += "Validation of parameter '" + ros_parameter.get_name() + "' returned false!";
+
353 } else if (!new_parameter->is_empty()) {
+
354 // update the value of the parameter in the map
+
355 modulo_core::translators::copy_parameter_value(new_parameter, parameter);
+
356 }
+
357 } catch (const std::exception& ex) {
+
358 result.successful = false;
+
359 result.reason += ex.what();
+
360 }
+
361 }
+
362 set_parameter_callback_called_ = true;
+
363 return result;
+
364}
+
365
+
366bool ControllerInterface::validate_parameter(const std::shared_ptr<ParameterInterface>& parameter) {
+
367 if (parameter->get_name() == "activation_timeout" || parameter->get_name() == "input_validity_period") {
+
368 auto value = parameter->get_parameter_value<double>();
+
369 if (value < 0.0 || value > std::numeric_limits<double>::max()) {
+
370 RCLCPP_ERROR(
+
371 get_node()->get_logger(), "Parameter value of parameter '%s' should be a positive finite number",
+
372 parameter->get_name().c_str());
+
373 return false;
+
374 }
+
375 }
+
376 return on_validate_parameter_callback(parameter);
+
377}
+
378
+
+
379bool ControllerInterface::on_validate_parameter_callback(const std::shared_ptr<ParameterInterface>&) {
+
380 return true;
+
381}
+
+
382
+
+
383void ControllerInterface::add_predicate(const std::string& name, bool predicate) {
+
384 add_variant_predicate(name, utilities::PredicateVariant(predicate));
+
385}
+
+
386
+
+
387void ControllerInterface::add_predicate(const std::string& name, const std::function<bool(void)>& predicate) {
+
388 add_variant_predicate(name, utilities::PredicateVariant(predicate));
+
389}
+
+
390
+
391void ControllerInterface::add_variant_predicate(const std::string& name, const utilities::PredicateVariant& predicate) {
+
392 if (name.empty()) {
+
393 RCLCPP_ERROR(get_node()->get_logger(), "Failed to add predicate: Provide a non empty string as a name.");
+
394 return;
+
395 }
+
396 if (predicates_.find(name) != predicates_.end()) {
+
397 RCLCPP_WARN(get_node()->get_logger(), "Predicate with name '%s' already exists, overwriting.", name.c_str());
+
398 } else {
+
399 RCLCPP_DEBUG(get_node()->get_logger(), "Adding predicate '%s'.", name.c_str());
+
400 }
+
401 predicates_.insert_or_assign(name, predicate);
+
402}
+
403
+
+
404bool ControllerInterface::get_predicate(const std::string& predicate_name) const {
+
405 auto predicate_iterator = predicates_.find(predicate_name);
+
406 // if there is no predicate with that name simply return false with an error message
+
407 if (predicate_iterator == predicates_.end()) {
+
408 RCLCPP_ERROR_THROTTLE(
+
409 get_node()->get_logger(), *get_node()->get_clock(), 1000,
+
410 "Failed to get predicate '%s': Predicate does not exists, returning false.", predicate_name.c_str());
+
411 return false;
+
412 }
+
413 // try to get the value from the variant as a bool
+
414 auto* ptr_value = std::get_if<bool>(&predicate_iterator->second);
+
415 if (ptr_value) {
+
416 return *ptr_value;
+
417 }
+
418 // if previous check failed, it means the variant is actually a callback function
+
419 auto callback_function = std::get<std::function<bool(void)>>(predicate_iterator->second);
+
420 bool value = false;
+
421 try {
+
422 value = (callback_function) ();
+
423 } catch (const std::exception& ex) {
+
424 RCLCPP_ERROR_THROTTLE(
+
425 get_node()->get_logger(), *get_node()->get_clock(), 1000,
+
426 "Failed to evaluate callback of predicate '%s', returning false: %s", predicate_name.c_str(), ex.what());
+
427 }
+
428 return value;
+
429}
+
+
430
+
+
431void ControllerInterface::set_predicate(const std::string& name, bool predicate) {
+
432 set_variant_predicate(name, utilities::PredicateVariant(predicate));
+
433}
+
+
434
+
+
435void ControllerInterface::set_predicate(const std::string& name, const std::function<bool(void)>& predicate) {
+
436 set_variant_predicate(name, utilities::PredicateVariant(predicate));
+
437}
+
+
438
+
439void ControllerInterface::set_variant_predicate(const std::string& name, const utilities::PredicateVariant& predicate) {
+
440 auto predicate_iterator = predicates_.find(name);
+
441 if (predicate_iterator == predicates_.end()) {
+
442 RCLCPP_ERROR_THROTTLE(
+
443 get_node()->get_logger(), *get_node()->get_clock(), 1000,
+
444 "Failed to set predicate '%s': Predicate does not exist.", name.c_str());
+
445 return;
+
446 }
+
447 predicate_iterator->second = predicate;
+
448 publish_predicate(name);// TODO: do we want that
+
449}
+
450
+
+
451void ControllerInterface::add_trigger(const std::string& trigger_name) {
+
452 if (trigger_name.empty()) {
+
453 RCLCPP_ERROR(get_node()->get_logger(), "Failed to add trigger: Provide a non empty string as a name.");
+
454 return;
+
455 }
+
456 if (triggers_.find(trigger_name) != triggers_.end() || predicates_.find(trigger_name) != predicates_.end()) {
+
457 RCLCPP_ERROR(
+
458 get_node()->get_logger(), "Failed to add trigger: there is already a trigger or predicate with name '%s'.",
+
459 trigger_name.c_str());
+
460 return;
+
461 }
+
462 triggers_.insert_or_assign(trigger_name, false);
+
463 add_predicate(trigger_name, [this, trigger_name] {
+
464 auto value = this->triggers_.at(trigger_name);
+
465 this->triggers_.at(trigger_name) = false;
+
466 return value;
+
467 });
+
468}
+
+
469
+
+
470void ControllerInterface::trigger(const std::string& trigger_name) {
+
471 if (triggers_.find(trigger_name) == triggers_.end()) {
+
472 RCLCPP_ERROR(
+
473 get_node()->get_logger(), "Failed to trigger: could not find trigger with name '%s'.", trigger_name.c_str());
+
474 return;
+
475 }
+
476 triggers_.at(trigger_name) = true;
+
477 publish_predicate(trigger_name);
+
478}
+
+
479
+
480modulo_interfaces::msg::Predicate ControllerInterface::get_predicate_message(const std::string& name) const {
+
481 modulo_interfaces::msg::Predicate message;
+
482 message.predicate = name;
+
483 message.value = get_predicate(name);
+
484 return message;
+
485}
+
486
+
487void ControllerInterface::publish_predicate(const std::string& name) const {
+
488 auto message(predicate_message_);
+
489 message.predicates.push_back(get_predicate_message(name));
+
490 predicate_publisher_->publish(message);
+
491}
+
492
+
493void ControllerInterface::publish_predicates() const {
+
494 auto message(predicate_message_);
+
495 for (const auto& predicate : predicates_) {
+
496 message.predicates.push_back(get_predicate_message(predicate.first));
+
497 }
+
498 predicate_publisher_->publish(message);
+
499}
+
500
+
501std::string ControllerInterface::validate_and_declare_signal(
+
502 const std::string& signal_name, const std::string& type, const std::string& default_topic, bool fixed_topic) {
+
503 auto parsed_signal_name = utilities::parse_topic_name(signal_name);
+
504 if (parsed_signal_name.empty()) {
+
505 RCLCPP_WARN(
+
506 get_node()->get_logger(),
+
507 "The parsed signal name for %s '%s' is empty. Provide a string with valid characters for the signal name "
+
508 "([a-zA-Z0-9_]).",
+
509 type.c_str(), signal_name.c_str());
+
510 return "";
+
511 }
+
512 if (signal_name != parsed_signal_name) {
+
513 RCLCPP_WARN(
+
514 get_node()->get_logger(),
+
515 "The parsed signal name for %s '%s' is '%s'. Use the parsed signal name to refer to this %s and its topic "
+
516 "parameter.",
+
517 type.c_str(), signal_name.c_str(), parsed_signal_name.c_str(), type.c_str());
+
518 }
+
519 if (inputs_.find(parsed_signal_name) != inputs_.end()) {
+
520 RCLCPP_WARN(get_node()->get_logger(), "Signal '%s' already exists as input.", parsed_signal_name.c_str());
+
521 return "";
+
522 }
+
523 if (outputs_.find(parsed_signal_name) != outputs_.end()) {
+
524 RCLCPP_WARN(get_node()->get_logger(), "Signal '%s' already exists as output", parsed_signal_name.c_str());
+
525 return "";
+
526 }
+
527 auto topic = default_topic.empty() ? "~/" + parsed_signal_name : default_topic;
+
528 auto parameter_name = parsed_signal_name + "_topic";
+
529 if (get_node()->has_parameter(parameter_name) && get_parameter(parameter_name)->is_empty()) {
+
530 set_parameter_value<std::string>(parameter_name, topic);
+
531 } else {
+
532 add_parameter<std::string>(
+
533 parameter_name, topic, "Signal topic name of " + type + " '" + parsed_signal_name + "'", fixed_topic);
+
534 }
+
535 RCLCPP_DEBUG(
+
536 get_node()->get_logger(), "Declared %s '%s' and parameter '%s' with value '%s'.", type.c_str(),
+
537 parsed_signal_name.c_str(), parameter_name.c_str(), topic.c_str());
+
538 return parsed_signal_name;
+
539}
+
540
+
541void ControllerInterface::create_input(
+
542 const ControllerInput& input, const std::string& name, const std::string& topic_name) {
+
543 auto parsed_name = validate_and_declare_signal(name, "input", topic_name);
+
544 if (!parsed_name.empty()) {
+
545 inputs_.insert_or_assign(name, input);
+
546 }
+
547}
+
548
+
549void ControllerInterface::add_inputs() {
+
550 for (auto& [name, input] : inputs_) {
+
551 try {
+
552 auto topic = get_parameter_value<std::string>(name + "_topic");
+
553 std::visit(
+
554 overloaded{
+
555 [&](const realtime_tools::RealtimeBuffer<std::shared_ptr<modulo_core::EncodedState>>&) {
+
556 subscriptions_.push_back(create_subscription<modulo_core::EncodedState>(name, topic));
+
557 },
+
558 [&](const realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Bool>>&) {
+
559 subscriptions_.push_back(create_subscription<std_msgs::msg::Bool>(name, topic));
+
560 },
+
561 [&](const realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Float64>>&) {
+
562 subscriptions_.push_back(create_subscription<std_msgs::msg::Float64>(name, topic));
+
563 },
+
564 [&](const realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Float64MultiArray>>&) {
+
565 subscriptions_.push_back(create_subscription<std_msgs::msg::Float64MultiArray>(name, topic));
+
566 },
+
567 [&](const realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Int32>>&) {
+
568 subscriptions_.push_back(create_subscription<std_msgs::msg::Int32>(name, topic));
+
569 },
+
570 [&](const realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::String>>&) {
+
571 subscriptions_.push_back(create_subscription<std_msgs::msg::String>(name, topic));
+
572 }},
+
573 input.buffer);
+
574 } catch (const std::exception& ex) {
+
575 RCLCPP_ERROR(get_node()->get_logger(), "Failed to add input '%s': %s", name.c_str(), ex.what());
+
576 }
+
577 }
+
578}
+
579
+
580void ControllerInterface::create_output(
+
581 const PublisherVariant& publishers, const std::string& name, const std::string& topic_name) {
+
582 auto parsed_name = validate_and_declare_signal(name, "output", topic_name);
+
583 if (!parsed_name.empty()) {
+
584 outputs_.insert_or_assign(name, publishers);
+
585 }
+
586}
+
587
+
588void ControllerInterface::add_outputs() {
+
589 for (auto& [name, publishers] : outputs_) {
+
590 try {
+
591 auto topic = get_parameter_value<std::string>(name + "_topic");
+
592 std::visit(
+
593 overloaded{
+
594 [&](EncodedStatePublishers& pub) {
+
595 std::get<1>(pub) = get_node()->create_publisher<modulo_core::EncodedState>(topic, qos_);
+
596 std::get<2>(pub) =
+
597 std::make_shared<realtime_tools::RealtimePublisher<modulo_core::EncodedState>>(std::get<1>(pub));
+
598 },
+
599 [&](BoolPublishers& pub) {
+
600 pub.first = get_node()->create_publisher<std_msgs::msg::Bool>(topic, qos_);
+
601 pub.second = std::make_shared<realtime_tools::RealtimePublisher<std_msgs::msg::Bool>>(pub.first);
+
602 },
+
603 [&](DoublePublishers& pub) {
+
604 pub.first = get_node()->create_publisher<std_msgs::msg::Float64>(topic, qos_);
+
605 pub.second = std::make_shared<realtime_tools::RealtimePublisher<std_msgs::msg::Float64>>(pub.first);
+
606 },
+
607 [&](DoubleVecPublishers& pub) {
+
608 pub.first = get_node()->create_publisher<std_msgs::msg::Float64MultiArray>(topic, qos_);
+
609 pub.second =
+
610 std::make_shared<realtime_tools::RealtimePublisher<std_msgs::msg::Float64MultiArray>>(pub.first);
+
611 },
+
612 [&](IntPublishers& pub) {
+
613 pub.first = get_node()->create_publisher<std_msgs::msg::Int32>(topic, qos_);
+
614 pub.second = std::make_shared<realtime_tools::RealtimePublisher<std_msgs::msg::Int32>>(pub.first);
+
615 },
+
616 [&](StringPublishers& pub) {
+
617 pub.first = get_node()->create_publisher<std_msgs::msg::String>(topic, qos_);
+
618 pub.second = std::make_shared<realtime_tools::RealtimePublisher<std_msgs::msg::String>>(pub.first);
+
619 }},
+
620 publishers);
+
621 } catch (const std::exception& ex) {
+
622 RCLCPP_ERROR(get_node()->get_logger(), "Failed to add input '%s': %s", name.c_str(), ex.what());
+
623 }
+
624 }
+
625}
+
626
+
627bool ControllerInterface::check_input_valid(const std::string& name) const {
+
628 if (inputs_.find(name) == inputs_.end()) {
+
629 RCLCPP_WARN_THROTTLE(
+
630 get_node()->get_logger(), *get_node()->get_clock(), 1000, "Could not find input '%s'", name.c_str());
+
631 return false;
+
632 }
+
633 if (static_cast<double>(std::chrono::duration_cast<std::chrono::nanoseconds>(
+
634 std::chrono::steady_clock::now() - inputs_.at(name).timestamp)
+
635 .count())
+
636 / 1e9
+
637 >= input_validity_period_) {
+
638 return false;
+
639 }
+
640 return true;
+
641}
+
642
+
643std::string ControllerInterface::validate_service_name(const std::string& service_name, const std::string& type) const {
+
644 std::string parsed_service_name = utilities::parse_topic_name(service_name);
+
645 if (parsed_service_name.empty()) {
+
646 RCLCPP_WARN(
+
647 get_node()->get_logger(),
+
648 "The parsed service name for %s service '%s' is empty. Provide a string with valid characters for the service "
+
649 "name "
+
650 "([a-zA-Z0-9_]).",
+
651 type.c_str(), service_name.c_str());
+
652 return "";
+
653 }
+
654 if (service_name != parsed_service_name) {
+
655 RCLCPP_WARN(
+
656 get_node()->get_logger(),
+
657 "The parsed name for '%s' service '%s' is '%s'. Use the parsed name to refer to this service.", type.c_str(),
+
658 service_name.c_str(), parsed_service_name.c_str());
+
659 }
+
660 if (empty_services_.find(parsed_service_name) != empty_services_.cend()) {
+
661 RCLCPP_WARN(
+
662 get_node()->get_logger(), "Service with name '%s' already exists as %s service.", parsed_service_name.c_str(),
+
663 type.c_str());
+
664 return "";
+
665 }
+
666 if (string_services_.find(parsed_service_name) != string_services_.cend()) {
+
667 RCLCPP_WARN(
+
668 get_node()->get_logger(), "Service with name '%s' already exists as %s service.", parsed_service_name.c_str(),
+
669 type.c_str());
+
670 return "";
+
671 }
+
672 RCLCPP_DEBUG(get_node()->get_logger(), "Adding %s service '%s'.", type.c_str(), parsed_service_name.c_str());
+
673 return parsed_service_name;
+
674}
+
675
+
+
676void ControllerInterface::add_service(
+
677 const std::string& service_name, const std::function<ControllerServiceResponse(void)>& callback) {
+
678 auto parsed_service_name = validate_service_name(service_name, "empty");
+
679 if (!parsed_service_name.empty()) {
+
680 try {
+
681 auto service = get_node()->create_service<modulo_component_interfaces::srv::EmptyTrigger>(
+
682 "~/" + parsed_service_name,
+
683 [this, callback](
+
684 const std::shared_ptr<modulo_component_interfaces::srv::EmptyTrigger::Request>,
+
685 std::shared_ptr<modulo_component_interfaces::srv::EmptyTrigger::Response> response) {
+
686 try {
+
687 if (this->command_mutex_.try_lock_for(100ms)) {
+
688 auto callback_response = callback();
+
689 this->command_mutex_.unlock();
+
690 response->success = callback_response.success;
+
691 response->message = callback_response.message;
+
692 } else {
+
693 response->success = false;
+
694 response->message = "Unable to acquire lock for command interface within 100ms";
+
695 }
+
696 } catch (const std::exception& ex) {
+
697 response->success = false;
+
698 response->message = ex.what();
+
699 }
+
700 },
+
701 qos_);
+
702 empty_services_.insert_or_assign(parsed_service_name, service);
+
703 } catch (const std::exception& ex) {
+
704 RCLCPP_ERROR(get_node()->get_logger(), "Failed to add service '%s': %s", parsed_service_name.c_str(), ex.what());
+
705 }
+
706 }
+
707}
+
+
708
+
+
709void ControllerInterface::add_service(
+
710 const std::string& service_name,
+
711 const std::function<ControllerServiceResponse(const std::string& string)>& callback) {
+
712 auto parsed_service_name = validate_service_name(service_name, "string");
+
713 if (!parsed_service_name.empty()) {
+
714 try {
+
715 auto service = get_node()->create_service<modulo_component_interfaces::srv::StringTrigger>(
+
716 "~/" + parsed_service_name,
+
717 [this, callback](
+
718 const std::shared_ptr<modulo_component_interfaces::srv::StringTrigger::Request> request,
+
719 std::shared_ptr<modulo_component_interfaces::srv::StringTrigger::Response> response) {
+
720 try {
+
721 if (this->command_mutex_.try_lock_for(100ms)) {
+
722 auto callback_response = callback(request->payload);
+
723 this->command_mutex_.unlock();
+
724 response->success = callback_response.success;
+
725 response->message = callback_response.message;
+
726 } else {
+
727 response->success = false;
+
728 response->message = "Unable to acquire lock for command interface within 100ms";
+
729 }
+
730 } catch (const std::exception& ex) {
+
731 response->success = false;
+
732 response->message = ex.what();
+
733 }
+
734 },
+
735 qos_);
+
736 string_services_.insert_or_assign(parsed_service_name, service);
+
737 } catch (const std::exception& ex) {
+
738 RCLCPP_ERROR(get_node()->get_logger(), "Failed to add service '%s': %s", parsed_service_name.c_str(), ex.what());
+
739 }
+
740 }
+
741}
+
+
742
+
+
743rclcpp::QoS ControllerInterface::get_qos() const {
+
744 return qos_;
+
745}
+
+
746
+
+
747void ControllerInterface::set_qos(const rclcpp::QoS& qos) {
+
748 qos_ = qos;
+
749}
+
+
750
+
+
751bool ControllerInterface::is_active() const {
+
752 return get_node()->get_current_state().label() == "active";
+
753}
+
+
754
+
755}// namespace modulo_controllers
+
modulo_controllers::ControllerInterface
Base controller class to combine ros2_control, control libraries and modulo.
Definition ControllerInterface.hpp:98
+
modulo_controllers::ControllerInterface::get_parameter
std::shared_ptr< state_representation::ParameterInterface > get_parameter(const std::string &name) const
Get a parameter by name.
Definition ControllerInterface.cpp:332
+
modulo_controllers::ControllerInterface::get_qos
rclcpp::QoS get_qos() const
Getter of the Quality of Service attribute.
Definition ControllerInterface.cpp:743
+
modulo_controllers::ControllerInterface::add_predicate
void add_predicate(const std::string &predicate_name, bool predicate_value)
Add a predicate to the map of predicates.
Definition ControllerInterface.cpp:383
+
modulo_controllers::ControllerInterface::set_predicate
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...
Definition ControllerInterface.cpp:431
+
modulo_controllers::ControllerInterface::command_interface_configuration
controller_interface::InterfaceConfiguration command_interface_configuration() const final
Configure the command interfaces.
Definition ControllerInterface.cpp:121
+
modulo_controllers::ControllerInterface::write_command_interfaces
virtual controller_interface::return_type write_command_interfaces(const rclcpp::Duration &period)
Write the command interfaces.
Definition ControllerInterface.cpp:267
+
modulo_controllers::ControllerInterface::on_configure
virtual CallbackReturn on_configure()
Configure the controller.
Definition ControllerInterface.cpp:91
+
modulo_controllers::ControllerInterface::evaluate
virtual controller_interface::return_type evaluate(const rclcpp::Time &time, const std::chrono::nanoseconds &period)=0
The control logic callback.
+
modulo_controllers::ControllerInterface::get_parameter_value
T get_parameter_value(const std::string &name) const
Get a parameter value by name.
Definition ControllerInterface.hpp:632
+
modulo_controllers::ControllerInterface::read_state_interfaces
virtual controller_interface::return_type read_state_interfaces()
Read the state interfaces.
Definition ControllerInterface.cpp:258
+
modulo_controllers::ControllerInterface::get_state_interfaces
std::unordered_map< std::string, double > get_state_interfaces(const std::string &name) const
Get a map containing the state interfaces by name of the parent tag.
Definition ControllerInterface.cpp:275
+
modulo_controllers::ControllerInterface::is_active
bool is_active() const
Check if the controller is currently in state active or not.
Definition ControllerInterface.cpp:751
+
modulo_controllers::ControllerInterface::on_init
CallbackReturn on_init() override
Declare parameters and register the on_set_parameters callback.
Definition ControllerInterface.cpp:27
+
modulo_controllers::ControllerInterface::set_qos
void set_qos(const rclcpp::QoS &qos)
Set the Quality of Service for ROS publishers and subscribers.
Definition ControllerInterface.cpp:747
+
modulo_controllers::ControllerInterface::trigger
void trigger(const std::string &trigger_name)
Latch the trigger with the provided name.
Definition ControllerInterface.cpp:470
+
modulo_controllers::ControllerInterface::get_predicate
bool get_predicate(const std::string &predicate_name) const
Get the logical value of a predicate.
Definition ControllerInterface.cpp:404
+
modulo_controllers::ControllerInterface::ControllerInterface
ControllerInterface(bool claim_all_state_interfaces=false)
Default constructor.
Definition ControllerInterface.cpp:21
+
modulo_controllers::ControllerInterface::add_state_interface
void add_state_interface(const std::string &name, const std::string &interface)
Add a state interface to the controller by name.
Definition ControllerInterface.cpp:95
+
modulo_controllers::ControllerInterface::get_command_interface
double get_command_interface(const std::string &name, const std::string &interface) const
Get the value of a command interface by name.
Definition ControllerInterface.cpp:283
+
modulo_controllers::ControllerInterface::on_activate
virtual CallbackReturn on_activate()
Activate the controller.
Definition ControllerInterface.cpp:204
+
modulo_controllers::ControllerInterface::add_service
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.
Definition ControllerInterface.cpp:676
+
modulo_controllers::ControllerInterface::get_state_interface
double get_state_interface(const std::string &name, const std::string &interface) const
Get the value of a state interface by name.
Definition ControllerInterface.cpp:279
+
modulo_controllers::ControllerInterface::update
controller_interface::return_type update(const rclcpp::Time &time, const rclcpp::Duration &period) final
Read the state interfaces, perform control evaluation and write the command interfaces.
Definition ControllerInterface.cpp:218
+
modulo_controllers::ControllerInterface::set_command_interface
void set_command_interface(const std::string &name, const std::string &interface, double value)
Set the value of a command interface by name.
Definition ControllerInterface.cpp:287
+
modulo_controllers::ControllerInterface::add_parameter
void add_parameter(const std::shared_ptr< state_representation::ParameterInterface > &parameter, const std::string &description, bool read_only=false)
Add a parameter.
+
modulo_controllers::ControllerInterface::add_trigger
void add_trigger(const std::string &trigger_name)
Add a trigger to the controller.
Definition ControllerInterface.cpp:451
+
modulo_controllers::ControllerInterface::hardware_name_
std::string hardware_name_
The hardware name provided by a parameter.
Definition ControllerInterface.hpp:448
+
modulo_controllers::ControllerInterface::on_deactivate
virtual CallbackReturn on_deactivate()
Deactivate the controller.
Definition ControllerInterface.cpp:213
+
modulo_controllers::ControllerInterface::add_command_interface
void add_command_interface(const std::string &name, const std::string &interface)
Add a command interface to the controller by name.
Definition ControllerInterface.cpp:99
+
modulo_controllers::ControllerInterface::state_interface_configuration
controller_interface::InterfaceConfiguration state_interface_configuration() const final
Configure the state interfaces.
Definition ControllerInterface.cpp:138
+
modulo_controllers::ControllerInterface::on_validate_parameter_callback
virtual bool on_validate_parameter_callback(const std::shared_ptr< state_representation::ParameterInterface > &parameter)
Parameter validation function to be redefined by derived controller classes.
Definition ControllerInterface.cpp:379
+
modulo_core::translators::write_parameter
rclcpp::Parameter write_parameter(const std::shared_ptr< state_representation::ParameterInterface > &parameter)
Write a ROS Parameter from a ParameterInterface pointer.
+
modulo_core::translators::get_ros_parameter_type
rclcpp::ParameterType get_ros_parameter_type(const state_representation::ParameterType &parameter_type)
Given a state representation parameter type, get the corresponding ROS parameter type.
+
modulo_core::translators::copy_parameter_value
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.
+
modulo_core::translators::read_parameter_const
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::EncodedState
std_msgs::msg::UInt8MultiArray EncodedState
Define the EncodedState as UInt8MultiArray message type.
Definition EncodedState.hpp:15
+
modulo_controllers::ControllerServiceResponse
Response structure to be returned by controller services.
Definition ControllerInterface.hpp:89
+
+ +
+Generated by doxygen 1.9.8 +
+ + diff --git a/versions/main/_controller_interface_8hpp_source.html b/versions/main/_controller_interface_8hpp_source.html new file mode 100644 index 000000000..8b44b6489 --- /dev/null +++ b/versions/main/_controller_interface_8hpp_source.html @@ -0,0 +1,718 @@ + + + + + + + +Modulo: /github/workspace/source/modulo_controllers/include/modulo_controllers/ControllerInterface.hpp Source File + + + + + + + + + +
+
+ + + + + + +
+
Modulo 4.1.0 +
+
+
+ + + + + + + + + +
+
+ + +
+
+
+
+
+
Loading...
+
Searching...
+
No Matches
+
+
+
+
+ + +
+
+
ControllerInterface.hpp
+
+
+
1#pragma once
+
2
+
3#include <mutex>
+
4
+
5#include <controller_interface/controller_interface.hpp>
+
6#include <controller_interface/helpers.hpp>
+
7#include <realtime_tools/realtime_buffer.h>
+
8#include <realtime_tools/realtime_publisher.h>
+
9
+
10#include <state_representation/parameters/ParameterMap.hpp>
+
11
+
12#include <modulo_core/EncodedState.hpp>
+
13#include <modulo_core/communication/MessagePair.hpp>
+
14#include <modulo_core/translators/message_writers.hpp>
+
15#include <modulo_core/translators/parameter_translators.hpp>
+
16#include <modulo_interfaces/msg/predicate_collection.hpp>
+
17
+
18#include "modulo_controllers/utils/utilities.hpp"
+
19
+
20#include <modulo_component_interfaces/srv/empty_trigger.hpp>
+
21#include <modulo_component_interfaces/srv/string_trigger.hpp>
+
22
+
23namespace modulo_controllers {
+
24
+
25typedef std::variant<
+
26 std::shared_ptr<rclcpp::Subscription<modulo_core::EncodedState>>,
+
27 std::shared_ptr<rclcpp::Subscription<std_msgs::msg::Bool>>,
+
28 std::shared_ptr<rclcpp::Subscription<std_msgs::msg::Float64>>,
+
29 std::shared_ptr<rclcpp::Subscription<std_msgs::msg::Float64MultiArray>>,
+
30 std::shared_ptr<rclcpp::Subscription<std_msgs::msg::Int32>>,
+
31 std::shared_ptr<rclcpp::Subscription<std_msgs::msg::String>>>
+
32 SubscriptionVariant;
+
33
+
34typedef std::variant<
+
35 realtime_tools::RealtimeBuffer<std::shared_ptr<modulo_core::EncodedState>>,
+
36 realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Bool>>,
+
37 realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Float64>>,
+
38 realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Float64MultiArray>>,
+
39 realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Int32>>,
+
40 realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::String>>>
+
41 BufferVariant;
+
42
+
43typedef std::tuple<
+
44 std::shared_ptr<state_representation::State>,
+
45 std::shared_ptr<rclcpp::Publisher<modulo_core::EncodedState>>,
+
46 realtime_tools::RealtimePublisherSharedPtr<modulo_core::EncodedState>>
+
47 EncodedStatePublishers;
+
48typedef std::pair<
+
49 std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Bool>>,
+
50 realtime_tools::RealtimePublisherSharedPtr<std_msgs::msg::Bool>>
+
51 BoolPublishers;
+
52typedef std::pair<
+
53 std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Float64>>,
+
54 realtime_tools::RealtimePublisherSharedPtr<std_msgs::msg::Float64>>
+
55 DoublePublishers;
+
56typedef std::pair<
+
57 std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Float64MultiArray>>,
+
58 realtime_tools::RealtimePublisherSharedPtr<std_msgs::msg::Float64MultiArray>>
+
59 DoubleVecPublishers;
+
60typedef std::pair<
+
61 std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Int32>>,
+
62 realtime_tools::RealtimePublisherSharedPtr<std_msgs::msg::Int32>>
+
63 IntPublishers;
+
64typedef std::pair<
+
65 std::shared_ptr<rclcpp::Publisher<std_msgs::msg::String>>,
+
66 realtime_tools::RealtimePublisherSharedPtr<std_msgs::msg::String>>
+
67 StringPublishers;
+
68
+
69typedef std::variant<
+
70 EncodedStatePublishers, BoolPublishers, DoublePublishers, DoubleVecPublishers, IntPublishers, StringPublishers>
+
71 PublisherVariant;
+
72
+
+
77struct ControllerInput {
+
78 ControllerInput(BufferVariant buffer_variant) : buffer(std::move(buffer_variant)) {}
+
79 BufferVariant buffer;
+
80 std::chrono::time_point<std::chrono::steady_clock> timestamp;
+
81};
+
+
82
+
+
89struct ControllerServiceResponse {
+
90 bool success;
+
91 std::string message;
+
92};
+
+
93
+
+
98class ControllerInterface : public controller_interface::ControllerInterface {
+
99public:
+
104 ControllerInterface(bool claim_all_state_interfaces = false);
+
105
+
110 CallbackReturn on_init() override;
+
111
+
118 CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) final;
+
119
+
126 CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) final;
+
127
+
133 CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) final;
+
134
+
141 controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) final;
+
142
+
147 controller_interface::InterfaceConfiguration state_interface_configuration() const final;
+
148
+
153 controller_interface::InterfaceConfiguration command_interface_configuration() const final;
+
154
+
155protected:
+
161 virtual CallbackReturn on_configure();
+
162
+
168 virtual CallbackReturn on_activate();
+
169
+
175 virtual CallbackReturn on_deactivate();
+
176
+
181 virtual controller_interface::return_type read_state_interfaces();
+
182
+
188 virtual controller_interface::return_type write_command_interfaces(const rclcpp::Duration& period);
+
189
+
198 virtual controller_interface::return_type
+
199 evaluate(const rclcpp::Time& time, const std::chrono::nanoseconds& period) = 0;
+
200
+
206 void add_state_interface(const std::string& name, const std::string& interface);
+
207
+
213 void add_command_interface(const std::string& name, const std::string& interface);
+
214
+
221 std::unordered_map<std::string, double> get_state_interfaces(const std::string& name) const;
+
222
+
230 double get_state_interface(const std::string& name, const std::string& interface) const;
+
231
+
239 double get_command_interface(const std::string& name, const std::string& interface) const;
+
240
+
247 void set_command_interface(const std::string& name, const std::string& interface, double value);
+
248
+
257 void add_parameter(
+
258 const std::shared_ptr<state_representation::ParameterInterface>& parameter, const std::string& description,
+
259 bool read_only = false);
+
260
+
271 template<typename T>
+
272 void add_parameter(const std::string& name, const T& value, const std::string& description, bool read_only = false);
+
273
+
284 virtual bool
+
285 on_validate_parameter_callback(const std::shared_ptr<state_representation::ParameterInterface>& parameter);
+
286
+
292 [[nodiscard]] std::shared_ptr<state_representation::ParameterInterface> get_parameter(const std::string& name) const;
+
293
+
300 template<typename T>
+
301 T get_parameter_value(const std::string& name) const;
+
302
+
311 template<typename T>
+
312 void set_parameter_value(const std::string& name, const T& value);
+
313
+
319 void add_predicate(const std::string& predicate_name, bool predicate_value);
+
320
+
326 void add_predicate(const std::string& predicate_name, const std::function<bool(void)>& predicate_function);
+
327
+
334 [[nodiscard]] bool get_predicate(const std::string& predicate_name) const;
+
335
+
343 void set_predicate(const std::string& predicate_name, bool predicate_value);
+
344
+
352 void set_predicate(const std::string& predicate_name, const std::function<bool(void)>& predicate_function);
+
353
+
360 void add_trigger(const std::string& trigger_name);
+
361
+
366 void trigger(const std::string& trigger_name);
+
367
+
376 template<typename T>
+
377 void add_input(const std::string& name, const std::string& topic_name = "");
+
378
+
387 template<typename T>
+
388 void add_output(const std::string& name, const std::string& topic_name = "");
+
389
+
397 template<typename T>
+
398 std::optional<T> read_input(const std::string& name);
+
399
+
408 template<typename T>
+
409 void write_output(const std::string& name, const T& data);
+
410
+
416 void add_service(const std::string& service_name, const std::function<ControllerServiceResponse(void)>& callback);
+
417
+
426 void add_service(
+
427 const std::string& service_name,
+
428 const std::function<ControllerServiceResponse(const std::string& string)>& callback);
+
429
+
434 [[nodiscard]] rclcpp::QoS get_qos() const;
+
435
+
440 void set_qos(const rclcpp::QoS& qos);
+
441
+
446 bool is_active() const;
+
447
+
448 std::string hardware_name_;
+
449
+
450private:
+
458 void add_interface(
+
459 const std::string& name, const std::string& interface, std::vector<std::string>& list, const std::string& type);
+
460
+
468 bool validate_parameter(const std::shared_ptr<state_representation::ParameterInterface>& parameter);
+
469
+
475 rcl_interfaces::msg::SetParametersResult on_set_parameters_callback(const std::vector<rclcpp::Parameter>& parameters);
+
476
+
482 void add_variant_predicate(const std::string& name, const utilities::PredicateVariant& predicate);
+
483
+
489 void set_variant_predicate(const std::string& name, const utilities::PredicateVariant& predicate);
+
490
+
495 modulo_interfaces::msg::Predicate get_predicate_message(const std::string& name) const;
+
496
+
501 void publish_predicate(const std::string& name) const;
+
502
+
506 void publish_predicates() const;
+
507
+
515 std::string validate_and_declare_signal(
+
516 const std::string& signal_name, const std::string& type, const std::string& default_topic,
+
517 bool fixed_topic = false);
+
518
+
527 void create_input(const ControllerInput& input, const std::string& name, const std::string& topic_name);
+
528
+
537 void create_output(const PublisherVariant& publishers, const std::string& name, const std::string& topic_name);
+
538
+
545 template<typename T>
+
546 std::shared_ptr<rclcpp::Subscription<T>> create_subscription(const std::string& name, const std::string& topic_name);
+
547
+
552 bool check_input_valid(const std::string& name) const;
+
553
+
557 template<typename PublisherT, typename MsgT, typename T>
+
558 void write_std_output(const std::string& name, const T& data);
+
559
+
563 void add_inputs();
+
564
+
568 void add_outputs();
+
569
+
576 std::string validate_service_name(const std::string& service_name, const std::string& type) const;
+
577
+
578 using controller_interface::ControllerInterfaceBase::command_interfaces_;
+
579 using controller_interface::ControllerInterfaceBase::state_interfaces_;
+
580
+
581 state_representation::ParameterMap parameter_map_;
+
582 bool set_parameter_callback_called_ = false;
+
583 std::shared_ptr<rclcpp::node_interfaces::OnSetParametersCallbackHandle>
+
584 parameter_cb_handle_;
+
585
+
586 std::vector<SubscriptionVariant> subscriptions_;
+
587 std::map<std::string, ControllerInput> inputs_;
+
588 std::map<std::string, std::shared_ptr<modulo_core::communication::MessagePairInterface>>
+
589 input_message_pairs_;
+
590 std::map<std::string, PublisherVariant> outputs_;
+
591 double input_validity_period_;
+
592 rclcpp::QoS qos_ = rclcpp::QoS(10);
+
593
+
594 std::map<std::string, std::shared_ptr<rclcpp::Service<modulo_component_interfaces::srv::EmptyTrigger>>>
+
595 empty_services_;
+
596 std::map<std::string, std::shared_ptr<rclcpp::Service<modulo_component_interfaces::srv::StringTrigger>>>
+
597 string_services_;
+
598
+
599 std::map<std::string, utilities::PredicateVariant> predicates_;
+
600 std::shared_ptr<rclcpp::Publisher<modulo_interfaces::msg::PredicateCollection>>
+
601 predicate_publisher_;
+
602 std::map<std::string, bool> triggers_;
+
603 modulo_interfaces::msg::PredicateCollection predicate_message_;
+
604 std::shared_ptr<rclcpp::TimerBase> predicate_timer_;
+
605
+
606 std::unordered_map<std::string, std::unordered_map<std::string, double>>
+
607 state_interface_data_;
+
608 std::vector<double> command_interface_data_;
+
609 std::unordered_map<std::string, std::unordered_map<std::string, unsigned int>>
+
610 command_interface_indices_;
+
611 std::vector<std::string> state_interface_names_;
+
612 std::vector<std::string> command_interface_names_;
+
613 bool claim_all_state_interfaces_;
+
614
+
615 std::timed_mutex command_mutex_;
+
616 // TODO make missed_locks an internal parameter
+
617 unsigned int missed_locks_;
+
618 bool on_init_called_;
+
619};
+
+
620
+
621template<typename T>
+
+
622inline void ControllerInterface::add_parameter(
+
623 const std::string& name, const T& value, const std::string& description, bool read_only) {
+
624 if (name.empty()) {
+
625 RCLCPP_ERROR(get_node()->get_logger(), "Failed to add parameter: Provide a non empty string as a name.");
+
626 return;
+
627 }
+
628 add_parameter(state_representation::make_shared_parameter(name, value), description, read_only);
+
629}
+
+
630
+
631template<typename T>
+
+
632inline T ControllerInterface::get_parameter_value(const std::string& name) const {
+
633 return parameter_map_.template get_parameter_value<T>(name);
+
634}
+
+
635
+
636template<typename T>
+
+
637inline void ControllerInterface::set_parameter_value(const std::string& name, const T& value) {
+
638 try {
+
639 rcl_interfaces::msg::SetParametersResult result =
+
640 get_node()
+
641 ->set_parameters(
+
642 {modulo_core::translators::write_parameter(state_representation::make_shared_parameter(name, value))})
+
643 .at(0);
+
644 if (!result.successful) {
+
645 RCLCPP_ERROR_THROTTLE(
+
646 get_node()->get_logger(), *get_node()->get_clock(), 1000,
+
647 "Failed to set parameter value of parameter '%s': %s", name.c_str(), result.reason.c_str());
+
648 }
+
649 } catch (const std::exception& ex) {
+
650 RCLCPP_ERROR_THROTTLE(
+
651 get_node()->get_logger(), *get_node()->get_clock(), 1000, "Failed to set parameter value of parameter '%s': %s",
+
652 name.c_str(), ex.what());
+
653 }
+
654}
+
+
655
+
656template<typename T>
+
+
657inline void ControllerInterface::add_input(const std::string& name, const std::string& topic_name) {
+
658 auto buffer = realtime_tools::RealtimeBuffer<std::shared_ptr<modulo_core::EncodedState>>();
+
659 auto input = ControllerInput(buffer);
+
660 create_input(input, name, topic_name);
+
661 input_message_pairs_.insert_or_assign(
+
662 name, modulo_core::communication::make_shared_message_pair(std::make_shared<T>(), get_node()->get_clock()));
+
663}
+
+
664
+
665template<>
+
666inline void ControllerInterface::add_input<bool>(const std::string& name, const std::string& topic_name) {
+
667 auto buffer = realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Bool>>();
+
668 auto input = ControllerInput(buffer);
+
669 create_input(input, name, topic_name);
+
670}
+
671
+
672template<>
+
673inline void ControllerInterface::add_input<double>(const std::string& name, const std::string& topic_name) {
+
674 auto buffer = realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Float64>>();
+
675 auto input = ControllerInput(buffer);
+
676 create_input(input, name, topic_name);
+
677}
+
678
+
679template<>
+
680inline void
+
681ControllerInterface::add_input<std::vector<double>>(const std::string& name, const std::string& topic_name) {
+
682 auto buffer = realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Float64MultiArray>>();
+
683 auto input = ControllerInput(buffer);
+
684 create_input(input, name, topic_name);
+
685}
+
686
+
687template<>
+
688inline void ControllerInterface::add_input<int>(const std::string& name, const std::string& topic_name) {
+
689 auto buffer = realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Int32>>();
+
690 auto input = ControllerInput(buffer);
+
691 create_input(input, name, topic_name);
+
692}
+
693
+
694template<>
+
695inline void ControllerInterface::add_input<std::string>(const std::string& name, const std::string& topic_name) {
+
696 auto buffer = realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::String>>();
+
697 auto input = ControllerInput(buffer);
+
698 create_input(input, name, topic_name);
+
699}
+
700
+
701template<typename T>
+
702inline std::shared_ptr<rclcpp::Subscription<T>>
+
703ControllerInterface::create_subscription(const std::string& name, const std::string& topic_name) {
+
704 return get_node()->create_subscription<T>(topic_name, qos_, [this, name](const std::shared_ptr<T> message) {
+
705 std::get<realtime_tools::RealtimeBuffer<std::shared_ptr<T>>>(inputs_.at(name).buffer).writeFromNonRT(message);
+
706 inputs_.at(name).timestamp = std::chrono::steady_clock::now();
+
707 });
+
708}
+
709
+
710template<typename T>
+
+
711inline void ControllerInterface::add_output(const std::string& name, const std::string& topic_name) {
+
712 std::shared_ptr<state_representation::State> state_ptr = std::make_shared<T>();
+
713 create_output(EncodedStatePublishers(state_ptr, {}, {}), name, topic_name);
+
714}
+
+
715
+
716template<>
+
717inline void ControllerInterface::add_output<bool>(const std::string& name, const std::string& topic_name) {
+
718 create_output(BoolPublishers(), name, topic_name);
+
719}
+
720
+
721template<>
+
722inline void ControllerInterface::add_output<double>(const std::string& name, const std::string& topic_name) {
+
723 create_output(DoublePublishers(), name, topic_name);
+
724}
+
725
+
726template<>
+
727inline void
+
728ControllerInterface::add_output<std::vector<double>>(const std::string& name, const std::string& topic_name) {
+
729 create_output(DoubleVecPublishers(), name, topic_name);
+
730}
+
731
+
732template<>
+
733inline void ControllerInterface::add_output<int>(const std::string& name, const std::string& topic_name) {
+
734 create_output(IntPublishers(), name, topic_name);
+
735}
+
736
+
737template<>
+
738inline void ControllerInterface::add_output<std::string>(const std::string& name, const std::string& topic_name) {
+
739 create_output(StringPublishers(), name, topic_name);
+
740}
+
741
+
742template<typename T>
+
+
743inline std::optional<T> ControllerInterface::read_input(const std::string& name) {
+
744 if (!check_input_valid(name)) {
+
745 return {};
+
746 }
+
747 auto message =
+
748 *std::get<realtime_tools::RealtimeBuffer<std::shared_ptr<modulo_core::EncodedState>>>(inputs_.at(name).buffer)
+
749 .readFromNonRT();
+
750 std::shared_ptr<state_representation::State> state;
+
751 try {
+
752 auto message_pair = input_message_pairs_.at(name);
+
753 message_pair->read<modulo_core::EncodedState, state_representation::State>(*message);
+
754 state = message_pair->get_message_pair<modulo_core::EncodedState, state_representation::State>()->get_data();
+
755 } catch (const std::exception& ex) {
+
756 RCLCPP_WARN_THROTTLE(
+
757 get_node()->get_logger(), *get_node()->get_clock(), 1000,
+
758 "Could not read EncodedState message on input '%s': %s", name.c_str(), ex.what());
+
759 return {};
+
760 }
+
761 if (state->is_empty()) {
+
762 return {};
+
763 }
+
764 try {
+
765 return *std::dynamic_pointer_cast<T>(state);
+
766 } catch (const std::exception& ex) {
+
767 RCLCPP_WARN_THROTTLE(
+
768 get_node()->get_logger(), *get_node()->get_clock(), 1000, "Could not cast input '%s' to desired state type: %s",
+
769 name.c_str(), ex.what());
+
770 }
+
771 return {};
+
772}
+
+
773
+
774template<>
+
775inline std::optional<bool> ControllerInterface::read_input<bool>(const std::string& name) {
+
776 if (!check_input_valid(name)) {
+
777 return {};
+
778 }
+
779 // no need to check for emptiness of the pointer: timestamps are default constructed to 0, so an input being valid
+
780 // means that a message was received
+
781 return (*std::get<realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Bool>>>(inputs_.at(name).buffer)
+
782 .readFromNonRT())
+
783 ->data;
+
784}
+
785
+
786template<>
+
787inline std::optional<double> ControllerInterface::read_input<double>(const std::string& name) {
+
788 if (!check_input_valid(name)) {
+
789 return {};
+
790 }
+
791 return (*std::get<realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Float64>>>(inputs_.at(name).buffer)
+
792 .readFromNonRT())
+
793 ->data;
+
794}
+
795
+
796template<>
+
797inline std::optional<std::vector<double>>
+
798ControllerInterface::read_input<std::vector<double>>(const std::string& name) {
+
799 if (!check_input_valid(name)) {
+
800 return {};
+
801 }
+
802 return (*std::get<realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Float64MultiArray>>>(
+
803 inputs_.at(name).buffer)
+
804 .readFromNonRT())
+
805 ->data;
+
806}
+
807
+
808template<>
+
809inline std::optional<int> ControllerInterface::read_input<int>(const std::string& name) {
+
810 if (!check_input_valid(name)) {
+
811 return {};
+
812 }
+
813 return (*std::get<realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Int32>>>(inputs_.at(name).buffer)
+
814 .readFromNonRT())
+
815 ->data;
+
816}
+
817
+
818template<>
+
819inline std::optional<std::string> ControllerInterface::read_input<std::string>(const std::string& name) {
+
820 if (!check_input_valid(name)) {
+
821 return {};
+
822 }
+
823 return (*std::get<realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::String>>>(inputs_.at(name).buffer)
+
824 .readFromNonRT())
+
825 ->data;
+
826}
+
827
+
828template<typename T>
+
+
829inline void ControllerInterface::write_output(const std::string& name, const T& data) {
+
830 if (data.is_empty()) {
+
831 RCLCPP_DEBUG_THROTTLE(
+
832 get_node()->get_logger(), *get_node()->get_clock(), 1000,
+
833 "Skipping publication of output '%s' due to emptiness of state", name.c_str());
+
834 return;
+
835 }
+
836 if (outputs_.find(name) == outputs_.end()) {
+
837 RCLCPP_WARN_THROTTLE(
+
838 get_node()->get_logger(), *get_node()->get_clock(), 1000, "Could not find output '%s'", name.c_str());
+
839 return;
+
840 }
+
841 EncodedStatePublishers publishers;
+
842 try {
+
843 publishers = std::get<EncodedStatePublishers>(outputs_.at(name));
+
844 } catch (const std::bad_variant_access&) {
+
845 RCLCPP_WARN_THROTTLE(
+
846 get_node()->get_logger(), *get_node()->get_clock(), 1000,
+
847 "Could not retrieve publisher for output '%s': Invalid output type", name.c_str());
+
848 return;
+
849 }
+
850 if (const auto output_type = std::get<0>(publishers)->get_type(); output_type != data.get_type()) {
+
851 RCLCPP_WARN_THROTTLE(
+
852 get_node()->get_logger(), *get_node()->get_clock(), 1000,
+
853 "Skipping publication of output '%s' due to wrong data type (expected '%s', got '%s')",
+
854 state_representation::get_state_type_name(output_type).c_str(),
+
855 state_representation::get_state_type_name(data.get_type()).c_str(), name.c_str());
+
856 return;
+
857 }
+
858 auto rt_pub = std::get<2>(publishers);
+
859 if (rt_pub && rt_pub->trylock()) {
+
860 try {
+
861 modulo_core::translators::write_message<T>(rt_pub->msg_, data, get_node()->get_clock()->now());
+
862 } catch (const modulo_core::exceptions::MessageTranslationException& ex) {
+
863 RCLCPP_ERROR_THROTTLE(
+
864 get_node()->get_logger(), *get_node()->get_clock(), 1000, "Failed to publish output '%s': %s", name.c_str(),
+
865 ex.what());
+
866 }
+
867 rt_pub->unlockAndPublish();
+
868 }
+
869}
+
+
870
+
871template<typename PublisherT, typename MsgT, typename T>
+
872void ControllerInterface::write_std_output(const std::string& name, const T& data) {
+
873 if (outputs_.find(name) == outputs_.end()) {
+
874 RCLCPP_WARN_THROTTLE(
+
875 get_node()->get_logger(), *get_node()->get_clock(), 1000, "Could not find output '%s'", name.c_str());
+
876 return;
+
877 }
+
878 PublisherT publishers;
+
879 try {
+
880 publishers = std::get<PublisherT>(outputs_.at(name));
+
881 } catch (const std::bad_variant_access&) {
+
882 RCLCPP_WARN_THROTTLE(
+
883 get_node()->get_logger(), *get_node()->get_clock(), 1000,
+
884 "Could not retrieve publisher for output '%s': Invalid output type", name.c_str());
+
885 return;
+
886 }
+
887 auto rt_pub = publishers.second;
+
888 if (rt_pub && rt_pub->trylock()) {
+
889 rt_pub->msg_.data = data;
+
890 rt_pub->unlockAndPublish();
+
891 }
+
892}
+
893
+
894template<>
+
895inline void ControllerInterface::write_output(const std::string& name, const bool& data) {
+
896 write_std_output<BoolPublishers, std_msgs::msg::Bool, bool>(name, data);
+
897}
+
898
+
899template<>
+
900inline void ControllerInterface::write_output(const std::string& name, const double& data) {
+
901 write_std_output<DoublePublishers, std_msgs::msg::Float64, double>(name, data);
+
902}
+
903
+
904template<>
+
905inline void ControllerInterface::write_output(const std::string& name, const std::vector<double>& data) {
+
906 write_std_output<DoubleVecPublishers, std_msgs::msg::Float64MultiArray, std::vector<double>>(name, data);
+
907}
+
908
+
909template<>
+
910inline void ControllerInterface::write_output(const std::string& name, const int& data) {
+
911 write_std_output<IntPublishers, std_msgs::msg::Int32, int>(name, data);
+
912}
+
913
+
914template<>
+
915inline void ControllerInterface::write_output(const std::string& name, const std::string& data) {
+
916 write_std_output<StringPublishers, std_msgs::msg::String, std::string>(name, data);
+
917}
+
918
+
919}// namespace modulo_controllers
+
modulo_controllers::ControllerInterface
Base controller class to combine ros2_control, control libraries and modulo.
Definition ControllerInterface.hpp:98
+
modulo_controllers::ControllerInterface::add_output
void add_output(const std::string &name, const std::string &topic_name="")
Add an output to the controller.
Definition ControllerInterface.hpp:711
+
modulo_controllers::ControllerInterface::write_output
void write_output(const std::string &name, const T &data)
Write an object to an output.
Definition ControllerInterface.hpp:829
+
modulo_controllers::ControllerInterface::get_parameter
std::shared_ptr< state_representation::ParameterInterface > get_parameter(const std::string &name) const
Get a parameter by name.
Definition ControllerInterface.cpp:332
+
modulo_controllers::ControllerInterface::get_qos
rclcpp::QoS get_qos() const
Getter of the Quality of Service attribute.
Definition ControllerInterface.cpp:743
+
modulo_controllers::ControllerInterface::add_predicate
void add_predicate(const std::string &predicate_name, bool predicate_value)
Add a predicate to the map of predicates.
Definition ControllerInterface.cpp:383
+
modulo_controllers::ControllerInterface::set_predicate
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...
Definition ControllerInterface.cpp:431
+
modulo_controllers::ControllerInterface::command_interface_configuration
controller_interface::InterfaceConfiguration command_interface_configuration() const final
Configure the command interfaces.
Definition ControllerInterface.cpp:121
+
modulo_controllers::ControllerInterface::write_command_interfaces
virtual controller_interface::return_type write_command_interfaces(const rclcpp::Duration &period)
Write the command interfaces.
Definition ControllerInterface.cpp:267
+
modulo_controllers::ControllerInterface::on_configure
virtual CallbackReturn on_configure()
Configure the controller.
Definition ControllerInterface.cpp:91
+
modulo_controllers::ControllerInterface::evaluate
virtual controller_interface::return_type evaluate(const rclcpp::Time &time, const std::chrono::nanoseconds &period)=0
The control logic callback.
+
modulo_controllers::ControllerInterface::get_parameter_value
T get_parameter_value(const std::string &name) const
Get a parameter value by name.
Definition ControllerInterface.hpp:632
+
modulo_controllers::ControllerInterface::read_state_interfaces
virtual controller_interface::return_type read_state_interfaces()
Read the state interfaces.
Definition ControllerInterface.cpp:258
+
modulo_controllers::ControllerInterface::get_state_interfaces
std::unordered_map< std::string, double > get_state_interfaces(const std::string &name) const
Get a map containing the state interfaces by name of the parent tag.
Definition ControllerInterface.cpp:275
+
modulo_controllers::ControllerInterface::is_active
bool is_active() const
Check if the controller is currently in state active or not.
Definition ControllerInterface.cpp:751
+
modulo_controllers::ControllerInterface::on_init
CallbackReturn on_init() override
Declare parameters and register the on_set_parameters callback.
Definition ControllerInterface.cpp:27
+
modulo_controllers::ControllerInterface::set_qos
void set_qos(const rclcpp::QoS &qos)
Set the Quality of Service for ROS publishers and subscribers.
Definition ControllerInterface.cpp:747
+
modulo_controllers::ControllerInterface::trigger
void trigger(const std::string &trigger_name)
Latch the trigger with the provided name.
Definition ControllerInterface.cpp:470
+
modulo_controllers::ControllerInterface::get_predicate
bool get_predicate(const std::string &predicate_name) const
Get the logical value of a predicate.
Definition ControllerInterface.cpp:404
+
modulo_controllers::ControllerInterface::read_input
std::optional< T > read_input(const std::string &name)
Read the most recent message of an input.
Definition ControllerInterface.hpp:743
+
modulo_controllers::ControllerInterface::add_state_interface
void add_state_interface(const std::string &name, const std::string &interface)
Add a state interface to the controller by name.
Definition ControllerInterface.cpp:95
+
modulo_controllers::ControllerInterface::get_command_interface
double get_command_interface(const std::string &name, const std::string &interface) const
Get the value of a command interface by name.
Definition ControllerInterface.cpp:283
+
modulo_controllers::ControllerInterface::on_activate
virtual CallbackReturn on_activate()
Activate the controller.
Definition ControllerInterface.cpp:204
+
modulo_controllers::ControllerInterface::add_service
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.
Definition ControllerInterface.cpp:676
+
modulo_controllers::ControllerInterface::get_state_interface
double get_state_interface(const std::string &name, const std::string &interface) const
Get the value of a state interface by name.
Definition ControllerInterface.cpp:279
+
modulo_controllers::ControllerInterface::update
controller_interface::return_type update(const rclcpp::Time &time, const rclcpp::Duration &period) final
Read the state interfaces, perform control evaluation and write the command interfaces.
Definition ControllerInterface.cpp:218
+
modulo_controllers::ControllerInterface::set_command_interface
void set_command_interface(const std::string &name, const std::string &interface, double value)
Set the value of a command interface by name.
Definition ControllerInterface.cpp:287
+
modulo_controllers::ControllerInterface::add_parameter
void add_parameter(const std::shared_ptr< state_representation::ParameterInterface > &parameter, const std::string &description, bool read_only=false)
Add a parameter.
+
modulo_controllers::ControllerInterface::add_input
void add_input(const std::string &name, const std::string &topic_name="")
Add an input to the controller.
Definition ControllerInterface.hpp:657
+
modulo_controllers::ControllerInterface::add_trigger
void add_trigger(const std::string &trigger_name)
Add a trigger to the controller.
Definition ControllerInterface.cpp:451
+
modulo_controllers::ControllerInterface::hardware_name_
std::string hardware_name_
The hardware name provided by a parameter.
Definition ControllerInterface.hpp:448
+
modulo_controllers::ControllerInterface::on_deactivate
virtual CallbackReturn on_deactivate()
Deactivate the controller.
Definition ControllerInterface.cpp:213
+
modulo_controllers::ControllerInterface::set_parameter_value
void set_parameter_value(const std::string &name, const T &value)
Set the value of a parameter.
Definition ControllerInterface.hpp:637
+
modulo_controllers::ControllerInterface::add_command_interface
void add_command_interface(const std::string &name, const std::string &interface)
Add a command interface to the controller by name.
Definition ControllerInterface.cpp:99
+
modulo_controllers::ControllerInterface::state_interface_configuration
controller_interface::InterfaceConfiguration state_interface_configuration() const final
Configure the state interfaces.
Definition ControllerInterface.cpp:138
+
modulo_controllers::ControllerInterface::on_validate_parameter_callback
virtual bool on_validate_parameter_callback(const std::shared_ptr< state_representation::ParameterInterface > &parameter)
Parameter validation function to be redefined by derived controller classes.
Definition ControllerInterface.cpp:379
+
modulo_core::exceptions::MessageTranslationException
An exception class to notify that the translation of a ROS message failed.
Definition MessageTranslationException.hpp:11
+
modulo_core::translators::write_parameter
rclcpp::Parameter write_parameter(const std::shared_ptr< state_representation::ParameterInterface > &parameter)
Write a ROS Parameter from a ParameterInterface pointer.
+
modulo_core::EncodedState
std_msgs::msg::UInt8MultiArray EncodedState
Define the EncodedState as UInt8MultiArray message type.
Definition EncodedState.hpp:15
+
modulo_controllers::ControllerInput
Input structure to save topic data in a realtime buffer and timestamps in one object.
Definition ControllerInterface.hpp:77
+
modulo_controllers::ControllerServiceResponse
Response structure to be returned by controller services.
Definition ControllerInterface.hpp:89
+
+ +
+Generated by doxygen 1.9.8 +
+ + diff --git a/versions/main/_robot_controller_interface_8cpp_source.html b/versions/main/_robot_controller_interface_8cpp_source.html new file mode 100644 index 000000000..866113694 --- /dev/null +++ b/versions/main/_robot_controller_interface_8cpp_source.html @@ -0,0 +1,503 @@ + + + + + + + +Modulo: /github/workspace/source/modulo_controllers/src/RobotControllerInterface.cpp Source File + + + + + + + + + +
+
+ + + + + + +
+
Modulo 4.1.0 +
+
+
+ + + + + + + + + +
+
+ + +
+
+
+
+
+
Loading...
+
Searching...
+
No Matches
+
+
+
+
+ + +
+
+
RobotControllerInterface.cpp
+
+
+
1#include "modulo_controllers/RobotControllerInterface.hpp"
+
2
+
3#include <hardware_interface/types/hardware_interface_type_values.hpp>
+
4
+
5#include <state_representation/exceptions/JointNotFoundException.hpp>
+
6
+
7using namespace state_representation;
+
8
+
9namespace modulo_controllers {
+
10
+
11static const std::map<std::string, JointStateVariable> interface_map =// NOLINT(cert-err58-cpp)
+
12 {{hardware_interface::HW_IF_POSITION, JointStateVariable::POSITIONS},
+
13 {hardware_interface::HW_IF_VELOCITY, JointStateVariable::VELOCITIES},
+
14 {hardware_interface::HW_IF_ACCELERATION, JointStateVariable::ACCELERATIONS},
+
15 {hardware_interface::HW_IF_EFFORT, JointStateVariable::TORQUES}};
+
16
+
17RobotControllerInterface::RobotControllerInterface() : RobotControllerInterface(false, "") {}
+
18
+
+
19RobotControllerInterface::RobotControllerInterface(bool robot_model_required, const std::string& control_type)
+
20 : ControllerInterface(true),
+
21 control_type_(control_type),
+
22 robot_model_required_(robot_model_required),
+
23 new_joint_command_ready_(false),
+
24 command_decay_factor_(0.0),
+
25 command_rate_limit_(std::numeric_limits<double>::infinity()) {
+
26 if (!control_type.empty() && interface_map.find(control_type) == interface_map.cend()) {
+
27 RCLCPP_ERROR(get_node()->get_logger(), "Invalid control type: %s", control_type.c_str());
+
28 throw std::invalid_argument("Invalid control type");
+
29 }
+
30}
+
+
31
+
+
32rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotControllerInterface::on_init() {
+
33 auto status = ControllerInterface::on_init();
+
34 if (status != CallbackReturn::SUCCESS) {
+
35 return status;
+
36 }
+
37
+
38 try {
+
39 add_parameter(
+
40 std::make_shared<Parameter<std::string>>("task_space_frame"),
+
41 "The frame name in the robot model to use for kinematics calculations (defaults to the last frame in the "
+
42 "model)");
+
43 add_parameter<bool>(
+
44 "sort_joints", true,
+
45 "If true, re-arrange the 'joints' parameter into a physically correct order according to the robot model");
+
46 add_parameter(
+
47 std::make_shared<Parameter<std::string>>("ft_sensor_name"),
+
48 "Optionally, the name of a force-torque sensor in the hardware interface");
+
49 add_parameter(
+
50 std::make_shared<Parameter<std::string>>("ft_sensor_reference_frame"),
+
51 "The reference frame of the force-torque sensor in the robot model");
+
52 add_parameter<double>(
+
53 "command_half_life", 0.1,
+
54 "A time constant for the exponential decay of the commanded velocity, acceleration or torque if no new command "
+
55 "is set");
+
56 add_parameter<double>(
+
57 "command_rate_limit", command_rate_limit_,
+
58 "The maximum allowable change in command on any interface expressed in command units / second");
+
59 } catch (const std::exception& e) {
+
60 RCLCPP_ERROR(get_node()->get_logger(), "Exception thrown during on_init stage with message: %s \n", e.what());
+
61 return CallbackReturn::ERROR;
+
62 }
+
63 return CallbackReturn::SUCCESS;
+
64}
+
+
65
+
+
66rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotControllerInterface::on_configure() {
+
67 if (*get_parameter("robot_description")) {
+
68 std::stringstream timestamp;
+
69 timestamp << std::time(nullptr);
+
70 auto urdf_path = "/tmp/" + hardware_name_ + "_" + timestamp.str();
+
71 try {
+
72 robot_model::Model::create_urdf_from_string(get_parameter_value<std::string>("robot_description"), urdf_path);
+
73 robot_ = std::make_shared<robot_model::Model>(hardware_name_, urdf_path);
+
74 RCLCPP_DEBUG(get_node()->get_logger(), "Generated robot model");
+
75 } catch (const std::exception& ex) {
+
76 RCLCPP_WARN(
+
77 get_node()->get_logger(),
+
78 "Could not generate robot model with temporary urdf from string content at path %s: %s", urdf_path.c_str(),
+
79 ex.what());
+
80 }
+
81 }
+
82 if (robot_model_required_ && robot_ == nullptr) {
+
83 RCLCPP_ERROR(get_node()->get_logger(), "Robot model is not available even though it's required by the controller.");
+
84 return CallbackReturn::ERROR;
+
85 }
+
86
+
87 if (robot_) {
+
88 if (get_parameter("task_space_frame")->is_empty()) {
+
89 task_space_frame_ = robot_->get_frames().back();
+
90 } else {
+
91 task_space_frame_ = get_parameter_value<std::string>("task_space_frame");
+
92 if (!robot_->get_pinocchio_model().existFrame(task_space_frame_)) {
+
93 RCLCPP_ERROR(
+
94 get_node()->get_logger(), "Provided task space frame %s does not exist in the robot model!",
+
95 task_space_frame_.c_str());
+
96 return CallbackReturn::ERROR;
+
97 }
+
98 }
+
99 }
+
100
+
101 auto joints = get_parameter("joints");
+
102 if (joints->is_empty() && !control_type_.empty()) {
+
103 RCLCPP_ERROR(
+
104 get_node()->get_logger(), "The 'joints' parameter is required for a non-zero control type %s!",
+
105 control_type_.c_str());
+
106 return CallbackReturn::ERROR;
+
107 }
+
108 joints_ = joints->get_parameter_value<std::vector<std::string>>();
+
109
+
110 // sort the interface joint names using the robot model if necessary
+
111 if (get_parameter_value<bool>("sort_joints") && robot_) {
+
112 if (joints_.size() != robot_->get_number_of_joints()) {
+
113 RCLCPP_WARN(
+
114 get_node()->get_logger(),
+
115 "The number of interface joints (%zu) does not match the number of joints in the robot model (%u)",
+
116 joints_.size(), robot_->get_number_of_joints());
+
117 }
+
118 std::vector<std::string> ordered_joints;
+
119 for (const auto& ordered_name : robot_->get_joint_frames()) {
+
120 for (const auto& joint_name : joints_) {
+
121 if (joint_name == ordered_name) {
+
122 ordered_joints.push_back(joint_name);
+
123 break;
+
124 }
+
125 }
+
126 }
+
127 if (ordered_joints.size() < joints_.size()) {
+
128 RCLCPP_ERROR(
+
129 get_node()->get_logger(), "%zu interface joints were not found in the robot model",
+
130 joints_.size() - ordered_joints.size());
+
131 return CallbackReturn::ERROR;
+
132 }
+
133 joints_ = ordered_joints;
+
134 }
+
135 joint_state_ = JointState(hardware_name_, joints_);
+
136
+
137 // set command interfaces from joints
+
138 if (!control_type_.empty()) {
+
139 if (control_type_ == hardware_interface::HW_IF_POSITION) {
+
140 previous_joint_command_values_ = std::vector<double>(joints_.size(), std::numeric_limits<double>::quiet_NaN());
+
141 } else {
+
142 previous_joint_command_values_ = std::vector<double>(joints_.size(), 0.0);
+
143 }
+
144 for (const auto& joint : joints_) {
+
145 add_command_interface(joint, control_type_);
+
146 }
+
147 }
+
148
+
149 auto ft_sensor_name = get_parameter("ft_sensor_name");
+
150 if (!ft_sensor_name->is_empty()) {
+
151 ft_sensor_name_ = ft_sensor_name->get_parameter_value<std::string>();
+
152 auto ft_sensor_reference_frame = get_parameter("ft_sensor_reference_frame");
+
153 if (ft_sensor_reference_frame->is_empty()) {
+
154 RCLCPP_ERROR(
+
155 get_node()->get_logger(),
+
156 "The 'ft_sensor_reference_frame' parameter cannot be empty if a force-torque sensor is specified!");
+
157 return CallbackReturn::ERROR;
+
158 }
+
159 ft_sensor_reference_frame_ = ft_sensor_reference_frame->get_parameter_value<std::string>();
+
160 // TODO check that there is no joint in between
+
161 if (robot_ != nullptr && !robot_->get_pinocchio_model().existFrame(ft_sensor_reference_frame_)) {
+
162 RCLCPP_ERROR(
+
163 get_node()->get_logger(), "The FT sensor reference frame '%s' does not exist on the robot model!",
+
164 ft_sensor_reference_frame_.c_str());
+
165 return CallbackReturn::ERROR;
+
166 }
+
167 }
+
168
+
169 command_decay_factor_ = -log(0.5) / get_parameter_value<double>("command_half_life");
+
170 command_rate_limit_ = get_parameter_value<double>("command_rate_limit");
+
171
+
172 RCLCPP_DEBUG(get_node()->get_logger(), "Configuration of RobotControllerInterface successful");
+
173 return CallbackReturn::SUCCESS;
+
174}
+
+
175
+
+
176rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotControllerInterface::on_activate() {
+
177 // initialize a force torque sensor state if applicable
+
178 if (!ft_sensor_name_.empty()) {
+
179 try {
+
180 get_state_interface(ft_sensor_name_, "force.x");
+
181 get_state_interface(ft_sensor_name_, "force.y");
+
182 get_state_interface(ft_sensor_name_, "force.z");
+
183 get_state_interface(ft_sensor_name_, "torque.x");
+
184 get_state_interface(ft_sensor_name_, "torque.y");
+
185 get_state_interface(ft_sensor_name_, "torque.z");
+
186 } catch (const std::out_of_range&) {
+
187 RCLCPP_ERROR(
+
188 get_node()->get_logger(), "The force torque sensor '%s' does not have all the expected state interfaces!",
+
189 ft_sensor_name_.c_str());
+
190 return CallbackReturn::FAILURE;
+
191 }
+
192 ft_sensor_ = CartesianWrench(ft_sensor_name_, ft_sensor_reference_frame_);
+
193 }
+
194
+
195 RCLCPP_DEBUG(get_node()->get_logger(), "Activation of RobotControllerInterface successful");
+
196 return CallbackReturn::SUCCESS;
+
197}
+
+
198
+
199controller_interface::return_type RobotControllerInterface::read_state_interfaces() {
+
200 auto status = ControllerInterface::read_state_interfaces();
+
201 if (status != controller_interface::return_type::OK) {
+
202 return status;
+
203 }
+
204
+
205 if (joint_state_.get_size() > 0 && joint_state_.is_empty()) {
+
206 RCLCPP_DEBUG(get_node()->get_logger(), "Reading first joint state");
+
207 joint_state_.set_zero();
+
208 }
+
209
+
210 for (const auto& joint : joints_) {
+
211 auto data = get_state_interfaces(joint);
+
212 unsigned int joint_index;
+
213 try {
+
214 joint_index = joint_state_.get_joint_index(joint);
+
215 } catch (exceptions::JointNotFoundException& ex) {
+
216 RCLCPP_ERROR_STREAM_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 1000, ex.what());
+
217 return controller_interface::return_type::ERROR;
+
218 }
+
219 for (const auto& [interface, value] : data) {
+
220 if (!std::isfinite(value)) {
+
221 RCLCPP_WARN_THROTTLE(
+
222 get_node()->get_logger(), *get_node()->get_clock(), 1000, "Value of state interface '%s/%s' is not finite",
+
223 joint.c_str(), interface.c_str());
+
224 return controller_interface::return_type::ERROR;
+
225 }
+
226 switch (interface_map.at(interface)) {
+
227 case JointStateVariable::POSITIONS:
+
228 joint_state_.set_position(value, joint_index);
+
229 break;
+
230 case JointStateVariable::VELOCITIES:
+
231 joint_state_.set_velocity(value, joint_index);
+
232 break;
+
233 case JointStateVariable::ACCELERATIONS:
+
234 joint_state_.set_acceleration(value, joint_index);
+
235 break;
+
236 case JointStateVariable::TORQUES:
+
237 joint_state_.set_torque(value, joint_index);
+
238 break;
+
239 default:
+
240 break;
+
241 }
+
242 }
+
243 }
+
244
+
245 if (!ft_sensor_name_.empty()) {
+
246 const auto& ft_sensor = get_state_interfaces(ft_sensor_name_);
+
247 std::vector<double> wrench = {ft_sensor.at("force.x"), ft_sensor.at("force.y"), ft_sensor.at("force.z"),
+
248 ft_sensor.at("torque.x"), ft_sensor.at("torque.y"), ft_sensor.at("torque.z")};
+
249 ft_sensor_.set_wrench(wrench);
+
250 }
+
251
+
252 return controller_interface::return_type::OK;
+
253}
+
254
+
255controller_interface::return_type RobotControllerInterface::write_command_interfaces(const rclcpp::Duration& period) {
+
256 if (!control_type_.empty()) {
+
257 double decay = 1.0 - command_decay_factor_ * period.seconds();
+
258 decay = std::max(0.0, decay);
+
259 double rate_limit = command_rate_limit_ * period.seconds();
+
260
+
261 auto joint_command_values = joint_command_values_.readFromRT();
+
262 if (joint_command_values == nullptr) {
+
263 new_joint_command_ready_ = false;
+
264 }
+
265
+
266 for (std::size_t index = 0; index < joints_.size(); ++index) {
+
267 double previous_command = previous_joint_command_values_.at(index);
+
268 if (new_joint_command_ready_) {
+
269 auto new_command = joint_command_values->at(index);
+
270 if (std::isfinite(previous_command) && std::abs(new_command - previous_command) > rate_limit) {
+
271 double command_offset = new_command - previous_command > 0.0 ? rate_limit : -rate_limit;
+
272 new_command = previous_command + command_offset;
+
273 }
+
274 set_command_interface(joints_.at(index), control_type_, new_command);
+
275 previous_joint_command_values_.at(index) = new_command;
+
276 } else if (control_type_ != hardware_interface::HW_IF_POSITION) {
+
277 auto new_command = previous_command * decay;
+
278 set_command_interface(joints_.at(index), control_type_, new_command);
+
279 previous_joint_command_values_.at(index) = new_command;
+
280 }
+
281 }
+
282 new_joint_command_ready_ = false;
+
283 }
+
284
+
285 return ControllerInterface::write_command_interfaces(period);
+
286}
+
287
+
+
288const JointState& RobotControllerInterface::get_joint_state() {
+
289 return joint_state_;
+
290}
+
+
291
+
+
292const CartesianState& RobotControllerInterface::get_cartesian_state() {
+
293 // TODO check if recompute is necessary?
+
294 compute_cartesian_state();
+
295 return cartesian_state_;
+
296}
+
+
297
+
+
298const CartesianWrench& RobotControllerInterface::get_ft_sensor() {
+
299 return ft_sensor_;
+
300}
+
+
301
+
+
302void RobotControllerInterface::compute_cartesian_state() {
+
303 if (robot_ != nullptr) {
+
304 cartesian_state_ = robot_->forward_kinematics(joint_state_, task_space_frame_);
+
305 cartesian_state_.set_twist(robot_->forward_velocity(joint_state_, task_space_frame_).get_twist());
+
306
+
307 if (!ft_sensor_.is_empty() && (ft_sensor_.get_reference_frame() == cartesian_state_.get_name())) {
+
308 auto ft_sensor_in_robot_frame = cartesian_state_ * ft_sensor_;
+
309 cartesian_state_.set_wrench(ft_sensor_in_robot_frame.get_wrench());
+
310 }
+
311 }
+
312}
+
+
313
+
+
314void RobotControllerInterface::set_joint_command(const JointState& joint_command) {
+
315 if (!joint_command) {
+
316 RCLCPP_DEBUG(get_node()->get_logger(), "set_joint_command called with an empty JointState");
+
317 return;
+
318 }
+
319
+
320 std::vector<double> joint_command_values;
+
321 joint_command_values.reserve(joints_.size());
+
322
+
323 for (const auto& joint : joints_) {
+
324 try {
+
325 switch (interface_map.at(control_type_)) {
+
326 case JointStateVariable::POSITIONS:
+
327 joint_command_values.push_back(joint_command.get_position(joint));
+
328 break;
+
329 case JointStateVariable::VELOCITIES:
+
330 joint_command_values.push_back(joint_command.get_velocity(joint));
+
331 break;
+
332 case JointStateVariable::ACCELERATIONS:
+
333 joint_command_values.push_back(joint_command.get_acceleration(joint));
+
334 break;
+
335 case JointStateVariable::TORQUES:
+
336 joint_command_values.push_back(joint_command.get_torque(joint));
+
337 break;
+
338 default:
+
339 break;
+
340 }
+
341 } catch (const state_representation::exceptions::JointNotFoundException& ex) {
+
342 RCLCPP_WARN_STREAM_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 1000, ex.what());
+
343 }
+
344 }
+
345
+
346 if (joint_command_values.size() == joints_.size()) {
+
347 joint_command_values_.writeFromNonRT(joint_command_values);
+
348 new_joint_command_ready_ = true;
+
349 } else {
+
350 RCLCPP_WARN_THROTTLE(
+
351 get_node()->get_logger(), *get_node()->get_clock(), 1000,
+
352 "Unable to set command values from provided joint command");
+
353 }
+
354}
+
+
355
+
+
356bool RobotControllerInterface::on_validate_parameter_callback(const std::shared_ptr<ParameterInterface>& parameter) {
+
357 if ((parameter->get_name() == "command_half_life" || parameter->get_name() == "command_rate_limit")
+
358 && parameter->get_parameter_value<double>() < 0.0) {
+
359 RCLCPP_ERROR(
+
360 get_node()->get_logger(), "Parameter value of '%s' should be greater than 0", parameter->get_name().c_str());
+
361 return false;
+
362 }
+
363 return true;
+
364}
+
+
365
+
366}// namespace modulo_controllers
+
modulo_controllers::ControllerInterface
Base controller class to combine ros2_control, control libraries and modulo.
Definition ControllerInterface.hpp:98
+
modulo_controllers::ControllerInterface::get_parameter
std::shared_ptr< state_representation::ParameterInterface > get_parameter(const std::string &name) const
Get a parameter by name.
Definition ControllerInterface.cpp:332
+
modulo_controllers::ControllerInterface::write_command_interfaces
virtual controller_interface::return_type write_command_interfaces(const rclcpp::Duration &period)
Write the command interfaces.
Definition ControllerInterface.cpp:267
+
modulo_controllers::ControllerInterface::get_parameter_value
T get_parameter_value(const std::string &name) const
Get a parameter value by name.
Definition ControllerInterface.hpp:632
+
modulo_controllers::ControllerInterface::read_state_interfaces
virtual controller_interface::return_type read_state_interfaces()
Read the state interfaces.
Definition ControllerInterface.cpp:258
+
modulo_controllers::ControllerInterface::get_state_interfaces
std::unordered_map< std::string, double > get_state_interfaces(const std::string &name) const
Get a map containing the state interfaces by name of the parent tag.
Definition ControllerInterface.cpp:275
+
modulo_controllers::ControllerInterface::on_init
CallbackReturn on_init() override
Declare parameters and register the on_set_parameters callback.
Definition ControllerInterface.cpp:27
+
modulo_controllers::ControllerInterface::get_state_interface
double get_state_interface(const std::string &name, const std::string &interface) const
Get the value of a state interface by name.
Definition ControllerInterface.cpp:279
+
modulo_controllers::ControllerInterface::set_command_interface
void set_command_interface(const std::string &name, const std::string &interface, double value)
Set the value of a command interface by name.
Definition ControllerInterface.cpp:287
+
modulo_controllers::ControllerInterface::add_parameter
void add_parameter(const std::shared_ptr< state_representation::ParameterInterface > &parameter, const std::string &description, bool read_only=false)
Add a parameter.
+
modulo_controllers::ControllerInterface::hardware_name_
std::string hardware_name_
The hardware name provided by a parameter.
Definition ControllerInterface.hpp:448
+
modulo_controllers::ControllerInterface::add_command_interface
void add_command_interface(const std::string &name, const std::string &interface)
Add a command interface to the controller by name.
Definition ControllerInterface.cpp:99
+
modulo_controllers::RobotControllerInterface
Base controller class that automatically associates joints with a JointState object.
Definition RobotControllerInterface.hpp:23
+
modulo_controllers::RobotControllerInterface::RobotControllerInterface
RobotControllerInterface()
Default constructor.
Definition RobotControllerInterface.cpp:17
+
modulo_controllers::RobotControllerInterface::ft_sensor_name_
std::string ft_sensor_name_
The name of a force torque sensor in the hardware description.
Definition RobotControllerInterface.hpp:98
+
modulo_controllers::RobotControllerInterface::get_cartesian_state
const state_representation::CartesianState & get_cartesian_state()
Access the Cartesian state object.
Definition RobotControllerInterface.cpp:292
+
modulo_controllers::RobotControllerInterface::on_activate
CallbackReturn on_activate() override
Activate the controller.
Definition RobotControllerInterface.cpp:176
+
modulo_controllers::RobotControllerInterface::on_validate_parameter_callback
bool on_validate_parameter_callback(const std::shared_ptr< state_representation::ParameterInterface > &parameter) override
Parameter validation function to be redefined by derived controller classes.
Definition RobotControllerInterface.cpp:356
+
modulo_controllers::RobotControllerInterface::get_joint_state
const state_representation::JointState & get_joint_state()
Access the joint state object.
Definition RobotControllerInterface.cpp:288
+
modulo_controllers::RobotControllerInterface::ft_sensor_reference_frame_
std::string ft_sensor_reference_frame_
The sensing reference frame.
Definition RobotControllerInterface.hpp:99
+
modulo_controllers::RobotControllerInterface::get_ft_sensor
const state_representation::CartesianWrench & get_ft_sensor()
Access the Cartesian wrench object.
Definition RobotControllerInterface.cpp:298
+
modulo_controllers::RobotControllerInterface::robot_
std::shared_ptr< robot_model::Model > robot_
Robot model object generated from URDF.
Definition RobotControllerInterface.hpp:95
+
modulo_controllers::RobotControllerInterface::compute_cartesian_state
void compute_cartesian_state()
Compute the Cartesian state from forward kinematics of the current joint state.
Definition RobotControllerInterface.cpp:302
+
modulo_controllers::RobotControllerInterface::on_init
CallbackReturn on_init() override
Declare parameters and register the on_set_parameters callback.
Definition RobotControllerInterface.cpp:32
+
modulo_controllers::RobotControllerInterface::set_joint_command
void set_joint_command(const state_representation::JointState &joint_command)
Set the joint command object.
Definition RobotControllerInterface.cpp:314
+
modulo_controllers::RobotControllerInterface::task_space_frame_
std::string task_space_frame_
The frame in task space for forward kinematics calculations, if applicable.
Definition RobotControllerInterface.hpp:96
+
modulo_controllers::RobotControllerInterface::on_configure
CallbackReturn on_configure() override
Configure the controller.
Definition RobotControllerInterface.cpp:66
+
+ +
+Generated by doxygen 1.9.8 +
+ + diff --git a/versions/main/_robot_controller_interface_8hpp_source.html b/versions/main/_robot_controller_interface_8hpp_source.html new file mode 100644 index 000000000..3304f421a --- /dev/null +++ b/versions/main/_robot_controller_interface_8hpp_source.html @@ -0,0 +1,179 @@ + + + + + + + +Modulo: /github/workspace/source/modulo_controllers/include/modulo_controllers/RobotControllerInterface.hpp Source File + + + + + + + + + +
+
+ + + + + + +
+
Modulo 4.1.0 +
+
+
+ + + + + + + + + +
+
+ + +
+
+
+
+
+
Loading...
+
Searching...
+
No Matches
+
+
+
+
+ + +
+
+
RobotControllerInterface.hpp
+
+
+
1#pragma once
+
2
+
3#include <realtime_tools/realtime_buffer.h>
+
4
+
5#include <robot_model/Model.hpp>
+
6#include <state_representation/space/cartesian/CartesianState.hpp>
+
7#include <state_representation/space/cartesian/CartesianWrench.hpp>
+
8#include <state_representation/space/joint/JointState.hpp>
+
9
+
10#include "modulo_controllers/ControllerInterface.hpp"
+
11
+
12namespace modulo_controllers {
+
13
+
+
23class RobotControllerInterface : public ControllerInterface {
+
24public:
+
28 RobotControllerInterface();
+
29
+
35 explicit RobotControllerInterface(bool robot_model_required, const std::string& control_type = "");
+
36
+
41 CallbackReturn on_init() override;
+
42
+
48 CallbackReturn on_configure() override;
+
49
+
54 CallbackReturn on_activate() override;
+
55
+
56protected:
+
61 const state_representation::JointState& get_joint_state();
+
62
+
68 const state_representation::CartesianState& get_cartesian_state();
+
69
+
74 const state_representation::CartesianWrench& get_ft_sensor();
+
75
+
81 void compute_cartesian_state();
+
82
+
87 void set_joint_command(const state_representation::JointState& joint_command);
+
88
+
92 bool
+
93 on_validate_parameter_callback(const std::shared_ptr<state_representation::ParameterInterface>& parameter) override;
+
94
+
95 std::shared_ptr<robot_model::Model> robot_;
+
96 std::string task_space_frame_;
+
97
+
98 std::string ft_sensor_name_;
+
99 std::string ft_sensor_reference_frame_;
+
100
+
101private:
+
106 controller_interface::return_type read_state_interfaces() final;
+
107
+
112 controller_interface::return_type write_command_interfaces(const rclcpp::Duration& period) final;
+
113
+
114 std::vector<std::string> joints_;
+
115 std::string control_type_;
+
116
+
117 bool robot_model_required_;
+
118 bool
+
119 new_joint_command_ready_;
+
120 double
+
121 command_decay_factor_;
+
122 double
+
123 command_rate_limit_;
+
124
+
125 realtime_tools::RealtimeBuffer<std::vector<double>> joint_command_values_;
+
126 std::vector<double> previous_joint_command_values_;
+
127
+
128 state_representation::JointState joint_state_;
+
129 state_representation::CartesianState cartesian_state_;
+
130 state_representation::CartesianWrench ft_sensor_;
+
131};
+
+
132
+
133}// namespace modulo_controllers
+
modulo_controllers::ControllerInterface
Base controller class to combine ros2_control, control libraries and modulo.
Definition ControllerInterface.hpp:98
+
modulo_controllers::ControllerInterface::get_parameter_value
T get_parameter_value(const std::string &name) const
Get a parameter value by name.
Definition ControllerInterface.hpp:632
+
modulo_controllers::RobotControllerInterface
Base controller class that automatically associates joints with a JointState object.
Definition RobotControllerInterface.hpp:23
+
modulo_controllers::RobotControllerInterface::RobotControllerInterface
RobotControllerInterface()
Default constructor.
Definition RobotControllerInterface.cpp:17
+
modulo_controllers::RobotControllerInterface::ft_sensor_name_
std::string ft_sensor_name_
The name of a force torque sensor in the hardware description.
Definition RobotControllerInterface.hpp:98
+
modulo_controllers::RobotControllerInterface::get_cartesian_state
const state_representation::CartesianState & get_cartesian_state()
Access the Cartesian state object.
Definition RobotControllerInterface.cpp:292
+
modulo_controllers::RobotControllerInterface::on_activate
CallbackReturn on_activate() override
Activate the controller.
Definition RobotControllerInterface.cpp:176
+
modulo_controllers::RobotControllerInterface::on_validate_parameter_callback
bool on_validate_parameter_callback(const std::shared_ptr< state_representation::ParameterInterface > &parameter) override
Parameter validation function to be redefined by derived controller classes.
Definition RobotControllerInterface.cpp:356
+
modulo_controllers::RobotControllerInterface::get_joint_state
const state_representation::JointState & get_joint_state()
Access the joint state object.
Definition RobotControllerInterface.cpp:288
+
modulo_controllers::RobotControllerInterface::ft_sensor_reference_frame_
std::string ft_sensor_reference_frame_
The sensing reference frame.
Definition RobotControllerInterface.hpp:99
+
modulo_controllers::RobotControllerInterface::get_ft_sensor
const state_representation::CartesianWrench & get_ft_sensor()
Access the Cartesian wrench object.
Definition RobotControllerInterface.cpp:298
+
modulo_controllers::RobotControllerInterface::robot_
std::shared_ptr< robot_model::Model > robot_
Robot model object generated from URDF.
Definition RobotControllerInterface.hpp:95
+
modulo_controllers::RobotControllerInterface::compute_cartesian_state
void compute_cartesian_state()
Compute the Cartesian state from forward kinematics of the current joint state.
Definition RobotControllerInterface.cpp:302
+
modulo_controllers::RobotControllerInterface::on_init
CallbackReturn on_init() override
Declare parameters and register the on_set_parameters callback.
Definition RobotControllerInterface.cpp:32
+
modulo_controllers::RobotControllerInterface::set_joint_command
void set_joint_command(const state_representation::JointState &joint_command)
Set the joint command object.
Definition RobotControllerInterface.cpp:314
+
modulo_controllers::RobotControllerInterface::task_space_frame_
std::string task_space_frame_
The frame in task space for forward kinematics calculations, if applicable.
Definition RobotControllerInterface.hpp:96
+
modulo_controllers::RobotControllerInterface::on_configure
CallbackReturn on_configure() override
Configure the controller.
Definition RobotControllerInterface.cpp:66
+
+ +
+Generated by doxygen 1.9.8 +
+ + diff --git a/versions/main/annotated.html b/versions/main/annotated.html index 61bb9eb03..97836b883 100644 --- a/versions/main/annotated.html +++ b/versions/main/annotated.html @@ -85,26 +85,31 @@  CComponentInterfaceBase interface class for modulo components to wrap a ROS Node with custom behaviour  CComponentServiceResponseResponse structure to be returned by component services  CLifecycleComponentA wrapper for rclcpp_lifecycle::LifecycleNode to simplify application composition through unified component interfaces while supporting lifecycle states and transitions - ▼Nmodulo_coreModulo Core - ▼NcommunicationModulo Core communication module for handling messages on publication and subscription interfaces - CMessagePairThe MessagePair stores a pointer to a variable and translates the value of this pointer back and forth between the corresponding ROS messages - CMessagePairInterfaceInterface class to enable non-templated writing and reading ROS messages from derived MessagePair instances through dynamic down-casting - CPublisherHandlerThe PublisherHandler handles different types of ROS publishers to activate, deactivate and publish data with those publishers - CPublisherInterfaceInterface class to enable non-templated activating/deactivating/publishing of ROS publishers from derived PublisherHandler instances through dynamic down-casting - CSubscriptionHandlerThe SubscriptionHandler handles different types of ROS subscriptions to receive data from those subscriptions - CSubscriptionInterfaceInterface class to enable non-templated subscriptions with ROS subscriptions from derived SubscriptionHandler instances through dynamic down-casting - ▼NexceptionsModulo Core exceptions module for defining exception classes - CCoreExceptionA base class for all core exceptions - CInvalidPointerCastExceptionAn exception class to notify if the result of getting an instance of a derived class through dynamic down-casting of an object of the base class is not a correctly typed instance of the derived class - CInvalidPointerExceptionAn exception class to notify if an object has no reference count (if the object is not owned by any pointer) when attempting to get a derived instance through dynamic down-casting - CMessageTranslationExceptionAn exception class to notify that the translation of a ROS message failed - CNullPointerExceptionAn exception class to notify that a certain pointer is null - CParameterTranslationExceptionAn exception class to notify incompatibility when translating parameters from different sources - ▼Nmodulo_utils - ▼Ntestutils - CPredicatesListener - CServiceClient - CComponentInterfacePublicInterfaceFriend class to the ComponentInterface to allow test fixtures to access protected and private members + ▼Nmodulo_controllers + CControllerInputInput structure to save topic data in a realtime buffer and timestamps in one object + CControllerInterfaceBase controller class to combine ros2_control, control libraries and modulo + CControllerServiceResponseResponse structure to be returned by controller services + CRobotControllerInterfaceBase controller class that automatically associates joints with a JointState object + ▼Nmodulo_coreModulo Core + ▼NcommunicationModulo Core communication module for handling messages on publication and subscription interfaces + CMessagePairThe MessagePair stores a pointer to a variable and translates the value of this pointer back and forth between the corresponding ROS messages + CMessagePairInterfaceInterface class to enable non-templated writing and reading ROS messages from derived MessagePair instances through dynamic down-casting + CPublisherHandlerThe PublisherHandler handles different types of ROS publishers to activate, deactivate and publish data with those publishers + CPublisherInterfaceInterface class to enable non-templated activating/deactivating/publishing of ROS publishers from derived PublisherHandler instances through dynamic down-casting + CSubscriptionHandlerThe SubscriptionHandler handles different types of ROS subscriptions to receive data from those subscriptions + CSubscriptionInterfaceInterface class to enable non-templated subscriptions with ROS subscriptions from derived SubscriptionHandler instances through dynamic down-casting + ▼NexceptionsModulo Core exceptions module for defining exception classes + CCoreExceptionA base class for all core exceptions + CInvalidPointerCastExceptionAn exception class to notify if the result of getting an instance of a derived class through dynamic down-casting of an object of the base class is not a correctly typed instance of the derived class + CInvalidPointerExceptionAn exception class to notify if an object has no reference count (if the object is not owned by any pointer) when attempting to get a derived instance through dynamic down-casting + CMessageTranslationExceptionAn exception class to notify that the translation of a ROS message failed + CNullPointerExceptionAn exception class to notify that a certain pointer is null + CParameterTranslationExceptionAn exception class to notify incompatibility when translating parameters from different sources + ▼Nmodulo_utils + ▼Ntestutils + CPredicatesListener + CServiceClient + CComponentInterfacePublicInterfaceFriend class to the ComponentInterface to allow test fixtures to access protected and private members diff --git a/versions/main/classes.html b/versions/main/classes.html index 86c954613..3314776e9 100644 --- a/versions/main/classes.html +++ b/versions/main/classes.html @@ -72,14 +72,14 @@
Class Index
-
A | C | I | L | M | N | P | S
+
A | C | I | L | M | N | P | R | S
A
AddServiceException (modulo_components::exceptions)
AddSignalException (modulo_components::exceptions)
C
-
Component (modulo_components)
ComponentException (modulo_components::exceptions)
ComponentInterface (modulo_components)
ComponentInterfacePublicInterface
ComponentParameterException (modulo_components::exceptions)
ComponentServiceResponse (modulo_components)
CoreException (modulo_core::exceptions)
+
Component (modulo_components)
ComponentException (modulo_components::exceptions)
ComponentInterface (modulo_components)
ComponentInterfacePublicInterface
ComponentParameterException (modulo_components::exceptions)
ComponentServiceResponse (modulo_components)
ControllerInput (modulo_controllers)
ControllerInterface (modulo_controllers)
ControllerServiceResponse (modulo_controllers)
CoreException (modulo_core::exceptions)
I
InvalidPointerCastException (modulo_core::exceptions)
InvalidPointerException (modulo_core::exceptions)
@@ -96,6 +96,9 @@
P
ParameterTranslationException (modulo_core::exceptions)
PredicatesListener (modulo_utils::testutils)
PublisherHandler (modulo_core::communication)
PublisherInterface (modulo_core::communication)
+
R
+
RobotControllerInterface (modulo_controllers)
+
S
ServiceClient (modulo_utils::testutils)
SubscriptionHandler (modulo_core::communication)
SubscriptionInterface (modulo_core::communication)
diff --git a/versions/main/classmodulo__controllers_1_1_controller_interface-members.html b/versions/main/classmodulo__controllers_1_1_controller_interface-members.html new file mode 100644 index 000000000..760fe4952 --- /dev/null +++ b/versions/main/classmodulo__controllers_1_1_controller_interface-members.html @@ -0,0 +1,145 @@ + + + + + + + +Modulo: Member List + + + + + + + + + +
+
+ + + + + + +
+
Modulo 4.1.0 +
+
+
+ + + + + + + + +
+
+ + +
+
+
+
+
+
Loading...
+
Searching...
+
No Matches
+
+
+
+
+ + +
+
+
modulo_controllers::ControllerInterface Member List
+
+
+ +

This is the complete list of members for modulo_controllers::ControllerInterface, including all inherited members.

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
add_command_interface(const std::string &name, const std::string &interface)modulo_controllers::ControllerInterfaceprotected
add_input(const std::string &name, const std::string &topic_name="")modulo_controllers::ControllerInterfaceinlineprotected
add_input(const std::string &name, const std::string &topic_name) (defined in modulo_controllers::ControllerInterface)modulo_controllers::ControllerInterfaceinlineprotected
add_input(const std::string &name, const std::string &topic_name) (defined in modulo_controllers::ControllerInterface)modulo_controllers::ControllerInterfaceinlineprotected
add_input(const std::string &name, const std::string &topic_name) (defined in modulo_controllers::ControllerInterface)modulo_controllers::ControllerInterfaceinlineprotected
add_output(const std::string &name, const std::string &topic_name="")modulo_controllers::ControllerInterfaceinlineprotected
add_output(const std::string &name, const std::string &topic_name) (defined in modulo_controllers::ControllerInterface)modulo_controllers::ControllerInterfaceinlineprotected
add_output(const std::string &name, const std::string &topic_name) (defined in modulo_controllers::ControllerInterface)modulo_controllers::ControllerInterfaceinlineprotected
add_output(const std::string &name, const std::string &topic_name) (defined in modulo_controllers::ControllerInterface)modulo_controllers::ControllerInterfaceinlineprotected
add_parameter(const std::shared_ptr< state_representation::ParameterInterface > &parameter, const std::string &description, bool read_only=false)modulo_controllers::ControllerInterfaceprotected
add_parameter(const std::string &name, const T &value, const std::string &description, bool read_only=false)modulo_controllers::ControllerInterfaceinlineprotected
add_predicate(const std::string &predicate_name, bool predicate_value)modulo_controllers::ControllerInterfaceprotected
add_predicate(const std::string &predicate_name, const std::function< bool(void)> &predicate_function)modulo_controllers::ControllerInterfaceprotected
add_service(const std::string &service_name, const std::function< ControllerServiceResponse(void)> &callback)modulo_controllers::ControllerInterfaceprotected
add_service(const std::string &service_name, const std::function< ControllerServiceResponse(const std::string &string)> &callback)modulo_controllers::ControllerInterfaceprotected
add_state_interface(const std::string &name, const std::string &interface)modulo_controllers::ControllerInterfaceprotected
add_trigger(const std::string &trigger_name)modulo_controllers::ControllerInterfaceprotected
command_interface_configuration() const finalmodulo_controllers::ControllerInterface
ControllerInterface(bool claim_all_state_interfaces=false)modulo_controllers::ControllerInterface
evaluate(const rclcpp::Time &time, const std::chrono::nanoseconds &period)=0modulo_controllers::ControllerInterfaceprotectedpure virtual
get_command_interface(const std::string &name, const std::string &interface) constmodulo_controllers::ControllerInterfaceprotected
get_parameter(const std::string &name) constmodulo_controllers::ControllerInterfaceprotected
get_parameter_value(const std::string &name) constmodulo_controllers::ControllerInterfaceinlineprotected
get_predicate(const std::string &predicate_name) constmodulo_controllers::ControllerInterfaceprotected
get_qos() constmodulo_controllers::ControllerInterfaceprotected
get_state_interface(const std::string &name, const std::string &interface) constmodulo_controllers::ControllerInterfaceprotected
get_state_interfaces(const std::string &name) constmodulo_controllers::ControllerInterfaceprotected
hardware_name_modulo_controllers::ControllerInterfaceprotected
is_active() constmodulo_controllers::ControllerInterfaceprotected
on_activate(const rclcpp_lifecycle::State &previous_state) finalmodulo_controllers::ControllerInterface
on_activate()modulo_controllers::ControllerInterfaceprotectedvirtual
on_configure(const rclcpp_lifecycle::State &previous_state) finalmodulo_controllers::ControllerInterface
on_configure()modulo_controllers::ControllerInterfaceprotectedvirtual
on_deactivate(const rclcpp_lifecycle::State &previous_state) finalmodulo_controllers::ControllerInterface
on_deactivate()modulo_controllers::ControllerInterfaceprotectedvirtual
on_init() overridemodulo_controllers::ControllerInterface
on_validate_parameter_callback(const std::shared_ptr< state_representation::ParameterInterface > &parameter)modulo_controllers::ControllerInterfaceprotectedvirtual
read_input(const std::string &name)modulo_controllers::ControllerInterfaceinlineprotected
read_input(const std::string &name) (defined in modulo_controllers::ControllerInterface)modulo_controllers::ControllerInterfaceinlineprotected
read_input(const std::string &name) (defined in modulo_controllers::ControllerInterface)modulo_controllers::ControllerInterfaceinlineprotected
read_input(const std::string &name) (defined in modulo_controllers::ControllerInterface)modulo_controllers::ControllerInterfaceinlineprotected
read_state_interfaces()modulo_controllers::ControllerInterfaceprotectedvirtual
set_command_interface(const std::string &name, const std::string &interface, double value)modulo_controllers::ControllerInterfaceprotected
set_parameter_value(const std::string &name, const T &value)modulo_controllers::ControllerInterfaceinlineprotected
set_predicate(const std::string &predicate_name, bool predicate_value)modulo_controllers::ControllerInterfaceprotected
set_predicate(const std::string &predicate_name, const std::function< bool(void)> &predicate_function)modulo_controllers::ControllerInterfaceprotected
set_qos(const rclcpp::QoS &qos)modulo_controllers::ControllerInterfaceprotected
state_interface_configuration() const finalmodulo_controllers::ControllerInterface
trigger(const std::string &trigger_name)modulo_controllers::ControllerInterfaceprotected
update(const rclcpp::Time &time, const rclcpp::Duration &period) finalmodulo_controllers::ControllerInterface
write_command_interfaces(const rclcpp::Duration &period)modulo_controllers::ControllerInterfaceprotectedvirtual
write_output(const std::string &name, const T &data)modulo_controllers::ControllerInterfaceinlineprotected
write_output(const std::string &name, const bool &data) (defined in modulo_controllers::ControllerInterface)modulo_controllers::ControllerInterfaceinline
write_output(const std::string &name, const double &data) (defined in modulo_controllers::ControllerInterface)modulo_controllers::ControllerInterfaceinline
write_output(const std::string &name, const std::vector< double > &data) (defined in modulo_controllers::ControllerInterface)modulo_controllers::ControllerInterfaceinline
write_output(const std::string &name, const int &data) (defined in modulo_controllers::ControllerInterface)modulo_controllers::ControllerInterfaceinline
write_output(const std::string &name, const std::string &data) (defined in modulo_controllers::ControllerInterface)modulo_controllers::ControllerInterfaceinline
+ +
+Generated by doxygen 1.9.8 +
+ + diff --git a/versions/main/classmodulo__controllers_1_1_controller_interface.html b/versions/main/classmodulo__controllers_1_1_controller_interface.html new file mode 100644 index 000000000..b052282c3 --- /dev/null +++ b/versions/main/classmodulo__controllers_1_1_controller_interface.html @@ -0,0 +1,2636 @@ + + + + + + + +Modulo: modulo_controllers::ControllerInterface Class Reference + + + + + + + + + +
+
+ + + + + + +
+
Modulo 4.1.0 +
+
+
+ + + + + + + + +
+
+ + +
+
+
+
+
+
Loading...
+
Searching...
+
No Matches
+
+
+
+
+ + +
+
+
+Public Member Functions | +Protected Member Functions | +Protected Attributes | +List of all members
+
modulo_controllers::ControllerInterface Class Referenceabstract
+
+
+ +

Base controller class to combine ros2_control, control libraries and modulo. + More...

+ +

#include <ControllerInterface.hpp>

+
+Inheritance diagram for modulo_controllers::ControllerInterface:
+
+
+ + +modulo_controllers::RobotControllerInterface + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Public Member Functions

 ControllerInterface (bool claim_all_state_interfaces=false)
 Default constructor.
 
CallbackReturn on_init () override
 Declare parameters and register the on_set_parameters callback.
 
CallbackReturn on_configure (const rclcpp_lifecycle::State &previous_state) final
 Set class properties from parameters and add signals.
 
CallbackReturn on_activate (const rclcpp_lifecycle::State &previous_state) final
 Initialize internal data attributes from configured interfaces and wait for valid states from hardware.
 
CallbackReturn on_deactivate (const rclcpp_lifecycle::State &previous_state) final
 Deactivate the controller.
 
controller_interface::return_type update (const rclcpp::Time &time, const rclcpp::Duration &period) final
 Read the state interfaces, perform control evaluation and write the command interfaces.
 
controller_interface::InterfaceConfiguration state_interface_configuration () const final
 Configure the state interfaces.
 
controller_interface::InterfaceConfiguration command_interface_configuration () const final
 Configure the command interfaces.
 
template<>
void write_output (const std::string &name, const bool &data)
 
template<>
void write_output (const std::string &name, const double &data)
 
template<>
void write_output (const std::string &name, const std::vector< double > &data)
 
template<>
void write_output (const std::string &name, const int &data)
 
template<>
void write_output (const std::string &name, const std::string &data)
 
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Protected Member Functions

virtual CallbackReturn on_configure ()
 Configure the controller.
 
virtual CallbackReturn on_activate ()
 Activate the controller.
 
virtual CallbackReturn on_deactivate ()
 Deactivate the controller.
 
virtual controller_interface::return_type read_state_interfaces ()
 Read the state interfaces.
 
virtual controller_interface::return_type write_command_interfaces (const rclcpp::Duration &period)
 Write the command interfaces.
 
virtual controller_interface::return_type evaluate (const rclcpp::Time &time, const std::chrono::nanoseconds &period)=0
 The control logic callback.
 
void add_state_interface (const std::string &name, const std::string &interface)
 Add a state interface to the controller by name.
 
void add_command_interface (const std::string &name, const std::string &interface)
 Add a command interface to the controller by name.
 
std::unordered_map< std::string, double > get_state_interfaces (const std::string &name) const
 Get a map containing the state interfaces by name of the parent tag.
 
double get_state_interface (const std::string &name, const std::string &interface) const
 Get the value of a state interface by name.
 
double get_command_interface (const std::string &name, const std::string &interface) const
 Get the value of a command interface by name.
 
void set_command_interface (const std::string &name, const std::string &interface, double value)
 Set the value of a command interface by name.
 
void add_parameter (const std::shared_ptr< state_representation::ParameterInterface > &parameter, const std::string &description, bool read_only=false)
 Add a parameter.
 
template<typename T >
void add_parameter (const std::string &name, const T &value, const std::string &description, bool read_only=false)
 Add a parameter.
 
virtual bool on_validate_parameter_callback (const std::shared_ptr< state_representation::ParameterInterface > &parameter)
 Parameter validation function to be redefined by derived controller classes.
 
std::shared_ptr< state_representation::ParameterInterface > get_parameter (const std::string &name) const
 Get a parameter by name.
 
template<typename T >
T get_parameter_value (const std::string &name) const
 Get a parameter value by name.
 
template<typename T >
void set_parameter_value (const std::string &name, const T &value)
 Set the value of a parameter.
 
void add_predicate (const std::string &predicate_name, bool predicate_value)
 Add a predicate to the map of predicates.
 
void add_predicate (const std::string &predicate_name, const std::function< bool(void)> &predicate_function)
 Add a predicate to the map of predicates based on a function to periodically call.
 
bool get_predicate (const std::string &predicate_name) const
 Get the logical value of a predicate.
 
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 set_predicate (const std::string &predicate_name, const std::function< bool(void)> &predicate_function)
 Set the value of the predicate given as parameter, if the predicate is not found does not do anything.
 
void add_trigger (const std::string &trigger_name)
 Add a trigger to the controller.
 
void trigger (const std::string &trigger_name)
 Latch the trigger with the provided name.
 
template<typename T >
void add_input (const std::string &name, const std::string &topic_name="")
 Add an input to the controller.
 
template<typename T >
void add_output (const std::string &name, const std::string &topic_name="")
 Add an output to the controller.
 
template<typename T >
std::optional< T > read_input (const std::string &name)
 Read the most recent message of an input.
 
template<typename T >
void write_output (const std::string &name, const T &data)
 Write an object to an output.
 
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.
 
void add_service (const std::string &service_name, const std::function< ControllerServiceResponse(const std::string &string)> &callback)
 Add a service to trigger a callback function with a string payload.
 
rclcpp::QoS get_qos () const
 Getter of the Quality of Service attribute.
 
void set_qos (const rclcpp::QoS &qos)
 Set the Quality of Service for ROS publishers and subscribers.
 
bool is_active () const
 Check if the controller is currently in state active or not.
 
template<>
void add_input (const std::string &name, const std::string &topic_name)
 
template<>
void add_input (const std::string &name, const std::string &topic_name)
 
template<>
void add_input (const std::string &name, const std::string &topic_name)
 
template<>
void add_output (const std::string &name, const std::string &topic_name)
 
template<>
void add_output (const std::string &name, const std::string &topic_name)
 
template<>
void add_output (const std::string &name, const std::string &topic_name)
 
template<>
std::optional< bool > read_input (const std::string &name)
 
template<>
std::optional< double > read_input (const std::string &name)
 
template<>
std::optional< int > read_input (const std::string &name)
 
+ + + + +

+Protected Attributes

std::string hardware_name_
 The hardware name provided by a parameter.
 
+

Detailed Description

+

Base controller class to combine ros2_control, control libraries and modulo.

+ +

Definition at line 98 of file ControllerInterface.hpp.

+

Constructor & Destructor Documentation

+ +

◆ ControllerInterface()

+ +
+
+ + + + + + + + +
modulo_controllers::ControllerInterface::ControllerInterface (bool claim_all_state_interfaces = false)
+
+ +

Default constructor.

+
Parameters
+ + +
claim_all_state_interfacesFlag to indicate if all state interfaces should be claimed
+
+
+ +

Definition at line 21 of file ControllerInterface.cpp.

+ +
+
+

Member Function Documentation

+ +

◆ add_command_interface()

+ +
+
+ + + + + +
+ + + + + + + + + + + + + + + + + + +
void modulo_controllers::ControllerInterface::add_command_interface (const std::string & name,
const std::string & interface 
)
+ 		+protected
+
+ +

Add a command interface to the controller by name.

+
Parameters
+ + + +
nameThe name of the parent tag, e.g. the name of the joint, sensor or gpio
interfaceThe desired command interface
+
+
+ +

Definition at line 99 of file ControllerInterface.cpp.

+ +
+
+ +

◆ add_input() [1/4]

+ +
+
+
+template<>
+ + + + + +
+ + + + + + + + + + + + + + + + + + +
void modulo_controllers::ControllerInterface::add_input (const std::string & name,
const std::string & topic_name 
)
+ 		+inlineprotected
+
+ +

Definition at line 666 of file ControllerInterface.hpp.

+ +
+
+ +

◆ add_input() [2/4]

+ +
+
+
+template<>
+ + + + + +
+ + + + + + + + + + + + + + + + + + +
void modulo_controllers::ControllerInterface::add_input (const std::string & name,
const std::string & topic_name 
)
+ 		+inlineprotected
+
+ +

Definition at line 673 of file ControllerInterface.hpp.

+ +
+
+ +

◆ add_input() [3/4]

+ +
+
+
+template<>
+ + + + + +
+ + + + + + + + + + + + + + + + + + +
void modulo_controllers::ControllerInterface::add_input (const std::string & name,
const std::string & topic_name 
)
+ 		+inlineprotected
+
+ +

Definition at line 688 of file ControllerInterface.hpp.

+ +
+
+ +

◆ add_input() [4/4]

+ +
+
+
+template<typename T >
+ + + + + +
+ + + + + + + + + + + + + + + + + + +
void modulo_controllers::ControllerInterface::add_input (const std::string & name,
const std::string & topic_name = "" 
)
+ 		+inlineprotected
+
+ +

Add an input to the controller.

+

Inputs should be added in the on_init function of the derived controllers. Doing this will create a ROS 2 subscription for the message type.

Template Parameters
+ + +
TThe type of the input data
+
+
+
Parameters
+ + + +
nameThe name of the input
topic_nameThe topic name of the input (defaults to ~/name)
+
+
+ +

Definition at line 657 of file ControllerInterface.hpp.

+ +
+
+ +

◆ add_output() [1/4]

+ +
+
+
+template<>
+ + + + + +
+ + + + + + + + + + + + + + + + + + +
void modulo_controllers::ControllerInterface::add_output (const std::string & name,
const std::string & topic_name 
)
+ 		+inlineprotected
+
+ +

Definition at line 717 of file ControllerInterface.hpp.

+ +
+
+ +

◆ add_output() [2/4]

+ +
+
+
+template<>
+ + + + + +
+ + + + + + + + + + + + + + + + + + +
void modulo_controllers::ControllerInterface::add_output (const std::string & name,
const std::string & topic_name 
)
+ 		+inlineprotected
+
+ +

Definition at line 722 of file ControllerInterface.hpp.

+ +
+
+ +

◆ add_output() [3/4]

+ +
+
+
+template<>
+ + + + + +
+ + + + + + + + + + + + + + + + + + +
void modulo_controllers::ControllerInterface::add_output (const std::string & name,
const std::string & topic_name 
)
+ 		+inlineprotected
+
+ +

Definition at line 733 of file ControllerInterface.hpp.

+ +
+
+ +

◆ add_output() [4/4]

+ +
+
+
+template<typename T >
+ + + + + +
+ + + + + + + + + + + + + + + + + + +
void modulo_controllers::ControllerInterface::add_output (const std::string & name,
const std::string & topic_name = "" 
)
+ 		+inlineprotected
+
+ +

Add an output to the controller.

+

Outputs should be added in the on_init function of the derived controllers. Doing this will create a ROS 2 publisher for the message type and wrap it in a RealtimePublisher.

Template Parameters
+ + +
TThe type of the output data
+
+
+
Parameters
+ + + +
nameThe name of the output
topic_nameThe topic name of the output (defaults to ~/name)
+
+
+ +

Definition at line 711 of file ControllerInterface.hpp.

+ +
+
+ +

◆ add_parameter() [1/2]

+ +
+
+ + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + +
void modulo_controllers::ControllerInterface::add_parameter (const std::shared_ptr< state_representation::ParameterInterface > & parameter,
const std::string & description,
bool read_only = false 
)
+ 		+protected
+
+ +

Add a parameter.

+

This method stores a pointer reference to an existing Parameter object in the local parameter map and declares the equivalent ROS parameter on the ROS interface.

Parameters
+ + + + +
parameterA ParameterInterface pointer to a Parameter instance
descriptionThe description of the parameter
read_onlyIf true, the value of the parameter cannot be changed after declaration
+
+
+ +
+
+ +

◆ add_parameter() [2/2]

+ +
+
+
+template<typename T >
+ + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
void modulo_controllers::ControllerInterface::add_parameter (const std::string & name,
const T & value,
const std::string & description,
bool read_only = false 
)
+ 		+inlineprotected
+
+ +

Add a parameter.

+

This method creates a new Parameter object instance to reference in the local parameter map and declares the equivalent ROS parameter on the ROS interface.

Template Parameters
+ + +
TThe type of the parameter
+
+
+
Parameters
+ + + + + +
nameThe name of the parameter
valueThe value of the parameter
descriptionThe description of the parameter
read_onlyIf true, the value of the parameter cannot be changed after declaration
+
+
+ +

Definition at line 622 of file ControllerInterface.hpp.

+ +
+
+ +

◆ add_predicate() [1/2]

+ +
+
+ + + + + +
+ + + + + + + + + + + + + + + + + + +
void modulo_controllers::ControllerInterface::add_predicate (const std::string & predicate_name,
bool predicate_value 
)
+ 		+protected
+
+ +

Add a predicate to the map of predicates.

+
Parameters
+ + + +
predicate_namethe name of the associated predicate
predicate_valuethe boolean value of the predicate
+
+
+ +

Definition at line 383 of file ControllerInterface.cpp.

+ +
+
+ +

◆ add_predicate() [2/2]

+ +
+
+ + + + + +
+ + + + + + + + + + + + + + + + + + +
void modulo_controllers::ControllerInterface::add_predicate (const std::string & predicate_name,
const std::function< bool(void)> & predicate_function 
)
+ 		+protected
+
+ +

Add a predicate to the map of predicates based on a function to periodically call.

+
Parameters
+ + + +
predicate_namethe name of the associated predicate
predicate_functionthe function to call that returns the value of the predicate
+
+
+ +

Definition at line 387 of file ControllerInterface.cpp.

+ +
+
+ +

◆ add_service() [1/2]

+ +
+
+ + + + + +
+ + + + + + + + + + + + + + + + + + +
void modulo_controllers::ControllerInterface::add_service (const std::string & service_name,
const std::function< ControllerServiceResponse(const std::string &string)> & callback 
)
+ 		+protected
+
+ +

Add a service to trigger a callback function with a string payload.

+

The string payload can have an arbitrary format to parameterize and control the callback behaviour as desired. It is the responsibility of the service callback to parse the string according to some payload format. When adding a service with a string payload, be sure to document the payload format appropriately.

Parameters
+ + + +
service_nameThe name of the service
callbackA service callback function with a string argument that returns a ControllerServiceResponse
+
+
+ +

Definition at line 709 of file ControllerInterface.cpp.

+ +
+
+ +

◆ add_service() [2/2]

+ +
+
+ + + + + +
+ + + + + + + + + + + + + + + + + + +
void modulo_controllers::ControllerInterface::add_service (const std::string & service_name,
const std::function< ControllerServiceResponse(void)> & callback 
)
+ 		+protected
+
+ +

Add a service to trigger a callback function with no input arguments.

+
Parameters
+ + + +
service_nameThe name of the service
callbackA service callback function with no arguments that returns a ControllerServiceResponse
+
+
+ +

Definition at line 676 of file ControllerInterface.cpp.

+ +
+
+ +

◆ add_state_interface()

+ +
+
+ + + + + +
+ + + + + + + + + + + + + + + + + + +
void modulo_controllers::ControllerInterface::add_state_interface (const std::string & name,
const std::string & interface 
)
+ 		+protected
+
+ +

Add a state interface to the controller by name.

+
Parameters
+ + + +
nameThe name of the parent tag, e.g. the name of the joint, sensor or gpio
interfaceThe desired state interface
+
+
+ +

Definition at line 95 of file ControllerInterface.cpp.

+ +
+
+ +

◆ add_trigger()

+ +
+
+ + + + + +
+ + + + + + + + +
void modulo_controllers::ControllerInterface::add_trigger (const std::string & trigger_name)
+ 		+protected
+
+ +

Add a trigger to the controller.

+

Triggers are predicates that are always false except when it's triggered in which case it is set back to false immediately after it is read.

Parameters
+ + +
trigger_nameThe name of the trigger
+
+
+ +

Definition at line 451 of file ControllerInterface.cpp.

+ +
+
+ +

◆ command_interface_configuration()

+ +
+
+ + + + + +
+ + + + + + + +
controller_interface::InterfaceConfiguration modulo_controllers::ControllerInterface::command_interface_configuration () const
+ 		+final
+
+ +

Configure the command interfaces.

+
Returns
The command interface configuration
+ +

Definition at line 121 of file ControllerInterface.cpp.

+ +
+
+ +

◆ evaluate()

+ +
+
+ + + + + +
+ + + + + + + + + + + + + + + + + + +
virtual controller_interface::return_type modulo_controllers::ControllerInterface::evaluate (const rclcpp::Time & time,
const std::chrono::nanoseconds & period 
)
+ 		+protectedpure virtual
+
+ +

The control logic callback.

+

This method should be overridden by derived classes. It is called in the update() method between reading the state interfaces and writing the command interfaces.

Parameters
+ + + +
timeThe controller clock time
periodTime elapsed since the last control evaluation
+
+
+
Returns
OK or ERROR
+ +
+
+ +

◆ get_command_interface()

+ +
+
+ + + + + +
+ + + + + + + + + + + + + + + + + + +
double modulo_controllers::ControllerInterface::get_command_interface (const std::string & name,
const std::string & interface 
) const
+ 		+protected
+
+ +

Get the value of a command interface by name.

+
Parameters
+ + + +
nameThe name of the parent tag, e.g. the name of the joint, sensor or gpio
interfaceThe desired command interface
+
+
+
Exceptions
+ + +
out_of_rangeif the desired command interface is not available
+
+
+
Returns
The value of the command interface
+ +

Definition at line 283 of file ControllerInterface.cpp.

+ +
+
+ +

◆ get_parameter()

+ +
+
+ + + + + +
+ + + + + + + + +
std::shared_ptr< ParameterInterface > modulo_controllers::ControllerInterface::get_parameter (const std::string & name) const
+ 		+protected
+
+ +

Get a parameter by name.

+
Parameters
+ + +
nameThe name of the parameter
+
+
+
Returns
The ParameterInterface pointer to a Parameter instance
+ +

Definition at line 332 of file ControllerInterface.cpp.

+ +
+
+ +

◆ get_parameter_value()

+ +
+
+
+template<typename T >
+ + + + + +
+ + + + + + + + +
T modulo_controllers::ControllerInterface::get_parameter_value (const std::string & name) const
+ 		+inlineprotected
+
+ +

Get a parameter value by name.

+
Template Parameters
+ + +
TThe type of the parameter
+
+
+
Parameters
+ + +
nameThe name of the parameter
+
+
+
Returns
The value of the parameter
+ +

Definition at line 632 of file ControllerInterface.hpp.

+ +
+
+ +

◆ get_predicate()

+ +
+
+ + + + + +
+ + + + + + + + +
bool modulo_controllers::ControllerInterface::get_predicate (const std::string & predicate_name) const
+ 		+protected
+
+ +

Get the logical value of a predicate.

+

If the predicate is not found or the callable function fails, the return value is false.

Parameters
+ + +
predicate_namethe name of the predicate to retrieve from the map of predicates
+
+
+
Returns
the value of the predicate as a boolean
+ +

Definition at line 404 of file ControllerInterface.cpp.

+ +
+
+ +

◆ get_qos()

+ +
+
+ + + + + +
+ + + + + + + +
rclcpp::QoS modulo_controllers::ControllerInterface::get_qos () const
+ 		+protected
+
+ +

Getter of the Quality of Service attribute.

+
Returns
The Quality of Service attribute
+ +

Definition at line 743 of file ControllerInterface.cpp.

+ +
+
+ +

◆ get_state_interface()

+ +
+
+ + + + + +
+ + + + + + + + + + + + + + + + + + +
double modulo_controllers::ControllerInterface::get_state_interface (const std::string & name,
const std::string & interface 
) const
+ 		+protected
+
+ +

Get the value of a state interface by name.

+
Parameters
+ + + +
nameThe name of the parent tag, e.g. the name of the joint, sensor or gpio
interfaceThe desired state interface
+
+
+
Exceptions
+ + +
out_of_rangeif the desired state interface is not available
+
+
+
Returns
The value of the state interface
+ +

Definition at line 279 of file ControllerInterface.cpp.

+ +
+
+ +

◆ get_state_interfaces()

+ +
+
+ + + + + +
+ + + + + + + + +
std::unordered_map< std::string, double > modulo_controllers::ControllerInterface::get_state_interfaces (const std::string & name) const
+ 		+protected
+
+ +

Get a map containing the state interfaces by name of the parent tag.

+
Parameters
+ + +
nameThe name of the parent tag, e.g. the name of the joint, sensor or gpio
+
+
+
Exceptions
+ + +
out_of_rangeif the desired state interface is not available
+
+
+
Returns
The map containing the values of the state interfaces
+ +

Definition at line 275 of file ControllerInterface.cpp.

+ +
+
+ +

◆ is_active()

+ +
+
+ + + + + +
+ + + + + + + +
bool modulo_controllers::ControllerInterface::is_active () const
+ 		+protected
+
+ +

Check if the controller is currently in state active or not.

+
Returns
True if the controller is active, false otherwise
+ +

Definition at line 751 of file ControllerInterface.cpp.

+ +
+
+ +

◆ on_activate() [1/2]

+ +
+
+ + + + + +
+ + + + + + + +
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn modulo_controllers::ControllerInterface::on_activate ()
+ 		+protectedvirtual
+
+ +

Activate the controller.

+

This method should be overridden by derived classes.

Returns
SUCCESS or ERROR
+ +

Reimplemented in modulo_controllers::RobotControllerInterface.

+ +

Definition at line 204 of file ControllerInterface.cpp.

+ +
+
+ +

◆ on_activate() [2/2]

+ +
+
+ + + + + +
+ + + + + + + + +
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn modulo_controllers::ControllerInterface::on_activate (const rclcpp_lifecycle::State & previous_state)
+ 		+final
+
+ +

Initialize internal data attributes from configured interfaces and wait for valid states from hardware.

+

This functions calls the internal on_activate() method

Parameters
+ + +
previous_stateThe previous lifecycle state
+
+
+
Returns
SUCCESS or ERROR
+ +

Definition at line 156 of file ControllerInterface.cpp.

+ +
+
+ +

◆ on_configure() [1/2]

+ +
+
+ + + + + +
+ + + + + + + +
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn modulo_controllers::ControllerInterface::on_configure ()
+ 		+protectedvirtual
+
+ +

Configure the controller.

+

This method should be overridden by derived classes.

Returns
SUCCESS or ERROR
+ +

Reimplemented in modulo_controllers::RobotControllerInterface.

+ +

Definition at line 91 of file ControllerInterface.cpp.

+ +
+
+ +

◆ on_configure() [2/2]

+ +
+
+ + + + + +
+ + + + + + + + +
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn modulo_controllers::ControllerInterface::on_configure (const rclcpp_lifecycle::State & previous_state)
+ 		+final
+
+ +

Set class properties from parameters and add signals.

+

This functions calls the internal on_configure() method

Parameters
+ + +
previous_stateThe previous lifecycle state
+
+
+
Returns
SUCCESS or ERROR
+ +

Definition at line 56 of file ControllerInterface.cpp.

+ +
+
+ +

◆ on_deactivate() [1/2]

+ +
+
+ + + + + +
+ + + + + + + +
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn modulo_controllers::ControllerInterface::on_deactivate ()
+ 		+protectedvirtual
+
+ +

Deactivate the controller.

+

This method should be overridden by derived classes.

Returns
SUCCESS or ERROR
+ +

Definition at line 213 of file ControllerInterface.cpp.

+ +
+
+ +

◆ on_deactivate() [2/2]

+ +
+
+ + + + + +
+ + + + + + + + +
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn modulo_controllers::ControllerInterface::on_deactivate (const rclcpp_lifecycle::State & previous_state)
+ 		+final
+
+ +

Deactivate the controller.

+
Parameters
+ + +
previous_stateThe previous lifecycle state
+
+
+
Returns
SUCCESS or ERROR
+ +

Definition at line 209 of file ControllerInterface.cpp.

+ +
+
+ +

◆ on_init()

+ +
+
+ + + + + +
+ + + + + + + +
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn modulo_controllers::ControllerInterface::on_init ()
+ 		+override
+
+ +

Declare parameters and register the on_set_parameters callback.

+
Returns
CallbackReturn status
+ +

Definition at line 27 of file ControllerInterface.cpp.

+ +
+
+ +

◆ on_validate_parameter_callback()

+ +
+
+ + + + + +
+ + + + + + + + +
bool modulo_controllers::ControllerInterface::on_validate_parameter_callback (const std::shared_ptr< state_representation::ParameterInterface > & parameter)
+ 		+protectedvirtual
+
+ +

Parameter validation function to be redefined by derived controller classes.

+

This method is automatically invoked whenever the ROS interface tried to modify a parameter. Validation and sanitization can be performed by reading or writing the value of the parameter through the ParameterInterface pointer, depending on the parameter name and desired controller behaviour. If the validation returns true, the updated parameter value (including any modifications) is applied. If the validation returns false, any changes to the parameter are discarded and the parameter value is not changed.

Parameters
+ + +
parameterA ParameterInterface pointer to a Parameter instance
+
+
+
Returns
The validation result
+ +

Reimplemented in modulo_controllers::RobotControllerInterface.

+ +

Definition at line 379 of file ControllerInterface.cpp.

+ +
+
+ +

◆ read_input() [1/4]

+ +
+
+
+template<typename T >
+ + + + + +
+ + + + + + + + +
std::optional< T > modulo_controllers::ControllerInterface::read_input (const std::string & name)
+ 		+inlineprotected
+
+ +

Read the most recent message of an input.

+
Template Parameters
+ + +
TThe expected type of the input data
+
+
+
Parameters
+ + +
nameThe name of the input
+
+
+
Returns
The data on the desired input channel if the input exists and is not older than parameter 'input_validity_period'
+ +

Definition at line 743 of file ControllerInterface.hpp.

+ +
+
+ +

◆ read_input() [2/4]

+ +
+
+
+template<>
+ + + + + +
+ + + + + + + + +
std::optional< int > modulo_controllers::ControllerInterface::read_input (const std::string & name)
+ 		+inlineprotected
+
+ +

Definition at line 743 of file ControllerInterface.hpp.

+ +
+
+ +

◆ read_input() [3/4]

+ +
+
+
+template<>
+ + + + + +
+ + + + + + + + +
std::optional< double > modulo_controllers::ControllerInterface::read_input (const std::string & name)
+ 		+inlineprotected
+
+ +

Definition at line 743 of file ControllerInterface.hpp.

+ +
+
+ +

◆ read_input() [4/4]

+ +
+
+
+template<>
+ + + + + +
+ + + + + + + + +
std::optional< bool > modulo_controllers::ControllerInterface::read_input (const std::string & name)
+ 		+inlineprotected
+
+ +

Definition at line 743 of file ControllerInterface.hpp.

+ +
+
+ +

◆ read_state_interfaces()

+ +
+
+ + + + + +
+ + + + + + + +
controller_interface::return_type modulo_controllers::ControllerInterface::read_state_interfaces ()
+ 		+protectedvirtual
+
+ +

Read the state interfaces.

+
Returns
OK or ERROR
+ +

Definition at line 258 of file ControllerInterface.cpp.

+ +
+
+ +

◆ set_command_interface()

+ +
+
+ + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + +
void modulo_controllers::ControllerInterface::set_command_interface (const std::string & name,
const std::string & interface,
double value 
)
+ 		+protected
+
+ +

Set the value of a command interface by name.

+
Parameters
+ + + + +
nameThe name of the parent tag, e.g. the name of the sensor or gpio
interfaceThe name of the command interface
valueThe new value of the interface
+
+
+ +

Definition at line 287 of file ControllerInterface.cpp.

+ +
+
+ +

◆ set_parameter_value()

+ +
+
+
+template<typename T >
+ + + + + +
+ + + + + + + + + + + + + + + + + + +
void modulo_controllers::ControllerInterface::set_parameter_value (const std::string & name,
const T & value 
)
+ 		+inlineprotected
+
+ +

Set the value of a parameter.

+

The parameter must have been previously declared. This method preserves the reference to the original Parameter instance

Template Parameters
+ + +
TThe type of the parameter
+
+
+
Parameters
+ + +
nameThe name of the parameter
+
+
+
Returns
The value of the parameter
+ +

Definition at line 637 of file ControllerInterface.hpp.

+ +
+
+ +

◆ set_predicate() [1/2]

+ +
+
+ + + + + +
+ + + + + + + + + + + + + + + + + + +
void modulo_controllers::ControllerInterface::set_predicate (const std::string & predicate_name,
bool predicate_value 
)
+ 		+protected
+
+ +

Set the value of the predicate given as parameter, if the predicate is not found does not do anything.

+

Even though the predicates are published periodically, the new value of this predicate will be published once immediately after setting it.

Parameters
+ + + +
predicate_namethe name of the predicate to retrieve from the map of predicates
predicate_valuethe new value of the predicate
+
+
+ +

Definition at line 431 of file ControllerInterface.cpp.

+ +
+
+ +

◆ set_predicate() [2/2]

+ +
+
+ + + + + +
+ + + + + + + + + + + + + + + + + + +
void modulo_controllers::ControllerInterface::set_predicate (const std::string & predicate_name,
const std::function< bool(void)> & predicate_function 
)
+ 		+protected
+
+ +

Set the value of the predicate given as parameter, if the predicate is not found does not do anything.

+

Even though the predicates are published periodically, the new value of this predicate will be published once immediately after setting it.

Parameters
+ + + +
predicate_namethe name of the predicate to retrieve from the map of predicates
predicate_functionthe function to call that returns the value of the predicate
+
+
+ +

Definition at line 435 of file ControllerInterface.cpp.

+ +
+
+ +

◆ set_qos()

+ +
+
+ + + + + +
+ + + + + + + + +
void modulo_controllers::ControllerInterface::set_qos (const rclcpp::QoS & qos)
+ 		+protected
+
+ +

Set the Quality of Service for ROS publishers and subscribers.

+
Parameters
+ + +
qosThe desired Quality of Service
+
+
+ +

Definition at line 747 of file ControllerInterface.cpp.

+ +
+
+ +

◆ state_interface_configuration()

+ +
+
+ + + + + +
+ + + + + + + +
controller_interface::InterfaceConfiguration modulo_controllers::ControllerInterface::state_interface_configuration () const
+ 		+final
+
+ +

Configure the state interfaces.

+
Returns
The state interface configuration
+ +

Definition at line 138 of file ControllerInterface.cpp.

+ +
+
+ +

◆ trigger()

+ +
+
+ + + + + +
+ + + + + + + + +
void modulo_controllers::ControllerInterface::trigger (const std::string & trigger_name)
+ 		+protected
+
+ +

Latch the trigger with the provided name.

+
Parameters
+ + +
trigger_nameThe name of the trigger
+
+
+ +

Definition at line 470 of file ControllerInterface.cpp.

+ +
+
+ +

◆ update()

+ +
+
+ + + + + +
+ + + + + + + + + + + + + + + + + + +
controller_interface::return_type modulo_controllers::ControllerInterface::update (const rclcpp::Time & time,
const rclcpp::Duration & period 
)
+ 		+final
+
+ +

Read the state interfaces, perform control evaluation and write the command interfaces.

+
Parameters
+ + + +
timeThe controller clock time
periodTime elapsed since the last control evaluation
+
+
+
Returns
OK or ERROR
+ +

Definition at line 218 of file ControllerInterface.cpp.

+ +
+
+ +

◆ write_command_interfaces()

+ +
+
+ + + + + +
+ + + + + + + + +
controller_interface::return_type modulo_controllers::ControllerInterface::write_command_interfaces (const rclcpp::Duration & period)
+ 		+protectedvirtual
+
+ +

Write the command interfaces.

+
Parameters
+ + +
periodTime elapsed since the last control evaluation
+
+
+
Returns
OK or ERROR
+ +

Definition at line 267 of file ControllerInterface.cpp.

+ +
+
+ +

◆ write_output() [1/6]

+ +
+
+
+template<>
+ + + + + +
+ + + + + + + + + + + + + + + + + + +
void modulo_controllers::ControllerInterface::write_output (const std::string & name,
const bool & data 
)
+ 		+inline
+
+ +

Definition at line 895 of file ControllerInterface.hpp.

+ +
+
+ +

◆ write_output() [2/6]

+ +
+
+
+template<>
+ + + + + +
+ + + + + + + + + + + + + + + + + + +
void modulo_controllers::ControllerInterface::write_output (const std::string & name,
const double & data 
)
+ 		+inline
+
+ +

Definition at line 900 of file ControllerInterface.hpp.

+ +
+
+ +

◆ write_output() [3/6]

+ +
+
+
+template<>
+ + + + + +
+ + + + + + + + + + + + + + + + + + +
void modulo_controllers::ControllerInterface::write_output (const std::string & name,
const int & data 
)
+ 		+inline
+
+ +

Definition at line 910 of file ControllerInterface.hpp.

+ +
+
+ +

◆ write_output() [4/6]

+ +
+
+
+template<>
+ + + + + +
+ + + + + + + + + + + + + + + + + + +
void modulo_controllers::ControllerInterface::write_output (const std::string & name,
const std::string & data 
)
+ 		+inline
+
+ +

Definition at line 915 of file ControllerInterface.hpp.

+ +
+
+ +

◆ write_output() [5/6]

+ +
+
+
+template<>
+ + + + + +
+ + + + + + + + + + + + + + + + + + +
void modulo_controllers::ControllerInterface::write_output (const std::string & name,
const std::vector< double > & data 
)
+ 		+inline
+
+ +

Definition at line 905 of file ControllerInterface.hpp.

+ +
+
+ +

◆ write_output() [6/6]

+ +
+
+
+template<typename T >
+ + + + + +
+ + + + + + + + + + + + + + + + + + +
void modulo_controllers::ControllerInterface::write_output (const std::string & name,
const T & data 
)
+ 		+inlineprotected
+
+ +

Write an object to an output.

+

This uses the realtime publisher from add_output() to simplify publishing data in the realtime context of the control loop.

Template Parameters
+ + +
TThe type of the the object to publish
+
+
+
Parameters
+ + + +
nameThe name of the output
stateThe object to publish
+
+
+ +

Definition at line 829 of file ControllerInterface.hpp.

+ +
+
+

Member Data Documentation

+ +

◆ hardware_name_

+ +
+
+ + + + + +
+ + + + +
std::string modulo_controllers::ControllerInterface::hardware_name_
+ 		+protected
+
+ +

The hardware name provided by a parameter.

+ +

Definition at line 448 of file ControllerInterface.hpp.

+ +
+
+
The documentation for this class was generated from the following files: +
+ +
+Generated by doxygen 1.9.8 +
+ + diff --git a/versions/main/classmodulo__controllers_1_1_controller_interface.png b/versions/main/classmodulo__controllers_1_1_controller_interface.png new file mode 100644 index 000000000..423222b8b Binary files /dev/null and b/versions/main/classmodulo__controllers_1_1_controller_interface.png differ diff --git a/versions/main/classmodulo__controllers_1_1_robot_controller_interface-members.html b/versions/main/classmodulo__controllers_1_1_robot_controller_interface-members.html new file mode 100644 index 000000000..f324a9baf --- /dev/null +++ b/versions/main/classmodulo__controllers_1_1_robot_controller_interface-members.html @@ -0,0 +1,154 @@ + + + + + + + +Modulo: Member List + + + + + + + + + +
+
+ + + + + + +
+
Modulo 4.1.0 +
+
+
+ + + + + + + + +
+
+ + +
+
+
+
+
+
Loading...
+
Searching...
+
No Matches
+
+
+
+
+ + +
+
+
modulo_controllers::RobotControllerInterface Member List
+
+
+ +

This is the complete list of members for modulo_controllers::RobotControllerInterface, including all inherited members.

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
add_command_interface(const std::string &name, const std::string &interface)modulo_controllers::ControllerInterfaceprotected
add_input(const std::string &name, const std::string &topic_name="")modulo_controllers::ControllerInterfaceinlineprotected
add_input(const std::string &name, const std::string &topic_name) (defined in modulo_controllers::ControllerInterface)modulo_controllers::ControllerInterfaceinlineprotected
add_input(const std::string &name, const std::string &topic_name) (defined in modulo_controllers::ControllerInterface)modulo_controllers::ControllerInterfaceinlineprotected
add_input(const std::string &name, const std::string &topic_name) (defined in modulo_controllers::ControllerInterface)modulo_controllers::ControllerInterfaceinlineprotected
add_output(const std::string &name, const std::string &topic_name="")modulo_controllers::ControllerInterfaceinlineprotected
add_output(const std::string &name, const std::string &topic_name) (defined in modulo_controllers::ControllerInterface)modulo_controllers::ControllerInterfaceinlineprotected
add_output(const std::string &name, const std::string &topic_name) (defined in modulo_controllers::ControllerInterface)modulo_controllers::ControllerInterfaceinlineprotected
add_output(const std::string &name, const std::string &topic_name) (defined in modulo_controllers::ControllerInterface)modulo_controllers::ControllerInterfaceinlineprotected
add_parameter(const std::shared_ptr< state_representation::ParameterInterface > &parameter, const std::string &description, bool read_only=false)modulo_controllers::ControllerInterfaceprotected
add_parameter(const std::string &name, const T &value, const std::string &description, bool read_only=false)modulo_controllers::ControllerInterfaceinlineprotected
add_predicate(const std::string &predicate_name, bool predicate_value)modulo_controllers::ControllerInterfaceprotected
add_predicate(const std::string &predicate_name, const std::function< bool(void)> &predicate_function)modulo_controllers::ControllerInterfaceprotected
add_service(const std::string &service_name, const std::function< ControllerServiceResponse(void)> &callback)modulo_controllers::ControllerInterfaceprotected
add_service(const std::string &service_name, const std::function< ControllerServiceResponse(const std::string &string)> &callback)modulo_controllers::ControllerInterfaceprotected
add_state_interface(const std::string &name, const std::string &interface)modulo_controllers::ControllerInterfaceprotected
add_trigger(const std::string &trigger_name)modulo_controllers::ControllerInterfaceprotected
command_interface_configuration() const finalmodulo_controllers::ControllerInterface
compute_cartesian_state()modulo_controllers::RobotControllerInterfaceprotected
ControllerInterface(bool claim_all_state_interfaces=false)modulo_controllers::ControllerInterface
evaluate(const rclcpp::Time &time, const std::chrono::nanoseconds &period)=0modulo_controllers::ControllerInterfaceprotectedpure virtual
ft_sensor_name_modulo_controllers::RobotControllerInterfaceprotected
ft_sensor_reference_frame_modulo_controllers::RobotControllerInterfaceprotected
get_cartesian_state()modulo_controllers::RobotControllerInterfaceprotected
get_command_interface(const std::string &name, const std::string &interface) constmodulo_controllers::ControllerInterfaceprotected
get_ft_sensor()modulo_controllers::RobotControllerInterfaceprotected
get_joint_state()modulo_controllers::RobotControllerInterfaceprotected
get_parameter(const std::string &name) constmodulo_controllers::ControllerInterfaceprotected
get_parameter_value(const std::string &name) constmodulo_controllers::ControllerInterfaceinlineprotected
get_predicate(const std::string &predicate_name) constmodulo_controllers::ControllerInterfaceprotected
get_qos() constmodulo_controllers::ControllerInterfaceprotected
get_state_interface(const std::string &name, const std::string &interface) constmodulo_controllers::ControllerInterfaceprotected
get_state_interfaces(const std::string &name) constmodulo_controllers::ControllerInterfaceprotected
hardware_name_modulo_controllers::ControllerInterfaceprotected
is_active() constmodulo_controllers::ControllerInterfaceprotected
on_activate() overridemodulo_controllers::RobotControllerInterfacevirtual
modulo_controllers::ControllerInterface::on_activate(const rclcpp_lifecycle::State &previous_state) finalmodulo_controllers::ControllerInterface
on_configure() overridemodulo_controllers::RobotControllerInterfacevirtual
modulo_controllers::ControllerInterface::on_configure(const rclcpp_lifecycle::State &previous_state) finalmodulo_controllers::ControllerInterface
on_deactivate(const rclcpp_lifecycle::State &previous_state) finalmodulo_controllers::ControllerInterface
on_deactivate()modulo_controllers::ControllerInterfaceprotectedvirtual
on_init() overridemodulo_controllers::RobotControllerInterface
on_validate_parameter_callback(const std::shared_ptr< state_representation::ParameterInterface > &parameter) overridemodulo_controllers::RobotControllerInterfaceprotectedvirtual
read_input(const std::string &name)modulo_controllers::ControllerInterfaceinlineprotected
read_input(const std::string &name) (defined in modulo_controllers::ControllerInterface)modulo_controllers::ControllerInterfaceinlineprotected
read_input(const std::string &name) (defined in modulo_controllers::ControllerInterface)modulo_controllers::ControllerInterfaceinlineprotected
read_input(const std::string &name) (defined in modulo_controllers::ControllerInterface)modulo_controllers::ControllerInterfaceinlineprotected
robot_modulo_controllers::RobotControllerInterfaceprotected
RobotControllerInterface()modulo_controllers::RobotControllerInterface
RobotControllerInterface(bool robot_model_required, const std::string &control_type="")modulo_controllers::RobotControllerInterfaceexplicit
set_command_interface(const std::string &name, const std::string &interface, double value)modulo_controllers::ControllerInterfaceprotected
set_joint_command(const state_representation::JointState &joint_command)modulo_controllers::RobotControllerInterfaceprotected
set_parameter_value(const std::string &name, const T &value)modulo_controllers::ControllerInterfaceinlineprotected
set_predicate(const std::string &predicate_name, bool predicate_value)modulo_controllers::ControllerInterfaceprotected
set_predicate(const std::string &predicate_name, const std::function< bool(void)> &predicate_function)modulo_controllers::ControllerInterfaceprotected
set_qos(const rclcpp::QoS &qos)modulo_controllers::ControllerInterfaceprotected
state_interface_configuration() const finalmodulo_controllers::ControllerInterface
task_space_frame_modulo_controllers::RobotControllerInterfaceprotected
trigger(const std::string &trigger_name)modulo_controllers::ControllerInterfaceprotected
update(const rclcpp::Time &time, const rclcpp::Duration &period) finalmodulo_controllers::ControllerInterface
write_output(const std::string &name, const T &data)modulo_controllers::ControllerInterfaceinlineprotected
write_output(const std::string &name, const bool &data) (defined in modulo_controllers::ControllerInterface)modulo_controllers::ControllerInterfaceinline
write_output(const std::string &name, const double &data) (defined in modulo_controllers::ControllerInterface)modulo_controllers::ControllerInterfaceinline
write_output(const std::string &name, const std::vector< double > &data) (defined in modulo_controllers::ControllerInterface)modulo_controllers::ControllerInterfaceinline
write_output(const std::string &name, const int &data) (defined in modulo_controllers::ControllerInterface)modulo_controllers::ControllerInterfaceinline
write_output(const std::string &name, const std::string &data) (defined in modulo_controllers::ControllerInterface)modulo_controllers::ControllerInterfaceinline
+ +
+Generated by doxygen 1.9.8 +
+ + diff --git a/versions/main/classmodulo__controllers_1_1_robot_controller_interface.html b/versions/main/classmodulo__controllers_1_1_robot_controller_interface.html new file mode 100644 index 000000000..c675cab85 --- /dev/null +++ b/versions/main/classmodulo__controllers_1_1_robot_controller_interface.html @@ -0,0 +1,801 @@ + + + + + + + +Modulo: modulo_controllers::RobotControllerInterface Class Reference + + + + + + + + + +
+
+ + + + + + +
+
Modulo 4.1.0 +
+
+
+ + + + + + + + +
+
+ + +
+
+
+
+
+
Loading...
+
Searching...
+
No Matches
+
+
+
+
+ + +
+
+
+Public Member Functions | +Protected Member Functions | +Protected Attributes | +List of all members
+
modulo_controllers::RobotControllerInterface Class Reference
+
+
+ +

Base controller class that automatically associates joints with a JointState object. + More...

+ +

#include <RobotControllerInterface.hpp>

+
+Inheritance diagram for modulo_controllers::RobotControllerInterface:
+
+
+ + +modulo_controllers::ControllerInterface + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Public Member Functions

 RobotControllerInterface ()
 Default constructor.
 
 RobotControllerInterface (bool robot_model_required, const std::string &control_type="")
 Constructor with robot model flag and a control type to determine the command interfaces to claim.
 
CallbackReturn on_init () override
 Declare parameters and register the on_set_parameters callback.
 
CallbackReturn on_configure () override
 Configure the controller.
 
CallbackReturn on_activate () override
 Activate the controller.
 
- Public Member Functions inherited from modulo_controllers::ControllerInterface
 ControllerInterface (bool claim_all_state_interfaces=false)
 Default constructor.
 
CallbackReturn on_init () override
 Declare parameters and register the on_set_parameters callback.
 
CallbackReturn on_configure (const rclcpp_lifecycle::State &previous_state) final
 Set class properties from parameters and add signals.
 
CallbackReturn on_activate (const rclcpp_lifecycle::State &previous_state) final
 Initialize internal data attributes from configured interfaces and wait for valid states from hardware.
 
CallbackReturn on_deactivate (const rclcpp_lifecycle::State &previous_state) final
 Deactivate the controller.
 
controller_interface::return_type update (const rclcpp::Time &time, const rclcpp::Duration &period) final
 Read the state interfaces, perform control evaluation and write the command interfaces.
 
controller_interface::InterfaceConfiguration state_interface_configuration () const final
 Configure the state interfaces.
 
controller_interface::InterfaceConfiguration command_interface_configuration () const final
 Configure the command interfaces.
 
template<>
void write_output (const std::string &name, const bool &data)
 
template<>
void write_output (const std::string &name, const double &data)
 
template<>
void write_output (const std::string &name, const std::vector< double > &data)
 
template<>
void write_output (const std::string &name, const int &data)
 
template<>
void write_output (const std::string &name, const std::string &data)
 
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Protected Member Functions

const state_representation::JointState & get_joint_state ()
 Access the joint state object.
 
const state_representation::CartesianState & get_cartesian_state ()
 Access the Cartesian state object.
 
const state_representation::CartesianWrench & get_ft_sensor ()
 Access the Cartesian wrench object.
 
void compute_cartesian_state ()
 Compute the Cartesian state from forward kinematics of the current joint state.
 
void set_joint_command (const state_representation::JointState &joint_command)
 Set the joint command object.
 
bool on_validate_parameter_callback (const std::shared_ptr< state_representation::ParameterInterface > &parameter) override
 Parameter validation function to be redefined by derived controller classes.
 
- Protected Member Functions inherited from modulo_controllers::ControllerInterface
virtual CallbackReturn on_deactivate ()
 Deactivate the controller.
 
virtual controller_interface::return_type evaluate (const rclcpp::Time &time, const std::chrono::nanoseconds &period)=0
 The control logic callback.
 
void add_state_interface (const std::string &name, const std::string &interface)
 Add a state interface to the controller by name.
 
void add_command_interface (const std::string &name, const std::string &interface)
 Add a command interface to the controller by name.
 
std::unordered_map< std::string, double > get_state_interfaces (const std::string &name) const
 Get a map containing the state interfaces by name of the parent tag.
 
double get_state_interface (const std::string &name, const std::string &interface) const
 Get the value of a state interface by name.
 
double get_command_interface (const std::string &name, const std::string &interface) const
 Get the value of a command interface by name.
 
void set_command_interface (const std::string &name, const std::string &interface, double value)
 Set the value of a command interface by name.
 
void add_parameter (const std::shared_ptr< state_representation::ParameterInterface > &parameter, const std::string &description, bool read_only=false)
 Add a parameter.
 
template<typename T >
void add_parameter (const std::string &name, const T &value, const std::string &description, bool read_only=false)
 Add a parameter.
 
std::shared_ptr< state_representation::ParameterInterface > get_parameter (const std::string &name) const
 Get a parameter by name.
 
template<typename T >
T get_parameter_value (const std::string &name) const
 Get a parameter value by name.
 
template<typename T >
void set_parameter_value (const std::string &name, const T &value)
 Set the value of a parameter.
 
void add_predicate (const std::string &predicate_name, bool predicate_value)
 Add a predicate to the map of predicates.
 
void add_predicate (const std::string &predicate_name, const std::function< bool(void)> &predicate_function)
 Add a predicate to the map of predicates based on a function to periodically call.
 
bool get_predicate (const std::string &predicate_name) const
 Get the logical value of a predicate.
 
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 set_predicate (const std::string &predicate_name, const std::function< bool(void)> &predicate_function)
 Set the value of the predicate given as parameter, if the predicate is not found does not do anything.
 
void add_trigger (const std::string &trigger_name)
 Add a trigger to the controller.
 
void trigger (const std::string &trigger_name)
 Latch the trigger with the provided name.
 
template<typename T >
void add_input (const std::string &name, const std::string &topic_name="")
 Add an input to the controller.
 
template<typename T >
void add_output (const std::string &name, const std::string &topic_name="")
 Add an output to the controller.
 
template<typename T >
std::optional< T > read_input (const std::string &name)
 Read the most recent message of an input.
 
template<typename T >
void write_output (const std::string &name, const T &data)
 Write an object to an output.
 
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.
 
void add_service (const std::string &service_name, const std::function< ControllerServiceResponse(const std::string &string)> &callback)
 Add a service to trigger a callback function with a string payload.
 
rclcpp::QoS get_qos () const
 Getter of the Quality of Service attribute.
 
void set_qos (const rclcpp::QoS &qos)
 Set the Quality of Service for ROS publishers and subscribers.
 
bool is_active () const
 Check if the controller is currently in state active or not.
 
template<>
void add_input (const std::string &name, const std::string &topic_name)
 
template<>
void add_input (const std::string &name, const std::string &topic_name)
 
template<>
void add_input (const std::string &name, const std::string &topic_name)
 
template<>
void add_output (const std::string &name, const std::string &topic_name)
 
template<>
void add_output (const std::string &name, const std::string &topic_name)
 
template<>
void add_output (const std::string &name, const std::string &topic_name)
 
template<>
std::optional< bool > read_input (const std::string &name)
 
template<>
std::optional< double > read_input (const std::string &name)
 
template<>
std::optional< int > read_input (const std::string &name)
 
+ + + + + + + + + + + + + + + + + +

+Protected Attributes

std::shared_ptr< robot_model::Model > robot_
 Robot model object generated from URDF.
 
std::string task_space_frame_
 The frame in task space for forward kinematics calculations, if applicable.
 
std::string ft_sensor_name_
 The name of a force torque sensor in the hardware description.
 
std::string ft_sensor_reference_frame_
 The sensing reference frame.
 
- Protected Attributes inherited from modulo_controllers::ControllerInterface
std::string hardware_name_
 The hardware name provided by a parameter.
 
+

Detailed Description

+

Base controller class that automatically associates joints with a JointState object.

+

The robot controller interface extends the functionality of the modulo controller interface byautomatically claiming all state interfaces from joints and command interfaces of a given type (position, velocity, effort or acceleration) for those same joints. Joint state and command are associated with these interfaces and abstracted as JointState pointers for derived classes to access. A robot model, Cartesian state and force-torque sensor state are similarly available based on the URDF and state interfaces.

+ +

Definition at line 23 of file RobotControllerInterface.hpp.

+

Constructor & Destructor Documentation

+ +

◆ RobotControllerInterface() [1/2]

+ +
+
+ + + + + + + +
modulo_controllers::RobotControllerInterface::RobotControllerInterface ()
+
+ +

Default constructor.

+ +

Definition at line 17 of file RobotControllerInterface.cpp.

+ +
+
+ +

◆ RobotControllerInterface() [2/2]

+ +
+
+ + + + + +
+ + + + + + + + + + + + + + + + + + +
modulo_controllers::RobotControllerInterface::RobotControllerInterface (bool robot_model_required,
const std::string & control_type = "" 
)
+ 		+explicit
+
+ +

Constructor with robot model flag and a control type to determine the command interfaces to claim.

+
Parameters
+ + + +
robot_model_requiredFlag to indicate if a robot model is required for the controller
control_typeOne of [position, velocity, effort or acceleration]
+
+
+ +

Definition at line 19 of file RobotControllerInterface.cpp.

+ +
+
+

Member Function Documentation

+ +

◆ compute_cartesian_state()

+ +
+
+ + + + + +
+ + + + + + + +
void modulo_controllers::RobotControllerInterface::compute_cartesian_state ()
+ 		+protected
+
+ +

Compute the Cartesian state from forward kinematics of the current joint state.

+

This should only be used if a robot model has been generated, in which case the forward kinematics is calculated to get pose and twist for the desired target frame.

+ +

Definition at line 302 of file RobotControllerInterface.cpp.

+ +
+
+ +

◆ get_cartesian_state()

+ +
+
+ + + + + +
+ + + + + + + +
const CartesianState & modulo_controllers::RobotControllerInterface::get_cartesian_state ()
+ 		+protected
+
+ +

Access the Cartesian state object.

+

This internally calls compute_cartesian_state()

Returns
A const reference to the CartesianState object
+ +

Definition at line 292 of file RobotControllerInterface.cpp.

+ +
+
+ +

◆ get_ft_sensor()

+ +
+
+ + + + + +
+ + + + + + + +
const CartesianWrench & modulo_controllers::RobotControllerInterface::get_ft_sensor ()
+ 		+protected
+
+ +

Access the Cartesian wrench object.

+
Returns
A const reference to the CartesianWrench object
+ +

Definition at line 298 of file RobotControllerInterface.cpp.

+ +
+
+ +

◆ get_joint_state()

+ +
+
+ + + + + +
+ + + + + + + +
const JointState & modulo_controllers::RobotControllerInterface::get_joint_state ()
+ 		+protected
+
+ +

Access the joint state object.

+
Returns
A const reference to the JointState object
+ +

Definition at line 288 of file RobotControllerInterface.cpp.

+ +
+
+ +

◆ on_activate()

+ +
+
+ + + + + +
+ + + + + + + +
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn modulo_controllers::RobotControllerInterface::on_activate ()
+ 		+overridevirtual
+
+ +

Activate the controller.

+

This method should be overridden by derived classes.

Returns
SUCCESS or ERROR
+

Initialize a fore torque sensor if applicable

+ +

Reimplemented from modulo_controllers::ControllerInterface.

+ +

Definition at line 176 of file RobotControllerInterface.cpp.

+ +
+
+ +

◆ on_configure()

+ +
+
+ + + + + +
+ + + + + + + +
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn modulo_controllers::RobotControllerInterface::on_configure ()
+ 		+overridevirtual
+
+ +

Configure the controller.

+

This method should be overridden by derived classes.

Returns
SUCCESS or ERROR
+

Create a robot model from the robot description, get and sort the joints and construct the internal joint state object.

+ +

Reimplemented from modulo_controllers::ControllerInterface.

+ +

Definition at line 66 of file RobotControllerInterface.cpp.

+ +
+
+ +

◆ on_init()

+ +
+
+ + + + + +
+ + + + + + + +
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn modulo_controllers::RobotControllerInterface::on_init ()
+ 		+override
+
+ +

Declare parameters and register the on_set_parameters callback.

+
Returns
CallbackReturn status
+

Declare additional parameters.

+ +

Definition at line 32 of file RobotControllerInterface.cpp.

+ +
+
+ +

◆ on_validate_parameter_callback()

+ +
+
+ + + + + +
+ + + + + + + + +
bool modulo_controllers::RobotControllerInterface::on_validate_parameter_callback (const std::shared_ptr< state_representation::ParameterInterface > & parameter)
+ 		+overrideprotectedvirtual
+
+ +

Parameter validation function to be redefined by derived controller classes.

+

This method is automatically invoked whenever the ROS interface tried to modify a parameter. Validation and sanitization can be performed by reading or writing the value of the parameter through the ParameterInterface pointer, depending on the parameter name and desired controller behaviour. If the validation returns true, the updated parameter value (including any modifications) is applied. If the validation returns false, any changes to the parameter are discarded and the parameter value is not changed.

Parameters
+ + +
parameterA ParameterInterface pointer to a Parameter instance
+
+
+
Returns
The validation result
+ +

Reimplemented from modulo_controllers::ControllerInterface.

+ +

Definition at line 356 of file RobotControllerInterface.cpp.

+ +
+
+ +

◆ set_joint_command()

+ +
+
+ + + + + +
+ + + + + + + + +
void modulo_controllers::RobotControllerInterface::set_joint_command (const state_representation::JointState & joint_command)
+ 		+protected
+
+ +

Set the joint command object.

+
Parameters
+ + +
joint_commandA JointState command object
+
+
+ +

Definition at line 314 of file RobotControllerInterface.cpp.

+ +
+
+

Member Data Documentation

+ +

◆ ft_sensor_name_

+ +
+
+ + + + + +
+ + + + +
std::string modulo_controllers::RobotControllerInterface::ft_sensor_name_
+ 		+protected
+
+ +

The name of a force torque sensor in the hardware description.

+ +

Definition at line 98 of file RobotControllerInterface.hpp.

+ +
+
+ +

◆ ft_sensor_reference_frame_

+ +
+
+ + + + + +
+ + + + +
std::string modulo_controllers::RobotControllerInterface::ft_sensor_reference_frame_
+ 		+protected
+
+ +

The sensing reference frame.

+ +

Definition at line 99 of file RobotControllerInterface.hpp.

+ +
+
+ +

◆ robot_

+ +
+
+ + + + + +
+ + + + +
std::shared_ptr<robot_model::Model> modulo_controllers::RobotControllerInterface::robot_
+ 		+protected
+
+ +

Robot model object generated from URDF.

+ +

Definition at line 95 of file RobotControllerInterface.hpp.

+ +
+
+ +

◆ task_space_frame_

+ +
+
+ + + + + +
+ + + + +
std::string modulo_controllers::RobotControllerInterface::task_space_frame_
+ 		+protected
+
+ +

The frame in task space for forward kinematics calculations, if applicable.

+ +

Definition at line 96 of file RobotControllerInterface.hpp.

+ +
+
+
The documentation for this class was generated from the following files: +
+ +
+Generated by doxygen 1.9.8 +
+ + diff --git a/versions/main/classmodulo__controllers_1_1_robot_controller_interface.png b/versions/main/classmodulo__controllers_1_1_robot_controller_interface.png new file mode 100644 index 000000000..e586a9302 Binary files /dev/null and b/versions/main/classmodulo__controllers_1_1_robot_controller_interface.png differ diff --git a/versions/main/dir_0e754abea120a9c0c4911cca1dd0951c.html b/versions/main/dir_0e754abea120a9c0c4911cca1dd0951c.html new file mode 100644 index 000000000..8b22cd919 --- /dev/null +++ b/versions/main/dir_0e754abea120a9c0c4911cca1dd0951c.html @@ -0,0 +1,93 @@ + + + + + + + +Modulo: /github/workspace/source/modulo_controllers/src Directory Reference + + + + + + + + + +
+
+ + + + + + +
+
Modulo 4.1.0 +
+
+
+ + + + + + + + +
+
+ + +
+
+
+
+
+
Loading...
+
Searching...
+
No Matches
+
+
+
+
+ + +
+
+
src Directory Reference
+
+
+ + + + + + +

+Files

 ControllerInterface.cpp
 
 RobotControllerInterface.cpp
 
+
+ +
+Generated by doxygen 1.9.8 +
+ + diff --git a/versions/main/dir_1086284b706fbaee53263fa501af42e8.html b/versions/main/dir_1086284b706fbaee53263fa501af42e8.html new file mode 100644 index 000000000..9d6b189bf --- /dev/null +++ b/versions/main/dir_1086284b706fbaee53263fa501af42e8.html @@ -0,0 +1,91 @@ + + + + + + + +Modulo: /github/workspace/source/modulo_controllers/include/modulo_controllers/utils Directory Reference + + + + + + + + + +
+
+ + + + + + +
+
Modulo 4.1.0 +
+
+
+ + + + + + + + +
+
+ + +
+
+
+
+
+
Loading...
+
Searching...
+
No Matches
+
+
+
+
+ + +
+
+
utils Directory Reference
+
+
+ + + + +

+Files

 utilities.hpp
 
+
+ +
+Generated by doxygen 1.9.8 +
+ + diff --git a/versions/main/dir_27c37b9b3673e5d3fc7170b21f143cd2.html b/versions/main/dir_27c37b9b3673e5d3fc7170b21f143cd2.html new file mode 100644 index 000000000..a1192f950 --- /dev/null +++ b/versions/main/dir_27c37b9b3673e5d3fc7170b21f143cd2.html @@ -0,0 +1,98 @@ + + + + + + + +Modulo: /github/workspace/source/modulo_controllers/include/modulo_controllers Directory Reference + + + + + + + + + +
+
+ + + + + + +
+
Modulo 4.1.0 +
+
+
+ + + + + + + + +
+
+ + +
+
+
+
+
+
Loading...
+
Searching...
+
No Matches
+
+
+
+
+ + +
+
+
modulo_controllers Directory Reference
+
+
+ + + + +

+Directories

 utils
 
+ + + + + +

+Files

 ControllerInterface.hpp
 
 RobotControllerInterface.hpp
 
+
+ +
+Generated by doxygen 1.9.8 +
+ + diff --git a/versions/main/dir_880c3100161de164f55db9e5265190dd.html b/versions/main/dir_880c3100161de164f55db9e5265190dd.html new file mode 100644 index 000000000..19f83f797 --- /dev/null +++ b/versions/main/dir_880c3100161de164f55db9e5265190dd.html @@ -0,0 +1,91 @@ + + + + + + + +Modulo: /github/workspace/source/modulo_controllers/include Directory Reference + + + + + + + + + +
+
+ + + + + + +
+
Modulo 4.1.0 +
+
+
+ + + + + + + + +
+
+ + +
+
+
+
+
+
Loading...
+
Searching...
+
No Matches
+
+
+
+
+ + +
+
+
include Directory Reference
+
+
+ + + + +

+Directories

 modulo_controllers
 
+
+ +
+Generated by doxygen 1.9.8 +
+ + diff --git a/versions/main/dir_9d27c852c776d117417a91cf72db7998.html b/versions/main/dir_9d27c852c776d117417a91cf72db7998.html index df36b9150..077cfabd5 100644 --- a/versions/main/dir_9d27c852c776d117417a91cf72db7998.html +++ b/versions/main/dir_9d27c852c776d117417a91cf72db7998.html @@ -81,7 +81,7 @@ Files  predicate_variant.hpp   - utilities.hpp + utilities.hpp  
diff --git a/versions/main/dir_d3939449d8b726061fb0cfc517c3fd5f.html b/versions/main/dir_d3939449d8b726061fb0cfc517c3fd5f.html new file mode 100644 index 000000000..63451762c --- /dev/null +++ b/versions/main/dir_d3939449d8b726061fb0cfc517c3fd5f.html @@ -0,0 +1,91 @@ + + + + + + + +Modulo: /github/workspace/source/modulo_controllers Directory Reference + + + + + + + + + +
+
+ + + + + + +
+
Modulo 4.1.0 +
+
+
+ + + + + + + + +
+
+ + +
+
+
+
+
+
Loading...
+
Searching...
+
No Matches
+
+
+
+
+ + +
+
+
modulo_controllers Directory Reference
+
+
+ + + + +

+Directories

 src
 
+
+ +
+Generated by doxygen 1.9.8 +
+ + diff --git a/versions/main/files.html b/versions/main/files.html index bb73067ce..d00610c1d 100644 --- a/versions/main/files.html +++ b/versions/main/files.html @@ -86,7 +86,7 @@  LookupTransformException.hpp  ▼ utilities  predicate_variant.hpp - utilities.hpp + utilities.hpp  Component.hpp  ComponentInterface.hpp  LifecycleComponent.hpp @@ -94,50 +94,60 @@  Component.cpp  ComponentInterface.cpp  LifecycleComponent.cpp - ▼ modulo_core - ▼ include - ▼ modulo_core - ▼ communication - MessagePair.hpp - MessagePairInterface.hpp - MessageType.hpp - PublisherHandler.hpp - PublisherInterface.hpp - PublisherType.hpp - SubscriptionHandler.hpp - SubscriptionInterface.hpp - ▼ exceptions - CoreException.hpp - InvalidPointerCastException.hpp - InvalidPointerException.hpp - MessageTranslationException.hpp - NullPointerException.hpp - ParameterTranslationException.hpp - ▼ translators - message_readers.hpp - message_writers.hpp - parameter_translators.hpp - EncodedState.hpp - ▼ src - ▼ communication - MessagePair.cpp - MessagePairInterface.cpp - PublisherInterface.cpp - SubscriptionHandler.cpp - SubscriptionInterface.cpp - ▼ translators - message_readers.cpp - message_writers.cpp - parameter_translators.cpp - ▼ modulo_utils - ▼ include - ▼ modulo_utils - ▼ testutils - PredicatesListener.hpp - ServiceClient.hpp - ▼ src - ▼ testutils - PredicatesListener.cpp + ▼ modulo_controllers + ▼ include + ▼ modulo_controllers + ▼ utils + utilities.hpp + ControllerInterface.hpp + RobotControllerInterface.hpp + ▼ src + ControllerInterface.cpp + RobotControllerInterface.cpp + ▼ modulo_core + ▼ include + ▼ modulo_core + ▼ communication + MessagePair.hpp + MessagePairInterface.hpp + MessageType.hpp + PublisherHandler.hpp + PublisherInterface.hpp + PublisherType.hpp + SubscriptionHandler.hpp + SubscriptionInterface.hpp + ▼ exceptions + CoreException.hpp + InvalidPointerCastException.hpp + InvalidPointerException.hpp + MessageTranslationException.hpp + NullPointerException.hpp + ParameterTranslationException.hpp + ▼ translators + message_readers.hpp + message_writers.hpp + parameter_translators.hpp + EncodedState.hpp + ▼ src + ▼ communication + MessagePair.cpp + MessagePairInterface.cpp + PublisherInterface.cpp + SubscriptionHandler.cpp + SubscriptionInterface.cpp + ▼ translators + message_readers.cpp + message_writers.cpp + parameter_translators.cpp + ▼ modulo_utils + ▼ include + ▼ modulo_utils + ▼ testutils + PredicatesListener.hpp + ServiceClient.hpp + ▼ src + ▼ testutils + PredicatesListener.cpp diff --git a/versions/main/functions.html b/versions/main/functions.html index 392835ed3..f032bfa26 100644 --- a/versions/main/functions.html +++ b/versions/main/functions.html @@ -73,22 +73,27 @@

- a -

- c -