ColorTracker.gen

/*
 * Copyright (c) 2022-2022 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 "openrobots2-idl >= 2.1"
#pragma require "vision-idl"

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

#include "ColorTrack.idl"
#include "Environment.idl"


/* -------------------------- MODULE DECLARATION --------------------------- */
component ColorTracker {
  version "1.0";
  email	  "shasthamsa@laas.fr";
  lang	  "c";
  doc 	  "A GenoM module for the ColorTracker using a monocular camera.";

  codels-require "opencv4, felix-g3utils, vision-idl, eigen3";
  uses		or_pose_estimator;

  exception e_BAD_IMAGE_PORT { short code; string<128> message; };
  exception e_BAD_POSE_PORT { short code; string<128> message; };
  exception e_BAD_OG_PORT { short code; string<128> message; };
  exception e_BAD_TARGET_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; };

  struct Pose {
    float x, y, z;
    float roll, pitch, yaw;
  };

  struct Size {
    float width, height;
  };

  struct CameraInfo {
    float focal_length;
    float field_of_view;
    float pixel_size;

    Size image_size;
  };

  /* -------------------------- IDS --------------------------- */
  ids {
    or::sensor::intrinsics intrinsics;	        // Camera intrinsics
    or::sensor::extrinsics extrinsics;	        // Camera extrinsics
    or::sensor::frame image_frame;		        // Image frame
    or::ColorTrack::PlateSequence plates, all_detected_plates;       // The plates sequence
    or::ColorTrack::ColorInfo color;	        // The color of the object to track
    CameraInfo camera_info;                     // Camera information

    or_pose_estimator::state frame_pose;
    Pose camera_pose;

    boolean debug, show_frames, start_tracking, publish_og;
    octet verbose_level;
    float distance_threshold, focal_length;
    Size map_size, object_size;
  };

  /* ------------- DEFINITION OF PORTS -------------- */
  // Robot
  port in or_pose_estimator::state DronePose {
    doc "The pose of the drone.";
  };

  // Camera
  port in or::sensor::frame Frame {
    doc "The image frame to process.";
  };
  port in or::sensor::intrinsics Intrinsics;
  port in or::sensor::extrinsics Extrinsics;

  // Navigation
  port out or::Environment::OccupancyGrid OccupancyGrid {
    doc "To visualise the findings in desired tool.";};

  // ColorTracker
  port out or::ColorTrack::PlateSequence PlatesInfo {
    doc "Information on detected blobs over time.";
  }; // The plates sequence

  port out or_rigid_body::state TargetPose;
  port multiple out or::sensor::frame output {
    doc "The image frame with the detected object.";
  };

  /* ------------------ TASK DEFINITION -------------------- */
  task track {
    period 100 ms;
    doc "Track the object in the image.";

    codel <start> InitPort(port out output)
      yield ether;

    throw e_OUT_OF_MEM, e_BAD_IMAGE_PORT;
  };

  /* ------------------ ACTIVITY DEFINITION --------------------- */

  activity color_track() {
    doc "Detect the color and keep track of coordinates from the image.";
    after set_color, set_distance_threshold, set_camera_pose, set_map_size;
    task track;

    codel <start> FetchPorts(port in Frame, port in Intrinsics, port in Extrinsics, port in DronePose,
          ids out plates, ids out all_detected_plates, ids in debug)
          yield pause::start, poll;
    codel <poll> FetchDataFromPorts(port in Frame, port in Intrinsics, port in Extrinsics, port in DronePose,
          ids out image_frame, ids out intrinsics, ids out extrinsics, ids out camera_info, ids out frame_pose, ids in debug)
          yield pause::poll, main, ether;
    codel <main> TrackObject(ids in start_tracking, ids in image_frame, ids in camera_info, ids in color,
          port in DronePose, ids in distance_threshold, ids out plates, ids inout all_detected_plates, ids in camera_pose,
          port out PlatesInfo, port out output, ids in debug, ids in show_frames, ids in publish_og)
          yield main, poll, publish, ether;
    codel <publish> PublishOG(ids in object_size, ids in map_size, port in PlatesInfo, port out OccupancyGrid)
          yield poll, ether;

    throw e_BAD_IMAGE_PORT, e_BAD_POSE_PORT, e_BAD_OG_PORT, e_BAD_TARGET_PORT, e_OPENCV_ERROR;
    interrupts color_track;
  };

  /* ------------------ SERVICE DEFINITION: Attributes -------------------- */
  attribute set_color(in color =: "Color to be detected")
  {
    doc "Set the color to be detected.";
  };

  attribute set_debug(in debug =: "Debug mode")
  {
    doc "Set the debug mode.";
  };

  attribute set_camera_pose(in camera_pose =: "Camera pose")
  {
    doc "Set the camera pose with respect to robot frame.";
  };

  attribute set_map_size(in map_size.width = 10.0 : "Estimated map width", in map_size.height = 10.0 : "Estimated map height")
  {
    doc "Set the size of the map.";
  };

  attribute set_distance_threshold(in distance_threshold =: "Distance threshold to sample the coordinates")
  {
    doc "Set the distance threshold to sample the coordinates using linear interpolation.";
  };

  attribute perform_tracking(in start_tracking =: "Start tracking the object")
  {
    doc "Start tracking the object.";
  };

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

  attribute publish_occupancy_grid(in publish_og =: "Publish occupancy grid")
  {
    doc "Publish the occupancy grid.";
  };

  attribute show_image_frames(in show_frames =: "Show image frames")
  {
    doc "Show image frames.";
  };

  attribute set_object_size(in object_size.width =: "Object width", in object_size.height =: "Object length / height")
  {
    doc "Set the size of the object.";
  };

  attribute set_focal_length(in focal_length =: "Focal length of the object at a distance of 1 meter")
  {
    doc "Set the focal length of the object at 1 m.";
  };

  /* ------------------ SERVICE DEFINITION: Functions -------------------- */

  function clear_findins()
  {
    doc "Clear the findings.";
    codel ClearFindings(ids inout all_detected_plates);
    throw e_OUT_OF_MEM;
  };

};