NASA Astrobee Robot Software  0.19.1
Flight software for the Astrobee robots operating inside the International Space Station.
sys_monitor::SysMonitor Class Reference

#include <sys_monitor.h>

Inheritance diagram for sys_monitor::SysMonitor:
Inheritance graph

Classes

struct  Fault
 
class  Watchdog
 

Public Member Functions

 SysMonitor ()
 
 ~SysMonitor ()
 
void AddFault (unsigned int fault_id, std::string const &fault_msg="", ros::Time time_occurred=ros::Time::now())
 
void AddFault (ff_msgs::Fault const &fault, bool check_added=false)
 
void ChangeFaultErrMsg (unsigned int fault_id, std::string err_msg)
 
void RemoveFault (unsigned int fault_id)
 
- Public Member Functions inherited from ff_util::FreeFlyerNodelet
 FreeFlyerNodelet (bool autostart_hb_timer=true)
 
 FreeFlyerNodelet (std::string const &name, bool autostart_hb_timer=true)
 
virtual ~FreeFlyerNodelet ()
 
void AssertFault (FaultKeys enum_key, std::string const &message, ros::Time time_fault_occurred=ros::Time::now())
 
void ClearAllFaults ()
 
void ClearFault (FaultKeys enum_key)
 
void PrintFaults ()
 
ros::NodeHandle * GetPlatformHandle (bool multithreaded=false)
 
ros::NodeHandle * GetPrivateHandle (bool multithreaded=false)
 
std::string GetName ()
 
std::string GetPlatform ()
 
std::string GetTransform (std::string const &child)
 

Protected Types

typedef std::shared_ptr< WatchdogWatchdogPtr
 

Protected Member Functions

void AddWatchDog (ros::Duration const &timeout, std::string const &node_name, uint const allowed_misses, uint const fault_id)
 
void SetFaultState (unsigned int fault_id, bool adding_fault)
 
void HeartbeatCallback (ff_msgs::HeartbeatConstPtr const &heartbeat)
 
void AssertFault (ff_util::FaultKeys enum_key, std::string const &message, ros::Time time_fault_occurred=ros::Time::now())
 
void ClearFault (ff_util::FaultKeys enum_key)
 
virtual void Initialize (ros::NodeHandle *nh)
 
void OutputFaultTables ()
 
void PublishCmd (ff_msgs::CommandStampedPtr cmd)
 
void PublishFaultConfig ()
 
void PublishFaultState ()
 
void PublishFaultResponse (unsigned int fault_id)
 
void PublishHeartbeatCallback (ros::TimerEvent const &te)
 
void PublishHeartbeat (bool initialization_fault=false)
 
void PublishTimeDiff (float time_diff_sec)
 
void StartupTimerCallback (ros::TimerEvent const &te)
 
void ReloadNodeletTimerCallback (ros::TimerEvent const &te)
 
bool ReadParams ()
 
bool ReadCommand (config_reader::ConfigReader::Table *entry, ff_msgs::CommandStampedPtr cmd)
 
bool NodeletService (ff_msgs::UnloadLoadNodelet::Request &req, ff_msgs::UnloadLoadNodelet::Response &res)
 
int LoadNodelet (ff_msgs::UnloadLoadNodelet::Request &req)
 
int UnloadNodelet (std::string const &nodelet, std::string const &manager)
 
- Protected Member Functions inherited from ff_util::FreeFlyerNodelet
virtual void Reset ()
 
virtual void Sleep ()
 
virtual void Wakeup ()
 
void StopHeartbeat ()
 
void SendDiagnostics (const std::vector< diagnostic_msgs::KeyValue > &keyval)
 
void Setup (ros::NodeHandle &nh, ros::NodeHandle &nh_mt, std::string plugin_name)
 

Protected Attributes

ff_msgs::FaultState fault_state_
 
ff_msgs::FaultConfig fault_config_
 
ff_msgs::Heartbeat heartbeat_
 
nodelet::NodeletLoad load_service_
 
nodelet::NodeletUnload unload_service_
 
ros::NodeHandle nh_
 
ros::Publisher pub_cmd_
 
ros::Publisher pub_heartbeat_
 
ros::Publisher pub_fault_config_
 
ros::Publisher pub_fault_state_
 
ros::Publisher pub_time_sync_
 
ros::Timer reload_params_timer_
 
ros::Timer startup_timer_
 
ros::Timer reload_nodelet_timer_
 
ros::Timer heartbeat_timer_
 
ros::ServiceServer unload_load_nodelet_service_
 
ros::Subscriber sub_hb_
 
std::map< unsigned int, std::shared_ptr< Fault > > all_faults_
 
std::map< std::string, WatchdogPtrwatch_dogs_
 
std::vector< std::string > unwatched_heartbeats_
 
std::vector< std::string > reloaded_nodelets_
 
