增加使用gy951 imu模块的代码

src/razor_imu_9dof/.gitignore

@@ -0,0 +1 @@
+*~

src/razor_imu_9dof/CHANGELOG.rst

@@ -0,0 +1,69 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package razor_imu_9dof
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.1.1 (02-07-2016)
+------------------
+* Passing razor_config_file as ros parameter in launch file (Daniel Koguciuk)
+
+1.1.0 (08-03-2015)
+------------------
+* Resolving bug in exiting display_3D_visualization (`#15 <https://github.com/KristofRobot/razor_imu_9dof/issues/15>`_)
+* Adding dynamic reconfigure for yaw calibration (Paul Bouchier)
+* Moving calibration values from firmware to ROS yaml file (`#13 <https://github.com/KristofRobot/razor_imu_9dof/issues/13>`_)
+
+    * Note: this is a BREAKING CHANGE - requires firmware update (updated firmware provided)
+    
+* Refactoring code: moved scripts to nodes, renamed node.py to imu_node.py (Paul Bouchier)
+* Adding diagnostic status reporting (Paul Bouchier)
+
+1.0.5 (15-11-2014)
+------------------
+* Moving scripts from nodes to scripts dir
+* Installing files in ``src`` and ``magnetometer_calibration``
+* Major cleanup of package.xml and CMakeLists.txt
+
+1.0.4 (15-11-2014)
+------------------
+* Adding press 'a' to align feature
+* Moving magnetometer calibration sketches under dedicated ``magnetometer_calibration`` directory
+* Adding magnetometer calibration sketches for Processing and Matlab (Paul Bouchier)
+* Setting default USB port to /dev/ttyUSB0 in all files
+* Adding graceful exit in case USB port not found
+* Adding queue_size=1
+* Fixing x linear accelerations sign
+
+1.0.3 (02-11-2014)
+------------------
+* Moving all file one directory up
+* Changing axis orientation in 3D visualization to be in line with REP 103
+* Additional output of linear accelerationa and angular velocity in 3D visualization 
+* Adding units of measurement to 3D visualization
+* Major graphical improvements to the 3D visualization
+* Adding explanation on different launch files
+
+1.0.2 (31-10-2014)
+------------------
+* Adding valid covariances (Paul Bouchier)
+* Fixing incorrect direction of yaw & pitch orientation (Paul Bouchier)
+* Fix Readme references & instructions (Paul Bouchier)
+* Converting acceleration to m/s^2 (Paul Bouchier)
+* Adapting to new output message YPRAG instead of YPRAMG (Paul Bouchier)
+* Updating package.xml links to ahrs site (Paul Bouchier)
+* Upgrading Arduino package to 1.4.2 from Peter Bartz' site (Paul Bouchier)
+* Fixing link to Peter Bartz' site (Paul Bouchier)
+* Documenting #ox output mode (Paul Bouchier)
+* Renaming new mode #define to better reflect what it does (Paul Bouchier) 
+* Adding #ox output mode back into Peter's code (Paul Bouchier)
+
+1.0.1 (15-03-2014)
+------------------
+* Cleaning up code based on catkin_lint report
+* Creating additional launch files for display/publishing
+* Implementing flush of first IMU results
+* Changing default port, and adding output showing which port was selected
+* Removing obsolete ``roslib`` import and ``roslib.load_manifest``
+
+1.0.0 (29-12-2013)
+------------------
+* First catkinized version

src/razor_imu_9dof/CMakeLists.txt

@@ -0,0 +1,34 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(razor_imu_9dof)
+
+find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure)
+
+generate_dynamic_reconfigure_options(cfg/imu.cfg)
+
+catkin_package(DEPENDS dynamic_reconfigure)
+
+install(DIRECTORY launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+install(DIRECTORY config
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+install(DIRECTORY cfg
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+install(DIRECTORY src
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+install(DIRECTORY magnetometer_calibration
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+catkin_install_python(PROGRAMS 
+	nodes/imu_node.py
+	nodes/display_3D_visualization.py
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/nodes
+)

src/razor_imu_9dof/README.md

@@ -0,0 +1,124 @@
+Official ROS Documentation
+--------------------------
+A much more extensive and standard ROS-style version of this documentation can be found on the ROS wiki at:
+
+http://wiki.ros.org/razor_imu_9dof
+
+
+Install and Configure ROS Package
+---------------------------------
+1) Install dependencies:
+
+	``$ sudo apt-get install python-visual``
+
+2) Download code:
+
+	``$ cd ~/catkin_workspace/src``
+
+	``$ git clone https://github.com/blmhemu/razor_imu_9dof.git``
+
+	``$ cd ..``
+	
+	``$ catkin_make``
+
+
+Install Arduino firmware
+-------------------------
+1) Open ``src/Razor_AHRS/Razor_AHRS.ino`` in Arduino IDE. Note: this is a modified version
+of Peter Bartz' original Arduino code (see https://github.com/ptrbrtz/razor-9dof-ahrs). 
+Use this version - it emits linear acceleration and angular velocity data required by the ROS Imu message
+
+2) Select your hardware here by uncommenting the right line in ``src/Razor_AHRS/Razor_AHRS.ino``, e.g.
+
+<pre>
+// HARDWARE OPTIONS
+/*****************************************************************/
+// Select your hardware here by uncommenting one line!
+//#define HW__VERSION_CODE 10125 // SparkFun "9DOF Razor IMU" version "SEN-10125" (HMC5843 magnetometer)
+//#define HW__VERSION_CODE 10736 // SparkFun "9DOF Razor IMU" version "SEN-10736" (HMC5883L magnetometer)
+//#define HW__VERSION_CODE 10183 // SparkFun "9DOF Sensor Stick" version "SEN-10183" (HMC5843 magnetometer)
+//#define HW__VERSION_CODE 10321 // SparkFun "9DOF Sensor Stick" version "SEN-10321" (HMC5843 magnetometer)
+#define HW__VERSION_CODE 10724 // SparkFun "9DOF Sensor Stick" version "SEN-10724" (HMC5883L magnetometer)
+</pre>
+
+3) Upload Arduino sketch to the Sparkfun 9DOF Razor IMU board
+
+
+Configure
+---------
+In its default configuration, ``razor_imu_9dof`` expects a yaml config file ``my_razor.yaml`` with:
+* USB port to use
+* Calibration parameters
+
+An example``razor.yaml`` file is provided.
+Copy that file to ``my_razor.yaml`` as follows:
+
+    $ roscd razor_imu_9dof/config
+    $ cp razor.yaml my_razor.yaml
+
+Then, edit ``my_razor.yaml`` as needed
+
+Launch
+------
+Publisher and 3D visualization:
+
+	$ roslaunch razor_imu_9dof razor-pub-and-display.launch
+
+Publisher only:
+
+	$ roslaunch razor_imu_9dof razor-pub.launch
+
+Publisher only with diagnostics:
+
+	$ roslaunch razor_imu_9dof razor-pub-diags.launch
+
+3D visualization only:
+
+	$ roslaunch razor_imu_9dof razor-display.launch
+
+Conventions
+-----------
+
+The IMU follows ENU Convention.
+
+East  = 0 degree
+
+North = 90 degree
+
+West  = 180 degree / -180 degree
+
+South = -90 degree
+
+Accelerations and angular accelerations are also taken care of. (Right Hand Rule)
+
+This standard is in lines with ROS REP 105. This driver can be used for direct interfacing with robot_localization node.
+
+The setup used is shown below for reference
+
+![alt text](https://raw.githubusercontent.com/blmhemu/razor_imu_9dof/indigo-devel/razor.png)
+
+Calibrate
+---------
+For best accuracy, follow the tutorial to calibrate the sensors:
+
+http://wiki.ros.org/razor_imu_9dof
+
+A copy of Peter Bartz's magnetometer calibration scripts from https://github.com/ptrbrtz/razor-9dof-ahrs is provided in the ``magnetometer_calibration`` directory.
+
+Update ``my_razor.yaml`` with the new calibration parameters.
+
+Dynamic Reconfigure
+-------------------
+After having launched the publisher with one of the launch commands listed above, 
+it is possible to dynamically reconfigure the yaw calibration.
+
+1) Run:
+
+    $ rosrun rqt_reconfigure rqt_reconfigure 
+    
+2) Select ``imu_node``. 
+
+3) Change the slider to move the calibration +/- 10 degrees. 
+If you are running the 3D visualization you'll see the display jump when the new calibration takes effect.
+
+The intent of this feature is to let you tune the alignment of the AHRS to the direction of the robot driving direction, so that if you can determine that, for example, the AHRS reads 30 degrees when the robot is actually going at 35 degrees as shown by e.g. GPS, you can tune the calibration to make it read 35. It's the compass-equivalent of bore-sighting a camera.

