<html><head><meta name="color-scheme" content="light dark"></head><body><pre style="word-wrap: break-word; white-space: pre-wrap;">%====================================================================
\section{An Implementation Example - the SimpleWaypoint  Behavior}
%====================================================================
\wikisec{grp=extend,file=IvPBuildSimpleWaypoint,short=A Short Example}

\label{sec_simple_wpt}

In this section an example IvP behavior is presented. It is a
simplified waypoint behavior version of the waypoint behavior in the
standard suite of behaviors distributed with the MOOS-IvP public
software bundle. The class name for this behavior is
\var{BHV\_SimpleWaypoint}. This behavior is distributed in the
moos-ivp-extend repository and should build out of the box.  After
going through the class itself, later in this section example
missions, also distributed with moos-ivp-extend, for running the
behavior are discussed.

%====================================================================
\subsection{The SimpleWaypoint Behavior Class Definition}
\label{sec_swpt_h}

The SimpleWaypoint behavior is configured with four parameters: a
single waypoint given in terms of local $x$ and $y$ coordinates, a
transit speed in meters per second, and a radius in meters around the
destination point within which the vehicle will be declared to have
arrived at its waypoint. The behavior, at every iteration of the helm
loop, notes the vehicle's own position in $x$ and $y$ local
coordinates. The idea is shown in Figure \ref{fig_simp_wpt}.

\vspace{0.15in}
\begin{figure}[H]
  \centering 
  \includegraphics[width=0.92\textwidth]{figures/simple_waypt.png}
  \caption{{\bf The SimpleWaypoint behavior}: The SimpleWaypoint
    behavior works with a single waypoint. The location of the
    waypoint is stored in the local variable \vvar{m\_nextpt} and is
    set during behavior configuration. The local variables
    \vvar{m\_osx} and \vvar{m\_osy} reflect the current vehicle
    (ownship) position updated at every helm iteration. The
    \vvar{m\_arrival\_radius} determines how close the vehicle needs
    to be from the waypoint destination before declaring completion.}
\label{fig_simp_wpt}
\end{figure}
\vspace{0.1in}

\noindent
The \var{BHV\_SimpleWaypoint} class definition is given below in
Listing \ref{lst_bhv_simple_h}. Note that it is declared to be a
subclass of the \var{IvPBehavior} superclass on line 8.  The three
helm-invoked overloadable functions are declared on lines 13-15. The
constructor is defined to take an \var{IvPDomain} as an argument.  The
helm will instantiate each behavior with the same helm-configured
domain as an argument to a behavior constructor.

\vspace{0.22in}
\begin{listing}{BHV\_SimpleWaypoint.h - the class definition for the "simple waypoint" behavior}
\label{lst_bhv_simple_h}
\scriptsize
\begin{verbatim}
  1  #ifndef BHV_SIMPLE_WAYPOINT_HEADER
  2  #define BHV_SIMPLE_WAYPOINT_HEADER
  3    
  4  #include &lt;string&gt;
  5  #include "IvPBehavior.h"
  6  #include "XYPoint.h"
  7   
  8  class BHV_SimpleWaypoint : public IvPBehavior {
  9  public:
 10    BHV_SimpleWaypoint(IvPDomain);
 11    ~BHV_SimpleWaypoint() {};
 12     
 13    bool         setParam(std::string, std::string);
 14    void         onIdleState();
 15    IvPFunction* onRunState();
 16     
 17  protected:
 18    void         postViewPoint(bool viewable=true);
 19    IvPFunction* buildFunctionWithZAIC();
 20    IvPFunction* buildFunctionWithReflector();
 21     
 22  protected: // Configuration parameters
 23    double       m_arrival_radius;
 24    XYPoint      m_nextpt;
 25    double       m_desired_speed;
 26    std::string  m_ipf_type;
 27    
 28  protected: // State variables
 29    double   m_osx;
 30    double   m_osy;
 31  };
 32     
 33  extern "C" {
 34    IvPBehavior * createBehavior(std::string name, IvPDomain domain) 
 35    {return new BHV_SimpleWaypoint(domain);}
 36  }
 37  #endif
\end{verbatim}
\end{listing}
\normalsize

\vspace{0.2in}
\noindent
The two configuration parameters depicted in Figure
\ref{fig_simp_wpt}, the waypoint and arrival radius, are declared on
lines 23-124. The two remaining configuration parameters, \var{"speed"}
and \var{"ipf\_type"} are on the following two lines. The former sets
the ideal speed for waypoint transiting, and the latter indicates the
type of IvP function to be generated. Two different ways of generating
an IvP function are implemented in this behavior to demonstrate two
different tools. The last part, lines 33-36 are the hooks needed for
each behavior class to implement the dynamic loading of behaviors into
the helm. These lines are therefore not present for behaviors compiled
into the IvP helm. These lines are very pertinent to the discussion of
extending" the helm.

