Merge pull request ros-drivers#4 from akhileshmalviya/master

/stereo/get_IMU output standard sensor_msgs/Imu message

README.md

@@ -69,10 +69,16 @@ e.g. :
 rostopic pub -1 /stereo/set_brightness std_msgs/Float64 "data: 6"
 ```
 
-* To read the X,Y,Z co-ordinates of the camera using the built-in IMU:
+* To read the inclination of the camera using the built-in IMU:
 
 ```bash
-rostopic echo /stereo/get_IMU 
+rostopic echo /stereo/get_inclination
+```
+
+* To read the angular velocity and linear acceleration of the camera using built-in IMU:
+
+```bash
+rostopic echo /stereo/get_IMU
 ```
 
 Known Issues

include/uvc_camera/tara_ros.h

@@ -11,6 +11,7 @@
 #include <yaml-cpp/yaml.h>
 #include "geometry_msgs/Point.h"
 #include "std_msgs/Bool.h"
+#include "sensor_msgs/Imu.h"
 
 using namespace std;
 
@@ -25,6 +26,10 @@ using namespace std;
 #define 		MinorVersion2_t	131
 #define 		MinorVersion3_t	652
 
+#define sampleFreq     119.0f                                   // sample frequency in Hz
+#define gyroMeasError  0.1                                      // gyroscope measurement error in rad/s
+#define betaDef        sqrt(3.0f / 4.0f) * gyroMeasError        // compute beta
+
 namespace uvc_camera {
 
 	void Sleep(unsigned int TimeInMilli);
@@ -47,6 +52,8 @@ namespace uvc_camera {
 			BOOL LoadCameraMatrix();
 			//IMU
 			void getInclination(double w_x, double w_y, double w_z, double a_x, double a_y, double a_z);
+			// getOrientation return the orientation in quaternion format
+			void getOrientation(double w_x, double w_y, double w_z, double a_x, double a_y, double a_z);
 			int returnValue;
 
 		private:
@@ -71,6 +78,7 @@ namespace uvc_camera {
 			ros::Publisher info_pub_right;    
 			ros::Publisher exposure_pub;
 			ros::Publisher brightness_pub;
+			ros::Publisher IMU_inclination_pub;
 			ros::Publisher IMU_pub;
 
 			ros::Subscriber time_sub;
@@ -83,6 +91,8 @@ namespace uvc_camera {
 			uvc_cam::Cam *cam;
 			boost::thread image_thread;
 			boost::thread IMU_thread;
+			volatile float beta;	// 2 * proportional gain (Kp)
+			volatile float q0, q1, q2, q3;	// quaternion of sensor frame relative to auxiliary frame
 
 			double angleX, angleY, angleZ; // Rotational angle for cube [NEW]
 			double RwEst[3];
@@ -99,6 +109,7 @@ namespace uvc_camera {
 			double GetIMUIntervalTime(IMUCONFIG_TypeDef	lIMUConfig);
 			BOOL DisableIMU();
 			BOOL checkFirmware (UINT8 MajorVersion, UINT8 MinorVersion1, UINT16 MinorVersion2, UINT16 MinorVersion3);		//Returns 1 if firmware supports auto exposure, else 0;
+			float invSqrt(float x);
 
 	};
 

src/tara_ros.cpp

@@ -153,7 +153,8 @@ namespace uvc_camera {
 				exposure_sub = node.subscribe ("set_exposure", 1, &taraCamera::callBackExposure, this);
 				brightness_sub = node.subscribe ("set_brightness", 1, &taraCamera::callBackBrightness, this);
 
-				IMU_pub = node.advertise<geometry_msgs::Point>("get_IMU", 1, true);
+				IMU_pub = node.advertise<sensor_msgs::Imu>("get_IMU", 1, true);
+				IMU_inclination_pub = node.advertise<geometry_msgs::Point>("get_inclination", 1, true);
 				IMU_thread = boost::thread(boost::bind(&taraCamera::IMU_enable, this));
 			}
 		}
@@ -603,6 +604,8 @@ namespace uvc_camera {
 		double wGyro = 10.0;
 		double norm;
 
+		getOrientation(g_x, g_y, g_z, a_x, a_y, a_z);
+
 		// Normalise the accelerometer measurement
 		norm = sqrt(a_x * a_x + a_y * a_y + a_z * a_z);
 		a_x /= norm;
@@ -679,7 +682,7 @@ namespace uvc_camera {
 		IMUValue.x = angleX;
 		IMUValue.y = angleY;
 		IMUValue.z = angleZ;
-		IMU_pub.publish(IMUValue);
+		IMU_inclination_pub.publish(IMUValue);
 
 	}
 	double taraCamera::GetIMUIntervalTime(IMUCONFIG_TypeDef	lIMUConfig)
@@ -905,4 +908,117 @@ namespace uvc_camera {
 			}
 		}
 	}
+	void taraCamera::getOrientation(double gx, double gy, double gz, double ax, double ay, double az)
+	{
+		float recipNorm;
+		float s0, s1, s2, s3;
+		float qDot1, qDot2, qDot3, qDot4;
+		float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
+
+
+		sensor_msgs::Imu IMUValue;
+
+		// To convert acceleration from 'mg' to 'm/s2'
+		IMUValue.linear_acceleration.x = ax * 9.80665 / 1000;
+		IMUValue.linear_acceleration.y = ay * 9.80665 / 1000;
+		IMUValue.linear_acceleration.z = az * 9.80665 / 1000;
+
+
+		// To convert acceleration from 'dps' to 'radians/second'
+
+		gx *= 0.0174533;
+		gy *= 0.0174533;
+		gz *= 0.0174533;
+
+		IMUValue.angular_velocity.x = gx;
+		IMUValue.angular_velocity.y = gy;
+		IMUValue.angular_velocity.z = gz;
+
+		// Orientation calculation implemented according to Madgwick
+		// Refer : http://x-io.co.uk/res/doc/madgwick_internal_report.pdf
+		// Accuracy - Unknown
+#if 0
+		// Rate of change of quaternion from gyroscope
+		qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
+		qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
+		qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
+		qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
+
+		// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
+		if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+
+			// Normalise accelerometer measurement
+			recipNorm = invSqrt(ax * ax + ay * ay + az * az);
+			ax *= recipNorm;
+			ay *= recipNorm;
+			az *= recipNorm;   
+
+			// Auxiliary variables to avoid repeated arithmetic
+			_2q0 = 2.0f * q0;
+			_2q1 = 2.0f * q1;
+			_2q2 = 2.0f * q2;
+			_2q3 = 2.0f * q3;
+			_4q0 = 4.0f * q0;
+			_4q1 = 4.0f * q1;
+			_4q2 = 4.0f * q2;
+			_8q1 = 8.0f * q1;
+			_8q2 = 8.0f * q2;
+			q0q0 = q0 * q0;
+			q1q1 = q1 * q1;
+			q2q2 = q2 * q2;
+			q3q3 = q3 * q3;
+
+			// Gradient decent algorithm corrective step
+			s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
+			s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
+			s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
+			s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
+			recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
+			s0 *= recipNorm;
+			s1 *= recipNorm;
+			s2 *= recipNorm;
+			s3 *= recipNorm;
+
+			// Apply feedback step
+			qDot1 -= beta * s0;
+			qDot2 -= beta * s1;
+			qDot3 -= beta * s2;
+			qDot4 -= beta * s3;
+		}
+
+		// Integrate rate of change of quaternion to yield quaternion
+		q0 += qDot1 * (1.0f / sampleFreq);
+		q1 += qDot2 * (1.0f / sampleFreq);
+		q2 += qDot3 * (1.0f / sampleFreq);
+		q3 += qDot4 * (1.0f / sampleFreq);
+
+		// Normalise quaternion
+		recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+		q0 *= recipNorm;
+		q1 *= recipNorm;
+		q2 *= recipNorm;
+		q3 *= recipNorm;
+
+		IMUValue.orientation.w = q0;
+		IMUValue.orientation.x = q1;
+		IMUValue.orientation.y = q2;
+		IMUValue.orientation.z = q3;
+#endif		
+		IMU_pub.publish(IMUValue);		
+	}
+
+	//---------------------------------------------------------------------------------------------------
+	// Fast inverse square-root
+	// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
+
+	float taraCamera::invSqrt(float x)
+	{
+		float halfx = 0.5f * x;
+		float y = x;
+		long i = *(long*)&y;
+		i = 0x5f3759df - (i>>1);
+		y = *(float*)&i;
+		y = y * (1.5f - (halfx * y * y));
+		return y;
+	}
 };