Public Member Functions
bool	detectFailedEncoder ()
bool	detectFailedEncoderCandidate (VectorDrive::Index *candidate)
void	diagnostic (diagnostic_updater::DiagnosticStatusWrapper &)
void	driveCallback (const grizzly_msgs::DriveConstPtr &)
void	encodersCallback (const grizzly_msgs::DriveConstPtr &)
	EncodersMonitor ()
	EncodersMonitor (ros::NodeHandle *nh)
bool	moving ()
bool	ok ()
Public Attributes
ros::Duration	encoder_fault_time_to_failure
double	encoder_speed_error_diff_threshold
ros::Duration	encoders_timeout
Protected Attributes
int	failed_encoder_
grizzly_msgs::DriveConstPtr	last_received_drive_
grizzly_msgs::DriveConstPtr	last_received_encoders_
ros::Subscriber	sub_drive_
ros::Subscriber	sub_encoders_
ros::Time	time_of_last_nonsuspect_encoders_

Detailed Description

Definition at line 37 of file encoders_monitor.h.

Constructor & Destructor Documentation

EncodersMonitor::EncodersMonitor ( ) [inline]

Definition at line 39 of file encoders_monitor.h.

EncodersMonitor::EncodersMonitor ( ros::NodeHandle * nh )

Separate ROS initialization step for better testability.

Definition at line 32 of file encoders_monitor.cpp.

bool EncodersMonitor::detectFailedEncoder ( )

Definition at line 88 of file encoders_monitor.cpp.

bool EncodersMonitor::detectFailedEncoderCandidate ( VectorDrive::Index * candidate )

Definition at line 57 of file encoders_monitor.cpp.

void EncodersMonitor::diagnostic ( diagnostic_updater::DiagnosticStatusWrapper & stat )

Prepare diagnostics. Called at 1Hz by the Updater.

Definition at line 134 of file encoders_monitor.cpp.

void EncodersMonitor::driveCallback ( const grizzly_msgs::DriveConstPtr & drive )

New commands went out to the motors.

Definition at line 178 of file encoders_monitor.cpp.

void EncodersMonitor::encodersCallback ( const grizzly_msgs::DriveConstPtr & encoders )

New encoder data received. The important logic here is detecting when an encoder or the associated cabling has failed. The general principle is:

compared commanded speed to actual speed (for an "error" value)
monitor how much time each encoder spends over an acceptable threshold of error.
compute the variance of the time-in-excess-error array. If it exceeds a a heuristically-set value, conclude that there may be a failed encoder. Future work on this function could also account for current consumption by motors; eg, a spike in current by a motor relative to the others with no associated speed change.

Definition at line 170 of file encoders_monitor.cpp.

bool EncodersMonitor::moving ( )

Called in the context of making sure the vehicle is stopped before releasing the estop assertion.

Definition at line 126 of file encoders_monitor.cpp.

bool EncodersMonitor::ok ( )

Definition at line 107 of file encoders_monitor.cpp.