Broadcasting Wiimote accelerator and gyro readings as IMU messages to Topic sensor_data/Imu
Initializes the Wiimote IMU publisher. Parameters: wiiMote: a bluetooth-connected, calibrated WIIMote instance freq: the message sending frequency in messages/sec. Max is 100, because the Wiimote only samples the sensors at 100Hz.
Reimplemented from wiimote_node.WiimoteDataSender.
Loop that obtains the latest wiimote state, publishes the IMU data, and sleeps. The IMU message, if fully filled in, contains information on orientation, acceleration (in m/s^2), and angular rate (in radians/sec). For each of these quantities, the IMU message format also wants the corresponding covariance matrix. Wiimote only gives us acceleration and angular rate. So we ensure that the orientation data entry is marked invalid. We do this by setting the first entry of its associated covariance matrix to -1. The covariance matrices are the 3x3 matrix with the axes' variance in the diagonal. We obtain the variance from the Wiimote instance.