%=======================================================================
\subsection{The SimpleWaypoint Behavior Class Implementation}
\label{sec_swpt_cpp}

The class implementation is given in Listings
\ref{lst_bhv_simple_cpp_a}-\ref{lst_bhv_simple_cpp_f} below.

%-----------------------------------------------------------------------
\subsubsection{The SimpleWaypoint Behavior Constructor}
The first part contains the class constructor in lines 17-34. On line
18, a call to the base-class constructor is made with the given
domain. A default for the behavior name is also set on line 20. On
line 21, the behavior declares that the domain over which it will
produce an IvP function is comprised of both the \var{course} and
\var{speed} variables. If the domain given to the behavior by the helm
in the constructor does not have either of these variables, a null IvP
domain will result in line 21. A null domain will make the behavior
thereafter not capable of running, and is considered a fatal error,
prompting the helm to post all-stop output values. This is purposely
drastic. Configuring the behaviors in a vehicle mission where one of
the behaviors is not runnable is worthy of stopping the helm and
addressing the problem. Since this condition is checked for on all
behaviors on each helm iteration, this problem would always reveal
itself at launch time, never during a mission, regardless of any
dynamic behavior configurations during a mission.

\pskip

On lines 25-27, default values for class member variables representing
key behavior parameters are set in the constructor. In lines 30-31,
class member variables representing behavior state variables are
initialized. The grouping of member variables into two sets, one that
represent parameter configurations and the other that otherwise
represent behavior state maintained during operation, is merely a
convention that has provided clarity in practice.

\vspace{0.2in}

\begin{listing}{BHV\_SimpleWaypoint.cpp - The SimpleWaypoint Behavior Constructor}
\label{lst_bhv_simple_cpp_a}
\vspace{0.1in}
\scriptsize
\begin{verbatim}
  1  #include &lt;cstdlib&gt;
  2  #include &lt;math.h&gt;
  3  #include "BHV_SimpleWaypoint.h"
  4  #include "MBUtils.h"
  5  #include "AngleUtils.h"
  6  #include "BuildUtils.h"
  7  #include "ZAIC_PEAK.h"
  8  #include "OF_Coupler.h"
  9  #include "OF_Reflector.h"
 10  #include "AOF_SimpleWaypoint.h"
 11    
 12  using namespace std;
 13    
 14  //-----------------------------------------------------------
 15  // Procedure: Constructor
 16    
 17  BHV_SimpleWaypoint::BHV_SimpleWaypoint(IvPDomain gdomain) : 
 18    IvPBehavior(gdomain)
 19  {
 20    IvPBehavior::setParam("name", "simple_waypoint");
 21    m_domain = subDomain(m_domain, "course,speed");
 22     
 23    // All distances are in meters, all speed in meters per second
 24    // Default values for configuration parameters 
 25    m_desired_speed  = 0; 
 26    m_arrival_radius = 10;
 27    m_ipf_type       = "zaic";
 28    
 29    // Default values for behavior state variables
 30    m_osx  = 0;
 31    m_osy  = 0;
 32  
 33    addInfoVars("NAV_X, NAV_Y");
 34  }
 35   
\end{verbatim}
\end{listing}
\normalsize

\vspace{0.2in} 

Finally, on line 33, the behavior declares two variables, \msvar{NAV\_X}
and \msvar{NAV\_Y}, representing vehicle ownship position. The IvP helm,
containing this behavior, will need to register for these to variables
on the behavior's behalf. This is the hook where the behavior tells
the helm what it needs from the MOOSDB. It is from these two variables
that the behavior will populate its variables \var{m\_osx} and
\var{m\_osy} representing the current vehicle position.

%-----------------------------------------------------------------------
\subsubsection{The SimpleWaypoint Behavior \func{setParam()} Function}
\label{sec_alder_sparam}

In Listing \ref{lst_bhv_simple_cpp_b} below, a key overloadable
behavior function is implemented, the \func{setParam()} function, in
lines 39-69. This function handles the configuration of the behavior
for its five parameters, \var{"ptx"}, \var{"pty"}, \var{"speed"},
\var{"radius"}, and \var{"ipf\_type"}. An example configuration for
this behavior is given in Listing \ref{lst_alder_bhv}. Behavior
parameters defined at the IvPBehavior superclass level, such as
\var{name}, \var{condition}, \var{endflag}, etc., are handled in the
\var{setParam()} function of the superclass. The helm, when it handles
a behavior parameter from a \var{*.bhv} file, first attempts to handle
the parameter at the superclass level. If the
\var{IvPBehavior::setParam()} function returns \var{false}, the helm
passes the parameter-value pair to the behavior's locally implemented
version of \func{setParam()}.

