Core functionalities for the drivers

This package contains two libraries that implement the common functionalities of the 3 kuka drivers to reduce code duplications and make the code more maintainable.

Wrapper methods for specific services

The kuka_drivers_core::communication_helpers is a header-only library providing wrapper methods for commonly used features.

Synchronous service calls

rclcpp does not provide synchronous service calls, this is implemented in the service_tools.hpp

It provides the sendRequest() endpoint with following arguments: - client [ClientT]: the initialized service client - request [RequestT]: the filled request with appropriate type - service_timeout_ms int: timeout for service discovery - response_timeout_ms int: timeout for getting the response

The method returns the service response (as a shared pointer).

Example for calling the ListControllers service of the controller_manager:

rclcpp::Client<controller_manager_msgs::srv::ListControllers>::SharedPtr get_controllers_client_;
auto request = std::make_shared<controller_manager_msgs::srv::ListControllers::Request>();

// [...]

auto response =  kuka_drivers_core::sendRequest<controller_manager_msgs::srv::ListControllers::Response>(get_controllers_client_, request, 0, 1000);

ros2_control state handling

The library also contains the ros2_control_tools.hpp header, which implements wrapper methods for modifying the states of controllers and hardware components.

Endpoints:

The changeHardwareState() can change the state of one hardware component and has the following arguments: - client [rclcpp::Client<:srv::sethardwarecomponentstate>::SharedPtr]: initialized client - hardware_name[std::string]: name of the hardware component -state[uint8_t of [enum](https://docs.ros2.org/foxy/api/lifecycle_msgs/msg/State.html)]: desired state after state change (only one transition is possible with one call) -timeout_ms` int: timeout for the response

The method returns whether the transition was successful.

The changeControllerState() can change the state of more controllers and has the following arguments: - client [rclcpp::Client<:srv::switchcontroller>::SharedPtr]: initialized client - activate_controllers [std::vector<std::string>]: names of the controllers to activate - deactivate_controllers [std::vector<std::string>]: names of the controllers to deactivate - strictness int32_t: whether to fail if one state change is unsuccessful

The method returns whether the transitions were successful.

Examples:

rclcpp::Client<controller_manager_msgs::srv::SetHardwareComponentState>::SharedPtr change_hardware_state_client_;
rclcpp::Client<controller_manager_msgs::srv::SwitchController>::SharedPtr change_controller_state_client_;

// [...]

// Activate hardware named 'lbr_iisy3_r760'
bool success1 = changeHardwareState(change_hardware_state_client_, "lbr_iisy3_r760", State::PRIMARY_STATE_ACTIVE);

// Activate 'joint_state_broadcaster' and 'joint_trajectory_controller'
bool success2 = changeControllerState(change_controller_state_client_, {"joint_state_broadcaster", "joint_trajectory_controller"}, {/*nothing to deactivate*/});

Core classes

Base classes with improved parameter handling

There are two core classes implemented in this repository, ROS2BaseNode for improved parameter handling, and ROS2BaseLCNode, which derives from the rclcpp_lifecycle::LifecycleNode class and implements lifecycle state transitions which would be usually implemented in the same way in every case. These are virtual functions, so it is possible to override them in the case of a different desired implementation.

Parameter handling

The base classes provide a wrapper method for parameter registration, which makes handling of parameters more convenient. This is done with the help of the ParameterHandler class, which includes a ParameterBase and a template Parameter<T> nested class for this purpose.

Improvements: - The add_on_set_parameters_callback() is called in both of the constructors to register the callback for parameter change requests. This makes sure that the initial values of the parameters are synced from the parameter server. - The parameter type is enforced automatically. - The registered callback has access over all of the registered parameters, therefore the parameter server and the node is always in sync - It is easy to register a callback for additional checks before requested parameter value is accepted. - The user can define the lifecycle states, in which parameter changes are allowed using the ParameterSetAccessRights structure. - There is a different endpoint for static parameters, which cannot be changed after initialization.

The Parameter<T> class supports the following parameter types (identical to the types supported by rclcpp::Parameter): - bool - int64_t (or type satisfying std::is_integral except bool) - double (or type satisfying std::is_floating_point) - std::string - std::vector<uint8_t> - std::vector<bool> - std::vector<int64_t> - std::vector<double> - std::vector<std::string>

The nodes provide the registerParameter() and registerStaticParameter() endpoints with the following arguments: - name [std::string]: name of the parameter - value [T]: default value of the parameter - rights [ParameterSetAccessRights]: structure defining whether modifying the parameter value is allowed in inactive and active states (only for ROS2BaseLCNode, value changes are always allowed in unconfigured state) - on_change_callback [std::function]: the callback to call when determining the validity of a parameter change request

Both methods register a parameter with the given name and type, and the on_change_callback is called if a parameter change is requested to check validity. In case of the registerStaticParameter(), the callback always returns false after initializing the value.

Example code for registering an integer parameter for both base nodes (onRateChangeRequest() checks the validity of the requested rate and returns a boolean):

  // For class derived from ROS2BaseLCNode
  registerParameter<int>(
    "rate", 2, kuka_drivers_core::ParameterSetAccessRights {true, false},
      [this](int rate) {
      return this->onRateChangeRequest(rate);
    });

  // For class derived from ROS2BaseNode
  registerParameter<int>(
    "rate", 2, [this](int rate) {
      return this->onRateChangeRequest(rate);
    });

To modify the callback that is called for validating every parameter change, one has to remove the registered callback and add the new one. For example to disable all parameter changes if a parameter_change_blocked_ flag is set, it is possible to add the condition to the registered callback:

  remove_on_set_parameters_callback(ParamCallback().get());
  ParamCallback() = this->add_on_set_parameters_callback(
    [this](const std::vector<rclcpp::Parameter> & parameters) {
      if (!parameter_change_blocked_) {
        return getParameterHandler().onParamChange(parameters);
      } else {
        rcl_interfaces::msg::SetParametersResult result;
        result.successful = false;
        return result;
      }
    });

Control mode handler

The package also contains the ControllerHandler class, which is responsible for tracking the active controllers and on control mode changes return the controller names that need to be activated and deactivated based on the control mode definitions.

There is a defined set of controllers that must be active for every control mode, the GetControllersForSwitch() method determines the switches necessary in case of a control mode change. The method returns the names of the controllers to be switched, to make this possible, the names of the controllers should be provided for every controller type with the UpdateControllerName() method.

If the switch was successful, the ApproveControllerActivation() and ApproveControllerDeactivation() methods should be called to update the internal state of the ControllerHandler class.

The class can also handle controllers that should be active in every control mode (e.g. joint_state_broadcaster), these should be be given in the constructor as the fixed_controllers argument (std::vector).

Type definitions and modified control node

Additionally common type definitions are included for control modes (see details on the wiki) and hardware interface types.

The package also contains the modified control_node that instantiates the controller_manager without managing the timing.