pr2_mechanism_controllers: pr2_mechanism_controllers: pr2

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 "pr2_mechanism_controllers/pr2_odometry.h"
00039 #include "pluginlib/class_list_macros.h"
00040 
00041 PLUGINLIB_DECLARE_CLASS(pr2_mechanism_controllers, Pr2Odometry, controller::Pr2Odometry, pr2_controller_interface::Controller)
00042 
00043 namespace controller {
00044 
00045 
00046   const static double ODOMETRY_THRESHOLD = 1e-4;
00047   const static double MAX_ALLOWABLE_SVD_TIME = 3e-4;
00048 
00049   Pr2Odometry::Pr2Odometry()
00050   {
00051     sequence_ = 0;
00052   }
00053 
00054   Pr2Odometry::~Pr2Odometry()
00055   {
00056   }
00057 
00058   bool Pr2Odometry::init(pr2_mechanism_model::RobotState *robot_state, ros::NodeHandle &node)
00059   {
00060     node_ = node;
00061 
00062     std::string prefix_param;
00063     node.searchParam("tf_prefix", prefix_param);
00064     node.getParam(prefix_param, tf_prefix_);
00065 
00066     node.param("odometer/initial_distance", odometer_distance_, 0.0);
00067     node.param("odometer/initial_angle", odometer_angle_, 0.0);
00068     node.param("odom/initial_x", odom_.x, 0.0);
00069     node.param("odom/initial_y", odom_.y, 0.0);
00070     node.param("odom/initial_yaw", odom_.z, 0.0);
00071 
00072     node.param("publish_tf", publish_tf_, true);
00073     ils_weight_type_ = "Gaussian";
00074     //    node.param<std::string> ("ils_weight_type", ils_weight_type_, "Gaussian");
00075     node.param<int> ("ils_max_iterations", ils_max_iterations_, 3);
00076     node.param<std::string> ("odom_frame", odom_frame_, "odom");
00077     node.param<std::string> ("base_footprint_frame", base_footprint_frame_, "base_footprint");
00078     node.param<std::string> ("base_link_frame", base_link_frame_, "base_link");
00079     //    node.param<double> ("base_link_floor_z_offset",  base_link_floor_z_offset_, 0.051);
00080 
00081     node.param<double> ("x_stddev", sigma_x_, 0.002);
00082     node.param<double> ("y_stddev", sigma_y_, 0.002);
00083     node.param<double> ("rotation_stddev", sigma_theta_, 0.017);
00084 
00085     node.param<double> ("cov_xy", cov_x_y_, 0.0);
00086     node.param<double> ("cov_xrotation", cov_x_theta_, 0.0);
00087     node.param<double> ("cov_yrotation", cov_y_theta_, 0.0);
00088     node.param<bool> ("verbose", verbose_, false);
00089 
00090     node.param("odom_publish_rate", odom_publish_rate_, 30.0);
00091     node.param("odometer_publish_rate", odometer_publish_rate_, 1.0);
00092     node.param("state_publish_rate", state_publish_rate_, 1.0);
00093     node.param("caster_calibration_multiplier", caster_calibration_multiplier_, 1.0);
00094 
00095     if(odom_publish_rate_ <= 0.0)
00096       {
00097         expected_publish_time_ = 0.0;
00098         publish_odom_ = false;
00099       }
00100     else
00101       {
00102         expected_publish_time_ = 1.0/odom_publish_rate_;
00103         publish_odom_ = true;
00104       }
00105 
00106     if(odometer_publish_rate_ <= 0.0)
00107       {
00108         expected_odometer_publish_time_ = 0.0;
00109         publish_odometer_ = false;
00110       }
00111     else
00112       {
00113         expected_odometer_publish_time_ = 1.0/odometer_publish_rate_;
00114         publish_odometer_ = true;
00115       }
00116 
00117     if(state_publish_rate_ <= 0.0)
00118       {
00119         expected_state_publish_time_ = 0.0;
00120         publish_state_ = false;
00121       }
00122     else
00123       {
00124         expected_state_publish_time_ = 1.0/state_publish_rate_;
00125         publish_state_ = true;
00126       }
00127 
00128 
00129 
00130     if(!base_kin_.init(robot_state, node_))
00131       return false;
00132 
00133     for(int i = 0; i < base_kin_.num_casters_; ++i)
00134     {
00135       if(!base_kin_.caster_[i].joint_->calibrated_)
00136       {
00137         ROS_ERROR("The Base odometry could not start because the casters were not calibrated. Relaunch the odometry after you see the caster calibration finish.");
00138         return false; // Casters are not calibrated
00139       }
00140     }
00141 
00142     cbv_rhs_.setZero();
00143     cbv_lhs_.setZero();
00144     cbv_soln_.setZero();
00145     fit_lhs_.setZero();
00146     fit_rhs_.setZero();
00147     fit_soln_.setZero();
00148     fit_residual_.setZero();
00149     odometry_residual_.setZero();
00150     weight_matrix_.setIdentity();
00151 
00152     if(verbose_)
00153       {
00154         matrix_publisher_.reset(new realtime_tools::RealtimePublisher<pr2_mechanism_controllers::OdometryMatrix>(node_,"odometry_matrix", 1));
00155         debug_publisher_.reset(new realtime_tools::RealtimePublisher<pr2_mechanism_controllers::DebugInfo>(node_,"debug", 1));
00156         debug_publisher_->msg_.timing.resize(3);
00157         matrix_publisher_->msg_.m.resize(48);
00158       }
00159 
00160     state_publisher_.reset(new realtime_tools::RealtimePublisher<pr2_mechanism_controllers::BaseOdometryState>(node_,"state", 1));
00161     odometer_publisher_.reset(new realtime_tools::RealtimePublisher<pr2_mechanism_controllers::Odometer>(node_,"odometer", 1));
00162     odometry_publisher_.reset(new realtime_tools::RealtimePublisher<nav_msgs::Odometry>(node_,odom_frame_, 1));
00163     transform_publisher_.reset(new realtime_tools::RealtimePublisher<tf::tfMessage>(node_,"/tf", 1));
00164     transform_publisher_->msg_.transforms.resize(1);
00165 
00166     state_publisher_->msg_.wheel_link_names.resize(base_kin_.num_wheels_);
00167     state_publisher_->msg_.drive_constraint_errors.resize(base_kin_.num_wheels_);
00168     state_publisher_->msg_.longitudinal_slip_constraint_errors.resize(base_kin_.num_wheels_);
00169     return true;
00170   }
00171 
00172   bool Pr2Odometry::isInputValid()
00173   {
00174     return true;
00175 
00176     for(int i=0; i < base_kin_.num_wheels_; i++)
00177       if(isnan(base_kin_.wheel_[i].joint_->velocity_) || isnan(base_kin_.wheel_[i].joint_->velocity_))
00178         return false;
00179 
00180     for(int i=0; i < base_kin_.num_casters_; i++)
00181       if(isnan(base_kin_.caster_[i].joint_->velocity_) || isnan(base_kin_.caster_[i].joint_->velocity_))
00182         return false;
00183 
00184     return true;
00185   }
00186 
00187   void Pr2Odometry::starting()
00188   {
00189     current_time_ = base_kin_.robot_state_->getTime();
00190     last_time_ = base_kin_.robot_state_->getTime();
00191     last_publish_time_ = base_kin_.robot_state_->getTime();
00192     last_transform_publish_time_ = base_kin_.robot_state_->getTime();
00193     last_state_publish_time_ = base_kin_.robot_state_->getTime();
00194     last_odometer_publish_time_ = base_kin_.robot_state_->getTime();
00195   }
00196 
00197   void Pr2Odometry::update()
00198   {
00199     if(!isInputValid())
00200       {
00201         if(verbose_)
00202           debug_publisher_->msg_.input_valid = false;
00203         ROS_DEBUG("Odometry:: Input velocities are invalid");
00204         return;
00205       }
00206     else
00207       {
00208         if(verbose_)
00209           debug_publisher_->msg_.input_valid = true;
00210       }
00211 
00212     current_time_ = base_kin_.robot_state_->getTime();
00213     ros::Time update_start = ros::Time::now();
00214     updateOdometry();
00215     double update_time = (ros::Time::now()-update_start).toSec();
00216     ros::Time publish_start = ros::Time::now();
00217     if(publish_odom_)
00218       publish();
00219     if(publish_odometer_)
00220       publishOdometer();
00221     if(publish_state_)
00222       publishState();
00223     if(publish_tf_)
00224       publishTransform();
00225     double publish_time = (ros::Time::now()-publish_start).toSec();
00226     if(verbose_)
00227       {
00228         debug_publisher_->msg_.timing[0] = update_time;
00229         debug_publisher_->msg_.timing[1] = publish_time;
00230         debug_publisher_->msg_.residual = odometry_residual_max_;
00231         debug_publisher_->msg_.sequence = sequence_;
00232         if(debug_publisher_->trylock())
00233           {
00234             debug_publisher_->unlockAndPublish();
00235           }
00236       }
00237     last_time_ = current_time_;
00238     sequence_++;
00239   }
00240 
00241   void Pr2Odometry::updateOdometry()
00242   {
00243     double dt = (current_time_ - last_time_).toSec();
00244     double theta = odom_.z;
00245     double costh = cos(theta);
00246     double sinth = sin(theta);
00247 
00248     computeBaseVelocity();
00249 
00250     double odom_delta_x = (odom_vel_.linear.x * costh - odom_vel_.linear.y * sinth) * dt;
00251     double odom_delta_y = (odom_vel_.linear.x * sinth + odom_vel_.linear.y * costh) * dt;
00252     double odom_delta_th = odom_vel_.angular.z * dt;
00253 
00254     odom_.x += odom_delta_x;
00255     odom_.y += odom_delta_y;
00256     odom_.z += odom_delta_th;
00257 
00258     ROS_DEBUG("Odometry:: Position: %f, %f, %f",odom_.x,odom_.y,odom_.z);
00259 
00260     odometer_distance_ += sqrt(odom_delta_x * odom_delta_x + odom_delta_y * odom_delta_y);
00261     odometer_angle_ += fabs(odom_delta_th);
00262   }
00263 
00264   void Pr2Odometry::getOdometry(geometry_msgs::Point &odom, geometry_msgs::Twist &odom_vel)
00265   {
00266     odom = odom_;
00267     odom_vel = odom_vel_;
00268     return;
00269   }
00270 
00271   void Pr2Odometry::getOdometryMessage(nav_msgs::Odometry &msg)
00272   {
00273     msg.header.frame_id = odom_frame_;
00274     msg.header.stamp = current_time_;
00275     msg.pose.pose.position.x = odom_.x;
00276     msg.pose.pose.position.y = odom_.y;
00277     msg.pose.pose.position.z = 0.0;
00278 
00279     btQuaternion quat_trans;
00280     quat_trans.setRPY(0.0, 0.0, odom_.z);
00281     msg.pose.pose.orientation.x = quat_trans.x();
00282     msg.pose.pose.orientation.y = quat_trans.y();
00283     msg.pose.pose.orientation.z = quat_trans.z();
00284     msg.pose.pose.orientation.w = quat_trans.w();
00285 
00286     msg.twist.twist = odom_vel_;
00287     /*  msg.twist.linear.x = odom_vel_.x*cos(odom_.z)-odom_vel_.y*sin(odom_.z);
00288         msg.twist.linear.y = odom_vel_.x*sin(odom_.z)+odom_vel_.y*cos(odom_.z);
00289     */
00290     populateCovariance(odometry_residual_max_,msg);
00291   }
00292 
00293   void Pr2Odometry::populateCovariance(const double &residual, nav_msgs::Odometry &msg)
00294   {
00295     // multiplier to scale covariance
00296     // the smaller the residual, the more reliable odom
00297     /*
00298     //this does not seem to work correctly, commenting it out
00299     double odom_multiplier;
00300     if (residual < 0.05)
00301     {
00302     odom_multiplier = ((residual-0.00001)*(0.99999/0.04999))+0.00001;
00303     }
00304     else
00305     {
00306     odom_multiplier = ((residual-0.05)*(19.0/0.15))+1.0;
00307     }
00308     odom_multiplier = fmax(0.00001, fmin(100.0, odom_multiplier));
00309     odom_multiplier *= 2.0;
00310     */
00311     double  odom_multiplier = 1.0;
00312 
00313     if(fabs(odom_vel_.linear.x) <= 1e-8 && fabs(odom_vel_.linear.y) <= 1e-8 && fabs(odom_vel_.angular.z) <= 1e-8)
00314     {
00315     //nav_msgs::Odometry has a 6x6 covariance matrix
00316       msg.pose.covariance[0] = 1e-12;
00317       msg.pose.covariance[7] = 1e-12;
00318       msg.pose.covariance[35] = 1e-12;
00319 
00320       msg.pose.covariance[1] = 1e-12;
00321       msg.pose.covariance[6] = 1e-12;
00322 
00323       msg.pose.covariance[31] = 1e-12;
00324       msg.pose.covariance[11] = 1e-12;
00325 
00326       msg.pose.covariance[30] = 1e-12;
00327       msg.pose.covariance[5] =  1e-12;
00328     }
00329     else
00330     {
00331     //nav_msgs::Odometry has a 6x6 covariance matrix
00332       msg.pose.covariance[0] = odom_multiplier*pow(sigma_x_,2);
00333       msg.pose.covariance[7] = odom_multiplier*pow(sigma_y_,2);
00334       msg.pose.covariance[35] = odom_multiplier*pow(sigma_theta_,2);
00335 
00336       msg.pose.covariance[1] = odom_multiplier*cov_x_y_;
00337       msg.pose.covariance[6] = odom_multiplier*cov_x_y_;
00338 
00339       msg.pose.covariance[31] = odom_multiplier*cov_y_theta_;
00340       msg.pose.covariance[11] = odom_multiplier*cov_y_theta_;
00341 
00342       msg.pose.covariance[30] = odom_multiplier*cov_x_theta_;
00343       msg.pose.covariance[5] =  odom_multiplier*cov_x_theta_;
00344     }
00345 
00346     msg.pose.covariance[14] = DBL_MAX;
00347     msg.pose.covariance[21] = DBL_MAX;
00348     msg.pose.covariance[28] = DBL_MAX;
00349 
00350     msg.twist.covariance = msg.pose.covariance;
00351   }
00352 
00353 
00354   void Pr2Odometry::getOdometry(double &x, double &y, double &yaw, double &vx, double &vy, double &vw)
00355   {
00356     x = odom_.x;
00357     y = odom_.y;
00358     yaw = odom_.z;
00359     vx = odom_vel_.linear.x;
00360     vy = odom_vel_.linear.y;
00361     vw = odom_vel_.angular.z;
00362   }
00363 
00364   void Pr2Odometry::computeBaseVelocity()
00365   {
00366     double steer_angle, wheel_speed, costh, sinth;
00367     geometry_msgs::Twist caster_local_velocity;
00368     geometry_msgs::Twist wheel_local_velocity;
00369     geometry_msgs::Point wheel_position;
00370     for(int i = 0; i < base_kin_.num_wheels_; i++)
00371       {
00372         base_kin_.wheel_[i].updatePosition();
00373         steer_angle = base_kin_.wheel_[i].parent_->joint_->position_;//I'm coming out properly!
00374         wheel_position = base_kin_.wheel_[i].position_;
00375         costh = cos(steer_angle);
00376         sinth = sin(steer_angle);
00377         wheel_speed = getCorrectedWheelSpeed(i);
00378         ROS_DEBUG("Odometry:: Wheel: %s, angle: %f, speed: %f",base_kin_.wheel_[i].link_name_.c_str(),steer_angle,wheel_speed);
00379         cbv_rhs_(i * 2, 0) = base_kin_.wheel_[i].wheel_radius_ * wheel_speed;
00380         cbv_rhs_(i * 2 + 1, 0) = 0;
00381 
00382         cbv_lhs_(i * 2, 0) = costh;
00383         cbv_lhs_(i * 2, 1) = sinth;
00384         cbv_lhs_(i * 2, 2) = -costh * wheel_position.y + sinth * wheel_position.x;
00385         cbv_lhs_(i * 2 + 1, 0) = -sinth;
00386         cbv_lhs_(i * 2 + 1, 1) = costh;
00387         cbv_lhs_(i * 2 + 1, 2) = sinth * wheel_position.y + costh * wheel_position.x;
00388       }
00389     cbv_soln_ = iterativeLeastSquares(cbv_lhs_, cbv_rhs_, ils_weight_type_, ils_max_iterations_);
00390 
00391     odometry_residual_ = cbv_lhs_ * cbv_soln_ - cbv_rhs_;
00392     odometry_residual_max_ = odometry_residual_.array().abs().maxCoeff();
00393     ROS_DEBUG("Odometry:: base velocity: %f, %f, %f",cbv_soln_(0,0), cbv_soln_(1,0), cbv_soln_(2,0));
00394     ROS_DEBUG("Odometry:: odometry residual: %f",odometry_residual_max_);
00395     odom_vel_.linear.x = cbv_soln_(0, 0);
00396     odom_vel_.linear.y = cbv_soln_(1, 0);
00397     odom_vel_.angular.z = cbv_soln_(2, 0);
00398   }
00399 
00400   double Pr2Odometry::getCorrectedWheelSpeed(const int &index)
00401   {
00402     double wheel_speed;
00403     geometry_msgs::Twist caster_local_vel;
00404     geometry_msgs::Twist wheel_local_vel;
00405     caster_local_vel.angular.z = base_kin_.wheel_[index].parent_->joint_->velocity_*caster_calibration_multiplier_;
00406     wheel_local_vel = base_kin_.pointVel2D(base_kin_.wheel_[index].offset_, caster_local_vel);
00407     wheel_speed = base_kin_.wheel_[index].joint_->velocity_ - wheel_local_vel.linear.x / (base_kin_.wheel_[index].wheel_radius_);
00408     return wheel_speed;
00409   }
00410 
00411   OdomMatrix3x1 Pr2Odometry::iterativeLeastSquares(const OdomMatrix16x3 &lhs, const OdomMatrix16x1 &rhs, const std::string &weight_type, const int &max_iter)
00412   {
00413     //    weight_matrix_ = Eigen::MatrixXf::Identity(16, 16);
00414     weight_matrix_.setIdentity();
00415     double svd_time = 0.0;
00416     bool pub_matrix = false;
00417     ros::Time tmp_start;
00418 
00419     for(int i = 0; i < max_iter; i++)
00420       {
00421         fit_lhs_ = weight_matrix_ * lhs;
00422         fit_rhs_ = weight_matrix_ * rhs;
00423 
00424         if(verbose_)
00425           tmp_start = ros::Time::now();
00426         Eigen::JacobiSVD<Eigen::MatrixXf> svdOfFit(fit_lhs_,Eigen::ComputeThinU|Eigen::ComputeThinV);
00427         fit_soln_ = svdOfFit.solve(fit_rhs_);
00428 
00429         ROS_DEBUG("Odometry:: fit_soln_: %f %f %f",fit_soln_(0,0), fit_soln_(1,0), fit_soln_(2,0));
00430 
00431         if(verbose_)
00432           {
00433             svd_time = (ros::Time::now()-tmp_start).toSec();
00434             debug_publisher_->msg_.timing[2] = svd_time;
00435             if((ros::Time::now()-tmp_start).toSec() > MAX_ALLOWABLE_SVD_TIME)
00436               {
00437                 for(int k = 0; k < 48; k++)
00438                   {
00439                     int i_index = k/3;
00440                     int j_index = k - i_index *3;
00441                     matrix_publisher_->msg_.m[i] = fit_lhs_(i_index,j_index);
00442                   }
00443                 pub_matrix = true;
00444               }
00445             if(pub_matrix)
00446               {
00447                 if(matrix_publisher_->trylock())
00448                   matrix_publisher_->unlockAndPublish();
00449                 break;
00450               }
00451           }
00452 
00453         fit_residual_ = rhs - lhs * fit_soln_;
00454         if(odometry_residual_.array().abs().maxCoeff() <= ODOMETRY_THRESHOLD)
00455         {
00456           ROS_DEBUG("Breaking out since odometry looks good");
00457           break;
00458         }
00459         for(int j = 0; j < base_kin_.num_wheels_; j++)
00460           {
00461             int fw = 2 * j;
00462             int sw = fw + 1;
00463             if(fabs(fit_residual_(fw, 0)) > fabs(fit_residual_(sw, 0)))
00464               {
00465                 fit_residual_(fw, 0) = fabs(fit_residual_(fw, 0));
00466                 fit_residual_(sw, 0) = fit_residual_(fw, 0);
00467               }
00468             else
00469               {
00470                 fit_residual_(fw, 0) = fabs(fit_residual_(sw, 0));
00471                 fit_residual_(sw, 0) = fit_residual_(fw, 0);
00472               }
00473           }
00474         weight_matrix_ = findWeightMatrix(fit_residual_, weight_type);
00475 
00476       }
00477     return fit_soln_;
00478   }
00479 
00480   OdomMatrix16x16 Pr2Odometry::findWeightMatrix(const OdomMatrix16x1 &residual, const std::string &weight_type)
00481   {
00482     //    Eigen::MatrixXf w_fit = Eigen::MatrixXf::Identity(16, 16);
00483     w_fit.setIdentity();
00484     double g_sigma = 0.1;
00485 
00486     /*
00487     double epsilon = 0;
00488       if(weight_type == std::string("BubeLagan"))
00489       {
00490         for(int i = 0; i < 2 * base_kin_.num_wheels_; i++)
00491           {
00492             if(fabs(residual(i, 0) > epsilon))
00493               epsilon = fabs(residual(i, 0));
00494           }
00495         epsilon = epsilon / 100.0;
00496         }*/
00497     for(int i = 0; i < 2 * base_kin_.num_wheels_; i++)
00498       {
00499         /*        if(weight_type == std::string("L1norm"))
00500           {
00501             w_fit(i, i) = 1.0 / (1 + sqrt(fabs(residual(i, 0))));
00502           }
00503         else if(weight_type == std::string("fair"))
00504           {
00505             w_fit(i, i) = 1.0 / (1 + fabs(residual(i, 0)));
00506           }
00507         else if(weight_type == std::string("Cauchy"))
00508           {
00509             w_fit(i, i) = 1.0 / (1 + residual(i, 0) * residual(i, 0));
00510           }
00511         else if(weight_type == std::string("BubeLangan"))
00512           {
00513             w_fit(i, i) = 1.0 / pow((1 + pow((residual(i, 0) / epsilon), 2)), 0.25);
00514           }
00515         else if(weight_type == std::string("Gaussian"))
00516         {*/
00517             w_fit(i, i) = sqrt(exp(-pow(residual(i, 0), 2) / (2 * g_sigma * g_sigma)));
00518             /*          }
00519         else // default to fair
00520           {
00521             w_fit(i, i) = 1.0 / (0.1 + sqrt(fabs(residual(i, 0))));
00522             }*/
00523       }
00524     return w_fit;
00525   }
00526 
00527   void Pr2Odometry::publishOdometer()
00528   {
00529     if(fabs((last_odometer_publish_time_ - current_time_).toSec()) < expected_odometer_publish_time_)
00530       return;
00531     if(odometer_publisher_->trylock())
00532     {
00533       odometer_publisher_->msg_.distance = odometer_distance_;
00534       odometer_publisher_->msg_.angle = odometer_angle_;
00535       odometer_publisher_->unlockAndPublish();
00536       last_odometer_publish_time_ = current_time_;
00537     }
00538   }
00539 
00540   void Pr2Odometry::publishState()
00541   {
00542     if(fabs((last_state_publish_time_ - current_time_).toSec()) < expected_state_publish_time_)
00543       return;
00544     if(state_publisher_->trylock())
00545     {
00546       for(int i=0; i < base_kin_.num_wheels_; i++)
00547       {
00548         state_publisher_->msg_.wheel_link_names[i] = base_kin_.wheel_[i].link_name_;
00549         state_publisher_->msg_.drive_constraint_errors[i] = odometry_residual_(2*i,0);
00550         state_publisher_->msg_.longitudinal_slip_constraint_errors[i] = odometry_residual_(2*i+1,0);
00551       }
00552       state_publisher_->msg_.velocity = odom_vel_;
00553       state_publisher_->unlockAndPublish();
00554       last_state_publish_time_ = current_time_;
00555     }
00556   }
00557 
00558   void Pr2Odometry::publish()
00559   {
00560     if(fabs((last_publish_time_ - current_time_).toSec()) < expected_publish_time_)
00561       return;
00562 
00563     if(odometry_publisher_->trylock())
00564       {
00565         getOdometryMessage(odometry_publisher_->msg_);
00566         odometry_publisher_->unlockAndPublish();
00567         last_publish_time_ = current_time_;
00568       }
00569   }
00570 
00571   void Pr2Odometry::publishTransform()
00572   {
00573     if(fabs((last_transform_publish_time_ - current_time_).toSec()) < expected_publish_time_)
00574       return;
00575     if(transform_publisher_->trylock())
00576     {
00577       double x(0.), y(0.0), yaw(0.0), vx(0.0), vy(0.0), vyaw(0.0);
00578       this->getOdometry(x, y, yaw, vx, vy, vyaw);
00579 
00580       geometry_msgs::TransformStamped &out = transform_publisher_->msg_.transforms[0];
00581       out.header.stamp = current_time_;
00582       out.header.frame_id =  tf::resolve(tf_prefix_,base_footprint_frame_);
00583       out.child_frame_id = tf::resolve(tf_prefix_,odom_frame_);
00584       out.transform.translation.x = -x * cos(yaw) - y * sin(yaw);
00585       out.transform.translation.y = +x * sin(yaw) - y * cos(yaw);
00586       out.transform.translation.z = 0;
00587       btQuaternion quat_trans;
00588       quat_trans.setRPY(0.0, 0.0, -yaw);
00589 
00590       out.transform.rotation.x = quat_trans.x();
00591       out.transform.rotation.y = quat_trans.y();
00592       out.transform.rotation.z = quat_trans.z();
00593       out.transform.rotation.w = quat_trans.w();
00594 
00595     /*        geometry_msgs::TransformStamped &out2 = transform_publisher_->msg_.transforms[1];
00596               out2.header.stamp = current_time_;
00597               out2.header.frame_id = base_footprint_frame_;
00598               out2.child_frame_id = base_link_frame_;
00599               out2.transform.translation.x = 0;
00600               out2.transform.translation.y = 0;
00601 
00602               // FIXME: this is the offset between base_link origin and the ideal floor
00603               out2.transform.translation.z = base_link_floor_z_offset_; // FIXME: this is hardcoded, considering we are deprecating base_footprint soon, I will not get this from URDF.
00604 
00605               out2.transform.rotation.x = 0;
00606               out2.transform.rotation.y = 0;
00607               out2.transform.rotation.z = 0;
00608               out2.transform.rotation.w = 1;
00609     */
00610       transform_publisher_->unlockAndPublish();
00611       last_transform_publish_time_ = current_time_;
00612     }
00613   }
00614 } // namespace
pr2_odometry.cpp
