From e96dcf51d02ffd76cf9b2d88c029e0f5df32717f Mon Sep 17 00:00:00 2001 From: akhileshmalviya Date: Wed, 8 Mar 2017 17:59:43 +0530 Subject: [PATCH 1/4] /stereo/get_IMU output standard sensor_msgs/Imu message --- include/uvc_camera/tara_ros.h | 11 ++++ src/tara_ros.cpp | 118 +++++++++++++++++++++++++++++++++- 2 files changed, 127 insertions(+), 2 deletions(-) diff --git a/include/uvc_camera/tara_ros.h b/include/uvc_camera/tara_ros.h index b362e5a..ffc5fee 100644 --- a/include/uvc_camera/tara_ros.h +++ b/include/uvc_camera/tara_ros.h @@ -11,6 +11,7 @@ #include #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); }; diff --git a/src/tara_ros.cpp b/src/tara_ros.cpp index 0a0823c..0dbaa24 100644 --- a/src/tara_ros.cpp +++ b/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("get_IMU", 1, true); + IMU_pub = node.advertise("get_IMU", 1, true); + IMU_inclination_pub = node.advertise("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,115 @@ 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; + + + + // 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; + + 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; + } }; From 4214c7b5777db1ecc971f83d82299bf6380e0cb1 Mon Sep 17 00:00:00 2001 From: akhileshmalviya Date: Wed, 8 Mar 2017 18:44:37 +0530 Subject: [PATCH 2/4] /stereo/get_IMU output standard sensor_msgs/Imu message --- README.md | 10 ++++++++-- src/tara_ros.cpp | 8 +++++--- 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 0975766..605b682 100644 --- a/README.md +++ b/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 the linear acceleration of the camera using IMU: + +```bash +rostopic echo /stereo/get_IMU ``` Known Issues diff --git a/src/tara_ros.cpp b/src/tara_ros.cpp index 0dbaa24..d7d8bc7 100644 --- a/src/tara_ros.cpp +++ b/src/tara_ros.cpp @@ -934,8 +934,10 @@ namespace uvc_camera { 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); @@ -1001,7 +1003,7 @@ namespace uvc_camera { IMUValue.orientation.x = q1; IMUValue.orientation.y = q2; IMUValue.orientation.z = q3; - +#endif IMU_pub.publish(IMUValue); } From dd06df2abcfe847e9ceeab485b371e2caa14ded2 Mon Sep 17 00:00:00 2001 From: akhileshmalviya Date: Wed, 8 Mar 2017 18:52:53 +0530 Subject: [PATCH 3/4] /stereo/get_IMU output standard sensor_msgs/Imu message --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 605b682..9ab16dd 100644 --- a/README.md +++ b/README.md @@ -75,7 +75,7 @@ rostopic pub -1 /stereo/set_brightness std_msgs/Float64 "data: 6" rostopic echo /stereo/get_inclination ``` -* To read the angular velocity and the linear acceleration of the camera using IMU: +* To read the angular velocity and linear acceleration of the camera using IMU: ```bash rostopic echo /stereo/get_IMU From 8148832a36a956668a2063819e527b365add12b1 Mon Sep 17 00:00:00 2001 From: akhileshmalviya Date: Wed, 8 Mar 2017 18:55:50 +0530 Subject: [PATCH 4/4] /stereo/get_IMU output standard sensor_msgs/Imu message --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 9ab16dd..7f6acf4 100644 --- a/README.md +++ b/README.md @@ -75,7 +75,7 @@ rostopic pub -1 /stereo/set_brightness std_msgs/Float64 "data: 6" rostopic echo /stereo/get_inclination ``` -* To read the angular velocity and linear acceleration of the camera using IMU: +* To read the angular velocity and linear acceleration of the camera using built-in IMU: ```bash rostopic echo /stereo/get_IMU