demo version

demo/camera_client/nerian_stream_unified/nerian_stream_unified.cpp

@@ -0,0 +1,179 @@
+/*******************************************************************************
+ * Copyright (c) 2021 Nerian Vision GmbH
+ * Copyright (c) 2022 Swinburne University of Technology
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *******************************************************************************/
+
+#include <opencv2/opencv.hpp>
+
+ // PCL headers must be included first!
+#include <pcl/pcl_base.h>
+#include <pcl/point_types.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/filters/extract_indices.h>
+
+#include <visiontransfer/deviceenumeration.h>
+#include <visiontransfer/asynctransfer.h>
+#include <visiontransfer/imageset.h>
+#include <visiontransfer/reconstruct3d.h>
+#include <iostream>
+#include <exception>
+#include <stdio.h>
+
+#include <senshamart/client/camera.hpp>
+#include <thread>
+#include <chrono>
+
+#include <unistd.h>
+
+#include <netinet/in.h>
+
+
+#ifdef _MSC_VER
+// Visual studio does not come with snprintf
+#define snprintf _snprintf_s
+#endif
+
+using namespace visiontransfer;
+namespace {
+  constexpr auto delta = std::chrono::seconds{ 1 } / 25;
+}
+
+
+int main(int argc, const char** argv) {
+
+  if (argc < 4) {
+    fprintf(stderr, "Expected %s <broker endpoint> <camera sensor name> <gps sensor name>\n", argv[0]);
+    return -1;
+  }
+
+  const char* const broker_endpoint = argv[1];
+  const char* const camera_sensor_name = argv[2];
+  const char* const gps_sensor_name = argv[3];
+
+  // Init AWS Code
+  senshamart::Camera_info init_info;
+
+  // Name, same as the KVS cloud. It is the hostname.
+  init_info.width = 1024;
+  init_info.height = 768;
+
+  init_info.broker_endpoint = broker_endpoint;
+  init_info.camera_sensor_name = camera_sensor_name;
+  init_info.gps_sensor_name = gps_sensor_name;
+
+
+  senshamart::Camera camera{ init_info };
+
+  int in;
+
+  senshamart::Clock::time_point now = senshamart::Clock::now();
+
+  std::cout << "[Nerian] Initialisation done" << std::endl;
+
+
+  // UDP socket to receive GNSS data
+  int sockfd, n;
+  struct sockaddr_in servaddr, cliaddr;
+  socklen_t len;
+  char gnssCoord[35] = "-0,-0,-0"; // Weird initialisation to indicate there is no GNSS data
+
+  sockfd = socket(AF_INET, SOCK_DGRAM, 0);
+
+  servaddr.sin_family = AF_INET;
+  servaddr.sin_addr.s_addr = htonl(INADDR_ANY);
+  servaddr.sin_port = htons(32100);
+  bind(sockfd, (struct sockaddr*)&servaddr, sizeof(servaddr));
+
+
+  try {
+    // Search for Nerian stereo devices
+    DeviceEnumeration deviceEnum;
+    DeviceEnumeration::DeviceList devices = deviceEnum.discoverDevices();
+    int cameraWaitingCycles = 0;
+    while (devices.size() == 0) {
+      // GG is shit because it does not restart services if they fail more than 3 times.
+      printf("[Nerian] No devices discovered! Waiting 0.5s and trying again. Total waiting time: %.1f seconds.\n", cameraWaitingCycles * 0.5);
+      std::this_thread::sleep_for(std::chrono::milliseconds(500));
+      devices = deviceEnum.discoverDevices();
+      cameraWaitingCycles++;
+      //return -1;
+    }
+
+    // Print devices
+    std::cout << "[Nerian] Discovered devices:" << std::endl;
+    for (unsigned int i = 0; i < devices.size(); i++) {
+      std::cout << "[Nerian] " << devices[i].toString() << std::endl;
+    }
+    std::cout << std::endl;
+
+    // Create an image transfer object that receives data from the first detected device
+    // and get the status of the camera
+    AsyncTransfer asyncTransfer(devices[0]);
+    auto status = devices[0].getStatus();
+
+    // Variables to calculate fps
+    std::chrono::time_point<std::chrono::system_clock> timeNewFrame, timeOldFrame;
+
+    // Receive and send images
+    while (true) {
+
+      // GNSS stuff
+      len = sizeof(cliaddr);
+      n = recvfrom(sockfd, gnssCoord, 35, MSG_DONTWAIT, (struct sockaddr*)&cliaddr, &len);
+      if (n > 0) {
+        gnssCoord[n] = 0;
+        printf("[GNSS] Received the following: ");
+        printf("%s", gnssCoord);
+
+        std::stringstream ss;
+        ss << gnssCoord;
+
+        std::string lat, lon, speed;
+        getline(ss, lat, ',');
+        getline(ss, lon, ',');
+        getline(ss, speed, ',');
+        printf("[Nerian] Sending GPS and Speed: %lf %lf %lf \n", std::stod(lat), std::stod(lon), std::stod(speed));
+        camera.add_gps(senshamart::Latitude{std::stod(lat)}, senshamart::Longitude{std::stod(lon)}, std::stod(speed));
+      } else {
+        printf("[Nerian] Not sending GPS and Speed\n");
+      }
+
+      // Receive image
+      ImageSet imageSet;
+      while (!asyncTransfer.collectReceivedImageSet(imageSet, 0.1 /*timeout*/)) {
+        // FIXME: Blocking code that we are not logging/handling. It needs testing
+        // Keep on trying until reception is successful
+      }
+
+      // Compute frame rate
+      timeOldFrame = timeNewFrame;
+      timeNewFrame = std::chrono::system_clock::now();
+      std::chrono::duration<double> elapsedSeconds = timeNewFrame - timeOldFrame;
+      std::cout << "[Nerian] Receiving image set at " << 1 / elapsedSeconds.count() << " fps" << std::endl;
+
+      // Nerian Camera Stuff
+        // Write only image 1, so we don't care about the other images. The other images are disparity maps
+        // Sending frames here
+      cv::Mat convertedImage;
+      imageSet.toOpenCVImage(0, convertedImage);  // Converting image 0 which is RGB
+      camera.add_frame(convertedImage);           // Sending RGB image in cv::Mat format
+
+      std::this_thread::sleep_until(now + delta);
+      now += delta;
+    }
+  } catch (const std::exception& ex) {
+    std::cerr << "Exception occurred: " << ex.what() << std::endl;
+  }
+
+  return 0;
+}