diff --git a/README.md b/README.md
index 13a953d..0f4620e 100644
--- a/README.md
+++ b/README.md
@@ -13,8 +13,8 @@ imu_attitude_to_tf node has to acknowledge Hector_slam: http://wiki.ros.org/hect
# UTM GPS coordinates input topic
# Fixed odometry frame
-# Base link frame == IMU frame, orientation specified by the imu_alignment_rpy rosparam.
-
+# Base link frame
+
# If false, translation=0. If true, translation from GPS UTM, set to zero when launched
@@ -24,9 +24,9 @@ imu_attitude_to_tf node has to acknowledge Hector_slam: http://wiki.ros.org/hect
# Filtering factor (1==full correction from GPS applied at each step. 0.1 recommended for smooth behavior)
# 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
-[3.14159265359, 0.0, 3.14159265359]
+[0.0, 0.0, 0.0]
# Initial magnetic north correction angle. Note that this value is later precised by the GPS information
diff --git a/src/gps_gyro_odom_to_tf.cpp b/src/gps_gyro_odom_to_tf.cpp
index 30dd9bd..78f3bbb 100644
--- a/src/gps_gyro_odom_to_tf.cpp
+++ b/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 imu_correction_rpy_(3, 0.0);
std::vector 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("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();
diff --git a/src/imu_and_wheel_odom_node.cpp b/src/imu_and_wheel_odom_node.cpp
index 717243c..377494d 100644
--- a/src/imu_and_wheel_odom_node.cpp
+++ b/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 imu_correction_rpy_(3, 0.0);
std::vector 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("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,
diff --git a/src/imu_attitude_and_fix_to_tf_node.cpp b/src/imu_attitude_and_fix_to_tf_node.cpp
index 5f93e50..d030391 100644
--- a/src/imu_attitude_and_fix_to_tf_node.cpp
+++ b/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 imu_correction_rpy_(3, 0.0);
std::vector 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("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,
diff --git a/src/imu_attitude_to_tf_node.cpp b/src/imu_attitude_to_tf_node.cpp
index 6ba145c..c0437c2 100644
--- a/src/imu_attitude_to_tf_node.cpp
+++ b/src/imu_attitude_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,8 +50,10 @@ tf::StampedTransform transform_;
tf::StampedTransform transformAcc_;
tf::Quaternion tmp_;
tf::Quaternion imu_alignment_;
+tf::Quaternion imu_alignment_correction_;
tf::Quaternion mag_north_correction_;
+std::vector imu_correction_rpy_(3, 0.0);
std::vector imu_alignment_rpy_(3,0.0);
double mag_north_correction_yaw_ = 0;
@@ -71,7 +74,7 @@ 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_;
transform_.setRotation(tmp_);
transform_.stamp_ = imu_msg.header.stamp;
@@ -108,37 +111,79 @@ 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 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.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.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");
+ } 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 IMU correction
+ if (!pn.getParam("imu_correction_rpy", imu_correction_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);
+ 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);
+ }
}
- }
- // 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("imu_topic", n, ros::Duration(1.0));
+ if (imu_msg_ptr == nullptr) {
+ ROS_ERROR("Unable to get IMU frame_id");
+ 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 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,
+ 0.0,
+ mag_north_correction_yaw_);
- // 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
+ // Prepare the transform, set the origin to zero
tfB_ = std::make_unique();
transform_.getOrigin().setX(0.0);
transform_.getOrigin().setY(0.0);