Public Member Functions | Protected Member Functions | Protected Attributes | List of all members
omip::RecursiveEstimatorNodeInterface< MeasurementTypeROS, StateTypeROS, RecursiveEstimatorFilterClass > Class Template Referenceabstract

This class defines an INTERFACE that all recursive estimator NODES need to implement It uses ROS TODO: For multimodality I will need different measurement types. More...

#include <RecursiveEstimatorNodeInterface.h>

Public Member Functions

template<class T >
bool getROSParameter (std::string param_name, T &param_container)
 
template<class T >
bool getROSParameter (std::string param_name, T &param_container, const T &default_value)
 
virtual void measurementCallback (const boost::shared_ptr< MeasurementTypeROS const > &measurement)=0
 Callback for the state predictions. More...
 
virtual void quitCallback (const std_msgs::EmptyConstPtr &msg)
 Callback to exit the node. More...
 
 RecursiveEstimatorNodeInterface (int num_external_state_predictors)
 Constructor. More...
 
virtual void run ()
 Creates a thread per state predictor that listens to their predictions, and this thread will listen to measurements. More...
 
virtual void spinStatePredictorQueue (int state_prediction_queue_idx)
 Spins one queue of predictors. More...
 
virtual void statePredictionCallback (const boost::shared_ptr< StateTypeROS const > &predicted_next_state)=0
 Callback for the state predictions. More...
 
virtual ~RecursiveEstimatorNodeInterface ()
 Destructor. More...
 

Protected Member Functions

virtual void _publishPredictedMeasurement () const =0
 Publish the prediction about the next measurement by this RE level. More...
 
virtual void _publishState () const =0
 Publish the current state of this RE level. More...
 

Protected Attributes

bool _active
 
ros::Time _current_measurement_time
 
ros::Duration _loop_period_ns
 
ros::Publisher _measurement_prediction_publisher
 
ros::Subscriber _measurement_subscriber
 
ros::NodeHandle _measurements_node_handle
 
ros::CallbackQueue_measurements_queue
 
std::string _namespace
 
ros::Subscriber _node_quit_subscriber
 
int _num_external_state_predictors
 
ros::Time _previous_measurement_time
 
RecursiveEstimatorFilterClass * _re_filter
 
std::vector< ros::NodeHandle_state_prediction_node_handles
 
ros::Publisher _state_prediction_publisher
 
std::vector< ros::CallbackQueue * > _state_prediction_queues
 
ros::Subscriber _state_prediction_subscriber
 
std::vector< boost::thread * > _state_predictor_listener_threads
 
ros::Publisher _state_publisher
 

Detailed Description

template<class MeasurementTypeROS, class StateTypeROS, class RecursiveEstimatorFilterClass>
class omip::RecursiveEstimatorNodeInterface< MeasurementTypeROS, StateTypeROS, RecursiveEstimatorFilterClass >

This class defines an INTERFACE that all recursive estimator NODES need to implement It uses ROS TODO: For multimodality I will need different measurement types.

Definition at line 52 of file RecursiveEstimatorNodeInterface.h.

Constructor & Destructor Documentation

template<class MeasurementTypeROS , class StateTypeROS , class RecursiveEstimatorFilterClass >
omip::RecursiveEstimatorNodeInterface< MeasurementTypeROS, StateTypeROS, RecursiveEstimatorFilterClass >::RecursiveEstimatorNodeInterface ( int  num_external_state_predictors)
inline

Constructor.

Parameters
num_external_state_predictors- Number of state predictors that the node will connect to

Definition at line 61 of file RecursiveEstimatorNodeInterface.h.

template<class MeasurementTypeROS , class StateTypeROS , class RecursiveEstimatorFilterClass >
virtual omip::RecursiveEstimatorNodeInterface< MeasurementTypeROS, StateTypeROS, RecursiveEstimatorFilterClass >::~RecursiveEstimatorNodeInterface ( )
inlinevirtual

Destructor.

Definition at line 92 of file RecursiveEstimatorNodeInterface.h.

Member Function Documentation

template<class MeasurementTypeROS , class StateTypeROS , class RecursiveEstimatorFilterClass >
virtual void omip::RecursiveEstimatorNodeInterface< MeasurementTypeROS, StateTypeROS, RecursiveEstimatorFilterClass >::_publishPredictedMeasurement ( ) const
protectedpure virtual

Publish the prediction about the next measurement by this RE level.

template<class MeasurementTypeROS , class StateTypeROS , class RecursiveEstimatorFilterClass >
virtual void omip::RecursiveEstimatorNodeInterface< MeasurementTypeROS, StateTypeROS, RecursiveEstimatorFilterClass >::_publishState ( ) const
protectedpure virtual

Publish the current state of this RE level.

template<class MeasurementTypeROS , class StateTypeROS , class RecursiveEstimatorFilterClass >
template<class T >
bool omip::RecursiveEstimatorNodeInterface< MeasurementTypeROS, StateTypeROS, RecursiveEstimatorFilterClass >::getROSParameter ( std::string  param_name,
T &  param_container 
)
inline

