Change imu_alignment parameter to imu_correction.

README.md

@@ -13,8 +13,8 @@ imu_attitude_to_tf node has to acknowledge Hector_slam: http://wiki.ros.org/hect
 <remap from="gps_odom_topic" to="/odom_utm" />    # UTM GPS coordinates input topic
 <param name="odom_frame" value="world" />         # Fixed odometry frame
 
-# Base link frame == IMU frame, orientation specified by the imu_alignment_rpy rosparam.
-<param name="base_frame" value="imu_link" />      
+# Base link frame
+<param name="base_frame" value="imu_link" />
 
 # If false, translation=0. If true, translation from GPS UTM, set to zero when launched
 <param name="publish_gps_translation" value="False" /> 
@@ -24,9 +24,9 @@ imu_attitude_to_tf node has to acknowledge Hector_slam: http://wiki.ros.org/hect
 <param name="gps_heading_correction_weight" value="0.1" /> # Filtering factor (1==full correction from GPS applied at each step. 0.1 recommended for smooth behavior)
 <param name="gps_heading_min_dist" value="1.5" /> # How long a GPS path segment should be for computing its tangent
 
-# Fixed rotation between the physical IMU and the base_link frame
+# Fixed rotation between the physical IMU and the base_link frame is taken from the static transform
 # Note that we neglect centrifugal acceleration, in skidoo, translation between IMU and Base link == 0
-<rosparam param="imu_alignment_rpy">[3.14159265359, 0.0, 3.14159265359]</rosparam>  <!--Pelicase box for skidoo, all angles in radians! -->
+<rosparam param="imu_correction_rpy">[0.0, 0.0, 0.0]</rosparam>  <!--Pelicase box for skidoo, all angles in radians! -->
 
 # Initial magnetic north correction angle. Note that this value is later precised by the GPS information
 <param name="mag_north_correction_yaw" value="0.27331833" /> <!-- 15.66 deg == 15°40'  => 0.27331833               0.43534373-->

src/gps_gyro_odom_to_tf.cpp

@@ -33,6 +33,7 @@
 
 #include "ros/ros.h"
 #include "tf/transform_broadcaster.h"
+#include "tf/transform_listener.h"
 #include "sensor_msgs/Imu.h"
 #include "nav_msgs/Odometry.h"
 #include "geometry_msgs/Quaternion.h"
@@ -48,12 +49,14 @@ tf::StampedTransform transform_;
 tf::Quaternion tmp_;
 tf::Quaternion mag_heading_delta_quat_;
 tf::Quaternion imu_alignment_;
+tf::Quaternion imu_alignment_correction_;
 tf::Quaternion mag_north_correction_;
 bool mag_heading_collected = false;
 bool mag_heading_applied   = false;
 tfScalar mag_yaw = std::nan("");
 
 
+std::vector<double> imu_correction_rpy_(3, 0.0);
 std::vector<double> imu_alignment_rpy_(3, 0.0);
 double mag_north_correction_yaw_ = 0;
 
