---

node.cc

Go to the documentation of this file.

/*********************************************************************
* File: node.cc                                                      *
* Description: C++  File for the RCS Systems Team's Library          *
* Purpose:                                                           *
*       RCS Systems are constructed using a heirarchy of nodes.      *
* Each node recieves commands from its superior and returns a status *
*  message to its superior. If it has subordinates the node will     *
* be responsible for sending commands to that subordinate and        *
* monitoring the subordinates status. Nodes may also read sensors,   *
* process sensor data, update the world model, plan future moves,    *
* execute commands and handle errors.  The NODE class is defined so  *
* that application nodes can be constructed with a consistent        *
* interface and to  provide application nodes with certain           *
* utilities.                                                         *
*********************************************************************/
#include "rcs_defs.hh"          /* EXTERN_C_STD_HEADERS */

/* Include Files */
#ifdef EXTERN_C_STD_HEADERS
extern "C"
{
#endif

#include <stdlib.h>             /* malloc(), free() */
#include <string.h>             /* strcpy() */
#include <signal.h>             /* SIGINT */
#include <sys/types.h>
#ifdef VXWORKS
#include <taskLib.h>
#endif

#ifdef EXTERN_C_STD_HEADERS
}
#endif

#include "nml.hh"               /* class NML */
#include "nmlmsg.hh"            /* class NMLmsg */
#include "cmd_msg.hh"           /* class RCS_CMD_MSG, RCS_GENERIC_CMD, */
                                /* class RCS_CMD_CHANNEL */
#include "stat_msg.hh"          /* class RCS_STAT_MSG, RCS_GENERIC_STAT, */
                                /* class RCS_STAT_CHANNEL */
#include "wm_msg.hh"            /* class RCS_WM_MSG, RCS_GENERIC_WM, */
                                /* class RCS_WM_CHANNEL */
#include "node.hh"              /* class NODE */
#include "nodelink.hh"          /* class NODE_LINK */
#include "rcs_prnt.hh"          /* rcs_print_error, rcs_print_debug */

/* Constructors and Destructors */


long default_node_config_flags = NODE_USE_GENERIC_COMMANDS |
  NODE_USE_STAT_BUFFER | NODE_USE_WM_BUFFER |
  NODE_CHECK_TIMES | NODE_RESET_STATE_ON_NEW_COMMAND |
  NODE_SEND_EXECUTING_ON_NEW_COMMAND;

/*******************************************************************
 Class: NODE
 Function: (Constructor)
 Parameters:
    NML_FORMAT_PTR _format_ptr; - pointer to a function which
         handles the neutral data format encoding and decoding
         for the buffers used by this node. (See NML Doc.)
    char _name; - string including the name of this node
          (The names of the buffers connecting to this name are
           computed from my_name by adding suffixes like "_cmd".)
    char _config_file; - name of NML configuration file.
    RCS_CMD_MSG _startup_cmd; - pointer to command message for
          this node to use until its superior sends it a command
 Purpose: To initialize a new NODE object.
 NOTES: valid will equal 1 if the NODE was properly initialized.
 Returns: NONE.
*******************************************************************/
NODE::NODE (NML_FORMAT_PTR _format_ptr, char *_name, char *_config_file,
            RCS_CMD_MSG * _startup_cmd)
{
  /* Indicate to user when the NODE is constructed. */
  rcs_print_debug (PRINT_NODE_CONSTRUCTORS,
                   "%s NODE constructor called.\n", _name);

  /* Set all the dynamically allocated pointers to NULL so the */
  /* so the destructors won't destroy them if they were never malloced, */
  /* and initialize some other NODE variables. */
  initialize_node_variables ();

  /* Set valid to zero incase we need to return from constructor early. */
  valid = 0;

  /* Store constructor parameters. */
  format_ptr = _format_ptr;
  config_file = _config_file;
  startup_cmd = _startup_cmd;

  /* Store the _name parameter in a new place. */
  name_length = strlen (_name);
  name = (char *) malloc (name_length + 1);
  if (NULL == name)
    {
      rcs_print_error ("NODE constructor: Couldn`t malloc space for name.\n");
      return;
    }
  strcpy (name, _name);

  /* Create an empty subordinate list. */
  sub_list = new RCS_LINKED_LIST;
  if (NULL == sub_list)
    {
      rcs_print_error ("%s: Couldn`t malloc space for list.\n", name);
      return;
    }

  wm_in_list = new RCS_LINKED_LIST;
  if (NULL == wm_in_list)
    {
      rcs_print_error ("%s: Couldn`t malloc space for list.\n", name);
      return;
    }


  if (-1 == initialize_command_buffer ())
    {
      return;
    }
  if (config_flags & NODE_SAVE_LAST_COMMAND && NULL != cmd_in)
    {
      if (NULL != cmd_in->cms)
        {
          last_cmd_in_msg = (RCS_CMD_MSG *) malloc (cmd_in->cms->size);
        }
    }

  if (config_flags & NODE_USE_STAT_BUFFER)
    {
      if (-1 == initialize_status_buffer ())
        {
          return;
        }
    }

  if (config_flags & NODE_USE_WM_BUFFER)
    {
      if (-1 == initialize_world_model_buffer ())
        {
          return;
        }
    }

  /* Set the valid flag to 1 since we made it this far. */
  valid = 1;

  /* Spawn servers for the command, status, and/or wm buffers if config */
  /* file says to. */
  nml_start ();

  /* Check if valid will set valid to zero if running this node could crash */
  /* the system. NODE::run() will do nothing if valid == 0 to avoid this. */
  check_if_valid ();

  rcs_print_debug (PRINT_NODE_CONSTRUCTORS,
                   "%s NODE constructor finished. (valid = %d)\n", _name,
                   valid);
}