Gets a parameter from ROS

Definition at line 163 of file RecursiveEstimatorNodeInterface.h.

template<class MeasurementTypeROS , class StateTypeROS , class RecursiveEstimatorFilterClass >
template<class T >
bool omip::RecursiveEstimatorNodeInterface< MeasurementTypeROS, StateTypeROS, RecursiveEstimatorFilterClass >::getROSParameter ( std::string  param_name,
T &  param_container,
const T &  default_value 
)
inline

Gets a parameter from ROS with a default value if it is not available

Definition at line 179 of file RecursiveEstimatorNodeInterface.h.

template<class MeasurementTypeROS , class StateTypeROS , class RecursiveEstimatorFilterClass >
virtual void omip::RecursiveEstimatorNodeInterface< MeasurementTypeROS, StateTypeROS, RecursiveEstimatorFilterClass >::measurementCallback ( const boost::shared_ptr< MeasurementTypeROS const > &  measurement)
pure virtual

Callback for the state predictions.

Parameters
predicted_next_state
template<class MeasurementTypeROS , class StateTypeROS , class RecursiveEstimatorFilterClass >
virtual void omip::RecursiveEstimatorNodeInterface< MeasurementTypeROS, StateTypeROS, RecursiveEstimatorFilterClass >::quitCallback ( const std_msgs::EmptyConstPtr &  msg)
inlinevirtual

Callback to exit the node.

Parameters
msg- Empty message that signals that the node should terminate

Definition at line 153 of file RecursiveEstimatorNodeInterface.h.

template<class MeasurementTypeROS , class StateTypeROS , class RecursiveEstimatorFilterClass >
virtual void omip::RecursiveEstimatorNodeInterface< MeasurementTypeROS, StateTypeROS, RecursiveEstimatorFilterClass >::run ( )
inlinevirtual

Creates a thread per state predictor that listens to their predictions, and this thread will listen to measurements.

Definition at line 105 of file RecursiveEstimatorNodeInterface.h.

template<class MeasurementTypeROS , class StateTypeROS , class RecursiveEstimatorFilterClass >
virtual void omip::RecursiveEstimatorNodeInterface< MeasurementTypeROS, StateTypeROS, RecursiveEstimatorFilterClass >::spinStatePredictorQueue ( int  state_prediction_queue_idx)
inlinevirtual

Spins one queue of predictors.

Parameters
state_prediction_queue_idx- The idx of the queue of predictions

Definition at line 126 of file RecursiveEstimatorNodeInterface.h.

template<class MeasurementTypeROS , class StateTypeROS , class RecursiveEstimatorFilterClass >
virtual void omip::RecursiveEstimatorNodeInterface< MeasurementTypeROS, StateTypeROS, RecursiveEstimatorFilterClass >::statePredictionCallback ( const boost::shared_ptr< StateTypeROS const > &  predicted_next_state)
pure virtual

Callback for the state predictions.

Parameters
predicted_next_state

Member Data Documentation

template<class MeasurementTypeROS , class StateTypeROS , class RecursiveEstimatorFilterClass >
bool omip::RecursiveEstimatorNodeInterface< MeasurementTypeROS, StateTypeROS, RecursiveEstimatorFilterClass >::_active
protected

Definition at line 235 of file RecursiveEstimatorNodeInterface.h.

template<class MeasurementTypeROS , class StateTypeROS , class RecursiveEstimatorFilterClass >
ros::Time omip::RecursiveEstimatorNodeInterface< MeasurementTypeROS, StateTypeROS, RecursiveEstimatorFilterClass >::_current_measurement_time
protected

Definition at line 231 of file RecursiveEstimatorNodeInterface.h.

template<class MeasurementTypeROS , class StateTypeROS , class RecursiveEstimatorFilterClass >
ros::Duration omip::RecursiveEstimatorNodeInterface< MeasurementTypeROS, StateTypeROS, RecursiveEstimatorFilterClass >::_loop_period_ns
protected

Definition at line 233 of file RecursiveEstimatorNodeInterface.h.

template<class MeasurementTypeROS , class StateTypeROS , class RecursiveEstimatorFilterClass >
ros::Publisher omip::RecursiveEstimatorNodeInterface< MeasurementTypeROS, StateTypeROS, RecursiveEstimatorFilterClass >::_measurement_prediction_publisher
protected

Definition at line 223 of file RecursiveEstimatorNodeInterface.h.

template<class MeasurementTypeROS , class StateTypeROS , class RecursiveEstimatorFilterClass >
ros::Subscriber omip::RecursiveEstimatorNodeInterface< MeasurementTypeROS, StateTypeROS, RecursiveEstimatorFilterClass >::_measurement_subscriber
protected

Definition at line 218 of file RecursiveEstimatorNodeInterface.h.

