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