ASDP_Analysis_Module_CXX.cpp

/*
 * Copyright (C) 2024: Arizona Board of Regents on Behalf of the University of Arizona
 */

// This is a client that connects to the first server it encounters and runs an example
// image analysis in C++ on the data stream coming from one of the cameras.  It gets a
// subset of the frames from the camera over time so that it can keep up.

/**
 * @file ASDP_Analysis_Module_CXX.cpp
 * @brief Apache Strap-Down Pilotage example analysis module using C++.
 *
* @author ReliaSolve.
* @date April 25, 2024.
*/

#include <iostream>
#include <chrono>
#include <map>
#include <ASDP_Core_API.h>
#include <ASDP_StreamPacketSortedQueue.h>

using namespace asdp;

std::shared_ptr<Message> WaitForMessageType(std::shared_ptr<Receiver> receiver, MessageID type, float seconds)
{
  std::shared_ptr<Message> empty;   ///< We return this on failure.
  std::chrono::steady_clock::time_point start = std::chrono::steady_clock::now();
  do {
    std::shared_ptr<StreamPacket> response;
    size_t offset = 0;
    Status status = receiver->ReceiveStreamPacket(0, response, offset);
    if ((status != OKAY) && (status != TIMEOUT)) {
      return empty;
    }
    if (response != nullptr) {
      std::shared_ptr<Message> message;
      status = response->GetNextMessage(message);
      if (status != OKAY) {
        return empty;
      }
      while (message != nullptr) {
        MessageID messageType;
        status = message->GetType(messageType);
        if (status != OKAY) {
          return empty;
        }
        if (messageType == type) {
          // Worked!
          return message;
        }
        status = response->GetNextMessage(message);
        if (status != OKAY) {
          return empty;
        }
      }
    }
  } while (std::chrono::duration<double>(std::chrono::steady_clock::now() - start).count() <= seconds);

  return empty;
}

void usage(std::string programName)
{
  std::cerr << "Usage: " << programName << " [--analysisServer URL] [--duration] <ip_address>" << std::endl;
  std::cerr << "  --analysisServer URL  URL of the analysis module server. Default is tcp://localhost." << std::endl;
  std::cerr << "  --duration SECONDS    Run for this many seconds. Default is 10." << std::endl;
  std::cerr << "  <ip_address> IP address to listen for servers on." << std::endl;
}