@@ -87,7 +90,7 @@ void imuMsgMagCallback(const sensor_msgs::Imu &imu_msg) {
     tfScalar pitch, roll;
 
     mag_heading_collected = true;
-    mag_heading_quat_ = mag_north_correction_ * mag_heading_quat_ * imu_alignment_;
+    mag_heading_quat_ = mag_north_correction_ * mag_heading_quat_ * imu_alignment_correction_ * imu_alignment_;
 
     tf::Matrix3x3 mat(mag_heading_quat_);
     mat.getEulerYPR(mag_yaw, pitch, roll);
@@ -160,31 +163,75 @@ int main(int argc, char **argv) {
     pn.param("publish_odom", p_publish_odom_, false);
     pn.param("odom_topic_name", p_odom_topic_name_, std::string("imu_odom"));
 
+    // Quaternion for Magnetic North correction
+    if (!pn.getParam("mag_north_correction_yaw", mag_north_correction_yaw_)) {
+        ROS_WARN("Parameter mag_north_correction_yaw is not a double, setting default 0 radians");
+    }
+
+    bool imu_alignment_set_;
     // Quaternion for IMU alignment
-    if (!pn.getParam("imu_alignment_rpy", imu_alignment_rpy_)) {
-        ROS_WARN("Parameter imu_alignment_rpy is not a list of three numbers, setting default 0,0,0");
-    } else {
-        if (imu_alignment_rpy_.size() != 3) {
+    if (pn.hasParam("imu_alignment_rpy")) {
+        imu_alignment_set_ = true;
+        ROS_WARN("Parameter imu_alignment_rpy is deprecated. Set correct IMU->base_link orientation in your URDF file and use imu_correction_rpy instead.");
+        if (!pn.getParam("imu_alignment_rpy", imu_alignment_rpy_)) {
             ROS_WARN("Parameter imu_alignment_rpy is not a list of three numbers, setting default 0,0,0");
-            imu_alignment_rpy_.assign(3, 0.0);
+        } else {
+            if (imu_alignment_rpy_.size() != 3) {
+                ROS_WARN("Parameter imu_alignment_rpy is not a list of three numbers, setting default 0,0,0");
+                imu_alignment_rpy_.assign(3, 0.0);
+            }
         }
+    } else {
+        imu_alignment_set_ = false;
     }
 
-    // Quaternion for Magnetic North correction
-    if (!pn.getParam("mag_north_correction_yaw", mag_north_correction_yaw_)) {
-        ROS_WARN("Parameter mag_north_correction_yaw is not a double, setting default 0 radians");
+    // Quaternion for IMU correction
+    if (!pn.getParam("imu_correction_rpy", imu_correction_rpy_))
+    {
+        ROS_WARN("Parameter imu_correction_rpy is not a list of three numbers, setting default 0,0,0");
+    } else	{
+        if (imu_correction_rpy_.size() != 3) {
+            ROS_WARN("Parameter imu_correction_rpy is not a list of three numbers, setting default 0,0,0");
+            imu_correction_rpy_.assign(3, 0.0);
+        }
     }
 
+	// get IMU frame_id
+	auto imu_msg_ptr = ros::topic::waitForMessage<sensor_msgs::Imu>("imu_topic", n, ros::Duration(1.0));
+	if (imu_msg_ptr == nullptr) {
+		delete tfB_;
+		delete odom_pub_;
+		throw ros::Exception("Unable to get IMU frame_id");
+	}
+
+	std::string imu_frame = imu_msg_ptr->header.frame_id;
+    // Get alignment quaternion
+    if (!imu_alignment_set_) {
+        // Get IMU->base_link tf
+        tf::TransformListener tf_listener;
+        tf::StampedTransform tf_imu_bl;
+
+        try {
+            tf_listener.waitForTransform(imu_frame, p_base_frame_, ros::Time(0), ros::Duration(1.0));
+            tf_listener.lookupTransform(imu_frame, p_base_frame_, ros::Time(0), tf_imu_bl);
+        } catch (tf::TransformException &ex) {
+            ROS_ERROR("Unable to get tf between IMU and base_link (%s->%s)", imu_frame.c_str(), p_base_frame_.c_str());
+            delete tfB_;
+            delete odom_pub_;
+            throw;
+        }
+        imu_alignment_ = tf_imu_bl.getRotation();
+    } else {
+        imu_alignment_.setRPY(imu_alignment_rpy_[0],
+                              imu_alignment_rpy_[1],
+                              imu_alignment_rpy_[2]);
+    }
 
-    // Evaluate alignment quternion
-    imu_alignment_.setRPY(imu_alignment_rpy_[0],
-                          imu_alignment_rpy_[1],
-                          imu_alignment_rpy_[2]);
+    // Evaluate correction quaternion
+    imu_alignment_correction_.setRPY(imu_correction_rpy_[0],
+                                     imu_correction_rpy_[1],
+                                     imu_correction_rpy_[2]);
 
-    // Evaluate nag. north corr. quternion
-    mag_north_correction_.setRPY(0.0,
-                                 0.0,
-                                 mag_north_correction_yaw_);
 
     // Prepare the transform, set the origin to zero
     tfB_ = new tf::TransformBroadcaster();

src/imu_and_wheel_odom_node.cpp

@@ -4,6 +4,7 @@
 
 #include "ros/ros.h"
 #include "tf/transform_broadcaster.h"
+#include "tf/transform_listener.h"
 #include "sensor_msgs/Imu.h"
 #include "nav_msgs/Odometry.h"
 #include "geometry_msgs/Quaternion.h"
@@ -23,10 +24,12 @@ tf::StampedTransform transform_;
 tf::Quaternion tmp_;
 tf::Quaternion current_attitude;
 tf::Quaternion imu_alignment_;
+tf::Quaternion imu_alignment_correction_;
 tf::Quaternion mag_north_correction_;
 
 
 
+std::vector<double> imu_correction_rpy_(3, 0.0);
 std::vector<double> imu_alignment_rpy_(3, 0.0);
 double mag_north_correction_yaw_ = 0;
 
@@ -62,7 +65,7 @@ void imuMsgCallback(const sensor_msgs::Imu &imu_msg) {
         return;
     }
 
-    tmp_ = mag_north_correction_ * tmp_ * imu_alignment_;
+    tmp_ = mag_north_correction_ * tmp_ * imu_alignment_correction_ * imu_alignment_;
     tmp_.normalize();
 
     current_attitude = tmp_;
@@ -180,26 +183,74 @@ int main(int argc, char **argv) {
     }
     p_longest_expected_input_odom_period = (1.0*MISSED_ODOM_MSG_SAFETY_MULTIPLIER)/p_wheel_odom_expected_rate;
 
-    // Quaternion for IMU alignment
-    if (!pn.getParam("imu_alignment_rpy", imu_alignment_rpy_)) {
-        ROS_WARN("Parameter imu_alignment_rpy is not a list of three numbers, setting default 0,0,0");
-    } else {
-        if (imu_alignment_rpy_.size() != 3) {
+	// Quaternion for Magnetic North correction
+	if (!pn.getParam("mag_north_correction_yaw", mag_north_correction_yaw_)) {
+		ROS_WARN("Parameter mag_north_correction_yaw is not a double, setting default 0 radians");
+	}
+
+	// get IMU frame_id
+	auto imu_msg_ptr = ros::topic::waitForMessage<sensor_msgs::Imu>("imu_topic", n, ros::Duration(1.0));
+	if (imu_msg_ptr == nullptr) {
+		delete tfB_;
+		delete odom_pub_;
+		throw ros::Exception("Unable to get IMU frame_id");
+	}
+	std::string imu_frame = imu_msg_ptr->header.frame_id;
+
+    bool imu_alignment_set_;
+	// Quaternion for IMU alignment
+	if (pn.hasParam("imu_alignment_rpy")) {
+        imu_alignment_set_ = true;
+        ROS_WARN("Parameter imu_alignment_rpy is deprecated. Set correct IMU->base_link orientation in your URDF file and use imu_correction_rpy instead.");
+        if (!pn.getParam("imu_alignment_rpy", imu_alignment_rpy_)) {
             ROS_WARN("Parameter imu_alignment_rpy is not a list of three numbers, setting default 0,0,0");
-            imu_alignment_rpy_.assign(3, 0.0);
+        } else {
+            if (imu_alignment_rpy_.size() != 3) {
+                ROS_WARN("Parameter imu_alignment_rpy is not a list of three numbers, setting default 0,0,0");
+                imu_alignment_rpy_.assign(3, 0.0);
+            }
         }
+    } else {
+        imu_alignment_set_ = false;
     }
 
-    // Quaternion for Magnetic North correction
-    if (!pn.getParam("mag_north_correction_yaw", mag_north_correction_yaw_)) {
-        ROS_WARN("Parameter mag_north_correction_yaw is not a double, setting default 0 radians");
-    }
-
-
-    // Evaluate alignment quternion
-    imu_alignment_.setRPY(imu_alignment_rpy_[0],
-                          imu_alignment_rpy_[1],
-                          imu_alignment_rpy_[2]);
+	// Quaternion for IMU correction
+	if (!pn.getParam("imu_correction_rpy", imu_correction_rpy_))
+	{
+		ROS_WARN("Parameter imu_correction_rpy is not a list of three numbers, setting default 0,0,0");
+	} else	{
+		if (imu_correction_rpy_.size() != 3) {
+			ROS_WARN("Parameter imu_correction_rpy is not a list of three numbers, setting default 0,0,0");
+			imu_correction_rpy_.assign(3, 0.0);
+		}
+	}
+
+	// Get alignment quaternion
+	if (!imu_alignment_set_) {
+		// Get IMU->base_link tf
+		tf::TransformListener tf_listener(n);
+		tf::StampedTransform tf_imu_bl;
+
+		try {
+			tf_listener.waitForTransform(imu_frame, p_base_frame_, ros::Time(0), ros::Duration(1.0));
+			tf_listener.lookupTransform(imu_frame, p_base_frame_, ros::Time(0), tf_imu_bl);
+		} catch (tf::TransformException &ex) {
+			ROS_ERROR("Unable to get tf between IMU and base_link (%s->%s)", imu_frame.c_str(), p_base_frame_.c_str());
+			delete tfB_;
+			delete odom_pub_;
+			throw;
+		}
+		imu_alignment_ = tf_imu_bl.getRotation();
+	} else {
+		imu_alignment_.setRPY(imu_alignment_rpy_[0],
+			imu_alignment_rpy_[1],
+			imu_alignment_rpy_[2]);
+	}
+
+	// Evaluate correction quaternion
+    imu_alignment_correction_.setRPY(imu_correction_rpy_[0],
+                          imu_correction_rpy_[1],
+                          imu_correction_rpy_[2]);
 
     // Evaluate nag. north corr. quternion
     mag_north_correction_.setRPY(0.0,

src/imu_attitude_and_fix_to_tf_node.cpp

@@ -33,6 +33,7 @@
 
 #include "ros/ros.h"
 #include "tf/transform_broadcaster.h"
+#include "tf/transform_listener.h"
 #include "sensor_msgs/Imu.h"
 #include "nav_msgs/Odometry.h"
 #include "geometry_msgs/Quaternion.h"
@@ -49,9 +50,11 @@ tf::TransformBroadcaster *tfB_;
 tf::StampedTransform transform_;
 tf::Quaternion tmp_;
 tf::Quaternion imu_alignment_;
+tf::Quaternion imu_alignment_correction_;
 tf::Quaternion mag_north_correction_;
 tf::Quaternion fixed_yaw_rotation_by_pi(0.0, 0.0, 1.0, 0);
 
+std::vector<double> imu_correction_rpy_(3, 0.0);
 std::vector<double> imu_alignment_rpy_(3, 0.0);
 double mag_north_correction_yaw_ = 0;
 
@@ -80,7 +83,7 @@ namespace tf { typedef btMatrix3x3 Matrix3x3; }
 void imuMsgCallback(const sensor_msgs::Imu &imu_msg) {
     tf::quaternionMsgToTF(imu_msg.orientation, tmp_);
 
-    tmp_ = mag_north_correction_ * tmp_ * imu_alignment_;
+    tmp_ = mag_north_correction_ * tmp_ * imu_alignment_correction_ * imu_alignment_;
     tmp_.normalize();
 
     transform_.setRotation(tmp_);
@@ -203,27 +206,72 @@ int main(int argc, char **argv) {
     pn.param("gps_heading_min_dist", p_gps_heading_min_dist, 3.0);
     pn.param("odom_topic_name", p_odom_topic_name_, std::string("imu_odom"));
 
+    if (!pn.getParam("mag_north_correction_yaw", mag_north_correction_yaw_)) {
+        ROS_WARN("Parameter mag_north_correction_yaw is not a double, setting default 0 radians");
+    }
 
+    bool imu_alignment_set_;
     // Quaternion for IMU alignment
-    if (!pn.getParam("imu_alignment_rpy", imu_alignment_rpy_)) {
-        ROS_WARN("Parameter imu_alignment_rpy is not a list of three numbers, setting default 0,0,0");
-    } else {
-        if (imu_alignment_rpy_.size() != 3) {
+    if (pn.hasParam("imu_alignment_rpy")) {
+        imu_alignment_set_ = true;
+        ROS_WARN("Parameter imu_alignment_rpy is deprecated. Set correct IMU->base_link orientation in your URDF file and use imu_correction_rpy instead.");
+        if (!pn.getParam("imu_alignment_rpy", imu_alignment_rpy_)) {
             ROS_WARN("Parameter imu_alignment_rpy is not a list of three numbers, setting default 0,0,0");
-            imu_alignment_rpy_.assign(3, 0.0);
+        } else {
+            if (imu_alignment_rpy_.size() != 3) {
+                ROS_WARN("Parameter imu_alignment_rpy is not a list of three numbers, setting default 0,0,0");
+                imu_alignment_rpy_.assign(3, 0.0);
+            }
         }
+    } else {
+        imu_alignment_set_ = false;
     }
 
-    // Quaternion for Magnetic North correction
-    if (!pn.getParam("mag_north_correction_yaw", mag_north_correction_yaw_)) {
-        ROS_WARN("Parameter mag_north_correction_yaw is not a double, setting default 0 radians");
+    // Quaternion for IMU correction
+    if (!pn.getParam("imu_correction_rpy", imu_correction_rpy_))
+    {
+        ROS_WARN("Parameter imu_correction_rpy is not a list of three numbers, setting default 0,0,0");
+    } else	{
+        if (imu_correction_rpy_.size() != 3) {
+            ROS_WARN("Parameter imu_correction_rpy is not a list of three numbers, setting default 0,0,0");
+            imu_correction_rpy_.assign(3, 0.0);
+        }
     }
 
+	// get IMU frame_id
+	auto imu_msg_ptr = ros::topic::waitForMessage<sensor_msgs::Imu>("imu_topic", n, ros::Duration(1.0));
+	if (imu_msg_ptr == nullptr) {
+		delete tfB_;
+		delete odom_pub_;
+		throw;
+	}
+	std::string imu_frame = imu_msg_ptr->header.frame_id;
+
+    // Get alignment quaternion
+    if (!imu_alignment_set_) {
+        // Get IMU->base_link tf
+        tf::TransformListener tf_listener;
+        tf::StampedTransform tf_imu_bl;
+
+        try {
+			tf_listener.waitForTransform(imu_frame, p_base_frame_, ros::Time(0), ros::Duration(1.0));
+			tf_listener.lookupTransform(imu_frame, p_base_frame_, ros::Time(0), tf_imu_bl);
+        } catch (tf::TransformException &ex) {
+            delete tfB_;
+            delete odom_pub_;
+			throw ros::Exception("Unable to get IMU frame_id");
+        }
+        imu_alignment_ = tf_imu_bl.getRotation();
+    } else {
+        imu_alignment_.setRPY(imu_alignment_rpy_[0],
+                              imu_alignment_rpy_[1],
+                              imu_alignment_rpy_[2]);
+    }
 
-    // Evaluate alignment quternion
-    imu_alignment_.setRPY(imu_alignment_rpy_[0],
-                          imu_alignment_rpy_[1],
-                          imu_alignment_rpy_[2]);
+    // Evaluate correction quaternion
+    imu_alignment_correction_.setRPY(imu_correction_rpy_[0],
+                                     imu_correction_rpy_[1],
+                                     imu_correction_rpy_[2]);
 
     // Evaluate nag. north corr. quternion
     mag_north_correction_.setRPY(0.0,