node.hh Source File

00001 /*********************************************************************
00002 * File: node.hh                                                      *
00003 * Description: C++ Header File for the RCS Systems Team's Library    *
00004 * Purpose:                                                           *
00005 *       RCS Systems are constructed using a heirarchy of nodes.      *
00006 * Each node recieves commands from its superior and returns a status *
00007 *  message to its superior. If it has subordinates the node will     *
00008 * be responsible for sending commands to that subordinate and        *
00009 * monitoring the subordinates status. Nodes may also read sensors,   *
00010 * process sensor data, update the world model, plan future moves,    *
00011 * execute commands and handle errors.  The NODE class is defined so  *
00012 * that application nodes can be constructed with a consistent        *
00013 * interface and to  provide application nodes with certain           *
00014 * utilities.                                                         *
00015 *********************************************************************/
00016 
00017 #ifndef NODE_HH                 /* Prevent re-definition if file included */
00018 #define NODE_HH                 /* more than once.  */
00019 
00020 #include "rcs_defs.hh"          /* EXTERN_C_STD_HEADERS */
00021 
00022 #ifdef EXTERN_C_STD_HEADERS
00023 extern "C"
00024 {
00025 #endif
00026 
00027 #ifdef VXWORKS
00028 #include <semLib.h>             /* SEM_ID */
00029 #endif
00030 
00031 #ifdef EXTERN_C_STD_HEADERS
00032 }
00033 #endif
00034 
00035 /* Include Files */
00036 #include "rcs_defs.hh"          /* RCS_EXPORT  */
00037 #include "cms.hh"               /* class CMS */
00038 #include "nml.hh"               /* class NML */
00039 #include "nmlmsg.hh"            /* class NMLmsg */
00040 #include "cmd_msg.hh"           /* class RCS_CMD_MSG, RCS_CMD_CHANNEL */
00041 #include "stat_msg.hh"          /* class RCS_STAT_MSG, RCS_STAT_CHANNEL */
00042 #include "wm_msg.hh"            /* class RCS_STAT_MSG, RCS_WM_CHANNEL */
00043 #include "timer.hh"             /* class RCS_TIMER */
00044 #include "linklist.hh"          /* class RCS_LINKED_LIST */
00045 
00046 // This hack should make the node stuff compile more easily for VXWORKS Centerline compiler
00047 #ifdef VXWORKS
00048 #ifdef NULL
00049 #undef NULL
00050 #endif
00051 #define NULL (0)
00052 #endif
00053 
00054 /* Define the node configuration flags */
00055 #define NODE_USE_GENERIC_COMMANDS             0x00000001
00056 #define NODE_WAIT_FOR_COMMANDS_TO_BE_READ     0x00000002
00057 #define NODE_CHECK_TIMES                      0x00000008
00058 #define NODE_EXECUTE_NEW_COMMANDS_ONLY        0x00000010
00059 #define NODE_RESET_STATE_ON_NEW_COMMAND       0x00000020
00060 #define NODE_USE_STAT_BUFFER                  0x00000040
00061 #define NODE_USE_WM_BUFFER                    0x00000080
00062 #define NODE_SEND_EXECUTING_ON_NEW_COMMAND    0x00000100
00063 #define NODE_SAVE_LAST_COMMAND                0x00000200
00064 
00065 extern long default_node_config_flags;
00066 
00067 /* Enumerated Data Types */
00068 
00069 /* The NODE_STATUS_TYPE is part of every status message. */
00070 enum NODE_STATUS_TYPE
00071 { NODE_INIT,
00072   NODE_WAITING,
00073   NODE_EXECUTING,
00074   NODE_DONE,
00075   NODE_ERROR
00076 };
00077 
00078 /* The OPER_MODE_TYPE is used to determine whether the node is currently */
00079  /* fully autonomous, being teleoperated, or some combination. */
00080 enum OPER_MODE_TYPE
00081 { AUTO, TELEOP, HYBRID };
00082 enum NODE_SECTION
00083 {
00084   NODE_GET_COMMAND_MESSAGE,     /* Read */
00085   NODE_GET_SUBORDINATES_STATUS,
00086   NODE_GET_OI_REQUESTS,
00087   NODE_READ_WORLD_MODEL_IN,
00088   NODE_READ_SENSORS,
00089   NODE_PROCESS_OI_REQUESTS,     /* Process/Modify */
00090   NODE_SENSORY_PROCESSOR,
00091   NODE_WORLD_MODELER,
00092   NODE_COMMAND_INDEPENDANT_PLANNER,
00093   NODE_HANDLE_GENERIC_COMMANDS,
00094   NODE_EXECUTE_COMMAND,
00095   NODE_OUTPUT_TO_ACTUATORS,     /* Write */
00096   NODE_SEND_OI_REPLIES,
00097   NODE_WRITE_WORLD_MODEL_OUT,
00098   NODE_SEND_SUBORDINATES_COMMANDS,
00099   NODE_SEND_STATUS_MESSAGE
00100 };
00101 
00102 #define RCS_COMMAND_SUFFIX "_cmd"
00103 #define RCS_COMMAND_SUFFIX_LENGTH 4
00104 #define RCS_STATUS_SUFFIX "_stat"
00105 #define RCS_STATUS_SUFFIX_LENGTH 5
00106 #define RCS_WM_SUFFIX "_wm"
00107 #define RCS_WM_SUFFIX_LENGTH 3
00108 
00109 /* Class Definitions. */
00110 
00111 class RCS_EXPORT NODE_LINK;
00112 
00113 enum RCS_COMMAND_SOURCE_TYPE
00114 {
00115   RCS_SUPERIOR_NODE,
00116   RCS_OPERATOR_INTERFACE,
00117 };
00118 
00119 class RCS_EXPORT NODE
00120 {
00121   RCS_CMD_CHANNEL *cmd_in;      /* comm channel to command buffer */
00122   RCS_STAT_CHANNEL *status_up;  /* comm channel to status buffer */
00123   RCS_WM_CHANNEL *wm_out;       /* comm channel to World Model */
00124   RCS_COMMAND_SOURCE_TYPE command_source;
00125 
00126   RCS_LINKED_LIST *sub_list;    /* List of subnodes. */
00127   RCS_LINKED_LIST *wm_in_list;  /* List of nodes to read wm of. */
00128   RCS_GENERIC_STATUS *generic_status;   /* Easy way to send simple status. */
00129   RCS_CMD_MSG *startup_cmd;
00130 
00131   friend class RCS_EXPORT NODE_LINK;
00132   friend class RCS_OI;
00133   friend class CMD_OI;
00134 public:
00135   long state;                   /* state in user-defined state table */
00136   long last_state;              /* state from 1 cycle ago */
00137   long last_state_of_last_command;      /* state before new command recieved. */
00138   long error_code;              /* user-defined error code  */
00139   long cycle_count;             /* Number of times node run (debug only) */
00140   NML_FORMAT_PTR format_ptr;
00141   char *config_file;
00142   NODE_SECTION current_section; /* section of node that returned -1 */
00143   NODE_STATUS_TYPE node_status; /* NODE_INIT, NODE_DONE, NODE_ERROR ... */
00144   OPER_MODE_TYPE oper_mode;     /* autonomous, teleoperated ... */
00145   int num_of_subord;            /* # of subordinates */
00146   int all_subordinates_done;    /* True if all sub_nodes have status DONE */
00147   int valid;                    /* are all neccessary comm channels ok */
00148   RCS_CMD_MSG *cmd_in_msg;      /* pointer to last command recieved */
00149   RCS_CMD_MSG *last_cmd_in_msg; /* storage to save the last command */
00150   RCS_STAT_MSG *status_up_msg;  /* pointer to message to send up. */
00151   RCS_WM_MSG *wm_out_msg;       /* pointer to message for World Model */
00152   NMLmsg *oper_in_msg;          /* pointer to last message recieved for oi */
00153   int status_up_new;            /* set by user to send new status up */
00154   int cmd_in_new;               /* set by NODE::get_command_message */
00155   int oper_in_new;
00156   int wm_out_new;
00157   NMLTYPE cmd_in_type;          /* set by NODE::get_command_message */
00158   NMLTYPE last_cmd_in_type;     /* set by NODE::get_command_message */
00159   NMLTYPE status_up_type;
00160   NMLTYPE oper_in_type;
00161   NMLTYPE wm_type;
00162   int cmd_in_type_new;          /* set by NODE::get_command_message */
00163 
00164   RCS_TIMER *cycle_timer;
00165   int cycle_timer_created;
00166   double cycle_time;
00167   double start_time;
00168   double finish_time;
00169 
00170 
00171   double last_cycle_time;
00172   double min_cycle_time;
00173   double max_cycle_time;
00174 
00175   double last_process_time;
00176   double min_process_time;
00177   double max_process_time;
00178 
00179 
00180 
00181   char *name;                   /* Name of Node. */
00182   int name_length;              /* length of name string  */
00183   long config_flags;
00184   int abort_flag;
00185 
00186   /* Constuctors and Destructors. */
00187     NODE (NML_FORMAT_PTR, char *n, char *file, RCS_CMD_MSG * _startup_cmd = NULL);
00188     virtual ~ NODE ();
00189   int initialize_node_variables ();
00190   int initialize_command_buffer ();
00191   int initialize_status_buffer ();
00192   int initialize_world_model_buffer ();
00193 
00194   //            int load_initial_state();
00195   //            int store_exec_data();
00196   void send_status (NODE_STATUS_TYPE, RCS_STAT_MSG * new_msg = NULL);
00197 
00198   void check_timer (RCS_TIMER *);
00199   void wait_for_timer (RCS_TIMER *);
00200   void run (double time);
00201   void run (RCS_TIMER * timer = NULL);
00202   void task (double time);
00203   void task (RCS_TIMER * timer = NULL);
00204   virtual void error_handler ();
00205   virtual void initialize_state_variables ();
00206 
00207   virtual int cyclic_read ();
00208   virtual int cyclic_process ();
00209   virtual int cyclic_write ();
00210 
00211   virtual int get_command_message ();   /* Read. */
00212   virtual int get_subordinates_status ();
00213   virtual int get_oi_requests ();
00214   virtual int read_world_model_in ();
00215   virtual int read_sensors ();
00216   virtual int process_oi_requests ();   /* Process/Modify */
00217   virtual int sensory_processor ();
00218   virtual int world_modeler ();
00219   virtual int command_independant_planner ();
00220   virtual int handle_generic_commands ();
00221   virtual void send_command_to_all (RCS_CMD_MSG *);
00222   virtual int execute_command ();
00223   virtual void init ();
00224   virtual void halt ();
00225   virtual int output_to_actuators ();   /* Write */
00226   virtual int send_oi_replies ();
00227   virtual int write_world_model_out ();
00228   virtual int send_subordinates_commands ();
00229   virtual int send_status_message ();
00230 
00231   virtual void check_if_valid ();
00232 
00233 };
00234 
00235 
00236 #endif /* !defined(NODE_HH) */