/* Functions called from constructor. */

/********************************************************************
 Class: NODE
 Function: initialize_node_variables
 Parameters: NONE
 Purpose: To initialize the node variables.
 Returns: 0 for success or -1 for failure.
*******************************************************************/
int
NODE::initialize_node_variables ()
{

  /* Set all the dynamically allocated pointers to NULL so the */
  /* so the destructors won't destroy them if they were never malloced. */
  name = (char *) NULL;
  name_length = 0;
  cmd_in = (RCS_CMD_CHANNEL *) NULL;
  status_up = (RCS_STAT_CHANNEL *) NULL;
  wm_out = (RCS_WM_CHANNEL *) NULL;

  sub_list = (RCS_LINKED_LIST *) NULL;
  generic_status = (RCS_GENERIC_STATUS *) NULL;
  cycle_timer = (RCS_TIMER *) NULL;
  wm_in_list = (RCS_LINKED_LIST *) NULL;

  cmd_in_msg = (RCS_CMD_MSG *) NULL;
  last_cmd_in_msg = (RCS_CMD_MSG *) NULL;

  /* Initialize some other NODE variables. */
  node_status = NODE_INIT;
  status_up_new = 0;
  cmd_in_new = 1;
  cmd_in_type_new = 1;
  config_flags = default_node_config_flags;
  wm_out_new = 0;
  oper_in_new = 0;
  state = 0;
  error_code = 0;
  num_of_subord = 0;
  all_subordinates_done = 0;
  cycle_count = 0;
  abort_flag = 0;
  min_cycle_time = 360000;
  max_cycle_time = 0;
  min_process_time = 360000;
  max_process_time = 0;
  return (0);
}

/********************************************************************
 Class: NODE
 Function: initialize_command_buffer
 Parameters: NONE
 Purpose: To initialize the command buffer by creating it if neccessary,
  writing a generic command into it and verifying that that command is there.
 Returns: 0 for success or -1 for failure.
*******************************************************************/
int
NODE::initialize_command_buffer ()
{

  if (NULL == cmd_in)
    {
      /* Compute the name of the command buffer. */
      char *command_buffer_name;
      command_buffer_name =
        (char *) malloc (name_length + RCS_COMMAND_SUFFIX_LENGTH + 1);
      if (NULL == command_buffer_name)
        {
          rcs_print_error ("%s: malloc failed.\n", name);
          return (-1);
        }
      strcpy (command_buffer_name, name);
      strcat (command_buffer_name, RCS_COMMAND_SUFFIX);

      /* Create a channel to the command buffer. */
      cmd_in = new RCS_CMD_CHANNEL (format_ptr, command_buffer_name,
                                    name, config_file);
      free (command_buffer_name);
      command_buffer_name = (char *) NULL;
    }

  /* Make sure cmd in object created. */
  if (NULL == cmd_in)
    {
      rcs_print_error ("%s: Couldn`t create cmd_in channel.\n", name);
      return -1;
    }

  /* Check to see if cmd_in properly constructed. */
  if (!cmd_in->valid ())
    {
      rcs_print_error ("%s: Command buffer channel invalid. \n", name);
      return -1;
    }

  /* Create the startup command if none was passed to constructor. */
  int startup_cmd_created = 0;
  if (NULL == startup_cmd)
    {
      startup_cmd = new RCS_GENERIC_CMD;
      startup_cmd_created = 1;
    }
  if (NULL == startup_cmd)
    {
      rcs_print_error ("%s: Can't create startup command.\n", name);
      return -1;
    }
  ((RCS_GENERIC_CMD *) startup_cmd)->gen_id = GENERIC_INIT;

  /* Write the startup command into the command buffer. */
  if (-1 == cmd_in->write (startup_cmd))
    {
      rcs_print_error ("%s: Error on initial write to command buffer.\n",
                       name);
      return -1;
    }

  /* Read back the startup command. */
  if (-1 == cmd_in->read ())
    {
      rcs_print_error ("%s: Error on initial read of command buffer.\n",
                       name);
      return -1;
    }
  cmd_in_msg = (RCS_CMD_MSG *) cmd_in->get_address ();
  cmd_in_type = cmd_in_msg->type;

  /* Check to see if startup command matches command read back. */
  if (cmd_in_type != startup_cmd->type ||
      cmd_in_msg->serial_number != startup_cmd->serial_number)
    {
      rcs_print_error ("%s: Error initializing the command buffer. \n", name);
      return -1;
    }

  if (startup_cmd_created)
    {
      delete startup_cmd;
      startup_cmd = (RCS_CMD_MSG *) NULL;
    }

  /* Return "success". */
  return (0);
}

