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] ));
}
