rosie_odometry.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 /*
00035  * Author: Sachin Chitta and Matthew Piccoli
00036  */
00037 
00038 #include "ias_mechanism_controllers/rosie_odometry.h"
00039 #include "pluginlib/class_list_macros.h"
00040 
00041 PLUGINLIB_DECLARE_CLASS(ias_mechanism_controllers, RosieOdometry, controller::RosieOdometry, pr2_controller_interface::Controller)
00042 
00043 namespace controller {
00044 
00045 RosieOdometry::RosieOdometry()
00046 {
00047 }
00048 
00049 RosieOdometry::~RosieOdometry()
00050 {
00051 }
00052 
00053 bool RosieOdometry::init(pr2_mechanism_model::RobotState *robot_state, ros::NodeHandle &node)
00054 {
00055   node_ = node;
00056   node.param("odometer/distance", odometer_distance_, 0.0);
00057   node.param("odometer/angle", odometer_angle_, 0.0);
00058   node.param("odom/x", odom_.x, 0.0);
00059   node.param("odom/y", odom_.y, 0.0);
00060   node.param("odom/z", odom_.z, 0.0);
00061 
00062   node.param<std::string> ("ils_weight_type", ils_weight_type_, "Gaussian");
00063   node.param<int> ("ils_max_iterations", ils_max_iterations_, 3);
00064   node.param<std::string> ("odom_frame", odom_frame_, "/odom");
00065   node.param<std::string> ("base_link_frame", base_link_frame_, "/base_link");
00066 
00067   node.param<double> ("sigma_x", sigma_x_, 0.002);
00068   node.param<double> ("sigma_y", sigma_y_, 0.002);
00069   node.param<double> ("sigma_theta", sigma_theta_, 0.017);
00070 
00071   node.param<double> ("cov_x_y", cov_x_y_, 0.0);
00072   node.param<double> ("cov_x_theta", cov_x_theta_, 0.0);
00073   node.param<double> ("cov_y_theta", cov_y_theta_, 0.0);
00074 
00075 
00076   node.param("expected_publish_time", expected_publish_time_, 0.03);
00077 
00078   if(!base_kin_.init(robot_state, node_))
00079     return false;
00080 
00081   cbv_lhs_ = Eigen::MatrixXf::Zero(16, 3);
00082   cbv_rhs_ = Eigen::MatrixXf::Zero(16, 1);
00083   cbv_soln_ = Eigen::MatrixXf::Zero(3, 1);
00084 
00085   fit_lhs_ = Eigen::MatrixXf::Zero(16, 3);
00086   fit_rhs_ = Eigen::MatrixXf::Zero(16, 1);
00087   fit_soln_ = Eigen::MatrixXf::Zero(3, 1);
00088 
00089   fit_residual_ = Eigen::MatrixXf::Zero(16, 1);
00090   odometry_residual_ = Eigen::MatrixXf::Zero(16, 1);
00091 
00092   weight_matrix_ = Eigen::MatrixXf::Identity(16, 16);
00093 
00094   odometry_publisher_.reset(new realtime_tools::RealtimePublisher<nav_msgs::Odometry>(node_,odom_frame_, 1));
00095   transform_publisher_.reset(new realtime_tools::RealtimePublisher<tf::tfMessage>(node_,"/tf", 1));
00096   transform_publisher_->msg_.transforms.resize(1);
00097 
00098   return true;
00099 }
00100 
00101 bool RosieOdometry::initXml(pr2_mechanism_model::RobotState *robot_state, TiXmlElement *config)
00102 {
00103   ros::NodeHandle n(config->Attribute("name"));
00104   return init(robot_state,n);
00105 }
00106 
00107 void RosieOdometry::starting()
00108 {
00109   current_time_ = base_kin_.robot_state_->getTime();
00110   last_time_ = base_kin_.robot_state_->getTime();
00111   last_publish_time_ = base_kin_.robot_state_->getTime();
00112 }
00113 
00114 void RosieOdometry::update()
00115 {
00116   current_time_ = base_kin_.robot_state_->getTime();
00117   updateOdometry();
00118   publish();
00119   last_time_ = current_time_;
00120 }
00121 
00122 void RosieOdometry::updateOdometry()
00123 {
00124   double dt = (current_time_ - last_time_).toSec();
00125   double theta = odom_.z;
00126   double costh = cos(theta);
00127   double sinth = sin(theta);
00128 
00129   computeBaseVelocity();
00130 
00131   double odom_delta_x = (odom_vel_.linear.x * costh - odom_vel_.linear.y * sinth) * dt;
00132   double odom_delta_y = (odom_vel_.linear.x * sinth + odom_vel_.linear.y * costh) * dt;
00133   double odom_delta_th = odom_vel_.angular.z * dt;
00134 
00135   odom_.x += odom_delta_x;
00136   odom_.y += odom_delta_y;
00137   odom_.z += odom_delta_th;
00138 
00139   odometer_distance_ += sqrt(odom_delta_x * odom_delta_x + odom_delta_y * odom_delta_y);
00140   odometer_angle_ += fabs(odom_delta_th);
00141 }
00142 
00143 void RosieOdometry::getOdometry(geometry_msgs::Point &odom, geometry_msgs::Twist &odom_vel)
00144 {
00145   odom = odom_;
00146   odom_vel = odom_vel_;
00147   return;
00148 }
00149 
00150 void RosieOdometry::getOdometryMessage(nav_msgs::Odometry &msg)
00151 {
00152   msg.header.frame_id = odom_frame_;
00153   msg.header.stamp = current_time_;
00154   msg.pose.pose.position.x = odom_.x;
00155   msg.pose.pose.position.y = odom_.y;
00156   msg.pose.pose.position.z = 0.0;
00157 
00158   tf::Quaternion quat_trans = tf::Quaternion(odom_.z, 0.0, 0.0);
00159   msg.pose.pose.orientation.x = quat_trans.x();
00160   msg.pose.pose.orientation.y = quat_trans.y();
00161   msg.pose.pose.orientation.z = quat_trans.z();
00162   msg.pose.pose.orientation.w = quat_trans.w();
00163 
00164   msg.twist.twist = odom_vel_;
00165 /*  msg.twist.linear.x = odom_vel_.x*cos(odom_.z)-odom_vel_.y*sin(odom_.z);
00166   msg.twist.linear.y = odom_vel_.x*sin(odom_.z)+odom_vel_.y*cos(odom_.z);
00167 */
00168   populateCovariance(odometry_residual_max_,msg);
00169 }
00170 
00171 void RosieOdometry::populateCovariance(double residual, nav_msgs::Odometry &msg)
00172 {
00173   // multiplier to scale covariance
00174   // the smaller the residual, the more reliable odom
00175   double odom_multiplier;
00176   if (residual < 0.05)
00177   {
00178     odom_multiplier = ((residual-0.00001)*(0.99999/0.04999))+0.00001;
00179   }
00180   else
00181   {
00182     odom_multiplier = ((residual-0.05)*(19.0/0.15))+1.0;
00183   }
00184   odom_multiplier = fmax(0.00001, fmin(100.0, odom_multiplier));
00185   odom_multiplier *= 2.0;
00186 
00187   //nav_msgs::Odometry has a 6x6 covariance matrix
00188   msg.pose.covariance[0] = odom_multiplier*pow(sigma_x_,2);
00189   msg.pose.covariance[7] = odom_multiplier*pow(sigma_y_,2);
00190   msg.pose.covariance[35] = odom_multiplier*pow(sigma_theta_,2);
00191 
00192   msg.pose.covariance[1] = odom_multiplier*cov_x_y_;
00193   msg.pose.covariance[6] = odom_multiplier*cov_x_y_;
00194 
00195   msg.pose.covariance[31] = odom_multiplier*cov_y_theta_;
00196   msg.pose.covariance[11] = odom_multiplier*cov_y_theta_;
00197 
00198   msg.pose.covariance[30] = odom_multiplier*cov_x_theta_;
00199   msg.pose.covariance[5] =  odom_multiplier*cov_x_theta_;
00200 
00201   msg.pose.covariance[14] = DBL_MAX;
00202   msg.pose.covariance[21] = DBL_MAX;
00203   msg.pose.covariance[28] = DBL_MAX;
00204 
00205   msg.twist.covariance = msg.pose.covariance;
00206 }
00207 
00208 
00209 void RosieOdometry::getOdometry(double &x, double &y, double &yaw, double &vx, double &vy, double &vw)
00210 {
00211   x = odom_.x;
00212   y = odom_.y;
00213   yaw = odom_.z;
00214   vx = odom_vel_.linear.x;
00215   vy = odom_vel_.linear.y;
00216   vw = odom_vel_.angular.z;
00217 }
00218 
00219 void RosieOdometry::computeBaseVelocity()
00220 {
00221   double steer_angle, wheel_speed, costh, sinth;
00222   geometry_msgs::Twist caster_local_velocity;
00223   geometry_msgs::Twist wheel_local_velocity;
00224   geometry_msgs::Point wheel_position;
00225   for(int i = 0; i < base_kin_.num_wheels_; i++)
00226   {
00227     base_kin_.wheel_[i].updatePosition();
00228     steer_angle = base_kin_.wheel_[i].parent_->joint_->position_;//I'm coming out properly!
00229     wheel_position = base_kin_.wheel_[i].position_;
00230     costh = cos(steer_angle);
00231     sinth = sin(steer_angle);
00232     wheel_speed = getCorrectedWheelSpeed(i);
00233     cbv_rhs_(i * 2, 0) = base_kin_.wheel_[i].wheel_radius_ * wheel_speed; 
00234     cbv_rhs_(i * 2 + 1, 0) = 0;
00235 
00236     cbv_lhs_(i * 2, 0) = costh;
00237     cbv_lhs_(i * 2, 1) = sinth;
00238     cbv_lhs_(i * 2, 2) = -costh * wheel_position.y + sinth * wheel_position.x;
00239     cbv_lhs_(i * 2 + 1, 0) = -sinth;
00240     cbv_lhs_(i * 2 + 1, 1) = costh;
00241     cbv_lhs_(i * 2 + 1, 2) = sinth * wheel_position.y + costh * wheel_position.x;
00242   }
00243   cbv_soln_ = iterativeLeastSquares(cbv_lhs_, cbv_rhs_, ils_weight_type_, ils_max_iterations_);
00244 
00245   odometry_residual_ = cbv_lhs_ * cbv_soln_ - cbv_rhs_;
00246   odometry_residual_max_ = odometry_residual_.cwise().abs().maxCoeff();
00247 
00248   odom_vel_.linear.x = cbv_soln_(0, 0);
00249   odom_vel_.linear.y = cbv_soln_(1, 0);
00250   odom_vel_.angular.z = cbv_soln_(2, 0);
00251 }
00252 
00253 double RosieOdometry::getCorrectedWheelSpeed(int index)
00254 {
00255   double wheel_speed;
00256   geometry_msgs::Twist caster_local_vel;
00257   geometry_msgs::Twist wheel_local_vel;
00258   caster_local_vel.angular.z = base_kin_.wheel_[index].parent_->joint_->velocity_;
00259   wheel_local_vel = base_kin_.pointVel2D(base_kin_.wheel_[index].offset_, caster_local_vel);
00260   wheel_speed = base_kin_.wheel_[index].joint_->velocity_ - wheel_local_vel.linear.x / (base_kin_.wheel_[index].wheel_radius_);  
00261   return wheel_speed;
00262 }
00263 
00264 Eigen::MatrixXf RosieOdometry::iterativeLeastSquares(Eigen::MatrixXf lhs, Eigen::MatrixXf rhs, std::string weight_type, int max_iter)
00265 {
00266   weight_matrix_ = Eigen::MatrixXf::Identity(16, 16);
00267   for(int i = 0; i < max_iter; i++)
00268   {
00269     fit_lhs_ = weight_matrix_ * lhs;
00270     fit_rhs_ = weight_matrix_ * rhs;
00271 
00272     fit_lhs_.svd().solve(fit_rhs_, &fit_soln_);
00273     fit_residual_ = rhs - lhs * fit_soln_;
00274 
00275     for(int j = 0; j < base_kin_.num_wheels_; j++)
00276     {
00277       int fw = 2 * j;
00278       int sw = fw + 1;
00279       if(fabs(fit_residual_(fw, 0)) > fabs(fit_residual_(sw, 0)))
00280       {
00281         fit_residual_(fw, 0) = fabs(fit_residual_(fw, 0));
00282         fit_residual_(sw, 0) = fit_residual_(fw, 0);
00283       }
00284       else
00285       {
00286         fit_residual_(fw, 0) = fabs(fit_residual_(sw, 0));
00287         fit_residual_(sw, 0) = fit_residual_(fw, 0);
00288       }
00289     }
00290     weight_matrix_ = findWeightMatrix(fit_residual_, weight_type);
00291   }
00292   return fit_soln_;
00293 }
00294 
00295 Eigen::MatrixXf RosieOdometry::findWeightMatrix(Eigen::MatrixXf residual, std::string weight_type)
00296 {
00297   Eigen::MatrixXf w_fit = Eigen::MatrixXf::Identity(16, 16);
00298   double epsilon = 0;
00299   double g_sigma = 0.1;
00300 
00301   if(weight_type == std::string("BubeLagan"))
00302   {
00303     for(int i = 0; i < 2 * base_kin_.num_wheels_; i++)
00304     {
00305       if(fabs(residual(i, 0) > epsilon))
00306         epsilon = fabs(residual(i, 0));
00307     }
00308     epsilon = epsilon / 100.0;
00309   }
00310   for(int i = 0; i < 2 * base_kin_.num_wheels_; i++)
00311   {
00312     if(weight_type == std::string("L1norm"))
00313     {
00314       w_fit(i, i) = 1.0 / (1 + sqrt(fabs(residual(i, 0))));
00315     }
00316     else if(weight_type == std::string("fair"))
00317     {
00318       w_fit(i, i) = 1.0 / (1 + fabs(residual(i, 0)));
00319     }
00320     else if(weight_type == std::string("Cauchy"))
00321     {
00322       w_fit(i, i) = 1.0 / (1 + residual(i, 0) * residual(i, 0));
00323     }
00324     else if(weight_type == std::string("BubeLangan"))
00325     {
00326       w_fit(i, i) = 1.0 / pow((1 + pow((residual(i, 0) / epsilon), 2)), 0.25);
00327     }
00328     else if(weight_type == std::string("Gaussian"))
00329     {
00330       w_fit(i, i) = sqrt(exp(-pow(residual(i, 0), 2) / (2 * g_sigma * g_sigma)));
00331     }
00332     else // default to fair
00333     {
00334       w_fit(i, i) = 1.0 / (0.1 + sqrt(fabs(residual(i, 0))));
00335     }
00336   }
00337   return w_fit;
00338 }
00339 
00340 void RosieOdometry::publish()
00341 {
00342   if(fabs((last_publish_time_ - current_time_).toSec()) < expected_publish_time_)
00343     return;
00344 
00345   if(odometry_publisher_->trylock())
00346   {
00347     getOdometryMessage(odometry_publisher_->msg_);
00348     odometry_publisher_->unlockAndPublish();
00349   }
00350 
00351   if(transform_publisher_->trylock())
00352   {
00353 
00354     double x(0.), y(0.0), yaw(0.0), vx(0.0), vy(0.0), vyaw(0.0);
00355     this->getOdometry(x, y, yaw, vx, vy, vyaw);
00356 
00357     geometry_msgs::TransformStamped &out = transform_publisher_->msg_.transforms[0];
00358     out.header.stamp = current_time_;
00359     out.header.frame_id = odom_frame_;
00360     out.child_frame_id = base_link_frame_;
00361     out.transform.translation.x = x;
00362     out.transform.translation.y = y;
00363     out.transform.translation.z = 0;
00364     tf::Quaternion quat_trans = tf::Quaternion(yaw, 0.0, 0.0);
00365 
00366     out.transform.rotation.x = quat_trans.x();
00367     out.transform.rotation.y = quat_trans.y();
00368     out.transform.rotation.z = quat_trans.z();
00369     out.transform.rotation.w = quat_trans.w();
00370 
00371     transform_publisher_->unlockAndPublish();
00372   }
00373 
00374   last_publish_time_ = current_time_;
00375 
00376 }
00377 } // namespace
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


ias_mechanism_controllers
Author(s): Lorenz Moesenlechner, Ingo Kresse
autogenerated on Thu May 23 2013 10:34:13