/********************************************************************
 Class: NODE
 Function: initialize_status_buffer
 Parameters: NONE
 Purpose: To initialize the status buffer by creating it if neccessary,
  writing a generic status message into it.
 Returns: 0 for success or -1 for failure.
*******************************************************************/
int
NODE::initialize_status_buffer ()
{
  if (status_up == NULL)
    {
      /* Compute the name of the status buffer. */
      char *status_buffer_name;
      status_buffer_name =
        (char *) malloc (name_length + RCS_STATUS_SUFFIX_LENGTH + 1);
      if (NULL == status_buffer_name)
        {
          rcs_print_error ("%s: malloc error.\n", name);
          return (-1);
        }
      strcpy (status_buffer_name, name);
      strcat (status_buffer_name, RCS_STATUS_SUFFIX);

      /* Create a new channel to the status buffer. */
      status_up = new RCS_STAT_CHANNEL (format_ptr, status_buffer_name,
                                        name, config_file);

      /* Free the space for the status_buffer_name */
      free (status_buffer_name);
      status_buffer_name = (char *) NULL;
    }

  /* Make sure status up object created. \n"); */
  if (NULL == status_up)
    {
      rcs_print_error ("%s: Couldn`t create status_up channel.\n", name);
      return (-1);
    }

  /* Check to see if status_up buffer properly constructed. */
  if (!status_up->valid ())
    {
      rcs_print_error ("%s: status_up object not properly constructed.\n",
                       name);
      return -1;
    }

  /* Create initial status object. */
  generic_status = new RCS_GENERIC_STATUS;
  if (NULL == generic_status)
    {
      rcs_print_error ("%s: Can't create initial status message.\n", name);
      return -1;
    }
  status_up_msg = generic_status;
  status_up_msg->node_status = NODE_INIT;

  /* Write initial status into status up buffer. */
  if (-1 == status_up->write (status_up_msg))
    {
      rcs_print_error ("%s: Error on initial write to status buffer.\n",
                       name);
      return -1;
    }

  /* Return "success". */
  return (0);
}

/********************************************************************
 Class: NODE
 Function: initialize_world_model_buffer
 Parameters: NONE
 Purpose: To initialize the status buffer by creating it if neccessary,
  writing a generic status message into it.
 Returns: 0 for success or -1 for failure.
*******************************************************************/
int
NODE::initialize_world_model_buffer ()
{
  if (NULL == wm_out)
    {
      /* Compute world model out buffer name. */
      char *wm_buffer_name;
      wm_buffer_name =
        (char *) malloc (name_length + RCS_WM_SUFFIX_LENGTH + 1);
      if (NULL == wm_buffer_name)
        {
          rcs_print_error ("%s: malloc error.\n", name);
        }
      strcpy (wm_buffer_name, name);
      strcat (wm_buffer_name, RCS_WM_SUFFIX);

      /* Create world model out channel. */
      wm_out =
        new RCS_WM_CHANNEL (format_ptr, wm_buffer_name, name, config_file);

      /* Release the memory for the buffer name. */
      free (wm_buffer_name);
      wm_buffer_name = NULL;
    }

  /* Check to see that wm_out created. */
  if (NULL == wm_out)
    {
      rcs_print_error ("%s: Can't create world model out channel. \n", name);
      return -1;
    }

  /* Check to see if wm_out properly constructed. */
  if (!wm_out->valid ())
    {
      rcs_print_error
        ("%s: World model out channel improperly constructed.\n", name);
      return -1;
    }

  /* Create initial world model message. */
  wm_out_msg = new RCS_GENERIC_WM;
  if (NULL == wm_out_msg)
    {
      rcs_print_error ("%s: Can't create initial world model message.\n",
                       name);
      return (-1);
    }

  if (-1 == wm_out->write (wm_out_msg))
    {
      rcs_print_error ("%s: Error on initial write to world model.\n", name);
      return -1;
    }
  delete wm_out_msg;
  wm_out_msg = NULL;

  /* Return "success". */
  return (0);
}

/*******************************************************************
* Class: NODE                                                      *
* Function: (Destructor)                                           *
* Parameters: NONE.                                                *
* Purpose: To deallocate and close a  NODE object.                 *
* Returns: NONE.                                                   *
*******************************************************************/
NODE::~NODE ()
{
  /* Indicate to user when the node is deleted. */
  rcs_print_debug (PRINT_NODE_DESTRUCTORS, "%s: Destructor called.\n", name);

  if (NULL != name)
    {
      free (name);
      name = NULL;
    }
  if (NULL != generic_status)
    {
      delete generic_status;
      generic_status = NULL;
    }
  if (NULL != sub_list)
    {
      delete sub_list;
      sub_list = NULL;
    }
  if (NULL != wm_in_list)
    {
      delete wm_in_list;
      wm_in_list = NULL;
    }
  if (NULL != last_cmd_in_msg)
    {
      free (last_cmd_in_msg);
      last_cmd_in_msg = NULL;
    }
  nml_cleanup ();

  rcs_print_debug (PRINT_NODE_DESTRUCTORS, "Destructor finished.\n");
}

