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
00035
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;
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
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
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_;
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 }