GraftImuTopic.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2013, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 /*
00031  * Author: Chad Rockey
00032  */
00033 
00034 #include <graft/GraftImuTopic.h>
00035 
00036 
00037 GraftImuTopic::GraftImuTopic()
00038 {
00039   for( int i=0; i<9; i++ ) {
00040     orientation_covariance_[i] = 0.0;
00041         angular_velocity_covariance_[i] = 0.0;
00042     linear_acceleration_covariance_[i] = 0.0;
00043   }
00044 }
00045 
00046 
00047 GraftImuTopic::~GraftImuTopic()
00048 {
00049 }
00050 
00051 
00052 void GraftImuTopic::callback(const sensor_msgs::Imu::ConstPtr& msg)
00053 {
00054   msg_ = msg;
00055 }
00056 
00057 
00058 void GraftImuTopic::setName(const std::string& name)
00059 {
00060   name_ = name;
00061 }
00062 
00063 std::string GraftImuTopic::getName()
00064 {
00065   return name_;
00066 }
00067 
00068 void GraftImuTopic::clearMessage()
00069 {
00070   msg_ = sensor_msgs::Imu::ConstPtr();
00071 }
00072 
00073 geometry_msgs::Twist::Ptr twistFromQuaternions(
00074     const geometry_msgs::Quaternion& quat,
00075     const geometry_msgs::Quaternion& last_quat,
00076     const double dt)
00077 {
00078   geometry_msgs::Twist::Ptr out(new geometry_msgs::Twist());
00079 
00080   if (dt < 1e-10)
00081   {
00082     return out;
00083   }
00084 
00085   tf::Quaternion tfq2, tfq1;
00086   tf::quaternionMsgToTF(quat, tfq2);
00087   tf::quaternionMsgToTF(last_quat, tfq1);
00088   tf::Transform tf2(tfq2, tf::Vector3(0, 0, 0));
00089   tf::Transform tf1(tfq1, tf::Vector3(0, 0, 0));
00090 
00091   tf::Transform dtf = tf1.inverse() * tf2;
00092 
00093   out->linear.x = 0;
00094   out->linear.y = 0;
00095   out->linear.y = 0;
00096   double rX, rY, rZ;
00097   dtf.getBasis().getEulerYPR(rZ, rY, rX);
00098   out->angular.x = rX/dt;
00099   out->angular.y = rY/dt;
00100   out->angular.z = rZ/dt;
00101 
00102   return out;
00103 }
00104 
00105 
00106 geometry_msgs::Vector3 accelFromQuaternion(
00107     const geometry_msgs::Quaternion& q,
00108     const double gravity_magnitude)
00109 {
00110   geometry_msgs::Vector3 out;
00111   if (q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w > 1e-10)
00112   {
00113     tf::Quaternion tfq;
00114     tf::quaternionMsgToTF(q, tfq);
00115     tf::Transform tft(tfq, tf::Vector3(0, 0, 0));
00116     tf::Vector3 gravity(0, 0, gravity_magnitude);
00117     tf::vector3TFToMsg(tft.inverse()*gravity, out);
00118   }
00119   return out;
00120 }
00121 
00122 
00123 graft::GraftSensorResidual::Ptr GraftImuTopic::h(const graft::GraftState& state)
00124 {
00125   graft::GraftSensorResidual::Ptr out(new graft::GraftSensorResidual());
00126   out->header = state.header;
00127   out->name = name_;
00128   out->pose = state.pose;
00129   out->twist = state.twist;
00130   out->accel = accelFromQuaternion(state.pose.orientation, 9.81);
00131   return out;
00132 }
00133 
00134 
00135 boost::array<double, 36> largeCovarianceFromSmallCovariance(
00136     const boost::array<double, 9>& angular_velocity_covariance)
00137 {
00138   boost::array<double, 36> out;
00139   for (size_t i = 0; i < out.size(); i++)
00140   {
00141     out[i] = 0;
00142   }
00143   for (size_t i = 0; i < 3; i++)
00144   {
00145     out[i+21] = angular_velocity_covariance[i];
00146   }
00147   for (size_t i = 3; i < 6; i++)
00148   {
00149     out[i+24] = angular_velocity_covariance[i];
00150   }
00151   for (size_t i = 6; i < 9; i++)
00152   {
00153     out[i+27] = angular_velocity_covariance[i];
00154   }
00155   return out;
00156 }
00157 
00158 graft::GraftSensorResidual::Ptr GraftImuTopic::z()
00159 {
00160   if (msg_ == NULL || ros::Time::now() - timeout_ > msg_->header.stamp)
00161   {
00162     ROS_WARN_THROTTLE(5.0, "%s (IMU) timeout", name_.c_str());
00163     return graft::GraftSensorResidual::Ptr();
00164   }
00165 
00166   graft::GraftSensorResidual::Ptr out(new graft::GraftSensorResidual());
00167   out->header = msg_->header;
00168   out->name = name_;
00169 
00170   if (delta_orientation_)
00171   {
00172     if (last_msg_ == NULL)
00173     {
00174       last_msg_ = msg_;
00175       return graft::GraftSensorResidual::Ptr();
00176     }
00177     double dt = (msg_->header.stamp - last_msg_->header.stamp).toSec();
00178     out->twist = *twistFromQuaternions(msg_->orientation, last_msg_->orientation, dt);
00179     last_msg_ = msg_;
00180     if (std::accumulate(angular_velocity_covariance_.begin(),angular_velocity_covariance_.end(),0.0) > 1e-15)
00181     {
00182       // Override message
00183       out->twist_covariance = largeCovarianceFromSmallCovariance(angular_velocity_covariance_);
00184     }
00185     else
00186     {
00187       // Use from message
00188       // Not sure what to do about covariance here...... robot_pose_ekf was strange
00189       out->twist_covariance = largeCovarianceFromSmallCovariance(msg_->orientation_covariance);
00190     }
00191     out->twist_covariance[35] = msg_->orientation_covariance[8];
00192   }
00193   else
00194   {
00195     out->pose.orientation = msg_->orientation;
00196     out->twist.angular = msg_->angular_velocity;
00197     if (std::accumulate(orientation_covariance_.begin(),orientation_covariance_.end(),0.0) > 1e-15)
00198     {
00199       // Override message
00200       out->pose_covariance = largeCovarianceFromSmallCovariance(orientation_covariance_);
00201     }
00202     else
00203     {
00204       // Use from message
00205       out->pose_covariance = largeCovarianceFromSmallCovariance(msg_->orientation_covariance);
00206     }
00207     if (std::accumulate(angular_velocity_covariance_.begin(),angular_velocity_covariance_.end(),0.0) > 1e-15)
00208     {
00209       // Override message
00210       out->twist_covariance = largeCovarianceFromSmallCovariance(angular_velocity_covariance_);
00211     }
00212     else
00213     {
00214       // Use from message
00215       out->twist_covariance = largeCovarianceFromSmallCovariance(msg_->angular_velocity_covariance);
00216     }
00217   }
00218 
00219   out->accel = msg_->linear_acceleration;
00220   if (std::accumulate(linear_acceleration_covariance_.begin(),linear_acceleration_covariance_.end(),0.0) > 1e-15)
00221   {
00222     // Override message
00223     out->accel_covariance = linear_acceleration_covariance_;
00224   }
00225   else
00226   {
00227     // Use from message
00228     out->accel_covariance = msg_->linear_acceleration_covariance;
00229   }
00230   return out;
00231 }
00232 
00233 
00234 void GraftImuTopic::useDeltaOrientation(bool delta_orientation)
00235 {
00236   delta_orientation_ = delta_orientation;
00237 }
00238 
00239 
00240 void GraftImuTopic::setTimeout(double timeout)
00241 {
00242   if (timeout < 1e-10)
00243   {
00244     timeout_ = ros::Duration(1e10);  // No timeout enforced
00245     return;
00246   }
00247   timeout_ = ros::Duration(timeout);
00248 }
00249 
00250 
00251 void GraftImuTopic::setOrientationCovariance(boost::array<double, 9>& cov)
00252 {
00253   orientation_covariance_ = cov;
00254 }
00255 
00256 
00257 void GraftImuTopic::setAngularVelocityCovariance(boost::array<double, 9>& cov)
00258 {
00259   angular_velocity_covariance_ = cov;
00260 }
00261 
00262 
00263 void GraftImuTopic::setLinearAccelerationCovariance(boost::array<double, 9>& cov)
00264 {
00265   linear_acceleration_covariance_ = cov;
00266 }
00267 
00268 
00269 sensor_msgs::Imu::ConstPtr GraftImuTopic::getMsg()
00270 {
00271   return msg_;
00272 }


graft
Author(s): Chad Rockey
autogenerated on Fri Aug 28 2015 10:53:16