/* Member Functions */

/*******************************************************************
* Class: NODE
* Function: run
* Parameters: NONE.
* Purpose: To run a NODE  through one cycle.
*         NODE::run calls each of the following functions which
* applications nodes can override, it also checks the returns of
* these functions and stops the rest of the cycle and calls the
* error_handler.
* FUNCTION                 WHEN SHOULD USER OVERRIDE
* get_command_message              never.
* get_subordinates_status          never.
* read_world_model_in              never.
* read_sensors                     to read some sensors.
* get_oi_requests                  to get input from operator.
* sensory_processor                to process sensor data.
* world_modeler                    to compute world model information
* process_oi_requests              to perform the operator interface request
* command_independant_planner      to do processing regardless of current cmd
* handle_generic_commands          never.
* execute_commands                 to execute the current command
* output_to_actuators              node is responsible for some actuators.
* send_subordinates_commands       never.
* send_status_message              never.
* send_oi_reply                    to output to an operator interface
* write_world_model_out            never.
*
* error_handler                    it's convienent to handle an error here.
*
* Returns: NONE.
*******************************************************************/
void
NODE::run (RCS_TIMER * _timer)
{
  if (!valid)
    {
      rcs_print_error ("%s: NODE not properly constructed. \n", name);
      abort_flag = 1;
      return;
    }

  check_timer (_timer);

  cycle_count++;
  last_state = state;
  rcs_print_debug (PRINT_NODE_CYCLES, "%s: Cycles = %d\n", name, cycle_count);

  if (-1 == cyclic_read ())
    {
      error_handler ();
      return;
    }

  if (-1 == cyclic_process ())
    {
      error_handler ();
      return;
    }

  if (-1 == cyclic_write ())
    {
      error_handler ();
      return;
    }

  wait_for_timer (_timer);

  if (abort_flag)
    {
      rcs_print_debug (PRINT_NODE_ABORT, "%s aborting!\n", name);
      return;
    }
  return;
}

/*******************************************************************
* Class: NODE
* Function: check_timer
* Parameters:
* RCS_TIMER *_timer;
*
* Purpose: Checks the cycle time, initializes the timer if neccessary,
* and prints some debug information.
* Outputs:  NONE.
* Returns: NONE.
*******************************************************************/
void
NODE::check_timer (RCS_TIMER * _timer)
{
  double time_check;
  static old_cycle_count;
  if ((NULL != _timer) && (_timer != cycle_timer))
    {
      if (NULL != cycle_timer && cycle_timer_created)
        {
          delete cycle_timer;
        }
      cycle_timer = _timer;
      cycle_timer->sync ();
      cycle_timer_created = 0;
    }

  if (NULL != cycle_timer)
    {
      cycle_time = cycle_timer->timeout;
    }

  if (config_flags & NODE_CHECK_TIMES)
    {
      time_check = etime ();
      if (0 != cycle_count)
        {
          last_cycle_time = time_check - start_time;
          if (last_cycle_time < min_cycle_time)
            {
              min_cycle_time = last_cycle_time;
            }
          if (last_cycle_time > max_cycle_time)
            {
              max_cycle_time = last_cycle_time;
            }
          rcs_print_debug (PRINT_NODE_CYCLE_TIMES,
                           "%s: cycle times (last, min, max) = (%lf, %lf, %lf)\n",
                           name, last_cycle_time, min_cycle_time,
                           max_cycle_time); \}
      start_time = time_check;
    }
}

/*******************************************************************
* Class: NODE                                                      *
* Function: run                                                    *
* Parameters:                                                      *
* RCS_TIMER *_timer; - Timer used for syncronization.
* Purpose: Waits until its time for the next cycle.
* Outputs:  NONE.                                                  *
* Returns: NONE.                                                   *
*******************************************************************/
void
NODE::wait_for_timer (RCS_TIMER * _timer)
{
  if (config_flags & NODE_CHECK_TIMES)
    {
      finish_time = etime ();
      last_process_time = finish_time - start_time;
      if (last_process_time < min_process_time)
        {
          min_process_time = last_process_time;
        }
      if (last_process_time > max_process_time)
        {
          max_process_time = last_process_time;
        }
      rcs_print_debug (PRINT_NODE_PROCESS_TIMES,
                       "%s: process times (last, min, max) = (%lf, %lf, %lf)\n",
                       name, last_process_time, min_process_time,
                       max_process_time);
    }
  if (NULL != cycle_timer)
    {
      int missed_cycles;
      missed_cycles = cycle_timer->wait ();
      if (missed_cycles > 0)
        {
          rcs_print_debug (PRINT_NODE_MISSED_CYCLES,
                           "%s: missed %d cycles.\n", name, missed_cycles);
        }
    }
  return;
}

