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