|
Modulo 5.0.0
|
Base interface class for modulo components to wrap a ROS Node with custom behaviour. More...
#include <ComponentInterface.hpp>
Public Member Functions | |
| virtual | ~ComponentInterface () |
| Virtual destructor. | |
| template<> | |
| double | get_period () const |
| template<> | |
| std::chrono::nanoseconds | get_period () const |
| template<> | |
| rclcpp::Duration | get_period () const |
Protected Member Functions | |
| ComponentInterface (const std::shared_ptr< rclcpp::node_interfaces::NodeInterfaces< ALL_RCLCPP_NODE_INTERFACES > > &interfaces) | |
| Constructor with all node interfaces. | |
| double | get_rate () const |
| Get the component rate in Hertz. | |
| template<typename T > | |
| T | get_period () const |
| Get the component period. | |
| virtual void | step () |
| Step function that is called periodically. | |
| virtual void | on_step_callback () |
| Steps to execute periodically. To be redefined by derived Component classes. | |
| void | add_parameter (const std::shared_ptr< state_representation::ParameterInterface > ¶meter, const std::string &description, bool read_only=false) |
| Add a parameter. | |
| template<typename T > | |
| void | add_parameter (const std::string &name, const T &value, const std::string &description, bool read_only=false) |
| Add a parameter. | |
| std::shared_ptr< state_representation::ParameterInterface > | get_parameter (const std::string &name) const |
| Get a parameter by name. | |
| template<typename T > | |
| T | get_parameter_value (const std::string &name) const |
| Get a parameter value by name. | |
| template<typename T > | |
| void | set_parameter_value (const std::string &name, const T &value) |
| Set the value of a parameter. | |
| virtual bool | on_validate_parameter_callback (const std::shared_ptr< state_representation::ParameterInterface > ¶meter) |
| Parameter validation function to be redefined by derived Component classes. | |
| template<typename T > | |
| void | add_assignment (const std::string &assignment_name) |
| Add an assignment to the map of assignments. | |
| template<typename T > | |
| void | set_assignment (const std::string &assignment_name, const T &assignment_value) |
| Set the value of an assignment. | |
| template<typename T > | |
| T | get_assignment (const std::string &assignment_name) const |
| Get the value of an assignment. | |
| void | add_predicate (const std::string &predicate_name, bool predicate_value) |
| Add a predicate to the map of predicates. | |
| void | add_predicate (const std::string &predicate_name, const std::function< bool(void)> &predicate_function) |
| Add a predicate to the map of predicates based on a function to periodically call. | |
| bool | get_predicate (const std::string &predicate_name) const |
| Get the logical value of a predicate. | |
| void | set_predicate (const std::string &predicate_name, bool predicate_value) |
| Set the value of the predicate given as parameter, if the predicate is not found does not do anything. | |
| void | set_predicate (const std::string &predicate_name, const std::function< bool(void)> &predicate_function) |
| Set the value of the predicate given as parameter, if the predicate is not found does not do anything. | |
| void | add_trigger (const std::string &trigger_name) |
| Add a trigger to the component. Triggers are predicates that are always false except when it's triggered in which case it is set back to false immediately after it is read. | |
| 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. | |
| 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. | |
| template<typename DataT > | |
| 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. | |
| template<typename DataT > | |
| void | add_input (const std::string &signal_name, const std::shared_ptr< DataT > &data, const std::function< void()> &callback, const std::string &default_topic="", bool fixed_topic=false) |
| Add and configure an input signal of the component. | |
| template<typename MsgT > | |
| void | add_input (const std::string &signal_name, const std::function< void(const std::shared_ptr< MsgT >)> &callback, const std::string &default_topic="", bool fixed_topic=false) |
| Add and configure an input signal of the component. | |
| template<typename DataT > | |
| std::string | create_output (modulo_core::communication::PublisherType publisher_type, 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 outputs. | |
| void | publish_output (const std::string &signal_name) |
| Trigger the publishing of an output. | |
| void | remove_input (const std::string &signal_name) |
| Remove an input from the map of inputs. | |
| 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 | add_service (const std::string &service_name, const std::function< ComponentServiceResponse(const std::string &string)> &callback) |
| Add a service to trigger a callback function with a string payload. | |
| void | add_periodic_callback (const std::string &name, const std::function< void(void)> &callback) |
| Add a periodic callback function. | |
| void | add_tf_broadcaster () |
| Configure a transform broadcaster. | |
| void | add_static_tf_broadcaster () |
| Configure a static transform broadcaster. | |
| void | add_tf_listener () |
| Configure a transform buffer and listener. | |
| void | send_transform (const state_representation::CartesianPose &transform) |
| Send a transform to TF. | |
| void | send_transforms (const std::vector< state_representation::CartesianPose > &transforms) |
| Send a vector of transforms to TF. | |
| void | send_static_transform (const state_representation::CartesianPose &transform) |
| Send a static transform to TF. | |
| void | send_static_transforms (const std::vector< state_representation::CartesianPose > &transforms) |
| Send a vector of static transforms to TF. | |
| 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. | |
| state_representation::CartesianPose | lookup_transform (const std::string &frame, const std::string &reference_frame="world", double validity_period=-1.0, const tf2::Duration &duration=tf2::Duration(std::chrono::microseconds(10))) |
| Look up a transform from TF. | |
| rclcpp::QoS | get_qos () const |
| Getter of the Quality of Service attribute. | |
| void | set_qos (const rclcpp::QoS &qos) |
| Set the Quality of Service for ROS publishers and subscribers. | |
| virtual void | raise_error () |
| Notify an error in the component. | |
| void | publish_predicates () |
| Helper function to publish all predicates. | |
| void | publish_outputs () |
| Helper function to publish all output signals. | |
| void | evaluate_periodic_callbacks () |
| Helper function to evaluate all periodic function callbacks. | |
| void | finalize_interfaces () |
| Finalize all interfaces. | |
Protected Attributes | |
| std::map< std::string, std::shared_ptr< modulo_core::communication::SubscriptionInterface > > | inputs_ |
| Map of inputs. | |
| std::map< std::string, std::shared_ptr< modulo_core::communication::PublisherInterface > > | outputs_ |
| Map of outputs. | |
| std::map< std::string, bool > | periodic_outputs_ |
| Map of outputs with periodic publishing flag. | |
Friends | |
| class | ComponentInterfacePublicInterface |
Base interface class for modulo components to wrap a ROS Node with custom behaviour.
This class is not intended for direct inheritance and usage by end-users. Instead, it defines the common interfaces for the derived classes modulo_components::Component and modulo_components::LifecycleComponent.
Definition at line 57 of file ComponentInterface.hpp.
|
virtual |
Virtual destructor.
Definition at line 52 of file ComponentInterface.cpp.
|
explicitprotected |
Constructor with all node interfaces.
| interfaces | Shared pointer to all the node interfaces of parent class |
Definition at line 13 of file ComponentInterface.cpp.
|
inlineprotected |
Add an assignment to the map of assignments.
| T | The type of the assignment |
| assignment_name | the name of the associated assignment |
Definition at line 857 of file ComponentInterface.hpp.
|
inlineprotected |
Add and configure an input signal of the component.
| MsgT | The ROS message type of the subscription |
| signal_name | Name of the input signal |
| callback | The callback to use for the subscription |
| default_topic | If set, the default value for the topic name to use |
| fixed_topic | If true, the topic name of the input signal is fixed |
Definition at line 760 of file ComponentInterface.hpp.
|
inlineprotected |
Add and configure an input signal of the component.
| DataT | Type of the data pointer |
| signal_name | Name of the input signal |
| data | Data to receive on the input signal |
| callback | Callback function to trigger after receiving the input signal |
| default_topic | If set, the default value for the topic name to use |
| fixed_topic | If true, the topic name of the input signal is fixed |
Definition at line 669 of file ComponentInterface.hpp.
|
inlineprotected |
Add and configure an input signal of the component.
| DataT | Type of the data pointer |
| signal_name | Name of the input signal |
| data | Data to receive on the input signal |
| default_topic | If set, the default value for the topic name to use |
| fixed_topic | If true, the topic name of the input signal is fixed |
Definition at line 662 of file ComponentInterface.hpp.
|
protected |
Add a parameter.
This method stores a pointer reference to an existing Parameter object in the local parameter map and declares the equivalent ROS parameter on the ROS interface.
| parameter | A ParameterInterface pointer to a Parameter instance |
| description | The description of the parameter |
| read_only | If true, the value of the parameter cannot be changed after declaration |
| ParameterException | if the parameter could not be added |
Definition at line 79 of file ComponentInterface.cpp.
|
inlineprotected |
Add a parameter.
This method creates a new Parameter object instance to reference in the local parameter map and declares the equivalent ROS parameter on the ROS interface.
| T | The type of the parameter |
| name | The name of the parameter |
| value | The value of the parameter |
| description | The description of the parameter |
| read_only | If true, the value of the parameter cannot be changed after declaration |
| ParameterException | if the parameter could not be added |
Definition at line 622 of file ComponentInterface.hpp.
|
protected |
Add a periodic callback function.
The provided function is evaluated periodically at the component step period.
| name | The name of the callback |
| callback | The callback function that is evaluated periodically |
Definition at line 450 of file ComponentInterface.cpp.
|
protected |
Add a predicate to the map of predicates.
| predicate_name | the name of the associated predicate |
| predicate_value | the boolean value of the predicate |
Definition at line 193 of file ComponentInterface.cpp.
|
protected |
Add a predicate to the map of predicates based on a function to periodically call.
| predicate_name | the name of the associated predicate |
| predicate_function | the function to call that returns the value of the predicate |
Definition at line 197 of file ComponentInterface.cpp.
|
protected |
Add a service to trigger a callback function with a string payload.
The string payload can have an arbitrary format to parameterize and control the callback behaviour as desired. It is the responsibility of the service callback to parse the string according to some payload format. When adding a service with a string payload, be sure to document the payload format appropriately.
| service_name | The name of the service |
| callback | A service callback function with a string argument that returns a ComponentServiceResponse |
Definition at line 397 of file ComponentInterface.cpp.
|
protected |
Add a service to trigger a callback function with no input arguments.
| service_name | The name of the service |
| callback | A service callback function with no arguments that returns a ComponentServiceResponse |
Definition at line 370 of file ComponentInterface.cpp.
|
protected |
Configure a static transform broadcaster.
Definition at line 476 of file ComponentInterface.cpp.
|
protected |
Configure a transform broadcaster.
Definition at line 465 of file ComponentInterface.cpp.
|
protected |
Configure a transform buffer and listener.
Definition at line 489 of file ComponentInterface.cpp.
|
protected |
Add a trigger to the component. Triggers are predicates that are always false except when it's triggered in which case it is set back to false immediately after it is read.
| trigger_name | The name of the trigger |
Definition at line 256 of file ComponentInterface.cpp.
|
inlineprotected |
Helper function to parse the signal name and add an unconfigured PublisherInterface to the map of outputs.
| DataT | Type of the data pointer |
| signal_name | Name of the output signal |
| data | Data to transmit on the output signal |
| default_topic | If set, the default value for the topic name to use |
| fixed_topic | If true, the topic name of the output signal is fixed |
| publish_on_step | If true, the output is published periodically on step |
| modulo_core::exceptions::AddSignalException | if the output could not be created (empty name, already registered) |
Definition at line 792 of file ComponentInterface.hpp.
|
protected |
Declare an input to create the topic parameter without adding it to the map of inputs yet.
| signal_name | The signal name of the input |
| default_topic | If set, the default value for the topic name to use |
| fixed_topic | If true, the topic name of the signal is fixed |
| modulo_core::exceptions::AddSignalException | if the input could not be declared (empty name or already created) |
Definition at line 289 of file ComponentInterface.cpp.
|
protected |
Declare an output to create the topic parameter without adding it to the map of outputs yet.
| signal_name | The signal name of the output |
| default_topic | If set, the default value for the topic name to use |
| fixed_topic | If true, the topic name of the signal is fixed |
| modulo_core::exceptions::AddSignalException | if the output could not be declared (empty name or already created) |
Definition at line 294 of file ComponentInterface.cpp.
|
protected |
Helper function to evaluate all periodic function callbacks.
Definition at line 604 of file ComponentInterface.cpp.
|
protected |
Finalize all interfaces.
Definition at line 616 of file ComponentInterface.cpp.
|
protected |
Get the value of an assignment.
| T | The type of the assignment |
| assignment_name | The name of the assignment to get |
| modulo_core::exceptions::InvalidAssignmentException | if the assignment does not exist or the type does not match |
| state_representation::exceptions::EmptyStateException | if the assignment has not been set yet |
Definition at line 914 of file ComponentInterface.hpp.
|
nodiscardprotected |
Get a parameter by name.
| name | The name of the parameter |
| modulo_core::exceptions::ParameterException | if the parameter could not be found |
Definition at line 126 of file ComponentInterface.cpp.
|
inlinenodiscardprotected |
Get a parameter value by name.
| T | The type of the parameter |
| name | The name of the parameter |
| modulo_core::exceptions::ParameterException | if the parameter value could not be accessed |
Definition at line 632 of file ComponentInterface.hpp.
|
protected |
Get the component period.
| double modulo_components::ComponentInterface::get_period | ( | ) | const |
Definition at line 61 of file ComponentInterface.cpp.
| std::chrono::nanoseconds modulo_components::ComponentInterface::get_period | ( | ) | const |
Definition at line 66 of file ComponentInterface.cpp.
| rclcpp::Duration modulo_components::ComponentInterface::get_period | ( | ) | const |
Definition at line 71 of file ComponentInterface.cpp.
|
nodiscardprotected |
Get the logical value of a predicate.
If the predicate is not found or the callable function fails, the return value is false.
| predicate_name | the name of the predicate to retrieve from the map of predicates |
Definition at line 219 of file ComponentInterface.cpp.
|
nodiscardprotected |
Getter of the Quality of Service attribute.
Definition at line 553 of file ComponentInterface.cpp.
|
protected |
Get the component rate in Hertz.
Definition at line 56 of file ComponentInterface.cpp.
|
nodiscardprotected |
Look up a transform from TF.
| frame | The desired frame of the transform |
| reference_frame | The desired reference frame of the transform |
| time_point | The time at which the value of the transform is desired |
| duration | How long to block the lookup call before failing |
| modulo_core::exceptions::LookupTransformException | if TF buffer/listener are unconfigured or if the lookupTransform call failed |
Definition at line 517 of file ComponentInterface.cpp.
|
nodiscardprotected |
Look up a transform from TF.
| frame | The desired frame of the transform |
| reference_frame | The desired reference frame of the transform |
| validity_period | The validity period of the latest transform from the time of lookup in seconds |
| duration | How long to block the lookup call before failing |
| modulo_core::exceptions::LookupTransformException | if TF buffer/listener are unconfigured, if the lookupTransform call failed, or if the transform is too old |
Definition at line 526 of file ComponentInterface.cpp.
|
protectedvirtual |
Steps to execute periodically. To be redefined by derived Component classes.
Definition at line 77 of file ComponentInterface.cpp.
|
protectedvirtual |
Parameter validation function to be redefined by derived Component classes.
This method is automatically invoked whenever the ROS interface tried to modify a parameter. Validation and sanitization can be performed by reading or writing the value of the parameter through the ParameterInterface pointer, depending on the parameter name and desired component behaviour. If the validation returns true, the updated parameter value (including any modifications) is applied. If the validation returns false, any changes to the parameter are discarded and the parameter value is not changed.
| parameter | A ParameterInterface pointer to a Parameter instance |
Definition at line 188 of file ComponentInterface.cpp.
|
protected |
Trigger the publishing of an output.
| signal_name | The name of the output signal |
| ComponentException | if the output is being published periodically or if the signal name could not be found |
Definition at line 331 of file ComponentInterface.cpp.
|
protected |
Helper function to publish all output signals.
Definition at line 590 of file ComponentInterface.cpp.
|
protected |
Helper function to publish all predicates.
Definition at line 578 of file ComponentInterface.cpp.
|
protectedvirtual |
Notify an error in the component.
Reimplemented in modulo_components::Component, and modulo_components::LifecycleComponent.
Definition at line 561 of file ComponentInterface.cpp.
|
protected |
Remove an input from the map of inputs.
| signal_name | The name of the input |
Definition at line 348 of file ComponentInterface.cpp.
|
protected |
Remove an output from the map of outputs.
| signal_name | The name of the output |
Definition at line 359 of file ComponentInterface.cpp.
|
protected |
Send a static transform to TF.
| transform | The transform to send |
Definition at line 509 of file ComponentInterface.cpp.
|
protected |
Send a vector of static transforms to TF.
| transforms | The vector of transforms to send |
Definition at line 513 of file ComponentInterface.cpp.
|
protected |
Send a transform to TF.
| transform | The transform to send |
Definition at line 501 of file ComponentInterface.cpp.
|
protected |
Send a vector of transforms to TF.
| transforms | The vector of transforms to send |
Definition at line 505 of file ComponentInterface.cpp.
|
protected |
Set the value of an assignment.
| T | The type of the assignment |
| assignment_name | The name of the assignment to set |
| assignment_value | The value of the assignment |
Definition at line 889 of file ComponentInterface.hpp.
|
inlineprotected |
Set the value of a parameter.
The parameter must have been previously declared. This method preserves the reference to the original Parameter instance
| T | The type of the parameter |
| name | The name of the parameter |
Definition at line 642 of file ComponentInterface.hpp.
|
protected |
Set the value of the predicate given as parameter, if the predicate is not found does not do anything.
Even though the predicates are published periodically, the new value of this predicate will be published once immediately after setting it.
| predicate_name | the name of the predicate to retrieve from the map of predicates |
| predicate_value | the new value of the predicate |
Definition at line 237 of file ComponentInterface.cpp.
|
protected |
Set the value of the predicate given as parameter, if the predicate is not found does not do anything.
Even though the predicates are published periodically, the new value of this predicate will be published once immediately after setting it.
| predicate_name | the name of the predicate to retrieve from the map of predicates |
| predicate_function | the function to call that returns the value of the predicate |
Definition at line 241 of file ComponentInterface.cpp.
|
protected |
Set the Quality of Service for ROS publishers and subscribers.
| qos | The desired Quality of Service |
Definition at line 557 of file ComponentInterface.cpp.
|
protectedvirtual |
Step function that is called periodically.
Definition at line 75 of file ComponentInterface.cpp.
|
protected |
Latch the trigger with the provided name.
| trigger_name | The name of the trigger |
Definition at line 277 of file ComponentInterface.cpp.
|
friend |
Definition at line 59 of file ComponentInterface.hpp.
|
protected |
Map of inputs.
Definition at line 474 of file ComponentInterface.hpp.
|
protected |
Map of outputs.
Definition at line 475 of file ComponentInterface.hpp.
|
protected |
Map of outputs with periodic publishing flag.
Definition at line 476 of file ComponentInterface.hpp.