ImuRosI::ImuRosI(ros::NodeHandle nh, ros::NodeHandle nh_private): Imu(), nh_(nh), nh_private_(nh_private), is_connected_(false), error_number_(0), target_publish_freq_(0.0), initialized_(false) { ROS_INFO ("Starting Phidgets IMU"); // **** get parameters if (!nh_private_.getParam ("period", period_)) period_ = 8; // 8 ms if (!nh_private_.getParam ("frame_id", frame_id_)) frame_id_ = "imu"; if (!nh_private_.getParam ("angular_velocity_stdev", angular_velocity_stdev_)) angular_velocity_stdev_ = 0.02 * (M_PI / 180.0); // 0.02 deg/s resolution, as per manual if (!nh_private_.getParam ("linear_acceleration_stdev", linear_acceleration_stdev_)) linear_acceleration_stdev_ = 300.0 * 1e-6 * G; // 300 ug as per manual bool has_compass_params = nh_private_.getParam ("cc_mag_field", cc_mag_field_) && nh_private_.getParam ("cc_offset0", cc_offset0_) && nh_private_.getParam ("cc_offset1", cc_offset1_) && nh_private_.getParam ("cc_offset2", cc_offset2_) && nh_private_.getParam ("cc_gain0", cc_gain0_) && nh_private_.getParam ("cc_gain1", cc_gain1_) && nh_private_.getParam ("cc_gain2", cc_gain2_) && nh_private_.getParam ("cc_t0", cc_T0_) && nh_private_.getParam ("cc_t1", cc_T1_) && nh_private_.getParam ("cc_t2", cc_T2_) && nh_private_.getParam ("cc_t3", cc_T3_) && nh_private_.getParam ("cc_t4", cc_T4_) && nh_private_.getParam ("cc_t5", cc_T5_); // **** advertise topics imu_publisher_ = nh_.advertise<ImuMsg>( "imu/data_raw", 5); mag_publisher_ = nh_.advertise<MagMsg>( "imu/mag", 5); cal_publisher_ = nh_.advertise<std_msgs::Bool>( "imu/is_calibrated", 5); // Set up the topic publisher diagnostics monitor for imu/data_raw // 1. The frequency status component monitors if imu data is published // within 10% tolerance of the desired frequency of 1.0 / period // 2. The timstamp status component monitors the delay between // the header.stamp of the imu message and the real (ros) time // the maximum tolerable drift is +- 100ms target_publish_freq_ = 1000.0 / static_cast<double>(period_); imu_publisher_diag_ptr_ = boost::make_shared<diagnostic_updater::TopicDiagnostic>( "imu/data_raw", boost::ref(diag_updater_), diagnostic_updater::FrequencyStatusParam(&target_publish_freq_, &target_publish_freq_, 0.1, 5), diagnostic_updater::TimeStampStatusParam(-0.1, 0.1) ); // **** advertise services cal_srv_ = nh_.advertiseService( "imu/calibrate", &ImuRosI::calibrateService, this); // **** initialize variables and device imu_msg_.header.frame_id = frame_id_; // build covariance matrices double ang_vel_var = angular_velocity_stdev_ * angular_velocity_stdev_; double lin_acc_var = linear_acceleration_stdev_ * linear_acceleration_stdev_; for (int i = 0; i < 3; ++i) for (int j = 0; j < 3; ++j) { int idx = j*3 +i; if (i == j) { imu_msg_.angular_velocity_covariance[idx] = ang_vel_var; imu_msg_.linear_acceleration_covariance[idx] = lin_acc_var; } else { imu_msg_.angular_velocity_covariance[idx] = 0.0; imu_msg_.linear_acceleration_covariance[idx] = 0.0; } } // signal that we have no orientation estimate (see Imu.msg) imu_msg_.orientation_covariance[0] = -1; // init diagnostics, we set the hardware id properly when the device is connected diag_updater_.setHardwareID("none"); diag_updater_.add("IMU Driver Status", this, &phidgets::ImuRosI::phidgetsDiagnostics); initDevice(); if (has_compass_params) { int result = CPhidgetSpatial_setCompassCorrectionParameters(imu_handle_, cc_mag_field_, cc_offset0_, cc_offset1_, cc_offset2_, cc_gain0_, cc_gain1_, cc_gain2_, cc_T0_, cc_T1_, cc_T2_, cc_T3_, cc_T4_, cc_T5_); if (result) { const char *err; CPhidget_getErrorDescription(result, &err); ROS_ERROR("Problem while trying to set compass correction params: '%s'.", err); } } else { ROS_INFO("No compass correction params found."); } }
int spatial_set_compass_correction_by_array(CPhidgetSpatialHandle phid, double *cc) { return report(CPhidgetSpatial_setCompassCorrectionParameters( phid, cc[0], cc[1], cc[2], cc[3], cc[4], cc[5], cc[6], cc[7], cc[8], cc[9], cc[10], cc[11], cc[12] )); }