Pre-comp dev merge

riptide_acoustics/CMakeLists.txt

@@ -8,27 +8,27 @@ endif()
 # find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
 find_package(rclcpp REQUIRED)
-find_package(tf2 REQUIRED)
-find_package(tf2_ros REQUIRED)
-find_package(geometry_msgs REQUIRED)
 # uncomment the following section in order to fill in
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
 
 add_executable(run_acoustics src/Acoustics.cpp)
 
 ament_target_dependencies(run_acoustics
-std_msgs 
-geometry_msgs
-rclcpp
-tf2
-tf2_ros)
+std_msgs
+std_srvs
+rclcpp)
 
 install(TARGETS run_acoustics
   DESTINATION lib/${PROJECT_NAME}
 )
 
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
 #yoink files
 file(GLOB src_py_files RELATIVE ${PROJECT_SOURCE_DIR} src/*.py)
 install(PROGRAMS ${src_py_files}

riptide_acoustics/launch/acoustics.launch.py

@@ -0,0 +1,23 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    return LaunchDescription([
+        DeclareLaunchArgument(name="mode", default_value="max",
+                              description="Mode of comparing buffer 0 and buffer 1 to find nearest pinger"),
+
+        Node(
+            package='riptide_acoustics',
+            executable='run_acoustics',
+            name='riptide_acoustics',
+            output='screen',
+            parameters=[
+                {
+                    "mode": LaunchConfiguration("mode")
+                }
+            ],
+            respawn=True
+        )
+    ])

riptide_acoustics/package.xml

@@ -12,9 +12,9 @@
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 
-  <depend>riptide_msgs2</depend>
-  <depend>rclpy</depend>
-  <depend>tf2</depend>
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
 
   <export>
     <build_type>ament_cmake</build_type>

riptide_acoustics/src/Acoustics.cpp

@@ -1,98 +1,224 @@
 #include <memory>
-#include <chrono>
-#include <functional>
-
-
+#include <vector>
+#include <algorithm>
+#include <numeric>
+#include <string>
 
 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/float32.hpp"
-#include "geometry_msgs/msg/transform_stamped.hpp"
-#include "geometry_msgs/msg/transform.hpp"
-
-#include "tf2_ros/transform_listener.h"
-#include "tf2_ros/buffer.h"
-#include "tf2/exceptions.h"
+#include "std_msgs/msg/bool.hpp"
+#include "std_srvs/srv/trigger.hpp"
 
+#define MODE_MAX_FALLBACK_DIFF 1000.0f
+#define P_VALUE 95.0f
 
-using namespace std::chrono_literals;
-using std::placeholders::_1;
+using namespace std::placeholders;
 
+// 1) start_sample -> collect amplitudes for buffer 0
+// 2) stop_sample -> close buffer 0
+// 3) start_sample -> collect ampltudes for buffer 1
+// 4) stop_sample -> close buffer 1, then compare and publish once to buffer_0_closer
 
-class MinimalSubscriber : public rclcpp::Node
+class Acoustics : public rclcpp::Node
 {
   public:
-    MinimalSubscriber()
+    Acoustics()
     : Node("riptide_acoustics")
     {
-      subscription_ = this->create_subscription<std_msgs::msg::Float32>(
-      "acoustics/delta_t", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1));
+      this->declare_parameter("mode", "avg");
+
+      ampSubscription = this->create_subscription<std_msgs::msg::Float32>(
+        "/talos/ivc/pinger/selected_freq_amp_stream", 10, std::bind(&Acoustics::ampCallback, this, _1));
+
+      resultPublisher = this->create_publisher<std_msgs::msg::Bool>(
+        "/talos/acoustics/buffer_0_closer", 10);
 
-      //initialize tf buffer and tf listener
-      tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
-      tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+      resultTimer = this->create_wall_timer(
+        std::chrono::seconds(1), std::bind(&Acoustics::publishResult, this));
 
-      portToStarboardTransformTimer = this->create_wall_timer(1s, std::bind(&MinimalSubscriber::getPortToStarboardTransform, this));
+      startSampleService = this->create_service<std_srvs::srv::Trigger>(
+        "/talos/acoustics/start_sample", std::bind(&Acoustics::startSample, this, _1, _2));
+
+      stopSampleService = this->create_service<std_srvs::srv::Trigger>(
+        "/talos/acoustics/stop_sample", std::bind(&Acoustics::stopSample, this, _1, _2));
     }
 
   private:
-    void topic_callback(const std_msgs::msg::Float32::SharedPtr msg) const
+    void ampCallback(const std_msgs::msg::Float32::SharedPtr msg)
+    {
+      if (sampling) {
+        buffers[activeBuffer].push_back(msg->data);
+      }
+    }
+
+    void startSample(const std_srvs::srv::Trigger::Request::SharedPtr, 
+                           std_srvs::srv::Trigger::Response::SharedPtr response)
     {
-      geometry_msgs::msg::TransformStamped t;
-
-      try {
-        t = tf_buffer_->lookupTransform(
-          "world", "talos/base_link",
-          tf2::TimePointZero);
-      } catch (const tf2::TransformException & ex) {
-          RCLCPP_INFO(
-            this->get_logger(), "Could not transform!");
+      if (sampling) {
+        response->success = false;
+        response->message = "Already sampling";
+        return;
+      }
+
+      buffers[activeBuffer].clear();
+      sampling = true;
+      haveResult = false;
+
+      response->success = true;
+      response->message = "Sampling into buffer " + std::to_string(activeBuffer);
+      RCLCPP_INFO(this->get_logger(), "Started sampling into buffer %d", activeBuffer);
+    }
+
+    void stopSample(const std_srvs::srv::Trigger::Request::SharedPtr, 
+                          std_srvs::srv::Trigger::Response::SharedPtr response)
+    {
+      if (!sampling) {
+        response->success = false;
+        response->message = "Not sampling";
+        return;
+      }
+
+      sampling = false;
+
+      RCLCPP_INFO(this->get_logger(), "Stopped sampling buffer %d with %zu samples", 
+                                      activeBuffer, buffers[activeBuffer].size());
+
+      if (activeBuffer == 0) {
+        activeBuffer = 1;
+        response->success = true;
+        response->message = "With mode [" + this->get_parameter("mode").as_string() + "] Buffer 0 closed with " + std::to_string(buffers[0].size()) + " samples";
         return;
       }
 
-      RCLCPP_INFO(this->get_logger(), "X: %f", t.transform.translation.x);
+      // buffer 1 just closed, compare and reset
+      compareBuffers(response);
+      activeBuffer = 0;
+    }
 
+    std::string make_comparison_response(const std::string &mode) {
+      auto summary = [&](const std::string &mode_label) {
+        return "mode => [" + mode_label + "], result => buffer "
+             + std::to_string(buffer0Closer ? 0 : 1)
+             + " closer [buffer 0: " + std::to_string(result_0)
+             + " buffer 1: " + std::to_string(result_1) + "]";
+      };
+
+      std::string resp = "";
+      if (mode == "avg") {
+        resp = summary("avg");
+      } else if (mode == "max") {
+        if (did_fallback_on_max) {
+          resp += "INFO: [FELLBACK TO AVG ON MAX MODE] ";
+        }
+        resp += summary("max");
+      } else if (mode == "p95") {
+        resp = summary("p95");
+        resp = "UNRECOGNIZED MODE, CHECK LAUNCH FILE OR SOMETHING ";
+      }
+      return resp;
     }
 
-    void getPortToStarboardTransform()
+    void compareBuffers(std_srvs::srv::Trigger::Response::SharedPtr response)
     {
-      //stamped msg
-      geometry_msgs::msg::TransformStamped stamped;
+      if (buffers[0].empty() || buffers[1].empty()) {
+        response->success = false;
+        response->message = "Cannot compare, buffer 0 has " + std::to_string(buffers[0].size()) +
+                            " samples and buffer 1 has " + std::to_string(buffers[1].size());
+        RCLCPP_WARN(this->get_logger(), "%s", response->message.c_str());
+        return;
+      }
 
-      //get transform from tf buffer
-      try {
+      reduceBuffers(buffers[0], buffers[1], this->get_parameter("mode").as_string());
+      haveResult = true;
+      publishResult();
 
-        //TODO: change to port and starboard pod frames
-        stamped = tf_buffer_->lookupTransform("talos/pose_sensor_link", "talos/base_link", tf2::TimePointZero);
-
-        portToStarboardTransform = stamped.transform;
+      response->success = true;
+      response->message = make_comparison_response(this->get_parameter("mode").as_string());
+      // response->message = "mode[" + this->get_parameter("mode").as_string() + "]buffer " + std::to_string(buffer0Closer ? 0 : 1) +
+      //                     " closer (buffer 0: " + std::to_string(result_0) +
+      //                     ", buffer 1: " + std::to_string(result_1) + ")";
+      RCLCPP_INFO(this->get_logger(), "%s", response->message.c_str());
+    }
 
-        RCLCPP_INFO(this->get_logger(), "Successfuly collected port to starboard transform!");
-      } catch (const tf2::TransformException & ex) {
-          RCLCPP_INFO(
-            this->get_logger(), "Could collect port to starboard transform, retrying!");
+    void publishResult()
+    {
+      if (!haveResult) {
         return;
       }
 
-      portToStarboardTransformTimer->cancel();
+      std_msgs::msg::Bool msg;
+      msg.data = buffer0Closer;
+      resultPublisher->publish(msg);
+    }
+
+    void compare_avg_buffers(const std::vector<float> &buffer0, const std::vector<float> &buffer1) {
+      result_0 = std::accumulate(buffer0.begin(), buffer0.end(), 0.0) / buffer0.size();
+      result_1 = std::accumulate(buffer1.begin(), buffer1.end(), 0.0) / buffer1.size();
+      buffer0Closer = result_0 >= result_1;
     }
 
-    rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr subscription_;
+    void compare_p95_buffers(const std::vector<float> &buffer0, const std::vector<float> &buffer1) {
+      // it's c++ so sorting is O(n) right? (fire bar - balke)
+      std::vector<float> sorted0 = buffer0;
+      std::vector<float> sorted1 = buffer1;
+      size_t idx_0 = static_cast<size_t>(std::clamp((double)P_VALUE, 0.0, 100.0) / 100.0 * (sorted0.size() - 1));
+      size_t idx_1 = static_cast<size_t>(std::clamp((double)P_VALUE, 0.0, 100.0) / 100.0 * (sorted1.size() - 1));
+      std::nth_element(sorted0.begin(), sorted0.begin() + idx_0, sorted0.end());
+      std::nth_element(sorted1.begin(), sorted1.begin() + idx_1, sorted1.end());
+      result_0 = sorted0[idx_0];
+      result_1 = sorted1[idx_1];
+      buffer0Closer = result_0 >= result_1;
+    }
+
+    void compare_max_buffers(const std::vector<float> &buffer0, const std::vector<float> &buffer1) {
+      result_0 = *std::max_element(buffer0.begin(), buffer0.end());
+      result_1 = *std::max_element(buffer1.begin(), buffer1.end());
+      if (std::abs(result_0 - result_1) < MODE_MAX_FALLBACK_DIFF) {
+        did_fallback_on_max = true;
+        compare_avg_buffers(buffer0, buffer1);
+      }
+    }
+
+    void reduceBuffers(const std::vector<float> &buffer0, const std::vector<float> &buffer1, std::string mode) {
+      if (mode == "avg") {
+        compare_avg_buffers(buffer0, buffer1);
+      }
+
+      if (mode == "max") {
+        compare_max_buffers(buffer0, buffer1);
+      }
 
-    //tf
-    std::shared_ptr<tf2_ros::TransformListener> tf_listener_{nullptr};
-    std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+      if (mode == "p95") {
+        compare_p95_buffers(buffer0, buffer1);
+      }
+    }
 
-    //port pod to starboard pod transform
-    geometry_msgs::msg::Transform portToStarboardTransform;
-    rclcpp::TimerBase::SharedPtr portToStarboardTransformTimer {nullptr};
+    rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr ampSubscription;
+    rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr resultPublisher;
+    rclcpp::TimerBase::SharedPtr resultTimer;
+    rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr startSampleService;
+    rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr stopSampleService;
+
+    // amplitude samples for the two comparison windows
+    std::vector<float> buffers[2];
+    int activeBuffer = 0;
+    bool sampling = false;
+
+    // last comparison result, invalid until the first compare completes
+    bool buffer0Closer = false;
+    bool haveResult = false;
+    bool did_fallback_on_max = false;
+    // the results of the comparison run on the buffers
+    float result_0 = 0.0f;
+    float result_1 = 0.0f;
 
 };
 
 
 int main(int argc, char * argv[])
 {
   rclcpp::init(argc, argv);
-  rclcpp::spin(std::make_shared<MinimalSubscriber>());
+  rclcpp::spin(std::make_shared<Acoustics>());
   rclcpp::shutdown();
   return 0;
-}
+}