/*******************************************************************
* Class: NODE
* Function: cyclic_read()
* Purpose: Reads in data from various sources, for each cycle.
* Returns: 0 for success or -1 for error.
*******************************************************************/
int
NODE::cyclic_read ()
{
  if (-1 == get_command_message ())     /* Read in cmd_in_message. */
    {
      current_section = NODE_GET_COMMAND_MESSAGE;
      return -1;
    }

  if (-1 == get_subordinates_status ()) /* Get status from subodinates. */
    {
      current_section = NODE_GET_SUBORDINATES_STATUS;
      return -1;;
    }

  if (-1 == read_world_model_in ())     /* Read wm_in_list */
    {
      current_section = NODE_READ_WORLD_MODEL_IN;
      return -1;
    }

  if (-1 == get_oi_requests ()) /* USER-DEFINED */
    {
      current_section = NODE_GET_OI_REQUESTS;
      return -1;
    }

  if (-1 == read_sensors ())    /* USER-DEFINED */
    {
      current_section = NODE_READ_SENSORS;
      return -1;
    }
  return 0;
}

/*******************************************************************
* Class: NODE
* Function: cyclic_process()
* Purpose: Performs 1 cycles worth of processing.
* Returns: 0 for success or -1 for error.
*******************************************************************/
int
NODE::cyclic_process ()
{
  if (-1 == process_oi_requests ())     /* USER-DEFINED */
    {
      current_section = NODE_PROCESS_OI_REQUESTS;
      return -1;
    }

  if (-1 == sensory_processor ())       /* USER-DEFINED */
    {
      current_section = NODE_SENSORY_PROCESSOR;
      return -1;
    }

  if (-1 == world_modeler ())   /* USER-DEFINED */
    {
      current_section = NODE_WORLD_MODELER;
      return -1;
    }

  if (-1 == command_independant_planner ())     /* USER-DEFINED */
    {
      current_section = NODE_COMMAND_INDEPENDANT_PLANNER;
      return -1;
    }

  if (cmd_in_new || !(config_flags & NODE_EXECUTE_NEW_COMMANDS_ONLY))
    {
      if (-1 == handle_generic_commands ())     /* Call init or halt if recieved. */
        {
          current_section = NODE_HANDLE_GENERIC_COMMANDS;
          return -1;
        }

      if (cmd_in_type == RCS_GENERIC_CMD_TYPE)
        {
          return 0;
        }

      if (-1 == execute_command ())     /* USER-DEFINED */
        {
          current_section = NODE_EXECUTE_COMMAND;
          return -1;
        }
    }
  return 0;
}



/*******************************************************************
* Class: NODE
* Function: cyclic_write()
* Purpose: Writes data out to various sources for this cycle.
* Returns: 0 for success or -1 for error.
*******************************************************************/
int
NODE::cyclic_write ()
{
  if (-1 == output_to_actuators ())     /* USER-DEFINED */
    {
      current_section = NODE_OUTPUT_TO_ACTUATORS;
      return -1;
    }

  if (-1 == send_oi_replies ()) /* USER-DEFINED */
    {
      current_section = NODE_SEND_OI_REPLIES;
      return -1;
    }

  if (-1 == write_world_model_out ())   /* USER-DEFINED */
    {
      current_section = NODE_WRITE_WORLD_MODEL_OUT;
      return -1;
    }

  if (-1 == send_subordinates_commands ())      /* Send Commands to Subs */
    {
      current_section = NODE_SEND_SUBORDINATES_COMMANDS;
      return -1;
    }

  if (-1 == send_status_message ())     /* Write status_up for my supervisor. */
    {
      current_section = NODE_SEND_STATUS_MESSAGE;
      return -1;
    }
  return (0);
}

/*******************************************************************
* Class: NODE                                                      *
* Function: run                                                    *
* Parameters:                                                      *
* double time; - time used for cycle_timer.                        *
* Purpose: Runs the node at the specified cycle time.
* Outputs:  NONE.                                                  *
* Returns: NONE.                                                   *
*******************************************************************/
void
NODE::run (double _time)
{
  if (_time != cycle_time)
    {
      if (NULL != cycle_timer && cycle_timer_created)
        {
          delete cycle_timer;
          cycle_timer = NULL;
        }
    }
  if (NULL == cycle_timer)
    {
      cycle_timer = new RCS_TIMER (_time);
      cycle_time = _time;
      cycle_timer_created = 1;
    }
  run ();
}


/*******************************************************************
* Class: NODE                                                      *
* Function: task                                                   *
* Parameters:                                                      *
* double time; - time used for cycle_timer.                        *
* Purpose: Reads in command messages and checks for old messages   *
*   or errors on the communication channel.                        *
* Outputs:  NONE.                                                  *
* Returns: NONE.                                                   *
*******************************************************************/
void
NODE::task (double time)
{
  if (time != cycle_time)
    {
      if (NULL != cycle_timer)
        {
          delete cycle_timer;
          cycle_timer = NULL;
        }
    }
  if (NULL == cycle_timer)
    {
      cycle_timer = new RCS_TIMER (time);
      cycle_time = time;
      cycle_timer_created = 1;
    }
  while (!abort_flag)
    {
      run ();
    }
}

/*******************************************************************
* Class: NODE                                                      *
* Function: run                                                    *
* Parameters:                                                      *
* double time; - time used for cycle_timer.                        *
* Purpose: Reads in command messages and checks for old messages   *
*   or errors on the communication channel.                        *
* Outputs:  NONE.                                                  *
* Returns: NONE.                                                   *
*******************************************************************/
void
NODE::task (RCS_TIMER * timer)
{
  if ((NULL != cycle_timer) && cycle_timer_created)
    {
      delete cycle_timer;
      cycle_timer = NULL;
    }

  cycle_timer = timer;
  cycle_time = timer->timeout;
  cycle_timer_created = 0;

  while (!abort_flag)
    {
      run ();
    }
}