\vspace{0.22in}
\begin{listing}{BHV\_SimpleWaypoint.cpp - The \func{setParam()} function}
\label{lst_bhv_simple_cpp_b}
\vspace{0.1in}
\scriptsize
\begin{verbatim}
 36  //---------------------------------------------------------------
 37  // Procedure: setParam - handle behavior configuration parameters
 38    
 39  bool BHV_SimpleWaypoint::setParam(string param, string val) 
 40  {
 41    // Convert the parameter to lower case for more general matching
 42    param = tolower(param);
 43    
 44    double double_val = atof(val.c_str());
 45    if((param == "ptx")  &amp;&amp; (isNumber(val))) {
 46      m_nextpt.set_vx(double_val);
 47      return(true);
 48    }
 49    else if((param == "pty") &amp;&amp; (isNumber(val))) {
 50      m_nextpt.set_vy(double_val);
 51      return(true);
 52    }
 53    else if((param == "speed") &amp;&amp; (double_val &gt; 0) &amp;&amp; (isNumber(val))) {
 54      m_desired_speed = double_val;
 55      return(true);
 56    }
 57    else if((param == "radius") &amp;&amp; (double_val &gt; 0) &amp;&amp; (isNumber(val))) {
 58      m_arrival_radius = double_val;
 59      return(true);
 60    }
 61    else if(param == "ipf_type") {
 62      val = tolower(val);    
 63      if((val == "zaic") || (val == "reflector")) {
 64        m_ipf_type = val;
 65        return(true);
 66      }
 67    }
 68    return(false);
 69  }
 70  
\end{verbatim}
\end{listing}
\normalsize
\vspace{0.1in}

A fair amount of error checking is done for parameter. For example, in
setting the \var{"speed"} parameter, the string value is checked to
ensure that is both numerical and larger than zero. Solid error
checking implemented in this function is a very good idea that will
save headaches down the road. This function should only return
\var{true} if it has been passed a proper parameter-value
pair. Another common practice is to perform a case insensitive
parameter match, e.g., \var{"pty"} and \var{"PTY"} are both allowable
configurations. This is done by converting the string representing the
parameter to lower case in line 42. In this case, the \func{tolower()}
function is defined in a local utility toolbox.

%-----------------------------------------------------------------------
\subsubsection{The SimpleWaypoint \func{onIdleState()} and \func{postViewPoint()} Functions}
\label{sec_alder_ois}

The \func{onIdleState()} function, lines 74-77, is only executed when
the behavior is in the {\em idle} state, i.e., not in the {\em
  running} state. See Sections \ref{sec_helm_overview} and
\ref{sec_bhv_onstate} for more on behavior states. In this behavior,
the only task executed in the \func{onIdleState()} function is to publish
a waypoint marker in the form of the MOOS variable \mvar{VIEW\_POINT}.

\vspace{0.22in}
\begin{listing}{BHV\_SimpleWaypoint.cpp -The \func{onIdleState()} and \func{postViewPoint()} functions}
\label{lst_bhv_simple_cpp_c}
\vspace{0.1in}
\scriptsize
\begin{verbatim}
 71  //-----------------------------------------------------------
 72  // Procedure: onIdleState
 73    
 74  void BHV_SimpleWaypoint::onIdleState() 
 75  {
 76    postViewPoint(false);
 77  }
 78    
 79  //-----------------------------------------------------------
 80  // Procedure: postViewPoint
 81     
 82  void BHV_SimpleWaypoint::postViewPoint(bool viewable) 
 83  {
 84    m_nextpt.set_label(m_us_name + "'s next waypoint");
 85    m_nextpt.set_type("waypoint");
 86    m_nextpt.set_source(m_descriptor);
 87      
 88    string point_spec;
 89    if(viewable)
 90      point_spec = m_nextpt.get_spec("active=true");
 91    else
 92      point_spec = m_nextpt.get_spec("active=false");
 93    postMessage("VIEW_POINT", point_spec);
 94  }
 95  
\end{verbatim}
\end{listing}
\normalsize

An example produced by this would be:

\prevgreen
\begin{verbatim}
 VIEW_POINT = "active,false:label,alder's next waypoint:
              type,waypoint:source,waypt_return:0,0,0"
\end{verbatim}
\postverb

\noindent
In this case, due to the \var{"active,false"} component, the posting of
this variable would serve to "erase" similar postings to this
variable made in the \func{onRunState()} function described next. For
more on how \mvar{VIEW\_POINT} is consumed, see the documentation for
the \app{pMarineViewer}.

%-----------------------------------------------------------------------
\subsubsection{The SimpleWaypoint Behavior \func{onRunState()} Function}

Implementation of the \func{onRunState()} function is where the primary
unique operation of the behavior is implemented. For the
SimpleWaypoint behavior, the full function is in Listing
\ref{lst_bhv_simple_cpp_d} below. It is implemented in four parts:

\begin{itemize}
\item Part 1: Get the vehicle position from the information buffer.
  \vspace{-0.07in}
\item Part 2: Determine if the waypoint has been reached and possibly
  enter {\em complete} mode.  \vspace{-0.07in}