int main(int argc, char** argv)
{
  uint32_t frameStride = 30;    ///< Read one out of every this many frames. Set to 1 for every frame.
  float durationSeconds = 10;   ///< Run for this many seconds
  std::string analysisURL = "tcp://localhost";  ///< URL of the analysis module server.
  std::string ip_address;       ///< IP address to listen on for ASDP servers.
  size_t realParams = 0;

  // Parse the command line arguments, with the first non-flag argument being the
  // name of the IP address to listen on.  There is a --serial flag to specify
  // the serial number of the server, which defaults to 1.
  for (int i = 1; i < argc; ++i) {
    if (std::string(argv[i]) == "--analysisServer") {
      if ((i + 1) >= argc) {
        usage(argv[0]);
        return 1;
      }
      analysisURL = argv[++i];
    }
    else if (std::string(argv[i]) == "--duration") {
      if ((i + 1) >= argc) {
        usage(argv[0]);
        return 1;
      }
      durationSeconds = std::stof(argv[++i]);
    }
    else if (argv[i][0] == '-' ) {
      std::cerr << "Unknown flag: " << argv[i] << std::endl;
      usage(argv[0]);
      return 1;
    } else switch (realParams++) {
      case 0:
        ip_address = argv[i];
        break;
      default:
        usage(argv[0]);
        return 2;
    }
  }
  if (realParams != 1) {
    usage(argv[0]);
    return 2;
  }

  // Open a JSON string server to report analysis results to.
  std::shared_ptr<JSONStringSender> jsonSender;
  Status s = JSONStringSender::Create(analysisURL, jsonSender);
  if (s != OKAY) {
    std::cerr << "Failed to create JSONStringSender: " << ErrorMessage(s) << std::endl;
    return 100;
  }

  // Open a client, specifying the IP address to listen on.
  CoreClient client(ip_address);
  if (client.GetConstructorStatus() != OKAY) {
    std::cerr << "Failed to open client: " << ErrorMessage(client.GetConstructorStatus()) << std::endl;
    return 3;
  }
  std::cout << "Listening for servers on " << ip_address << std::endl;

  // Wait for up to two seconds to allow servers to send Discovery messages.
  std::chrono::steady_clock::time_point start = std::chrono::steady_clock::now();
  std::map<uint32_t, std::string> servers;
  Status threadStatus;
  Status status;
  do {
    status = client.GetDiscoveryThreadStatus(threadStatus);
    if (status != OKAY) {
      std::cerr << "Failed to get discovery thread status: " << ErrorMessage(status) << std::endl;
      return 4;
    }
    if (threadStatus != OKAY) {
      std::cerr << "Discovery thread status: " << ErrorMessage(threadStatus) << std::endl;
      return 5;
    }
    status = client.IdentifiedServers(servers);
    if (status != OKAY) {
      std::cerr << "Failed to get identified servers: " << ErrorMessage(status) << std::endl;
      return 6;
    }
    if (!servers.empty()) { break; }
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
  } while (std::chrono::duration<double>(std::chrono::steady_clock::now() - start).count() <= 2.0);
  if (servers.empty()) {
    std::cerr << "No servers found; be sure to run the server first." << std::endl;
    return 7;
  }
  std::cout << "Servers found: " << servers.size() << std::endl;
  for (const auto& server : servers) {
    std::cout << "  " << server.second << std::endl;
  }

  // Connect to the first server found.
  std::cout << "Connecting to " << servers.begin()->second << std::endl;
  uint16_t major, minor, patch;
  status = client.ConnectToServer(servers.begin()->second, major, minor, patch);
  if (status != OKAY) {
    std::cerr << "Failed to connect to server: " << ErrorMessage(status) << std::endl;
    return 8;
  }
  std::cout << "  Connected to server version " << major << "." << minor << "." << patch
    << " with serial number " << servers.begin()->first << std::endl;

  // Get the main stream receiver
  std::shared_ptr<Receiver> receiver;
  status = client.GetMainStreamReceiver(receiver);
  if (status != OKAY) {
    std::cerr << "Failed to get main stream receiver: " << ErrorMessage(status) << std::endl;
    return 10;
  }

  // Ensure that we get a state message from the server within a reasonable time.
  // Report information about the cameras that were found.
  std::shared_ptr<Message> msg = WaitForMessageType(receiver, STATE, 5.0);
  if (msg == nullptr) {
    std::cerr << "Did not get state message." << std::endl;
    return 12;
  }
  MessageState state(*msg);
  if (state.GetConstructorStatus() != OKAY) {
    std::cerr << "Failed to construct state message: " << ErrorMessage(state.GetConstructorStatus()) << std::endl;
    return 13;
  }
  std::vector<CameraInfo> cameras;
  status = state.GetCameras(cameras);
  std::cout << "Found " << cameras.size() << " cameras" << std::endl;
  if (cameras.empty()) {
    std::cerr << "No cameras found." << std::endl;
    return 14;
  }

  // Try streaming data from first camera at its highest rate.
  uint32_t camID = 1;
  {
    // Find the minimum period for the camera and which internal trigger ID it uses, then
    // configure the trigger to run at that rate.
    TriggerInfo ti;
    ti.ID = cameras[camID - 1].trigger;
    ti.mode = 1;
    ti.period = cameras[camID - 1].minTriggerPeriod;
    ti.offset = 0;
    ti.trackingFactor = 0.5;
    status = client.SendCommandPacket(CommandPacketConfigureTrigger(ti));
    if (status != OKAY) {
      std::cerr << "Failed to configure trigger: " << ErrorMessage(status) << std::endl;
      return 29;
    }

    // Construct a UDP receiver for a stream from the camera.
    ReceiverUDP receiverUDP;
    if (receiverUDP.GetConstructorStatus() != OKAY) {
      std::cerr << "Error constructing ReceiverUDP: " << ErrorMessage(receiverUDP.GetConstructorStatus()) << std::endl;
      return 30;
    }
    uint16_t port;
    asdp::Status status = receiverUDP.GetPort(port);
    if (status != asdp::OKAY) {
      std::cerr << "Error getting port from ReceiverUDP: " << ErrorMessage(status) << std::endl;
      return 31;
    }

    std::cout << "Analyzing every " << frameStride << " images from camera " << camID << " on port " << port
      << " for " << durationSeconds << " seconds" << std::endl;

    // Request the camera to stream full-frame images once every frameStride frames.
    StreamEndpoint endpoint(ip_address, port);
    SubregionDescription region;
    region.cameraID = camID;
    region.skipFrames = frameStride - 1;
    region.startTimeSeconds = 0;
    region.startTimeMicroseconds = 0;
    region.left = 0;
    region.top = 0;
    region.right = cameras[camID - 1].width - 1;
    region.bottom = cameras[camID - 1].height - 1;
    status = client.SendCommandPacket(CommandPacketStreamSubregion(endpoint, region));
    if (status != OKAY) {
      std::cerr << "Failed to stream images: " << ErrorMessage(status) << std::endl;
      return 32;
    }

    // Use a sorting queue to ensure that we process the messages in order even if the UDP packets
    // arrive out of order.
    StreamPacketSortedQueue sortedQueue(50);

    // Do analysis on frames until we've run for the specified duration.
    // We initialize the data every FRAME_BEGIN, accumulate over all FRAME_DATA, and complete at FRAME_END
    start = std::chrono::steady_clock::now();
    size_t numFrames = 0;
    double sum = 0;
    size_t pixels = 0;
    do {
      // Get the next packet.
      std::shared_ptr<asdp::StreamPacket> packet;
      size_t offset = 0;
      status = receiverUDP.ReceiveStreamPacket(10.0, packet, offset);
      if (status != asdp::OKAY) {
        std::cerr << "Error receiving StreamPacket: " << ErrorMessage(status) << std::endl;
        return 33;
      }
      // Add to the sorted queue and then handle any messages that are ready to be processed.
      std::list< std::shared_ptr<StreamPacket> > readyPackets = sortedQueue.AddPacket(packet);
      if (readyPackets.size() > 1) {
        std::cerr << "Warning: More than one packet ready to process (re-ordered or missing packet)." << std::endl;
      }
      while (!readyPackets.empty()) {

        std::shared_ptr<asdp::StreamPacket> receiveStreamPacket = readyPackets.front();
        readyPackets.pop_front();

        // Get and handle all messages from the packet.
        std::shared_ptr<asdp::Message> message;
        status = receiveStreamPacket->GetNextMessage(message);
        if (status != asdp::OKAY) {
          std::cerr << "Error getting message from packet: " << ErrorMessage(status) << std::endl;
          return 35;
        }
        while (message != nullptr) {
          asdp::MessageID rID;
          status = message->GetType(rID);
          if (status != asdp::OKAY) {
            std::cerr << "Error getting type from message: " << ErrorMessage(status) << std::endl;
            return 36;
          }
          if (rID == CONSOLIDATED_FRAME_DATA) {
            MessageConsolidatedFrameData frameData(*message);
            if (frameData.GetConstructorStatus() != asdp::OKAY) {
              std::cerr << "Error constructing FrameData message: " << ErrorMessage(frameData.GetConstructorStatus()) << std::endl;
              return 37;
            }

            bool isBeginFrame;
            status = frameData.GetBeginFrameFlag(isBeginFrame);
            if (status != asdp::OKAY) {
              std::cerr << "Error getting begin frame flag from FrameData message: " << ErrorMessage(status) << std::endl;
              return 39;
            }
            if (isBeginFrame) {
              sum = 0;
              pixels = 0;
            }

            // Find out how many pixels are in the frame and sum their values.
            if (frameData.GetConstructorStatus() != asdp::OKAY) {
              std::cerr << "Error constructing FrameData message: " << ErrorMessage(frameData.GetConstructorStatus()) << std::endl;
              return 37;
            }
            uint16_t left, right, top, bottom;
            status = frameData.GetLeft(left);
            if (status != asdp::OKAY) {
              std::cerr << "Error getting left from FrameData message: " << ErrorMessage(status) << std::endl;
              return 38;
            }
            status = frameData.GetRight(right);
            if (status != asdp::OKAY) {
              std::cerr << "Error getting right from FrameData message: " << ErrorMessage(status) << std::endl;
              return 39;
            }
            status = frameData.GetTop(top);
            if (status != asdp::OKAY) {
              std::cerr << "Error getting top from FrameData message: " << ErrorMessage(status) << std::endl;
              return 40;
            }
            status = frameData.GetBottom(bottom);
            if (status != asdp::OKAY) {
              std::cerr << "Error getting bottom from FrameData message: " << ErrorMessage(status) << std::endl;
              return 41;
            }
            uint8_t* rawData;
            status = frameData.GetDataPointer(rawData);
            if (status != asdp::OKAY) {
              std::cerr << "Error getting data pointer from FrameData message: " << ErrorMessage(status) << std::endl;
              return 42;
            }
            pixels += (right - left + 1) * (bottom - top + 1);
            uint16_t* data = reinterpret_cast<uint16_t*>(rawData);
            uint16_t stride = (right - left + 1);
            for (uint16_t y = 0; y <= bottom - top; ++y) {
              for (uint16_t x = 0; x <= right - left; ++x) {
                sum += data[y * stride + x];
              }
            }

            bool isEndFrame;
            status = frameData.GetEndFrameFlag(isEndFrame);
            if (status != asdp::OKAY) {
              std::cerr << "Error getting end frame flag from FrameData message: " << ErrorMessage(status) << std::endl;
              return 42;
            }
            if (isEndFrame) {
              std::cout << "  Frame " << ++numFrames << " has average value " << sum / pixels << std::endl;
              // Send the analysis result as a JSON object.
              Time time;
              status = frameData.GetTime(time);
              if (status != asdp::OKAY) {
                std::cerr << "Error getting time from FrameData message: " << ErrorMessage(status) << std::endl;
                return 43;
              }
              std::string jsonString = std::string("{ \"CamID\": ") + std::to_string(camID)
                    + ", \"Time\": "
                    + "[" + std::to_string(time.seconds) + "," + std::to_string(time.microseconds) + "], "
                    + "\"Name\": \"Average\", "
                    + " \"Value\": " + std::to_string(sum/pixels) + "}";
              status = jsonSender->Send(jsonString);
              if (status != asdp::OKAY) {
                std::cerr << "Error sending JSON string: " << ErrorMessage(status) << std::endl;
                return 44;
              }
            }
          }

          status = receiveStreamPacket->GetNextMessage(message);
          if (status != asdp::OKAY) {
            std::cerr << "Error getting first message from packet: " << ErrorMessage(status) << std::endl;
            return 45;
          }
        }
      }

    } while (std::chrono::duration<double>(std::chrono::steady_clock::now() - start).count() <= durationSeconds);
    std::cout << "Analyzed " << numFrames << " images." << std::endl;

    // Turn off streaming.
    status = client.SendCommandPacket(CommandPacketCancelSubregion(camID, endpoint));
    if (status != OKAY) {
      std::cerr << "Failed to cancel stream images: " << ErrorMessage(status) << std::endl;
      return 46;
    }
  }

  std::cout << std::endl << "Success!" << std::endl;
  return 0;
}