Class ImuHardwareInterface

Defined in File imu_hardware_interface.hpp

Inheritance Relationships

Base Type

public bitbots_ros_control::HardwareInterface (Class HardwareInterface)

Class Documentation

class bitbots_ros_control::ImuHardwareInterface : public bitbots_ros_control::HardwareInterface 

Public Functions

explicit ImuHardwareInterface(rclcpp::Node::SharedPtr nh, std::shared_ptr<DynamixelDriver> &driver, int id, std::string topic, std::string frame, std::string name)

virtual bool init()

virtual void read(const rclcpp::Time &t, const rclcpp::Duration &dt)

virtual void write(const rclcpp::Time &t, const rclcpp::Duration &dt)

virtual void restoreAfterPowerCycle()

Private Functions

void setIMURanges(const std::shared_ptr<bitbots_msgs::srv::IMURanges::Request> req, std::shared_ptr<bitbots_msgs::srv::IMURanges::Response> resp)

void calibrateGyro(const std::shared_ptr<std_srvs::srv::Empty::Request> req, std::shared_ptr<std_srvs::srv::Empty::Response> resp)

void resetGyroCalibration(const std::shared_ptr<std_srvs::srv::Empty::Request> req, std::shared_ptr<std_srvs::srv::Empty::Response> resp)

void setComplementaryFilterParams(const std::shared_ptr<bitbots_msgs::srv::ComplementaryFilterParams::Request> req, std::shared_ptr<bitbots_msgs::srv::ComplementaryFilterParams::Response> resp)

void calibrateAccel(const std::shared_ptr<std_srvs::srv::Empty::Request> req, std::shared_ptr<std_srvs::srv::Empty::Response> resp)

void resetAccelCalibraton(const std::shared_ptr<std_srvs::srv::Empty::Request> req, std::shared_ptr<std_srvs::srv::Empty::Response> resp)

void readAccelCalibration(const std::shared_ptr<bitbots_msgs::srv::AccelerometerCalibration::Request> req, std::shared_ptr<bitbots_msgs::srv::AccelerometerCalibration::Response> resp)

void setAccelCalibrationThreshold(const std::shared_ptr<bitbots_msgs::srv::SetAccelerometerCalibrationThreshold::Request> req, std::shared_ptr<bitbots_msgs::srv::SetAccelerometerCalibrationThreshold::Response> resp)

Private Members

rclcpp::Node::SharedPtr nh_

std::shared_ptr<DynamixelDriver> driver_

int id_

std::string topic_

std::string frame_

std::string name_

std::array<uint8_t, 40> data_

std::array<uint8_t, 28> accel_calib_data_

uint32_t last_seq_number_ = {}

std::array<double, 4> orientation_ = {}

std::array<double, 9> orientation_covariance_ = {}

std::array<double, 3> angular_velocity_ = {}

std::array<double, 9> angular_velocity_covariance_ = {}

std::array<double, 3> linear_acceleration_ = {}

std::array<double, 9> linear_acceleration_covariance_ = {}

diagnostic_msgs::msg::DiagnosticStatus status_imu_

bool write_ranges_ = false

uint8_t gyro_range_

uint8_t accel_range_

bool calibrate_gyro_ = false

bool reset_gyro_calibration_ = false

bool write_complementary_filter_params_ = false

bool do_adaptive_gain_

bool do_bias_estimation_

float accel_gain_

float bias_alpha_

bool calibrate_accel_ = false

bool reset_accel_calibration_ = false

bool read_accel_calibration_ = false

float accel_calib_threshold_read_

float accel_calib_bias_[3]

float accel_calib_scale_[3]

bool set_accel_calib_threshold_ = false

float accel_calib_threshold_

rclcpp::Service<bitbots_msgs::srv::IMURanges>::SharedPtr imu_ranges_service_

rclcpp::Service<std_srvs::srv::Empty>::SharedPtr calibrate_gyro_service_

rclcpp::Service<std_srvs::srv::Empty>::SharedPtr reset_gyro_calibration_service_

rclcpp::Service<bitbots_msgs::srv::ComplementaryFilterParams>::SharedPtr complementary_filter_params_service_

rclcpp::Service<std_srvs::srv::Empty>::SharedPtr calibrate_accel_service_

rclcpp::Service<std_srvs::srv::Empty>::SharedPtr reset_accel_calibration_service_

rclcpp::Service<bitbots_msgs::srv::AccelerometerCalibration>::SharedPtr read_accel_calibration_service_

rclcpp::Service<bitbots_msgs::srv::SetAccelerometerCalibrationThreshold>::SharedPtr set_accel_calib_threshold_service_

rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_

rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr diagnostic_pub_

sensor_msgs::msg::Imu imu_msg_

int diag_counter_