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 #include <hector_pose_estimation/pose_estimation.h>
00030 #include <hector_pose_estimation/system/imu_input.h>
00031
00032 namespace hector_pose_estimation {
00033
00034 namespace {
00035 static PoseEstimation *the_instance = 0;
00036 }
00037
00038 PoseEstimation::PoseEstimation(const SystemPtr& system)
00039 : system_(system)
00040 , state_is_dirty_(true)
00041 , covariance_is_dirty_(true)
00042 , status_()
00043 , measurement_status_()
00044 , rate_(new Rate("rate"))
00045 , gravity_(new Gravity ("gravity"))
00046 , zerorate_(new ZeroRate("zerorate"))
00047
00048 {
00049 if (!the_instance) the_instance = this;
00050
00051 world_frame_ = "world";
00052 nav_frame_ = "nav";
00053 base_frame_ = "base_link";
00054 stabilized_frame_ = "base_stabilized";
00055 footprint_frame_ = "base_footprint";
00056
00057 alignment_time_ = 0.0;
00058
00059 parameters().add("world_frame", world_frame_);
00060 parameters().add("nav_frame", nav_frame_);
00061 parameters().add("base_frame", base_frame_);
00062 parameters().add("stabilized_frame", stabilized_frame_);
00063 parameters().add("footprint_frame", footprint_frame_);
00064 parameters().add("position_frame", position_frame_);
00065 parameters().add(global_reference_.parameters());
00066 parameters().add("alignment_time", alignment_time_);
00067
00068
00069 addMeasurement(rate_);
00070 addMeasurement(gravity_);
00071 addMeasurement(zerorate_);
00072
00073 }
00074
00075 PoseEstimation::~PoseEstimation()
00076 {
00077 cleanup();
00078 }
00079
00080 PoseEstimation *PoseEstimation::Instance() {
00081 if (!the_instance) the_instance = new PoseEstimation();
00082 return the_instance;
00083 }
00084
00085 bool PoseEstimation::init()
00086 {
00087
00088 globalReference()->updated();
00089
00090
00091 if (!system_) return false;
00092
00093
00094 if (!system_->init()) return false;
00095
00096
00097 for(Measurements::iterator it = measurements_.begin(); it != measurements_.end(); ++it)
00098 if (!(*it)->init()) return false;
00099
00100
00101 reset();
00102
00103 return true;
00104 }
00105
00106 void PoseEstimation::cleanup()
00107 {
00108
00109 if (filter_) filter_.reset();
00110
00111
00112 system_->cleanup();
00113
00114
00115 for(Measurements::iterator it = measurements_.begin(); it != measurements_.end(); ++it) (*it)->cleanup();
00116 }
00117
00118 void PoseEstimation::reset()
00119 {
00120
00121 if (filter_) cleanup();
00122
00123
00124 if (!system_) return;
00125
00126
00127 filter_.reset(new BFL::ExtendedKalmanFilter(system_->getPrior()));
00128 updated();
00129
00130
00131 alignment_start_ = ros::Time();
00132 if (alignment_time_ > 0) {
00133 status_ = STATE_ALIGNMENT;
00134 } else {
00135 status_ = static_cast<SystemStatus>(0);
00136 }
00137 measurement_status_ = 0;
00138
00139
00140 system_->reset();
00141 for(Measurements::iterator it = measurements_.begin(); it != measurements_.end(); ++it) (*it)->reset();
00142 }
00143
00144 void PoseEstimation::update(const SystemInput& input, ros::Time new_timestamp)
00145 {
00146 ros::Duration dt;
00147
00148
00149 if (!system_) return;
00150
00151
00152 system_->setInput(input);
00153 if (!timestamp_.isZero()) dt = new_timestamp - timestamp_;
00154 timestamp_ = new_timestamp;
00155
00156
00157 update(dt.toSec());
00158 }
00159
00160 void PoseEstimation::update(double dt)
00161 {
00162
00163 if (dt < -1.0)
00164 reset();
00165 else if (dt < 0.0)
00166 return;
00167
00168
00169 if (!system_ || !filter_) return;
00170
00171
00172 const ImuInput *imu = dynamic_cast<const ImuInput *>(&system_->getInput());
00173 #ifdef USE_RATE_SYSTEM_MODEL
00174 if (imu && rate_) {
00175 ROS_DEBUG("Updating with measurement model %s", rate_->getName().c_str());
00176 rate_->update(*this, Rate::Update(imu->getRate()));
00177 }
00178 #endif // USE_RATE_SYSTEM_MODEL
00179
00180
00181 system_->update(*this, dt);
00182 updateSystemStatus(system_->getStatusFlags(), STATE_ROLLPITCH | STATE_YAW | STATE_XY_POSITION | STATE_XY_VELOCITY | STATE_Z_POSITION | STATE_Z_VELOCITY);
00183
00184
00185 SystemStatus measurement_status = 0;
00186
00187 for(Measurements::iterator it = measurements_.begin(); it != measurements_.end(); ++it) {
00188 MeasurementPtr measurement = *it;
00189 if (!measurement->active(getSystemStatus())) continue;
00190
00191
00192 if (measurement == rate_) continue;
00193
00194 if (imu && measurement == gravity_) {
00195 ROS_DEBUG("Updating with pseudo measurement model %s", gravity_->getName().c_str());
00196 gravity_->update(*this, Gravity::Update(imu->getAccel()));
00197 continue;
00198 }
00199
00200 if (imu && measurement == zerorate_) {
00201 ROS_DEBUG("Updating with pseudo measurement model %s", zerorate_->getName().c_str());
00202 zerorate_->update(*this, ZeroRate::Update(imu->getRate()[2]));
00203 continue;
00204 }
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214 if (inSystemStatus(STATE_ALIGNMENT)) continue;
00215
00216
00217 measurement->process(*this);
00218 measurement_status |= measurement->getStatusFlags();
00219 measurement->increase_timer(dt);
00220 }
00221
00222
00223 setMeasurementStatus(measurement_status);
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248 if (inSystemStatus(STATE_ALIGNMENT)) {
00249 if (alignment_start_.isZero()) alignment_start_ = timestamp_;
00250 if ((timestamp_ - alignment_start_).toSec() >= alignment_time_) {
00251 updateSystemStatus(STATE_DEGRADED, STATE_ALIGNMENT);
00252 }
00253 } else if (inSystemStatus(STATE_ROLLPITCH | STATE_YAW | STATE_XY_POSITION | STATE_Z_POSITION)) {
00254
00255 updateSystemStatus(STATE_READY, STATE_DEGRADED);
00256 } else {
00257
00258 updateSystemStatus(STATE_DEGRADED, STATE_READY);
00259 }
00260 }
00261
00262 void PoseEstimation::updated() {
00263 state_is_dirty_ = covariance_is_dirty_ = true;
00264 setState(system_->limitState(getState()));
00265 }
00266
00267 const SystemPtr& PoseEstimation::setSystem(const SystemPtr& new_system) {
00268 if (system_) {
00269 cleanup();
00270 system_.reset();
00271 }
00272
00273 system_ = new_system;
00274 return system_;
00275 }
00276
00277 const SystemPtr& PoseEstimation::setSystem(System *new_system) {
00278 return setSystem(SystemPtr(new_system));
00279 }
00280
00281 const SystemModel *PoseEstimation::getSystemModel() const {
00282 if (!system_) return 0;
00283 return system_->getModel();
00284 }
00285
00286 const SystemPtr& PoseEstimation::getSystem() const {
00287 return system_;
00288 }
00289
00290 const MeasurementPtr& PoseEstimation::addMeasurement(const MeasurementPtr& measurement) {
00291 measurements_.push_back(measurement);
00292 return measurement;
00293 }
00294
00295 const MeasurementPtr& PoseEstimation::addMeasurement(const std::string& name, const MeasurementPtr& measurement) {
00296 measurement->setName(name);
00297 return addMeasurement(measurement);
00298 }
00299
00300 const MeasurementPtr& PoseEstimation::addMeasurement(Measurement *measurement) {
00301 return addMeasurement(MeasurementPtr(measurement));
00302 }
00303
00304 MeasurementPtr PoseEstimation::getMeasurement(const std::string &name) const {
00305 for(Measurements::const_iterator it = measurements_.begin(); it != measurements_.end(); ++it) {
00306 if ((*it)->getName() == name) return *it;
00307 }
00308 return MeasurementPtr();
00309 }
00310
00311 const StateVector& PoseEstimation::getState() {
00312 if (state_is_dirty_) {
00313 state_ = filter_->PostGet()->ExpectedValueGet();
00314 state_is_dirty_ = false;
00315 }
00316 return state_;
00317 }
00318
00319 const StateCovariance& PoseEstimation::getCovariance() {
00320 if (covariance_is_dirty_) {
00321 covariance_ = filter_->PostGet()->CovarianceGet();
00322 covariance_is_dirty_ = false;
00323 }
00324 return covariance_;
00325 }
00326
00327 void PoseEstimation::setState(const StateVector& state) {
00328 filter_->PostGet()->ExpectedValueSet(state);
00329 state_ = state;
00330 state_is_dirty_ = false;
00331 }
00332
00333 void PoseEstimation::setCovariance(const StateCovariance& covariance) {
00334 filter_->PostGet()->CovarianceSet(covariance);
00335 covariance_is_dirty_ = true;
00336 }
00337
00338
00339 SystemStatus PoseEstimation::getSystemStatus() const {
00340 return status_;
00341 }
00342
00343 SystemStatus PoseEstimation::getMeasurementStatus() const {
00344 return measurement_status_;
00345 }
00346
00347 bool PoseEstimation::inSystemStatus(SystemStatus test_status) const {
00348 return (getSystemStatus() & test_status) == test_status;
00349 }
00350
00351 bool PoseEstimation::setSystemStatus(SystemStatus new_status) {
00352 if (status_callback_ && !status_callback_(new_status)) return false;
00353
00354 SystemStatus set = new_status & ~status_;
00355 SystemStatus cleared = status_ & ~new_status;
00356 if (set) ROS_INFO_STREAM("Set system state " << getSystemStatusString(set));
00357 if (cleared) ROS_INFO_STREAM("Cleared system state " << getSystemStatusString(cleared));
00358
00359 status_ = new_status;
00360 return true;
00361 }
00362
00363 bool PoseEstimation::setMeasurementStatus(SystemStatus new_measurement_status) {
00364 SystemStatus set = new_measurement_status & ~measurement_status_;
00365 SystemStatus cleared = measurement_status_ & ~new_measurement_status;
00366 if (set) ROS_INFO_STREAM("Set measurement state " << getSystemStatusString(set));
00367 if (cleared) ROS_INFO_STREAM("Cleared measurement state " << getSystemStatusString(cleared));
00368
00369 measurement_status_ = new_measurement_status;
00370 return true;
00371 }
00372
00373 bool PoseEstimation::updateSystemStatus(SystemStatus set, SystemStatus clear) {
00374 return setSystemStatus((status_ & ~clear) | set);
00375 }
00376
00377 bool PoseEstimation::updateMeasurementStatus(SystemStatus set, SystemStatus clear) {
00378 return setMeasurementStatus((measurement_status_ & ~clear) | set);
00379 }
00380
00381 void PoseEstimation::setSystemStatusCallback(SystemStatusCallback callback) {
00382 status_callback_ = callback;
00383 }
00384
00385 ros::Time PoseEstimation::getTimestamp() const {
00386 return timestamp_;
00387 }
00388
00389 void PoseEstimation::setTimestamp(ros::Time timestamp) {
00390 timestamp_ = timestamp;
00391 }
00392
00393 void PoseEstimation::getHeader(std_msgs::Header& header) {
00394 header.stamp = timestamp_;
00395 header.frame_id = nav_frame_;
00396 }
00397
00398 void PoseEstimation::getState(nav_msgs::Odometry& state, bool with_covariances) {
00399 getHeader(state.header);
00400 getPose(state.pose.pose);
00401 getVelocity(state.twist.twist.linear);
00402 getRate(state.twist.twist.angular);
00403 state.child_frame_id = base_frame_;
00404
00405 if (with_covariances) {
00406 double qw = state_(QUATERNION_W);
00407 double qx = state_(QUATERNION_X);
00408 double qy = state_(QUATERNION_Y);
00409 double qz = state_(QUATERNION_Z);
00410 Matrix quat_to_angular_rate(3,4);
00411 quat_to_angular_rate(1,1) = -qx;
00412 quat_to_angular_rate(1,2) = qw;
00413 quat_to_angular_rate(1,3) = -qz;
00414 quat_to_angular_rate(1,4) = qy;
00415 quat_to_angular_rate(2,1) = -qy;
00416 quat_to_angular_rate(2,2) = -qz;
00417 quat_to_angular_rate(2,3) = qw;
00418 quat_to_angular_rate(2,4) = qx;
00419 quat_to_angular_rate(3,1) = -qz;
00420 quat_to_angular_rate(3,2) = qy;
00421 quat_to_angular_rate(3,3) = -qx;
00422 quat_to_angular_rate(3,4) = qw;
00423 quat_to_angular_rate *= 2.0;
00424
00425 getCovariance();
00426
00427
00428 for(int i = 0; i < 3; ++i)
00429 for(int j = 0; j < 3; ++j)
00430 state.pose.covariance[i*6+j] = covariance_(POSITION_X + i, POSITION_X + j);
00431
00432
00433 SymmetricMatrix covariance_rot(quat_to_angular_rate * covariance_.sub(QUATERNION_W,QUATERNION_Z,QUATERNION_W,QUATERNION_Z) * quat_to_angular_rate.transpose());
00434 for(int i = 0; i < 3; ++i)
00435 for(int j = 0; j < 3; ++j)
00436 state.pose.covariance[(i+3)*6+(j+3)] = covariance_rot(i+1,j+1);
00437
00438
00439 Matrix covariance_cross(quat_to_angular_rate * covariance_.sub(QUATERNION_W,QUATERNION_Z,POSITION_X,POSITION_Z));
00440 for(int i = 0; i < 3; ++i)
00441 for(int j = 0; j < 3; ++j)
00442 state.pose.covariance[(i+3)*6+j] = state.pose.covariance[j*6+(i+3)] = covariance_cross(i+1,j+1);
00443
00444
00445 for(int i = 0; i < 3; ++i)
00446 for(int j = 0; j < 3; ++j)
00447 state.twist.covariance[i*6+j] = covariance_(VELOCITY_X + i, VELOCITY_X + j);
00448
00449
00450 SymmetricMatrix gyro_noise(quat_to_angular_rate * system_->getModel()->AdditiveNoiseSigmaGet().sub(QUATERNION_W,QUATERNION_Z,QUATERNION_W,QUATERNION_Z) * quat_to_angular_rate.transpose());
00451 for(int i = 0; i < 3; ++i)
00452 for(int j = 0; j < 3; ++j)
00453 #ifdef USE_RATE_SYSTEM_MODEL
00454 state.twist.covariance[(i+3)*6+(j+3)] = covariance_(RATE_X + i, RATE_X + j);
00455 #else // USE_RATE_SYSTEM_MODEL
00456 state.twist.covariance[(i+3)*6+(j+3)] = covariance_(BIAS_GYRO_X + i, BIAS_GYRO_X + j) + gyro_noise(i+1,j+1);
00457 #endif // USE_RATE_SYSTEM_MODEL
00458
00459
00460 for(int i = 0; i < 3; ++i)
00461 for(int j = 0; j < 3; ++j)
00462 #ifdef USE_RATE_SYSTEM_MODEL
00463 state.twist.covariance[(i+3)*6+(j+3)] = state.twist.covariance[j*6+(i+3)] = covariance_(RATE_X + i, VELOCITY_X + j);
00464 #else // USE_RATE_SYSTEM_MODEL
00465 state.twist.covariance[(i+3)*6+j] = state.twist.covariance[j*6+(i+3)] = covariance_(BIAS_GYRO_X + i, VELOCITY_X + j);
00466 #endif // USE_RATE_SYSTEM_MODEL
00467 }
00468 }
00469
00470 void PoseEstimation::getPose(tf::Pose& pose) {
00471 tf::Quaternion quaternion;
00472 getPosition(pose.getOrigin());
00473 getOrientation(quaternion);
00474 pose.setRotation(quaternion);
00475 }
00476
00477 void PoseEstimation::getPose(tf::Stamped<tf::Pose>& pose) {
00478 getPose(static_cast<tf::Pose &>(pose));
00479 pose.stamp_ = timestamp_;
00480 pose.frame_id_ = nav_frame_;
00481 }
00482
00483 void PoseEstimation::getPose(geometry_msgs::Pose& pose) {
00484 getPosition(pose.position);
00485 getOrientation(pose.orientation);
00486 }
00487
00488 void PoseEstimation::getPose(geometry_msgs::PoseStamped& pose) {
00489 getHeader(pose.header);
00490 getPose(pose.pose);
00491 }
00492
00493 void PoseEstimation::getPosition(tf::Point& point) {
00494 getState();
00495 point = tf::Point(state_(POSITION_X), state_(POSITION_Y), state_(POSITION_Z));
00496 }
00497
00498 void PoseEstimation::getPosition(tf::Stamped<tf::Point>& point) {
00499 getPosition(static_cast<tf::Point &>(point));
00500 point.stamp_ = timestamp_;
00501 point.frame_id_ = nav_frame_;
00502 }
00503
00504 void PoseEstimation::getPosition(geometry_msgs::Point& point) {
00505 getState();
00506 point.x = state_(POSITION_X);
00507 point.y = state_(POSITION_Y);
00508 point.z = state_(POSITION_Z);
00509 }
00510
00511 void PoseEstimation::getPosition(geometry_msgs::PointStamped& point) {
00512 getHeader(point.header);
00513 getPosition(point.point);
00514 }
00515
00516 void PoseEstimation::getGlobalPosition(double &latitude, double &longitude, double &altitude) {
00517 getState();
00518 double north = state_(POSITION_X) * globalReference()->heading().cos - state_(POSITION_Y) * globalReference()->heading().sin;
00519 double east = -state_(POSITION_X) * globalReference()->heading().sin - state_(POSITION_Y) * globalReference()->heading().cos;
00520 latitude = global_reference_.position().latitude + north / globalReference()->radius().north;
00521 longitude = global_reference_.position().longitude + east / globalReference()->radius().east;
00522 altitude = global_reference_.position().altitude + state_(POSITION_Z);
00523 }
00524
00525 void PoseEstimation::getGlobalPosition(sensor_msgs::NavSatFix& global)
00526 {
00527 getHeader(global.header);
00528 global.header.frame_id = world_frame_;
00529
00530 if ((getSystemStatus() & STATE_XY_POSITION) && globalReference()->hasPosition()) {
00531 global.status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
00532 } else {
00533 global.status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;
00534 }
00535
00536 getGlobalPosition(global.latitude, global.longitude, global.altitude);
00537 global.latitude *= 180.0/M_PI;
00538 global.longitude *= 180.0/M_PI;
00539 }
00540
00541 void PoseEstimation::getOrientation(tf::Quaternion& quaternion) {
00542 getState();
00543 quaternion = tf::Quaternion(state_(QUATERNION_X), state_(QUATERNION_Y), state_(QUATERNION_Z), state_(QUATERNION_W));
00544 }
00545
00546 void PoseEstimation::getOrientation(tf::Stamped<tf::Quaternion>& quaternion) {
00547 getOrientation(static_cast<tf::Quaternion &>(quaternion));
00548 quaternion.stamp_ = timestamp_;
00549 quaternion.frame_id_ = nav_frame_;
00550 }
00551
00552 void PoseEstimation::getOrientation(geometry_msgs::Quaternion& quaternion) {
00553 getState();
00554 quaternion.w = state_(QUATERNION_W);
00555 quaternion.x = state_(QUATERNION_X);
00556 quaternion.y = state_(QUATERNION_Y);
00557 quaternion.z = state_(QUATERNION_Z);
00558 }
00559
00560 void PoseEstimation::getOrientation(geometry_msgs::QuaternionStamped& quaternion) {
00561 getHeader(quaternion.header);
00562 getOrientation(quaternion.quaternion);
00563 }
00564
00565 void PoseEstimation::getOrientation(double &yaw, double &pitch, double &roll) {
00566 tf::Quaternion quaternion;
00567 getOrientation(quaternion);
00568 #ifdef TF_MATRIX3x3_H
00569 tf::Matrix3x3(quaternion).getRPY(roll, pitch, yaw);
00570 #else
00571 btMatrix3x3(quaternion).getRPY(roll, pitch, yaw);
00572 #endif
00573 }
00574
00575 void PoseEstimation::getImuWithBiases(geometry_msgs::Vector3& linear_acceleration, geometry_msgs::Vector3& angular_velocity) {
00576 getState();
00577 const ImuInput *imu = dynamic_cast<const ImuInput *>(&system_->getInput());
00578 if (imu) {
00579 linear_acceleration.x = imu->getAccel()[0] + state_(BIAS_ACCEL_X);
00580 linear_acceleration.y = imu->getAccel()[1] + state_(BIAS_ACCEL_Y);
00581 linear_acceleration.z = imu->getAccel()[2] + state_(BIAS_ACCEL_Z);
00582 } else {
00583 linear_acceleration.x = 0.0;
00584 linear_acceleration.y = 0.0;
00585 linear_acceleration.z = 0.0;
00586 }
00587
00588 #ifdef USE_RATE_SYSTEM_MODEL
00589 Rate::MeasurementVector rate_body = rate_->getModel()->PredictionGet(0, state_);
00590 angular_velocity.x = rate_body(1);
00591 angular_velocity.y = rate_body(2);
00592 angular_velocity.z = rate_body(3);
00593 #else // USE_RATE_SYSTEM_MODEL
00594 if (imu) {
00595 angular_velocity.x = imu->getRate()[0] + state_(BIAS_GYRO_X);
00596 angular_velocity.y = imu->getRate()[1] + state_(BIAS_GYRO_Y);
00597 angular_velocity.z = imu->getRate()[2] + state_(BIAS_GYRO_Z);
00598 } else {
00599 angular_velocity.x = 0.0;
00600 angular_velocity.y = 0.0;
00601 angular_velocity.z = 0.0;
00602 }
00603 #endif // USE_RATE_SYSTEM_MODEL
00604 }
00605
00606 void PoseEstimation::getVelocity(tf::Vector3& vector) {
00607 getState();
00608 vector = tf::Vector3(state_(VELOCITY_X), state_(VELOCITY_Y), state_(VELOCITY_Z));
00609 }
00610
00611 void PoseEstimation::getVelocity(tf::Stamped<tf::Vector3>& vector) {
00612 getVelocity(static_cast<tf::Vector3 &>(vector));
00613 vector.stamp_ = timestamp_;
00614 vector.frame_id_ = nav_frame_;
00615 }
00616
00617 void PoseEstimation::getVelocity(geometry_msgs::Vector3& vector) {
00618 getState();
00619 vector.x = state_(VELOCITY_X);
00620 vector.y = state_(VELOCITY_Y);
00621 vector.z = state_(VELOCITY_Z);
00622 }
00623
00624 void PoseEstimation::getVelocity(geometry_msgs::Vector3Stamped& vector) {
00625 getHeader(vector.header);
00626 getVelocity(vector.vector);
00627 }
00628
00629 void PoseEstimation::getRate(tf::Vector3& vector) {
00630 getState();
00631 #ifdef USE_RATE_SYSTEM_MODEL
00632 vector = tf::Vector3(state_(RATE_X), state_(RATE_Y), state_(RATE_Z));
00633 #else // USE_RATE_SYSTEM_MODEL
00634 const ImuInput *imu = dynamic_cast<const ImuInput *>(&system_->getInput());
00635 if (imu) {
00636 vector = tf::Vector3(imu->getRate()[0] + state_(BIAS_GYRO_X), imu->getRate()[1] + state_(BIAS_GYRO_Y), imu->getRate()[2] + state_(BIAS_GYRO_Z));
00637 } else {
00638 vector = tf::Vector3(0,0,0);
00639 }
00640 #endif // USE_RATE_SYSTEM_MODEL
00641 }
00642
00643 void PoseEstimation::getRate(tf::Stamped<tf::Vector3>& vector) {
00644 getRate(static_cast<tf::Vector3 &>(vector));
00645 vector.stamp_ = timestamp_;
00646 vector.frame_id_ = nav_frame_;
00647 }
00648
00649 void PoseEstimation::getRate(geometry_msgs::Vector3& vector) {
00650 getState();
00651 #ifdef USE_RATE_SYSTEM_MODEL
00652 vector.x = state_(RATE_X);
00653 vector.y = state_(RATE_Y);
00654 vector.z = state_(RATE_Z);
00655 #else // USE_RATE_SYSTEM_MODEL
00656 const ImuInput *imu = dynamic_cast<const ImuInput *>(&system_->getInput());
00657 if (imu) {
00658 vector.x = imu->getRate()[0] + state_(BIAS_GYRO_X);
00659 vector.y = imu->getRate()[1] + state_(BIAS_GYRO_Y);
00660 vector.z = imu->getRate()[2] + state_(BIAS_GYRO_Z);
00661 } else {
00662 vector.x = 0.0;
00663 vector.y = 0.0;
00664 vector.z = 0.0;
00665 }
00666 #endif // USE_RATE_SYSTEM_MODEL
00667 }
00668
00669 void PoseEstimation::getRate(geometry_msgs::Vector3Stamped& vector) {
00670 getHeader(vector.header);
00671 getRate(vector.vector);
00672 }
00673
00674 void PoseEstimation::getBias(tf::Vector3& angular_velocity, tf::Vector3& linear_acceleration) {
00675 getState();
00676 angular_velocity.setX(state_(BIAS_GYRO_X));
00677 angular_velocity.setY(state_(BIAS_GYRO_Y));
00678 angular_velocity.setZ(state_(BIAS_GYRO_Z));
00679 linear_acceleration.setX(state_(BIAS_ACCEL_X));
00680 linear_acceleration.setY(state_(BIAS_ACCEL_Y));
00681 linear_acceleration.setZ(state_(BIAS_ACCEL_Z));
00682 }
00683
00684 void PoseEstimation::getBias(tf::Stamped<tf::Vector3>& angular_velocity, tf::Stamped<tf::Vector3>& linear_acceleration) {
00685 getBias(static_cast<tf::Vector3 &>(angular_velocity), static_cast<tf::Vector3 &>(linear_acceleration));
00686 angular_velocity.stamp_ = timestamp_;
00687 angular_velocity.frame_id_ = base_frame_;
00688 linear_acceleration.stamp_ = timestamp_;
00689 linear_acceleration.frame_id_ = base_frame_;
00690 }
00691
00692 void PoseEstimation::getBias(geometry_msgs::Vector3& angular_velocity, geometry_msgs::Vector3& linear_acceleration) {
00693 getState();
00694 angular_velocity.x = state_(BIAS_GYRO_X);
00695 angular_velocity.y = state_(BIAS_GYRO_Y);
00696 angular_velocity.z = state_(BIAS_GYRO_Z);
00697 linear_acceleration.x = state_(BIAS_ACCEL_X);
00698 linear_acceleration.y = state_(BIAS_ACCEL_Y);
00699 linear_acceleration.z = state_(BIAS_ACCEL_Z);
00700 }
00701
00702 void PoseEstimation::getBias(geometry_msgs::Vector3Stamped& angular_velocity, geometry_msgs::Vector3Stamped& linear_acceleration) {
00703 getBias(angular_velocity.vector, linear_acceleration.vector);
00704 angular_velocity.header.stamp = timestamp_;
00705 angular_velocity.header.frame_id = base_frame_;
00706 linear_acceleration.header.stamp = timestamp_;
00707 linear_acceleration.header.frame_id = base_frame_;
00708 }
00709
00710 void PoseEstimation::getTransforms(std::vector<tf::StampedTransform>& transforms) {
00711 tf::Quaternion orientation;
00712 tf::Point position;
00713 getOrientation(orientation);
00714 getPosition(position);
00715
00716 tf::Transform transform(orientation, position);
00717 double y,p,r;
00718 transform.getBasis().getEulerYPR(y,p,r);
00719
00720 std::string parent_frame = nav_frame_;
00721
00722 if(!position_frame_.empty()) {
00723 tf::Transform position_transform;
00724 position_transform.getBasis().setIdentity();
00725 position_transform.setOrigin(tf::Point(position.x(), position.y(), position.z()));
00726 transforms.push_back(tf::StampedTransform(position_transform, timestamp_, parent_frame, position_frame_ ));
00727 }
00728
00729 if (!footprint_frame_.empty()) {
00730 tf::Transform footprint_transform;
00731 footprint_transform.getBasis().setEulerYPR(y, 0.0, 0.0);
00732 footprint_transform.setOrigin(tf::Point(position.x(), position.y(), 0.0));
00733 transforms.push_back(tf::StampedTransform(footprint_transform, timestamp_, parent_frame, footprint_frame_));
00734
00735 parent_frame = footprint_frame_;
00736 transform = footprint_transform.inverseTimes(transform);
00737 }
00738
00739 if (!stabilized_frame_.empty()) {
00740 tf::Transform stabilized_transform(transform);
00741 #ifdef TF_MATRIX3x3_H
00742 tf::Matrix3x3 rollpitch_rotation; rollpitch_rotation.setEulerYPR(0.0, p, r);
00743 #else
00744 btMatrix3x3 rollpitch_rotation; rollpitch_rotation.setEulerYPR(0.0, p, r);
00745 #endif
00746 stabilized_transform = stabilized_transform * tf::Transform(rollpitch_rotation.inverse());
00747 transforms.push_back(tf::StampedTransform(stabilized_transform, timestamp_, parent_frame, stabilized_frame_));
00748
00749 parent_frame = stabilized_frame_;
00750 transform = stabilized_transform.inverseTimes(transform);
00751 }
00752
00753 transforms.push_back(tf::StampedTransform(transform, timestamp_, parent_frame, base_frame_));
00754
00755
00756
00757
00758
00759
00760
00761
00762
00763
00764
00765
00766
00767
00768
00769
00770
00771
00772
00773
00774
00775
00776 }
00777
00778 void PoseEstimation::updateWorldToOtherTransform(tf::StampedTransform& world_to_other_transform) {
00779 world_to_other_transform.frame_id_ = world_frame_;
00780
00781 double y,p,r;
00782 world_to_other_transform.getBasis().getEulerYPR(y,p,r);
00783 if (!(getSystemStatus() & STATE_ROLLPITCH)) { r = p = 0.0; }
00784 if (!(getSystemStatus() & STATE_YAW)) { y = 0.0; }
00785 if (!(getSystemStatus() & STATE_XY_POSITION)) { world_to_other_transform.getOrigin().setX(0.0); world_to_other_transform.getOrigin().setY(0.0); }
00786 if (!(getSystemStatus() & STATE_Z_POSITION)) { world_to_other_transform.getOrigin().setZ(0.0); }
00787 world_to_other_transform.getBasis().setEulerYPR(y, p, r);
00788 }
00789
00790 ParameterList PoseEstimation::getParameters() const {
00791 ParameterList parameters = parameters_;
00792
00793 if (system_) {
00794 parameters.copy(system_->getName(), system_->parameters());
00795 }
00796
00797 for(Measurements::const_iterator it = measurements_.begin(); it != measurements_.end(); ++it) {
00798 parameters.copy((*it)->getName(), (*it)->parameters());
00799 }
00800
00801 return parameters;
00802 }
00803
00804 GlobalReference* PoseEstimation::globalReference() {
00805 return &global_reference_;
00806 }
00807
00808 }