00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #include "imu_complementary_filter/complementary_filter_ros.h"
00033
00034 #include <std_msgs/Float64.h>
00035 #include <std_msgs/Bool.h>
00036
00037 namespace imu_tools {
00038
00039 ComplementaryFilterROS::ComplementaryFilterROS(
00040 const ros::NodeHandle& nh,
00041 const ros::NodeHandle& nh_private):
00042 nh_(nh),
00043 nh_private_(nh_private),
00044 initialized_filter_(false)
00045 {
00046 ROS_INFO("Starting ComplementaryFilterROS");
00047 initializeParams();
00048
00049 int queue_size = 5;
00050
00051
00052 imu_publisher_ = nh_.advertise<sensor_msgs::Imu>(ros::names::resolve("imu") + "/data", queue_size);
00053
00054 if (publish_debug_topics_)
00055 {
00056 rpy_publisher_ = nh_.advertise<geometry_msgs::Vector3Stamped>(
00057 ros::names::resolve("imu") + "/rpy/filtered", queue_size);
00058
00059 if (filter_.getDoBiasEstimation())
00060 {
00061 state_publisher_ = nh_.advertise<std_msgs::Bool>(
00062 ros::names::resolve("imu") + "/steady_state", queue_size);
00063 }
00064 }
00065
00066
00067 imu_subscriber_.reset(new ImuSubscriber(nh_, ros::names::resolve("imu") + "/data_raw", queue_size));
00068
00069
00070 if (use_mag_)
00071 {
00072 mag_subscriber_.reset(new MagSubscriber(nh_, ros::names::resolve("imu") + "/mag", queue_size));
00073
00074 sync_.reset(new Synchronizer(
00075 SyncPolicy(queue_size), *imu_subscriber_, *mag_subscriber_));
00076 sync_->registerCallback(
00077 boost::bind(&ComplementaryFilterROS::imuMagCallback, this, _1, _2));
00078 }
00079 else
00080 {
00081 imu_subscriber_->registerCallback(
00082 &ComplementaryFilterROS::imuCallback, this);
00083 }
00084 }
00085
00086 ComplementaryFilterROS::~ComplementaryFilterROS()
00087 {
00088 ROS_INFO("Destroying ComplementaryFilterROS");
00089 }
00090
00091 void ComplementaryFilterROS::initializeParams()
00092 {
00093 double gain_acc;
00094 double gain_mag;
00095 bool do_bias_estimation;
00096 double bias_alpha;
00097 bool do_adaptive_gain;
00098
00099 if (!nh_private_.getParam ("fixed_frame", fixed_frame_))
00100 fixed_frame_ = "odom";
00101 if (!nh_private_.getParam ("use_mag", use_mag_))
00102 use_mag_ = false;
00103 if (!nh_private_.getParam ("publish_tf", publish_tf_))
00104 publish_tf_ = false;
00105 if (!nh_private_.getParam ("reverse_tf", reverse_tf_))
00106 reverse_tf_ = false;
00107 if (!nh_private_.getParam ("constant_dt", constant_dt_))
00108 constant_dt_ = 0.0;
00109 if (!nh_private_.getParam ("publish_debug_topics", publish_debug_topics_))
00110 publish_debug_topics_ = false;
00111 if (!nh_private_.getParam ("gain_acc", gain_acc))
00112 gain_acc = 0.01;
00113 if (!nh_private_.getParam ("gain_mag", gain_mag))
00114 gain_mag = 0.01;
00115 if (!nh_private_.getParam ("do_bias_estimation", do_bias_estimation))
00116 do_bias_estimation = true;
00117 if (!nh_private_.getParam ("bias_alpha", bias_alpha))
00118 bias_alpha = 0.01;
00119 if (!nh_private_.getParam ("do_adaptive_gain", do_adaptive_gain))
00120 do_adaptive_gain = true;
00121
00122 filter_.setDoBiasEstimation(do_bias_estimation);
00123 filter_.setDoAdaptiveGain(do_adaptive_gain);
00124
00125 if(!filter_.setGainAcc(gain_acc))
00126 ROS_WARN("Invalid gain_acc passed to ComplementaryFilter.");
00127 if (use_mag_)
00128 {
00129 if(!filter_.setGainMag(gain_mag))
00130 ROS_WARN("Invalid gain_mag passed to ComplementaryFilter.");
00131 }
00132 if (do_bias_estimation)
00133 {
00134 if(!filter_.setBiasAlpha(bias_alpha))
00135 ROS_WARN("Invalid bias_alpha passed to ComplementaryFilter.");
00136 }
00137
00138
00139 if (constant_dt_ < 0.0)
00140 {
00141
00142
00143 ROS_WARN("constant_dt parameter is %f, must be >= 0.0. Setting to 0.0", constant_dt_);
00144 constant_dt_ = 0.0;
00145 }
00146 }
00147
00148 void ComplementaryFilterROS::imuCallback(const ImuMsg::ConstPtr& imu_msg_raw)
00149 {
00150 const geometry_msgs::Vector3& a = imu_msg_raw->linear_acceleration;
00151 const geometry_msgs::Vector3& w = imu_msg_raw->angular_velocity;
00152 const ros::Time& time = imu_msg_raw->header.stamp;
00153
00154
00155 if (!initialized_filter_)
00156 {
00157 time_prev_ = time;
00158 initialized_filter_ = true;
00159 return;
00160 }
00161
00162
00163 double dt;
00164 if (constant_dt_ > 0.0)
00165 dt = constant_dt_;
00166 else
00167 dt = (time - time_prev_).toSec();
00168
00169 time_prev_ = time;
00170
00171
00172 filter_.update(a.x, a.y, a.z, w.x, w.y, w.z, dt);
00173
00174
00175 publish(imu_msg_raw);
00176 }
00177
00178 void ComplementaryFilterROS::imuMagCallback(const ImuMsg::ConstPtr& imu_msg_raw,
00179 const MagMsg::ConstPtr& mag_msg)
00180 {
00181 const geometry_msgs::Vector3& a = imu_msg_raw->linear_acceleration;
00182 const geometry_msgs::Vector3& w = imu_msg_raw->angular_velocity;
00183 const geometry_msgs::Vector3& m = mag_msg->magnetic_field;
00184 const ros::Time& time = imu_msg_raw->header.stamp;
00185
00186
00187 if (!initialized_filter_)
00188 {
00189 time_prev_ = time;
00190 initialized_filter_ = true;
00191 return;
00192 }
00193
00194
00195 double dt = (time - time_prev_).toSec();
00196 time_prev_ = time;
00197
00198
00199
00200 if (isnan(m.x) || isnan(m.y) || isnan(m.z))
00201 filter_.update(a.x, a.y, a.z, w.x, w.y, w.z, dt);
00202 else
00203 filter_.update(a.x, a.y, a.z, w.x, w.y, w.z, m.x, m.y, m.z, dt);
00204
00205
00206
00207
00208
00209 publish(imu_msg_raw);
00210 }
00211
00212 tf::Quaternion ComplementaryFilterROS::hamiltonToTFQuaternion(
00213 double q0, double q1, double q2, double q3) const
00214 {
00215
00216
00217 return tf::Quaternion(q1, q2, q3, q0);
00218 }
00219
00220 void ComplementaryFilterROS::publish(
00221 const sensor_msgs::Imu::ConstPtr& imu_msg_raw)
00222 {
00223
00224 double q0, q1, q2, q3;
00225 filter_.getOrientation(q0, q1, q2, q3);
00226 tf::Quaternion q = hamiltonToTFQuaternion(q0, q1, q2, q3);
00227
00228
00229 boost::shared_ptr<sensor_msgs::Imu> imu_msg =
00230 boost::make_shared<sensor_msgs::Imu>(*imu_msg_raw);
00231 tf::quaternionTFToMsg(q, imu_msg->orientation);
00232
00233
00234 if (filter_.getDoBiasEstimation())
00235 {
00236 imu_msg->angular_velocity.x -= filter_.getAngularVelocityBiasX();
00237 imu_msg->angular_velocity.y -= filter_.getAngularVelocityBiasY();
00238 imu_msg->angular_velocity.z -= filter_.getAngularVelocityBiasZ();
00239 }
00240
00241 imu_publisher_.publish(imu_msg);
00242
00243 if (publish_debug_topics_)
00244 {
00245
00246 geometry_msgs::Vector3Stamped rpy;
00247 rpy.header = imu_msg_raw->header;
00248
00249 tf::Matrix3x3 M;
00250 M.setRotation(q);
00251 M.getRPY(rpy.vector.x, rpy.vector.y, rpy.vector.z);
00252 rpy_publisher_.publish(rpy);
00253
00254
00255 if (filter_.getDoBiasEstimation())
00256 {
00257 std_msgs::Bool state_msg;
00258 state_msg.data = filter_.getSteadyState();
00259 state_publisher_.publish(state_msg);
00260 }
00261 }
00262
00263 if (publish_tf_)
00264 {
00265
00266 tf::Transform transform;
00267 transform.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
00268 transform.setRotation(q);
00269
00270 if (reverse_tf_)
00271 {
00272 tf_broadcaster_.sendTransform(
00273 tf::StampedTransform(transform.inverse(),
00274 imu_msg_raw->header.stamp,
00275 imu_msg_raw->header.frame_id,
00276 fixed_frame_));
00277 }
00278 else
00279 {
00280 tf_broadcaster_.sendTransform(
00281 tf::StampedTransform(transform,
00282 imu_msg_raw->header.stamp,
00283 fixed_frame_,
00284 imu_msg_raw->header.frame_id));
00285 }
00286 }
00287 }
00288
00289 }