\item Part 3: Build a status message regarding the waypoint for third
  party viewers.  \vspace{-0.07in}
\item Part 4: Build an IvP function with either the ZAIC or Reflector
  tool.
\end{itemize}

\vspace{0.1in}
\noindent
In the first part, lines 101-109, information from the information
buffer is retrieved regarding the vehicle's own position. This is done
with the \func{getBufferDoubleVal()} function described in Section
\ref{sec_bhv_util_funcs}. In this behavior the result of the query to
the buffer is stored in the \var{ok1} and \var{ok2} variables and
subsequently checked and handled in lines 106-109. In this behavior, if
essential information like the vehicle's own position is missing, a
warning is posted (line 107) and the \app{onRunState()} function
returns without producing an objective function (line 108). In such a
case the behavior would be considered to be in the running state, but
not the active state for the present iteration.

\vspace{0.1in}
\noindent
A fair point to raise regarding Part 1 is the possibility that the
vehicle's position information is in the buffer but has become so old
that it no longer reflects the vehicle's true current position. In
other words, what if the navigation module on board the vehicle has
somehow shut down?  First, in most situations with a vehicle
implementing the backseat/front-seat driver architecture, a heartbeat
monitor for the navigation system is typically put in place at the
larger autonomy system level and an all-stop would be invoked
overriding the helm. However, for the sake of having some fail-safe
redundancy {\em within} the helm to handle this situation, the
\bvar{no\_starve} parameter could be used (Section
\ref{bhv_common_params}) for this behavior, or any behavior since it
is defined at the \var{IvPBehavior} superclass level. An example of
it's usage is shown in Listing \ref{lst_alder_bhv}, setting a
\bvar{no\_starve} threshold of 3 seconds for \msvar{NAV\_X} and
\msvar{NAV\_Y}.

\vspace{0.25in}

\begin{listing}{BHV\_SimpleWaypoint.cpp - the \func{onRunState()} implementation}
\label{lst_bhv_simple_cpp_d}
\scriptsize
\vspace{0.1in}
\begin{verbatim}
  96  //-----------------------------------------------------------
  97  // Procedure: onRunState
  98    
  99  IvPFunction *BHV_SimpleWaypoint::onRunState() 
 100  {
 101    // Part 1: Get vehicle position from InfoBuffer and post a 
 102    // warning if problem is encountered
 103    bool ok1, ok2;
 104    m_osx = getBufferDoubleVal("NAV_X", ok1);
 105    m_osy = getBufferDoubleVal("NAV_Y", ok2);
 106    if(!ok1 || !ok2) {
 107      postWMessage("No ownship X/Y info in info_buffer.");
 108      return(0);
 109    }
 110     
 111    // Part 2: Determine if the vehicle has reached the destination 
 112    // point and if so, declare completion.
 113    double dist = hypot((m_nextpt.x()-m_osx), (m_nextpt.y()-m_osy));
 114    if(dist &lt;= m_arrival_radius) {
 115      setComplete();
 116      postViewPoint(false);
 117      return(0);
 118    }
 119     
 120    // Part 3: Post the waypoint as a string for consumption by 
 121    // a viewer application.
 122    postViewPoint(true);
 123     
 124    // Part 4: Build the IvP function with either the ZAIC tool 
 125    // or the Reflector tool.
 126    IvPFunction *ipf = 0;
 127    if(m_ipf_type == "zaic")
 128      ipf = buildFunctionWithZAIC();
 129    else
 130      ipf = buildFunctionWithReflector();
 131    if(ipf == 0) 
 132      postWMessage("Problem Creating the IvP Function");
 133      
 134    return(ipf);
 135  }
 136  
\end{verbatim}
\end{listing}
\normalsize
\vspace{0.15in}

%------------------------------------------- Pt 2 of onRunState()   
In Part 2 of the \func{onRunState()} function, in lines 111-118 of
Listing \ref{lst_bhv_simple_cpp_d}, the determination of waypoint
arrival is made. This is just a simple comparison between the current
distance of the vehicle and the waypoint to the configured arrival
radius in the parameter \bvar{m\_arrival\_radius}. If a determination
of arrival is made, the behavior calls the \func{setComplete()}
function.  This function is defined at the behavior superclass level
and was described in detail in Section \ref{sec_bhv_util_funcs}. The
invocation of this function will put the behavior in the group of {\em
  completed} behaviors on the next helm iteration. In the current
iteration this behavior would be considered in the {\em running} state,
but not the {\em active} state since the \func{onRunState()} function
returns (line 117) without generating an objective function.
See Section \ref{sec_run_states} for more on behavior run states.

%---------------------------------------- Part 3 of onRunState()
\vspace{0.1in}
\noindent
In Part 3, the behavior generates a visual artifact for consumption by
a viewer, for rendering the waypoint the behavior is using as its
destination. This point is shown in Figures \ref{fig_alder1} and
\ref{fig_alder2} with the label \var{"Alder's next waypoint"}. An
example produced by this would be:

