GraftOdometryTopic.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/GraftOdometryTopic.h>
00035 
00036 GraftOdometryTopic::GraftOdometryTopic():
00037     absolute_pose_(false), delta_pose_(false), use_velocities_(false), timeout_(1.0)
00038 {
00039   for( int i=0; i<36; i++ ) {
00040     pose_covariance_[i] = 0.0;
00041     twist_covariance_[i] = 0.0;
00042   }
00043 }
00044 
00045 
00046 GraftOdometryTopic::~GraftOdometryTopic()
00047 {
00048 }
00049 
00050 
00051 void GraftOdometryTopic::callback(const nav_msgs::Odometry::ConstPtr& msg)
00052 {
00053   msg_ = msg;
00054 }
00055 
00056 
00057 void GraftOdometryTopic::setName(const std::string& name)
00058 {
00059   name_ = name;
00060 }
00061 
00062 
00063 std::string GraftOdometryTopic::getName()
00064 {
00065   return name_;
00066 }
00067 
00068 
00069 void GraftOdometryTopic::clearMessage()
00070 {
00071   msg_ = nav_msgs::Odometry::ConstPtr();
00072 }
00073 
00074 
00075 geometry_msgs::Twist::Ptr twistFromPoses(
00076     const geometry_msgs::Pose& pose,
00077     const geometry_msgs::Pose& last_pose,
00078     const double dt)
00079 {
00080   geometry_msgs::Twist::Ptr out(new geometry_msgs::Twist());
00081 
00082   if (dt < 1e-10)
00083   {
00084     return out;
00085   }
00086 
00087   tf::Quaternion tfq2, tfq1;
00088   tf::quaternionMsgToTF(pose.orientation, tfq2);
00089   tf::quaternionMsgToTF(last_pose.orientation, tfq1);
00090   tf::Transform tf2(tfq2, tf::Vector3(pose.position.x, pose.position.y, pose.position.z));
00091   tf::Transform tf1(tfq1, tf::Vector3(last_pose.position.x, last_pose.position.y, last_pose.position.z));
00092 
00093   tf::Transform dtf = tf1.inverse() * tf2;
00094 
00095   out->linear.x = dtf.getOrigin().getX()/dt;
00096   out->linear.y = dtf.getOrigin().getY()/dt;
00097   out->linear.y = dtf.getOrigin().getZ()/dt;
00098   double rX, rY, rZ;
00099   dtf.getBasis().getEulerYPR(rZ, rY, rX);
00100   out->angular.x = rX/dt;
00101   out->angular.y = rY/dt;
00102   out->angular.z = rZ/dt;
00103 
00104   return out;
00105 }
00106 
00107 
00108 graft::GraftSensorResidual::Ptr GraftOdometryTopic::h(const graft::GraftState& state)
00109 {
00110   graft::GraftSensorResidual::Ptr out(new graft::GraftSensorResidual());
00111   out->header = state.header;
00112   out->name = name_;
00113   out->pose = state.pose;
00114   out->twist = state.twist;
00115   return out;
00116 }
00117 
00118 
00119 graft::GraftSensorResidual::Ptr GraftOdometryTopic::z()
00120 {
00121   if (msg_ == NULL || ros::Time::now() - timeout_ > msg_->header.stamp)
00122   {
00123     ROS_WARN_THROTTLE(5.0, "%s (Odometry) timeout", name_.c_str());
00124     return graft::GraftSensorResidual::Ptr();
00125   }
00126 
00127   graft::GraftSensorResidual::Ptr out(new graft::GraftSensorResidual());
00128   out->header = msg_->header;
00129   out->name = name_;
00130 
00131   if (delta_pose_)
00132   {
00133     if (last_msg_ == NULL)
00134     {
00135       last_msg_ = msg_;
00136       return graft::GraftSensorResidual::Ptr();
00137     }
00138     double dt = (msg_->header.stamp - last_msg_->header.stamp).toSec();
00139     out->twist = *twistFromPoses(msg_->pose.pose, last_msg_->pose.pose, dt);
00140     last_msg_ = msg_;
00141     if (std::accumulate(twist_covariance_.begin(),twist_covariance_.end(),0.0) > 1e-15)
00142     {
00143       // Override message
00144       out->twist_covariance = twist_covariance_;
00145     }
00146     else
00147     {
00148       // Use from message
00149       // Not sure what to do about covariance here...... robot_pose_ekf was strange
00150       out->twist_covariance = msg_->pose.covariance;
00151     }
00152   }
00153   else if(use_velocities_)
00154   {
00155     out->twist = msg_->twist.twist;
00156     if (std::accumulate(twist_covariance_.begin(),twist_covariance_.end(),0.0) > 1e-15)
00157     {
00158       // Override message
00159       out->twist_covariance = twist_covariance_;
00160     }
00161     else
00162     {
00163       // Use from message
00164       out->twist_covariance = msg_->twist.covariance;
00165     }
00166   }
00167 
00168   if (absolute_pose_ && !delta_pose_)  // Delta Pose and Absolute Pose are incompatible
00169   {
00170     out->pose = msg_->pose.pose;
00171     if (std::accumulate(pose_covariance_.begin(),pose_covariance_.end(),0.0) > 1e-15)
00172     {
00173       // Override message
00174       out->pose_covariance = pose_covariance_;
00175     }
00176     else
00177     {
00178       // Use from message
00179       out->pose_covariance = msg_->pose.covariance;
00180     }
00181   }
00182   return out;
00183 }
00184 
00185 
00186 void GraftOdometryTopic::setTimeout(double timeout)
00187 {
00188   if (timeout < 1e-10)
00189   {
00190     timeout_ = ros::Duration(1e10);  // No timeout enforced
00191     return;
00192   }
00193   timeout_ = ros::Duration(timeout);
00194 }
00195 
00196 
00197 void GraftOdometryTopic::setPoseCovariance(boost::array<double, 36>& cov)
00198 {
00199   pose_covariance_ = cov;
00200 }
00201 
00202 
00203 void GraftOdometryTopic::setTwistCovariance(boost::array<double, 36>& cov)
00204 {
00205   twist_covariance_ = cov;
00206 }
00207 
00208 
00209 void GraftOdometryTopic::useAbsolutePose(bool absolute_pose)
00210 {
00211   absolute_pose_ = absolute_pose;
00212 }
00213 
00214 
00215 void GraftOdometryTopic::useDeltaPose(bool delta_pose)
00216 {
00217   delta_pose_ = delta_pose;
00218 }
00219 
00220 
00221 void GraftOdometryTopic::useVelocities(bool use_velocities)
00222 {
00223   use_velocities_ = use_velocities;
00224 }
00225 
00226 
00227 nav_msgs::Odometry::ConstPtr GraftOdometryTopic::getMsg()
00228 {
00229   return msg_;
00230 }


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