/*******************************************************************
* Class: NODE                                                      *
* Function: get_command_message                                    *
* Parameters: NONE.                                                *
* Purpose: Reads in command messages and checks for old messages   *
*   or errors on the communication channel.                        *
* Outputs:                                                         *
*  RCS_CMD_MSG *cmd_in_msg; - pointer to message sent to       *
*                               superior.                          *
*  NMLTYPE cmd_in_type; - NML type id of last message recieved.    *
*  int state; - set to zero when new cmd recieved.                 *
*  int new_cmd_in; - set to one when new cmd recieved set to 0     *
*                    otherwise.                                    *
* Returns: 0 for success.                                          *
*          -1 if an error occured.                                 *
*******************************************************************/
int
NODE::get_command_message ()
{
  NMLTYPE read_value;
  if (config_flags & NODE_SAVE_LAST_COMMAND && NULL != cmd_in_msg &&
      NULL != last_cmd_in_msg)
    {
      memcpy (last_cmd_in_msg, cmd_in_msg, cmd_in_msg->size);
    }

  switch (read_value = cmd_in->read ())
    {
    case -1:
      rcs_print_error ("%s::get_command_message: Can`t read command in.\n",
                       name);
      return (-1);
    case 0:
      cmd_in_new = 0;
      return (0);
    default:
      cmd_in_new = 1;
      cmd_in_msg = cmd_in->get_address ();
      last_state_of_last_command = state;
      if (config_flags & NODE_RESET_STATE_ON_NEW_COMMAND)
        {
          state = 0;
        }
      if (config_flags & NODE_SEND_EXECUTING_ON_NEW_COMMAND)
        {
          send_status (NODE_EXECUTING);
        }
      node_status = NODE_EXECUTING;
      cmd_in_type_new = (cmd_in_type != read_value);
      last_cmd_in_type = cmd_in_type;
      cmd_in_type = read_value;
      rcs_print_debug (PRINT_COMMANDS_RECIEVED,
                       "%s: New cmd recieved. (type = %ld (0x%lX))\n", name,
                       cmd_in_type, cmd_in_type);
    }
  return (0);
}

/*******************************************************************
* Class: NODE                                                      *
* Function: get_subordinates_status                                *
* Parameters: NONE.                                                *
* Outputs:    NONE.                                                *
* Returns: 0 for success.                                          *
*          -1 if an error occured.                                 *
*******************************************************************/
int
NODE::get_subordinates_status ()
{
  NODE_LINK *node_link;

  if (0 == num_of_subord)
    {
      all_subordinates_done = 1;
      return (0);
    }

  if (NULL == sub_list)
    {
      return (-1);
    }
  all_subordinates_done = 1;

  node_link = (NODE_LINK *) sub_list->get_head ();
  while (NULL != node_link)
    {
      if (-1 == node_link->get_status ())
        {
          rcs_print_error ("%s::get_subodinates_status:\n", name);
          rcs_print_error ("   Error getting  %s's status.\n",
                           node_link->name);
          all_subordinates_done = 0;
          return (-1);
        }
      if (NODE_DONE != node_link->node_status)
        {
          all_subordinates_done = 0;
        }
      node_link = (NODE_LINK *) sub_list->get_next ();
    }

  return (0);
}

/*******************************************************************
* Class: NODE                                                      *
* Function: get_oi_message                                         *
* Parameters: NONE.                                                *
* Outputs:    NONE.                                                *
* Returns: 0 for success.                                          *
*          -1 if an error occured.                                 *
*******************************************************************/
int
NODE::get_oi_requests ()
{
  return (0);
}

/*******************************************************************
* Class: NODE                                                      *
* Function: read_world_model_in                                    *
* Parameters: NONE.                                                *
* Outputs:    NONE.                                                *
* Returns: 0 for success.                                          *
*          -1 if an error occured.                                 *
*******************************************************************/
int
NODE::read_world_model_in ()
{
  NODE_LINK *node_link;
  if (NULL == wm_in_list)
    {
      return (-1);
    }

  node_link = (NODE_LINK *) wm_in_list->get_head ();
  while (NULL != node_link)
    {
      if (-1 == node_link->read_wm ())
        {
          rcs_print_error ("%s::read_world_model_in:\n", name);
          rcs_print_error ("   Error getting  %s's wm.\n", node_link->name);
          return (-1);
        }
      node_link = (NODE_LINK *) wm_in_list->get_next ();
    }

  return (0);
}

/*******************************************************************
* Class: NODE                                                      *
* Function: determine_command                                      *
* Parameters: NONE.                                                *
* Outputs:    NONE.                                                *
* Returns: 0 for success.                                          *
*          -1 if an error occured.                                 *
*******************************************************************/
int
NODE::process_oi_requests ()
{
  return (0);
}

