Skip to content
Snippets Groups Projects
Commit e27109f2 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Add odometry things to IsMotion

parent c0fb2122
No related branches found
No related tags found
1 merge request!387Resolve "Publish odom"
This commit is part of merge request !387. Comments created here will be created in the context of that merge request.
......@@ -30,12 +30,18 @@ class IsMotion
virtual VectorComposite getState(const StateStructure& _structure = "") const = 0;
virtual VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const = 0;
VectorComposite getOdometry ( ) const;
private:
void setOdometry(const VectorComposite& _zero_odom) {odometry_ = _zero_odom;}
public:
const StateStructure& getStateStructure ( ) { return state_structure_; };
void setStateStructure(std::string _state_structure) { state_structure_ = _state_structure; };
void addToProblem(ProblemPtr _prb_ptr, IsMotionPtr _motion_ptr);
protected:
StateStructure state_structure_; ///< The structure of the state vector (to retrieve state blocks from frames)
VectorComposite odometry_;
};
......@@ -45,6 +51,12 @@ inline IsMotion::IsMotion(const StateStructure& _structure) :
//
}
inline wolf::VectorComposite IsMotion::getOdometry ( ) const
{
return odometry_;
}
}
///// IMPLEMENTATION ///////
......
......@@ -5,5 +5,6 @@ using namespace wolf;
void IsMotion::addToProblem(ProblemPtr _prb_ptr, IsMotionPtr _motion_ptr)
{
setOdometry(_prb_ptr->stateZero(state_structure_));
_prb_ptr->addProcessorIsMotion(_motion_ptr);
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment