AnymalStateEstimator
The state estimator for anymal.
any_state_estimator::AnyStateEstimator< RobotContainersRos_ > Class Template Referenceabstract

Base class for a generic state estimator using an imu and actuator readings as input and publishes a robot state as output. More...

#include <AnyStateEstimator.hpp>

Inheritance diagram for any_state_estimator::AnyStateEstimator< RobotContainersRos_ >:
Inheritance graph

Public Types

using RobotContainer = typename RobotContainersRos_::ConcreteContainers
 
using RobotContainerRos = RobotContainersRos_
 
using RobotStateContainer = typename RobotContainerRos::RobotStateRos
 
using ActuatorReadingsContainer = typename RobotContainerRos::ActuatorReadingsRos
 
using ImuContainer = typename RobotContainerRos::ImuRos
 
using ImuShm = typename ImuContainer::type
 
using ImuRos = typename ImuContainer::msgType
 
using RobotStateShm = typename RobotStateContainer::type
 
using RobotStateRos = typename RobotStateContainer::msgType
 
using ActuatorReadingsShm = typename ActuatorReadingsContainer::type
 
using ActuatorReadingsRos = typename ActuatorReadingsContainer::msgType
 

Public Member Functions

 AnyStateEstimator ()=delete
 
 AnyStateEstimator (any_node::Node::NodeHandlePtr nh)
 Constructor. More...
 
 ~AnyStateEstimator () override=default
 
bool init () override
 
void preCleanup () override
 
void cleanup () override
 
bool update (const any_worker::WorkerEvent &event)
 Is called periodically to update the estimator when running standalone. More...
 

Protected Member Functions

virtual bool resetEstimator (const kindr::HomTransformQuatD &pose)=0
 Resets the state estimator to pose. More...
 
virtual bool resetEstimatorHere ()=0
 Resets the state estimator at the current pose. More...
 
virtual void receiveMeasurements ()=0
 Receives measurements from your subscriber. Gets called before preprocessMeasurements. More...
 
virtual void preprocessMeasurements ()=0
 Preprocesses the measurements. Gets called before advancing the estimator. More...
 
virtual void advanceEstimator ()=0
 Advances the estimator by updating the filter. Gets called before setting the estimator output. More...
 
virtual void setOutput ()=0
 Prepare the output to be published. Gets called before publishing. More...
 
virtual void publish ()=0
 Publishes desired output via cosmo. More...
 
virtual void initImpl ()
 Initializes further objects. More...
 
virtual void initializeMessages ()
 Initializes messages in derived class. More...
 
virtual void readParameters ()
 Reads parameters from the parameter server. More...
 
virtual void initializePublishers ()
 Initializes additional publishers in the derived class. More...
 
virtual void initializeSubscribers ()
 Initializes additional subscribers in the derived class. More...
 
virtual void advertiseServices ()
 Initializes services. More...
 
virtual void addVariablesToLog ()
 Adds variables to the signal logger. More...
 
virtual void publishRos ()
 Publishes data over ros. More...
 
void update ()
 The main update function of the estimator. Calls receiveMeasurements, preprocessMeasurements, advanceEstimator, setOutput and publish. More...
 
void startWorkers ()
 Starts workers for the synced update and ros publishing. More...
 
bool updateSynced (const any_worker::WorkerEvent &event)
 Calls the update method and afterwards sleeps until the next iteration. More...
 
bool publishRos (const any_worker::WorkerEvent &event)
 Publishes data over ros via publishRos() and afterwards sleeps until the next update has finished. More...
 
void imuCallback (const ImuShm &msg)
 Handles incoming imu messages. More...
 
void actuatorReadingsCallback (const ActuatorReadingsShm &msg)
 Handles incoming actuator reading messages. More...
 
bool resetService (any_state_estimator_msgs::ResetStateEstimator::Request &req, any_state_estimator_msgs::ResetStateEstimator::Response &res)
 Wraps reset estimator, is called by a ros service. More...
 