/*******************************************************************
* Class: NODE                                                      *
* Function: read_sensors                                           *
* Parameters: NONE.                                                *
* Outputs:    NONE.                                                *
* Returns: 0 for success.                                          *
*          -1 if an error occured.                                 *
*******************************************************************/
int
NODE::read_sensors ()
{
  return (0);
}

/*******************************************************************
* Class: NODE                                                      *
* Function: sensory_processor                                      *
* Parameters: NONE.                                                *
* Outputs:    NONE.                                                *
* Returns: 0 for success.                                          *
*          -1 if an error occured.                                 *
*******************************************************************/
int
NODE::sensory_processor ()
{
  return (0);
}

/*******************************************************************
* Class: NODE                                                      *
* Function: world_modeler                                          *
* Parameters: NONE.                                                *
* Outputs:    NONE.                                                *
* Returns: 0 for success.                                          *
*          -1 if an error occured.                                 *
*******************************************************************/
int
NODE::world_modeler ()
{
  return (0);
}

/*******************************************************************
* Class: NODE                                                      *
* Function: command_independant_planner
* Parameters: NONE.                                                *
* Outputs:    NONE.                                                *
* Returns: 0 for success.                                          *
*          -1 if an error occured.                                 *
*******************************************************************/
int
NODE::command_independant_planner ()
{
  return (0);
}

/*******************************************************************
* Class: NODE                                                      *
* Function: handle_generic_commands                                *
* Parameters: NONE.                                                *
* Outputs:    NONE.                                                *
* Returns: 0 for success.                                          *
*          -1 if an error occured.                                 *
*******************************************************************/
int
NODE::handle_generic_commands ()
{
  RCS_GENERIC_CMD *gen_cmd;


  if (config_flags & NODE_USE_GENERIC_COMMANDS)
    {
      /* Check if the last command was a generic commands. */
      if ((RCS_GENERIC_CMD_TYPE == cmd_in_type) && (NULL != sub_list))
        {
          gen_cmd = ((RCS_GENERIC_CMD *) cmd_in_msg);

          /* Call the command function associated with this command. */
          switch (gen_cmd->gen_id)
            {
            case GENERIC_INIT:
              init ();
              break;
            case GENERIC_HALT:
              halt ();
              break;
            default:
              break;
            }
        }
    }

  return (0);
}

/*******************************************************************
* Class: NODE                                                      *
* Function: send_command_to_all                                    *
* Parameters:                                                      *
* RCS_CMD_MSG *cmd; - The command that will be sent to all         *
* subordinates on the sub_list.                                    *
* Outputs:    NONE.                                                *
* Returns:   NONE.                                                 *
*******************************************************************/
void
NODE::send_command_to_all (RCS_CMD_MSG * cmd)
{
  NODE_LINK *node_link;

  if (NULL != sub_list)
    {
      /* Send the generic command to all the subnodes. */
      node_link = (NODE_LINK *) sub_list->get_head ();
      while (NULL != node_link)
        {
          node_link->new_command (cmd);
          node_link = (NODE_LINK *) sub_list->get_next ();
        }
    }
}

/*******************************************************************
* Class: NODE                                                      *
* Function: execute_commands
* Parameters: NONE.                                                *
* Outputs:    NONE.                                                *
* Returns: 0 for success.                                          *
*          -1 if an error occured.                                 *
*******************************************************************/
int
NODE::execute_command ()
{
  return (0);
}

/*******************************************************************
* Class: NODE                                                      *
* Function: initialize_state_variables                             *
* Parameters: NONE.                                                *
* Outputs:    NONE.                                                *
* Returns: NONE.                                                   *
*******************************************************************/
void
NODE::initialize_state_variables ()
{
}

/*******************************************************************
* Class: NODE                                                      *
* Function: init                                                   *
* Parameters: NONE.                                                *
* Outputs:    NONE.                                                *
* Returns: NONE.                                                   *
*******************************************************************/
void
NODE::init ()
{
  switch (state)
    {
    case 0:
      send_command_to_all (cmd_in_msg);
      initialize_state_variables ();
      state = 1;
      break;
    case 1:
      if (all_subordinates_done)
        {
          send_status (NODE_DONE);
          state = 2;
        }
      break;
    case 2:
      break;
    default:
      break;
    }
}

/*******************************************************************
* Class: NODE                                                      *
* Function: halt                                                   *
* Parameters: NONE.                                                *
* Outputs:    NONE.                                                *
* Returns: NONE.                                                   *
*******************************************************************/
void
NODE::halt ()
{
  switch (state)
    {
    case 0:
      send_command_to_all (cmd_in_msg);
      abort_flag = 1;
      state = 1;
      break;
    case 1:
      break;
    default:
      break;
    }
}

/*******************************************************************
* Class: NODE                                                      *
* Function: output_to_actuators                                    *
* Parameters: NONE.                                                *
* Outputs:    NONE.                                                *
* Returns: 0 for success.                                          *
*          -1 if an error occured.                                 *
*******************************************************************/
int
NODE::output_to_actuators ()
{
  return (0);
}


