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