\prevgreen
\begin{verbatim}
 VIEW_POINT = "label=alder's next waypoint,type=waypoint,source=transit,x=60,y=-40"
\end{verbatim}
\postverb

\noindent
Compare this to the value for \mvar{VIEW\_POINT} generated in the
\func{onIdleState()} function described in Section
\ref{sec_alder_ois}.  This variable-value pair is generated by the
behavior for posting on each invocation of the \func{onRunState()}
even though the value posted does not generally change between
iterations.  The posting of the variable-value pair is done with the
\func{postMessage()} function, described in Section
\ref{sec_bhv_util_funcs}.  The invocation of \func{postMessage()} will
result in an actual post to the MOOSDB only if the value of string
posted changes. Successive duplicate postings are filtered out by the
Duplication Filter.  The Duplication Filter was described in Section
\ref{auto_filter} where alternative functions for overriding the
filter are given to accommodate other situations.  The
\app{postMessage()} function was discussed in Section
\ref{sec_bhv_util_funcs}.


%---------------------------------------- Part 4 of onRunState()

\vspace{0.1in}
\noindent
In Part 4 of the \func{onRunState()} function, in lines 124-135 of
Listing \ref{lst_bhv_simple_cpp_d}, an IvP function is generated over
a domain of heading and speed choices to reflect the goal of reaching
a waypoint given a current vehicle position. For the purposes of
providing an example usage of the IvPBuild Toolbox, the behavior is
implemented to produced an IvP function using two different methods of
the toolbox. The SimpleWaypoint behavior can be configured with the
\var{"ipf\_type"} parameter, as shown in Listing
\ref{lst_bhv_simple_cpp_b}, to accept either the configuration of
\var{"zaic"} or \var{"reflector"}. In the example mission, mission
"Alder" described below in Section \ref{sec_swpt_mission}, the helm
is configured in Listing \ref{lst_alder_bhv} to use the ZAIC tool on
the outbound transit trip, and the Reflector tool on the return trip.
The implementation of the \func{onRunState()} function merely checks
the type of IvP function desired and makes the appropriate call to
either the \func{buildFunctionWithZAIC()} function or the
\func{buildFunctionWithReflector()} function. These two functions are
described next.

%----------------------------------------------------------------------
\subsubsection{The SimpleWaypoint Behavior \func{buildFunctionWithZAIC()} Function}

When the SimpleWaypoint behavior is configured to generate an IvP
function with the ZAIC tool, it invokes the function
\func{buildFunctionWithZAIC()}, shown below in Listing
\ref{lst_bhv_simple_cpp_e}. Of the three ZAIC tools described in
Section \ref{sec_bldu_zaics}, the \var{ZAIC\_PEAK} is used in this
behavior. It is used to generate two one-variable IvP functions. The
first function is defined over the \var{speed} decision variable in
lines 142-151.  The second function is defined over \var{heading}
decision variable. The functions are shown in Figure
\ref{fig_one_var_alder}.

\vspace{0.25in}

\begin{listing}{BHV\_SimpleWaypoint.cpp - the \func{buildFunctionWithZAIC()} implementation}
\label{lst_bhv_simple_cpp_e}
\scriptsize
\vspace{0.1in}
\begin{verbatim}
  137  //-----------------------------------------------------------
  138  // Procedure: buildFunctionWithZAIC
  139     
  140  IvPFunction *BHV_SimpleWaypoint::buildFunctionWithZAIC() 
  141  {
  142    ZAIC_PEAK spd_zaic(m_domain, "speed");
  143    spd_zaic.setSummit(m_desired_speed);
  144    spd_zaic.setPeakWidth(0.5);
  145    spd_zaic.setBaseWidth(1.0);
  146    spd_zaic.setSummitDelta(0.8);  
  147    if(spd_zaic.stateOK() == false) {
  148      string warnings = "Speed ZAIC problems " + spd_zaic.getWarnings();
  149      postWMessage(warnings);
  150      return(0);
  151    }
  152      
  153    double rel_ang_to_wpt = relAng(m_osx, m_osy, m_nextpt.x(), m_nextpt.y());
  154    ZAIC_PEAK crs_zaic(m_domain, "course");
  155    crs_zaic.setSummit(rel_ang_to_wpt);
  156    crs_zaic.setPeakWidth(0);
  157    crs_zaic.setBaseWidth(180.0);
  158    crs_zaic.setSummitDelta(0);  
  159    crs_zaic.setValueWrap(true);
  160    if(crs_zaic.stateOK() == false) {
  161      string warnings = "Course ZAIC problems " + crs_zaic.getWarnings();
  162      postWMessage(warnings);
  163      return(0);
  164    }
  165     
  166    IvPFunction *spd_ipf = spd_zaic.extractIvPFunction();
  167    IvPFunction *crs_ipf = crs_zaic.extractIvPFunction();
  168     
  169    OF_Coupler coupler;
  170    IvPFunction *ivp_function = coupler.couple(crs_ipf, spd_ipf, 50, 50);
  171     
  172    return(ivp_function);
  173  }
  174  
