void publishState(void)
{
uint8_t buf[10];
const int ret = libusb_control_transfer(dev, 0xC0, 0x32, 0x0, 0x0, buf, 10, 0);
if (ret != 10)
{
ROS_ERROR_STREAM("Error in accelerometer reading, libusb_control_transfer returned " << ret);
ros::shutdown();
}
const uint16_t ux = ((uint16_t)buf[2] << 8) | buf[3];
const uint16_t uy = ((uint16_t)buf[4] << 8) | buf[5];
const uint16_t uz = ((uint16_t)buf[6] << 8) | buf[7];
const double accelerometer_x = (int16_t)ux;
const double accelerometer_y = (int16_t)uy;
const double accelerometer_z = ((int16_t)uz);
const int8_t tilt_angle = (int8_t)buf[8];
const uint8_t tilt_status = buf[9];
// publish IMU
sensor_msgs::Imu imu_msg;
if (pub_imu.getNumSubscribers() > 0)
{
imu_msg.header.stamp = ros::Time::now();
imu_msg.linear_acceleration.x = filters[0].getValue(interpolators[0].getValue(accelerometer_x) * GRAVITY);
imu_msg.linear_acceleration.y = filters[1].getValue(interpolators[1].getValue(accelerometer_y) * GRAVITY);
imu_msg.linear_acceleration.z = filters[2].getValue(interpolators[2].getValue(accelerometer_z) * GRAVITY);
imu_msg.linear_acceleration_covariance[0] = imu_msg.linear_acceleration_covariance[4]
= imu_msg.linear_acceleration_covariance[8] = 0.01; // @todo - what should these be?
imu_msg.angular_velocity_covariance[0] = -1; // indicates angular velocity not provided
imu_msg.orientation_covariance[0] = -1; // indicates orientation not provided
double magnitude = sqrt((imu_msg.linear_acceleration.x * imu_msg.linear_acceleration.x) + (imu_msg.linear_acceleration.y * imu_msg.linear_acceleration.y) + (imu_msg.linear_acceleration.z + imu_msg.linear_acceleration.z));
//Only publish when the summed magnitued is within 15% of the expected force due to gravity
double error = magnitude - GRAVITY;
double delta = magnitude - lastForce;
double deltaExpected = deltaFilter.getValue(delta);
if(abs(delta - deltaExpected) < (0.1 * GRAVITY) && abs(error) < (GRAVITY * ERROR_MARGIN)){
tf::Quaternion q;
double pitch = -atan2( imu_msg.linear_acceleration.z, sqrt((imu_msg.linear_acceleration.x * imu_msg.linear_acceleration.x) + (imu_msg.linear_acceleration.y * imu_msg.linear_acceleration.y)) );
double roll = atan2( imu_msg.linear_acceleration.x, sqrt((imu_msg.linear_acceleration.z * imu_msg.linear_acceleration.z) + (imu_msg.linear_acceleration.y * imu_msg.linear_acceleration.y)) );
q.setRPY( roll, pitch, 0.0 );
imu_msg.header.frame_id = "/camera_link";
imu_msg.orientation.x = q.x();
imu_msg.orientation.y = q.y();
imu_msg.orientation.z = q.z();
imu_msg.orientation.w = q.w();
pub_imu.publish(imu_msg);
}
}
// publish tilt angle and status
if (pub_tilt_angle.getNumSubscribers() > 0)
{
std_msgs::Float64 tilt_angle_msg;
tilt_angle_msg.data = double(tilt_angle) / 2.;
pub_tilt_angle.publish(tilt_angle_msg);
}
if (pub_tilt_status.getNumSubscribers() > 0)
{
std_msgs::UInt8 tilt_status_msg;
tilt_status_msg.data = tilt_status;
pub_tilt_status.publish(tilt_status_msg);
}
}
