29 #ifndef HECTOR_POSE_ESTIMATION_H 30 #define HECTOR_POSE_ESTIMATION_H 38 #include <boost/shared_ptr.hpp> 46 #include <geometry_msgs/PoseStamped.h> 47 #include <geometry_msgs/PointStamped.h> 48 #include <geometry_msgs/QuaternionStamped.h> 49 #include <geometry_msgs/Vector3Stamped.h> 50 #include <nav_msgs/Odometry.h> 51 #include <sensor_msgs/NavSatFix.h> 52 #include <geographic_msgs/GeoPoint.h> 53 #include <geographic_msgs/GeoPose.h> 58 class GlobalReference;
78 template <
typename ConcreteSystemModel>
const SystemPtr&
addSystem(ConcreteSystemModel *model,
const std::string& name =
"system");
85 template <
class ConcreteMeasurementModel>
const MeasurementPtr&
addMeasurement(ConcreteMeasurementModel *model,
const std::string& name);
118 virtual void getHeader(std_msgs::Header& header);
119 virtual void getState(nav_msgs::Odometry&
state,
bool with_covariances =
true);
122 virtual void getPose(geometry_msgs::Pose& pose);
123 virtual void getPose(geometry_msgs::PoseStamped& pose);
126 virtual void getPosition(geometry_msgs::Point& pose);
127 virtual void getPosition(geometry_msgs::PointStamped& pose);
128 virtual void getGlobal(
double& latitude,
double& longitude,
double& altitude);
129 virtual void getGlobalPosition(
double& latitude,
double& longitude,
double& altitude);
130 virtual void getGlobal(geographic_msgs::GeoPoint& global);
131 virtual void getGlobal(sensor_msgs::NavSatFix& global);
134 virtual void getGlobal(geographic_msgs::GeoPose& global);
138 virtual void getOrientation(geometry_msgs::QuaternionStamped& pose);
139 virtual void getOrientation(
double &yaw,
double &pitch,
double &roll);
140 virtual void getImuWithBiases(geometry_msgs::Vector3& linear_acceleration, geometry_msgs::Vector3& angular_velocity);
143 virtual void getVelocity(geometry_msgs::Vector3& vector);
144 virtual void getVelocity(geometry_msgs::Vector3Stamped& vector);
147 virtual void getRate(geometry_msgs::Vector3& vector);
148 virtual void getRate(geometry_msgs::Vector3Stamped& vector);
151 virtual void getBias(geometry_msgs::Vector3& angular_velocity, geometry_msgs::Vector3& linear_acceleration);
152 virtual void getBias(geometry_msgs::Vector3Stamped& angular_velocity, geometry_msgs::Vector3Stamped& linear_acceleration);
153 virtual void getTransforms(std::vector<tf::StampedTransform>& transforms);
194 template <
typename ConcreteSystemModel>
199 template <
typename ConcreteSystemModel>
205 template <
typename SystemType>
208 return boost::static_pointer_cast<SystemType>(
getSystem(name));
211 template <
class ConcreteMeasurementModel>
216 template <
typename MeasurementType>
219 return boost::static_pointer_cast<MeasurementType>(
getMeasurement(name));
222 template <
class InputType>
225 if (input)
return input;
227 input.reset(
new InputType());
228 if (!
addInput(boost::static_pointer_cast<Input>(input), name)) input.reset();
234 #endif // HECTOR_POSE_ESTIMATION_H static boost::shared_ptr< Measurement_< ConcreteModel > > create(ConcreteModel *model, const std::string &name)
virtual bool setSystemStatus(SystemStatus new_status)
const SystemPtr & addSystem(const SystemPtr &system, const std::string &name="system")
boost::shared_ptr< System > SystemPtr
virtual void getRate(tf::Vector3 &vector)
boost::shared_ptr< ZeroRate > zerorate_update_
boost::shared_ptr< SystemType > getSystem_(const std::string &name) const
virtual const GlobalReferencePtr & globalReference()
virtual bool updateMeasurementStatus(SystemStatus set, SystemStatus clear)
virtual ParameterList & parameters()
virtual const State & state() const
SymmetricMatrix Covariance
InputPtr getInput(const std::string &name) const
virtual void getPose(tf::Pose &pose)
PoseEstimation(const SystemPtr &system=SystemPtr(), const StatePtr &state=StatePtr())
ParameterList parameters_
virtual void updateWorldToOtherTransform(tf::StampedTransform &world_to_other_transform)
MeasurementPtr getMeasurement(const std::string &name) const
boost::shared_ptr< Measurement > MeasurementPtr
static boost::shared_ptr< System_< ConcreteModel > > create(ConcreteModel *model, const std::string &name="system")
std::string footprint_frame_
const SystemPtr & addSystem(System *system)
const MeasurementPtr & addMeasurement(Measurement *measurement)
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
boost::shared_ptr< MeasurementType > getMeasurement_(const std::string &name) const
Eigen::Quaternion< ScalarType > Quaternion
virtual boost::shared_ptr< const Filter > filter() 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_
virtual const ros::Time & getTimestamp() const
boost::shared_ptr< Gravity > gravity_update_
virtual void getHeader(std_msgs::Header &header)
virtual void getPosition(tf::Point &point)
void update(ros::Time timestamp)
Measurements measurements_
virtual SystemStatus getMeasurementStatus() const
boost::shared_ptr< Derived > getType(const key_type &key) const
const MeasurementPtr & addMeasurement(const MeasurementPtr &measurement, const std::string &name=std::string())
ros::Time alignment_start_
unsigned int SystemStatus
boost::shared_ptr< InputType > getInputType(const std::string &name) const
static PoseEstimation * Instance()
virtual boost::shared_ptr< Filter > filter()
virtual void getState(nav_msgs::Odometry &state, bool with_covariances=true)
boost::shared_ptr< InputType > addInput(const std::string &name=std::string())
boost::shared_ptr< Rate > rate_update_
virtual bool getWorldToNavTransform(geometry_msgs::TransformStamped &transform)
virtual void getGlobalPosition(double &latitude, double &longitude, double &altitude)
InputPtr addInput(Input *input, const std::string &name=std::string())
virtual bool inSystemStatus(SystemStatus test_status) const
virtual void setTimestamp(const ros::Time ×tamp)
virtual const ParameterList & parameters() const
virtual void getVelocity(tf::Vector3 &vector)
virtual const State::Covariance & getCovariance()
virtual const State::Vector & getStateVector()
virtual SystemStatus getSystemStatus() const
virtual bool setMeasurementStatus(SystemStatus new_status)
virtual bool updateSystemStatus(SystemStatus set, SystemStatus clear)
virtual void getBias(geometry_msgs::Vector3 &angular_velocity, geometry_msgs::Vector3 &linear_acceleration)
boost::shared_ptr< Input > InputPtr
boost::shared_ptr< State > StatePtr
virtual void getGlobal(double &latitude, double &longitude, double &altitude)
virtual void getOrientation(tf::Quaternion &quaternion)