\end{verbatim}
\end{listing}
\normalsize

\vspace{0.15in}
\noindent
The first step in creating an \var{IvPFunction} with the
\var{ZAIC\_PEAK} tool is to create an instance of the
\var{ZAIC\_PEAK}, on line 142, passing it the \var{IvPDomain} used by
the behavior and set in the behavior constructor in Listing
\ref{lst_bhv_simple_cpp_a}. The ZAIC constructor is also passed the
name of the one variable in the \var{IvPDomain} for which to create
the \var{IvPFunction}. The \var{ZAIC\_PEAK} parameters, set in lines
143-146, are described in detail in Section \ref{sec_zaic_peak}.  In
lines 147-151, a check is made to determine whether the
\var{ZAIC\_PEAK} instance has been configured properly. The
\var{stateOK()} function returns \var{false} if there were any
configuration problems, and the string returned by \func{getWarnings()}
function on line 148 will provide insight into any configuration
errors. The \func{postWMessage()} function on lines 149 and 162 will
result in the helm posting to the MOOSDB the variable
\msvar{BHV\_WARNING} with the contents of string \var{warnings}. The
\func{postWMessage()} function is discussed in Section
\ref{sec_bhv_util_funcs} on page \pageref{lab_postwmessage}.

\vspace{0.1in}
\noindent
The second one-variable function, defined over \var{course}, is
created with a second \var{ZAIC\_PEAK} instance in lines
153-164. First, the angle between the current vehicle position and the
waypoint destination is calculated on line 153, with a call to the
\var{relAng()} function, defined in one of the MOOS-IvP utility
libraries. The creation and configuration of the \var{ZAIC\_PEAK}
instance proceeds much in same way as for the first one. In this case,
the \var{valuewrap} parameter is set to \var{true} on line 159 to
indicate that the domain values should ``wrap around'', that is, a
course of 350 is 20 degrees separated from a course of 10 degrees, not
separated by 340 degrees. The \var{valuewrap} parameter is discussed
in Section \ref{zaic_peak_interface} on page \pageref{lab_valuewrap}.

\vspace{0.15in}
\begin{figure}[H]
  \centering 
  \includegraphics[width=0.95\textwidth]{figures/one-var-alder.png}
  \caption{{\bf IvP functions produced by the ZAIC\_PEAK Tool}: The
    two functions produced by the SimpleWaypoint behavior by use of
    the \vvar{ZAIC\_PEAK} tool would produce a function over
    \vvar{speed} with six pieces and a function over \vvar{heading}
    with three pieces. }
  \label{fig_one_var_alder}
\end{figure}
\vspace{0.1in}

\noindent
When these two functions are coupled using the \var{Coupler} tool,
lines 169-170, an IvP function over the coupled decision 2D space is
created as shown in Figure \ref{fig_coupled_alder} below. The Coupler
tool is discussed in Section \ref{sec_coupler} on page 
\pageref{sec_coupler}.

\vspace{0.15in}
\begin{figure}[H]
  \centering 
  \includegraphics[width=0.65\textwidth]{figures/alder-ipf-zaic-labels.png}
  \caption{{\bf An IvP function coupled from two one-variable
      functions}: This function is created by coupling the pair of
    one-variable functions of Figure \ref{fig_one_var_alder} using the
    \vvar{OF\_Coupler} tool. Decisions of increasing speed are
    represented by points on the function radially farther from the
    center.}
\label{fig_coupled_alder}
\end{figure}
\vspace{0.1in}

\noindent
The two one-variable functions are combined with an equal weight of 50
on line 170. The choice of relative weight has a distinct influence
over the resulting function. In Figure \ref{fig_coupled_alder_ab}
below, two IvP functions similarly generated, but with alternative
weights are shown. The Coupler, by default, normalizes the combined
function to the range of $[0,100]$. This can be turned off, or
normalized with a different range as described in Section
\ref{sec_coupler}. The range of $[0, 100]$ is a common range for
functions returned by an IvP behavior to the IvP Solver. 

% Say more about priority weights somewhere?????
% When are they applied? Responsibilities of the implementor?
% The solver, before solving the multi-objective optimization problem,
% applies the scalar priority weight to the function

\vspace{0.15in}
\begin{figure}[H]
  \centering 
  \includegraphics[width=0.99\textwidth]{figures/alder1-ipf-zaic-labels.png}
  \caption{{\bf Two IvP functions coupled from the same two
      one-variable functions}: These functions were created by
    coupling the pair of one-variable functions of Figure
    \ref{fig_one_var_alder} using the \vvar{OF\_Coupler} tool. They
    differ only in the relative weight applied to each function. The
    one on the left had a weight of 75 to the \var{speed} function and
    a weight of 25 to the \var{course} function. These weights are
    reversed on the right. The function on the right shows two
    off-peak decision points with equal utility rating. Decisions of
    increasing speed are represented by points on the function
    radially farther from the center.}