std::string time_diff_node_
 
config_reader::ConfigReader config_params_
 
bool time_diff_fault_triggered_
 
bool log_time_llp_
 
bool log_time_hlp_
 
int pub_queue_size_
 
int sub_queue_size_
 
int num_current_blocking_fault_
 
unsigned int startup_time_
 
unsigned int reload_nodelet_timeout_
 
unsigned int heartbeat_pub_rate_
 
float time_drift_thres_sec_
 
- Protected Attributes inherited from ff_util::FreeFlyerNodelet
std::map< std::string, int > faults_
 

Additional Inherited Members

- Public Types inherited from ff_util::FreeFlyerNodelet
enum  ResolveType : uint8_t { NAMESPACE = 0, TRANSFORM = 1, RESOURCE = 1 }
 

Member Typedef Documentation

◆ WatchdogPtr

typedef std::shared_ptr<Watchdog> sys_monitor::SysMonitor::WatchdogPtr
protected

Constructor & Destructor Documentation

◆ SysMonitor()

sys_monitor::SysMonitor::SysMonitor ( )

◆ ~SysMonitor()

sys_monitor::SysMonitor::~SysMonitor ( )

Member Function Documentation

◆ AddFault() [1/2]

void sys_monitor::SysMonitor::AddFault ( ff_msgs::Fault const &  fault,
bool  check_added = false 
)

◆ AddFault() [2/2]

void sys_monitor::SysMonitor::AddFault ( unsigned int  fault_id,
std::string const &  fault_msg = "",
ros::Time  time_occurred = ros::Time::now() 
)

Add fault to fault state and publish fault state.

Parameters
fault_idunique fault id
fault_msgmessage containing fault information
time_occurredtime fault occurred

◆ AddWatchDog()

void sys_monitor::SysMonitor::AddWatchDog ( ros::Duration const &  timeout,
std::string const &  node_name,
uint const  allowed_misses,
uint const  fault_id 
)
protected

Add a watchdog for node_name if one does not exist

Parameters
node_nameunique node name, use ros::this_node::getName()
timeoutwatchdog timer timeout
allowed_missesallowable missed watchdog timeouts
fault_idunique fault id

◆ AssertFault()

void sys_monitor::SysMonitor::AssertFault ( ff_util::FaultKeys  enum_key,
std::string const &  message,
ros::Time  time_fault_occurred = ros::Time::now() 
)
protected

◆ ChangeFaultErrMsg()

void sys_monitor::SysMonitor::ChangeFaultErrMsg ( unsigned int  fault_id,
std::string  err_msg 
)

◆ ClearFault()

void sys_monitor::SysMonitor::ClearFault ( ff_util::FaultKeys  enum_key)
protected

◆ HeartbeatCallback()

void sys_monitor::SysMonitor::HeartbeatCallback ( ff_msgs::HeartbeatConstPtr const &  heartbeat)
protected

Heartbeat callback will reset watchdog for the sender's node

Parameters
heartbeatreceived message

◆ Initialize()

void sys_monitor::SysMonitor::Initialize ( ros::NodeHandle *  nh)
protectedvirtual

Reimplemented from ff_util::FreeFlyerNodelet.

◆ LoadNodelet()

int sys_monitor::SysMonitor::LoadNodelet ( ff_msgs::UnloadLoadNodelet::Request &  req)
protected

◆ NodeletService()

bool sys_monitor::SysMonitor::NodeletService ( ff_msgs::UnloadLoadNodelet::Request &  req,
ff_msgs::UnloadLoadNodelet::Response &  res 
)
protected

◆ OutputFaultTables()

void sys_monitor::SysMonitor::OutputFaultTables ( )
protected

◆ PublishCmd()

void sys_monitor::SysMonitor::PublishCmd ( ff_msgs::CommandStampedPtr  cmd)
protected

◆ PublishFaultConfig()

void sys_monitor::SysMonitor::PublishFaultConfig ( )
protected

◆ PublishFaultResponse()

void sys_monitor::SysMonitor::PublishFaultResponse ( unsigned int  fault_id)
protected

◆ PublishFaultState()

void sys_monitor::SysMonitor::PublishFaultState ( )
protected

◆ PublishHeartbeat()

void sys_monitor::SysMonitor::PublishHeartbeat ( bool  initialization_fault = false)
protected

◆ PublishHeartbeatCallback()

void sys_monitor::SysMonitor::PublishHeartbeatCallback ( ros::TimerEvent const &  te)
protected

◆ PublishTimeDiff()

void sys_monitor::SysMonitor::PublishTimeDiff ( float  time_diff_sec)
protected

◆ ReadCommand()

bool sys_monitor::SysMonitor::ReadCommand ( config_reader::ConfigReader::Table entry,
ff_msgs::CommandStampedPtr  cmd 
)
protected

