# Day 3: Autonomy (Whiteboard)

## Hands-on with goby_frontseat_interface

Publish/subscribe API:
- `data_from_frontseat`: data from frontseat -> backseat (payload). Could also include other data (sensor feeds)
- `desired_course`: usually desired heading, speed, depth, etc.
- `helm_state`: DRIVE or PARK
- `node_status`: convenience republish of the node_status field in `data_from_frontseat`
- `raw_in/raw_out`: The raw stream to/from the frontseat (NMEA-0183, binary protocol, "NMEA-0183")
- `status`: The states of the three subsystems.

### Basic Simulator driver

Each driver is an implementation of a base class: `goby::middleware::frontseat::InterfaceBase`

Each driver must implement six virtual methods:

- `send_command_to_frontseat`: called when the interface receives an outgoing command (often desired course)
- `send_data_to_frontseat`: often nothing, sensor data connected to the payload that are needed by the frontseat
- `send_raw_to_frontseat`: for "backdoor" feature to send raw messages from other processes
- `frontseat_state()`: the driver must provide its understanding of the frontseat state
- `frontseat_providing_data()`: true if the frontseat is streaming "normal" data.
- `loop()`: analogous to the loop() in the Application: 10 Hz.

In response, the driver has to call 4 signals:

- `signal_command_response`: called when we get an ACK from the frontseat.
- `signal_data_from_frontseat`: called when we get data from the frontseat (node_status, nav data)
- `signal_raw_from/to_frontseat`: called with the raw message received/sent by the driver

`goby_basic_frontseat_simulator`: standalone C++ application w/ very simple dynamic simulator with an ASCII payload API:

> means message from goby_frontseat_interface (backseat) to goby_basic_frontseat_simulator (frontseat)
< means message from goby_basic_frontseat_simulator (frontseat) to goby_frontseat_interface (backseat)

- goby_basic_frontseat_simulator runs a TCP server.
- all messages are "\r\n" terminated

1. simulator init message
(duration = 0 means no timeout)
(duration, freq, accel, hdg_rate, z_rate, warp are optional)
FREQ = control loop frequency in hertz
ACCEL = vehicle acceleration in m/s
HDG_RATE = vehicle turn rate in deg/s
Z_RATE = vehicle dive rate in m/s
WARP = run this factor faster than real time
> START,LAT:42.1234,LON:-72,DURATION:600,FREQ:10,ACCEL:0.5,HDG_RATE:45,Z_RATE:1,WARP:1

2. frontseat state messages
  sent after successful START command
< CTRL,STATE:PAYLOAD 
  sent after DURATION is up in START command
< CTRL,STATE:IDLE

3. frontseat nav command
  generated from primitive dynamics model
< NAV,LAT:42.1234,LON:-72.5435,DEPTH:200,HEADING:223,SPEED:1.4

4. backseat desired course command
> CMD,HEADING:260,SPEED:1.5,DEPTH:100

5. frontseat response to backseat CMD
  CMD is good
< CMD,RESULT:OK
  error in the CMD
< CMD,RESULT:ERROR


### Running the frontseat interface

`goby_frontseat_interface` must have a driver

Plugin model:

flowchart TD
  basic_simulator_frontseat_driver.h & basic_simulator_frontseat_driver.cpp -->|compiler|libgoby_frontseat_basic_simulator.so
  libgoby_frontseat_basic_simulator.so -->|dlopen, runtime, from FRONTSEAT_DRIVER_LIBRARY| goby_frontseat_interface

## Goby / MOOS interface

`pHelmIvP`
- multi-objective (heading, speed, depth) behavior-based (plugin api for writing new behaviors: "follow waypoints", "avoid collisions", "trail another vehicle", "your own") decision engine (MOOS-IvP).
- MOOS application

So we need a "bridge" between MOOS and Goby

### goby::moos::Translator

- `goby::moos::Translator` (actually a typedef for `BasicTranslator<goby::middleware::SimpleThread>`)
- goby Thread (interthread/interprocess comms)
- `CMOOSCommClient`: MOOS "interprocess" client

flowchart TD
   subgraph goby_moos_gateway or other MultiThreadApplication
   subgraph Translator
      CMOOSCommClient
      CMOOSCommClient <-->|your code| SimpleThread 
      SimpleThread
   end
   end
   CMOOSCommClient <--> MOOSDB
   pHelmIvP <--> MOOSDB
   SimpleThread <--> gobyd
   goby_app1 <--> gobyd
   goby_app2 <--> gobyd

