Waraps sensor data

.github/workflows/docker-image.yml

@@ -1,6 +1,5 @@
 name: Docker Image CI and CMake Checks
 on:
-  push:
   pull_request:
     branches: [ "main" ]
 

CMakeLists.txt

@@ -179,7 +179,7 @@ set(SOURCES
         ${CMAKE_CURRENT_SOURCE_DIR}/src/options.h
         ${CMAKE_CURRENT_SOURCE_DIR}/src/algorithms/pso_seeker.cpp
         ${CMAKE_CURRENT_SOURCE_DIR}/src/awpu.cpp
-
+        ${CMAKE_CURRENT_SOURCE_DIR}/src/geometry.cpp
         ${CMAKE_CURRENT_SOURCE_DIR}/src/audio/audio_wrapper.cpp
         ${CMAKE_CURRENT_SOURCE_DIR}/src/aw_control_unit.cpp
 )
@@ -203,25 +203,6 @@ target_link_libraries(beamformer
 target_include_directories(beamformer PUBLIC ${Boost_INCLUDE_DIRS})
 
 
-add_executable(awpu
-        ${CMAKE_CURRENT_SOURCE_DIR}/src/config.h
-        ${CMAKE_CURRENT_SOURCE_DIR}/src/receiver.cpp
-        ${CMAKE_CURRENT_SOURCE_DIR}/src/antenna.cpp
-        ${CMAKE_CURRENT_SOURCE_DIR}/src/pipeline.cpp
-        ${CMAKE_CURRENT_SOURCE_DIR}/src/delay.cpp
-        ${CMAKE_CURRENT_SOURCE_DIR}/src/options.h
-        ${CMAKE_CURRENT_SOURCE_DIR}/src/algorithms/pso_seeker.cpp
-        ${CMAKE_CURRENT_SOURCE_DIR}/src/awpu.cpp
-        ${CMAKE_CURRENT_SOURCE_DIR}/src/geometry.cpp
-)
-
-# Linking
-target_link_libraries(awpu
-        Eigen3::Eigen
-        ${OpenCV_LIBS}
-)
-
-
 
 add_dependencies(beamformer config)
 

Dockerfile

@@ -71,12 +71,21 @@ WORKDIR /WARA-PS-MQTT-Agent
 RUN cmake -S . -B build \
     && cmake --build build/ --target install
 
+WORKDIR /
+RUN git clone https://github.com/acoustic-warfare/WARA-PS-MQTT-Agent.git
+WORKDIR /WARA-PS-MQTT-Agent
+RUN cmake -S . -B build \
+    && cmake --build build/ --target install
+
 RUN git clone https://github.com/Rookfighter/pso-cpp.git && \
     mkdir -p pso-cpp/build && \
     cd pso-cpp/build && \
     cmake .. && make install
 
 # Create app directory (optional, since it will be mounted)
+RUN ldconfig
+
+# Create app directory
 RUN mkdir -p /usr/src/app
 
 # Set working directory

src/aw_control_unit.cpp

@@ -28,8 +28,13 @@ void AWControlUnit::Start() {
     cv::Mat frame(Y_RES, X_RES, CV_8UC1);
     cv::Mat colorFrame(Y_RES, X_RES, CV_8UC1);
 
-    // Used to time sensor publishing
-    auto timer = std::chrono::system_clock::now();
+    if (usingWaraPS_)
+        data_thread_ = std::thread([this] {
+            while (client_.running()) {
+                publishData();
+                std::this_thread::sleep_for(std::chrono::seconds(1));
+            }
+        });
 
     while ((usingWaraPS_ && client_.running()) || !usingWaraPS_) {
         awpu.draw_heatmap(&frame);
@@ -42,10 +47,7 @@ void AWControlUnit::Start() {
         std::unique_lock lock(pauseMutex_);
         pausedCV_.wait(lock, [&] { return !paused_; });
 
-        if(usingWaraPS_)
-            publishData(timer);
-
-        if(cv::waitKey(1) == 'q' || (usingWaraPS_ && !client_.running())) {
+        if (cv::waitKey(1) == 'q' || (usingWaraPS_ && !client_.running())) {
             break;
         }
     }
@@ -56,20 +58,19 @@ void AWControlUnit::Start() {
     }
     if (usingWaraPS_) {
         client_.Stop();
+        data_thread_.join();
     }
 }
 
 // Publishes data every second
-void AWControlUnit::publishData(std::chrono::time_point<std::chrono::system_clock> &timer) {
-    if(std::chrono::system_clock::now() - timer < std::chrono::seconds(1))
-        return;
-
-    timer = std::chrono::system_clock::now();
-
+void AWControlUnit::publishData() {
     if (usingGps_ && gps_waiting(&gpsData_, 1000)) {
         if (gps_read(&gpsData_, nullptr, 0) == -1) {
             std::cerr << "GPS Read error" << std::endl;
-        } else {
+        } else if ((isfinite(gpsData_.fix.altitude) &&
+                    isfinite(gpsData_.fix.latitude) &&
+                    isfinite(gpsData_.fix.longitude))) { // Sending NaN breaks WARA PS Arena
+
             nlohmann::json gpsJson = {
                     {"longitude", std::to_string(gpsData_.fix.longitude)},
                     {"latitude",  std::to_string(gpsData_.fix.latitude)},
@@ -83,12 +84,14 @@ void AWControlUnit::publishData(std::chrono::time_point<std::chrono::system_cloc
     client_.PublishMessage("sensor/heading", std::to_string(90.0)); // Currently not a real value
     client_.PublishMessage("sensor/course", std::to_string(0));
     client_.PublishMessage("sensor/speed", std::to_string(0));
+    client_.PublishMessage("sensor/camera_tags", "[ \"LJUDKRIGET\" ]");
 }
 
-AWControlUnit::AWControlUnit() : client_(WARAPS_NAME, WARAPS_ADDRESS, std::getenv("MQTT_USERNAME"), std::getenv("MQTT_PASSWORD")) {
+AWControlUnit::AWControlUnit() : client_(WARAPS_NAME, WARAPS_ADDRESS,
+                                         std::getenv("MQTT_USERNAME") == nullptr ? "" : std::getenv("MQTT_USERNAME"),
+                                         std::getenv("MQTT_PASSWORD") == nullptr ? "" : std::getenv("MQTT_PASSWORD")) {
     int gpsError = gps_open(GPS_ADDRESS, std::to_string(GPS_PORT).c_str(), &gpsData_);
-    gpsError |= gps_stream(&gpsData_, WATCH_ENABLE | WATCH_JSON,
-                           nullptr); // We only want the stream data, not pure buffer data
+    gpsError |= gps_stream(&gpsData_, WATCH_ENABLE | WATCH_JSON, nullptr);
 
     if (gpsError != 0) {
         std::cerr << "GPS Error: " << gps_errstr(gpsError) << std::endl;
@@ -97,27 +100,4 @@ AWControlUnit::AWControlUnit() : client_(WARAPS_NAME, WARAPS_ADDRESS, std::geten
     } else {
         usingGps_ = true;
     }
-
-    client_.SetCommandCallback("focus_bf", [&](const nlohmann::json &payload) {
-        float theta = payload["theta"];
-        float phi = payload["phi"];
-        float duration =
-                payload.contains("duration") ? (float) payload["duration"] : 5.0f;
-
-        std::cout << "Theta: " << theta << "\nPhi: " << phi << std::endl;
-        client_.PublishMessage("exec/response", std::string("Focusing beamformer for " +
-                                                            std::to_string(duration)));
-    });
-
-    client_.SetCommandCallback("pause", [&](const nlohmann::json &payload) {
-        std::unique_lock lock(pauseMutex_);
-        paused_ = true;
-    });
-
-    client_.SetCommandCallback("unpause", [&](const nlohmann::json &payload) {
-        std::unique_lock lock(pauseMutex_);
-        paused_ = true;
-        lock.unlock();
-        pausedCV_.notify_all();
-    });
 }

src/aw_control_unit.h

@@ -18,6 +18,7 @@ class AWControlUnit {
     std::vector<AWProcessingUnit> processingUnits;
     WaraPSClient client_;
     gps_data_t gpsData_{};
+    std::thread data_thread_;
 
     bool usingGps_ = false;
     bool usingWaraPS_ = false;
@@ -27,7 +28,7 @@ class AWControlUnit {
 
     bool paused_ = false;
 
-    void publishData(std::chrono::time_point<std::chrono::system_clock> &timer);
+    void publishData();
 public:
     AWControlUnit();
     AWControlUnit(const AWControlUnit&) = delete;