- Modulo 3.1.0
+ Modulo 3.2.0
|
@@ -325,824 +325,832 @@
594 const rclcpp::NodeOptions& options, modulo_core::communication::PublisherType publisher_type,
- 595 const std::string& fallback_name
-
- 597 NodeT(utilities::parse_node_name(options, fallback_name), options), publisher_type_(publisher_type) {
+ 595 const std::string& fallback_name)
+ 596 : NodeT(utilities::parse_node_name(options, fallback_name), utilities::modify_parameter_overrides(options)),
+ 597 publisher_type_(publisher_type) {
599 parameter_cb_handle_ = NodeT::add_on_set_parameters_callback(
600 [
this](
const std::vector<rclcpp::Parameter>& parameters) -> rcl_interfaces::msg::SetParametersResult {
601 return this->on_set_parameters_callback(parameters);
- 603 this->add_parameter(
"period", 0.1,
"The time interval in seconds for all periodic callbacks",
true);
-
- 605 this->predicate_publisher_ =
- 606 rclcpp::create_publisher<modulo_component_interfaces::msg::Predicate>(*
this,
"/predicates", this->qos_);
- 607 this->add_predicate(
"in_error_state",
false);
-
- 609 this->step_timer_ = this->create_wall_timer(
- 610 std::chrono::nanoseconds(
static_cast<int64_t
>(this->get_parameter_value<double>(
"period") * 1e9)),
- 611 [
this] { this->step(); });
-
-
-
-
-
-
-
-
-
-
-
- 623 const std::string& name,
const T& value,
const std::string& description,
bool read_only
-
-
- 626 RCLCPP_ERROR(this->get_logger(),
"Failed to add parameter: Provide a non empty string as a name.");
-
-
- 629 this->
add_parameter(state_representation::make_shared_parameter(name, value), description, read_only);
-
-
-
-
-
-
- 636 return this->parameter_map_.template get_parameter_value<T>(name);
- 637 }
catch (
const state_representation::exceptions::InvalidParameterException& ex) {
-
- 639 "Failed to get parameter value of parameter '" + name +
"': " + ex.what());
-
-
-
-
-
- 645 const std::shared_ptr<state_representation::ParameterInterface>& parameter,
const std::string& description,
-
-
- 648 this->set_parameter_callback_called_ =
false;
- 649 rclcpp::Parameter ros_param;
-
-
-
-
-
- 655 if (!NodeT::has_parameter(parameter->get_name())) {
- 656 RCLCPP_DEBUG_STREAM(this->get_logger(),
"Adding parameter '" << parameter->get_name() <<
"'.");
- 657 this->parameter_map_.set_parameter(parameter);
-
- 659 rcl_interfaces::msg::ParameterDescriptor descriptor;
- 660 descriptor.description = description;
- 661 descriptor.read_only = read_only;
- 662 if (parameter->is_empty()) {
- 663 descriptor.dynamic_typing =
true;
-
- 665 NodeT::declare_parameter(parameter->get_name(), rclcpp::ParameterValue{}, descriptor);
-
- 667 NodeT::declare_parameter(parameter->get_name(), ros_param.get_parameter_value(), descriptor);
-
- 669 if (!this->set_parameter_callback_called_) {
- 670 auto result = this->on_set_parameters_callback({NodeT::get_parameters({parameter->get_name()})});
- 671 if (!result.successful) {
- 672 NodeT::undeclare_parameter(parameter->get_name());
-
-
-
- 676 }
catch (
const std::exception& ex) {
- 677 this->parameter_map_.remove_parameter(parameter->get_name());
-
-
-
- 681 RCLCPP_DEBUG_STREAM(this->get_logger(),
"Parameter '" << parameter->get_name() <<
"' already exists.");
-
-
-
-
- 686inline std::shared_ptr<state_representation::ParameterInterface>
-
-
- 689 return this->parameter_map_.get_parameter(name);
- 690 }
catch (
const state_representation::exceptions::InvalidParameterException& ex) {
-
-
-
-
-
-
-
-
- 699 rcl_interfaces::msg::SetParametersResult result = NodeT::set_parameter(
-
- 701 if (!result.successful) {
- 702 RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
- 703 "Failed to set parameter value of parameter '" << name <<
"': " << result.reason);
-
- 705 }
catch (
const std::exception& ex) {
- 706 RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
- 707 "Failed to set parameter value of parameter '" << name <<
"': " << ex.what());
-
-
-
-
-
- 713 const std::shared_ptr<state_representation::ParameterInterface>& parameter
-
- 715 if (parameter->get_name() ==
"period") {
-
- 717 if (value <= 0.0 || !std::isfinite(value)) {
- 718 RCLCPP_ERROR(this->get_logger(),
"Value for parameter 'period' has to be a positive finite number.");
-
-
-
-
-
-
-
-
- 727 const std::shared_ptr<state_representation::ParameterInterface>&
-
-
-
-
-
- 733inline rcl_interfaces::msg::SetParametersResult
-
- 735 rcl_interfaces::msg::SetParametersResult result;
- 736 result.successful =
true;
- 737 for (
const auto& ros_parameter : parameters) {
-
- 739 if (ros_parameter.get_name().substr(0, 27) ==
"qos_overrides./tf.publisher") {
-
-
-
- 743 auto parameter = parameter_map_.get_parameter(ros_parameter.get_name());
-
-
-
- 747 if (!this->validate_parameter(new_parameter)) {
- 748 result.successful =
false;
- 749 result.reason +=
"Validation of parameter '" + ros_parameter.get_name() +
"' returned false!";
- 750 }
else if (!new_parameter->is_empty()) {
-
-
-
- 754 }
catch (
const std::exception& ex) {
- 755 result.successful =
false;
- 756 result.reason += ex.what();
-
-
- 759 this->set_parameter_callback_called_ =
true;
-
-
-
-
-
- 765 this->add_variant_predicate(name, utilities::PredicateVariant(predicate));
-
-
-
-
- 770 const std::string& name,
const std::function<
bool(
void)>& predicate
-
- 772 this->add_variant_predicate(name, utilities::PredicateVariant(predicate));
-
-
-
-
- 777 const std::string& name,
const utilities::PredicateVariant& predicate
-
-
- 780 RCLCPP_ERROR(this->get_logger(),
"Failed to add predicate: Provide a non empty string as a name.");
-
-
- 783 if (this->predicates_.find(name) != this->predicates_.end()) {
- 784 RCLCPP_WARN_STREAM(this->get_logger(),
"Predicate with name '" << name <<
"' already exists, overwriting.");
-
- 786 RCLCPP_DEBUG_STREAM(this->get_logger(),
"Adding predicate '" << name <<
"'.");
-
- 788 this->predicates_.insert_or_assign(name, predicate);
-
-
-
-
- 793 auto predicate_iterator = this->predicates_.find(predicate_name);
-
- 795 if (predicate_iterator == this->predicates_.end()) {
- 796 RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
- 797 "Failed to get predicate '" << predicate_name
- 798 <<
"': Predicate does not exists, returning false.");
-
-
-
- 802 auto* ptr_value = std::get_if<bool>(&predicate_iterator->second);
-
-
-
-
- 807 auto callback_function = std::get<std::function<bool(
void)>>(predicate_iterator->second);
-
-
- 810 value = (callback_function)();
- 811 }
catch (
const std::exception& ex) {
- 812 RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
- 813 "Failed to evaluate callback of predicate '" << predicate_name
- 814 <<
"', returning false: " << ex.what());
-
-
-
-
-
-
- 821 if (trigger_name.empty()) {
- 822 RCLCPP_ERROR(this->get_logger(),
"Failed to add trigger: Provide a non empty string as a name.");
-
-
- 825 if (this->triggers_.find(trigger_name) != this->triggers_.end()
- 826 || this->predicates_.find(trigger_name) != this->predicates_.end()) {
- 827 RCLCPP_ERROR_STREAM(this->get_logger(),
"Failed to add trigger: there is already a trigger or "
- 828 "predicate with name '" << trigger_name <<
"'.");
-
-
- 831 this->triggers_.insert_or_assign(trigger_name,
false);
-
- 833 trigger_name, [
this, trigger_name] {
- 834 auto value = this->triggers_.at(trigger_name);
- 835 this->triggers_.at(trigger_name) =
false;
-
-
-
-
-
-
- 842 if (this->triggers_.find(trigger_name) == this->triggers_.end()) {
- 843 RCLCPP_ERROR_STREAM(this->get_logger(),
"Failed to trigger: could not find trigger"
- 844 " with name '" << trigger_name <<
"'.");
-
-
- 847 this->triggers_.at(trigger_name) =
true;
-
-
-
-
-
- 853 const std::string& name,
const utilities::PredicateVariant& predicate
-
- 855 auto predicate_iterator = this->predicates_.find(name);
- 856 if (predicate_iterator == this->predicates_.end()) {
- 857 RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
- 858 "Failed to set predicate '" << name <<
"': Predicate does not exist.");
-
-
- 861 predicate_iterator->second = predicate;
-
-
-
-
-
- 867 this->set_variant_predicate(name, utilities::PredicateVariant(predicate));
-
-
-
-
- 872 const std::string& name,
const std::function<
bool(
void)>& predicate
-
- 874 this->set_variant_predicate(name, utilities::PredicateVariant(predicate));
-
-
-
-
-
- 880 const std::string& signal_name, std::map<std::string, std::shared_ptr<T>>& signal_map,
bool skip_check
+ 603 this->add_parameter(
"rate", 10,
"The rate in Hertz for all periodic callbacks",
true);
+ 604 this->add_parameter(
"period", 0.1,
"The time interval in seconds for all periodic callbacks",
true);
+
+ 606 this->predicate_publisher_ =
+ 607 rclcpp::create_publisher<modulo_component_interfaces::msg::Predicate>(*
this,
"/predicates", this->qos_);
+ 608 this->add_predicate(
"in_error_state",
false);
+
+ 610 this->step_timer_ = this->create_wall_timer(
+ 611 std::chrono::nanoseconds(
static_cast<int64_t
>(this->get_parameter_value<double>(
"period") * 1e9)),
+ 612 [
this] { this->step(); });
+
+
+
+
+
+
+
+
+
+
+
+ 624 const std::string& name,
const T& value,
const std::string& description,
bool read_only
+
+
+ 627 RCLCPP_ERROR(this->get_logger(),
"Failed to add parameter: Provide a non empty string as a name.");
+
+
+ 630 this->
add_parameter(state_representation::make_shared_parameter(name, value), description, read_only);
+
+
+
+
+
+
+ 637 return this->parameter_map_.template get_parameter_value<T>(name);
+ 638 }
catch (
const state_representation::exceptions::InvalidParameterException& ex) {
+
+ 640 "Failed to get parameter value of parameter '" + name +
"': " + ex.what());
+
+
+
+
+
+ 646 const std::shared_ptr<state_representation::ParameterInterface>& parameter,
const std::string& description,
+
+
+ 649 this->set_parameter_callback_called_ =
false;
+ 650 rclcpp::Parameter ros_param;
+
+
+
+
+
+ 656 if (!NodeT::has_parameter(parameter->get_name())) {
+ 657 RCLCPP_DEBUG_STREAM(this->get_logger(),
"Adding parameter '" << parameter->get_name() <<
"'.");
+ 658 this->parameter_map_.set_parameter(parameter);
+
+ 660 rcl_interfaces::msg::ParameterDescriptor descriptor;
+ 661 descriptor.description = description;
+ 662 descriptor.read_only = read_only;
+ 663 if (parameter->is_empty()) {
+ 664 descriptor.dynamic_typing =
true;
+
+ 666 NodeT::declare_parameter(parameter->get_name(), rclcpp::ParameterValue{}, descriptor);
+
+ 668 NodeT::declare_parameter(parameter->get_name(), ros_param.get_parameter_value(), descriptor);
+
+ 670 if (!this->set_parameter_callback_called_) {
+ 671 auto result = this->on_set_parameters_callback({NodeT::get_parameters({parameter->get_name()})});
+ 672 if (!result.successful) {
+ 673 NodeT::undeclare_parameter(parameter->get_name());
+
+
+
+ 677 }
catch (
const std::exception& ex) {
+ 678 this->parameter_map_.remove_parameter(parameter->get_name());
+
+
+
+ 682 RCLCPP_DEBUG_STREAM(this->get_logger(),
"Parameter '" << parameter->get_name() <<
"' already exists.");
+
+
+
+
+ 687inline std::shared_ptr<state_representation::ParameterInterface>
+
+
+ 690 return this->parameter_map_.get_parameter(name);
+ 691 }
catch (
const state_representation::exceptions::InvalidParameterException& ex) {
+
+
+
+
+
+
+
+
+ 700 rcl_interfaces::msg::SetParametersResult result = NodeT::set_parameter(
+
+ 702 if (!result.successful) {
+ 703 RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
+ 704 "Failed to set parameter value of parameter '" << name <<
"': " << result.reason);
+
+ 706 }
catch (
const std::exception& ex) {
+ 707 RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
+ 708 "Failed to set parameter value of parameter '" << name <<
"': " << ex.what());
+
+
+
+
+
+ 714 const std::shared_ptr<state_representation::ParameterInterface>& parameter
+
+ 716 if (parameter->get_name() ==
"rate") {
+
+ 718 if (value <= 0 || !std::isfinite(value)) {
+ 719 RCLCPP_ERROR(this->get_logger(),
"Value for parameter 'rate' has to be a positive finite number.");
+
+
+
+ 723 if (parameter->get_name() ==
"period") {
+ 724 auto value = parameter->get_parameter_value<
double>();
+ 725 if (value <= 0.0 || !std::isfinite(value)) {
+ 726 RCLCPP_ERROR(this->get_logger(),
"Value for parameter 'period' has to be a positive finite number.");
+
+
+
+
+
+
+
+
+ 735 const std::shared_ptr<state_representation::ParameterInterface>&
+
+
+
+
+
+ 741inline rcl_interfaces::msg::SetParametersResult
+
+ 743 rcl_interfaces::msg::SetParametersResult result;
+ 744 result.successful =
true;
+ 745 for (
const auto& ros_parameter : parameters) {
+
+ 747 if (ros_parameter.get_name().substr(0, 27) ==
"qos_overrides./tf.publisher") {
+
+
+
+ 751 auto parameter = parameter_map_.get_parameter(ros_parameter.get_name());
+
+
+
+ 755 if (!this->validate_parameter(new_parameter)) {
+ 756 result.successful =
false;
+ 757 result.reason +=
"Validation of parameter '" + ros_parameter.get_name() +
"' returned false!";
+ 758 }
else if (!new_parameter->is_empty()) {
+
+
+
+ 762 }
catch (
const std::exception& ex) {
+ 763 result.successful =
false;
+ 764 result.reason += ex.what();
+
+
+ 767 this->set_parameter_callback_called_ =
true;
+
+
+
+
+
+ 773 this->add_variant_predicate(name, utilities::PredicateVariant(predicate));
+
+
+
+
+ 778 const std::string& name,
const std::function<
bool(
void)>& predicate
+
+ 780 this->add_variant_predicate(name, utilities::PredicateVariant(predicate));
+
+
+
+
+ 785 const std::string& name,
const utilities::PredicateVariant& predicate
+
+
+ 788 RCLCPP_ERROR(this->get_logger(),
"Failed to add predicate: Provide a non empty string as a name.");
+
+
+ 791 if (this->predicates_.find(name) != this->predicates_.end()) {
+ 792 RCLCPP_WARN_STREAM(this->get_logger(),
"Predicate with name '" << name <<
"' already exists, overwriting.");
+
+ 794 RCLCPP_DEBUG_STREAM(this->get_logger(),
"Adding predicate '" << name <<
"'.");
+
+ 796 this->predicates_.insert_or_assign(name, predicate);
+
+
+
+
+ 801 auto predicate_iterator = this->predicates_.find(predicate_name);
+
+ 803 if (predicate_iterator == this->predicates_.end()) {
+ 804 RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
+ 805 "Failed to get predicate '" << predicate_name
+ 806 <<
"': Predicate does not exists, returning false.");
+
+
+
+ 810 auto* ptr_value = std::get_if<bool>(&predicate_iterator->second);
+
+
+
+
+ 815 auto callback_function = std::get<std::function<bool(
void)>>(predicate_iterator->second);
+
+
+ 818 value = (callback_function)();
+ 819 }
catch (
const std::exception& ex) {
+ 820 RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
+ 821 "Failed to evaluate callback of predicate '" << predicate_name
+ 822 <<
"', returning false: " << ex.what());
+
+
+
+
+
+
+ 829 if (trigger_name.empty()) {
+ 830 RCLCPP_ERROR(this->get_logger(),
"Failed to add trigger: Provide a non empty string as a name.");
+
+
+ 833 if (this->triggers_.find(trigger_name) != this->triggers_.end()
+ 834 || this->predicates_.find(trigger_name) != this->predicates_.end()) {
+ 835 RCLCPP_ERROR_STREAM(this->get_logger(),
"Failed to add trigger: there is already a trigger or "
+ 836 "predicate with name '" << trigger_name <<
"'.");
+
+
+ 839 this->triggers_.insert_or_assign(trigger_name,
false);
+
+ 841 trigger_name, [
this, trigger_name] {
+ 842 auto value = this->triggers_.at(trigger_name);
+ 843 this->triggers_.at(trigger_name) =
false;
+
+
+
+
+
+
+ 850 if (this->triggers_.find(trigger_name) == this->triggers_.end()) {
+ 851 RCLCPP_ERROR_STREAM(this->get_logger(),
"Failed to trigger: could not find trigger"
+ 852 " with name '" << trigger_name <<
"'.");
+
+
+ 855 this->triggers_.at(trigger_name) =
true;
+
+
+
+
+
+ 861 const std::string& name,
const utilities::PredicateVariant& predicate
+
+ 863 auto predicate_iterator = this->predicates_.find(name);
+ 864 if (predicate_iterator == this->predicates_.end()) {
+ 865 RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
+ 866 "Failed to set predicate '" << name <<
"': Predicate does not exist.");
+
+
+ 869 predicate_iterator->second = predicate;
+
+
+
+
+
+ 875 this->set_variant_predicate(name, utilities::PredicateVariant(predicate));
+
+
+
+
+ 880 const std::string& name,
const std::function<
bool(
void)>& predicate
- 882 if (!skip_check && signal_map.find(signal_name) == signal_map.cend()) {
-
-
- 885 RCLCPP_DEBUG_STREAM(this->get_logger(),
"Removing signal '" << signal_name <<
"'.");
- 886 signal_map.at(signal_name).reset();
- 887 return signal_map.erase(signal_name);
-
-
-
-
-
- 893 if (!this->
template remove_signal(signal_name, this->
inputs_)) {
- 894 auto parsed_signal_name = utilities::parse_topic_name(signal_name);
- 895 if (!this->
template remove_signal(parsed_signal_name, this->
inputs_)) {
- 896 RCLCPP_DEBUG_STREAM(this->get_logger(),
- 897 "Unknown input '" << signal_name <<
"' (parsed name was '" << parsed_signal_name <<
"').");
-
-
-
-
-
-
- 904 if (!this->
template remove_signal(signal_name, this->
outputs_)) {
- 905 auto parsed_signal_name = utilities::parse_topic_name(signal_name);
- 906 if (!this->
template remove_signal(parsed_signal_name, this->
outputs_)) {
- 907 RCLCPP_DEBUG_STREAM(this->get_logger(),
- 908 "Unknown output '" << signal_name <<
"' (parsed name was '" << parsed_signal_name <<
"').");
-
-
-
-
-
-
- 915 const std::string& signal_name,
const std::string& type,
const std::string& default_topic,
bool fixed_topic
-
- 917 std::string parsed_signal_name = utilities::parse_topic_name(signal_name);
- 918 if (parsed_signal_name.empty()) {
-
- 920 "The parsed signal name for " + type +
" '" + signal_name
- 921 +
"' is empty. Provide a string with valid characters for the signal name ([a-zA-Z0-9_]).");
-
- 923 if (this->
inputs_.find(parsed_signal_name) != this->inputs_.cend()) {
- 924 throw exceptions::AddSignalException(
"Signal with name '" + parsed_signal_name +
"' already exists as input.");
-
- 926 if (this->
outputs_.find(parsed_signal_name) != this->outputs_.cend()) {
- 927 throw exceptions::AddSignalException(
"Signal with name '" + parsed_signal_name +
"' already exists as output.");
-
- 929 std::string topic_name = default_topic.empty() ?
"~/" + parsed_signal_name : default_topic;
- 930 auto parameter_name = parsed_signal_name +
"_topic";
- 931 if (NodeT::has_parameter(parameter_name) && this->
get_parameter(parameter_name)->is_empty()) {
- 932 this->set_parameter_value<std::string>(parameter_name, topic_name);
-
-
- 935 parameter_name, topic_name,
"Signal topic name of " + type +
" '" + parsed_signal_name +
"'", fixed_topic);
+ 882 this->set_variant_predicate(name, utilities::PredicateVariant(predicate));
+
+
+
+
+
+ 888 const std::string& signal_name, std::map<std::string, std::shared_ptr<T>>& signal_map,
bool skip_check
+
+ 890 if (!skip_check && signal_map.find(signal_name) == signal_map.cend()) {
+
+
+ 893 RCLCPP_DEBUG_STREAM(this->get_logger(),
"Removing signal '" << signal_name <<
"'.");
+ 894 signal_map.at(signal_name).reset();
+ 895 return signal_map.erase(signal_name);
+
+
+
+
+
+ 901 if (!this->
template remove_signal(signal_name, this->
inputs_)) {
+ 902 auto parsed_signal_name = utilities::parse_topic_name(signal_name);
+ 903 if (!this->
template remove_signal(parsed_signal_name, this->
inputs_)) {
+ 904 RCLCPP_DEBUG_STREAM(this->get_logger(),
+ 905 "Unknown input '" << signal_name <<
"' (parsed name was '" << parsed_signal_name <<
"').");
+
+
+
+
+
+
+ 912 if (!this->
template remove_signal(signal_name, this->
outputs_)) {
+ 913 auto parsed_signal_name = utilities::parse_topic_name(signal_name);
+ 914 if (!this->
template remove_signal(parsed_signal_name, this->
outputs_)) {
+ 915 RCLCPP_DEBUG_STREAM(this->get_logger(),
+ 916 "Unknown output '" << signal_name <<
"' (parsed name was '" << parsed_signal_name <<
"').");
+
+
+
+
+
+
+ 923 const std::string& signal_name,
const std::string& type,
const std::string& default_topic,
bool fixed_topic
+
+ 925 std::string parsed_signal_name = utilities::parse_topic_name(signal_name);
+ 926 if (parsed_signal_name.empty()) {
+
+ 928 "The parsed signal name for " + type +
" '" + signal_name
+ 929 +
"' is empty. Provide a string with valid characters for the signal name ([a-zA-Z0-9_]).");
+
+ 931 if (this->
inputs_.find(parsed_signal_name) != this->inputs_.cend()) {
+ 932 throw exceptions::AddSignalException(
"Signal with name '" + parsed_signal_name +
"' already exists as input.");
+
+ 934 if (this->
outputs_.find(parsed_signal_name) != this->outputs_.cend()) {
+ 935 throw exceptions::AddSignalException(
"Signal with name '" + parsed_signal_name +
"' already exists as output.");
- 937 RCLCPP_DEBUG_STREAM(this->get_logger(),
- 938 "Declared signal '" << parsed_signal_name <<
"' and parameter '" << parameter_name
- 939 <<
"' with value '" << topic_name <<
"'.");
-
-
-
-
- 944 const std::string& signal_name,
const std::string& default_topic,
bool fixed_topic
-
- 946 this->declare_signal(signal_name,
"input", default_topic, fixed_topic);
-
-
-
-
- 951 const std::string& signal_name,
const std::string& default_topic,
bool fixed_topic
-
- 953 this->declare_signal(signal_name,
"output", default_topic, fixed_topic);
-
-
-
- 957template<
typename DataT>
-
- 959 const std::string& signal_name,
const std::shared_ptr<DataT>& data,
const std::string& default_topic,
-
-
- 962 this->
add_input(signal_name, data, [] {}, default_topic, fixed_topic);
-
-
-
- 966template<
typename DataT>
-
- 968 const std::string& signal_name,
const std::shared_ptr<DataT>& data,
const std::function<
void()>& user_callback,
- 969 const std::string& default_topic,
bool fixed_topic
-
-
-
- 973 std::string parsed_signal_name = utilities::parse_topic_name(signal_name);
- 974 if (data ==
nullptr) {
-
- 976 "Invalid data pointer for input '" + parsed_signal_name +
"'.");
-
- 978 this->
declare_input(parsed_signal_name, default_topic, fixed_topic);
- 979 auto topic_name = this->get_parameter_value<std::string>(parsed_signal_name +
"_topic");
- 980 RCLCPP_DEBUG_STREAM(this->get_logger(),
- 981 "Adding input '" << parsed_signal_name <<
"' with topic name '" << topic_name <<
"'.");
- 982 auto message_pair = make_shared_message_pair(data, this->get_clock());
- 983 std::shared_ptr<SubscriptionInterface> subscription_interface;
- 984 switch (message_pair->get_type()) {
- 985 case MessageType::BOOL: {
- 986 auto subscription_handler = std::make_shared<SubscriptionHandler<std_msgs::msg::Bool>>(message_pair);
- 987 auto subscription = NodeT::template create_subscription<std_msgs::msg::Bool>(
- 988 topic_name, this->
qos_, subscription_handler->get_callback(user_callback));
- 989 subscription_interface = subscription_handler->create_subscription_interface(subscription);
-
-
- 992 case MessageType::FLOAT64: {
- 993 auto subscription_handler = std::make_shared<SubscriptionHandler<std_msgs::msg::Float64>>(message_pair);
- 994 auto subscription = NodeT::template create_subscription<std_msgs::msg::Float64>(
- 995 topic_name, this->
qos_, subscription_handler->get_callback(user_callback));
- 996 subscription_interface = subscription_handler->create_subscription_interface(subscription);
-
-
- 999 case MessageType::FLOAT64_MULTI_ARRAY: {
- 1000 auto subscription_handler =
- 1001 std::make_shared<SubscriptionHandler<std_msgs::msg::Float64MultiArray>>(message_pair);
- 1002 auto subscription = NodeT::template create_subscription<std_msgs::msg::Float64MultiArray>(
+ 937 std::string topic_name = default_topic.empty() ?
"~/" + parsed_signal_name : default_topic;
+ 938 auto parameter_name = parsed_signal_name +
"_topic";
+ 939 if (NodeT::has_parameter(parameter_name) && this->
get_parameter(parameter_name)->is_empty()) {
+ 940 this->set_parameter_value<std::string>(parameter_name, topic_name);
+
+
+ 943 parameter_name, topic_name,
"Signal topic name of " + type +
" '" + parsed_signal_name +
"'", fixed_topic);
+
+ 945 RCLCPP_DEBUG_STREAM(this->get_logger(),
+ 946 "Declared signal '" << parsed_signal_name <<
"' and parameter '" << parameter_name
+ 947 <<
"' with value '" << topic_name <<
"'.");
+
+
+
+
+ 952 const std::string& signal_name,
const std::string& default_topic,
bool fixed_topic
+
+ 954 this->declare_signal(signal_name,
"input", default_topic, fixed_topic);
+
+
+
+
+ 959 const std::string& signal_name,
const std::string& default_topic,
bool fixed_topic
+
+ 961 this->declare_signal(signal_name,
"output", default_topic, fixed_topic);
+
+
+
+ 965template<
typename DataT>
+
+ 967 const std::string& signal_name,
const std::shared_ptr<DataT>& data,
const std::string& default_topic,
+
+
+ 970 this->
add_input(signal_name, data, [] {}, default_topic, fixed_topic);
+
+
+
+ 974template<
typename DataT>
+
+ 976 const std::string& signal_name,
const std::shared_ptr<DataT>& data,
const std::function<
void()>& user_callback,
+ 977 const std::string& default_topic,
bool fixed_topic
+
+
+
+ 981 std::string parsed_signal_name = utilities::parse_topic_name(signal_name);
+ 982 if (data ==
nullptr) {
+
+ 984 "Invalid data pointer for input '" + parsed_signal_name +
"'.");
+
+ 986 this->
declare_input(parsed_signal_name, default_topic, fixed_topic);
+ 987 auto topic_name = this->get_parameter_value<std::string>(parsed_signal_name +
"_topic");
+ 988 RCLCPP_DEBUG_STREAM(this->get_logger(),
+ 989 "Adding input '" << parsed_signal_name <<
"' with topic name '" << topic_name <<
"'.");
+ 990 auto message_pair = make_shared_message_pair(data, this->get_clock());
+ 991 std::shared_ptr<SubscriptionInterface> subscription_interface;
+ 992 switch (message_pair->get_type()) {
+ 993 case MessageType::BOOL: {
+ 994 auto subscription_handler = std::make_shared<SubscriptionHandler<std_msgs::msg::Bool>>(message_pair);
+ 995 auto subscription = NodeT::template create_subscription<std_msgs::msg::Bool>(
+ 996 topic_name, this->
qos_, subscription_handler->get_callback(user_callback));
+ 997 subscription_interface = subscription_handler->create_subscription_interface(subscription);
+
+
+ 1000 case MessageType::FLOAT64: {
+ 1001 auto subscription_handler = std::make_shared<SubscriptionHandler<std_msgs::msg::Float64>>(message_pair);
+ 1002 auto subscription = NodeT::template create_subscription<std_msgs::msg::Float64>(
1003 topic_name, this->
qos_, subscription_handler->get_callback(user_callback));
1004 subscription_interface = subscription_handler->create_subscription_interface(subscription);
- 1007 case MessageType::INT32: {
- 1008 auto subscription_handler = std::make_shared<SubscriptionHandler<std_msgs::msg::Int32>>(message_pair);
- 1009 auto subscription = NodeT::template create_subscription<std_msgs::msg::Int32>(
- 1010 topic_name, this->
qos_, subscription_handler->get_callback(user_callback));
- 1011 subscription_interface = subscription_handler->create_subscription_interface(subscription);
-
-
- 1014 case MessageType::STRING: {
- 1015 auto subscription_handler = std::make_shared<SubscriptionHandler<std_msgs::msg::String>>(message_pair);
- 1016 auto subscription = NodeT::template create_subscription<std_msgs::msg::String>(
- 1017 topic_name, this->
qos_, subscription_handler->get_callback(user_callback));
- 1018 subscription_interface = subscription_handler->create_subscription_interface(subscription);
-
-
- 1021 case MessageType::ENCODED_STATE: {
- 1022 auto subscription_handler = std::make_shared<SubscriptionHandler<modulo_core::EncodedState>>(message_pair);
- 1023 auto subscription = NodeT::template create_subscription<modulo_core::EncodedState>(
- 1024 topic_name, this->
qos_, subscription_handler->get_callback(user_callback));
- 1025 subscription_interface = subscription_handler->create_subscription_interface(subscription);
-
-
-
- 1029 this->
inputs_.insert_or_assign(parsed_signal_name, subscription_interface);
- 1030 }
catch (
const std::exception& ex) {
- 1031 RCLCPP_ERROR_STREAM(this->get_logger(),
"Failed to add input '" << signal_name <<
"': " << ex.what());
-
-
-
- 1035template<
class NodeT>
- 1036template<
typename MsgT>
-
- 1038 const std::string& signal_name,
const std::function<
void(
const std::shared_ptr<MsgT>)>& callback,
- 1039 const std::string& default_topic,
bool fixed_topic
-
-
-
- 1043 std::string parsed_signal_name = utilities::parse_topic_name(signal_name);
- 1044 this->
declare_input(parsed_signal_name, default_topic, fixed_topic);
- 1045 auto topic_name = this->get_parameter_value<std::string>(parsed_signal_name +
"_topic");
- 1046 RCLCPP_DEBUG_STREAM(this->get_logger(),
- 1047 "Adding input '" << parsed_signal_name <<
"' with topic name '" << topic_name <<
"'.");
- 1048 auto subscription = NodeT::template create_subscription<MsgT>(topic_name, this->
qos_, callback);
- 1049 auto subscription_interface =
- 1050 std::make_shared<SubscriptionHandler<MsgT>>()->create_subscription_interface(subscription);
- 1051 this->
inputs_.insert_or_assign(parsed_signal_name, subscription_interface);
- 1052 }
catch (
const std::exception& ex) {
- 1053 RCLCPP_ERROR_STREAM(this->get_logger(),
"Failed to add input '" << signal_name <<
"': " << ex.what());
-
-
-
- 1057template<
class NodeT>
-
- 1059 std::string parsed_service_name = utilities::parse_topic_name(service_name);
- 1060 if (parsed_service_name.empty()) {
-
- 1062 "The parsed service name for service '" + service_name
- 1063 +
"' is empty. Provide a string with valid characters for the signal name ([a-zA-Z0-9_]).");
-
- 1065 if (this->empty_services_.find(parsed_service_name) != this->empty_services_.cend()
- 1066 || this->string_services_.find(parsed_service_name) != this->string_services_.cend()) {
- 1067 throw exceptions::AddServiceException(
"Service with name '" + parsed_service_name +
"' already exists.");
-
- 1069 return parsed_service_name;
-
-
- 1072template<
class NodeT>
-
-
-
-
- 1077 std::string parsed_service_name = this->validate_service_name(service_name);
- 1078 RCLCPP_DEBUG_STREAM(this->get_logger(),
"Adding empty service '" << parsed_service_name <<
"'.");
- 1079 auto service = NodeT::template create_service<modulo_component_interfaces::srv::EmptyTrigger>(
- 1080 "~/" + parsed_service_name, [callback](
- 1081 const std::shared_ptr<modulo_component_interfaces::srv::EmptyTrigger::Request>,
- 1082 std::shared_ptr<modulo_component_interfaces::srv::EmptyTrigger::Response> response
-
-
- 1085 auto callback_response = callback();
- 1086 response->success = callback_response.success;
- 1087 response->message = callback_response.message;
- 1088 }
catch (
const std::exception& ex) {
- 1089 response->success =
false;
- 1090 response->message = ex.what();
-
-
- 1093 this->empty_services_.insert_or_assign(parsed_service_name, service);
- 1094 }
catch (
const std::exception& ex) {
- 1095 RCLCPP_ERROR_STREAM(this->get_logger(),
"Failed to add service '" << service_name <<
"': " << ex.what());
-
-
-
- 1099template<
class NodeT>
-
- 1101 const std::string& service_name,
const std::function<
ComponentServiceResponse(
const std::string&
string)>& callback
-
-
- 1104 std::string parsed_service_name = this->validate_service_name(service_name);
- 1105 RCLCPP_DEBUG_STREAM(this->get_logger(),
"Adding string service '" << parsed_service_name <<
"'.");
- 1106 auto service = NodeT::template create_service<modulo_component_interfaces::srv::StringTrigger>(
- 1107 "~/" + parsed_service_name, [callback](
- 1108 const std::shared_ptr<modulo_component_interfaces::srv::StringTrigger::Request> request,
- 1109 std::shared_ptr<modulo_component_interfaces::srv::StringTrigger::Response> response
-
-
- 1112 auto callback_response = callback(request->payload);
- 1113 response->success = callback_response.success;
- 1114 response->message = callback_response.message;
- 1115 }
catch (
const std::exception& ex) {
- 1116 response->success =
false;
- 1117 response->message = ex.what();
-
-
- 1120 this->string_services_.insert_or_assign(parsed_service_name, service);
- 1121 }
catch (
const std::exception& ex) {
- 1122 RCLCPP_ERROR_STREAM(this->get_logger(),
"Failed to add service '" << service_name <<
"': " << ex.what());
-
-
-
- 1126template<
class NodeT>
-
-
-
- 1130 RCLCPP_ERROR(this->get_logger(),
"Failed to add periodic function: Provide a non empty string as a name.");
-
-
- 1133 if (this->periodic_callbacks_.find(name) != this->periodic_callbacks_.end()) {
- 1134 RCLCPP_WARN_STREAM(this->get_logger(),
"Periodic function '" << name <<
"' already exists, overwriting.");
-
- 1136 RCLCPP_DEBUG_STREAM(this->get_logger(),
"Adding periodic function '" << name <<
"'.");
-
- 1138 this->periodic_callbacks_.template insert_or_assign(name, callback);
-
-
- 1141template<
class NodeT>
-
- 1143 if (this->tf_broadcaster_ ==
nullptr) {
- 1144 RCLCPP_DEBUG(this->get_logger(),
"Adding TF broadcaster.");
- 1145 console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE);
- 1146 this->tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(*
this);
-
- 1148 RCLCPP_DEBUG(this->get_logger(),
"TF broadcaster already exists.");
-
-
-
- 1152template<
class NodeT>
-
- 1154 if (this->static_tf_broadcaster_ ==
nullptr) {
- 1155 RCLCPP_DEBUG(this->get_logger(),
"Adding static TF broadcaster.");
- 1156 console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE);
- 1157 tf2_ros::StaticBroadcasterQoS qos;
- 1158 rclcpp::PublisherOptionsWithAllocator<std::allocator<void>> options;
- 1159 this->static_tf_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(*
this, qos, options);
-
- 1161 RCLCPP_DEBUG(this->get_logger(),
"Static TF broadcaster already exists.");
-
-
-
- 1165template<
class NodeT>
-
- 1167 if (this->tf_buffer_ ==
nullptr || this->tf_listener_ ==
nullptr) {
- 1168 RCLCPP_DEBUG(this->get_logger(),
"Adding TF buffer and listener.");
- 1169 console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE);
- 1170 this->tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
- 1171 this->tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*this->tf_buffer_);
-
- 1173 RCLCPP_DEBUG(this->get_logger(),
"TF buffer and listener already exist.");
-
-
-
- 1177template<
class NodeT>
-
- 1179 this->
send_transforms(std::vector<state_representation::CartesianPose>{transform});
-
-
- 1182template<
class NodeT>
-
-
-
-
-
- 1188template<
class NodeT>
-
-
-
-
- 1193template<
class NodeT>
-
-
-
-
-
- 1199template<
class NodeT>
-
-
- 1202 const std::vector<state_representation::CartesianPose>& transforms,
const std::shared_ptr<T>& tf_broadcaster,
-
-
- 1205 std::string modifier = is_static ?
"static " :
"";
- 1206 if (tf_broadcaster ==
nullptr) {
- 1207 RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
- 1208 "Failed to send " << modifier <<
"transform: No " << modifier
- 1209 <<
"TF broadcaster configured.");
-
-
-
- 1213 std::vector<geometry_msgs::msg::TransformStamped> transform_messages;
- 1214 transform_messages.reserve(transforms.size());
- 1215 for (
const auto& tf : transforms) {
- 1216 geometry_msgs::msg::TransformStamped transform_message;
-
- 1218 transform_messages.emplace_back(transform_message);
-
- 1220 tf_broadcaster->sendTransform(transform_messages);
- 1221 }
catch (
const std::exception& ex) {
- 1222 RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
- 1223 "Failed to send " << modifier <<
"transform: " << ex.what());
-
-
-
- 1227template<
class NodeT>
-
- 1229 const std::string& frame,
const std::string& reference_frame,
const tf2::TimePoint& time_point,
- 1230 const tf2::Duration& duration
-
- 1232 if (this->tf_buffer_ ==
nullptr || this->tf_listener_ ==
nullptr) {
-
-
-
- 1236 return this->tf_buffer_->lookupTransform(reference_frame, frame, time_point, duration);;
- 1237 }
catch (
const tf2::TransformException& ex) {
- 1238 throw exceptions::LookupTransformException(std::string(
"Failed to lookup transform: ").append(ex.what()));
-
-
-
- 1242template<
class NodeT>
-
- 1244 const std::string& frame,
const std::string& reference_frame,
const tf2::TimePoint& time_point,
- 1245 const tf2::Duration& duration
-
- 1247 auto transform = this->lookup_ros_transform(frame, reference_frame, time_point, duration);
- 1248 state_representation::CartesianPose result(frame, reference_frame);
-
-
-
-
- 1253template<
class NodeT>
-
- 1255 const std::string& frame,
const std::string& reference_frame,
double validity_period,
const tf2::Duration& duration
-
-
- 1258 this->lookup_ros_transform(frame, reference_frame, tf2::TimePoint(std::chrono::microseconds(0)), duration);
- 1259 if (validity_period > 0.0 && (this->get_clock()->now() - transform.header.stamp).seconds() > validity_period) {
-
-
- 1262 state_representation::CartesianPose result(frame, reference_frame);
-
-
-
-
- 1267template<
class NodeT>
-
- 1269 modulo_component_interfaces::msg::Predicate message;
- 1270 message.component = this->get_node_base_interface()->get_fully_qualified_name();
- 1271 message.predicate = name;
-
- 1273 this->predicate_publisher_->publish(message);
-
-
- 1276template<
class NodeT>
-
- 1278 for (
const auto& predicate : this->predicates_) {
-
-
-
-
- 1283template<
class NodeT>
-
- 1285 auto parsed_signal_name = utilities::parse_topic_name(signal_name);
- 1286 if (this->
outputs_.find(parsed_signal_name) == this->outputs_.cend()) {
-
+ 1007 case MessageType::FLOAT64_MULTI_ARRAY: {
+ 1008 auto subscription_handler =
+ 1009 std::make_shared<SubscriptionHandler<std_msgs::msg::Float64MultiArray>>(message_pair);
+ 1010 auto subscription = NodeT::template create_subscription<std_msgs::msg::Float64MultiArray>(
+ 1011 topic_name, this->
qos_, subscription_handler->get_callback(user_callback));
+ 1012 subscription_interface = subscription_handler->create_subscription_interface(subscription);
+
+
+ 1015 case MessageType::INT32: {
+ 1016 auto subscription_handler = std::make_shared<SubscriptionHandler<std_msgs::msg::Int32>>(message_pair);
+ 1017 auto subscription = NodeT::template create_subscription<std_msgs::msg::Int32>(
+ 1018 topic_name, this->
qos_, subscription_handler->get_callback(user_callback));
+ 1019 subscription_interface = subscription_handler->create_subscription_interface(subscription);
+
+
+ 1022 case MessageType::STRING: {
+ 1023 auto subscription_handler = std::make_shared<SubscriptionHandler<std_msgs::msg::String>>(message_pair);
+ 1024 auto subscription = NodeT::template create_subscription<std_msgs::msg::String>(
+ 1025 topic_name, this->
qos_, subscription_handler->get_callback(user_callback));
+ 1026 subscription_interface = subscription_handler->create_subscription_interface(subscription);
+
+
+ 1029 case MessageType::ENCODED_STATE: {
+ 1030 auto subscription_handler = std::make_shared<SubscriptionHandler<modulo_core::EncodedState>>(message_pair);
+ 1031 auto subscription = NodeT::template create_subscription<modulo_core::EncodedState>(
+ 1032 topic_name, this->
qos_, subscription_handler->get_callback(user_callback));
+ 1033 subscription_interface = subscription_handler->create_subscription_interface(subscription);
+
+
+
+ 1037 this->
inputs_.insert_or_assign(parsed_signal_name, subscription_interface);
+ 1038 }
catch (
const std::exception& ex) {
+ 1039 RCLCPP_ERROR_STREAM(this->get_logger(),
"Failed to add input '" << signal_name <<
"': " << ex.what());
+
+
+
+ 1043template<
class NodeT>
+ 1044template<
typename MsgT>
+
+ 1046 const std::string& signal_name,
const std::function<
void(
const std::shared_ptr<MsgT>)>& callback,
+ 1047 const std::string& default_topic,
bool fixed_topic
+
+
+
+ 1051 std::string parsed_signal_name = utilities::parse_topic_name(signal_name);
+ 1052 this->
declare_input(parsed_signal_name, default_topic, fixed_topic);
+ 1053 auto topic_name = this->get_parameter_value<std::string>(parsed_signal_name +
"_topic");
+ 1054 RCLCPP_DEBUG_STREAM(this->get_logger(),
+ 1055 "Adding input '" << parsed_signal_name <<
"' with topic name '" << topic_name <<
"'.");
+ 1056 auto subscription = NodeT::template create_subscription<MsgT>(topic_name, this->
qos_, callback);
+ 1057 auto subscription_interface =
+ 1058 std::make_shared<SubscriptionHandler<MsgT>>()->create_subscription_interface(subscription);
+ 1059 this->
inputs_.insert_or_assign(parsed_signal_name, subscription_interface);
+ 1060 }
catch (
const std::exception& ex) {
+ 1061 RCLCPP_ERROR_STREAM(this->get_logger(),
"Failed to add input '" << signal_name <<
"': " << ex.what());
+
+
+
+ 1065template<
class NodeT>
+
+ 1067 std::string parsed_service_name = utilities::parse_topic_name(service_name);
+ 1068 if (parsed_service_name.empty()) {
+
+ 1070 "The parsed service name for service '" + service_name
+ 1071 +
"' is empty. Provide a string with valid characters for the signal name ([a-zA-Z0-9_]).");
+
+ 1073 if (this->empty_services_.find(parsed_service_name) != this->empty_services_.cend()
+ 1074 || this->string_services_.find(parsed_service_name) != this->string_services_.cend()) {
+ 1075 throw exceptions::AddServiceException(
"Service with name '" + parsed_service_name +
"' already exists.");
+
+ 1077 return parsed_service_name;
+
+
+ 1080template<
class NodeT>
+
+
+
+
+ 1085 std::string parsed_service_name = this->validate_service_name(service_name);
+ 1086 RCLCPP_DEBUG_STREAM(this->get_logger(),
"Adding empty service '" << parsed_service_name <<
"'.");
+ 1087 auto service = NodeT::template create_service<modulo_component_interfaces::srv::EmptyTrigger>(
+ 1088 "~/" + parsed_service_name, [callback](
+ 1089 const std::shared_ptr<modulo_component_interfaces::srv::EmptyTrigger::Request>,
+ 1090 std::shared_ptr<modulo_component_interfaces::srv::EmptyTrigger::Response> response
+
+
+ 1093 auto callback_response = callback();
+ 1094 response->success = callback_response.success;
+ 1095 response->message = callback_response.message;
+ 1096 }
catch (
const std::exception& ex) {
+ 1097 response->success =
false;
+ 1098 response->message = ex.what();
+
+
+ 1101 this->empty_services_.insert_or_assign(parsed_service_name, service);
+ 1102 }
catch (
const std::exception& ex) {
+ 1103 RCLCPP_ERROR_STREAM(this->get_logger(),
"Failed to add service '" << service_name <<
"': " << ex.what());
+
+
+
+ 1107template<
class NodeT>
+
+ 1109 const std::string& service_name,
const std::function<
ComponentServiceResponse(
const std::string&
string)>& callback
+
+
+ 1112 std::string parsed_service_name = this->validate_service_name(service_name);
+ 1113 RCLCPP_DEBUG_STREAM(this->get_logger(),
"Adding string service '" << parsed_service_name <<
"'.");
+ 1114 auto service = NodeT::template create_service<modulo_component_interfaces::srv::StringTrigger>(
+ 1115 "~/" + parsed_service_name, [callback](
+ 1116 const std::shared_ptr<modulo_component_interfaces::srv::StringTrigger::Request> request,
+ 1117 std::shared_ptr<modulo_component_interfaces::srv::StringTrigger::Response> response
+
+
+ 1120 auto callback_response = callback(request->payload);
+ 1121 response->success = callback_response.success;
+ 1122 response->message = callback_response.message;
+ 1123 }
catch (
const std::exception& ex) {
+ 1124 response->success =
false;
+ 1125 response->message = ex.what();
+
+
+ 1128 this->string_services_.insert_or_assign(parsed_service_name, service);
+ 1129 }
catch (
const std::exception& ex) {
+ 1130 RCLCPP_ERROR_STREAM(this->get_logger(),
"Failed to add service '" << service_name <<
"': " << ex.what());
+
+
+
+ 1134template<
class NodeT>
+
+
+
+ 1138 RCLCPP_ERROR(this->get_logger(),
"Failed to add periodic function: Provide a non empty string as a name.");
+
+
+ 1141 if (this->periodic_callbacks_.find(name) != this->periodic_callbacks_.end()) {
+ 1142 RCLCPP_WARN_STREAM(this->get_logger(),
"Periodic function '" << name <<
"' already exists, overwriting.");
+
+ 1144 RCLCPP_DEBUG_STREAM(this->get_logger(),
"Adding periodic function '" << name <<
"'.");
+
+ 1146 this->periodic_callbacks_.template insert_or_assign(name, callback);
+
+
+ 1149template<
class NodeT>
+
+ 1151 if (this->tf_broadcaster_ ==
nullptr) {
+ 1152 RCLCPP_DEBUG(this->get_logger(),
"Adding TF broadcaster.");
+ 1153 console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE);
+ 1154 this->tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(*
this);
+
+ 1156 RCLCPP_DEBUG(this->get_logger(),
"TF broadcaster already exists.");
+
+
+
+ 1160template<
class NodeT>
+
+ 1162 if (this->static_tf_broadcaster_ ==
nullptr) {
+ 1163 RCLCPP_DEBUG(this->get_logger(),
"Adding static TF broadcaster.");
+ 1164 console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE);
+ 1165 tf2_ros::StaticBroadcasterQoS qos;
+ 1166 rclcpp::PublisherOptionsWithAllocator<std::allocator<void>> options;
+ 1167 this->static_tf_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(*
this, qos, options);
+
+ 1169 RCLCPP_DEBUG(this->get_logger(),
"Static TF broadcaster already exists.");
+
+
+
+ 1173template<
class NodeT>
+
+ 1175 if (this->tf_buffer_ ==
nullptr || this->tf_listener_ ==
nullptr) {
+ 1176 RCLCPP_DEBUG(this->get_logger(),
"Adding TF buffer and listener.");
+ 1177 console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE);
+ 1178 this->tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
+ 1179 this->tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*this->tf_buffer_);
+
+ 1181 RCLCPP_DEBUG(this->get_logger(),
"TF buffer and listener already exist.");
+
+
+
+ 1185template<
class NodeT>
+
+ 1187 this->
send_transforms(std::vector<state_representation::CartesianPose>{transform});
+
+
+ 1190template<
class NodeT>
+
+
+
+
+
+ 1196template<
class NodeT>
+
+
+
+
+ 1201template<
class NodeT>
+
+
+
+
+
+ 1207template<
class NodeT>
+
+
+ 1210 const std::vector<state_representation::CartesianPose>& transforms,
const std::shared_ptr<T>& tf_broadcaster,
+
+
+ 1213 std::string modifier = is_static ?
"static " :
"";
+ 1214 if (tf_broadcaster ==
nullptr) {
+ 1215 RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
+ 1216 "Failed to send " << modifier <<
"transform: No " << modifier
+ 1217 <<
"TF broadcaster configured.");
+
+
+
+ 1221 std::vector<geometry_msgs::msg::TransformStamped> transform_messages;
+ 1222 transform_messages.reserve(transforms.size());
+ 1223 for (
const auto& tf : transforms) {
+ 1224 geometry_msgs::msg::TransformStamped transform_message;
+
+ 1226 transform_messages.emplace_back(transform_message);
+
+ 1228 tf_broadcaster->sendTransform(transform_messages);
+ 1229 }
catch (
const std::exception& ex) {
+ 1230 RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
+ 1231 "Failed to send " << modifier <<
"transform: " << ex.what());
+
+
+
+ 1235template<
class NodeT>
+
+ 1237 const std::string& frame,
const std::string& reference_frame,
const tf2::TimePoint& time_point,
+ 1238 const tf2::Duration& duration
+
+ 1240 if (this->tf_buffer_ ==
nullptr || this->tf_listener_ ==
nullptr) {
+
+
+
+ 1244 return this->tf_buffer_->lookupTransform(reference_frame, frame, time_point, duration);;
+ 1245 }
catch (
const tf2::TransformException& ex) {
+ 1246 throw exceptions::LookupTransformException(std::string(
"Failed to lookup transform: ").append(ex.what()));
+
+
+
+ 1250template<
class NodeT>
+
+ 1252 const std::string& frame,
const std::string& reference_frame,
const tf2::TimePoint& time_point,
+ 1253 const tf2::Duration& duration
+
+ 1255 auto transform = this->lookup_ros_transform(frame, reference_frame, time_point, duration);
+ 1256 state_representation::CartesianPose result(frame, reference_frame);
+
+
+
+
+ 1261template<
class NodeT>
+
+ 1263 const std::string& frame,
const std::string& reference_frame,
double validity_period,
const tf2::Duration& duration
+
+
+ 1266 this->lookup_ros_transform(frame, reference_frame, tf2::TimePoint(std::chrono::microseconds(0)), duration);
+ 1267 if (validity_period > 0.0 && (this->get_clock()->now() - transform.header.stamp).seconds() > validity_period) {
+
+
+ 1270 state_representation::CartesianPose result(frame, reference_frame);
+
+
+
+
+ 1275template<
class NodeT>
+
+ 1277 modulo_component_interfaces::msg::Predicate message;
+ 1278 message.component = this->get_node_base_interface()->get_fully_qualified_name();
+ 1279 message.predicate = name;
+
+ 1281 this->predicate_publisher_->publish(message);
+
+
+ 1284template<
class NodeT>
+
+ 1286 for (
const auto& predicate : this->predicates_) {
+
-
-
-
-
- 1293 this->
outputs_.at(parsed_signal_name)->publish();
-
- 1295 RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
- 1296 "Failed to publish output '" << parsed_signal_name <<
"': " << ex.what());
-
-
-
- 1300template<
class NodeT>
-
- 1302 for (
const auto& [signal, publisher] : this->
outputs_) {
-
-
- 1305 publisher->publish();
-
-
- 1308 RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
- 1309 "Failed to publish output '" << signal <<
"': " << ex.what());
-
-
-
-
- 1314template<
class NodeT>
-
- 1316 for (
const auto& [name, callback] : this->periodic_callbacks_) {
-
-
- 1319 }
catch (
const std::exception& ex) {
- 1320 RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
- 1321 "Failed to evaluate periodic function callback '" << name <<
"': " << ex.what());
-
-
-
-
- 1326template<
class NodeT>
- 1327template<
typename DataT>
-
- 1329 const std::string& signal_name,
const std::shared_ptr<DataT>& data,
const std::string& default_topic,
- 1330 bool fixed_topic,
bool publish_on_step
-
-
-
- 1334 auto parsed_signal_name = utilities::parse_topic_name(signal_name);
- 1335 if (data ==
nullptr) {
-
- 1337 "Invalid data pointer for output '" + parsed_signal_name +
"'.");
-
- 1339 this->
declare_output(parsed_signal_name, default_topic, fixed_topic);
- 1340 RCLCPP_DEBUG_STREAM(this->get_logger(),
- 1341 "Creating output '" << parsed_signal_name <<
"' (provided signal name was '" << signal_name
-
- 1343 auto message_pair = make_shared_message_pair(data, this->get_clock());
-
- 1345 parsed_signal_name, std::make_shared<PublisherInterface>(this->publisher_type_, message_pair));
-
- 1347 return parsed_signal_name;
-
-
- 1350 }
catch (
const std::exception& ex) {
-
-
-
-
- 1355template<
class NodeT>
-
- 1357 RCLCPP_DEBUG(this->get_logger(),
"raise_error called: Setting predicate 'in_error_state' to true.");
-
-
-
- 1361template<
class NodeT>
-
-
-
-
- 1366template<
class NodeT>
-
-
-
-
+
+
+ 1291template<
class NodeT>
+
+ 1293 auto parsed_signal_name = utilities::parse_topic_name(signal_name);
+ 1294 if (this->
outputs_.find(parsed_signal_name) == this->outputs_.cend()) {
+
+
+
+
+
+
+ 1301 this->
outputs_.at(parsed_signal_name)->publish();
+
+ 1303 RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
+ 1304 "Failed to publish output '" << parsed_signal_name <<
"': " << ex.what());
+
+
+
+ 1308template<
class NodeT>
+
+ 1310 for (
const auto& [signal, publisher] : this->
outputs_) {
+
+
+ 1313 publisher->publish();
+
+
+ 1316 RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
+ 1317 "Failed to publish output '" << signal <<
"': " << ex.what());
+
+
+
+
+ 1322template<
class NodeT>
+
+ 1324 for (
const auto& [name, callback] : this->periodic_callbacks_) {
+
+
+ 1327 }
catch (
const std::exception& ex) {
+ 1328 RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
+ 1329 "Failed to evaluate periodic function callback '" << name <<
"': " << ex.what());
+
+
+
+
+ 1334template<
class NodeT>
+ 1335template<
typename DataT>
+
+ 1337 const std::string& signal_name,
const std::shared_ptr<DataT>& data,
const std::string& default_topic,
+ 1338 bool fixed_topic,
bool publish_on_step
+
+
+
+ 1342 auto parsed_signal_name = utilities::parse_topic_name(signal_name);
+ 1343 if (data ==
nullptr) {
+
+ 1345 "Invalid data pointer for output '" + parsed_signal_name +
"'.");
+
+ 1347 this->
declare_output(parsed_signal_name, default_topic, fixed_topic);
+ 1348 RCLCPP_DEBUG_STREAM(this->get_logger(),
+ 1349 "Creating output '" << parsed_signal_name <<
"' (provided signal name was '" << signal_name
+
+ 1351 auto message_pair = make_shared_message_pair(data, this->get_clock());
+
+ 1353 parsed_signal_name, std::make_shared<PublisherInterface>(this->publisher_type_, message_pair));
+
+ 1355 return parsed_signal_name;
+
+
+ 1358 }
catch (
const std::exception& ex) {
+
+
+
+
+ 1363template<
class NodeT>
+
+ 1365 RCLCPP_DEBUG(this->get_logger(),
"raise_error called: Setting predicate 'in_error_state' to true.");
+
+
+
+ 1369template<
class NodeT>
+
+
+
+
+ 1374template<
class NodeT>
+
+
+
+
Base interface class for modulo components to wrap a ROS Node with custom behaviour.
void add_parameter(const std::shared_ptr< state_representation::ParameterInterface > ¶meter, const std::string &description, bool read_only=false)
Add a parameter.
virtual void on_step_callback()
Steps to execute periodically. To be redefined by derived Component classes.
std::string create_output(const std::string &signal_name, const std::shared_ptr< DataT > &data, const std::string &default_topic, bool fixed_topic, bool publish_on_step)
Helper function to parse the signal name and add an unconfigured PublisherInterface to the map of out...
T get_parameter_value(const std::string &name) const
Get a parameter value by name.
virtual void raise_error()
Put the component in error state by setting the 'in_error_state' predicate to true.
void add_predicate(const std::string &predicate_name, bool predicate_value)
Add a predicate to the map of predicates.
virtual void step()
Step function that is called periodically.
void add_periodic_callback(const std::string &name, const std::function< void(void)> &callback)
Add a periodic callback function.
void send_static_transforms(const std::vector< state_representation::CartesianPose > &transforms)
Send a vector of static transforms to TF.
void publish_outputs()
Helper function to publish all output signals.
void remove_output(const std::string &signal_name)
Remove an output from the map of outputs.
void add_service(const std::string &service_name, const std::function< ComponentServiceResponse(void)> &callback)
Add a service to trigger a callback function with no input arguments.
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 publish_predicates()
Helper function to publish all predicates.
void add_trigger(const std::string &trigger_name)
Add a trigger to the component. Triggers are predicates that are always false except when it's trigge...
void trigger(const std::string &trigger_name)
Latch the trigger with the provided name.
void declare_input(const std::string &signal_name, const std::string &default_topic="", bool fixed_topic=false)
Declare an input to create the topic parameter without adding it to the map of inputs yet.
bool get_predicate(const std::string &predicate_name)
Get the logical value of a predicate.
void add_parameter(const std::shared_ptr< state_representation::ParameterInterface > ¶meter, const std::string &description, bool read_only=false)
Add a parameter.
virtual void on_step_callback()
Steps to execute periodically. To be redefined by derived Component classes.
std::string create_output(const std::string &signal_name, const std::shared_ptr< DataT > &data, const std::string &default_topic, bool fixed_topic, bool publish_on_step)
Helper function to parse the signal name and add an unconfigured PublisherInterface to the map of out...
T get_parameter_value(const std::string &name) const
Get a parameter value by name.
virtual void raise_error()
Put the component in error state by setting the 'in_error_state' predicate to true.
void add_predicate(const std::string &predicate_name, bool predicate_value)
Add a predicate to the map of predicates.
virtual void step()
Step function that is called periodically.
void add_periodic_callback(const std::string &name, const std::function< void(void)> &callback)
Add a periodic callback function.
void send_static_transforms(const std::vector< state_representation::CartesianPose > &transforms)
Send a vector of static transforms to TF.
void publish_outputs()
Helper function to publish all output signals.
void remove_output(const std::string &signal_name)
Remove an output from the map of outputs.
void add_service(const std::string &service_name, const std::function< ComponentServiceResponse(void)> &callback)
Add a service to trigger a callback function with no input arguments.
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 publish_predicates()
Helper function to publish all predicates.
void add_trigger(const std::string &trigger_name)
Add a trigger to the component. Triggers are predicates that are always false except when it's trigge...
void trigger(const std::string &trigger_name)
Latch the trigger with the provided name.
void declare_input(const std::string &signal_name, const std::string &default_topic="", bool fixed_topic=false)
Declare an input to create the topic parameter without adding it to the map of inputs yet.
bool get_predicate(const std::string &predicate_name)
Get the logical value of a predicate.
rclcpp::QoS qos_
Quality of Service for ROS publishers and subscribers.
void publish_output(const std::string &signal_name)
Trigger the publishing of an output.
void evaluate_periodic_callbacks()
Helper function to evaluate all periodic function callbacks.
void add_input(const std::string &signal_name, const std::shared_ptr< DataT > &data, const std::string &default_topic="", bool fixed_topic=false)
Add and configure an input signal of the component.
void publish_output(const std::string &signal_name)
Trigger the publishing of an output.
void evaluate_periodic_callbacks()
Helper function to evaluate all periodic function callbacks.
void add_input(const std::string &signal_name, const std::shared_ptr< DataT > &data, const std::string &default_topic="", bool fixed_topic=false)
Add and configure an input signal of the component.
std::map< std::string, std::shared_ptr< modulo_core::communication::PublisherInterface > > outputs_
Map of outputs.
rclcpp::QoS get_qos() const
Getter of the Quality of Service attribute.
void send_transform(const state_representation::CartesianPose &transform)
Send a transform to TF.
void remove_input(const std::string &signal_name)
Remove an input from the map of inputs.
void add_tf_broadcaster()
Configure a transform broadcaster.
void send_transforms(const std::vector< state_representation::CartesianPose > &transforms)
Send a vector of transforms to TF.
void add_static_tf_broadcaster()
Configure a static transform broadcaster.
void send_static_transform(const state_representation::CartesianPose &transform)
Send a static transform to TF.
void add_tf_listener()
Configure a transform buffer and listener.
std::shared_ptr< state_representation::ParameterInterface > get_parameter(const std::string &name) const
Get a parameter by name.
rclcpp::QoS get_qos() const
Getter of the Quality of Service attribute.
void send_transform(const state_representation::CartesianPose &transform)
Send a transform to TF.
void remove_input(const std::string &signal_name)
Remove an input from the map of inputs.
void add_tf_broadcaster()
Configure a transform broadcaster.
void send_transforms(const std::vector< state_representation::CartesianPose > &transforms)
Send a vector of transforms to TF.
void add_static_tf_broadcaster()
Configure a static transform broadcaster.
void send_static_transform(const state_representation::CartesianPose &transform)
Send a static transform to TF.
void add_tf_listener()
Configure a transform buffer and listener.
std::shared_ptr< state_representation::ParameterInterface > get_parameter(const std::string &name) const
Get a parameter by name.
std::map< std::string, bool > periodic_outputs_
Map of outputs with periodic publishing flag.
void set_parameter_value(const std::string &name, const T &value)
Set the value of a parameter.
void declare_output(const std::string &signal_name, const std::string &default_topic="", bool fixed_topic=false)
Declare an output to create the topic parameter without adding it to the map of outputs yet.
void publish_transforms(const std::vector< state_representation::CartesianPose > &transforms, const std::shared_ptr< T > &tf_broadcaster, bool is_static=false)
Helper function to send a vector of transforms through a transform broadcaster.
void set_qos(const rclcpp::QoS &qos)
Set the Quality of Service for ROS publishers and subscribers.
virtual bool on_validate_parameter_callback(const std::shared_ptr< state_representation::ParameterInterface > ¶meter)
Parameter validation function to be redefined by derived Component classes.
void set_parameter_value(const std::string &name, const T &value)
Set the value of a parameter.
void declare_output(const std::string &signal_name, const std::string &default_topic="", bool fixed_topic=false)
Declare an output to create the topic parameter without adding it to the map of outputs yet.
void publish_transforms(const std::vector< state_representation::CartesianPose > &transforms, const std::shared_ptr< T > &tf_broadcaster, bool is_static=false)
Helper function to send a vector of transforms through a transform broadcaster.
void set_qos(const rclcpp::QoS &qos)
Set the Quality of Service for ROS publishers and subscribers.
virtual bool on_validate_parameter_callback(const std::shared_ptr< state_representation::ParameterInterface > ¶meter)
Parameter validation function to be redefined by derived Component classes.
std::map< std::string, std::shared_ptr< modulo_core::communication::SubscriptionInterface > > inputs_
Map of inputs.
void publish_predicate(const std::string &name)
Helper function to publish a predicate.
state_representation::CartesianPose lookup_transform(const std::string &frame, const std::string &reference_frame, const tf2::TimePoint &time_point, const tf2::Duration &duration)
Look up a transform from TF.
void publish_predicate(const std::string &name)
Helper function to publish a predicate.
state_representation::CartesianPose lookup_transform(const std::string &frame, const std::string &reference_frame, const tf2::TimePoint &time_point, const tf2::Duration &duration)
Look up a transform from TF.
Friend class to the ComponentInterface to allow test fixtures to access protected and private members...
An exception class to notify errors when adding a service.
An exception class to notify errors when adding a signal.