bool resetHereService (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
 Wraps reset estimator here, is called by a ros service. More...
 

Protected Attributes

double timeStep_ {0.0}
 
double initializationDuration_ {0.0}
 
double iterDurationMs_ {0.0}
 
double updateDurationMs_ {0.0}
 
RobotStateShm estimatedState_
 
ImuShm imu_
 
ActuatorReadingsShm actuatorReadings_
 
std::string syncSlaveName_
 
std::string syncMasterName_
 
std::unique_ptr< cosmo::SyncSlave > syncSlave_
 
std::unique_ptr< cosmo::SyncMaster > syncMaster_
 
const std::chrono::microseconds receiveMaxLockTime_ {std::chrono::microseconds{50}}
 
const std::chrono::microseconds sendMaxLockTime_ {std::chrono::microseconds{50}}
 
cosmo_ros::SubscriberRosPtr< ImuShm, ImuRos, ImuContainer::template ConversionTrait > imuSubscriber_
 
cosmo_ros::SubscriberRosPtr< ActuatorReadingsShm, ActuatorReadingsRos, ActuatorReadingsContainer::template ConversionTrait > actuatorReadingsSubscriber_
 
cosmo_ros::PublisherRosPtr< RobotStateShm, RobotStateRos, RobotStateContainer::template ConversionTrait > robotStatePublisher_
 
ros::ServiceServer resetService_
 
ros::ServiceServer resetHereService_
 
bool isSimulation_
 
bool isStandalone_
 
std::condition_variable_any cvUpdate_
 
std::atomic< bool > stopUpdating_ {true}
 
std::mutex mutexUpdate_
 
std::mutex mutexStateEstimator_
 
std::chrono::time_point< std::chrono::steady_clock > timePrevIteration_
 
std::atomic< unsigned long > updateCounter_ {0lu}
 

Detailed Description

template<typename RobotContainersRos_>
class any_state_estimator::AnyStateEstimator< RobotContainersRos_ >

Base class for a generic state estimator using an imu and actuator readings as input and publishes a robot state as output.

Template Parameters
RobotContainersRos_romo::RobotContainerRos

Member Typedef Documentation

template<typename RobotContainersRos_ >
using any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::ActuatorReadingsContainer = typename RobotContainerRos::ActuatorReadingsRos
template<typename RobotContainersRos_ >
using any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::ActuatorReadingsRos = typename ActuatorReadingsContainer::msgType
template<typename RobotContainersRos_ >
using any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::ActuatorReadingsShm = typename ActuatorReadingsContainer::type
template<typename RobotContainersRos_ >
using any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::ImuContainer = typename RobotContainerRos::ImuRos
template<typename RobotContainersRos_ >
using any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::ImuRos = typename ImuContainer::msgType
template<typename RobotContainersRos_ >
using any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::ImuShm = typename ImuContainer::type
template<typename RobotContainersRos_ >
using any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::RobotContainer = typename RobotContainersRos_::ConcreteContainers
template<typename RobotContainersRos_ >
using any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::RobotContainerRos = RobotContainersRos_
template<typename RobotContainersRos_ >
using any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::RobotStateContainer = typename RobotContainerRos::RobotStateRos
template<typename RobotContainersRos_ >
using any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::RobotStateRos = typename RobotStateContainer::msgType
template<typename RobotContainersRos_ >
using any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::RobotStateShm = typename RobotStateContainer::type

Constructor & Destructor Documentation

template<typename RobotContainersRos_ >
any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::AnyStateEstimator ( )
delete
template<typename RobotContainersRos_ >
any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::AnyStateEstimator ( any_node::Node::NodeHandlePtr  nh)
explicit

Constructor.

Parameters
[in]nhNodeHandle
template<typename RobotContainersRos_ >
any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::~AnyStateEstimator ( )
overridedefault

Member Function Documentation

template<typename RobotContainersRos_ >
void any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::actuatorReadingsCallback ( const ActuatorReadingsShm msg)
protected

Handles incoming actuator reading messages.

Parameters
[in]msgThe message
template<typename RobotContainersRos_ >
virtual void any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::addVariablesToLog ( )
inlineprotectedvirtual
template<typename RobotContainersRos_ >
virtual void any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::advanceEstimator ( )
protectedpure virtual

Advances the estimator by updating the filter. Gets called before setting the estimator output.

Implemented in anymal_state_estimator::AnymalStateEstimator< ConcreteDescription_, RobotState_, RobotContainersRos_, Filter_ >.

template<typename RobotContainersRos_ >
virtual void any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::advertiseServices ( )
inlineprotectedvirtual
template<typename RobotContainersRos_ >
void any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::cleanup ( )
override
template<typename RobotContainersRos_ >
void any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::imuCallback ( const ImuShm msg)
protected

Handles incoming imu messages.

Parameters
[in]msgThe message
template<typename RobotContainersRos_ >
bool any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::init ( )
override
template<typename RobotContainersRos_ >
virtual void any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::initializeMessages ( )
inlineprotectedvirtual
template<typename RobotContainersRos_ >
virtual void any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::initializePublishers ( )
inlineprotectedvirtual

Initializes additional publishers in the derived class.

Reimplemented in anymal_state_estimator::AnymalStateEstimator< ConcreteDescription_, RobotState_, RobotContainersRos_, Filter_ >.

template<typename RobotContainersRos_ >
virtual void any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::initializeSubscribers ( )
inlineprotectedvirtual

Initializes additional subscribers in the derived class.

Reimplemented in anymal_state_estimator::AnymalStateEstimator< ConcreteDescription_, RobotState_, RobotContainersRos_, Filter_ >.

template<typename RobotContainersRos_ >
virtual void any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::initImpl ( )
inlineprotectedvirtual
template<typename RobotContainersRos_ >
void any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::preCleanup ( )
override
template<typename RobotContainersRos_ >
virtual void any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::preprocessMeasurements ( )
protectedpure virtual

Preprocesses the measurements. Gets called before advancing the estimator.

Implemented in anymal_state_estimator::AnymalStateEstimator< ConcreteDescription_, RobotState_, RobotContainersRos_, Filter_ >.

template<typename RobotContainersRos_ >
virtual void any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::publish ( )
protectedpure virtual
template<typename RobotContainersRos_ >
virtual void any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::publishRos ( )
inlineprotectedvirtual
template<typename RobotContainersRos_ >
bool any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::publishRos ( const any_worker::WorkerEvent &  event)
protected

Publishes data over ros via publishRos() and afterwards sleeps until the next update has finished.

Parameters
[in]eventThe event
Returns
true if successful
template<typename RobotContainersRos_ >
virtual void any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::readParameters ( )
inlineprotectedvirtual
template<typename RobotContainersRos_ >
virtual void any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::receiveMeasurements ( )
protectedpure virtual

Receives measurements from your subscriber. Gets called before preprocessMeasurements.

Implemented in anymal_state_estimator::AnymalStateEstimator< ConcreteDescription_, RobotState_, RobotContainersRos_, Filter_ >.

template<typename RobotContainersRos_ >
virtual bool any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::resetEstimator ( const kindr::HomTransformQuatD &  pose)
protectedpure virtual

Resets the state estimator to pose.

Parameters
[in]poseThe initial pose
Returns
true if successful

Implemented in anymal_state_estimator::AnymalStateEstimator< ConcreteDescription_, RobotState_, RobotContainersRos_, Filter_ >.

template<typename RobotContainersRos_ >
virtual bool any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::resetEstimatorHere ( )
protectedpure virtual

Resets the state estimator at the current pose.

Returns
true if successful

Implemented in anymal_state_estimator::AnymalStateEstimator< ConcreteDescription_, RobotState_, RobotContainersRos_, Filter_ >.

template<typename RobotContainersRos_ >
bool any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::resetHereService ( std_srvs::Trigger::Request &  req,
std_srvs::Trigger::Response &  res 
)
protected

Wraps reset estimator here, is called by a ros service.

Parameters
reqThe reset request
resThe reset response
Returns
true if successful
template<typename RobotContainersRos_ >
bool any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::resetService ( any_state_estimator_msgs::ResetStateEstimator::Request &  req,
any_state_estimator_msgs::ResetStateEstimator::Response &  res 
)
protected

Wraps reset estimator, is called by a ros service.

Parameters
reqThe reset request
resThe reset response
Returns
true if successful
template<typename RobotContainersRos_ >
virtual void any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::setOutput ( )
protectedpure virtual

Prepare the output to be published. Gets called before publishing.

Implemented in anymal_state_estimator::AnymalStateEstimator< ConcreteDescription_, RobotState_, RobotContainersRos_, Filter_ >.

template<typename RobotContainersRos_ >
void any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::startWorkers ( )
protected

Starts workers for the synced update and ros publishing.

template<typename RobotContainersRos_ >
bool any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::update ( const any_worker::WorkerEvent &  event)

Is called periodically to update the estimator when running standalone.

Parameters
[in]eventThe event
Returns
true if successful
template<typename RobotContainersRos_ >
void any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::update ( )
protected

The main update function of the estimator. Calls receiveMeasurements, preprocessMeasurements, advanceEstimator, setOutput and publish.

template<typename RobotContainersRos_ >
bool any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::updateSynced ( const any_worker::WorkerEvent &  event)
protected

Calls the update method and afterwards sleeps until the next iteration.

Parameters
[in]eventThe event
Returns
true if successful

Member Data Documentation

template<typename RobotContainersRos_ >
ActuatorReadingsShm any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::actuatorReadings_
protected
template<typename RobotContainersRos_ >
cosmo_ros::SubscriberRosPtr<ActuatorReadingsShm, ActuatorReadingsRos, ActuatorReadingsContainer::template ConversionTrait> any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::actuatorReadingsSubscriber_
protected
template<typename RobotContainersRos_ >
std::condition_variable_any any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::cvUpdate_
protected
template<typename RobotContainersRos_ >
RobotStateShm any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::estimatedState_
protected
template<typename RobotContainersRos_ >
ImuShm any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::imu_
protected
template<typename RobotContainersRos_ >
cosmo_ros::SubscriberRosPtr<ImuShm, ImuRos, ImuContainer::template ConversionTrait> any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::imuSubscriber_
protected
template<typename RobotContainersRos_ >
double any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::initializationDuration_ {0.0}
protected
template<typename RobotContainersRos_ >
bool any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::isSimulation_
protected
template<typename RobotContainersRos_ >
bool any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::isStandalone_
protected
template<typename RobotContainersRos_ >
double any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::iterDurationMs_ {0.0}
protected
template<typename RobotContainersRos_ >
std::mutex any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::mutexStateEstimator_
protected
template<typename RobotContainersRos_ >
std::mutex any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::mutexUpdate_
protected
template<typename RobotContainersRos_ >
const std::chrono::microseconds any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::receiveMaxLockTime_ {std::chrono::microseconds{50}}
protected
template<typename RobotContainersRos_ >
ros::ServiceServer any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::resetHereService_
protected
template<typename RobotContainersRos_ >
ros::ServiceServer any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::resetService_
protected
template<typename RobotContainersRos_ >
cosmo_ros::PublisherRosPtr<RobotStateShm, RobotStateRos, RobotStateContainer::template ConversionTrait> any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::robotStatePublisher_
protected
template<typename RobotContainersRos_ >
const std::chrono::microseconds any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::sendMaxLockTime_ {std::chrono::microseconds{50}}
protected
template<typename RobotContainersRos_ >
std::atomic<bool> any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::stopUpdating_ {true}
protected
template<typename RobotContainersRos_ >
std::unique_ptr<cosmo::SyncMaster> any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::syncMaster_
protected
template<typename RobotContainersRos_ >
std::string any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::syncMasterName_
protected
template<typename RobotContainersRos_ >
std::unique_ptr<cosmo::SyncSlave> any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::syncSlave_
protected
template<typename RobotContainersRos_ >
std::string any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::syncSlaveName_
protected
template<typename RobotContainersRos_ >
std::chrono::time_point<std::chrono::steady_clock> any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::timePrevIteration_
protected
template<typename RobotContainersRos_ >
double any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::timeStep_ {0.0}
protected
template<typename RobotContainersRos_ >
std::atomic<unsigned long> any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::updateCounter_ {0lu}
protected
template<typename RobotContainersRos_ >
double any_state_estimator::AnyStateEstimator< RobotContainersRos_ >::updateDurationMs_ {0.0}
protected

The documentation for this class was generated from the following file: