From 9e687b852eb32f8088c2470b3f597784b0979239 Mon Sep 17 00:00:00 2001
From: Alex Johnson <alexjohnson@ufl.edu>
Date: Sun, 20 Oct 2024 01:12:25 +0000
Subject: [PATCH] python publisher for sdgps solution to hydrophone pings

---
 .../drivers/mil_passive_sonar/CMakeLists.txt  |  2 +-
 .../scripts/ping_publisher.py                 | 68 +++++++++++++++++++
 .../src/fakeping_pipeline.sh                  |  3 +
 .../drivers/mil_passive_sonar/src/pipeline.sh |  3 +
 4 files changed, 75 insertions(+), 1 deletion(-)
 create mode 100755 mil_common/drivers/mil_passive_sonar/scripts/ping_publisher.py
 create mode 100755 mil_common/drivers/mil_passive_sonar/src/fakeping_pipeline.sh
 create mode 100755 mil_common/drivers/mil_passive_sonar/src/pipeline.sh

diff --git a/mil_common/drivers/mil_passive_sonar/CMakeLists.txt b/mil_common/drivers/mil_passive_sonar/CMakeLists.txt
index 8ec65d3e7..6c73b3328 100644
--- a/mil_common/drivers/mil_passive_sonar/CMakeLists.txt
+++ b/mil_common/drivers/mil_passive_sonar/CMakeLists.txt
@@ -37,4 +37,4 @@ add_executable(sylphase_sonar_ros_bridge src/sylphase_ros_bridge.cpp)
 target_link_libraries(sylphase_sonar_ros_bridge ${catkin_LIBRARIES})
 add_dependencies(sylphase_sonar_ros_bridge ${catkin_EXPORTED_TARGETS})
 
-install(PROGRAMS scripts/ping_printer scripts/ping_logger scripts/ping_plotter scripts/hydrophones DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+install(PROGRAMS scripts/ping_publisher.py scripts/ping_printer scripts/ping_logger scripts/ping_plotter scripts/hydrophones DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
diff --git a/mil_common/drivers/mil_passive_sonar/scripts/ping_publisher.py b/mil_common/drivers/mil_passive_sonar/scripts/ping_publisher.py
new file mode 100755
index 000000000..aab9156d9
--- /dev/null
+++ b/mil_common/drivers/mil_passive_sonar/scripts/ping_publisher.py
@@ -0,0 +1,68 @@
+#!/usr/bin/env python3
+
+import contextlib
+import json
+import socket
+
+import rospy
+from geometry_msgs.msg import Point
+from mil_passive_sonar.msg import ProcessedPing
+from std_msgs.msg import Header
+
+
+def main():
+    # Define the server address and port
+    HOST = "127.0.0.1"
+    PORT = 2007  # The port used by the server
+
+    # Create a socket
+    with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
+        s.connect((HOST, PORT))
+        print(f"Connected to {HOST}:{PORT}")
+        pub = rospy.Publisher("ping", ProcessedPing, queue_size=10)
+        rospy.init_node("pingpublisher", anonymous=True)
+
+        while not rospy.is_shutdown():
+            # Receive data
+            data = s.recv(1024)  # Buffer size is 1024 bytes
+            if not data:
+                break
+
+            # Parse the JSON data
+            try:
+                json_data = json.loads(data.decode("utf-8"))
+                # Create a ProcessedPing message
+                ping_msg = ProcessedPing()
+
+                # Populate the header
+                ping_msg.header = Header()
+                ping_msg.header.seq += 1  # Increment sequence number
+                ping_msg.header.stamp = rospy.Time.now()
+                ping_msg.header.frame_id = "ping_frame"  # Set your frame_id
+
+                # Populate the position
+                ping_msg.position = Point()
+                ping_msg.position.x = json_data["origin_direction_body"][0]
+                ping_msg.position.y = json_data["origin_direction_body"][1]
+                ping_msg.position.z = json_data["origin_direction_body"][2]
+
+                # Populate the frequency and amplitude
+                ping_msg.freq = json_data["frequency_Hz"]
+                ping_msg.amplitude = json_data[
+                    "origin_distance_m"
+                ]  # You can modify this as needed
+                ping_msg.valid = True  # Or set to False based on your logic
+
+                # Publish the message
+                pub.publish(ping_msg)
+                rospy.loginfo(f"Published: {ping_msg}")
+
+            except json.JSONDecodeError as e:
+                rospy.loginfo(f"JSON Decode Error: {e}")
+            except KeyError as e:
+                rospy.loginfo(f"Key Error: {e}")
+
+
+if __name__ == "__main__":
+    with contextlib.suppress(rospy.ROSInterruptException):
+        main()
diff --git a/mil_common/drivers/mil_passive_sonar/src/fakeping_pipeline.sh b/mil_common/drivers/mil_passive_sonar/src/fakeping_pipeline.sh
new file mode 100755
index 000000000..cff851f10
--- /dev/null
+++ b/mil_common/drivers/mil_passive_sonar/src/fakeping_pipeline.sh
@@ -0,0 +1,3 @@
+#!/bin/bash
+
+sdgps generate-fake-sonar-solution --rate 1 --position '[-30, 10, -2]' --circle '{"radius":3,"axis":[0,0,1],"period":30}' ! sonar-simulate --role rover --transmit-pattern robosub-2019 --base-hydrophone-arrangement robosub-2019 --rover-hydrophone-arrangement uf-mil-sub7 --cn0 80 --multipath extreme ! filter-sonar-raw-samples --highpass 15e3 --lowpass 45e3 --notch 40e3 ! extract-robosub-pings ! robosub-ping-solver ! listen-robosub-ping-solution-tcp 2007
diff --git a/mil_common/drivers/mil_passive_sonar/src/pipeline.sh b/mil_common/drivers/mil_passive_sonar/src/pipeline.sh
new file mode 100755
index 000000000..2a180afe2
--- /dev/null
+++ b/mil_common/drivers/mil_passive_sonar/src/pipeline.sh
@@ -0,0 +1,3 @@
+#!/bin/bash
+
+sdgps connect-sonar-raw-tcp --host 192.168.37.51 --port 2006 ! filter-sonar-raw-samples --highpass 15e3 --lowpass 45e3 --notch 40e3 ! extract-robosub-pings ! robosub-ping-solver ! listen-robosub-ping-solution-tcp 2007