Class PhidgetImuSensor

Defined in File phidget_imu_sensor.hpp

Inheritance Relationships

Base Type

public hardware_interface::SensorInterface

Class Documentation

class PhidgetImuSensor : public hardware_interface::SensorInterface 

Class that implements SensorInterface from ros2_control for PhidgetSpatial IMU.

Public Functions

CallbackReturn on_init(const hardware_interface::HardwareInfo &hardware_info) override

CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state) override

CallbackReturn on_cleanup(const rclcpp_lifecycle::State &previous_state) override

CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state) override

CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override

CallbackReturn on_shutdown(const rclcpp_lifecycle::State &previous_state) override

CallbackReturn on_error(const rclcpp_lifecycle::State &previous_state) override

std::vector<StateInterface> export_state_interfaces() override

return_type read(const rclcpp::Time&, const rclcpp::Duration&) override

Public Static Attributes

static constexpr size_t kImuInterfacesSize = 10

static constexpr double KImuMagneticFieldUnknownValue = 1e300

static constexpr float G = 9.80665

Protected Types

enum ImuMeasurements

Values:

enumerator orientation_x

enumerator orientation_y

enumerator orientation_z

enumerator orientation_w

enumerator angular_velocity_x

enumerator angular_velocity_y

enumerator angular_velocity_z

enumerator linear_acceleration_x

enumerator linear_acceleration_y

enumerator linear_acceleration_z

Protected Functions

void CheckSensorName() const

Checks if the sensor name defined in the urdf matches the expected name.

Throws:: std::runtime_error – If the sensor name does not match the expected name.

void CheckStatesSize() const

Checks if the number of state interfaces defined in the urdf matches the expected size.

Throws:: std::runtime_error – If the number of state interfaces does not match the expected size.

void SetInitialValues()

void CheckInterfaces() const

void ReadObligatoryParams()

void ReadCompassParams()

void ReadMadgwickFilterParams()

void CheckMadgwickFilterWorldFrameParam()

bool IsParamDefined(const std::string &param_name) const

bool AreParamsDefined(const std::unordered_set<std::string> &params_names) const

void ConfigureCompassParams()

void ConfigureHeating()

void ConfigureMadgwickFilter()

void SpatialDataCallback(const double acceleration[3], const double angular_rate[3], const double magnetic_field[3], const double timestamp)

void SpatialAttachCallback()

void SpatialDetachCallback()

geometry_msgs::msg::Vector3 ParseMagnitude(const double magnetic_field[3])

geometry_msgs::msg::Vector3 ParseGyration(const double angular_rate[3])

geometry_msgs::msg::Vector3 ParseAcceleration(const double acceleration[3])

void InitializeMadgwickAlgorithm(const geometry_msgs::msg::Vector3 &mag_compensated, const geometry_msgs::msg::Vector3 &lin_acc, const rclcpp::Time &timestamp)

void RestartMadgwickAlgorithm()

bool IsIMUCalibrated(const geometry_msgs::msg::Vector3 &mag_compensated)

bool IsVectorFinite(const geometry_msgs::msg::Vector3 &vec)

void UpdateMadgwickAlgorithm(const geometry_msgs::msg::Vector3 &ang_vel, const geometry_msgs::msg::Vector3 &lin_acc, const geometry_msgs::msg::Vector3 &mag_compensated, const double dt)

void UpdateMadgwickAlgorithmIMU(const geometry_msgs::msg::Vector3 &ang_vel, const geometry_msgs::msg::Vector3 &lin_acc, const double dt)

void UpdateAccelerationAndGyrationStateValues(const geometry_msgs::msg::Vector3 &ang_vel, const geometry_msgs::msg::Vector3 &lin_acc)

void UpdateAllStatesValues(const geometry_msgs::msg::Vector3 &ang_vel, const geometry_msgs::msg::Vector3 &lin_acc)

void SetStateValuesToNans()

void Calibrate()

bool IsMagnitudeSynchronizedWithAccelerationAndGyration(const geometry_msgs::msg::Vector3 &mag_compensated)

Protected Attributes

std::vector<double> imu_sensor_state_

rclcpp::Logger logger_ = {rclcpp::get_logger("PhidgetImuSensor")}

rclcpp::Clock steady_clock_ = {RCL_STEADY_TIME}

phidgets_spatial::Params params_

std::unique_ptr<phidgets::Spatial> spatial_

std::unique_ptr<ImuFilter> filter_

WorldFrame::WorldFrame world_frame_

bool imu_connected_ = false

bool imu_calibrated_ = false

std::mutex calibration_mutex_

std::condition_variable calibration_cv_

bool algorithm_initialized_ = false

rclcpp::Time last_spatial_data_timestamp_

Protected Static Attributes

static const std::array<std::string, kImuInterfacesSize> kImuInterfacesNames = {"orientation.x", "orientation.y", "orientation.z", "orientation.w", "angular_velocity.x", "angular_velocity.y", "angular_velocity.z", "linear_acceleration.x", "linear_acceleration.y", "linear_acceleration.z",}