|
Modulo 5.0.0
|
Base controller class to combine ros2_control, control libraries and modulo. More...
#include <BaseControllerInterface.hpp>
Public Member Functions | |
| BaseControllerInterface () | |
| Default constructor. | |
| CallbackReturn | on_init () override |
| Declare parameters and register the on_set_parameters callback. | |
| CallbackReturn | on_configure (const rclcpp_lifecycle::State &previous_state) override |
| Add signals. | |
| template<> | |
| void | write_output (const std::string &name, const bool &data) |
| template<> | |
| void | write_output (const std::string &name, const double &data) |
| template<> | |
| void | write_output (const std::string &name, const std::vector< double > &data) |
| template<> | |
| void | write_output (const std::string &name, const int &data) |
| template<> | |
| void | write_output (const std::string &name, const std::string &data) |
Protected Member Functions | |
| 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. | |
| virtual bool | on_validate_parameter_callback (const std::shared_ptr< state_representation::ParameterInterface > ¶meter) |
| Parameter validation function to be redefined by derived controller classes. | |
| std::shared_ptr< state_representation::ParameterInterface > | get_parameter (const std::string &name) const |
| Get a parameter by name. | |
| template<typename T > | |
| T | get_parameter_value (const std::string &name) const |
| Get a parameter value by name. | |
| template<typename T > | |
| void | set_parameter_value (const std::string &name, const T &value) |
| Set the value of a parameter. | |
| 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 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 controller. | |
| void | trigger (const std::string &trigger_name) |
| Latch the trigger with the provided name. | |
| template<typename T > | |
| void | add_input (const std::string &name, const std::string &topic_name="") |
| Add an input to the controller. | |
| template<typename T > | |
| void | add_output (const std::string &name, const std::string &topic_name="") |
| Add an output to the controller. | |
| template<typename T > | |
| std::optional< T > | read_input (const std::string &name) |
| Read the most recent message of an input. | |
| template<typename T > | |
| void | write_output (const std::string &name, const T &data) |
| Write an object to an output. | |
| void | add_service (const std::string &service_name, const std::function< ControllerServiceResponse(void)> &callback) |
| Add a service to trigger a callback function with no input arguments. | |
| void | add_service (const std::string &service_name, const std::function< ControllerServiceResponse(const std::string &string)> &callback) |
| Add a service to trigger a callback function with a string payload. | |
| void | add_service_lockfree (const std::string &service_name, const std::function< ControllerServiceResponse(void)> &callback) |
| void | add_service_lockfree (const std::string &service_name, const std::function< ControllerServiceResponse(const std::string &string)> &callback) |
| void | add_tf_listener () |
| Configure a transform buffer and listener. | |
| 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. | |
| void | add_tf_broadcaster () |
| Configure a transform broadcaster. | |
| void | add_static_tf_broadcaster () |
| Configure a static transform broadcaster. | |
| 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. | |
| rclcpp::QoS | get_qos () const |
| Getter of the Quality of Service attribute. | |
| void | set_qos (const rclcpp::QoS &qos) |
| Set the Quality of Service for ROS publishers and subscribers. | |
| bool | is_active () const |
| Check if the controller is currently in state active or not. | |
| std::timed_mutex & | get_command_mutex () |
| Get the reference to the command mutex. | |
| template<> | |
| void | add_input (const std::string &name, const std::string &topic_name) |
| template<> | |
| void | add_input (const std::string &name, const std::string &topic_name) |
| template<> | |
| void | add_input (const std::string &name, const std::string &topic_name) |
| template<> | |
| void | add_output (const std::string &name, const std::string &topic_name) |
| template<> | |
| void | add_output (const std::string &name, const std::string &topic_name) |
| template<> | |
| void | add_output (const std::string &name, const std::string &topic_name) |
| template<> | |
| std::optional< bool > | read_input (const std::string &name) |
| template<> | |
| std::optional< double > | read_input (const std::string &name) |
| template<> | |
| std::optional< int > | read_input (const std::string &name) |
Base controller class to combine ros2_control, control libraries and modulo.
Definition at line 111 of file BaseControllerInterface.hpp.
| modulo_controllers::BaseControllerInterface::BaseControllerInterface | ( | ) |
Default constructor.
Definition at line 23 of file BaseControllerInterface.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 1089 of file BaseControllerInterface.hpp.
|
inlineprotected |
Definition at line 719 of file BaseControllerInterface.hpp.
|
inlineprotected |
Definition at line 726 of file BaseControllerInterface.hpp.
|
inlineprotected |
Definition at line 741 of file BaseControllerInterface.hpp.
|
inlineprotected |
Add an input to the controller.
Inputs should be added in the on_init function of the derived controllers. Doing this will create a ROS 2 subscription for the message type.
| T | The type of the input data |
| name | The name of the input |
| topic_name | The topic name of the input (defaults to ~/name) |
Definition at line 685 of file BaseControllerInterface.hpp.
|
inlineprotected |
Definition at line 784 of file BaseControllerInterface.hpp.
|
inlineprotected |
Definition at line 789 of file BaseControllerInterface.hpp.
|
inlineprotected |
Definition at line 800 of file BaseControllerInterface.hpp.
|
inlineprotected |
Add an output to the controller.
Outputs should be added in the on_init function of the derived controllers. Doing this will create a ROS 2 publisher for the message type and wrap it in a RealtimePublisher.
| T | The type of the output data |
| name | The name of the output |
| topic_name | The topic name of the output (defaults to ~/name) |
Definition at line 764 of file BaseControllerInterface.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 |
|
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 |
Definition at line 645 of file BaseControllerInterface.hpp.
|
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 181 of file BaseControllerInterface.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 185 of file BaseControllerInterface.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 ControllerServiceResponse |
Definition at line 501 of file BaseControllerInterface.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 ControllerServiceResponse |
Definition at line 496 of file BaseControllerInterface.cpp.
|
protected |
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 ControllerServiceResponse |
Definition at line 512 of file BaseControllerInterface.cpp.
|
protected |
| service_name | The name of the service |
| callback | A service callback function with no arguments that returns a ControllerServiceResponse |
Definition at line 507 of file BaseControllerInterface.cpp.
|
protected |
Configure a static transform broadcaster.
Definition at line 577 of file BaseControllerInterface.cpp.
|
protected |
Configure a transform broadcaster.
Definition at line 567 of file BaseControllerInterface.cpp.
|
protected |
Configure a transform buffer and listener.
Definition at line 518 of file BaseControllerInterface.cpp.
|
protected |
Add a trigger to the controller.
Triggers are predicates that are always false except when it's triggered in which case it is set back to false immediately after it is read.
| trigger_name | The name of the trigger |
Definition at line 243 of file BaseControllerInterface.cpp.
|
inlineprotected |
Get the value of an assignment.
| T | The type of the assignment value |
| 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 1152 of file BaseControllerInterface.hpp.
|
protected |
Get the reference to the command mutex.
Definition at line 616 of file BaseControllerInterface.cpp.
|
nodiscardprotected |
Get a parameter by name.
| name | The name of the parameter |
Definition at line 115 of file BaseControllerInterface.cpp.
|
inlineprotected |
Get a parameter value by name.
| T | The type of the parameter |
| name | The name of the parameter |
Definition at line 655 of file BaseControllerInterface.hpp.
|
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 206 of file BaseControllerInterface.cpp.
|
nodiscardprotected |
Getter of the Quality of Service attribute.
Definition at line 604 of file BaseControllerInterface.cpp.
|
protected |
Check if the controller is currently in state active or not.
Definition at line 612 of file BaseControllerInterface.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 529 of file BaseControllerInterface.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 538 of file BaseControllerInterface.cpp.
|
override |
Add signals.
| previous_state | The previous lifecycle state |
Definition at line 49 of file BaseControllerInterface.cpp.
|
override |
Declare parameters and register the on_set_parameters callback.
Definition at line 26 of file BaseControllerInterface.cpp.
|
protectedvirtual |
Parameter validation function to be redefined by derived controller classes.
This method is automatically invoked whenever the ROS interface tried to modify a parameter. Validation and sanitization can be performed by reading or writing the value of the parameter through the ParameterInterface pointer, depending on the parameter name and desired controller behaviour. If the validation returns true, the updated parameter value (including any modifications) is applied. If the validation returns false, any changes to the parameter are discarded and the parameter value is not changed.
| parameter | A ParameterInterface pointer to a Parameter instance |
Reimplemented in modulo_controllers::ControllerInterface, and modulo_controllers::RobotControllerInterface.
Definition at line 177 of file BaseControllerInterface.cpp.
|
inlineprotected |
Read the most recent message of an input.
| T | The expected type of the input data |
| name | The name of the input |
Definition at line 810 of file BaseControllerInterface.hpp.
|
inlineprotected |
Definition at line 810 of file BaseControllerInterface.hpp.
|
inlineprotected |
Definition at line 810 of file BaseControllerInterface.hpp.
|
inlineprotected |
Definition at line 810 of file BaseControllerInterface.hpp.
|
protected |
Send a static transform to TF.
| transform | The transform to send |
Definition at line 595 of file BaseControllerInterface.cpp.
|
protected |
Send a vector of static transforms to TF.
| transforms | The vector of transforms to send |
Definition at line 599 of file BaseControllerInterface.cpp.
|
protected |
Send a transform to TF.
| transform | The transform to send |
Definition at line 587 of file BaseControllerInterface.cpp.
|
protected |
Send a vector of transforms to TF.
| transforms | The vector of transforms to send |
Definition at line 591 of file BaseControllerInterface.cpp.
|
inlineprotected |
Set an assignment.
| T | The type of the assignment |
| assignment_name | The name of the assignment to publish |
| assignment_value | The value of the assignment |
Definition at line 1121 of file BaseControllerInterface.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 665 of file BaseControllerInterface.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 224 of file BaseControllerInterface.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 228 of file BaseControllerInterface.cpp.
|
protected |
Set the Quality of Service for ROS publishers and subscribers.
| qos | The desired Quality of Service |
Definition at line 608 of file BaseControllerInterface.cpp.
|
protected |
Latch the trigger with the provided name.
| trigger_name | The name of the trigger |
Definition at line 264 of file BaseControllerInterface.cpp.
|
inline |
Definition at line 1003 of file BaseControllerInterface.hpp.
|
inline |
Definition at line 1008 of file BaseControllerInterface.hpp.
|
inline |
Definition at line 1018 of file BaseControllerInterface.hpp.
|
inline |
Definition at line 1023 of file BaseControllerInterface.hpp.
|
inline |
Definition at line 1013 of file BaseControllerInterface.hpp.
|
inlineprotected |
Write an object to an output.
This uses the realtime publisher from add_output() to simplify publishing data in the realtime context of the control loop.
| T | The type of the the object to publish |
| name | The name of the output |
| state | The object to publish |
Definition at line 910 of file BaseControllerInterface.hpp.