You can have multiple Translators

flowchart TD
   subgraph goby_moos_gateway or other MultiThreadApplication
   subgraph FrontSeatTranslator
      CMOOSCommClient1[CMOOSCommClient]
      CMOOSCommClient1 <-->|your code| SimpleThread1
      SimpleThread1[SimpleThread]
   end
   subgraph MOOSSensorsTranslator
      CMOOSCommClient2[CMOOSCommClient]
      CMOOSCommClient2 <-->|your code| SimpleThread2
      SimpleThread2[SimpleThread]
   end
   end
   CMOOSCommClient1 <--> MOOSDB
   CMOOSCommClient2 <--> MOOSDB
   pHelmIvP <--> MOOSDB
   iSensor <--> MOOSDB
   SimpleThread1 <--> gobyd
   SimpleThread2 <--> gobyd

FrontSeatTranslation : BasicTranslator

flowchart TD
    subgraph MOOS/Goby
    subgraph goby_moos_gateway
    FrontSeatTranslation
    end
    end
    subgraph MOOS
    pHelmIvP[pHelmIvP]
    pHelmIvP -->|DESIRED_*, IVPHELM_STATE| FrontSeatTranslation 
    FrontSeatTranslation -->|NAV_*| pNodeReporter
    pNodeReporter -->|NODE_REPORT| pHelmIvP
    end
    subgraph Goby
    goby_frontseat_interface -->|node_status| FrontSeatTranslation
    FrontSeatTranslation -->|helm_state, desired_course| goby_frontseat_interface
    end

GOBY_MOOS_GATEWAY_PLUGINS=liblamss_goby3_gateway_plugin.so.0:libgoby_coroner_moos_gateway_plugin.so.30  goby_moos_gateway

We can just run the FrontSeatTranslation directly in `goby_frontseat_interface`:

flowchart TD
    
    subgraph Goby
    subgraph goby_frontseat_interface
    FrontSeatTranslation
    driver[frontseat::InterfaceBase implementation]
    end
    end
    subgraph MOOS
    pHelmIvP[pHelmIvP]
    pHelmIvP -->|DESIRED_*, IVPHELM_STATE| FrontSeatTranslation 
    FrontSeatTranslation -->|NAV_*| pNodeReporter
    pNodeReporter -->|NODE_REPORT| pHelmIvP
    end
    subgraph Goby
    driver -->|node_status| FrontSeatTranslation
    FrontSeatTranslation -->|helm_state| driver
    end
    driver <--> frontseat

## Command pHelmIvP through Goby

command from the topside to the USV.

Create a new `goby::moos::Translator` subclass `CommandTranslation` and we'll run this goby_moos_gateway.

flowchart TD
   subgraph topside
       goby_liaison
   end
   goby_liaison -->|usv_command|goby3_course_usv_manager

   subgraph usv
   goby3_course_usv_manager -->|usv_command| SimpleThread1
   subgraph goby_moos_gateway
   subgraph CommandTranslator
      CMOOSCommClient1[CMOOSCommClient]
      SimpleThread1 -->|convert| CMOOSCommClient1
      SimpleThread1[SimpleThread]
   end
   end
   CMOOSCommClient1 -->|DEPLOY_STATE, POLYGON_UPDATES| pHelmIvP
   end

- Added our new Loiter behavior to usv.bhv.in
- Wrote the new Translator thread
- Run our new Translator thread (within `goby_moos_gateway`)

### Time in Goby

C++11 and newer `std::chrono`

Goby is C++14

- point in time (e.g. right now) or duration (2 hours, 5 minutes) -> `std::chrono`
- date representation -> `boost::posix_time::ptime`
- serializable representation:
- For points in time: seconds (double) or microseconds (int64_t or uint64_t) since the UNIX epoch (1 Jan 1970 midnight UTC) -> boost::units::quantity<...time>, typedef to `goby::time::MicroTime` (for `int64_t` microseconds) to `goby::time::SITime` (double seconds version)
- For durations: seconds (double) or microseconds (int64_t) -> `goby::time::MicroTime` or the `SITime`.

Can be converted with the `goby::time::convert()` (for time points) and the `goby::time::convert_duration()` (for durations).

- `message_timestamp`: assume is a time point.
- `deploy_duration`: assume is a duration.

libdccl.org: more on the units

---