Read command will read in the command name and arguments to a command. If no command is specified, false is returned.

◆ ReadParams()

bool sys_monitor::SysMonitor::ReadParams ( )
protected

Read params will read in all the parameters from the lua config files. When reloading parameters with this function, the watch dog will be cleared and restarted. False is returned if unable to read in config files.

◆ ReloadNodeletTimerCallback()

void sys_monitor::SysMonitor::ReloadNodeletTimerCallback ( ros::TimerEvent const &  te)
protected

◆ RemoveFault()

void sys_monitor::SysMonitor::RemoveFault ( unsigned int  fault_id)

◆ SetFaultState()

void sys_monitor::SysMonitor::SetFaultState ( unsigned int  fault_id,
bool  adding_fault 
)
protected

◆ StartupTimerCallback()

void sys_monitor::SysMonitor::StartupTimerCallback ( ros::TimerEvent const &  te)
protected

◆ UnloadNodelet()

int sys_monitor::SysMonitor::UnloadNodelet ( std::string const &  nodelet,
std::string const &  manager 
)
protected

Member Data Documentation

◆ all_faults_

std::map<unsigned int, std::shared_ptr<Fault> > sys_monitor::SysMonitor::all_faults_
protected

◆ config_params_

config_reader::ConfigReader sys_monitor::SysMonitor::config_params_
protected

◆ fault_config_

ff_msgs::FaultConfig sys_monitor::SysMonitor::fault_config_
protected

◆ fault_state_

ff_msgs::FaultState sys_monitor::SysMonitor::fault_state_
protected

◆ heartbeat_

ff_msgs::Heartbeat sys_monitor::SysMonitor::heartbeat_
protected

◆ heartbeat_pub_rate_

unsigned int sys_monitor::SysMonitor::heartbeat_pub_rate_
protected

◆ heartbeat_timer_

ros::Timer sys_monitor::SysMonitor::heartbeat_timer_
protected

◆ load_service_

nodelet::NodeletLoad sys_monitor::SysMonitor::load_service_
protected

◆ log_time_hlp_

bool sys_monitor::SysMonitor::log_time_hlp_
protected

◆ log_time_llp_

bool sys_monitor::SysMonitor::log_time_llp_
protected

◆ nh_

ros::NodeHandle sys_monitor::SysMonitor::nh_
protected

◆ num_current_blocking_fault_

int sys_monitor::SysMonitor::num_current_blocking_fault_
protected

◆ pub_cmd_

ros::Publisher sys_monitor::SysMonitor::pub_cmd_
protected

◆ pub_fault_config_

ros::Publisher sys_monitor::SysMonitor::pub_fault_config_
protected

◆ pub_fault_state_

ros::Publisher sys_monitor::SysMonitor::pub_fault_state_
protected

◆ pub_heartbeat_

ros::Publisher sys_monitor::SysMonitor::pub_heartbeat_
protected

◆ pub_queue_size_

int sys_monitor::SysMonitor::pub_queue_size_
protected

◆ pub_time_sync_

ros::Publisher sys_monitor::SysMonitor::pub_time_sync_
protected

◆ reload_nodelet_timeout_

unsigned int sys_monitor::SysMonitor::reload_nodelet_timeout_
protected

◆ reload_nodelet_timer_

ros::Timer sys_monitor::SysMonitor::reload_nodelet_timer_
protected

◆ reload_params_timer_

ros::Timer sys_monitor::SysMonitor::reload_params_timer_
protected

◆ reloaded_nodelets_

std::vector<std::string> sys_monitor::SysMonitor::reloaded_nodelets_
protected

◆ startup_time_

unsigned int sys_monitor::SysMonitor::startup_time_
protected

◆ startup_timer_

ros::Timer sys_monitor::SysMonitor::startup_timer_
protected

◆ sub_hb_

ros::Subscriber sys_monitor::SysMonitor::sub_hb_
protected

◆ sub_queue_size_

int sys_monitor::SysMonitor::sub_queue_size_
protected

◆ time_diff_fault_triggered_

bool sys_monitor::SysMonitor::time_diff_fault_triggered_
protected

◆ time_diff_node_

std::string sys_monitor::SysMonitor::time_diff_node_
protected

◆ time_drift_thres_sec_

float sys_monitor::SysMonitor::time_drift_thres_sec_
protected

◆ unload_load_nodelet_service_

ros::ServiceServer sys_monitor::SysMonitor::unload_load_nodelet_service_
protected

◆ unload_service_

nodelet::NodeletUnload sys_monitor::SysMonitor::unload_service_
protected

◆ unwatched_heartbeats_

std::vector<std::string> sys_monitor::SysMonitor::unwatched_heartbeats_
protected

◆ watch_dogs_

std::map<std::string, WatchdogPtr> sys_monitor::SysMonitor::watch_dogs_
protected

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