void imu_publish_data(const ros::TimerEvent& e) {
sensor_msgs::Imu imu_msg_;
double aX = 0.0, aY = 0.0, aZ = 0.0;
freenect_raw_device_state *state;
freenect_update_device_state (f_dev);
state = freenect_get_device_state (f_dev);
freenect_get_mks_accel (state, &aX, &aY, &aZ);
imu_msg_.header.stamp = ros::Time::now();
imu_msg_.linear_acceleration.x = aX;
imu_msg_.linear_acceleration.y = aY;
imu_msg_.linear_acceleration.z = aZ;
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
imu_publisher.publish(imu_msg_);
}
void *freenect_threadfunc(void *arg)
{
freenect_set_tilt_degs(f_dev,freenect_angle);
freenect_set_led(f_dev,LED_RED);
freenect_set_depth_callback(f_dev, depth_cb);
freenect_set_rgb_callback(f_dev, rgb_cb);
freenect_set_rgb_format(f_dev, FREENECT_FORMAT_RGB);
freenect_set_depth_format(f_dev, FREENECT_FORMAT_11_BIT);
freenect_start_depth(f_dev);
freenect_start_rgb(f_dev);
printf("'w'-tilt up, 's'-level, 'x'-tilt down, '0'-'6'-select LED mode\n");
while(!die && freenect_process_events(f_ctx) >= 0 )
{
freenect_raw_device_state* state;
freenect_update_device_state(f_dev);
state = freenect_get_device_state(f_dev);
double dx,dy,dz;
freenect_get_mks_accel(state, &dx, &dy, &dz);
printf("\r raw acceleration: %4d %4d %4d mks acceleration: %4f %4f %4f", state->accelerometer_x, state->accelerometer_y, state->accelerometer_z, dx, dy, dz);
fflush(stdout);
}
printf("\nshutting down streams...\n");
freenect_stop_depth(f_dev);
freenect_stop_rgb(f_dev);
freenect_close_device(f_dev);
freenect_shutdown(f_ctx);
printf("-- done!\n");
return NULL;
}
FreenectDeviceState getState() const {
return FreenectDeviceState(freenect_get_device_state(m_dev));
}