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


pr2_mechanism_controllers
Author(s): Sachin Chita, John Hsu, Melonee Wise
autogenerated on Thu Aug 27 2015 14:22:52