src/razor_imu_9dof/cfg/imu.cfg

@@ -0,0 +1,9 @@
+#!/usr/bin/env python
+PACKAGE = "razor_imu_9dof"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+gen.add("yaw_calibration", double_t, 0, "Yaw Calibration", 0, -10, 10)
+
+exit(gen.generate(PACKAGE, "razor_imu_9dof", "imu"))

src/razor_imu_9dof/config/my_razor.yaml

@@ -0,0 +1,35 @@
+## USB port
+#port: /dev/IMU
+port: /dev/ttyUSB0
+
+
+##### Calibration ####
+### accelerometer
+accel_x_min: -250.0
+accel_x_max: 250.0
+accel_y_min: -250.0
+accel_y_max: 250.0
+accel_z_min: -250.0
+accel_z_max: 250.0
+
+### magnetometer
+# standard calibration
+magn_x_min: -600.0
+magn_x_max: 600.0
+magn_y_min: -600.0
+magn_y_max: 600.0
+magn_z_min: -600.0
+magn_z_max: 600.0
+
+# extended calibration
+calibration_magn_use_extended: false
+magn_ellipsoid_center: [0, 0, 0]
+magn_ellipsoid_transform: [[0, 0, 0], [0, 0, 0], [0, 0, 0]]
+
+# AHRS to robot calibration
+imu_yaw_calibration: 0.0
+
+### gyroscope
+gyro_average_offset_x: 0.0
+gyro_average_offset_y: 0.0
+gyro_average_offset_z: 0.0

src/razor_imu_9dof/config/razor.yaml

@@ -0,0 +1,34 @@
+## USB port
+port: /dev/ttyUSB0
+
+
+##### Calibration ####
+### accelerometer
+accel_x_min: -250.0
+accel_x_max: 250.0
+accel_y_min: -250.0
+accel_y_max: 250.0
+accel_z_min: -250.0
+accel_z_max: 250.0
+
+### magnetometer
+# standard calibration
+magn_x_min: -600.0
+magn_x_max: 600.0
+magn_y_min: -600.0
+magn_y_max: 600.0
+magn_z_min: -600.0
+magn_z_max: 600.0
+
+# extended calibration
+calibration_magn_use_extended: false
+magn_ellipsoid_center: [0, 0, 0]
+magn_ellipsoid_transform: [[0, 0, 0], [0, 0, 0], [0, 0, 0]]
+
+# AHRS to robot calibration
+imu_yaw_calibration: 0.0
+
+### gyroscope
+gyro_average_offset_x: 0.0
+gyro_average_offset_y: 0.0
+gyro_average_offset_z: 0.0

src/razor_imu_9dof/config/razor_diags.yaml

@@ -0,0 +1,7 @@
+pub_rate: 1.0
+analyzers:
+  Razor9DofImu:
+    type: diagnostic_aggregator/GenericAnalyzer
+    path: 'Imu'
+    timeout: 5.0
+    contains: ['Razor_Imu']

src/razor_imu_9dof/launch/razor-display.launch

@@ -0,0 +1,6 @@
+<launch>
+
+  <node pkg="razor_imu_9dof" type="display_3D_visualization.py" name="display_3D_visualization_node" output="screen">
+  </node>
+  
+</launch>

src/razor_imu_9dof/launch/razor-pub-and-display.launch

@@ -0,0 +1,8 @@
+<launch>
+  <arg name="razor_config_file" default="$(find razor_imu_9dof)/config/my_razor.yaml"/>
+  <node pkg="razor_imu_9dof" type="imu_node.py" name="imu_node" output="screen">
+    <rosparam file="$(arg razor_config_file)" command="load"/>
+  </node>
+  <node pkg="razor_imu_9dof" type="display_3D_visualization.py" name="display_3D_visualization_node" output="screen">
+  </node>
+</launch>

src/razor_imu_9dof/launch/razor-pub-diags.launch

@@ -0,0 +1,12 @@
+<launch>
+    <arg name="razor_config_file" default="$(find razor_imu_9dof)/config/my_razor.yaml"/>
+    <node pkg="razor_imu_9dof" type="imu_node.py" name="imu_node" output="screen">
+        <rosparam file="$(arg razor_config_file)" command="load"/>
+    </node>
+
+    <node pkg="diagnostic_aggregator" type="aggregator_node" name="diagnostic_aggregator"  clear_params="true">
+        <rosparam command="load" file="$(find razor_imu_9dof)/config/razor_diags.yaml" />
+    </node>
+
+    <node pkg="rqt_robot_monitor" type="rqt_robot_monitor" name="rqt_robot_monitor" />
+</launch>

src/razor_imu_9dof/launch/razor-pub.launch

@@ -0,0 +1,6 @@
+<launch>
+  <arg name="razor_config_file" default="$(find razor_imu_9dof)/config/my_razor.yaml"/>
+  <node pkg="razor_imu_9dof" type="imu_node.py" name="imu_node" output="screen">
+    <rosparam file="$(arg razor_config_file)" command="load"/>
+  </node>
+</launch>

src/razor_imu_9dof/magnetometer_calibration/Matlab/magnetometer_calibration/ellipsoid_fit.m

@@ -0,0 +1,148 @@
+function [ center, radii, evecs, v ] = ellipsoid_fit( X, flag, equals )
+%
+% Fit an ellispoid/sphere to a set of xyz data points:
+%
+%   [center, radii, evecs, pars ] = ellipsoid_fit( X )
+%   [center, radii, evecs, pars ] = ellipsoid_fit( [x y z] );
+%   [center, radii, evecs, pars ] = ellipsoid_fit( X, 1 );
+%   [center, radii, evecs, pars ] = ellipsoid_fit( X, 2, 'xz' );
+%   [center, radii, evecs, pars ] = ellipsoid_fit( X, 3 );
+%
+% Parameters:
+% * X, [x y z]   - Cartesian data, n x 3 matrix or three n x 1 vectors
+% * flag         - 0 fits an arbitrary ellipsoid (default),
+%                - 1 fits an ellipsoid with its axes along [x y z] axes
+%                - 2 followed by, say, 'xy' fits as 1 but also x_rad = y_rad
+%                - 3 fits a sphere
+%
+% Output:
+% * center    -  ellispoid center coordinates [xc; yc; zc]
+% * ax        -  ellipsoid radii [a; b; c]
+% * evecs     -  ellipsoid radii directions as columns of the 3x3 matrix
+% * v         -  the 9 parameters describing the ellipsoid algebraically: 
+%                Ax^2 + By^2 + Cz^2 + 2Dxy + 2Exz + 2Fyz + 2Gx + 2Hy + 2Iz = 1
+%
+% Author:
+% Yury Petrov, Northeastern University, Boston, MA
+%
+
+error( nargchk( 1, 3, nargin ) );  % check input arguments
+if nargin == 1
+    flag = 0;  % default to a free ellipsoid
+end
+if flag == 2 && nargin == 2
+    equals = 'xy';
+end
+    
+if size( X, 2 ) ~= 3
+    error( 'Input data must have three columns!' );
+else
+    x = X( :, 1 );
+    y = X( :, 2 );
+    z = X( :, 3 );
+end
+
+% need nine or more data points
+if length( x ) < 9 && flag == 0 
+   error( 'Must have at least 9 points to fit a unique ellipsoid' );
+end
+if length( x ) < 6 && flag == 1
+   error( 'Must have at least 6 points to fit a unique oriented ellipsoid' );
+end
+if length( x ) < 5 && flag == 2
+   error( 'Must have at least 5 points to fit a unique oriented ellipsoid with two axes equal' );
+end
+if length( x ) < 3 && flag == 3
+   error( 'Must have at least 4 points to fit a unique sphere' );
+end
+
+if flag == 0
+    % fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Dxy + 2Exz + 2Fyz + 2Gx + 2Hy + 2Iz = 1
+    D = [ x .* x, ...
+          y .* y, ...
+          z .* z, ...
+      2 * x .* y, ...
+      2 * x .* z, ...
+      2 * y .* z, ...
+      2 * x, ...
+      2 * y, ... 
+      2 * z ];  % ndatapoints x 9 ellipsoid parameters
+elseif flag == 1
+    % fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Gx + 2Hy + 2Iz = 1
+    D = [ x .* x, ...
+          y .* y, ...
+          z .* z, ...
+      2 * x, ...
+      2 * y, ... 
+      2 * z ];  % ndatapoints x 6 ellipsoid parameters
+elseif flag == 2
+    % fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Gx + 2Hy + 2Iz = 1,
+    % where A = B or B = C or A = C
+    if strcmp( equals, 'yz' ) || strcmp( equals, 'zy' )
+        D = [ y .* y + z .* z, ...
+            x .* x, ...
+            2 * x, ...
+            2 * y, ...
+            2 * z ];
+    elseif strcmp( equals, 'xz' ) || strcmp( equals, 'zx' )
+        D = [ x .* x + z .* z, ...
+            y .* y, ...
+            2 * x, ...
+            2 * y, ...
+            2 * z ];
+    else
+        D = [ x .* x + y .* y, ...
+            z .* z, ...
+            2 * x, ...
+            2 * y, ...
+            2 * z ];
+    end
+else
+    % fit sphere in the form A(x^2 + y^2 + z^2) + 2Gx + 2Hy + 2Iz = 1
+    D = [ x .* x + y .* y + z .* z, ...
+      2 * x, ...
+      2 * y, ... 
+      2 * z ];  % ndatapoints x 4 sphere parameters
+end
+
+% solve the normal system of equations
+v = ( D' * D ) \ ( D' * ones( size( x, 1 ), 1 ) );
+
+% find the ellipsoid parameters
+if flag == 0
+    % form the algebraic form of the ellipsoid
+    A = [ v(1) v(4) v(5) v(7); ...
+          v(4) v(2) v(6) v(8); ...
+          v(5) v(6) v(3) v(9); ...
+          v(7) v(8) v(9) -1 ];
+    % find the center of the ellipsoid
+    center = -A( 1:3, 1:3 ) \ [ v(7); v(8); v(9) ];
+    % form the corresponding translation matrix
+    T = eye( 4 );
+    T( 4, 1:3 ) = center';
+    % translate to the center
+    R = T * A * T';
+    % solve the eigenproblem
+    [ evecs evals ] = eig( R( 1:3, 1:3 ) / -R( 4, 4 ) );
+    radii = sqrt( 1 ./ diag( evals ) );
+else
+    if flag == 1
+        v = [ v(1) v(2) v(3) 0 0 0 v(4) v(5) v(6) ];
+    elseif flag == 2
+        if strcmp( equals, 'xz' ) || strcmp( equals, 'zx' )
+            v = [ v(1) v(2) v(1) 0 0 0 v(3) v(4) v(5) ];
+        elseif strcmp( equals, 'yz' ) || strcmp( equals, 'zy' )
+            v = [ v(2) v(1) v(1) 0 0 0 v(3) v(4) v(5) ];
+        else % xy
+            v = [ v(1) v(1) v(2) 0 0 0 v(3) v(4) v(5) ];
+        end
+    else
+        v = [ v(1) v(1) v(1) 0 0 0 v(2) v(3) v(4) ];
+    end
+    center = ( -v( 7:9 ) ./ v( 1:3 ) )';
+    gam = 1 + ( v(7)^2 / v(1) + v(8)^2 / v(2) + v(9)^2 / v(3) );
+    radii = ( sqrt( gam ./ v( 1:3 ) ) )';
+    evecs = eye( 3 );
+end
+
+

