RobotControllerInterface.hpp

#pragma once
 
#include <realtime_tools/realtime_buffer.h>
 
#include <robot_model/Model.hpp>
#include <state_representation/space/cartesian/CartesianState.hpp>
#include <state_representation/space/cartesian/CartesianWrench.hpp>
#include <state_representation/space/joint/JointState.hpp>
 
#include "modulo_controllers/ControllerInterface.hpp"
 
namespace modulo_controllers {
 
class RobotControllerInterface : public ControllerInterface {
public:
  RobotControllerInterface();
 
  explicit RobotControllerInterface(
      bool robot_model_required, const std::string& control_type = "", bool load_geometries = false);
 
  CallbackReturn add_interfaces() override;
 
  CallbackReturn on_configure() override;
 
  CallbackReturn on_activate() override;
 
  CallbackReturn on_deactivate() override;
 
protected:
  const state_representation::JointState& get_joint_state();
 
  const state_representation::CartesianState& get_cartesian_state();
 
  const state_representation::CartesianWrench& get_ft_sensor();
 
  void compute_cartesian_state();
 
  void set_joint_command(const state_representation::JointState& joint_command);
 
  bool
  on_validate_parameter_callback(const std::shared_ptr<state_representation::ParameterInterface>& parameter) override;
 
  std::shared_ptr<robot_model::Model> robot_;
  std::string task_space_frame_;
 
  std::string ft_sensor_name_;           
  std::string ft_sensor_reference_frame_;
 
private:
  controller_interface::return_type read_state_interfaces() final;
 
  controller_interface::return_type write_command_interfaces(const rclcpp::Duration& period) final;
 
  std::vector<std::string> joints_;
  std::string control_type_;       
 
  bool robot_model_required_;
  bool load_geometries_;     
  bool
      new_joint_command_ready_;
  double
      command_decay_factor_;
  double
      command_rate_limit_;
 
  realtime_tools::RealtimeBuffer<std::vector<double>> joint_command_values_;
  std::vector<double> previous_joint_command_values_;
 
  state_representation::JointState joint_state_;
  state_representation::CartesianState cartesian_state_;
  state_representation::CartesianWrench ft_sensor_;
};
 
}// namespace modulo_controllers