$search
00001 //================================================================================================= 00002 // Copyright (c) 2011, Johannes Meyer and Martin Nowara, TU Darmstadt 00003 // All rights reserved. 00004 00005 // Redistribution and use in source and binary forms, with or without 00006 // modification, are permitted provided that the following conditions are met: 00007 // * Redistributions of source code must retain the above copyright 00008 // notice, this list of conditions and the following disclaimer. 00009 // * Redistributions in binary form must reproduce the above copyright 00010 // notice, this list of conditions and the following disclaimer in the 00011 // documentation and/or other materials provided with the distribution. 00012 // * Neither the name of the Flight Systems and Automatic Control group, 00013 // TU Darmstadt, nor the names of its contributors may be used to 00014 // endorse or promote products derived from this software without 00015 // specific prior written permission. 00016 00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 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 // , heading_(new Heading("heading")) 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 // position_frame_ = "base_position"; 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 // add default measurements 00069 addMeasurement(rate_); 00070 addMeasurement(gravity_); 00071 addMeasurement(zerorate_); 00072 // addMeasurement(heading_); 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 // initialize global reference 00088 globalReference()->updated(); 00089 00090 // check if system is initialized 00091 if (!system_) return false; 00092 00093 // initialize system 00094 if (!system_->init()) return false; 00095 00096 // initialize all measurements 00097 for(Measurements::iterator it = measurements_.begin(); it != measurements_.end(); ++it) 00098 if (!(*it)->init()) return false; 00099 00100 // reset (or initialize) filter and measurements 00101 reset(); 00102 00103 return true; 00104 } 00105 00106 void PoseEstimation::cleanup() 00107 { 00108 // delete filter instance 00109 if (filter_) filter_.reset(); 00110 00111 // cleanup system 00112 system_->cleanup(); 00113 00114 // cleanup all measurements 00115 for(Measurements::iterator it = measurements_.begin(); it != measurements_.end(); ++it) (*it)->cleanup(); 00116 } 00117 00118 void PoseEstimation::reset() 00119 { 00120 // reset extended Kalman filter 00121 if (filter_) cleanup(); 00122 00123 // check if system is initialized 00124 if (!system_) return; 00125 00126 // initialize new filter 00127 filter_.reset(new BFL::ExtendedKalmanFilter(system_->getPrior())); 00128 updated(); 00129 00130 // set initial status 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 // reset system and all measurements 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 // check if system is initialized 00149 if (!system_) return; 00150 00151 // set input and calculate time diff dt 00152 system_->setInput(input); 00153 if (!timestamp_.isZero()) dt = new_timestamp - timestamp_; 00154 timestamp_ = new_timestamp; 00155 00156 // do the update step 00157 update(dt.toSec()); 00158 } 00159 00160 void PoseEstimation::update(double dt) 00161 { 00162 // check dt 00163 if (dt < -1.0) 00164 reset(); 00165 else if (dt < 0.0) 00166 return; 00167 00168 // check if system and filter is initialized 00169 if (!system_ || !filter_) return; 00170 00171 // filter rate measurement first 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 // time update step 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 // iterate through measurements and do the measurement update steps 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 // special updates 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 // if (measurement == &heading_) { 00207 // ROS_DEBUG("Updating with pseudo measurement model %s", heading_.getName().c_str()); 00208 // Heading::Update y(0.0); 00209 // heading_.update(*this, y); 00210 // continue; 00211 // } 00212 00213 // skip all other measurements during alignment 00214 if (inSystemStatus(STATE_ALIGNMENT)) continue; 00215 00216 // process the incoming queue 00217 measurement->process(*this); 00218 measurement_status |= measurement->getStatusFlags(); 00219 measurement->increase_timer(dt); 00220 } 00221 00222 // update the measurement status 00223 setMeasurementStatus(measurement_status); 00224 00225 // // pseudo updates 00226 // if (gravity_.active(getSystemStatus())) { 00227 // ROS_DEBUG("Updating with pseudo measurement model %s", gravity_.getName().c_str()); 00228 // Gravity::Update y(imu->getAccel()); 00229 // // gravity_.enable(); 00230 // if (gravity_.update(*this, y)) measurement_status |= gravity_.getStatusFlags(); 00231 // } else { 00232 // // gravity_.disable(); 00233 // } 00234 00235 // if (zerorate_.active(getSystemStatus())) { 00236 // ROS_DEBUG("Updating with pseudo measurement model %s", zerorate_.getName().c_str()); 00237 // ZeroRate::Update y(imu->getRate()); 00238 // // zerorate_.enable(); 00239 // if (zerorate_.update(*this, y)) measurement_status |= zerorate_.getStatusFlags(); 00240 // } else { 00241 // // zerorate_.disable(); 00242 // } 00243 00244 // std::cout << "x_est = [" << getState().transpose() << "]" << std::endl; 00245 // std::cout << "P_est = [" << filter_->PostGet()->CovarianceGet() << "]" << std::endl; 00246 00247 // switch overall system state 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 // if (!(status_ & STATE_READY) && (status_ & STATE_ROLLPITCH) && (status_ & STATE_YAW) && (status_ & STATE_XY_POSITION) && (status_ & STATE_Z_POSITION)) { 00255 updateSystemStatus(STATE_READY, STATE_DEGRADED); 00256 } else { 00257 // if ((status_ & STATE_READY) && !((status_ & STATE_ROLLPITCH) && (status_ & STATE_YAW) && (status_ & STATE_XY_POSITION) && (status_ & STATE_Z_POSITION))) { 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 // position covariance 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 // rotation covariance (world-fixed) 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 // cross position/rotation covariance 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 // velocity covariance 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 // angular rate covariance 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 // cross velocity/angular_rate variance 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 getGlobalPosition(global.latitude, global.longitude, global.altitude); 00531 global.latitude *= 180.0/M_PI; 00532 global.longitude *= 180.0/M_PI; 00533 } 00534 00535 void PoseEstimation::getOrientation(tf::Quaternion& quaternion) { 00536 getState(); 00537 quaternion = tf::Quaternion(state_(QUATERNION_X), state_(QUATERNION_Y), state_(QUATERNION_Z), state_(QUATERNION_W)); 00538 } 00539 00540 void PoseEstimation::getOrientation(tf::Stamped<tf::Quaternion>& quaternion) { 00541 getOrientation(static_cast<tf::Quaternion &>(quaternion)); 00542 quaternion.stamp_ = timestamp_; 00543 quaternion.frame_id_ = nav_frame_; 00544 } 00545 00546 void PoseEstimation::getOrientation(geometry_msgs::Quaternion& quaternion) { 00547 getState(); 00548 quaternion.w = state_(QUATERNION_W); 00549 quaternion.x = state_(QUATERNION_X); 00550 quaternion.y = state_(QUATERNION_Y); 00551 quaternion.z = state_(QUATERNION_Z); 00552 } 00553 00554 void PoseEstimation::getOrientation(geometry_msgs::QuaternionStamped& quaternion) { 00555 getHeader(quaternion.header); 00556 getOrientation(quaternion.quaternion); 00557 } 00558 00559 void PoseEstimation::getOrientation(double &yaw, double &pitch, double &roll) { 00560 tf::Quaternion quaternion; 00561 getOrientation(quaternion); 00562 #ifdef TF_MATRIX3x3_H 00563 tf::Matrix3x3(quaternion).getRPY(roll, pitch, yaw); 00564 #else 00565 btMatrix3x3(quaternion).getRPY(roll, pitch, yaw); 00566 #endif 00567 } 00568 00569 void PoseEstimation::getImuWithBiases(geometry_msgs::Vector3& linear_acceleration, geometry_msgs::Vector3& angular_velocity) { 00570 getState(); 00571 const ImuInput *imu = dynamic_cast<const ImuInput *>(&system_->getInput()); 00572 if (imu) { 00573 linear_acceleration.x = imu->getAccel()[0] + state_(BIAS_ACCEL_X); 00574 linear_acceleration.y = imu->getAccel()[1] + state_(BIAS_ACCEL_Y); 00575 linear_acceleration.z = imu->getAccel()[2] + state_(BIAS_ACCEL_Z); 00576 } else { 00577 linear_acceleration.x = 0.0; 00578 linear_acceleration.y = 0.0; 00579 linear_acceleration.z = 0.0; 00580 } 00581 00582 #ifdef USE_RATE_SYSTEM_MODEL 00583 Rate::MeasurementVector rate_body = rate_->getModel()->PredictionGet(0, state_); 00584 angular_velocity.x = rate_body(1); 00585 angular_velocity.y = rate_body(2); 00586 angular_velocity.z = rate_body(3); 00587 #else // USE_RATE_SYSTEM_MODEL 00588 if (imu) { 00589 angular_velocity.x = imu->getRate()[0] + state_(BIAS_GYRO_X); 00590 angular_velocity.y = imu->getRate()[1] + state_(BIAS_GYRO_Y); 00591 angular_velocity.z = imu->getRate()[2] + state_(BIAS_GYRO_Z); 00592 } else { 00593 angular_velocity.x = 0.0; 00594 angular_velocity.y = 0.0; 00595 angular_velocity.z = 0.0; 00596 } 00597 #endif // USE_RATE_SYSTEM_MODEL 00598 } 00599 00600 void PoseEstimation::getVelocity(tf::Vector3& vector) { 00601 getState(); 00602 vector = tf::Vector3(state_(VELOCITY_X), state_(VELOCITY_Y), state_(VELOCITY_Z)); 00603 } 00604 00605 void PoseEstimation::getVelocity(tf::Stamped<tf::Vector3>& vector) { 00606 getVelocity(static_cast<tf::Vector3 &>(vector)); 00607 vector.stamp_ = timestamp_; 00608 vector.frame_id_ = nav_frame_; 00609 } 00610 00611 void PoseEstimation::getVelocity(geometry_msgs::Vector3& vector) { 00612 getState(); 00613 vector.x = state_(VELOCITY_X); 00614 vector.y = state_(VELOCITY_Y); 00615 vector.z = state_(VELOCITY_Z); 00616 } 00617 00618 void PoseEstimation::getVelocity(geometry_msgs::Vector3Stamped& vector) { 00619 getHeader(vector.header); 00620 getVelocity(vector.vector); 00621 } 00622 00623 void PoseEstimation::getRate(tf::Vector3& vector) { 00624 getState(); 00625 #ifdef USE_RATE_SYSTEM_MODEL 00626 vector = tf::Vector3(state_(RATE_X), state_(RATE_Y), state_(RATE_Z)); 00627 #else // USE_RATE_SYSTEM_MODEL 00628 const ImuInput *imu = dynamic_cast<const ImuInput *>(&system_->getInput()); 00629 if (imu) { 00630 vector = tf::Vector3(imu->getRate()[0] + state_(BIAS_GYRO_X), imu->getRate()[1] + state_(BIAS_GYRO_Y), imu->getRate()[2] + state_(BIAS_GYRO_Z)); 00631 } else { 00632 vector = tf::Vector3(0,0,0); 00633 } 00634 #endif // USE_RATE_SYSTEM_MODEL 00635 } 00636 00637 void PoseEstimation::getRate(tf::Stamped<tf::Vector3>& vector) { 00638 getRate(static_cast<tf::Vector3 &>(vector)); 00639 vector.stamp_ = timestamp_; 00640 vector.frame_id_ = nav_frame_; 00641 } 00642 00643 void PoseEstimation::getRate(geometry_msgs::Vector3& vector) { 00644 getState(); 00645 #ifdef USE_RATE_SYSTEM_MODEL 00646 vector.x = state_(RATE_X); 00647 vector.y = state_(RATE_Y); 00648 vector.z = state_(RATE_Z); 00649 #else // USE_RATE_SYSTEM_MODEL 00650 const ImuInput *imu = dynamic_cast<const ImuInput *>(&system_->getInput()); 00651 if (imu) { 00652 vector.x = imu->getRate()[0] + state_(BIAS_GYRO_X); 00653 vector.y = imu->getRate()[1] + state_(BIAS_GYRO_Y); 00654 vector.z = imu->getRate()[2] + state_(BIAS_GYRO_Z); 00655 } else { 00656 vector.x = 0.0; 00657 vector.y = 0.0; 00658 vector.z = 0.0; 00659 } 00660 #endif // USE_RATE_SYSTEM_MODEL 00661 } 00662 00663 void PoseEstimation::getRate(geometry_msgs::Vector3Stamped& vector) { 00664 getHeader(vector.header); 00665 getRate(vector.vector); 00666 } 00667 00668 void PoseEstimation::getBias(tf::Vector3& angular_velocity, tf::Vector3& linear_acceleration) { 00669 getState(); 00670 angular_velocity.setX(state_(BIAS_GYRO_X)); 00671 angular_velocity.setY(state_(BIAS_GYRO_Y)); 00672 angular_velocity.setZ(state_(BIAS_GYRO_Z)); 00673 linear_acceleration.setX(state_(BIAS_ACCEL_X)); 00674 linear_acceleration.setY(state_(BIAS_ACCEL_Y)); 00675 linear_acceleration.setZ(state_(BIAS_ACCEL_Z)); 00676 } 00677 00678 void PoseEstimation::getBias(tf::Stamped<tf::Vector3>& angular_velocity, tf::Stamped<tf::Vector3>& linear_acceleration) { 00679 getBias(static_cast<tf::Vector3 &>(angular_velocity), static_cast<tf::Vector3 &>(linear_acceleration)); 00680 angular_velocity.stamp_ = timestamp_; 00681 angular_velocity.frame_id_ = base_frame_; 00682 linear_acceleration.stamp_ = timestamp_; 00683 linear_acceleration.frame_id_ = base_frame_; 00684 } 00685 00686 void PoseEstimation::getBias(geometry_msgs::Vector3& angular_velocity, geometry_msgs::Vector3& linear_acceleration) { 00687 getState(); 00688 angular_velocity.x = state_(BIAS_GYRO_X); 00689 angular_velocity.y = state_(BIAS_GYRO_Y); 00690 angular_velocity.z = state_(BIAS_GYRO_Z); 00691 linear_acceleration.x = state_(BIAS_ACCEL_X); 00692 linear_acceleration.y = state_(BIAS_ACCEL_Y); 00693 linear_acceleration.z = state_(BIAS_ACCEL_Z); 00694 } 00695 00696 void PoseEstimation::getBias(geometry_msgs::Vector3Stamped& angular_velocity, geometry_msgs::Vector3Stamped& linear_acceleration) { 00697 getBias(angular_velocity.vector, linear_acceleration.vector); 00698 angular_velocity.header.stamp = timestamp_; 00699 angular_velocity.header.frame_id = base_frame_; 00700 linear_acceleration.header.stamp = timestamp_; 00701 linear_acceleration.header.frame_id = base_frame_; 00702 } 00703 00704 void PoseEstimation::getTransforms(std::vector<tf::StampedTransform>& transforms) { 00705 tf::Quaternion orientation; 00706 tf::Point position; 00707 getOrientation(orientation); 00708 getPosition(position); 00709 00710 tf::Transform transform(orientation, position); 00711 double y,p,r; 00712 transform.getBasis().getEulerYPR(y,p,r); 00713 00714 std::string parent_frame = nav_frame_; 00715 00716 if(!position_frame_.empty()) { 00717 tf::Transform position_transform; 00718 position_transform.getBasis().setIdentity(); 00719 position_transform.setOrigin(tf::Point(position.x(), position.y(), position.z())); 00720 transforms.push_back(tf::StampedTransform(position_transform, timestamp_, parent_frame, position_frame_ )); 00721 } 00722 00723 if (!footprint_frame_.empty()) { 00724 tf::Transform footprint_transform; 00725 footprint_transform.getBasis().setEulerYPR(y, 0.0, 0.0); 00726 footprint_transform.setOrigin(tf::Point(position.x(), position.y(), 0.0)); 00727 transforms.push_back(tf::StampedTransform(footprint_transform, timestamp_, parent_frame, footprint_frame_)); 00728 00729 parent_frame = footprint_frame_; 00730 transform = footprint_transform.inverseTimes(transform); 00731 } 00732 00733 if (!stabilized_frame_.empty()) { 00734 tf::Transform stabilized_transform(transform); 00735 #ifdef TF_MATRIX3x3_H 00736 tf::Matrix3x3 rollpitch_rotation; rollpitch_rotation.setEulerYPR(0.0, p, r); 00737 #else 00738 btMatrix3x3 rollpitch_rotation; rollpitch_rotation.setEulerYPR(0.0, p, r); 00739 #endif 00740 stabilized_transform = stabilized_transform * tf::Transform(rollpitch_rotation.inverse()); 00741 transforms.push_back(tf::StampedTransform(stabilized_transform, timestamp_, parent_frame, stabilized_frame_)); 00742 00743 parent_frame = stabilized_frame_; 00744 transform = stabilized_transform.inverseTimes(transform); 00745 } 00746 00747 transforms.push_back(tf::StampedTransform(transform, timestamp_, parent_frame, base_frame_)); 00748 00749 // transforms.resize(3); 00750 00751 // transforms[0].stamp_ = timestamp_; 00752 // transforms[0].frame_id_ = nav_frame_; 00753 // transforms[0].child_frame_id_ = footprint_frame_; 00754 // transforms[0].setOrigin(tf::Point(position.x(), position.y(), 0.0)); 00755 // rotation.setEulerYPR(y,0.0,0.0); 00756 // transforms[0].setBasis(rotation); 00757 00758 // transforms[1].stamp_ = timestamp_; 00759 // transforms[1].frame_id_ = footprint_frame_; 00760 // transforms[1].child_frame_id_ = stabilized_frame_; 00761 // transforms[1].setIdentity(); 00762 // transforms[1].setOrigin(tf::Point(0.0, 0.0, position.z())); 00763 00764 // transforms[2].stamp_ = timestamp_; 00765 // transforms[2].frame_id_ = stabilized_frame_; 00766 // transforms[2].child_frame_id_ = base_frame_; 00767 // transforms[2].setIdentity(); 00768 // rotation.setEulerYPR(0.0,p,r); 00769 // transforms[2].setBasis(rotation); 00770 } 00771 00772 void PoseEstimation::updateWorldToOtherTransform(tf::StampedTransform& world_to_other_transform) { 00773 world_to_other_transform.frame_id_ = world_frame_; 00774 00775 double y,p,r; 00776 world_to_other_transform.getBasis().getEulerYPR(y,p,r); 00777 if (!(getSystemStatus() & STATE_ROLLPITCH)) { r = p = 0.0; } 00778 if (!(getSystemStatus() & STATE_YAW)) { y = 0.0; } 00779 if (!(getSystemStatus() & STATE_XY_POSITION)) { world_to_other_transform.getOrigin().setX(0.0); world_to_other_transform.getOrigin().setY(0.0); } 00780 if (!(getSystemStatus() & STATE_Z_POSITION)) { world_to_other_transform.getOrigin().setZ(0.0); } 00781 world_to_other_transform.getBasis().setEulerYPR(y, p, r); 00782 } 00783 00784 ParameterList PoseEstimation::getParameters() const { 00785 ParameterList parameters = parameters_; 00786 00787 if (system_) { 00788 parameters.copy(system_->getName(), system_->parameters()); 00789 } 00790 00791 for(Measurements::const_iterator it = measurements_.begin(); it != measurements_.end(); ++it) { 00792 parameters.copy((*it)->getName(), (*it)->parameters()); 00793 } 00794 00795 return parameters; 00796 } 00797 00798 GlobalReference* PoseEstimation::globalReference() { 00799 return &global_reference_; 00800 } 00801 00802 } // namespace hector_pose_estimation