/*******************************************************************
* Class: NODE                                                      *
* Function: send_oi_replies
* Parameters: NONE.
* Outputs:    NONE.
* Returns: 0 for success.
*          -1 if an error occured.
*******************************************************************/
int
NODE::send_oi_replies ()
{
  return (0);
}

/*******************************************************************
* Class: NODE                                                      *
* Function: write_world_model_out                                  *
* Parameters: NONE.                                                *
* Outputs:                                                         *
* Returns: 0 for success.                                          *
*          -1 if an error occured.                                 *
*******************************************************************/
int
NODE::write_world_model_out ()
{
  if (wm_out_new)
    {
      wm_out_new = 0;
      if (NULL != wm_out_msg)
        {
          if (-1 == wm_out->write (wm_out_msg))
            {
              rcs_print_error ("%s: Can`t write to wm.", name);
              return (-1);
            }
        }
      rcs_print_debug (PRINT_NEW_WM, "%s: New wm. \n", name);
    }
  return (0);
}

/*******************************************************************
* Class: NODE                                                      *
* Function: send_subordinates_commands                             *
* Parameters: NONE.                                                *
* Outputs:    NONE.                                                *
* Returns: 0 for success.                                          *
*          -1 if an error occured.                                 *
*******************************************************************/
int
NODE::send_subordinates_commands ()
{
  NODE_LINK *node_link;
  if (NULL == sub_list)
    {
      return (-1);
    }

  node_link = (NODE_LINK *) sub_list->get_head ();
  while (NULL != node_link)
    {
      if (-1 == node_link->send_command ())
        {
          rcs_print_error ("%s::send_subordinates_commands:\n", name);
          rcs_print_error ("   Error sending %s a command.\n",
                           node_link->name);
          return (-1);
        }
      node_link = (NODE_LINK *) sub_list->get_next ();
    }
  return (0);
}

/*******************************************************************
* Class: NODE                                                      *
* Function: send_status_up                                         *
* Parameters: NONE.                                                *
* Outputs:                                                         *
* Returns: 0 for success.                                          *
*          -1 if an error occured.                                 *
*******************************************************************/
int
NODE::send_status_message ()
{
  if (status_up_new)
    {
      status_up_new = 0;
      if (NULL != status_up_msg)
        {
          status_up_msg->serial_number = cmd_in_msg->serial_number;
          if (-1 == status_up->write (status_up_msg))
            {
              rcs_print_error
                ("%s::send_status_up_message: Can`t send status.", name);
              return (-1);
            }
        }
      rcs_print_debug (PRINT_STATUS_SENT, "%s: New status sent. \n", name);
      rcs_print_debug (PRINT_STATUS_SENT,
                       " (type = %ld (0x%lX), status = %d)\n",
                       status_up_msg->type, status_up_msg->type,
                       status_up_msg->node_status);
    }
  return (0);
}

/*******************************************************************
* Class: NODE                                                      *
* Function: error_handler                                          *
* Parameters: NONE.                                                *
* Purpose: Prints an error message.                                *
* Outputs: NONE.                                                   *
* Returns: NONE.                                                   *
*******************************************************************/
void
NODE::error_handler ()
{
  rcs_print_error ("%s: error occured in section %d.\n", name,
                   current_section);
  rcs_print_error ("(error_code = %d)\n", error_code);
  switch (current_section)
    {
    case NODE_GET_COMMAND_MESSAGE:
    case NODE_GET_SUBORDINATES_STATUS:
    case NODE_GET_OI_REQUESTS:
    case NODE_READ_WORLD_MODEL_IN:
    case NODE_WRITE_WORLD_MODEL_OUT:
    case NODE_SEND_SUBORDINATES_COMMANDS:
    case NODE_SEND_STATUS_MESSAGE:
      valid = 0;
      break;
    default:
      break;
    }
}


/*******************************************************************
* Class: NODE                                                      *
* Function: check_if_valid                                         *
* Parameters: NONE.                                                *
* Outputs:                                                         *
* int valid; - set to 0 if any required communication channel      *
*     has an invalid configuration.                                *
*   (valid must not be 0 for the node to run.)                     *
* Returns: NONE.                                                   *
*******************************************************************/
void
NODE::check_if_valid ()
{
  if (NULL == cmd_in)
    {
      valid = 0;
      return;
    }
  if (!cmd_in->valid ())
    {
      valid = 0;
      return;
    }
  if (config_flags & NODE_USE_STAT_BUFFER)
    {
      if (NULL == status_up)
        {
          valid = 0;
          return;
        }
      if (!status_up->valid ())
        {
          valid = 0;
          return;
        }
    }
  if (config_flags & NODE_USE_WM_BUFFER)
    {
      if (NULL == wm_out)
        {
          valid = 0;
          return;
        }
      if (!wm_out->valid ())
        {
          valid = 0;
          return;
        }
    }
}

/* Handle Simple Status Messages. */
void
NODE::send_status (NODE_STATUS_TYPE new_status, RCS_STAT_MSG * new_msg)
{
  node_status = new_status;
  if (NULL != new_msg)
    {
      new_msg->node_status = new_status;
      status_up_msg = new_msg;
    }
  else
    {
      generic_status->node_status = new_status;
      status_up_msg = generic_status;
    }
  status_up_new = 1;
}

---