1#include "modulo_controllers/BaseControllerInterface.hpp"
5#include <lifecycle_msgs/msg/state.hpp>
7#include <modulo_core/translators/message_readers.hpp>
10struct overloaded : Ts... {
11 using Ts::operator()...;
14overloaded(Ts...) -> overloaded<Ts...>;
17using namespace state_representation;
18using namespace std::chrono_literals;
20namespace modulo_controllers {
23 : controller_interface::
ControllerInterface(), input_validity_period_(std::numeric_limits<double>::quiet_NaN()) {}
28 pre_set_parameter_cb_handle_ = get_node()->add_pre_set_parameters_callback(
29 [
this](std::vector<rclcpp::Parameter>& parameters) {
return this->pre_set_parameters_callback(parameters); });
30 on_set_parameter_cb_handle_ = get_node()->add_on_set_parameters_callback(
31 [
this](
const std::vector<rclcpp::Parameter>& parameters) -> rcl_interfaces::msg::SetParametersResult {
32 return this->on_set_parameters_callback(parameters);
34 add_parameter<double>(
"predicate_publishing_rate", 10.0,
"The rate at which to publish controller predicates");
36 "input_validity_period", 1.0,
"The maximum age of an input state before discarding it as expired");
37 return CallbackReturn::SUCCESS;
40rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
46 if (predicates_.size()) {
47 predicate_publisher_ =
48 get_node()->create_publisher<modulo_interfaces::msg::PredicateCollection>(
"/predicates", qos_);
49 predicate_message_.node = get_node()->get_fully_qualified_name();
50 predicate_message_.type = modulo_interfaces::msg::PredicateCollection::CONTROLLER;
52 predicate_timer_ = get_node()->create_wall_timer(
54 [
this]() { this->publish_predicates(); });
57 RCLCPP_DEBUG(get_node()->get_logger(),
"Configuration of BaseControllerInterface successful");
58 return CallbackReturn::SUCCESS;
62 const std::shared_ptr<ParameterInterface>& parameter,
const std::string& description,
bool read_only) {
63 rclcpp::Parameter ros_param;
69 if (!get_node()->has_parameter(parameter->get_name())) {
70 RCLCPP_DEBUG(get_node()->get_logger(),
"Adding parameter '%s'.", parameter->get_name().c_str());
71 parameter_map_.set_parameter(parameter);
72 read_only_parameters_.insert_or_assign(parameter->get_name(),
false);
74 rcl_interfaces::msg::ParameterDescriptor descriptor;
75 descriptor.description = description;
76 descriptor.read_only = read_only;
78 set_parameters_result_.successful =
true;
79 set_parameters_result_.reason =
"";
80 if (parameter->is_empty()) {
81 descriptor.dynamic_typing =
true;
83 get_node()->declare_parameter(parameter->get_name(), rclcpp::ParameterValue{}, descriptor);
85 get_node()->declare_parameter(parameter->get_name(), ros_param.get_parameter_value(), descriptor);
87 std::vector<rclcpp::Parameter> ros_parameters{get_node()->get_parameters({parameter->get_name()})};
88 pre_set_parameters_callback(ros_parameters);
89 auto result = on_set_parameters_callback(ros_parameters);
90 if (!result.successful) {
91 get_node()->undeclare_parameter(parameter->get_name());
94 read_only_parameters_.at(parameter->get_name()) = read_only;
95 }
catch (
const std::exception& ex) {
96 parameter_map_.remove_parameter(parameter->get_name());
97 read_only_parameters_.erase(parameter->get_name());
101 RCLCPP_DEBUG(get_node()->get_logger(),
"Parameter '%s' already exists.", parameter->get_name().c_str());
107 return parameter_map_.get_parameter(name);
108 }
catch (
const state_representation::exceptions::InvalidParameterException& ex) {
113void BaseControllerInterface::pre_set_parameters_callback(std::vector<rclcpp::Parameter>& parameters) {
114 if (pre_set_parameter_callback_called_) {
115 pre_set_parameter_callback_called_ =
false;
118 rcl_interfaces::msg::SetParametersResult result;
119 result.successful =
true;
120 for (
auto& ros_parameter : parameters) {
122 auto parameter = parameter_map_.get_parameter(ros_parameter.get_name());
123 if (read_only_parameters_.at(ros_parameter.get_name())) {
124 RCLCPP_DEBUG(get_node()->get_logger(),
"Parameter '%s' is read only.", ros_parameter.get_name().c_str());
130 if (!this->validate_parameter(new_parameter)) {
131 result.successful =
false;
132 result.reason +=
"Validation of parameter '" + ros_parameter.get_name() +
"' returned false!";
133 }
else if (!new_parameter->is_empty()) {
138 }
catch (
const std::exception& ex) {
139 result.successful =
false;
140 result.reason += ex.what();
143 set_parameters_result_ = result;
146rcl_interfaces::msg::SetParametersResult
147BaseControllerInterface::on_set_parameters_callback(
const std::vector<rclcpp::Parameter>&) {
148 auto result = set_parameters_result_;
149 set_parameters_result_.successful =
true;
150 set_parameters_result_.reason =
"";
154bool BaseControllerInterface::validate_parameter(
const std::shared_ptr<ParameterInterface>& parameter) {
155 if (parameter->get_name() ==
"predicate_publishing_rate" || parameter->get_name() ==
"input_validity_period") {
156 auto value = parameter->get_parameter_value<
double>();
157 if (value < 0.0 || value > std::numeric_limits<double>::max()) {
159 get_node()->get_logger(),
"Parameter value of parameter '%s' should be a positive finite number",
160 parameter->get_name().c_str());
172 add_predicate(predicate_name, [predicate_value]() {
return predicate_value; });
176 const std::string& predicate_name,
const std::function<
bool(
void)>& predicate_function) {
177 if (predicate_name.empty()) {
178 RCLCPP_ERROR(get_node()->get_logger(),
"Failed to add predicate: Provide a non empty string as a name.");
181 if (predicates_.find(predicate_name) != predicates_.end()) {
183 get_node()->get_logger(),
"Predicate with name '%s' already exists, overwriting.", predicate_name.c_str());
185 RCLCPP_DEBUG(get_node()->get_logger(),
"Adding predicate '%s'.", predicate_name.c_str());
188 this->predicates_.insert_or_assign(predicate_name,
Predicate(predicate_function));
189 }
catch (
const std::exception& ex) {
191 get_node()->get_logger(),
"Failed to evaluate callback of predicate '%s', returning false: %s",
192 predicate_name.c_str(), ex.what());
197 auto predicate_it = predicates_.find(predicate_name);
198 if (predicate_it == predicates_.end()) {
199 RCLCPP_ERROR_THROTTLE(
200 get_node()->get_logger(), *get_node()->get_clock(), 1000,
201 "Failed to get predicate '%s': Predicate does not exists, returning false.", predicate_name.c_str());
205 return predicate_it->second.get_value();
206 }
catch (
const std::exception& ex) {
207 RCLCPP_ERROR_THROTTLE(
208 get_node()->get_logger(), *get_node()->get_clock(), 1000,
209 "Failed to evaluate callback of predicate '%s', returning false: %s", predicate_name.c_str(), ex.what());
215 set_predicate(predicate_name, [predicate_value]() {
return predicate_value; });
219 const std::string& predicate_name,
const std::function<
bool(
void)>& predicate_function) {
220 auto predicate_it = predicates_.find(predicate_name);
221 if (predicate_it == predicates_.end()) {
222 RCLCPP_ERROR_THROTTLE(
223 get_node()->get_logger(), *get_node()->get_clock(), 1000,
224 "Failed to set predicate '%s': Predicate does not exist.", predicate_name.c_str());
227 predicate_it->second.set_predicate(predicate_function);
228 if (
auto new_predicate = predicate_it->second.query(); new_predicate) {
229 publish_predicate(predicate_name, *new_predicate);
234 if (trigger_name.empty()) {
235 RCLCPP_ERROR(get_node()->get_logger(),
"Failed to add trigger: Provide a non empty string as a name.");
238 if (std::find(triggers_.cbegin(), triggers_.cend(), trigger_name) != triggers_.cend()) {
240 get_node()->get_logger(),
"Failed to add trigger: there is already a trigger with name '%s'.",
241 trigger_name.c_str());
244 if (predicates_.find(trigger_name) != predicates_.end()) {
246 get_node()->get_logger(),
"Failed to add trigger: there is already a predicate with name '%s'.",
247 trigger_name.c_str());
250 triggers_.push_back(trigger_name);
255 if (std::find(triggers_.cbegin(), triggers_.cend(), trigger_name) == triggers_.cend()) {
257 get_node()->get_logger(),
"Failed to trigger: could not find trigger with name '%s'.", trigger_name.c_str());
262 predicates_.at(trigger_name).set_predicate([]() {
return false; });
265modulo_interfaces::msg::Predicate
266BaseControllerInterface::get_predicate_message(
const std::string& name,
bool value)
const {
267 modulo_interfaces::msg::Predicate message;
268 message.predicate = name;
269 message.value = value;
273void BaseControllerInterface::publish_predicate(
const std::string& predicate_name,
bool value)
const {
274 auto message(predicate_message_);
275 message.predicates.push_back(get_predicate_message(predicate_name, value));
276 predicate_publisher_->publish(message);
279void BaseControllerInterface::publish_predicates() {
280 auto message(predicate_message_);
281 for (
auto predicate_it = predicates_.begin(); predicate_it != predicates_.end(); ++predicate_it) {
282 if (
auto new_predicate = predicate_it->second.query(); new_predicate) {
283 message.predicates.push_back(get_predicate_message(predicate_it->first, *new_predicate));
286 if (message.predicates.size()) {
287 predicate_publisher_->publish(message);
291std::string BaseControllerInterface::validate_and_declare_signal(
292 const std::string& signal_name,
const std::string& type,
const std::string& default_topic,
bool fixed_topic) {
293 auto parsed_signal_name = modulo_utils::parsing::parse_topic_name(signal_name);
294 if (parsed_signal_name.empty()) {
296 get_node()->get_logger(),
297 "The parsed signal name for %s '%s' is empty. Provide a string with valid characters for the signal name "
299 type.c_str(), signal_name.c_str());
302 if (signal_name != parsed_signal_name) {
304 get_node()->get_logger(),
305 "The parsed signal name for %s '%s' is '%s'. Use the parsed signal name to refer to this %s and its topic "
307 type.c_str(), signal_name.c_str(), parsed_signal_name.c_str(), type.c_str());
309 if (inputs_.find(parsed_signal_name) != inputs_.end()) {
310 RCLCPP_WARN(get_node()->get_logger(),
"Signal '%s' already exists as input.", parsed_signal_name.c_str());
313 if (outputs_.find(parsed_signal_name) != outputs_.end()) {
314 RCLCPP_WARN(get_node()->get_logger(),
"Signal '%s' already exists as output", parsed_signal_name.c_str());
317 auto topic = default_topic.empty() ?
"~/" + parsed_signal_name : default_topic;
318 auto parameter_name = parsed_signal_name +
"_topic";
319 if (get_node()->has_parameter(parameter_name) &&
get_parameter(parameter_name)->is_empty()) {
323 parameter_name, topic,
"Signal topic name of " + type +
" '" + parsed_signal_name +
"'", fixed_topic);
326 get_node()->get_logger(),
"Declared %s '%s' and parameter '%s' with value '%s'.", type.c_str(),
327 parsed_signal_name.c_str(), parameter_name.c_str(), topic.c_str());
328 return parsed_signal_name;
331void BaseControllerInterface::create_input(
332 const ControllerInput& input,
const std::string& name,
const std::string& topic_name) {
333 auto parsed_name = validate_and_declare_signal(name,
"input", topic_name);
334 if (!parsed_name.empty()) {
335 inputs_.insert_or_assign(parsed_name, input);
339void BaseControllerInterface::add_inputs() {
340 for (
auto& [name, input] : inputs_) {
345 [&](
const realtime_tools::RealtimeBuffer<std::shared_ptr<EncodedState>>&) {
346 subscriptions_.push_back(create_subscription<EncodedState>(name, topic));
348 [&](
const realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Bool>>&) {
349 subscriptions_.push_back(create_subscription<std_msgs::msg::Bool>(name, topic));
351 [&](
const realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Float64>>&) {
352 subscriptions_.push_back(create_subscription<std_msgs::msg::Float64>(name, topic));
354 [&](
const realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Float64MultiArray>>&) {
355 subscriptions_.push_back(create_subscription<std_msgs::msg::Float64MultiArray>(name, topic));
357 [&](
const realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Int32>>&) {
358 subscriptions_.push_back(create_subscription<std_msgs::msg::Int32>(name, topic));
360 [&](
const realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::String>>&) {
361 subscriptions_.push_back(create_subscription<std_msgs::msg::String>(name, topic));
363 [&](
const std::any&) {
364 custom_input_configuration_callables_.at(name)(name, topic);
367 }
catch (
const std::exception& ex) {
368 RCLCPP_ERROR(get_node()->get_logger(),
"Failed to add input '%s': %s", name.c_str(), ex.what());
373void BaseControllerInterface::create_output(
374 const PublisherVariant& publishers,
const std::string& name,
const std::string& topic_name) {
375 auto parsed_name = validate_and_declare_signal(name,
"output", topic_name);
376 if (!parsed_name.empty()) {
377 outputs_.insert_or_assign(parsed_name, publishers);
381void BaseControllerInterface::add_outputs() {
382 for (
auto& [name, publishers] : outputs_) {
387 [&](EncodedStatePublishers& pub) {
388 std::get<1>(pub) = get_node()->create_publisher<EncodedState>(topic, qos_);
389 std::get<2>(pub) = std::make_shared<realtime_tools::RealtimePublisher<EncodedState>>(std::get<1>(pub));
391 [&](BoolPublishers& pub) {
392 pub.first = get_node()->create_publisher<std_msgs::msg::Bool>(topic, qos_);
393 pub.second = std::make_shared<realtime_tools::RealtimePublisher<std_msgs::msg::Bool>>(pub.first);
395 [&](DoublePublishers& pub) {
396 pub.first = get_node()->create_publisher<std_msgs::msg::Float64>(topic, qos_);
397 pub.second = std::make_shared<realtime_tools::RealtimePublisher<std_msgs::msg::Float64>>(pub.first);
399 [&](DoubleVecPublishers& pub) {
400 pub.first = get_node()->create_publisher<std_msgs::msg::Float64MultiArray>(topic, qos_);
402 std::make_shared<realtime_tools::RealtimePublisher<std_msgs::msg::Float64MultiArray>>(pub.first);
404 [&](IntPublishers& pub) {
405 pub.first = get_node()->create_publisher<std_msgs::msg::Int32>(topic, qos_);
406 pub.second = std::make_shared<realtime_tools::RealtimePublisher<std_msgs::msg::Int32>>(pub.first);
408 [&](StringPublishers& pub) {
409 pub.first = get_node()->create_publisher<std_msgs::msg::String>(topic, qos_);
410 pub.second = std::make_shared<realtime_tools::RealtimePublisher<std_msgs::msg::String>>(pub.first);
412 [&](CustomPublishers& pub) {
413 custom_output_configuration_callables_.at(name)(pub, name);
416 }
catch (
const std::bad_any_cast& ex) {
417 RCLCPP_ERROR(get_node()->get_logger(),
"Failed to add custom output '%s': %s", name.c_str(), ex.what());
418 }
catch (
const std::exception& ex) {
419 RCLCPP_ERROR(get_node()->get_logger(),
"Failed to add output '%s': %s", name.c_str(), ex.what());
425 input_validity_period_ = input_validity_period;
428bool BaseControllerInterface::check_input_valid(
const std::string& name)
const {
429 if (inputs_.find(name) == inputs_.end()) {
430 RCLCPP_WARN_THROTTLE(
431 get_node()->get_logger(), *get_node()->get_clock(), 1000,
"Could not find input '%s'", name.c_str());
434 if (
static_cast<double>(std::chrono::duration_cast<std::chrono::nanoseconds>(
435 std::chrono::steady_clock::now() - inputs_.at(name).timestamp)
438 >= input_validity_period_) {
445BaseControllerInterface::validate_service_name(
const std::string& service_name,
const std::string& type)
const {
446 std::string parsed_service_name = modulo_utils::parsing::parse_topic_name(service_name);
447 if (parsed_service_name.empty()) {
449 get_node()->get_logger(),
450 "The parsed service name for %s service '%s' is empty. Provide a string with valid characters for the service "
453 type.c_str(), service_name.c_str());
456 if (service_name != parsed_service_name) {
458 get_node()->get_logger(),
459 "The parsed name for '%s' service '%s' is '%s'. Use the parsed name to refer to this service.", type.c_str(),
460 service_name.c_str(), parsed_service_name.c_str());
462 if (empty_services_.find(parsed_service_name) != empty_services_.cend()) {
464 get_node()->get_logger(),
"Service with name '%s' already exists as empty service.",
465 parsed_service_name.c_str());
468 if (string_services_.find(parsed_service_name) != string_services_.cend()) {
470 get_node()->get_logger(),
"Service with name '%s' already exists as string service.",
471 parsed_service_name.c_str());
474 RCLCPP_DEBUG(get_node()->get_logger(),
"Adding %s service '%s'.", type.c_str(), parsed_service_name.c_str());
475 return parsed_service_name;
480 auto parsed_service_name = validate_service_name(service_name,
"empty");
481 if (!parsed_service_name.empty()) {
483 auto service = get_node()->create_service<modulo_interfaces::srv::EmptyTrigger>(
484 "~/" + parsed_service_name,
486 const std::shared_ptr<modulo_interfaces::srv::EmptyTrigger::Request>,
487 std::shared_ptr<modulo_interfaces::srv::EmptyTrigger::Response> response) {
489 if (this->command_mutex_.try_lock_for(100ms)) {
490 auto callback_response = callback();
491 this->command_mutex_.unlock();
492 response->success = callback_response.success;
493 response->message = callback_response.message;
495 response->success =
false;
496 response->message =
"Unable to acquire lock for command interface within 100ms";
498 }
catch (
const std::exception& ex) {
499 response->success =
false;
500 response->message = ex.what();
504 empty_services_.insert_or_assign(parsed_service_name, service);
505 }
catch (
const std::exception& ex) {
506 RCLCPP_ERROR(get_node()->get_logger(),
"Failed to add service '%s': %s", parsed_service_name.c_str(), ex.what());
512 const std::string& service_name,
514 auto parsed_service_name = validate_service_name(service_name,
"string");
515 if (!parsed_service_name.empty()) {
517 auto service = get_node()->create_service<modulo_interfaces::srv::StringTrigger>(
518 "~/" + parsed_service_name,
520 const std::shared_ptr<modulo_interfaces::srv::StringTrigger::Request> request,
521 std::shared_ptr<modulo_interfaces::srv::StringTrigger::Response> response) {
523 if (this->command_mutex_.try_lock_for(100ms)) {
524 auto callback_response = callback(request->payload);
525 this->command_mutex_.unlock();
526 response->success = callback_response.success;
527 response->message = callback_response.message;
529 response->success =
false;
530 response->message =
"Unable to acquire lock for command interface within 100ms";
532 }
catch (
const std::exception& ex) {
533 response->success =
false;
534 response->message = ex.what();
538 string_services_.insert_or_assign(parsed_service_name, service);
539 }
catch (
const std::exception& ex) {
540 RCLCPP_ERROR(get_node()->get_logger(),
"Failed to add service '%s': %s", parsed_service_name.c_str(), ex.what());
554 return get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE;
558 return command_mutex_;
std::shared_ptr< state_representation::ParameterInterface > get_parameter(const std::string &name) const
Get a parameter by name.
CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state) override
Add signals.
rclcpp::QoS get_qos() const
Getter of the Quality of Service attribute.
bool is_active() const
Check if the controller is currently in state active or not.
void set_parameter_value(const std::string &name, const T &value)
Set the value of a parameter.
T get_parameter_value(const std::string &name) const
Get a parameter value by name.
void set_input_validity_period(double input_validity_period)
Set the input validity period of input signals.
CallbackReturn on_init() override
Declare parameters and register the on_set_parameters callback.
void add_parameter(const std::shared_ptr< state_representation::ParameterInterface > ¶meter, const std::string &description, bool read_only=false)
Add a parameter.
void trigger(const std::string &trigger_name)
Latch the trigger with the provided name.
void set_qos(const rclcpp::QoS &qos)
Set the Quality of Service for ROS publishers and subscribers.
void set_predicate(const std::string &predicate_name, bool predicate_value)
Set the value of the predicate given as parameter, if the predicate is not found does not do anything...
void add_predicate(const std::string &predicate_name, bool predicate_value)
Add a predicate to the map of predicates.
bool get_predicate(const std::string &predicate_name) const
Get the logical value of a predicate.
virtual bool on_validate_parameter_callback(const std::shared_ptr< state_representation::ParameterInterface > ¶meter)
Parameter validation function to be redefined by derived controller classes.
BaseControllerInterface()
Default constructor.
void add_service(const std::string &service_name, const std::function< ControllerServiceResponse(void)> &callback)
Add a service to trigger a callback function with no input arguments.
std::timed_mutex & get_command_mutex()
Get the reference to the command mutex.
void add_trigger(const std::string &trigger_name)
Add a trigger to the controller.
An exception class to notify errors with parameters in modulo classes.
An exception class to notify incompatibility when translating parameters from different sources.
rclcpp::Parameter write_parameter(const std::shared_ptr< state_representation::ParameterInterface > ¶meter)
Write a ROS Parameter from a ParameterInterface pointer.
rclcpp::ParameterType get_ros_parameter_type(const state_representation::ParameterType ¶meter_type)
Given a state representation parameter type, get the corresponding ROS parameter type.
void copy_parameter_value(const std::shared_ptr< const state_representation::ParameterInterface > &source_parameter, const std::shared_ptr< state_representation::ParameterInterface > ¶meter)
Copy the value of one parameter interface into another.
std::shared_ptr< state_representation::ParameterInterface > read_parameter_const(const rclcpp::Parameter &ros_parameter, const std::shared_ptr< const state_representation::ParameterInterface > ¶meter)
Update the parameter value of a ParameterInterface from a ROS Parameter object only if the two parame...
Response structure to be returned by controller services.