IMU data publication plugin. More...
Public Member Functions | |
std::string const | get_name () const |
Plugin name (CamelCase) | |
const message_map | get_rx_handlers () |
Return map with message rx handlers. | |
IMUPubPlugin () | |
void | initialize (UAS &uas_, ros::NodeHandle &nh, diagnostic_updater::Updater &diag_updater) |
Plugin initializer. | |
Private Member Functions | |
void | fill_imu_msg_attitude (sensor_msgs::ImuPtr &imu_msg, tf::Quaternion &orientation, double xg, double yg, double zg) |
fill imu/data message | |
void | fill_imu_msg_raw (sensor_msgs::ImuPtr &imu_msg, double xg, double yg, double zg, double xa, double ya, double za) |
fill imu/data_raw message, store linear acceleration for imu/data | |
void | handle_attitude (const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) |
void | handle_attitude_quaternion (const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) |
void | handle_highres_imu (const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) |
void | handle_raw_imu (const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) |
void | handle_scaled_imu (const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) |
void | handle_scaled_pressure (const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) |
void | setup_covariance (boost::array< double, 9 > &cov, double stdev) |
void | uas_store_attitude (tf::Quaternion &orientation, geometry_msgs::Vector3 &gyro_vec, geometry_msgs::Vector3 &acc_vec) |
Private Attributes | |
boost::array< double, 9 > | angular_velocity_cov |
std::string | frame_id |
bool | has_att_quat |
bool | has_hr_imu |
bool | has_scaled_imu |
ros::Publisher | imu_pub |
ros::Publisher | imu_raw_pub |
geometry_msgs::Vector3 | linear_accel_vec |
boost::array< double, 9 > | linear_acceleration_cov |
ros::Publisher | magn_pub |
boost::array< double, 9 > | magnetic_cov |
boost::array< double, 9 > | orientation_cov |
ros::Publisher | press_pub |
ros::Publisher | temp_pub |
UAS * | uas |
boost::array< double, 9 > | unk_orientation_cov |
Static Private Attributes | |
static constexpr double | GAUSS_TO_TESLA = 1.0e-4 |
static constexpr double | MILLIBAR_TO_PASCAL = 1.0e2 |
static constexpr double | MILLIG_TO_MS2 = 9.80665 / 1000.0 |
static constexpr double | MILLIRS_TO_RADSEC = 1.0e-3 |
static constexpr double | MILLIT_TO_TESLA = 1000.0 |
IMU data publication plugin.
Definition at line 43 of file imu_pub.cpp.