python publisher for sdgps solution to hydrophone pings (#1293)

* python publisher for sdgps solution to hydrophone pings

* Fixed PR comments

* scripts/ping_publisher.py: Make obvious that first log is from hydrophones library

---------

Co-authored-by: Cameron Brown <[email protected]>

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 scripts/ping_printer scripts/ping_logger scripts/ping_plotter scripts/hydrophones DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

mil_common/drivers/mil_passive_sonar/scripts/ping_publisher.py

@@ -0,0 +1,71 @@
+#!/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
+
+    # Create a socket
+    with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
+        pub = rospy.Publisher("hydrophones/solved", ProcessedPing, queue_size=10)
+        s.connect((HOST, PORT))
+        rospy.loginfo(
+            f"\nping_publisher connected to {HOST}:{PORT}, forwarding TCP messages to {pub.resolved_name}...",
+        )
+
+        # Need to ignore the first 2 JSON Parse Errors (nothing wrong)
+        parse_error_count = 0
+
+        while not rospy.is_shutdown():
+            # Receive data
+            data = s.recv(1024)
+            if not data:
+                break
+
+            # Parse the JSON data
+            try:
+                json_data = json.loads(data.decode("utf-8"))
+                ping_msg = ProcessedPing()
+
+                # Populate the header
+                ping_msg.header = Header()
+                ping_msg.header.seq += 1
+                ping_msg.header.stamp = rospy.Time.now()
+                ping_msg.header.frame_id = "hydrophones"
+
+                # 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"]
+                ping_msg.valid = True
+
+                # Publish the message
+                pub.publish(ping_msg)
+            except json.JSONDecodeError as e:
+                parse_error_count += 1
+                # ignore first two (normal behavior)
+                if parse_error_count > 2:
+                    rospy.logerr(f"JSONDecodeError: {e}")
+            except KeyError as e:
+                rospy.logerr(f"Key Error: {e}")
+
+
+if __name__ == "__main__":
+    with contextlib.suppress(rospy.ROSInterruptException):
+        rospy.init_node("pingpublisher", anonymous=True)
+        main()

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

mil_common/drivers/mil_passive_sonar/src/pipeline.sh

@@ -0,0 +1,3 @@
+#!/bin/bash
+
+sdgps connect-sonar-raw-tcp --host 192.168.37.61 --port 2006 ! filter-sonar-raw-samples --highpass 15e3 --lowpass 45e3 --notch 40e3 ! extract-robosub-pings ! robosub-ping-solver ! listen-robosub-ping-solution-tcp 2007