36 #include <boost/weak_ptr.hpp> 41 static PoseEstimation *the_instance = 0;
46 , rate_update_(new
Rate(
"rate"))
47 , gravity_update_(new
Gravity (
"gravity"))
48 , zerorate_update_(new
ZeroRate(
"zerorate"))
50 if (!the_instance) the_instance =
this;
90 #ifdef EIGEN_RUNTIME_NO_MALLOC 91 Eigen::internal::set_is_malloc_allowed(
true);
105 if (!(*it)->init(*
this,
state()))
return false;
109 if (!(*it)->init(*
this,
state()))
return false;
116 (*it)->setFilter(
filter_.get());
118 (*it)->setFilter(
filter_.get());
154 (*it)->reset(
state());
155 (*it)->getPrior(
state());
159 (*it)->reset(
state());
234 measurement_status |= measurement->getStatusFlags();
235 measurement->increase_timer(dt);
243 system_status |= system->getStatusFlags();
248 if (!
state().valid()) {
249 ROS_FATAL(
"Invalid state, resetting...");
267 #ifdef EIGEN_RUNTIME_NO_MALLOC 269 Eigen::internal::set_is_malloc_allowed(
false);
275 (*it)->limitState(
state());
280 if (!name.empty() && system->getName().empty()) system->setName(name);
287 if (!name.empty()) input->setName(name);
293 if (name.empty()) name = value.
getName();
296 ROS_WARN(
"Set input \"%s\", but this input is not registered by any system model.", name.c_str());
305 if (!name.empty()) measurement->setName(name);
306 parameters().
add(measurement->getName(), measurement->parameters());
371 getRate(msg.twist.twist.angular);
376 msg.twist.twist.angular.x = rate_nav.x();
377 msg.twist.twist.angular.y = rate_nav.y();
378 msg.twist.twist.angular.z = rate_nav.z();
381 if (with_covariances) {
382 Eigen::Map< Eigen::Matrix<geometry_msgs::PoseWithCovariance::_covariance_type::value_type,6,6> > pose_covariance_msg(msg.pose.covariance.data());
383 Eigen::Map< Eigen::Matrix<geometry_msgs::TwistWithCovariance::_covariance_type::value_type,6,6> > twist_covariance_msg(msg.twist.covariance.data());
387 pose_covariance_msg.block<3,3>(0,0) =
state().
position()->getCovariance();
391 if (
state().orientation()) {
392 pose_covariance_msg.block<3,3>(3,3) =
state().
orientation()->getCovariance();
396 if (
state().position() &&
state().orientation()) {
398 pose_covariance_msg.block<3,3>(3,0) = pose_covariance_msg.block<3,3>(0,3).transpose();
402 if (
state().velocity()) {
403 twist_covariance_msg.block<3,3>(0,0) =
state().
velocity()->getCovariance();
407 if (
state().rate()) {
408 twist_covariance_msg.block<3,3>(3,3) =
state().
rate()->getCovariance();
413 pose_covariance_msg.block<3,3>(0,3) =
state().
velocity()->getCrossVariance(*
state().rate());
414 pose_covariance_msg.block<3,3>(3,0) = pose_covariance_msg.block<3,3>(0,3).transpose();
427 getPose(static_cast<tf::Pose &>(pose));
444 point =
tf::Point(position.x(), position.y(), position.z());
455 point.x = position.x();
456 point.y = position.y();
457 point.z = position.z();
475 getGlobal(latitude, longitude, altitude);
481 global.latitude *= 180.0/M_PI;
482 global.longitude *= 180.0/M_PI;
491 global.status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
493 global.status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;
497 global.latitude *= 180.0/M_PI;
498 global.longitude *= 180.0/M_PI;
501 global.status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
503 global.status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;
516 quaternion.w = global_orientation.w();
517 quaternion.x = global_orientation.x();
518 quaternion.y = global_orientation.y();
519 quaternion.z = global_orientation.z();
524 getGlobal(global.position, global.orientation);
529 quaternion =
tf::Quaternion(orientation.x(), orientation.y(), orientation.z(), orientation.w());
540 quaternion.w = orientation.w();
541 quaternion.x = orientation.x();
542 quaternion.y = orientation.y();
543 quaternion.z = orientation.z();
554 #ifdef TF_MATRIX3x3_H 557 btMatrix3x3(quaternion).getRPY(roll, pitch, yaw);
566 linear_acceleration.x = input->getAcceleration().x();
567 linear_acceleration.y = input->getAcceleration().y();
568 linear_acceleration.z = input->getAcceleration().z();
570 linear_acceleration.x = 0.0;
571 linear_acceleration.y = 0.0;
572 linear_acceleration.z = 0.0;
576 linear_acceleration.x -= accel->getModel()->getError().x();
577 linear_acceleration.y -= accel->getModel()->getError().y();
578 linear_acceleration.z -= accel->getModel()->getError().z();
586 vector =
tf::Vector3(velocity.x(), velocity.y(), velocity.z());
597 vector.x = velocity.x();
598 vector.y = velocity.y();
599 vector.z = velocity.z();
608 geometry_msgs::Vector3 rate;
614 getRate(static_cast<tf::Vector3 &>(vector));
620 if (
state().rate()) {
631 vector.x = input->getRate().x();
632 vector.y = input->getRate().y();
633 vector.z = input->getRate().z();
641 vector.x -= gyro->getModel()->getError().x();
642 vector.y -= gyro->getModel()->getError().y();
643 vector.z -= gyro->getModel()->getError().z();
659 angular_velocity.x = gyro->
getModel()->getError().x();
660 angular_velocity.y = gyro->getModel()->getError().y();
661 angular_velocity.z = gyro->getModel()->getError().z();
663 angular_velocity.x = 0.0;
664 angular_velocity.y = 0.0;
665 angular_velocity.z = 0.0;
669 linear_acceleration.x = accel->getModel()->getError().x();
670 linear_acceleration.y = accel->getModel()->getError().y();
671 linear_acceleration.z = accel->getModel()->getError().z();
673 linear_acceleration.x = 0.0;
674 linear_acceleration.y = 0.0;
675 linear_acceleration.z = 0.0;
679 void PoseEstimation::getBias(geometry_msgs::Vector3Stamped& angular_velocity, geometry_msgs::Vector3Stamped& linear_acceleration) {
680 getBias(angular_velocity.vector, linear_acceleration.vector);
713 transform = footprint_transform.
inverseTimes(transform);
718 #ifdef TF_MATRIX3x3_H 721 btMatrix3x3 rollpitch_rotation; rollpitch_rotation.
setEulerYPR(0.0, p, r);
723 stabilized_transform = stabilized_transform *
tf::Transform(rollpitch_rotation.inverse());
727 transform = stabilized_transform.
inverseTimes(transform);
virtual bool setSystemStatus(SystemStatus new_status)
const SystemPtr & addSystem(const SystemPtr &system, const std::string &name="system")
virtual void getRate(tf::Vector3 &vector)
boost::shared_ptr< ZeroRate > zerorate_update_
TFSIMD_FORCE_INLINE void setX(tfScalar x)
virtual const GlobalReferencePtr & globalReference()
virtual bool updateMeasurementStatus(SystemStatus set, SystemStatus clear)
TFSIMD_FORCE_INLINE void setY(tfScalar y)
virtual const boost::shared_ptr< OrientationStateType > & orientation() const
virtual ParameterList & parameters()
virtual const State & state() const
void getEulerYPR(tfScalar &yaw, tfScalar &pitch, tfScalar &roll, unsigned int solution_number=1) const
SymmetricMatrix Covariance
InputPtr getInput(const std::string &name) const
static const GlobalReferencePtr & Instance()
virtual void getPose(tf::Pose &pose)
const Ptr & add(const Ptr &p, const key_type &key)
PoseEstimation(const SystemPtr &system=SystemPtr(), const StatePtr &state=StatePtr())
virtual void updateWorldToOtherTransform(tf::StampedTransform &world_to_other_transform)
const_iterator end() const
const_iterator begin() const
TFSIMD_FORCE_INLINE void setZ(tfScalar z)
const State::RotationMatrix & R() const
void setEulerYPR(tfScalar eulerZ, tfScalar eulerY, tfScalar eulerX)
virtual const boost::shared_ptr< PositionStateType > & position() const
std::string footprint_frame_
void setRate(const Eigen::MatrixBase< Derived > &rate)
TFSIMD_FORCE_INLINE const tfScalar & y() const
InputPtr setInput(const Input &input, std::string name=std::string())
Ptr get(const key_type &key) const
virtual ~PoseEstimation()
SystemPtr getSystem(const std::string &name) const
ParameterList & add(const std::string &key, T &value, const T &default_value)
TFSIMD_FORCE_INLINE const tfScalar & x() const
Eigen::Quaternion< ScalarType > Quaternion
virtual const Covariance & getCovariance() const
virtual void getImuWithBiases(geometry_msgs::Vector3 &linear_acceleration, geometry_msgs::Vector3 &angular_velocity)
virtual void getTransforms(std::vector< tf::StampedTransform > &transforms)
std::string position_frame_
std::string stabilized_frame_
TFSIMD_FORCE_INLINE const tfScalar & z() const
virtual const ros::Time & getTimestamp() const
boost::shared_ptr< Gravity > gravity_update_
void getRPY(tfScalar &roll, tfScalar &pitch, tfScalar &yaw, unsigned int solution_number=1) const
virtual void getHeader(std_msgs::Header &header)
ListType::const_iterator iterator
virtual void getPosition(tf::Point &point)
void update(ros::Time timestamp)
Measurements measurements_
virtual SystemStatus getMeasurementStatus() const
TFSIMD_FORCE_INLINE const tfScalar & y() const
virtual const Vector & getVector() const
virtual bool inSystemStatus(SystemStatus test_status) const
const MeasurementPtr & addMeasurement(const MeasurementPtr &measurement, const std::string &name=std::string())
ros::Time alignment_start_
unsigned int SystemStatus
static PoseEstimation * Instance()
virtual const boost::shared_ptr< VelocityStateType > & velocity() const
virtual void getState(nav_msgs::Odometry &state, bool with_covariances=true)
boost::shared_ptr< InputType > addInput(const std::string &name=std::string())
virtual bool updateSystemStatus(SystemStatus set, SystemStatus clear)
virtual SystemStatus getSystemStatus() const
virtual bool setMeasurementStatus(SystemStatus new_status)
void setTimestamp(const ros::Time ×tamp)
boost::shared_ptr< Rate > rate_update_
virtual bool getWorldToNavTransform(geometry_msgs::TransformStamped &transform)
virtual void getGlobalPosition(double &latitude, double &longitude, double &altitude)
virtual bool setSystemStatus(SystemStatus new_status)
virtual Model * getModel() const
virtual bool inSystemStatus(SystemStatus test_status) const
virtual void setTimestamp(const ros::Time ×tamp)
virtual const boost::shared_ptr< RateStateType > & rate() const
virtual void getVelocity(tf::Vector3 &vector)
VectorBlock< const Vector, 3 > ConstRateType
virtual const State::Covariance & getCovariance()
void setAcceleration(const Eigen::MatrixBase< Derived > &acceleration)
virtual const State::Vector & getStateVector()
virtual SystemStatus getSystemStatus() const
virtual bool setMeasurementStatus(SystemStatus new_status)
virtual bool updateSystemStatus(SystemStatus set, SystemStatus clear)
VectorBlock< const Vector, 3 > ConstPositionType
virtual void getBias(geometry_msgs::Vector3 &angular_velocity, geometry_msgs::Vector3 &linear_acceleration)
boost::shared_ptr< Input > InputPtr
virtual SystemStatus getMeasurementStatus() const
const ros::Time & getTimestamp() const
virtual bool updateMeasurementStatus(SystemStatus set, SystemStatus clear)
ColumnVector_< 3 >::type ColumnVector3
virtual void getGlobal(double &latitude, double &longitude, double &altitude)
VectorBlock< const Vector, 3 > ConstVelocityType
virtual void getOrientation(tf::Quaternion &quaternion)