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");
35 return CallbackReturn::SUCCESS;
38rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
43 if (predicates_.size()) {
44 predicate_publisher_ =
45 get_node()->create_publisher<modulo_interfaces::msg::PredicateCollection>(
"/predicates", qos_);
46 predicate_message_.node = get_node()->get_fully_qualified_name();
47 predicate_message_.type = modulo_interfaces::msg::PredicateCollection::CONTROLLER;
49 predicate_timer_ = get_node()->create_wall_timer(
51 [
this]() { this->publish_predicates(); });
54 RCLCPP_DEBUG(get_node()->get_logger(),
"Configuration of BaseControllerInterface successful");
55 return CallbackReturn::SUCCESS;
59 const std::shared_ptr<ParameterInterface>& parameter,
const std::string& description,
bool read_only) {
60 rclcpp::Parameter ros_param;
66 if (!get_node()->has_parameter(parameter->get_name())) {
67 RCLCPP_DEBUG(get_node()->get_logger(),
"Adding parameter '%s'.", parameter->get_name().c_str());
68 parameter_map_.set_parameter(parameter);
69 read_only_parameters_.insert_or_assign(parameter->get_name(),
false);
71 rcl_interfaces::msg::ParameterDescriptor descriptor;
72 descriptor.description = description;
73 descriptor.read_only = read_only;
75 set_parameters_result_.successful =
true;
76 set_parameters_result_.reason =
"";
77 if (parameter->is_empty()) {
78 descriptor.dynamic_typing =
true;
80 get_node()->declare_parameter(parameter->get_name(), rclcpp::ParameterValue{}, descriptor);
82 get_node()->declare_parameter(parameter->get_name(), ros_param.get_parameter_value(), descriptor);
84 std::vector<rclcpp::Parameter> ros_parameters{get_node()->get_parameters({parameter->get_name()})};
85 pre_set_parameters_callback(ros_parameters);
86 auto result = on_set_parameters_callback(ros_parameters);
87 if (!result.successful) {
88 get_node()->undeclare_parameter(parameter->get_name());
91 read_only_parameters_.at(parameter->get_name()) = read_only;
92 }
catch (
const std::exception& ex) {
93 parameter_map_.remove_parameter(parameter->get_name());
94 read_only_parameters_.erase(parameter->get_name());
98 RCLCPP_DEBUG(get_node()->get_logger(),
"Parameter '%s' already exists.", parameter->get_name().c_str());
104 return parameter_map_.get_parameter(name);
105 }
catch (
const state_representation::exceptions::InvalidParameterException& ex) {
110void BaseControllerInterface::pre_set_parameters_callback(std::vector<rclcpp::Parameter>& parameters) {
111 if (pre_set_parameter_callback_called_) {
112 pre_set_parameter_callback_called_ =
false;
115 rcl_interfaces::msg::SetParametersResult result;
116 result.successful =
true;
117 for (
auto& ros_parameter : parameters) {
119 auto parameter = parameter_map_.get_parameter(ros_parameter.get_name());
120 if (read_only_parameters_.at(ros_parameter.get_name())) {
121 RCLCPP_DEBUG(get_node()->get_logger(),
"Parameter '%s' is read only.", ros_parameter.get_name().c_str());
127 if (!this->validate_parameter(new_parameter)) {
128 result.successful =
false;
129 result.reason +=
"Validation of parameter '" + ros_parameter.get_name() +
"' returned false!";
130 }
else if (!new_parameter->is_empty()) {
135 }
catch (
const std::exception& ex) {
136 result.successful =
false;
137 result.reason += ex.what();
140 set_parameters_result_ = result;
143rcl_interfaces::msg::SetParametersResult
144BaseControllerInterface::on_set_parameters_callback(
const std::vector<rclcpp::Parameter>&) {
145 auto result = set_parameters_result_;
146 set_parameters_result_.successful =
true;
147 set_parameters_result_.reason =
"";
151bool BaseControllerInterface::validate_parameter(
const std::shared_ptr<ParameterInterface>& parameter) {
152 if (parameter->get_name() ==
"activation_timeout" || parameter->get_name() ==
"input_validity_period") {
153 auto value = parameter->get_parameter_value<
double>();
154 if (value < 0.0 || value > std::numeric_limits<double>::max()) {
156 get_node()->get_logger(),
"Parameter value of parameter '%s' should be a positive finite number",
157 parameter->get_name().c_str());
169 add_predicate(predicate_name, [predicate_value]() {
return predicate_value; });
173 const std::string& predicate_name,
const std::function<
bool(
void)>& predicate_function) {
174 if (predicate_name.empty()) {
175 RCLCPP_ERROR(get_node()->get_logger(),
"Failed to add predicate: Provide a non empty string as a name.");
178 if (predicates_.find(predicate_name) != predicates_.end()) {
180 get_node()->get_logger(),
"Predicate with name '%s' already exists, overwriting.", predicate_name.c_str());
182 RCLCPP_DEBUG(get_node()->get_logger(),
"Adding predicate '%s'.", predicate_name.c_str());
185 this->predicates_.insert_or_assign(predicate_name,
Predicate(predicate_function));
186 }
catch (
const std::exception& ex) {
188 get_node()->get_logger(),
"Failed to evaluate callback of predicate '%s', returning false: %s",
189 predicate_name.c_str(), ex.what());
194 auto predicate_it = predicates_.find(predicate_name);
195 if (predicate_it == predicates_.end()) {
196 RCLCPP_ERROR_THROTTLE(
197 get_node()->get_logger(), *get_node()->get_clock(), 1000,
198 "Failed to get predicate '%s': Predicate does not exists, returning false.", predicate_name.c_str());
202 return predicate_it->second.get_value();
203 }
catch (
const std::exception& ex) {
204 RCLCPP_ERROR_THROTTLE(
205 get_node()->get_logger(), *get_node()->get_clock(), 1000,
206 "Failed to evaluate callback of predicate '%s', returning false: %s", predicate_name.c_str(), ex.what());
212 set_predicate(predicate_name, [predicate_value]() {
return predicate_value; });
216 const std::string& predicate_name,
const std::function<
bool(
void)>& predicate_function) {
217 auto predicate_it = predicates_.find(predicate_name);
218 if (predicate_it == predicates_.end()) {
219 RCLCPP_ERROR_THROTTLE(
220 get_node()->get_logger(), *get_node()->get_clock(), 1000,
221 "Failed to set predicate '%s': Predicate does not exist.", predicate_name.c_str());
224 predicate_it->second.set_predicate(predicate_function);
225 if (
auto new_predicate = predicate_it->second.query(); new_predicate) {
226 publish_predicate(predicate_name, *new_predicate);
231 if (trigger_name.empty()) {
232 RCLCPP_ERROR(get_node()->get_logger(),
"Failed to add trigger: Provide a non empty string as a name.");
235 if (std::find(triggers_.cbegin(), triggers_.cend(), trigger_name) != triggers_.cend()) {
237 get_node()->get_logger(),
"Failed to add trigger: there is already a trigger with name '%s'.",
238 trigger_name.c_str());
241 if (predicates_.find(trigger_name) != predicates_.end()) {
243 get_node()->get_logger(),
"Failed to add trigger: there is already a predicate with name '%s'.",
244 trigger_name.c_str());
247 triggers_.push_back(trigger_name);
252 if (std::find(triggers_.cbegin(), triggers_.cend(), trigger_name) == triggers_.cend()) {
254 get_node()->get_logger(),
"Failed to trigger: could not find trigger with name '%s'.", trigger_name.c_str());
259 predicates_.at(trigger_name).set_predicate([]() {
return false; });
262modulo_interfaces::msg::Predicate
263BaseControllerInterface::get_predicate_message(
const std::string& name,
bool value)
const {
264 modulo_interfaces::msg::Predicate message;
265 message.predicate = name;
266 message.value = value;
270void BaseControllerInterface::publish_predicate(
const std::string& predicate_name,
bool value)
const {
271 auto message(predicate_message_);
272 message.predicates.push_back(get_predicate_message(predicate_name, value));
273 predicate_publisher_->publish(message);
276void BaseControllerInterface::publish_predicates() {
277 auto message(predicate_message_);
278 for (
auto predicate_it = predicates_.begin(); predicate_it != predicates_.end(); ++predicate_it) {
279 if (
auto new_predicate = predicate_it->second.query(); new_predicate) {
280 message.predicates.push_back(get_predicate_message(predicate_it->first, *new_predicate));
283 if (message.predicates.size()) {
284 predicate_publisher_->publish(message);
288std::string BaseControllerInterface::validate_and_declare_signal(
289 const std::string& signal_name,
const std::string& type,
const std::string& default_topic,
bool fixed_topic) {
290 auto parsed_signal_name = modulo_utils::parsing::parse_topic_name(signal_name);
291 if (parsed_signal_name.empty()) {
293 get_node()->get_logger(),
294 "The parsed signal name for %s '%s' is empty. Provide a string with valid characters for the signal name "
296 type.c_str(), signal_name.c_str());
299 if (signal_name != parsed_signal_name) {
301 get_node()->get_logger(),
302 "The parsed signal name for %s '%s' is '%s'. Use the parsed signal name to refer to this %s and its topic "
304 type.c_str(), signal_name.c_str(), parsed_signal_name.c_str(), type.c_str());
306 if (inputs_.find(parsed_signal_name) != inputs_.end()) {
307 RCLCPP_WARN(get_node()->get_logger(),
"Signal '%s' already exists as input.", parsed_signal_name.c_str());
310 if (outputs_.find(parsed_signal_name) != outputs_.end()) {
311 RCLCPP_WARN(get_node()->get_logger(),
"Signal '%s' already exists as output", parsed_signal_name.c_str());
314 auto topic = default_topic.empty() ?
"~/" + parsed_signal_name : default_topic;
315 auto parameter_name = parsed_signal_name +
"_topic";
316 if (get_node()->has_parameter(parameter_name) &&
get_parameter(parameter_name)->is_empty()) {
320 parameter_name, topic,
"Signal topic name of " + type +
" '" + parsed_signal_name +
"'", fixed_topic);
323 get_node()->get_logger(),
"Declared %s '%s' and parameter '%s' with value '%s'.", type.c_str(),
324 parsed_signal_name.c_str(), parameter_name.c_str(), topic.c_str());
325 return parsed_signal_name;
328void BaseControllerInterface::create_input(
329 const ControllerInput& input,
const std::string& name,
const std::string& topic_name) {
330 auto parsed_name = validate_and_declare_signal(name,
"input", topic_name);
331 if (!parsed_name.empty()) {
332 inputs_.insert_or_assign(parsed_name, input);
336void BaseControllerInterface::add_inputs() {
337 for (
auto& [name, input] : inputs_) {
342 [&](
const realtime_tools::RealtimeBuffer<std::shared_ptr<EncodedState>>&) {
343 subscriptions_.push_back(create_subscription<EncodedState>(name, topic));
345 [&](
const realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Bool>>&) {
346 subscriptions_.push_back(create_subscription<std_msgs::msg::Bool>(name, topic));
348 [&](
const realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Float64>>&) {
349 subscriptions_.push_back(create_subscription<std_msgs::msg::Float64>(name, topic));
351 [&](
const realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Float64MultiArray>>&) {
352 subscriptions_.push_back(create_subscription<std_msgs::msg::Float64MultiArray>(name, topic));
354 [&](
const realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Int32>>&) {
355 subscriptions_.push_back(create_subscription<std_msgs::msg::Int32>(name, topic));
357 [&](
const realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::String>>&) {
358 subscriptions_.push_back(create_subscription<std_msgs::msg::String>(name, topic));
360 [&](
const std::any&) {
361 custom_input_configuration_callables_.at(name)(name, topic);
364 }
catch (
const std::exception& ex) {
365 RCLCPP_ERROR(get_node()->get_logger(),
"Failed to add input '%s': %s", name.c_str(), ex.what());
370void BaseControllerInterface::create_output(
371 const PublisherVariant& publishers,
const std::string& name,
const std::string& topic_name) {
372 auto parsed_name = validate_and_declare_signal(name,
"output", topic_name);
373 if (!parsed_name.empty()) {
374 outputs_.insert_or_assign(parsed_name, publishers);
378void BaseControllerInterface::add_outputs() {
379 for (
auto& [name, publishers] : outputs_) {
384 [&](EncodedStatePublishers& pub) {
385 std::get<1>(pub) = get_node()->create_publisher<EncodedState>(topic, qos_);
386 std::get<2>(pub) = std::make_shared<realtime_tools::RealtimePublisher<EncodedState>>(std::get<1>(pub));
388 [&](BoolPublishers& pub) {
389 pub.first = get_node()->create_publisher<std_msgs::msg::Bool>(topic, qos_);
390 pub.second = std::make_shared<realtime_tools::RealtimePublisher<std_msgs::msg::Bool>>(pub.first);
392 [&](DoublePublishers& pub) {
393 pub.first = get_node()->create_publisher<std_msgs::msg::Float64>(topic, qos_);
394 pub.second = std::make_shared<realtime_tools::RealtimePublisher<std_msgs::msg::Float64>>(pub.first);
396 [&](DoubleVecPublishers& pub) {
397 pub.first = get_node()->create_publisher<std_msgs::msg::Float64MultiArray>(topic, qos_);
399 std::make_shared<realtime_tools::RealtimePublisher<std_msgs::msg::Float64MultiArray>>(pub.first);
401 [&](IntPublishers& pub) {
402 pub.first = get_node()->create_publisher<std_msgs::msg::Int32>(topic, qos_);
403 pub.second = std::make_shared<realtime_tools::RealtimePublisher<std_msgs::msg::Int32>>(pub.first);
405 [&](StringPublishers& pub) {
406 pub.first = get_node()->create_publisher<std_msgs::msg::String>(topic, qos_);
407 pub.second = std::make_shared<realtime_tools::RealtimePublisher<std_msgs::msg::String>>(pub.first);
409 [&](CustomPublishers& pub) {
410 custom_output_configuration_callables_.at(name)(pub, name);
413 }
catch (
const std::bad_any_cast& ex) {
414 RCLCPP_ERROR(get_node()->get_logger(),
"Failed to add custom output '%s': %s", name.c_str(), ex.what());
415 }
catch (
const std::exception& ex) {
416 RCLCPP_ERROR(get_node()->get_logger(),
"Failed to add output '%s': %s", name.c_str(), ex.what());
422 input_validity_period_ = input_validity_period;
425bool BaseControllerInterface::check_input_valid(
const std::string& name)
const {
426 if (inputs_.find(name) == inputs_.end()) {
427 RCLCPP_WARN_THROTTLE(
428 get_node()->get_logger(), *get_node()->get_clock(), 1000,
"Could not find input '%s'", name.c_str());
431 if (
static_cast<double>(std::chrono::duration_cast<std::chrono::nanoseconds>(
432 std::chrono::steady_clock::now() - inputs_.at(name).timestamp)
435 >= input_validity_period_) {
442BaseControllerInterface::validate_service_name(
const std::string& service_name,
const std::string& type)
const {
443 std::string parsed_service_name = modulo_utils::parsing::parse_topic_name(service_name);
444 if (parsed_service_name.empty()) {
446 get_node()->get_logger(),
447 "The parsed service name for %s service '%s' is empty. Provide a string with valid characters for the service "
450 type.c_str(), service_name.c_str());
453 if (service_name != parsed_service_name) {
455 get_node()->get_logger(),
456 "The parsed name for '%s' service '%s' is '%s'. Use the parsed name to refer to this service.", type.c_str(),
457 service_name.c_str(), parsed_service_name.c_str());
459 if (empty_services_.find(parsed_service_name) != empty_services_.cend()) {
461 get_node()->get_logger(),
"Service with name '%s' already exists as empty service.",
462 parsed_service_name.c_str());
465 if (string_services_.find(parsed_service_name) != string_services_.cend()) {
467 get_node()->get_logger(),
"Service with name '%s' already exists as string service.",
468 parsed_service_name.c_str());
471 RCLCPP_DEBUG(get_node()->get_logger(),
"Adding %s service '%s'.", type.c_str(), parsed_service_name.c_str());
472 return parsed_service_name;
477 auto parsed_service_name = validate_service_name(service_name,
"empty");
478 if (!parsed_service_name.empty()) {
480 auto service = get_node()->create_service<modulo_interfaces::srv::EmptyTrigger>(
481 "~/" + parsed_service_name,
483 const std::shared_ptr<modulo_interfaces::srv::EmptyTrigger::Request>,
484 std::shared_ptr<modulo_interfaces::srv::EmptyTrigger::Response> response) {
486 if (this->command_mutex_.try_lock_for(100ms)) {
487 auto callback_response = callback();
488 this->command_mutex_.unlock();
489 response->success = callback_response.success;
490 response->message = callback_response.message;
492 response->success =
false;
493 response->message =
"Unable to acquire lock for command interface within 100ms";
495 }
catch (
const std::exception& ex) {
496 response->success =
false;
497 response->message = ex.what();
501 empty_services_.insert_or_assign(parsed_service_name, service);
502 }
catch (
const std::exception& ex) {
503 RCLCPP_ERROR(get_node()->get_logger(),
"Failed to add service '%s': %s", parsed_service_name.c_str(), ex.what());
509 const std::string& service_name,
511 auto parsed_service_name = validate_service_name(service_name,
"string");
512 if (!parsed_service_name.empty()) {
514 auto service = get_node()->create_service<modulo_interfaces::srv::StringTrigger>(
515 "~/" + parsed_service_name,
517 const std::shared_ptr<modulo_interfaces::srv::StringTrigger::Request> request,
518 std::shared_ptr<modulo_interfaces::srv::StringTrigger::Response> response) {
520 if (this->command_mutex_.try_lock_for(100ms)) {
521 auto callback_response = callback(request->payload);
522 this->command_mutex_.unlock();
523 response->success = callback_response.success;
524 response->message = callback_response.message;
526 response->success =
false;
527 response->message =
"Unable to acquire lock for command interface within 100ms";
529 }
catch (
const std::exception& ex) {
530 response->success =
false;
531 response->message = ex.what();
535 string_services_.insert_or_assign(parsed_service_name, service);
536 }
catch (
const std::exception& ex) {
537 RCLCPP_ERROR(get_node()->get_logger(),
"Failed to add service '%s': %s", parsed_service_name.c_str(), ex.what());
551 return get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE;
555 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.