\label{fig_coupled_alder_ab}
\end{figure}
\vspace{0.1in}

\noindent
In each IvP function in Figures \ref{fig_coupled_alder} and
\ref{fig_coupled_alder_ab}, the location of the {\em peak} of the
function is the same. When this behavior is the only active behavior,
the path taken by the vehicle will be the same regardless of the
weights chosen to combine the two one-variable functions with the
Coupler. The only thing that matters is the value of the \var{summit}
parameters passed to the two ZAIC tools creating the two one-variable
functions. The off-peak characteristics of the function begin to
matter when the behavior is coordinated with functions from other
behaviors. In the function on the right in Figure
\ref{fig_coupled_alder_ab}, two off-peak decisions with equal utility
values are shown. One point represents a decision with the heading
more toward the destination with a speed much higher than the desired
speed, and the other decision represents a heading less toward the
destination but with speed near the optimal speed. In the absence of
mission metrics that clarify the relative utility of sub-optimal
transit paths versus sub-optimal speeds, the construction of the
off-peak shape of the objective function is typically a subjective
decision of the behavior implementor.

%--------------------------------------------------------------------
\subsubsection{The SimpleWaypoint Behavior \func{buildFunctionWithReflector()} Function}

The Reflector tool can be used to generate objective functions that
cannot otherwise be formed as the product of the coupling of two
independent functions. This gives the behavior implementor more
freedom to generate functions with off-peak characteristics more
in-line with the goals of the behavior. The Reflector tool is
described in detail in Sections \ref{sec_bldu_reflector} and
\ref{sec_bldu_advanced}.

\vspace{0.1in}
\noindent
The use of the Reflector tool in the SimpleWaypoint behavior is given
in Listing \ref{lst_bhv_simple_cpp_f} below. The Reflector generates
an IvP function approximation of an underlying function, an instance
of the \var{AOF\_SimpleWaypoint} class. The underlying function and
the IvP function are defined over the same domain. This domain is 
passed to the underlying function in its constructor (line 183). The 
underlying function is passed required parameters (lines 184-188)
and initialized (line 189). If any part of the initialization fails, 
a null IvP function is returned (line 180, 196).

\vspace{0.25in}

\begin{listing}{BHV\_SimpleWaypoint.cpp - the \func{buildFunctionWithReflector()} implementation}
\label{lst_bhv_simple_cpp_f}
\scriptsize
\vspace{0.1in}
\begin{verbatim}
 175  //-----------------------------------------------------------
 176  // Procedure: buildFunctionWithReflector
 177    
 178  IvPFunction *BHV_SimpleWaypoint::buildFunctionWithReflector() 
 179  {
 180    IvPFunction *ivp_function = 0;
 181     
 182    bool ok = true;
 183    AOF_SimpleWaypoint aof_wpt(m_domain);
 184    ok = ok &amp;&amp; aof_wpt.setParam("desired_speed", m_desired_speed);
 185    ok = ok &amp;&amp; aof_wpt.setParam("osx", m_osx);
 186    ok = ok &amp;&amp; aof_wpt.setParam("osy", m_osy);
 187    ok = ok &amp;&amp; aof_wpt.setParam("ptx", m_nextpt.x());
 188    ok = ok &amp;&amp; aof_wpt.setParam("pty", m_nextpt.y());
 189    ok = ok &amp;&amp; aof_wpt.initialize();
 190    if(ok) {
 191      OF_Reflector reflector(&amp;aof_wpt);
 192      reflector.create(1000);
 193      ivp_function = reflector.extractIvPFunction();
 194    }
 195     
 196    return(ivp_function);
 197  }
\end{verbatim}
\end{listing}
\normalsize


\vspace{0.1in}
\noindent
The Reflector tool does its work (lines 191-193) after it has been
determined that a proper instance of the underlying function,
\var{AOF\_SimpleWaypoint}, has been created and initialized. The
Reflector tool has several options for creating a piecewise defined
IvP function, described later in Sections \ref{sec_bldu_reflector} and
\ref{sec_bldu_advanced}. The simplest method is to specify the number
of pieces desired in the piecewise function, in this case 1000 pieces
were requested, on line 192. After creation, the IvP function is 
extracted from the Reflector (line 193), and returned (line 196).

\vspace{0.1in}
\noindent
An example of the IvP function created with the Reflector is shown in
Figure \ref{fig_alder_reflector}. The function represents a preference
for maneuvers that bring the vehicle toward the waypoint at a desired
speed. The values of off-peak areas are evaluated based on (a) the
rate of closure, (b) the rate of detour, and (c) the deviation from
the desired speed. For this function generated by the Reflector, and
the particular underlying function, it is not possible to generate a
function of equal form using the ZAIC tools.

