Go to the documentation of this file.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
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
00183 out->twist_covariance = largeCovarianceFromSmallCovariance(angular_velocity_covariance_);
00184 }
00185 else
00186 {
00187
00188
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
00200 out->pose_covariance = largeCovarianceFromSmallCovariance(orientation_covariance_);
00201 }
00202 else
00203 {
00204
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
00210 out->twist_covariance = largeCovarianceFromSmallCovariance(angular_velocity_covariance_);
00211 }
00212 else
00213 {
00214
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
00223 out->accel_covariance = linear_acceleration_covariance_;
00224 }
00225 else
00226 {
00227
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);
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 }