foxglove.gen

/*
 * Copyright (c) 2023 LAAS/CNRS
 *
 * Author: Selvakumar H S - LAAS/CNRS
 *
 * Permission to use, copy, modify, and/or distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 */

#pragma require "felix-idl"
#pragma require "vision-idl"

#include "or/sensor/camera.gen"
#include "or/pose/pose_estimator.gen"

/* -------------------------- MODULE DECLARATION --------------------------- */
component FoxgloveStudio {
  version "1.0";
  email	  "shasthamsa@laas.fr";
  lang	  "c";
  require "genom3 >= 2.99.26";
  doc 	  "A GenoM module for foxglove studio integration.";

  codels-require "opencv4, felix-g3utils, vision-idl, flatbuffers, openssl, zlib";

  exception e_BAD_PORT { short code; string<128> message; };
  exception e_OPENCV_ERROR { short code; string<128> message; };
  exception e_OUT_OF_MEM { short code; string<128> message; };
  exception e_BAD_CONFIG { short code; string<128> message; };

  enum PortType {
    or_sensor_frame,
    or_sensor_imu,
    or_sensor_magnetometer,
    or_pose_estimator_state,
    or_sensor_gps
  };

  struct Port {
    string<128> name;
    PortType type;
  };

  enum clocksrc {
    clock_local_coarse,
    clock_local_ntp,
    clock_utc_offset,
    clock_utc
  };

  struct ntp_offset_s {
    double offset, jitter;
  };

  enum frame_s {
    frame_enu,
    frame_nwu,
    frame_ned
  };

  enum fix_s {
    fix_none,
    fix_interp,
    fix_single,
    fix_diff,
    fix_rtk_float,
    fix_rtk,
    fix_fixed
  };

  enum monitor_op_s {
    monitor_above,
    monitor_below
  };

  struct llh_s {
    double latitude;	/* geodetic, degrees */
    double longitude;
    double height;	/* above ellipsoid */
  };

  struct ecef_s {
    double x, y, z;
  };

  struct enu_s {
    double east, north, up;
  };

  struct noise_s {
    double posxy_sigma, posz_sigma;
    double velxy_sigma, velz_sigma;
  };

  struct param_s {
    string<32> model;
    double rate;
    double ecutoff;
    noise_s noise;
    clocksrc tsmode;
    frame_s frame;
  };

  struct satlist_s {
    string<8> system;
    sequence<unsigned short, 64> used;
    sequence<unsigned short, 64> unused;
  };

  struct solution_s {
    or::time::ts ts;	/* timestamp on local clock */
    or::time::ts utc;	/* UTC timestamp */
    fix_s fix;
    llh_s llh;
    enu_s pos, pos_sigma;
    enu_s vel, vel_sigma;
    unsigned short sats, totalsats;
    sequence<satlist_s, 8> satlist;
  };

  /* -------------------------- IDS --------------------------- */
  ids {
    sequence<Port> ports;

    boolean start_foxglove_server;
    string server_ip;
    unsigned short server_port;
    boolean debug;
    octet verbose_level;
  };

  /* ------------- DEFINITION OF PORTS -------------- */
  // Camera
  port multiple in or::sensor::frame frames {
    doc "The image frame port.";
  };

  // Sensor
  port multiple in or_pose_estimator::state measure {
    doc "The sensor measurements.";
  };

  port multiple in solution_s gps {
    doc "The gps measurements.";
  };

  // State
  port multiple in or_pose_estimator::state states {
    doc "The pose estimation states.";
  };

  /* ------------------ TASK DEFINITION -------------------- */
  task main {
    doc "Publish to foxglove studio.";

    codel<start> setup_server_configuration(ids in server_ip, ids in server_port)
        yield ether;

    throw e_BAD_CONFIG, e_BAD_PORT, e_OPENCV_ERROR, e_OUT_OF_MEM;
  };

  /* ------------------ ACTIVITY DEFINITION --------------------- */
  activity add_port(in string<128> port_name =: "Genom port name", in PortType port_type =: "Genom port type") {
    doc "Add a port to the list of ports to be published.";
    task main;

    codel<start> setup_port_info(in port_name, in port_type, ids inout ports)
        yield ready, pause::start;
    codel <ready> wait_for_publishing(ids in start_foxglove_server)
        yield publish, ready, ether;
    codel<publish> publish_data(ids in start_foxglove_server, ids in ports, in frames, in measure, in states, in gps)
        yield publish, ready, pause::start;

    throws e_BAD_PORT, e_OUT_OF_MEM;
  };

  activity remove_port(in string<128> port_name =: "Genom port name") {
    doc "Remove a port from the list of ports to be published.";
    task main;

    codel<start> remove_port_info(in port_name, ids inout ports)
        yield ether;

    throws e_BAD_PORT, e_OUT_OF_MEM;
  };

  /* ------------------ SERVICE DEFINITION: Attributes -------------------- */
  attribute set_debug(in debug = FALSE : "Enable debug (default: false)" ) {
    doc "Set the debug mode.";
  };

  attribute set_verbose_level(in verbose_level = 0 : "Verbose level") {
    doc "Set the verbose level.";
  };

  attribute setup_server(in server_ip ="0.0.0.0" : "Websocket server ip",
                            in server_port = 8765 : "Websocket server port") {
    doc "Setup the websocket connection.";
  };

  /* ------------------ SERVICE DEFINITION: Functions -------------------- */
  function start_foxglove_server()
  {
    doc "Start the foxglove server.";
    validate check_info(in ::ids);

    codel start_server(ids out start_foxglove_server);
    throws e_BAD_CONFIG, e_BAD_PORT, e_OPENCV_ERROR, e_OUT_OF_MEM;
  };

  function stop_foxglove_server()
  {
    doc "Stop the foxglove server.";
    validate check_info(in ::ids);

    codel stop_server(ids out start_foxglove_server);
    throws e_BAD_CONFIG, e_BAD_PORT, e_OPENCV_ERROR, e_OUT_OF_MEM;
  };
};