calibration_base.h

Go to the documentation of this file.

//
// Created by guanlin on 23-3-14.
//
 
#pragma once
 
#include <ros/ros.h>
#include <controller_interface/multi_interface_controller.h>
#include <rm_common/hardware_interface/actuator_extra_interface.h>
#include <rm_common/hardware_interface/robot_state_interface.h>
#include <rm_common/hardware_interface/gpio_interface.h>
#include <hardware_interface/joint_command_interface.h>
#include <effort_controllers/joint_velocity_controller.h>
#include <effort_controllers/joint_position_controller.h>
#include <control_msgs/QueryCalibrationState.h>
 
namespace rm_calibration_controllers
{
template <typename... T>
class CalibrationBase : public controller_interface::MultiInterfaceController<T...>
{
public:
  CalibrationBase() = default;
  bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override;
  void starting(const ros::Time& time) override;
  void stopping(const ros::Time& time) override;
 
protected:
  bool isCalibrated(control_msgs::QueryCalibrationState::Request& req,
                    control_msgs::QueryCalibrationState::Response& resp);
  ros::ServiceServer is_calibrated_srv_;
  enum State
  {
    INITIALIZED,
    CALIBRATED
  };
  int state_{};
  double velocity_search_{};
  bool calibration_success_ = false;
  rm_control::ActuatorExtraHandle actuator_;
  effort_controllers::JointVelocityController velocity_ctrl_;
  effort_controllers::JointPositionController position_ctrl_;
};
 
}  // namespace rm_calibration_controllers