src/razor_imu_9dof/magnetometer_calibration/Matlab/magnetometer_calibration/ellipsoid_fit_license.txt

@@ -0,0 +1,24 @@
+Copyright (c) 2009, Yury Petrov
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without 
+modification, are permitted provided that the following conditions are 
+met:
+
+    * Redistributions of source code must retain the above copyright 
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright 
+      notice, this list of conditions and the following disclaimer in 
+      the documentation and/or other materials provided with the distribution
+      
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 
+LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
+CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 
+SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
+INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
+CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
+POSSIBILITY OF SUCH DAMAGE.

src/razor_imu_9dof/magnetometer_calibration/Matlab/magnetometer_calibration/magnetometer_calibration.m

@@ -0,0 +1,71 @@
+%******************************************************************************************
+% Magnetometer Calibration Skript for Razor AHRS v1.4.2
+% 9 Degree of Measurement Attitude and Heading Reference System
+% for Sparkfun "9DOF Razor IMU" and "9DOF Sensor Stick"
+%
+% Released under GNU GPL (General Public License) v3.0
+% Copyright (C) 2013 Peter Bartz [http://ptrbrtz.net]
+% Copyright (C) 2012 Quality & Usability Lab, Deutsche Telekom Laboratories, TU Berlin
+% Written by Peter Bartz (peter-bartz@gmx.de)
+%
+% Infos, updates, bug reports, contributions and feedback:
+%     https://github.com/ptrbrtz/razor-9dof-ahrs
+%******************************************************************************************
+
+% read magnetometer data from Processing Sketch directory
+file = fopen('../../Processing/Magnetometer_calibration/magnetom.float');
+[M, c] = fread(file, [3, inf], 'float', 'b');
+
+x = M(1,:)';
+y = M(2,:)';
+z = M(3,:)';
+
+% do ellipsoid fitting
+[e_center, e_radii, e_eigenvecs, e_algebraic] = ellipsoid_fit([x, y, z]);
+
+% compensate distorted magnetometer data
+% e_eigenvecs is an orthogonal matrix, so ' can be used instead of inv()
+S = [x - e_center(1), y - e_center(2), z - e_center(3)]'; % translate and make array
+scale = inv([e_radii(1) 0 0; 0 e_radii(2) 0; 0 0 e_radii(3)]) * min(e_radii); % scaling matrix
+map = e_eigenvecs'; % transformation matrix to map ellipsoid axes to coordinate system axes
+invmap = e_eigenvecs; % inverse of above
+comp = invmap * scale * map;
+S = comp * S; % do compensation
+
+% output info
+fprintf('In the Razor_AHRS.ino, under "SENSOR CALIBRATION" find the section that reads "Magnetometer (extended calibration)"\n');
+fprintf('Replace the existing 3 lines with these:\n\n');
+fprintf('#define CALIBRATION__MAGN_USE_EXTENDED true\n');
+fprintf('const float magn_ellipsoid_center[3] = {%.6g, %.6g, %.6g};\n', e_center);
+fprintf('const float magn_ellipsoid_transform[3][3] = {{%.6g, %.6g, %.6g}, {%.6g, %.6g, %.6g}, {%.6g, %.6g, %.6g}};\n', comp);
+
+% draw ellipsoid fit
+figure;
+hold on;
+plot3(x, y, z, '.r'); % original data
+
+maxd = max(e_radii);
+step = maxd / 50;
+[xp, yp, zp] = meshgrid(-maxd:step:maxd + e_center(1), -maxd:step:maxd + e_center(2), -maxd:step:maxd + e_center(3));
+
+Ellipsoid = e_algebraic(1) *xp.*xp +   e_algebraic(2) * yp.*yp + e_algebraic(3)   * zp.*zp + ...
+          2*e_algebraic(4) *xp.*yp + 2*e_algebraic(5) * xp.*zp + 2*e_algebraic(6) * yp.*zp + ...
+          2*e_algebraic(7) *xp     + 2*e_algebraic(8) * yp     + 2*e_algebraic(9) * zp;
+p = patch(isosurface(xp, yp, zp, Ellipsoid, 1));
+set(p, 'FaceColor', 'g', 'EdgeColor', 'none');
+
+alpha(0.5);
+view(-70, 40);
+axis vis3d;
+axis equal;
+camlight;
+lighting phong;
+
+% draw original and compensated data
+figure;
+hold on;
+plot3( x, y, z, '.r' ); % original magnetometer data
+plot3(S(1,:), S(2,:), S(3,:), 'b.'); % compensated data
+view( -70, 40 );
+axis vis3d;
+axis equal;

src/razor_imu_9dof/magnetometer_calibration/Processing/Magnetometer_calibration/Magnetometer_calibration.pde

@@ -0,0 +1,374 @@
+/******************************************************************************************
+* Magnetometer Sampling Sketch for Razor AHRS v1.4.2
+* 9 Degree of Measurement Attitude and Heading Reference System
+* for Sparkfun "9DOF Razor IMU" and "9DOF Sensor Stick"
+*
+* Released under GNU GPL (General Public License) v3.0
+* Copyright (C) 2013 Peter Bartz [http://ptrbrtz.net]
+* Copyright (C) 2011-2012 Quality & Usability Lab, Deutsche Telekom Laboratories, TU Berlin
+* Written by Peter Bartz (peter-bartz@gmx.de)
+*
+* Infos, updates, bug reports, contributions and feedback:
+*     https://github.com/ptrbrtz/razor-9dof-ahrs
+******************************************************************************************/
+
+/*
+  NOTE: There seems to be a bug with the serial library in Processing versions 1.5
+  and 1.5.1: "WARNING: RXTX Version mismatch ...".
+  Processing 2.0.x seems to work just fine. Later versions may too.
+  Alternatively, the older version 1.2.1 also works and is still available on the web.
+*/
+
+/*
+  IMPORTANT: You have to install a library, before this sketch can be run!
+  We're using EJML for matrix math, because it's really fast:
+  http://code.google.com/p/java-matrix-benchmark/
+  Also, it's released under LGPL, which fits well with our GPL.
+  Get the library from: http://code.google.com/p/efficient-java-matrix-library/ (you only need
+  the .jar file), find your Processing "libraries" folder (normally this is Processing/libraries
+  in your user documents folder). Create a folder "EJML" inside "libraries",
+  create a folder "library" inside "EJML" and put the .jar inside. Rename to EJML.jar. So you
+  should have "libraries/EJML/library/EJML.jar". Restart Processing and you're good.
+  More info on installing libraries in Processing: http://wiki.processing.org/w/How_to_Install_a_Contributed_Library
+  Tested to be working with EJML 0.17 and 0.23.
+*/
+import org.ejml.data.*;
+import org.ejml.simple.*;
+import org.ejml.ops.*;
+
+// IF THE SKETCH CRASHES OR HANGS ON STARTUP, MAKE SURE YOU ARE USING THE RIGHT SERIAL PORT:
+// 1. Have a look at the Processing console output of this sketch.
+// 2. Look for the serial port list and find the port you need (it's the same as in Arduino).
+// 3. Set your port number here:
+final static int SERIAL_PORT_NUM = 0;
+// 4. Try again.
+
+
+
+import processing.opengl.*;
+import processing.serial.*;
+import java.io.*;
+
+final static int SERIAL_PORT_BAUD_RATE = 57600;
+
+final static int NUM_MAGN_SAMPLES = 10000;
+float magnetom[][] = new float[NUM_MAGN_SAMPLES][3];
+int magnetomIndex = 0;
+
+PFont font;
+Serial serial;
+
+boolean synched = false;
+
+// Skip incoming serial stream data until token is found
+boolean readToken(Serial serial, String token) {
+  // Wait until enough bytes are available
+  if (serial.available() < token.length())
+    return false;
+  
+  // Check if incoming bytes match token
+  for (int i = 0; i < token.length(); i++) {
+    if (serial.read() != token.charAt(i))
+      return false;
+  }
+  
+  return true;
+}
+
+// Global setup
+void setup() {
+  // Setup graphics
+  size(800, 800, OPENGL);
+  smooth();
+  noStroke();
+  frameRate(50);
+  colorMode(HSB);
+  
+  // Load font
+  font = loadFont("Univers-66.vlw");
+  textFont(font);
+  
+  // Setup serial port I/O
+  println("AVAILABLE SERIAL PORTS:");
+  println(Serial.list());
+  String portName = Serial.list()[SERIAL_PORT_NUM];
+  println();
+  println("HAVE A LOOK AT THE LIST ABOVE AND SET THE RIGHT SERIAL PORT NUMBER IN THE CODE!");
+  println("  -> Using port " + SERIAL_PORT_NUM + ": " + portName);
+  serial = new Serial(this, portName, SERIAL_PORT_BAUD_RATE);
+}
+
+void setupRazor() {
+  println("Trying to setup and synch Razor...\n");
+  
+  // On Mac OSX and Linux (Windows too?) the board will do a reset when we connect, which is really bad.
+  // See "Automatic (Software) Reset" on http://www.arduino.cc/en/Main/ArduinoBoardProMini
+  // So we have to wait until the bootloader is finished and the Razor firmware can receive commands.
+  // To prevent this, disconnect/cut/unplug the DTR line going to the board. This also has the advantage,
+  // that the angles you receive are stable right from the beginning. 
+  delay(3000);  // 3 seconds should be enough
+  
+  // Set Razor output parameters
+  serial.write("#osrb");  // Turn on binary output of raw sensor data
+  serial.write("#o1");    // Turn on continuous streaming output
+  serial.write("#oe0");   // Disable error message output
+  
+  // Synch with Razor
+  serial.clear();  // Clear input buffer up to here
+  serial.write("#s00");  // Request synch token
+}
+
+float readFloat(Serial s) {
+  // Convert from little endian (Razor) to big endian (Java) and interpret as float
+  return Float.intBitsToFloat(s.read() + (s.read() << 8) + (s.read() << 16) + (s.read() << 24));
+}
+
+void skipBytes(Serial s, int numBytes) {
+  for (int i = 0; i < numBytes; i++) {
+    s.read();
+  }  
+}
+
+void draw() {
+  // Reset scene
+  lights();
+
+  // Sync with Razor 
+  if (!synched) {
+    background(0);
+    textAlign(CENTER);
+    fill(255);
+    text("Connecting to Razor...", width/2, height/2, -200);
+    
+    if (frameCount == 2)
+      setupRazor();  // Set ouput params and request synch token
+    else if (frameCount > 2)
+      synched = readToken(serial, "#SYNCH00\r\n");  // Look for synch token
+    return;
+  }
+    
+  // Output "max samples reached" message?
+  if (magnetomIndex == NUM_MAGN_SAMPLES - 1) {
+    fill(0, 255, 255);
+    text("MAX NUMBER OF SAMPLES REACHED!", width/2, 0, -250);
+    println("MAX NUMBER OF SAMPLES REACHED!");
+  }
+ 
+  pushMatrix(); {
+    translate(width/2, height/2, -900);
+    
+    // Draw sphere and background once
+    if (magnetomIndex == 0) {
+      background(0);
+      noFill();
+      stroke(255);
+      sphereDetail(10);
+      sphere(400);
+      fill(200);
+      text("Press 'r' to reset. Press SPACE to output calibration parameters to console and quit.", 0, 1100, -600);
+    }
+  
+    // Read and draw new sample
+    if (magnetomIndex < NUM_MAGN_SAMPLES && serial.available() >= 36) {
+      // Read all available magnetometer data from serial port
+      while (serial.available() >= 36) {
+        // Skip accel data
+        skipBytes(serial, 12);
+        // Read magn data
+        magnetom[magnetomIndex][0] = readFloat(serial);  // x
+        magnetom[magnetomIndex][1] = readFloat(serial);  // y
+        magnetom[magnetomIndex][2] = readFloat(serial);  // z
+        // Skip gyro data
+        skipBytes(serial, 12);
+      }
+      
+      // Draw new point
+      fill((magnetom[magnetomIndex][2] + 800) / 8, 255, 255);
+      noStroke();
+      translate(magnetom[magnetomIndex][0], magnetom[magnetomIndex][1], magnetom[magnetomIndex][2]);
+      sphere(5);
+      
+      magnetomIndex++;
+    }
+  } popMatrix();
+}
+
+void keyPressed() {
+  switch (key) {
+    case '0':  // Turn Razor's continuous output stream off
+      serial.write("#o0");
+      break;
+    case '1':  // Turn Razor's continuous output stream on
+      serial.write("#o1");
+      break;
+    case 'f':  // Request one single yaw/pitch/roll frame from Razor (use when continuous streaming is off)
+      serial.write("#f");
+      break;
+    case ' ':  // Calculate and output calibration parameters, output binary magnetometer samples file, quit
+    case ENTER:
+    case RETURN:
+      outputCalibration();  // Do the magic
+      exit();  // We're done
+      break;
+    case 'r':  // Reset samples and clear screen
+      magnetomIndex = 0;
+      break;
+  }
+}
+
+void outputCalibration() {
+  /* ELLIPSOID FITTING CODE */
+  // Adaption of 'ellipsoid_fit' matlab code by Yury Petrov (See 'Matlab' folder
+  // that came with the archive containing this file).
+  
+  // Check if we have at least 9 sample points
+  if (magnetomIndex < 9) {
+    println("ERROR: not enough magnetometer samples. We need at least 9 points.");
+    exit();
+  }
+  
+  // Seperate xyz magnetometer values and make column vectors
+  SimpleMatrix x = new SimpleMatrix(magnetomIndex, 1);
+  SimpleMatrix y = new SimpleMatrix(magnetomIndex, 1);
+  SimpleMatrix z = new SimpleMatrix(magnetomIndex, 1);
+  for (int i = 0; i < magnetomIndex; i++) {
+    x.set(i, magnetom[i][0]);
+    y.set(i, magnetom[i][1]);
+    z.set(i, magnetom[i][2]);
+  }
+  
+  
+  /*
+  % fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Dxy + 2Exz + 2Fyz + 2Gx + 2Hy + 2Iz = 1
+  D = [ x .* x, ...
+        y .* y, ...
+        z .* z, ...
+    2 * x .* y, ...
+    2 * x .* z, ...
+    2 * y .* z, ...
+    2 * x, ...
+    2 * y, ... 
+     2 * z ];  % ndatapoints x 9 ellipsoid parameters
+  */
+  SimpleMatrix D = new SimpleMatrix(x.numRows(), 9);
+  D.insertIntoThis(0, 0, x.elementMult(x));
+  D.insertIntoThis(0, 1, y.elementMult(y));
+  D.insertIntoThis(0, 2, z.elementMult(z));
+  D.insertIntoThis(0, 3, x.elementMult(y).scale(2));
+  D.insertIntoThis(0, 4, x.elementMult(z).scale(2));
+  D.insertIntoThis(0, 5, y.elementMult(z).scale(2));
+  D.insertIntoThis(0, 6, x.scale(2));
+  D.insertIntoThis(0, 7, y.scale(2));
+  D.insertIntoThis(0, 8, z.scale(2));
+  
+  /*
+  % solve the normal system of equations
+  v = ( D' * D ) \ ( D' * ones( size( x, 1 ), 1 ) );
+  */
+  SimpleMatrix tempA = D.transpose().mult(D);
+  SimpleMatrix ones = x.copy(); ones.set(1);
+  SimpleMatrix tempB = D.transpose().mult(ones);
+  SimpleMatrix v = tempA.solve(tempB);
+  
+  /*
+  % form the algebraic form of the ellipsoid
+  A = [ v(1) v(4) v(5) v(7); ...
+        v(4) v(2) v(6) v(8); ...
+        v(5) v(6) v(3) v(9); ...
+        v(7) v(8) v(9) -1 ];
+  */
+  SimpleMatrix A = new SimpleMatrix(new double[][]
+    {{v.get(0), v.get(3), v.get(4), v.get(6)},
+     {v.get(3), v.get(1), v.get(5), v.get(7)},
+     {v.get(4), v.get(5), v.get(2), v.get(8)},
+     {v.get(6), v.get(7), v.get(8),     -1.0}});
+  
+  /*
+  % find the center of the ellipsoid
+  center = -A( 1:3, 1:3 ) \ [ v(7); v(8); v(9) ];
+  */
+  SimpleMatrix center = A.extractMatrix(0, 3, 0, 3).scale(-1).solve(v.extractMatrix(6, 9, 0, 1));
+
+  /*
+  % form the corresponding translation matrix
+  T = eye( 4 );
+  T( 4, 1:3 ) = center';
+  */
+  SimpleMatrix T = SimpleMatrix.identity(4);
+  T.insertIntoThis(3, 0, center.transpose());
+  
+  /*
+  % translate to the center
+  R = T * A * T';
+  % solve the eigenproblem
+  [ evecs evals ] = eig( R( 1:3, 1:3 ) / -R( 4, 4 ) );
+  radii = sqrt( 1 ./ diag( evals ) );
+  */
+  SimpleMatrix R = T.mult(A).mult(T.transpose());
+  SimpleEVD evd = R.extractMatrix(0, 3, 0, 3).divide(-R.get(3, 3)).eig();
+
+  SimpleMatrix evecs = new SimpleMatrix(3, 3);
+  evecs.insertIntoThis(0, 0, evd.getEigenVector(0));
+  evecs.insertIntoThis(0, 1, evd.getEigenVector(1));
+  evecs.insertIntoThis(0, 2, evd.getEigenVector(2));
+  
+  SimpleMatrix radii = new SimpleMatrix(new double[][]
+    {{Math.sqrt(1.0 / evd.getEigenvalue(0).getReal()),
+      Math.sqrt(1.0 / evd.getEigenvalue(1).getReal()),
+      Math.sqrt(1.0 / evd.getEigenvalue(2).getReal())}});
+
+  //center.print();
+  //evecs.print();
+  //radii.print();
+
+  
+  /* CALCULATE COMPENSATION MATRIX */
+  // Adaption of my 'magnetometer_calibration' matlab code. (See 'Matlab' folder
+  // that came with the archive containing this file).
+  /*
+  % compensate distorted magnetometer data
+  % e_eigenvecs is an orthogonal matrix, so ' can be used instead of inv()
+  scale = inv([e_radii(1) 0 0; 0 e_radii(2) 0; 0 0 e_radii(3)]) * min(e_radii); % scaling matrix
+  map = e_eigenvecs'; % transformation matrix to map ellipsoid axes to coordinate system axes
+  invmap = e_eigenvecs; % inverse of above
+  comp = invmap * scale * map;
+  */
+  SimpleMatrix scale = new SimpleMatrix(3, 3);
+  scale.zero();
+  scale.set(0, 0, radii.get(0));
+  scale.set(1, 1, radii.get(1));
+  scale.set(2, 2, radii.get(2));
+  scale = scale.invert().scale(CommonOps.elementMin(radii.getMatrix()));
+  
+  SimpleMatrix map = evecs.transpose();
+  SimpleMatrix invmap = evecs;
+  SimpleMatrix comp = invmap.mult(scale).mult(map);
+  //comp.print();
+  
+  /* OUTPUT RESULTS */
+  // Output magnetometer samples file
+  try {
+    println("Trying to write " + magnetomIndex + " sample points to file magnetom.float ...");
+    FileOutputStream fos = new FileOutputStream(sketchPath("magnetom.float"));
+    DataOutputStream dos = new DataOutputStream(fos);
+    for (int i = 0; i < magnetomIndex; i++) {
+      dos.writeFloat(magnetom[i][0]);
+      dos.writeFloat(magnetom[i][1]);
+      dos.writeFloat(magnetom[i][2]);
+    }
+    fos.close();
+    println("Done.");
+  } catch(Exception e) {
+    println("Exception: " + e.toString());
+  }
+  println("\n");
+
+  // Output calibration
+  System.out.printf("In the Razor_AHRS.ino, under 'SENSOR CALIBRATION' find the section that reads 'Magnetometer (extended calibration)'\n");
+  System.out.printf("Replace the existing 3 lines with these:\n\n");
+  System.out.printf("#define CALIBRATION__MAGN_USE_EXTENDED true\n");
+  System.out.printf("const float magn_ellipsoid_center[3] = {%.6g, %.6g, %.6g};\n", center.get(0), center.get(1), center.get(2));
+  System.out.printf("const float magn_ellipsoid_transform[3][3] = {{%.6g, %.6g, %.6g}, {%.6g, %.6g, %.6g}, {%.6g, %.6g, %.6g}};\n",
+    comp.get(0), comp.get(1), comp.get(2), comp.get(3), comp.get(4), comp.get(5), comp.get(6), comp.get(7), comp.get(8));
+  println("\n");
+}

src/razor_imu_9dof/nodes/display_3D_visualization.py

@@ -0,0 +1,171 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2012, Tang Tiong Yew
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+#    * Redistributions of source code must retain the above copyright
+#      notice, this list of conditions and the following disclaimer.
+#    * Redistributions in binary form must reproduce the above copyright
+#      notice, this list of conditions and the following disclaimer in the
+#      documentation and/or other materials provided with the distribution.
+#    * Neither the name of the Willow Garage, Inc. nor the names of its
+#      contributors may be used to endorse or promote products derived from
+#       this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import rospy
+from visual import *
+import math
+import wx
+
+from sensor_msgs.msg import Imu
+from tf.transformations import euler_from_quaternion
+
+rad2degrees = 180.0/math.pi
+precision = 2 #round to this number of digits
+yaw_offset = 0 #used to align animation upon key press
+
+
+#Create shutdown hook to kill visual displays
+def shutdown_hook():
+    #print "Killing displays"
+    wx.Exit()
+
+#register shutdown hook
+rospy.on_shutdown(shutdown_hook)
+
+
+# Main scene
+scene=display(title="9DOF Razor IMU Main Screen")
+scene.range=(1.3,1.3,1.3)
+scene.forward = (1,0,-0.25)
+# Change reference axis (x,y,z) - z is up
+scene.up=(0,0,1)
+scene.width=500
+scene.height=500
+
+# Second scene (Roll, Pitch, Yaw)
+scene2 = display(title='9DOF Razor IMU Roll, Pitch, Yaw',x=550, y=0, width=500, height=500,center=(0,0,0), background=(0,0,0))
+scene2.range=(1,1,1)
+scene2.select()
+#Roll, Pitch, Yaw
+#Default reference, i.e. x runs right, y runs up, z runs backward (out of screen)
+cil_roll = cylinder(pos=(-0.5,0.3,0),axis=(0.2,0,0),radius=0.01,color=color.red)
+cil_roll2 = cylinder(pos=(-0.5,0.3,0),axis=(-0.2,0,0),radius=0.01,color=color.red)
+cil_pitch = arrow(pos=(0.5,0.3,0),axis=(0,0,-0.4),shaftwidth=0.02,color=color.green)
+arrow_course = arrow(pos=(0.0,-0.4,0),color=color.cyan,axis=(0,0.2,0), shaftwidth=0.02, fixedwidth=1)
+
+#Roll,Pitch,Yaw labels
+label(pos=(-0.5,0.6,0),text="Roll (degrees / radians)",box=0,opacity=0)
+label(pos=(0.5,0.6,0),text="Pitch (degrees / radians)",box=0,opacity=0)
+label(pos=(0.0,0.02,0),text="Yaw (degrees / radians)",box=0,opacity=0)
+label(pos=(0.0,-0.16,0),text="E",box=0,opacity=0,color=color.yellow)
+label(pos=(0.0,-0.64,0),text="W",box=0,opacity=0,color=color.yellow)
+label(pos=(-0.24,-0.4,0),text="N",box=0,opacity=0,color=color.yellow)
+label(pos=(0.24,-0.4,0),text="S",box=0,opacity=0,color=color.yellow)
+label(pos=(0.18,-0.22,0),height=7,text="SE",box=0,color=color.yellow)
+label(pos=(-0.18,-0.22,0),height=7,text="NE",box=0,color=color.yellow)
+label(pos=(0.18,-0.58,0),height=7,text="SW",box=0,color=color.yellow)
+label(pos=(-0.18,-0.58,0),height=7,text="NW",box=0,color=color.yellow)
+
+rollLabel = label(pos=(-0.5,0.52,0),text="-",box=0,opacity=0,height=12)
+pitchLabel = label(pos=(0.5,0.52,0),text="-",box=0,opacity=0,height=12)
+yawLabel = label(pos=(0,-0.06,0),text="-",box=0,opacity=0,height=12)
+
+#acceleration labels
+label(pos=(0,0.9,0),text="Linear Acceleration x / y / z (m/s^2)",box=0,opacity=0)
+label(pos=(0,-0.8,0),text="Angular Velocity x / y / z (rad/s)",box=0,opacity=0)
+linAccLabel = label(pos=(0,0.82,0),text="-",box=0,opacity=0,height=12)
+angVelLabel = label(pos=(0,-0.88,0),text="-",box=0,opacity=0,height=12)
+
+# Main scene objects
+scene.select()
+# Reference axis (x,y,z) - using ROS conventions (REP 103) - z is up, y left (north, 90 deg), x is forward (east, 0 deg)
+# In visual, z runs up, x runs forward, y runs left (see scene.up command earlier) 
+# So everything is good
+arrow(color=color.green,axis=(1,0,0), shaftwidth=0.04, fixedwidth=1)
+arrow(color=color.green,axis=(0,1,0), shaftwidth=0.04 , fixedwidth=1)
+arrow(color=color.green,axis=(0,0,1), shaftwidth=0.04, fixedwidth=1)
+
+# labels
+label(pos=(0,0,-1.2),text="Press 'a' to align",box=0,opacity=0)
+label(pos=(1,0.1,0),text="X",box=0,opacity=0)
+label(pos=(0,1,-0.1),text="Y",box=0,opacity=0)
+label(pos=(0,-0.1,1),text="Z",box=0,opacity=0)
+# IMU object
+platform = box(length=1.0, height=0.05, width=0.65, color=color.red,up=(0,0,1),axis=(-1,0,0))
+p_line = box(length=1.1,height=0.08,width=0.1,color=color.yellow,up=(0,0,1),axis=(-1,0,0))
+plat_arrow = arrow(length=-0.8,color=color.cyan,up=(0,0,1),axis=(-1,0,0), shaftwidth=0.06, fixedwidth=1)
+plat_arrow_up = arrow(length=0.4,color=color.cyan,up=(-1,0,0),axis=(0,0,1), shaftwidth=0.06, fixedwidth=1)
+rospy.init_node("display_3D_visualization_node")
+
+def processIMU_message(imuMsg):
+    global yaw_offset
+
+    roll=0
+    pitch=0
+    yaw=0
+
+    quaternion = (
+      imuMsg.orientation.x,
+      imuMsg.orientation.y,
+      imuMsg.orientation.z,
+      imuMsg.orientation.w)
+    (roll,pitch,yaw) = euler_from_quaternion(quaternion)
+
+    #add align offset to yaw
+    yaw += yaw_offset
+
+    axis=(-cos(pitch)*cos(yaw),-cos(pitch)*sin(yaw),sin(pitch)) 
+    up=(sin(roll)*sin(yaw)+cos(roll)*sin(pitch)*cos(yaw),-sin(roll)*cos(yaw)+cos(roll)*sin(pitch)*sin(yaw),cos(roll)*cos(pitch))
+    platform.axis=axis
+    platform.up=up
+    platform.length=1.0
+    plat_arrow_up.axis=up
+    plat_arrow_up.up=axis
+    plat_arrow_up.length=0.4
+    plat_arrow.axis=axis
+    plat_arrow.up=up
+    plat_arrow.length=-0.8
+    p_line.axis=axis
+    p_line.up=up
+    p_line.length=1.1
+    cil_roll.axis=(-0.2*cos(roll),0.2*sin(roll),0)
+    cil_roll2.axis=(0.2*cos(roll),-0.2*sin(roll),0)
+    cil_pitch.axis=(0,-0.4*sin(pitch),-0.4*cos(pitch))
+    #remove yaw_offset from yaw display
+    arrow_course.axis=(-0.2*sin(yaw-yaw_offset),0.2*cos(yaw-yaw_offset),0)
+    
+    #display in degrees / radians
+    rollLabel.text = str(round(roll*rad2degrees, precision)) + " / " + str(round(roll,precision))
+    pitchLabel.text = str(round(pitch*rad2degrees, precision)) + " / " + str(round(pitch, precision))
+    #remove yaw_offset from yaw display
+    yawLabel.text = str(round((yaw-yaw_offset)*rad2degrees, precision)) + " / " + str(round((yaw-yaw_offset), precision))
+
+    linAccLabel.text = str(round(imuMsg.linear_acceleration.x, precision)) + " / " + str(round(imuMsg.linear_acceleration.y, precision)) + " / " + str(round(imuMsg.linear_acceleration.z, precision))
+    angVelLabel.text = str(round(imuMsg.angular_velocity.x, precision)) + " / " + str(round(imuMsg.angular_velocity.y, precision)) + " / " + str(round(imuMsg.angular_velocity.z, precision))
+
+    #check for align key press - if pressed, next refresh will be aligned
+    if scene.kb.keys: # event waiting to be processed?
+        s = scene.kb.getkey() # get keyboard info
+        if s == 'a':
+            #align key pressed - align
+            yaw_offset += -yaw
+
+
+sub = rospy.Subscriber('imu', Imu, processIMU_message)
+rospy.spin()

src/razor_imu_9dof/nodes/imu_node.py

@@ -0,0 +1,287 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2012, Tang Tiong Yew
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+#    * Redistributions of source code must retain the above copyright
+#      notice, this list of conditions and the following disclaimer.
+#    * Redistributions in binary form must reproduce the above copyright
+#      notice, this list of conditions and the following disclaimer in the
+#      documentation and/or other materials provided with the distribution.
+#    * Neither the name of the Willow Garage, Inc. nor the names of its
+#      contributors may be used to endorse or promote products derived from
+#       this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import rospy
+import serial
+import string
+import math
+import sys
+
+#from time import time
+from sensor_msgs.msg import Imu
+from tf.transformations import quaternion_from_euler
+from dynamic_reconfigure.server import Server
+from razor_imu_9dof.cfg import imuConfig
+from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
+
+degrees2rad = math.pi/180.0
+imu_yaw_calibration = 0.0
+
+# Callback for dynamic reconfigure requests
+def reconfig_callback(config, level):
+    global imu_yaw_calibration
+    rospy.loginfo("""Reconfigure request for yaw_calibration: %d""" %(config['yaw_calibration']))
+    #if imu_yaw_calibration != config('yaw_calibration'):
+    imu_yaw_calibration = config['yaw_calibration']
+    rospy.loginfo("Set imu_yaw_calibration to %d" % (imu_yaw_calibration))
+    return config
+
+rospy.init_node("razor_node")
+#We only care about the most recent measurement, i.e. queue_size=1
+pub = rospy.Publisher('imu', Imu, queue_size=1)
+srv = Server(imuConfig, reconfig_callback)  # define dynamic_reconfigure callback
+diag_pub = rospy.Publisher('diagnostics', DiagnosticArray, queue_size=1)
+diag_pub_time = rospy.get_time();
+
+imuMsg = Imu()
+
+# Orientation covariance estimation:
+# Observed orientation noise: 0.3 degrees in x, y, 0.6 degrees in z
+# Magnetometer linearity: 0.1% of full scale (+/- 2 gauss) => 4 milligauss
+# Earth's magnetic field strength is ~0.5 gauss, so magnetometer nonlinearity could
+# cause ~0.8% yaw error (4mgauss/0.5 gauss = 0.008) => 2.8 degrees, or 0.050 radians
+# i.e. variance in yaw: 0.0025
+# Accelerometer non-linearity: 0.2% of 4G => 0.008G. This could cause
+# static roll/pitch error of 0.8%, owing to gravity orientation sensing
+# error => 2.8 degrees, or 0.05 radians. i.e. variance in roll/pitch: 0.0025
+# so set all covariances the same.
+imuMsg.orientation_covariance = [
+0.0025 , 0 , 0,
+0, 0.0025, 0,
+0, 0, 0.0025
+]
+
+# Angular velocity covariance estimation:
+# Observed gyro noise: 4 counts => 0.28 degrees/sec
+# nonlinearity spec: 0.2% of full scale => 8 degrees/sec = 0.14 rad/sec
+# Choosing the larger (0.14) as std dev, variance = 0.14^2 ~= 0.02
+imuMsg.angular_velocity_covariance = [
+0.02, 0 , 0,
+0 , 0.02, 0,
+0 , 0 , 0.02
+]
+
+# linear acceleration covariance estimation:
+# observed acceleration noise: 5 counts => 20milli-G's ~= 0.2m/s^2
+# nonliniarity spec: 0.5% of full scale => 0.2m/s^2
+# Choosing 0.2 as std dev, variance = 0.2^2 = 0.04
+imuMsg.linear_acceleration_covariance = [
+0.04 , 0 , 0,
+0 , 0.04, 0,
+0 , 0 , 0.04
+]
+
+default_port='/dev/IMU'
+port = rospy.get_param('~port', default_port)
+
+#read calibration parameters
+#port = rospy.get_param('~port', default_port)
+
+#accelerometer
+accel_x_min = rospy.get_param('~accel_x_min', -250.0)
+accel_x_max = rospy.get_param('~accel_x_max', 250.0)
+accel_y_min = rospy.get_param('~accel_y_min', -250.0)
+accel_y_max = rospy.get_param('~accel_y_max', 250.0)
+accel_z_min = rospy.get_param('~accel_z_min', -250.0)
+accel_z_max = rospy.get_param('~accel_z_max', 250.0)
+
+# magnetometer
+magn_x_min = rospy.get_param('~magn_x_min', -600.0)
+magn_x_max = rospy.get_param('~magn_x_max', 600.0)
+magn_y_min = rospy.get_param('~magn_y_min', -600.0)
+magn_y_max = rospy.get_param('~magn_y_max', 600.0)
+magn_z_min = rospy.get_param('~magn_z_min', -600.0)
+magn_z_max = rospy.get_param('~magn_z_max', 600.0)
+calibration_magn_use_extended = rospy.get_param('~calibration_magn_use_extended', False)
+magn_ellipsoid_center = rospy.get_param('~magn_ellipsoid_center', [0, 0, 0])
+magn_ellipsoid_transform = rospy.get_param('~magn_ellipsoid_transform', [[0, 0, 0], [0, 0, 0], [0, 0, 0]])
+imu_yaw_calibration = rospy.get_param('~imu_yaw_calibration', 0.0)
+
+# gyroscope
+gyro_average_offset_x = rospy.get_param('~gyro_average_offset_x', 0.0)
+gyro_average_offset_y = rospy.get_param('~gyro_average_offset_y', 0.0)
+gyro_average_offset_z = rospy.get_param('~gyro_average_offset_z', 0.0)
+
+#rospy.loginfo("%f %f %f %f %f %f", accel_x_min, accel_x_max, accel_y_min, accel_y_max, accel_z_min, accel_z_max)
+#rospy.loginfo("%f %f %f %f %f %f", magn_x_min, magn_x_max, magn_y_min, magn_y_max, magn_z_min, magn_z_max)
+#rospy.loginfo("%s %s %s", str(calibration_magn_use_extended), str(magn_ellipsoid_center), str(magn_ellipsoid_transform[0][0]))
+#rospy.loginfo("%f %f %f", gyro_average_offset_x, gyro_average_offset_y, gyro_average_offset_z)
+
+# Check your COM port and baud rate
+rospy.loginfo("Opening %s...", port)
+try:
+    ser = serial.Serial(port=port, baudrate=57600, timeout=1)
+except serial.serialutil.SerialException:
+    rospy.logerr("IMU not found at port "+port + ". Did you specify the correct port in the launch file?")
+    #exit
+    sys.exit(0)
+
+roll=0
+pitch=0
+yaw=0
+seq=0
+accel_factor = 9.806 / 256.0    # sensor reports accel as 256.0 = 1G (9.8m/s^2). Convert to m/s^2.
+rospy.loginfo("Giving the razor IMU board 5 seconds to boot...")
+rospy.sleep(5) # Sleep for 5 seconds to wait for the board to boot
+
+### configure board ###
+#stop datastream
+ser.write('#o0' + chr(13))
+
+#discard old input
+#automatic flush - NOT WORKING
+#ser.flushInput()  #discard old input, still in invalid format
+#flush manually, as above command is not working
+discard = ser.readlines() 
+
+#set output mode
+ser.write('#ox' + chr(13)) # To start display angle and sensor reading in text
+
+rospy.loginfo("Writing calibration values to razor IMU board...")
+#set calibration values
+ser.write('#caxm' + str(accel_x_min) + chr(13))
+ser.write('#caxM' + str(accel_x_max) + chr(13))
+ser.write('#caym' + str(accel_y_min) + chr(13))
+ser.write('#cayM' + str(accel_y_max) + chr(13))
+ser.write('#cazm' + str(accel_z_min) + chr(13))
+ser.write('#cazM' + str(accel_z_max) + chr(13))
+
+if (not calibration_magn_use_extended):
+    ser.write('#cmxm' + str(magn_x_min) + chr(13))
+    ser.write('#cmxM' + str(magn_x_max) + chr(13))
+    ser.write('#cmym' + str(magn_y_min) + chr(13))
+    ser.write('#cmyM' + str(magn_y_max) + chr(13))
+    ser.write('#cmzm' + str(magn_z_min) + chr(13))
+    ser.write('#cmzM' + str(magn_z_max) + chr(13))
+else:
+    ser.write('#ccx' + str(magn_ellipsoid_center[0]) + chr(13))
+    ser.write('#ccy' + str(magn_ellipsoid_center[1]) + chr(13))
+    ser.write('#ccz' + str(magn_ellipsoid_center[2]) + chr(13))
+    ser.write('#ctxX' + str(magn_ellipsoid_transform[0][0]) + chr(13))
+    ser.write('#ctxY' + str(magn_ellipsoid_transform[0][1]) + chr(13))
+    ser.write('#ctxZ' + str(magn_ellipsoid_transform[0][2]) + chr(13))
+    ser.write('#ctyX' + str(magn_ellipsoid_transform[1][0]) + chr(13))
+    ser.write('#ctyY' + str(magn_ellipsoid_transform[1][1]) + chr(13))
+    ser.write('#ctyZ' + str(magn_ellipsoid_transform[1][2]) + chr(13))
+    ser.write('#ctzX' + str(magn_ellipsoid_transform[2][0]) + chr(13))
+    ser.write('#ctzY' + str(magn_ellipsoid_transform[2][1]) + chr(13))
+    ser.write('#ctzZ' + str(magn_ellipsoid_transform[2][2]) + chr(13))
+
+ser.write('#cgx' + str(gyro_average_offset_x) + chr(13))
+ser.write('#cgy' + str(gyro_average_offset_y) + chr(13))
+ser.write('#cgz' + str(gyro_average_offset_z) + chr(13))
+
+#print calibration values for verification by user
+ser.flushInput()
+ser.write('#p' + chr(13))
+calib_data = ser.readlines()
+calib_data_print = "Printing set calibration values:\r\n"
+for line in calib_data:
+    calib_data_print += line
+rospy.loginfo(calib_data_print)
+
+#start datastream
+ser.write('#o1' + chr(13))
+
+#automatic flush - NOT WORKING
+#ser.flushInput()  #discard old input, still in invalid format
+#flush manually, as above command is not working - it breaks the serial connection
+rospy.loginfo("Flushing first 200 IMU entries...")
+for x in range(0, 200):
+    line = ser.readline()
+rospy.loginfo("Publishing IMU data...")
+#f = open("raw_imu_data.log", 'w')
+
+while not rospy.is_shutdown():
+    line = ser.readline()
+    line = line.replace("#YPRAG=", "")   # Delete "#YPRAG="
+    #f.write(line)                     # Write to the output log file
+    words = string.split(line, ",")    # Fields split
+    if len(words) > 2:
+        #in AHRS firmware z axis points down, in ROS z axis points up (see REP 103)
+	#Added -90 to make it compatible with ENU (REP 103)
+        yaw_deg = -90-float(words[0])
+        yaw_deg = yaw_deg + imu_yaw_calibration
+        if yaw_deg > 180.0:
+            yaw_deg = yaw_deg - 360.0
+        if yaw_deg < -180.0:
+            yaw_deg = yaw_deg + 360.0
+        yaw = yaw_deg*degrees2rad
+        #in AHRS firmware y axis points right, in ROS y axis points left (see REP 103)
+        pitch = float(words[1])*degrees2rad
+        roll = -float(words[2])*degrees2rad
+
+        # Publish message
+        # AHRS firmware accelerations are negated
+        # This means y and z are correct for ROS, but x needs reversing
+        imuMsg.linear_acceleration.x = -float(words[3]) * accel_factor
+        imuMsg.linear_acceleration.y = float(words[4]) * accel_factor
+        imuMsg.linear_acceleration.z = float(words[5]) * accel_factor
+
+        #in AHRS firmware x axis points right,in ROS x axis points left(see REP 103)
+        imuMsg.angular_velocity.x = -float(words[6])
+        imuMsg.angular_velocity.y = float(words[7])
+        #in AHRS firmware z axis points down,in ROS z axis points up(see REP 103) 
+        imuMsg.angular_velocity.z = -float(words[8])
+
+    #rospy.loginfo("roll: "+str(roll)+",pitch: "+str(pitch)+", yaw: "+str(yaw))
+    q = quaternion_from_euler(roll, pitch, yaw)
+    imuMsg.orientation.x = q[0]
+    imuMsg.orientation.y = q[1]
+    imuMsg.orientation.z = q[2]
+    imuMsg.orientation.w = q[3]
+    imuMsg.header.stamp= rospy.Time.now()
+    imuMsg.header.frame_id = 'base_imu_link'
+    imuMsg.header.seq = seq
+    seq = seq + 1
+    pub.publish(imuMsg)
+
+    if (diag_pub_time < rospy.get_time()) :
+        diag_pub_time += 1
+        diag_arr = DiagnosticArray()
+        diag_arr.header.stamp = rospy.get_rostime()
+        diag_arr.header.frame_id = '1'
+        diag_msg = DiagnosticStatus()
+        diag_msg.name = 'Razor_Imu'
+        diag_msg.level = DiagnosticStatus.OK
+        diag_msg.message = 'Received AHRS measurement'
+        diag_msg.values.append(KeyValue('roll (deg)',
+                                str(roll*(180.0/math.pi))))
+        diag_msg.values.append(KeyValue('pitch (deg)',
+                                str(pitch*(180.0/math.pi))))
+        diag_msg.values.append(KeyValue('yaw (deg)',
+                                str(yaw*(180.0/math.pi))))
+        diag_msg.values.append(KeyValue('sequence number', str(seq)))
+        diag_arr.status.append(diag_msg)
+        diag_pub.publish(diag_arr)
+        
+#stop datastream
+ser.write('#o0' + chr(13))
+ser.close
+#f.close

src/razor_imu_9dof/package.xml

@@ -0,0 +1,35 @@
+<package>
+  <name>razor_imu_9dof</name>
+  <version>1.1.1</version>
+  <description>
+     razor_imu_9dof is a package that provides a ROS driver
+     for the Sparkfun Razor IMU 9DOF. It also provides Arduino
+     firmware that runs on the Razor board, and which must be
+     installed on the Razor board for the system to work. A node
+     which displays the attitude (roll, pitch and yaw) of the Razor board 
+     (or any IMU) is provided for testing.
+  </description>
+
+  <maintainer email="krirobo@gmail.com">Kristof Robot</maintainer>
+
+  <license>BSD</license>
+
+  <author>Tang Tiong Yew</author>
+  <author>Kristof Robot</author>
+  <author>Paul Bouchier</author>
+  <author>Peter Bartz</author>
+
+  <url type="website">http://ros.org/wiki/razor_imu_9dof</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>dynamic_reconfigure</build_depend>
+
+  <run_depend>rospy</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>python-serial</run_depend>
+  <run_depend>dynamic_reconfigure</run_depend>
+</package>
+
+

src/razor_imu_9dof/scripts/IMU.rules

@@ -0,0 +1,7 @@
+# set the udev rule , make the device_port be fixed by arduino
+# Need Update "KERNELS==1-1.5" value, invoke command to get value:
+# if gy-951 modle usb device name is ttyUSB1
+# udevadm info --attribute-walk --name=/dev/ttyUSB1 | grep KERNELS
+# you can get third line value
+KERNELS=="1-5", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0777", SYMLINK+="IMU"
+

src/razor_imu_9dof/scripts/create_udev_rules.sh

@@ -0,0 +1,14 @@
+#!/bin/bash
+
+echo "remap the device serial port(ttyUSBX) to  IMU"
+echo "IMU usb connection as /dev/IMU , check it using the command : ls -l /dev|grep ttyUSB"
+echo "start copy IMU.rules to  /etc/udev/rules.d/"
+sudo cp ./IMU.rules  /etc/udev/rules.d
+echo " "
+echo "Restarting udev"
+echo ""
+sudo udevadm control --reload-rules
+sudo service udev restart
+sudo udevadm trigger
+echo "rename IMU device mount point finish "
+

src/razor_imu_9dof/scripts/delete_udev_rules.sh

@@ -0,0 +1,12 @@
+#!/bin/bash
+
+echo "delete remap the device serial port(ttyUSBX) to IMU"
+echo "sudo rm /etc/udev/rules.d/IMU.rules"
+sudo rm /etc/udev/rules.d/IMU.rules
+echo " "
+echo "Restarting udev"
+echo ""
+sudo udevadm control --reload-rules
+sudo service udev restart
+sudo udevadm trigger
+echo "finish  delete"

src/robot_bringup/launch/odom_ekf.launch

@@ -0,0 +1,6 @@
+<launch>
+  <node pkg="robot_bringup" type="odom_ekf.py" name="odom_ekf" output="screen">
+    <remap from="input" to="/robot_pose_ekf/odom_combined"/>
+    <remap from="output" to="/odom_ekf"/>
+  </node>
+</launch>

src/robot_bringup/launch/odom_ekf.py

@@ -0,0 +1,63 @@
+#!/usr/bin/env python
+
+""" odom_ekf.py - Version 1.1 2013-12-20
+
+    Republish the /robot_pose_ekf/odom_combined topic which is of type 
+    geometry_msgs/PoseWithCovarianceStamped as an equivalent message of
+    type nav_msgs/Odometry so we can view it in RViz.
+
+    Created for the Pi Robot Project: http://www.pirobot.org
+    Copyright (c) 2012 Patrick Goebel.  All rights reserved.
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.5
+    
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details at:
+    
+    http://www.gnu.org/licenses/gpl.html
+      
+"""
+
+import rospy
+from geometry_msgs.msg import PoseWithCovarianceStamped
+from nav_msgs.msg import Odometry
+
+class OdomEKF():
+    def __init__(self):
+        # Give the node a name
+        rospy.init_node('odom_ekf_node', anonymous=False)
+
+        # Publisher of type nav_msgs/Odometry
+        self.ekf_pub = rospy.Publisher('output', Odometry, queue_size=20)
+        
+        # Wait for the /odom_combined topic to become available
+        rospy.wait_for_message('input', PoseWithCovarianceStamped)
+        
+        # Subscribe to the /odom_combined topic
+        rospy.Subscriber('input', PoseWithCovarianceStamped, self.pub_ekf_odom)
+        
+        rospy.loginfo("Publishing combined odometry on /odom_ekf")
+        
+    def pub_ekf_odom(self, msg):
+        odom = Odometry()
+        odom.header = msg.header
+        odom.header.frame_id = '/odom_combined'
+        odom.child_frame_id = 'base_footprint'
+        odom.pose = msg.pose
+        
+        self.ekf_pub.publish(odom)
+        
+if __name__ == '__main__':
+    try:
+        OdomEKF()
+        rospy.spin()
+    except:
+        pass
+        
+
+