T265.gen

#pragma require "openrobots2-idl >= 2.0"

#include "or/pose/pose_estimator.gen"
#include "or/pose/rigid_body.gen"
#include "Apriltag.gen"

component T265
{
    version "0.0";
    doc "This component interfaces with Intel T265 Realsense device.";
    require   "genom3 >= 2.99.30";
    codels-requires "visp >= 3.3.1";

    /* NATIVE TYPES */
    native realsense_grabber;
    native vp_image;
    native vp_homogeneous_matrix;
    native vp_odometry;
    native log_s;

    exception e_sys { short code; string<128> what; };

    typedef sequence<apriltag::tag> tags;

    /* Output ports */
    port out or_pose_estimator::state odom_state;
    port out tags port_tags;

    /* Constants */
    const double period_image_ms = 33.333;

    ids
    {
        /* Grabber container */
        realsense_grabber rs_grabber;

        /* Frame number */
        long frame_nb;

        /* Image and pose counts */
        long image_count, pose_count;

        /* Odometry data containers */
        vp_odometry poseref_odo_sensor;

        /* Gyro angular velocity container */
        or::t3d::avel imu_gyro_data;

        /* Accelerometer linear acceleration */
        or::t3d::acc imu_accel_data;

        /* Image container */
        vp_image I_left, I_right;
        vp_image I_left_undistorted, I_right_undistorted;

        /* AprilTag detection parameters */
        double tag_size;
        double quad_decimate;

        /* Detected AprilTags container */
        tags detected_tags;

        /* Variables */
        boolean is_publishing, pose_enabled, display_enabled, detection_enabled;
        short nb_display_coefficient;
        vp_homogeneous_matrix pre_tf, post_tf;

        /* logging */
        log_s log;
    };

    attribute set_tag_size(in tag_size);

/******************** FUNCTIONS ********************/
    function start_publishing()
    {
        doc "Start retreiving data from T265 sensor. (To be used after pause_publish)";

        codel start_publish(out ::ids);
    };

    function pause_publishing()
    {
        doc "Pauses retreiving data from T265 sensor.";

        codel pause_publish(out is_publishing);
    };

    function set_pre_tf(in sequence< sequence<double,4> , 4 > new_pre_tf)
    {
        doc "Set precedent multiplication transformation. See documentation for more info.";
        codel set_pre_tf_codel(in new_pre_tf, out pre_tf);
    };

    function set_post_tf(in sequence< sequence<double,4> , 4> new_post_tf)
    {
        doc "Set post multiplication transformation. See documentation for more info.";
        codel set_post_tf_codel(in new_post_tf, out post_tf);
    };

    function set_display_frequency(in double frequency)
    {
        doc "Set display frequency. Maximum possible display frequency is 30Hz. 0 will disable display.";
        codel set_display_frequency_codel(in frequency, out nb_display_coefficient, out display_enabled);
    };

    function enable_pose()
    {
        doc "Enable publishing pose data.";

        codel enable_pose_codel(out pose_enabled);
    };

    function disable_pose()
    {
        doc "Disable pose data.";

        codel disable_pose_codel(out pose_enabled);
    };

    function enable_detection()
    {
        doc "Enable detection. (AprilTag, ...)";
        codel enable_detection_codel(out detection_enabled);
    };

    function disable_detection()
    {
        doc "Disable detection.";
        codel disable_detection_codel(out detection_enabled);
    };

    function get_tags_ID(out sequence<short> tags_id)
    {
        doc "List all detected tags IDs.";

        codel get_tags_ID_codel(in detected_tags, out tags_id);
    };

/* -------------------LOGGING-----------------------------*/
    function log(in string<64> path = "/tmp/T265.log": ": Log file name",
                in unsigned long decimation = 1: ": Reduced logging frequency") {
        doc		"Log odometry data";

        codel T265_log(in path, in decimation, inout log);

        throw e_sys;
    };

    function log_stop() {
        doc		"Stop logging";

        codel T265_log_stop(out log);
    };

/******************** TASKS ********************/
    task pose_port_refresh
    {
        doc "This task is used to refresh pose output port at 200Hz.";

        period 10ms;

        codel<start> init_port(out ::ids, out odom_state)
            yield loop;
        codel<loop> refresh_pose(out log, in pose_enabled, in rs_grabber, in poseref_odo_sensor, in pre_tf, in post_tf, out odom_state)
            yield pause::loop, stop;
        codel<stop> stop_pose_display(out poseref_odo_sensor, out pre_tf, out post_tf, out log)
            yield ether;
    };

    task apriltag_detection
    {
        doc "AprilTag detection.";

        period 33 ms;

        codel<start> init_detector(in detection_enabled, in rs_grabber, in I_left, in I_right, out tag_size, out detected_tags, out port_tags, out I_left_undistorted, out I_right_undistorted)
            yield pause::start, loop;
        codel<loop>  loop_detector(in detection_enabled, in is_publishing, in I_left, in I_right, inout I_left_undistorted, inout I_right_undistorted, in tag_size, out detected_tags, out port_tags)
            yield pause::loop, stop;
        codel<stop>  kill_detector(out detected_tags, out I_left_undistorted, out I_right_undistorted)
            yield ether;
    };

    task image_display
    {
        doc "This task is used to display images.";

        period 33ms;

        codel<start> init_display(in is_publishing, in display_enabled, in I_left, in I_right, in I_left_undistorted, in I_right_undistorted, in rs_grabber)
            yield pause::start, loop;
        codel<loop> refresh_display(in is_publishing, in display_enabled, in image_count, in nb_display_coefficient, in I_left, in I_right, in I_left_undistorted, in I_right_undistorted, in detection_enabled, in detected_tags)
            yield pause::loop, stop;
        codel<stop> stop_display(out rs_grabber, out I_left, out I_right, out I_left_undistorted, out I_right_undistorted, out is_publishing)
            yield ether;
    };

};