\vspace{0.15in}
\begin{figure}[H]
  \centering 
  \includegraphics[width=0.65\textwidth]{figures/alder-reflector-labels.png}
  \caption{{\bf An IvP function created with the Reflector tool}: The
    function represents a preference for maneuvers that bring the
    vehicle toward the waypoint at a desired speed. The values of
    off-peak areas are evaluated based on (a) the rate of closure, (b)
    the rate of detour, and (c) the deviation from the desired speed.
    Decisions of increasing speed are represented by points on the
    function radially farther from the center.}
\label{fig_alder_reflector}
\end{figure}

%===========================================================================
\subsection{Running an Example Mission with the SimpleWaypoint Behavior}
\label{sec_swpt_mission}

An example mission file, \var{alder.moos}, and behavior file,
\var{alder.bhv}, have been configured to demonstrate the usage of the
SimpleWaypoint behavior. The files may be found in the
\var{moos-ivp-extend} tree. Refer back to Section
\ref{obtaining_extend} for information on obtaining this tree from the
web. The example mission with the SimpleWaypoint behavior is called
the Alder mission and is found by:

\begin{consoleverb}
 $ cd moos-ivp-extend/missions/alder
 $  ls
 $ README   alder.bhv   alder.moos 
\end{consoleverb}

\vspace{0.1in}
\noindent
The behavior file is given in Listing \ref{lst_alder_bhv} below. The
SimpleWaypoint behavior is used twice. It is used to transit to a
point and then to return to the starting point. The transiting use of
the behavior is configured in lines 7-20, and the returning use of the
behavior in lines 23-35. 

\vspace{0.25in}
\begin{listing}{The \var{alder.bhv} file - For running the Alder example mission}
\label{lst_alder_bhv}
\scriptsize
\vspace{0.1in}
\begin{verbatim}
   1  //--------    FILE: alder.bhv   -------------
   2   
   3  initialize   DEPLOY = false
   5  initialize   RETURN = false
   6    
   7  //----------------------------------------------
   8  Behavior = BHV_SimpleWaypoint
   9  { 
  10    name       = transit_to_point
  11    pwt        = 100
  12    condition  = RETURN = false
  13    condition  = DEPLOY = true
  14    endflag    = RETURN = true
  15    
  16         speed = 2.0   // meters per second
  17        radius = 8.0
  18           ptx = 60
  19           pty = -40
  20      ipf_type = zaic
  21  }
  22   
  23  //----------------------------------------------
  24  Behavior = BHV_SimpleWaypoint
  25  {
  26    name       = waypt_return
  27    pwt        = 100
  28    condition  = RETURN = true
  29    condition  = DEPLOY = true
  30  
  31         speed = 2.0
  32        radius = 8.0
  33           ptx = 0
  34           pty = 0
  35      ipf_type = reflector
  36  }
\end{verbatim}
\end{listing}
\normalsize

\vspace{0.15in}
\noindent
Both behaviors are idle upon startup. Presumably the transit behavior
is activated first by setting \msvar{DEPLOY=true}, and the second instance
of the behavior is activated when the transit behavior completes and 
sets its endflag. The \var{alder.moos} file is not discussed here, but
may be examined in the tree.

\vspace{0.1in}
\noindent
The example mission may be started by:

\vspace{0.1in}
\begin{consoleverb}
 $ cd moos-ivp-extend/missions/alder
 $ pAntler alder.moos
\end{consoleverb}
\vspace{0.1in}

\noindent
The \app{pMarineViewer} window should launch, and look similar to
image in Figure \ref{fig_alder1}. After clicking on the \var{DEPLOY}
button in the lower right corner, the transiting instance of the
SimpleWaypoint behavior becomes active and the vehicle begins to form
a track to the waypoint as shown. Clicking on the \var{DEPLOY} button
initiates a MOOS poke on the MOOSDB connected to both the
\app{pMarineViewer}, and \app{pHelmIvP} application.

\vspace{0.15in}
\begin{figure}[H]
  \centering 
  \includegraphics[width=0.99\textwidth]{figures/alder1.png}
  \caption{{\bf The SimpleWaypoint behavior in action}: After the user
    clicks on the \vvar{DEPLOY} button, the condition for the
    transiting SimpleWaypoint behavior is satisfied (line 13 in
    Listing \ref{lst_alder_bhv}).}
\label{fig_alder1}
\end{figure}
\vspace{0.1in}

\vspace{0.15in}
\begin{figure}[H]
  \centering 
  \includegraphics[width=0.99\textwidth]{figures/alder2.png}
  \caption{{\bf The SimpleWaypoint behavior in action}: After the
    vehicle has reached the waypoint prescribed in the transiting
    instance of the SimpleWaypoint behavior, the second instance of
    the SimpleWaypoint behavior, returning to the start point, becomes
    active.}
\label{fig_alder2}
\end{figure}

</pre></body></html>