template<class MeasurementTypeROS , class StateTypeROS , class RecursiveEstimatorFilterClass >
ros::NodeHandle omip::RecursiveEstimatorNodeInterface< MeasurementTypeROS, StateTypeROS, RecursiveEstimatorFilterClass >::_measurements_node_handle
protected

Definition at line 228 of file RecursiveEstimatorNodeInterface.h.

template<class MeasurementTypeROS , class StateTypeROS , class RecursiveEstimatorFilterClass >
ros::CallbackQueue* omip::RecursiveEstimatorNodeInterface< MeasurementTypeROS, StateTypeROS, RecursiveEstimatorFilterClass >::_measurements_queue
protected

Definition at line 227 of file RecursiveEstimatorNodeInterface.h.

template<class MeasurementTypeROS , class StateTypeROS , class RecursiveEstimatorFilterClass >
std::string omip::RecursiveEstimatorNodeInterface< MeasurementTypeROS, StateTypeROS, RecursiveEstimatorFilterClass >::_namespace
protected

Definition at line 208 of file RecursiveEstimatorNodeInterface.h.

template<class MeasurementTypeROS , class StateTypeROS , class RecursiveEstimatorFilterClass >
ros::Subscriber omip::RecursiveEstimatorNodeInterface< MeasurementTypeROS, StateTypeROS, RecursiveEstimatorFilterClass >::_node_quit_subscriber
protected

Definition at line 220 of file RecursiveEstimatorNodeInterface.h.

template<class MeasurementTypeROS , class StateTypeROS , class RecursiveEstimatorFilterClass >
int omip::RecursiveEstimatorNodeInterface< MeasurementTypeROS, StateTypeROS, RecursiveEstimatorFilterClass >::_num_external_state_predictors
protected

Definition at line 212 of file RecursiveEstimatorNodeInterface.h.

template<class MeasurementTypeROS , class StateTypeROS , class RecursiveEstimatorFilterClass >
ros::Time omip::RecursiveEstimatorNodeInterface< MeasurementTypeROS, StateTypeROS, RecursiveEstimatorFilterClass >::_previous_measurement_time
protected

Definition at line 232 of file RecursiveEstimatorNodeInterface.h.

template<class MeasurementTypeROS , class StateTypeROS , class RecursiveEstimatorFilterClass >
RecursiveEstimatorFilterClass* omip::RecursiveEstimatorNodeInterface< MeasurementTypeROS, StateTypeROS, RecursiveEstimatorFilterClass >::_re_filter
protected

Definition at line 206 of file RecursiveEstimatorNodeInterface.h.

template<class MeasurementTypeROS , class StateTypeROS , class RecursiveEstimatorFilterClass >
std::vector<ros::NodeHandle> omip::RecursiveEstimatorNodeInterface< MeasurementTypeROS, StateTypeROS, RecursiveEstimatorFilterClass >::_state_prediction_node_handles
protected

Definition at line 213 of file RecursiveEstimatorNodeInterface.h.

template<class MeasurementTypeROS , class StateTypeROS , class RecursiveEstimatorFilterClass >
ros::Publisher omip::RecursiveEstimatorNodeInterface< MeasurementTypeROS, StateTypeROS, RecursiveEstimatorFilterClass >::_state_prediction_publisher
protected

Definition at line 222 of file RecursiveEstimatorNodeInterface.h.

template<class MeasurementTypeROS , class StateTypeROS , class RecursiveEstimatorFilterClass >
std::vector<ros::CallbackQueue*> omip::RecursiveEstimatorNodeInterface< MeasurementTypeROS, StateTypeROS, RecursiveEstimatorFilterClass >::_state_prediction_queues
protected

Definition at line 214 of file RecursiveEstimatorNodeInterface.h.

template<class MeasurementTypeROS , class StateTypeROS , class RecursiveEstimatorFilterClass >
ros::Subscriber omip::RecursiveEstimatorNodeInterface< MeasurementTypeROS, StateTypeROS, RecursiveEstimatorFilterClass >::_state_prediction_subscriber
protected

Definition at line 219 of file RecursiveEstimatorNodeInterface.h.

template<class MeasurementTypeROS , class StateTypeROS , class RecursiveEstimatorFilterClass >
std::vector<boost::thread*> omip::RecursiveEstimatorNodeInterface< MeasurementTypeROS, StateTypeROS, RecursiveEstimatorFilterClass >::_state_predictor_listener_threads
protected

Definition at line 215 of file RecursiveEstimatorNodeInterface.h.

template<class MeasurementTypeROS , class StateTypeROS , class RecursiveEstimatorFilterClass >
ros::Publisher omip::RecursiveEstimatorNodeInterface< MeasurementTypeROS, StateTypeROS, RecursiveEstimatorFilterClass >::_state_publisher
protected

Definition at line 221 of file RecursiveEstimatorNodeInterface.h.


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


omip_common
Author(s): Roberto Martín-Martín
autogenerated on Mon Jun 10 2019 14:06:05