void initialize(UAS &uas_) { PluginBase::initialize(uas_); double linear_stdev, angular_stdev, orientation_stdev, mag_stdev; /** * @warning A rotation from the aircraft-frame to the base_link frame is applied. * Additionally, it is reported the orientation of the vehicle to describe the * transformation from the ENU frame to the base_link frame (ENU <-> base_link). * THIS ORIENTATION IS NOT THE SAME AS THAT REPORTED BY THE FCU (NED <-> aircraft). */ imu_nh.param<std::string>("frame_id", frame_id, "base_link"); imu_nh.param("linear_acceleration_stdev", linear_stdev, 0.0003); // check default by MPU6000 spec imu_nh.param("angular_velocity_stdev", angular_stdev, 0.02 * (M_PI / 180.0)); // check default by MPU6000 spec imu_nh.param("orientation_stdev", orientation_stdev, 1.0); imu_nh.param("magnetic_stdev", mag_stdev, 0.0); setup_covariance(linear_acceleration_cov, linear_stdev); setup_covariance(angular_velocity_cov, angular_stdev); setup_covariance(orientation_cov, orientation_stdev); setup_covariance(magnetic_cov, mag_stdev); setup_covariance(unk_orientation_cov, 0.0); imu_pub = imu_nh.advertise<sensor_msgs::Imu>("data", 10); magn_pub = imu_nh.advertise<sensor_msgs::MagneticField>("mag", 10); temp_imu_pub = imu_nh.advertise<sensor_msgs::Temperature>("temperature_imu", 10); temp_baro_pub = imu_nh.advertise<sensor_msgs::Temperature>("temperature_baro", 10); static_press_pub = imu_nh.advertise<sensor_msgs::FluidPressure>("static_pressure", 10); diff_press_pub = imu_nh.advertise<sensor_msgs::FluidPressure>("diff_pressure", 10); imu_raw_pub = imu_nh.advertise<sensor_msgs::Imu>("data_raw", 10); // Reset has_* flags on connection change enable_connection_cb(); }
void initialize(UAS &uas_, ros::NodeHandle &nh, diagnostic_updater::Updater &diag_updater) { double linear_stdev, angular_stdev, orientation_stdev, mag_stdev; uas = &uas_; nh.param<std::string>("imu/frame_id", frame_id, "fcu"); nh.param("imu/linear_acceleration_stdev", linear_stdev, 0.0003); // check default by MPU6000 spec nh.param("imu/angular_velocity_stdev", angular_stdev, 0.02 * (M_PI / 180.0)); // check default by MPU6000 spec nh.param("imu/orientation_stdev", orientation_stdev, 1.0); nh.param("imu/magnetic_stdev", mag_stdev, 0.0); setup_covariance(linear_acceleration_cov, linear_stdev); setup_covariance(angular_velocity_cov, angular_stdev); setup_covariance(orientation_cov, orientation_stdev); setup_covariance(magnetic_cov, mag_stdev); setup_covariance(unk_orientation_cov, 0.0); imu_pub = nh.advertise<sensor_msgs::Imu>("imu/data", 10); magn_pub = nh.advertise<sensor_msgs::MagneticField>("imu/mag", 10); temp_pub = nh.advertise<sensor_msgs::Temperature>("imu/temperature", 10); press_pub = nh.advertise<sensor_msgs::FluidPressure>("imu/atm_pressure", 10); imu_raw_pub = nh.advertise<sensor_msgs::Imu>("imu/data_raw", 10); }